75 volatile int32_t *p __attribute__((used));
130 asm volatile(
"push r16 \n\t");
131 asm volatile(
"in r16,0x3F \n\t");
132 asm volatile(
"push r16 \n\t");
133 asm volatile(
"push r30 \n\t");
134 asm volatile(
"push r31 \n\t");
135 asm volatile(
"lds r30,p \n\t");
136 asm volatile(
"lds r31,p+1 \n\t");
138 asm volatile(
"ldd r16,z+24 \n\t");
139 asm volatile(
"cpi r16,0x01 \n\t");
140 asm volatile(
"breq _pid \n\t");
142 asm volatile(
"ldd r16,z+0 \n\t");
143 asm volatile(
"cpi r16,0x00 \n\t");
144 asm volatile(
"brne _pid \n\t");
146 asm volatile(
"ldd r16,z+1 \n\t");
147 asm volatile(
"cpi r16,0x00 \n\t");
148 asm volatile(
"brne _pid \n\t");
150 asm volatile(
"ldd r16,z+2 \n\t");
151 asm volatile(
"cpi r16,0x00 \n\t");
152 asm volatile(
"brne _pid \n\t");
154 asm volatile(
"ldd r16,z+3 \n\t");
155 asm volatile(
"cpi r16,0x00 \n\t");
156 asm volatile(
"brne _pid \n\t");
158 asm volatile(
"subi r30,83 \n\t");
159 asm volatile(
"sbci r31,0 \n\t");
161 asm volatile(
"jmp _AccelerationAlgorithm \n\t");
163 asm volatile(
"_pid: \n\t");
164 asm volatile(
"push r0 \n\t");
165 asm volatile(
"push r1 \n\t");
166 asm volatile(
"push r2 \n\t");
167 asm volatile(
"push r3 \n\t");
168 asm volatile(
"push r4 \n\t");
169 asm volatile(
"push r5 \n\t");
170 asm volatile(
"push r6 \n\t");
171 asm volatile(
"push r7 \n\t");
172 asm volatile(
"push r8 \n\t");
173 asm volatile(
"push r9 \n\t");
174 asm volatile(
"push r10 \n\t");
175 asm volatile(
"push r11 \n\t");
176 asm volatile(
"push r12 \n\t");
177 asm volatile(
"push r13 \n\t");
178 asm volatile(
"push r14 \n\t");
179 asm volatile(
"push r15 \n\t");
180 asm volatile(
"push r17 \n\t");
181 asm volatile(
"push r18 \n\t");
182 asm volatile(
"push r19 \n\t");
183 asm volatile(
"push r20 \n\t");
184 asm volatile(
"push r21 \n\t");
185 asm volatile(
"push r22 \n\t");
186 asm volatile(
"push r23 \n\t");
187 asm volatile(
"push r24 \n\t");
188 asm volatile(
"push r25 \n\t");
189 asm volatile(
"push r26 \n\t");
190 asm volatile(
"push r27 \n\t");
191 asm volatile(
"push r28 \n\t");
192 asm volatile(
"push r29 \n\t");
201 asm volatile(
"nop \n\t");
202 asm volatile(
"nop \n\t");
203 asm volatile(
"nop \n\t");
204 asm volatile(
"nop \n\t");
205 asm volatile(
"nop \n\t");
206 asm volatile(
"nop \n\t");
207 asm volatile(
"nop \n\t");
208 asm volatile(
"nop \n\t");
209 asm volatile(
"nop \n\t");
210 asm volatile(
"nop \n\t");
211 asm volatile(
"nop \n\t");
212 asm volatile(
"nop \n\t");
213 asm volatile(
"nop \n\t");
214 asm volatile(
"nop \n\t");
215 asm volatile(
"nop \n\t");
216 asm volatile(
"nop \n\t");
217 asm volatile(
"nop \n\t");
234 asm volatile(
"pop r29 \n\t");
235 asm volatile(
"pop r28 \n\t");
236 asm volatile(
"pop r27 \n\t");
237 asm volatile(
"pop r26 \n\t");
238 asm volatile(
"pop r25 \n\t");
239 asm volatile(
"pop r24 \n\t");
240 asm volatile(
"pop r23 \n\t");
241 asm volatile(
"pop r22 \n\t");
242 asm volatile(
"pop r21 \n\t");
243 asm volatile(
"pop r20 \n\t");
244 asm volatile(
"pop r19 \n\t");
245 asm volatile(
"pop r18 \n\t");
246 asm volatile(
"pop r17 \n\t");
247 asm volatile(
"pop r15 \n\t");
248 asm volatile(
"pop r14 \n\t");
249 asm volatile(
"pop r13 \n\t");
250 asm volatile(
"pop r12 \n\t");
251 asm volatile(
"pop r11 \n\t");
252 asm volatile(
"pop r10 \n\t");
253 asm volatile(
"pop r9 \n\t");
254 asm volatile(
"pop r8 \n\t");
255 asm volatile(
"pop r7 \n\t");
256 asm volatile(
"pop r6 \n\t");
257 asm volatile(
"pop r5 \n\t");
258 asm volatile(
"pop r4 \n\t");
259 asm volatile(
"pop r3 \n\t");
260 asm volatile(
"pop r2 \n\t");
261 asm volatile(
"pop r1 \n\t");
262 asm volatile(
"pop r0 \n\t");
263 asm volatile(
"pop r31 \n\t");
264 asm volatile(
"pop r30 \n\t");
265 asm volatile(
"pop r16 \n\t");
266 asm volatile(
"out 0x3F,r16 \n\t");
267 asm volatile(
"pop r16 \n\t");
268 asm volatile(
"reti \n\t");
277 static float deltaSpeedAngle = 0.0;
278 static uint8_t loops = 0;
299 curAngle = (((uint16_t)data[0]) << 8 ) | (uint16_t)data[1];
309 if(deltaAngle < -2047)
315 else if(deltaAngle > 2047)
324 deltaSpeedAngle += (float)deltaAngle;
335 deltaSpeedAngle = 0.0;
348 float float2::getFloatValue(
void)
356 a.i = (uint32_t)(this->value >> 25);
361 uint64_t float2::getRawValue(
void)
366 void float2::setValue(
float val)
376 this->value = ((uint64_t)a.i) << 25;
379 bool float2::operator<=(
const float &value)
381 if(this->getFloatValue() > value)
386 if(this->getFloatValue() == value)
388 if((this->value & 0x0000000000007FFF) > 0)
397 bool float2::operator<=(
const float2 &value)
399 if((this->value >> 56) > (value.value >> 56))
404 if((this->value >> 56) == (value.value >> 56))
406 if( (this->value >> 48) < (value.value >> 48) )
411 if( (this->value >> 48) == (value.value >> 48) )
413 if((this->value & 0x0000FFFFFFFFFFFF) <= (value.value & 0x0000FFFFFFFFFFFF))
423 float2 & float2::operator=(
const float &value)
425 this->setValue(value);
430 float2 & float2::operator+=(
const float &value)
438 uint64_t tempMant, tempExp;
441 if((this->value >> 56) == (temp.value >> 56))
445 cnt = (temp.value >> 48) - (this->value >> 48);
448 tempExp = (temp.value >> 48);
450 this->value &= 0x0000FFFFFFFFFFFF;
451 this->value |= 0x0001000000000000;
454 tempMant = (temp.value & 0x0000FFFFFFFFFFFF) | 0x0001000000000000;
455 tempMant += this->value;
457 while(tempMant > 0x2000000000000)
463 tempMant &= 0x0000FFFFFFFFFFFF;
464 this->value = (tempExp << 48) | tempMant;
468 this->value = temp.value;
474 cnt = (this->value >> 48) - (temp.value >> 48);
478 tempExp = (this->value >> 48);
480 temp.value &= 0x0000FFFFFFFFFFFF;
481 temp.value |= 0x0001000000000000;
484 tempMant = (this->value & 0x0000FFFFFFFFFFFF) | 0x0001000000000000;
485 tempMant += temp.value;
487 while(tempMant > 0x2000000000000)
493 tempMant &= 0x0000FFFFFFFFFFFF;
494 this->value = (tempExp << 48) | tempMant;
499 else if((this->value >> 56) == 1)
501 this->value &= 0x00FFFFFFFFFFFFFF;
505 cnt = (temp.value >> 48) - (this->value >> 48);
509 tempExp = (temp.value >> 48);
511 this->value &= 0x0000FFFFFFFFFFFF;
512 this->value |= 0x0001000000000000;
515 tempMant = (temp.value & 0x0000FFFFFFFFFFFF) | 0x0001000000000000;
517 tempMant -= this->value;
519 if(tempMant > 0x8000000000000000)
522 tempMant &= 0x0000FFFFFFFFFFFF;
526 while(tempMant < 0x1000000000000)
532 tempMant &= 0x0000FFFFFFFFFFFF;
534 this->value = (tempExp << 48) | tempMant;
539 this->value = temp.value;
545 cnt = (this->value >> 48) - (temp.value >> 48);
548 tempExp = (this->value >> 48);
550 temp.value &= 0x0000FFFFFFFFFFFF;
551 temp.value |= 0x0001000000000000;
554 tempMant = (this->value & 0x0000FFFFFFFFFFFF) | 0x0001000000000000;
556 tempMant -= temp.value;
558 if(tempMant > 0x8000000000000000)
560 tempMant &= 0x0000FFFFFFFFFFFF;
564 while(tempMant < 0x1000000000000)
570 tempMant &= 0x0000FFFFFFFFFFFF;
572 this->value = (tempExp << 48) | tempMant;
573 this->value |= 0x0100000000000000;
580 temp.value &= 0x00FFFFFFFFFFFFFF;
584 cnt = (this->value >> 48) - (temp.value >> 48);
587 tempExp = (this->value >> 48);
589 temp.value &= 0x0000FFFFFFFFFFFF;
590 temp.value |= 0x0001000000000000;
593 tempMant = (this->value & 0x0000FFFFFFFFFFFF) | 0x0001000000000000;
595 tempMant -= temp.value;
597 if(tempMant > 0x8000000000000000)
599 tempMant &= 0x0000FFFFFFFFFFFF;
603 while(tempMant < 0x1000000000000)
609 tempMant &= 0x0000FFFFFFFFFFFF;
611 this->value = (tempExp << 48) | tempMant;
617 cnt = (temp.value >> 48) - (this->value >> 48);
620 tempExp = (temp.value >> 48);
622 this->value &= 0x0000FFFFFFFFFFFF;
623 this->value |= 0x0001000000000000;
626 tempMant = (temp.value & 0x0000FFFFFFFFFFFF) | 0x0001000000000000;
628 tempMant -= this->value;
630 if(tempMant > 0x8000000000000000)
632 tempMant &= 0x0000FFFFFFFFFFFF;
636 while(tempMant < 0x1000000000000)
642 tempMant &= 0x0000FFFFFFFFFFFF;
644 this->value = (tempExp << 48) | tempMant;
645 this->value |= 0x0100000000000000;
650 this->value = temp.value;
651 this->value |= 0x0100000000000000;
671 Vout = analogRead(TEMP)*0.0048828125;
672 NTC = ((
R*5.0)/Vout)-
R;
674 T =
A +
B*NTC +
C*NTC*NTC*NTC;
687 return (
float)this->angleMoved*0.087890625;
692 return this->curSpeed;
710 TIMSK1 = (1 << OCIE1A);
711 TCCR1A = (1 << WGM11);
712 TCCR1B = (1 << WGM12) | (1 << WGM13) | (1 << CS10);
722 this->encoderOffset = (((uint16_t)data[0]) << 8 ) | (uint16_t)data[1];
727 this->angleMoved = 0;
728 this->revolutions = 0;
734 return (
float)this->angle*0.087890625;
743 return (((uint16_t)data[0]) << 8 )| (uint16_t)data[1];
768 else if(data == 0x10)
773 else if(data == 0x20)
786 this->setMaxAcceleration(1000.0);
787 this->setMaxVelocity(1000.0);
789 p = &(this->control);
801 this->setMaxVelocity(vel);
802 this->setMaxAcceleration(accel);
804 p = &(this->control);
814 this->acceleration = accel;
817 this->multiplier.setValue((this->acceleration/(
INTFREQ*
INTFREQ)));
819 if(this->state !=
STOP)
821 if(this->continous == 1)
823 this->runContinous(this->direction);
827 this->moveSteps(this->totalSteps - this->currentStep + 1, this->direction, this->hold);
834 return this->acceleration;
841 this->velocity = 0.5005;
844 else if(vel > 28000.0)
846 this->velocity = 28000.0;
851 this->velocity = vel;
855 this->cruiseDelay = (uint16_t)((
INTFREQ/this->velocity) - 0.5);
857 if(this->state !=
STOP)
859 if(this->continous == 1)
861 this->runContinous(this->direction);
865 this->moveSteps(this->totalSteps - this->currentStep + 1, this->direction, this->hold);
872 return this->velocity;
889 curVel =
INTFREQ/this->exactDelay.getFloatValue();
890 if(dir != digitalRead(DIR))
892 this->direction = dir;
894 this->initialDecelSteps = (uint32_t)(((curVel*curVel))/(2.0*this->acceleration));
895 this->accelSteps = (uint32_t)((this->velocity*this->velocity)/(2.0*this->acceleration));
897 this->exactDelay.setValue(
INTFREQ/sqrt((curVel*curVel) + 2.0*this->acceleration));
899 if(this->exactDelay.getFloatValue() >= 65535.5)
901 this->delay = 0xFFFF;
905 this->delay = (uint16_t)(this->exactDelay.getFloatValue() - 0.5);
910 if(curVel > this->velocity)
913 this->initialDecelSteps = (uint32_t)(((this->velocity*this->velocity) - (curVel*curVel))/(-2.0*this->acceleration));
914 this->accelSteps = 0;
917 else if(curVel < this->velocity)
920 this->accelSteps = (uint32_t)(((this->velocity*this->velocity) - (curVel*curVel))/(2.0*this->acceleration));
932 this->direction = dir;
942 this->accelSteps = (velocity*velocity)/(2.0*acceleration);
944 this->exactDelay.setValue(
INTFREQ/sqrt(2.0*this->acceleration));
946 if(this->exactDelay.getFloatValue() > 65535.0)
948 this->delay = 0xFFFF;
952 this->delay = (uint16_t)(this->exactDelay.getFloatValue() - 0.5);
975 else if(holdMode ==
SOFT)
977 this->disableMotor();
984 this->direction = dir;
985 this->hold = holdMode;
986 this->totalSteps = steps;
991 curVel =
INTFREQ/this->exactDelay.getFloatValue();
993 if(dir != digitalRead(DIR))
996 this->initialDecelSteps = (uint32_t)((curVel*curVel)/(2.0*this->acceleration));
997 this->accelSteps = (uint32_t)((this->velocity * this->velocity)/(2.0*this->acceleration));
998 this->totalSteps += this->initialDecelSteps;
1000 if(this->accelSteps > (this->totalSteps >> 1))
1002 this->accelSteps = this->decelSteps = (this->totalSteps >> 1);
1003 this->accelSteps += this->totalSteps - this->accelSteps - this->decelSteps;
1007 this->decelSteps = this->accelSteps;
1008 this->cruiseSteps = this->totalSteps - this->accelSteps - this->decelSteps;
1011 this->exactDelay.setValue(
INTFREQ/sqrt((curVel*curVel) + 2.0*this->acceleration));
1013 if(this->exactDelay.getFloatValue() >= 65535.5)
1015 this->delay = 0xFFFF;
1019 this->delay = (uint16_t)(this->exactDelay.getFloatValue() - 0.5);
1024 if(curVel > this->velocity)
1027 this->initialDecelSteps = (uint32_t)(((this->velocity*this->velocity) - (curVel*curVel))/(-2.0*this->acceleration));
1028 this->accelSteps = 0;
1029 this->decelSteps = (uint32_t)((this->velocity*this->velocity)/(2.0*this->acceleration));
1030 this->exactDelay.setValue((
INTFREQ/sqrt((curVel*curVel) + 2*this->acceleration)));
1032 if(this->totalSteps <= (this->initialDecelSteps + this->decelSteps))
1034 this->cruiseSteps = 0;
1038 this->cruiseSteps = steps - this->initialDecelSteps - this->decelSteps;
1044 else if(curVel < this->velocity)
1046 this->state =
ACCEL;
1047 this->accelSteps = (uint32_t)(((this->velocity*this->velocity) - (curVel*curVel))/(2.0*this->acceleration));
1049 if(this->accelSteps > (this->totalSteps >> 1))
1051 this->accelSteps = this->decelSteps = (this->totalSteps >> 1);
1052 this->accelSteps += this->totalSteps - this->accelSteps - this->decelSteps;
1053 this->cruiseSteps = 0;
1057 this->decelSteps = this->accelSteps;
1058 this->cruiseSteps = this->totalSteps - this->accelSteps - this->decelSteps;
1061 this->cruiseSteps = steps - this->accelSteps - this->decelSteps;
1062 this->initialDecelSteps = 0;
1068 this->decelSteps = (uint32_t)((this->velocity*this->velocity)/(2.0*this->acceleration));
1069 this->accelSteps = 0;
1070 this->initialDecelSteps = 0;
1072 if(this->decelSteps >= this->totalSteps)
1074 this->cruiseSteps = 0;
1078 this->cruiseSteps = steps - this->decelSteps;
1094 this->state =
ACCEL;
1095 this->accelSteps = (uint32_t)((this->velocity * this->velocity)/(2.0*this->acceleration));
1096 this->initialDecelSteps = 0;
1098 if(this->accelSteps > (steps >> 1))
1100 this->cruiseSteps = 0;
1101 this->accelSteps = this->decelSteps = (steps >> 1);
1102 this->accelSteps += steps - this->accelSteps - this->decelSteps;
1107 this->decelSteps = this->accelSteps;
1108 this->cruiseSteps = steps - this->accelSteps - this->decelSteps;
1110 this->exactDelay.setValue(
INTFREQ/sqrt(2.0*this->acceleration));
1112 if(this->exactDelay.getFloatValue() > 65535.0)
1114 this->delay = 0xFFFF;
1118 this->delay = (uint16_t)(this->exactDelay.getFloatValue() - 0.5);
1123 this->enableMotor();
1134 this->hold = holdMode;
1145 if(holdMode ==
SOFT)
1147 this->disableMotor();
1150 else if (holdMode ==
HARD)
1152 this->enableMotor();
1167 this->hold = holdMode;
1171 curVel =
INTFREQ/this->exactDelay.getFloatValue();
1173 this->decelSteps = (uint32_t)((curVel*curVel)/(2.0*this->acceleration));
1174 this->accelSteps = this->initialDecelSteps = this->cruiseSteps = 0;
1175 this->state =
DECEL;
1177 this->exactDelay.setValue(
INTFREQ/sqrt(2.0*this->acceleration));
1179 if(this->exactDelay.getFloatValue() > 65535.0)
1181 this->delay = 0xFFFF;
1185 this->delay = (uint16_t)(this->exactDelay.getFloatValue() - 0.5);
1193 if(holdMode ==
SOFT)
1195 this->disableMotor();
1198 else if (holdMode ==
HARD)
1200 this->enableMotor();
1206 uint8_t microStepping,
1207 float faultTolerance,
1208 float faultHysteresis,
1216 this->encoder.setup(mode);
1220 this->encoder.setHome();
1234 digitalWrite(2,HIGH);
1235 digitalWrite(3,HIGH);
1236 digitalWrite(4,HIGH);
1237 attachInterrupt(digitalPinToInterrupt(2),
interrupt0, CHANGE);
1238 attachInterrupt(digitalPinToInterrupt(3),
interrupt1, FALLING);
1240 this->tolerance = faultTolerance;
1241 this->hysteresis = faultHysteresis;
1244 this->pTerm = pTerm/10000.0;
1245 this->iTerm = iTerm/(10000.0*500.0);
1246 this->dTerm = dTerm/(10000.0/500.0);
1249 this->stepConversion = ((float)(200*microStepping))/4096.0;
1250 this->angleToStep = ((float)(200*microStepping))/360.0;
1251 TCCR2B &= ~((1 << CS20) | (1 << CS21) | (1 << CS22) | (1 << WGM22));
1252 TCCR2A &= ~((1 << WGM20) | (1 << WGM21));
1253 TCCR2B |= (1 << CS21)| (1 << WGM22);
1254 TCCR2A |= (1 << WGM21) | (1 << WGM20);
1257 this->enableMotor();
1263 TIFR2 |= (1 << OCF2A);
1264 TIMSK2 |= (1 << OCIE2A);
1271 TIMSK2 &= ~(1 << OCIE2A);
1286 return this->direction;
1291 if(this->state !=
STOP)
1301 if(this->direction ==
CW)
1303 return this->stepsSinceReset;
1307 return this->stepsSinceReset;
1313 this->pwmD8(current);
1318 if(!(TCCR1A & (1 << COM1B1)))
1321 TCCR1A |= (1 << COM1B1);
1335 OCR1B = (uint16_t)(duty + 0.5);
1342 if(!(TCCR1A & (1 << COM1B1)))
1345 TCCR1A |= (1 << COM1B1);
1351 TCCR1A &= ~(1 << COM1B1);
1357 if(!(TCCR2A & (1 << COM2B1)))
1360 TCCR2A |= (1 << COM2B1);
1374 OCR2B = (uint16_t)(duty + 0.5);
1381 if(!(TCCR2A & (1 << COM2B1)))
1384 TCCR2A |= (1 << COM2B1);
1390 TCCR2A &= ~(1 << COM2B1);
1401 this->stepCnt = (int32_t)(setPoint*this->angleToStep);
1409 diff = angle - this->encoder.getAngleMoved();
1410 steps = (uint32_t)((abs(diff)*angleToStep) + 0.5);
1414 this->moveSteps(steps,
CCW, holdMode);
1418 this->moveSteps(steps,
CW, holdMode);
1428 steps = (int32_t)((angle*angleToStep) - 0.5);
1429 this->moveSteps(steps,
CCW, holdMode);
1433 steps = (int32_t)((angle*angleToStep) + 0.5);
1434 this->moveSteps(steps,
CW, holdMode);
1440 static float oldError = 0.0;
1443 static float accumError = 0.0;
1448 static uint32_t speed = 10000;
1449 static uint32_t oldMicros = 0;
1455 error = (float)this->stepCnt;
1456 if(this->speedValue[0] == oldMicros)
1466 speed = this->speedValue[0] - this->speedValue[1];
1470 curAngle = (((uint16_t)data[0]) << 8 ) | (uint16_t)data[1];
1471 this->encoder.angle = curAngle;
1472 curAngle -= this->encoder.encoderOffset;
1478 deltaAngle = (int16_t)this->encoder.oldAngle - (int16_t)curAngle;
1480 if(deltaAngle < -2047)
1482 this->encoder.revolutions--;
1486 else if(deltaAngle > 2047)
1488 this->encoder.revolutions++;
1492 this->encoder.angleMoved = (int32_t)curAngle + (4096*(int32_t)this->encoder.revolutions);
1493 this->encoder.oldAngle = curAngle;
1495 error = (((float)this->encoder.angleMoved * this->stepConversion) - error);
1497 if(error < -this->tolerance)
1500 this->control = (int32_t)error;
1505 integral = error*this->iTerm;
1506 accumError += integral;
1512 output -= this->pTerm*error;
1513 output -= accumError;
1514 output -= this->dTerm*(error - oldError);
1520 output *= (float)speed;
1525 accumError -= integral;
1536 else if(error > this->tolerance)
1539 this->control = (int32_t)error;
1542 integral = error*this->iTerm;
1543 accumError += integral;
1545 output -= this->pTerm*error;
1546 output -= accumError;
1547 output -= this->dTerm*(error - oldError);
1553 output *= (float)speed;
1558 accumError -= integral;
1571 if(error >= -this->hysteresis && error <= this->hysteresis)
1573 PORTB |= (PIND & 0x04) >> 2;
1585 static float oldError = 0.0;
1588 static float accumError = 0.0;
1594 static uint32_t speed = 10000;
1595 static uint32_t oldMicros = 0;
1605 error = (float)this->stepsSinceReset;
1606 if(this->exactDelay.getFloatValue() >= 1.0)
1608 speed = (uint32_t)((this->exactDelay.getFloatValue() *
INTPERIOD));
1621 curAngle = (((uint16_t)data[0]) << 8 ) | (uint16_t)data[1];
1622 this->encoder.angle = curAngle;
1623 curAngle -= this->encoder.encoderOffset;
1629 deltaAngle = (int16_t)this->encoder.oldAngle - (int16_t)curAngle;
1631 if(deltaAngle < -2047)
1633 this->encoder.revolutions--;
1637 else if(deltaAngle > 2047)
1639 this->encoder.revolutions++;
1643 this->encoder.angleMoved = (int32_t)curAngle + (4096*(int32_t)this->encoder.revolutions);
1644 this->encoder.oldAngle = curAngle;
1646 error = (((float)this->encoder.angleMoved * this->stepConversion) - error);
1648 if(error < -this->tolerance)
1651 this->control = (int32_t)error;
1656 integral = error*this->iTerm;
1657 accumError += integral;
1663 output -= this->pTerm*error;
1664 output -= accumError;
1665 output -= this->dTerm*(error - oldError);
1671 output *= (float)speed;
1675 accumError -= integral;
1687 else if(error > this->tolerance)
1690 this->control = (int32_t)error;
1693 integral = error*this->iTerm;
1694 accumError += integral;
1696 output -= this->pTerm*error;
1697 output -= accumError;
1698 output -= this->dTerm*(error - oldError);
1704 output *= (float)speed;
1708 accumError -= integral;
1723 if(error >= -this->hysteresis && error <= this->hysteresis)
1726 if(this->hold || this->state)
1761 while (!(TWCR & (1 << TWINT)))
1771 status = TWSR & 0xF8;
1776 bool i2cMaster::read(uint8_t slaveAddr, uint8_t regAddr, uint8_t numOfBytes, uint8_t *data)
1778 uint8_t i, buff[numOfBytes];
1780 TIMSK1 &= ~(1 << OCIE1A);
1800 for(i = 0; i < (numOfBytes - 1); i++)
1817 TIMSK1 |= (1 << OCIE1A);
1826 TIMSK1 &= ~(1 << OCIE1A);
1840 for(i = 0; i < numOfBytes; i++)
1850 TIMSK1 |= (1 << OCIE1A);
1859 if(this->cmd((1 << TWINT) | (1 << TWEN) | (1 << TWEA)) ==
false)
1868 if(this->cmd((1 << TWINT) | (1 << TWEN)) ==
false)
1882 this->cmd((1<<TWINT) | (1<<TWSTA) | (1<<TWEN));
1884 if (this->getStatus() !=
START && this->getStatus() !=
REPSTART)
1890 TWDR = (addr << 1) | RW;
1891 this->cmd((1 << TWINT) | (1 << TWEN));
1907 return this->start(addr, RW);
1914 this->cmd((1 << TWINT) | (1 << TWEN));
1923 TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWSTO);
1926 while (TWCR & (1 << TWSTO));
uStepper(void)
Constructor of uStepper class.
uStepperEncoder(void)
Constructor.
void interrupt0(void)
Used by dropin feature to take in step pulses.
void TIMER1_COMPA_vect(void) __attribute__((signal
Measures angle and speed of motor.
uint16_t getStrength(void)
Measure the strength of the magnet.
void setCurrent(double current)
Set motor output current.
float getSpeed(void)
Measure the current speed of the motor.
volatile uint16_t counter
bool write(uint8_t slaveAddr, uint8_t regAddr, uint8_t numOfBytes, uint8_t *data)
sets up I2C connection to device, writes a number of data bytes and closes the connection ...
bool getMotorState(void)
Get the current state of the motor.
Prototype of class for accessing all features of the uStepper in a single object. ...
void pid(void)
This method handles the actual PID controller calculations, if enabled.
void TIMER2_COMPA_vect(void) __attribute__((signal
Used to apply step pulses to the motor.
void enableMotor(void)
Enables the stepper driver output stage.
float getMaxAcceleration(void)
Get the value of the maximum motor acceleration.
float getTemp(void)
Request a reading of current temperature.
#define INTPIDDELAYCONSTANT
void updateSetPoint(float setPoint)
Updates setpoint for the motor.
bool getCurrentDirection(void)
Returns the direction the motor is currently configured to rotate.
void pidDropIn(void)
This method handles the actual PID controller calculations for drop-in feature, if enabled...
float getAngleMoved(void)
Measure the angle moved from reference position.
void setup(uint8_t mode=NORMAL, uint8_t microStepping=SIXTEEN, float faultTolerance=10.0, float faultHysteresis=5.0, float pTerm=1.0, float iTerm=0.02, float dterm=0.006, bool setHome=true)
Initializes the different parts of the uStepper object.
void hardStop(bool holdMode)
Stop the motor without deceleration.
void setMaxAcceleration(float accel)
Set the maximum acceleration of the stepper motor.
void startTimer(void)
Starts timer for stepper algorithm.
bool cmd(uint8_t cmd)
Sends commands over the I2C bus.
void setMaxVelocity(float vel)
Sets the maximum rotational velocity of the motor.
float getMaxVelocity(void)
Returns the maximum rotational velocity of the motor.
bool writeByte(uint8_t data)
Writes a byte to a device on the I2C bus.
i2cMaster(void)
Constructor.
void softStop(bool holdMode)
Stop the motor with deceleration.
uint8_t detectMagnet(void)
Detect if magnet is present and within range.
volatile int32_t stepsSinceReset
uint8_t getStatus(void)
Get current I2C status.
void pwmD8(double duty)
Generate PWM signal on digital output 8.
volatile int32_t angleMoved
bool restart(uint8_t addr, bool RW)
Restarts connection between arduino and I2C device.
bool readByte(bool ack, uint8_t *data)
Reads a byte from the I2C bus.
volatile uint32_t speedValue[2]
volatile uint16_t oldAngle
void stopTimer(void)
Stops the timer for the stepper algorithm.
uint8_t getAgc(void)
Read the current AGC value of the encoder chip.
bool start(uint8_t addr, bool RW)
sets up connection between arduino and I2C device.
void runContinous(bool dir)
Make the motor rotate continuously.
#define ENCODERSPEEDCONSTANT
Prototype of class for accessing the TWI (I2C) interface of the AVR (master mode only).
void setHome(void)
Define new reference(home) position.
volatile int16_t revolutions
float getAngle(void)
Measure the current shaft angle.
void moveToAngle(float angle, bool holdMode)
Moves the motor to an absolute angle.
bool stop(void)
Closes the I2C connection.
void moveAngle(float angle, bool holdMode)
Moves the motor to a relative angle.
void begin(void)
Setup TWI (I2C) interface.
void interrupt1(void)
Used by dropin feature to take in enable signal.
Function prototypes and definitions for the uStepper library.
int32_t getStepsSinceReset(void)
Get the number of steps applied since reset.
void setup(uint8_t mode)
Setup the encoder.
void moveSteps(int32_t steps, bool dir, bool holdMode)
Make the motor perform a predefined number of steps.
void pwmD3(double duty)
Generate PWM signal on digital output 3.
uStepperTemp(void)
Constructor.
bool read(uint8_t slaveAddr, uint8_t regAddr, uint8_t numOfBytes, uint8_t *data)
sets up I2C connection to device, reads a number of data bytes and closes the connection ...
void disableMotor(void)
Disables the stepper driver output stage.