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);
714 this->encoderOffset = (((uint16_t)data[0]) << 8 ) | (uint16_t)data[1];
716 this->angleMoved = 0;
717 this->revolutions = 0;
727 this->encoderOffset = (((uint16_t)data[0]) << 8 ) | (uint16_t)data[1];
732 this->angleMoved = 0;
733 this->revolutions = 0;
739 return (
float)this->angle*0.087890625;
748 return (((uint16_t)data[0]) << 8 )| (uint16_t)data[1];
773 else if(data == 0x10)
778 else if(data == 0x20)
791 this->setMaxAcceleration(1000.0);
792 this->setMaxVelocity(1000.0);
794 p = &(this->control);
806 this->setMaxVelocity(vel);
807 this->setMaxAcceleration(accel);
809 p = &(this->control);
819 this->acceleration = accel;
822 this->multiplier.setValue((this->acceleration/(
INTFREQ*
INTFREQ)));
824 if(this->state !=
STOP)
826 if(this->continous == 1)
828 this->runContinous(this->direction);
832 this->moveSteps(this->totalSteps - this->currentStep + 1, this->direction, this->hold);
839 return this->acceleration;
846 this->velocity = 0.5005;
849 else if(vel > 28000.0)
851 this->velocity = 28000.0;
856 this->velocity = vel;
860 this->cruiseDelay = (uint16_t)((
INTFREQ/this->velocity) - 0.5);
862 if(this->state !=
STOP)
864 if(this->continous == 1)
866 this->runContinous(this->direction);
870 this->moveSteps(this->totalSteps - this->currentStep + 1, this->direction, this->hold);
877 return this->velocity;
894 curVel =
INTFREQ/this->exactDelay.getFloatValue();
895 if(dir != digitalRead(DIR))
897 this->direction = dir;
899 this->initialDecelSteps = (uint32_t)(((curVel*curVel))/(2.0*this->acceleration));
900 this->accelSteps = (uint32_t)((this->velocity*this->velocity)/(2.0*this->acceleration));
902 this->exactDelay.setValue(
INTFREQ/sqrt((curVel*curVel) + 2.0*this->acceleration));
904 if(this->exactDelay.getFloatValue() >= 65535.5)
906 this->delay = 0xFFFF;
910 this->delay = (uint16_t)(this->exactDelay.getFloatValue() - 0.5);
915 if(curVel > this->velocity)
918 this->initialDecelSteps = (uint32_t)(((this->velocity*this->velocity) - (curVel*curVel))/(-2.0*this->acceleration));
919 this->accelSteps = 0;
922 else if(curVel < this->velocity)
925 this->accelSteps = (uint32_t)(((this->velocity*this->velocity) - (curVel*curVel))/(2.0*this->acceleration));
937 this->direction = dir;
947 this->accelSteps = (velocity*velocity)/(2.0*acceleration);
949 this->exactDelay.setValue(
INTFREQ/sqrt(2.0*this->acceleration));
951 if(this->exactDelay.getFloatValue() > 65535.0)
953 this->delay = 0xFFFF;
957 this->delay = (uint16_t)(this->exactDelay.getFloatValue() - 0.5);
976 this->direction = dir;
977 this->hold = holdMode;
978 this->totalSteps = steps;
983 curVel =
INTFREQ/this->exactDelay.getFloatValue();
985 if(dir != digitalRead(DIR))
988 this->initialDecelSteps = (uint32_t)((curVel*curVel)/(2.0*this->acceleration));
989 this->accelSteps = (uint32_t)((this->velocity * this->velocity)/(2.0*this->acceleration));
990 this->totalSteps += this->initialDecelSteps;
992 if(this->accelSteps > (this->totalSteps >> 1))
994 this->accelSteps = this->decelSteps = (this->totalSteps >> 1);
995 this->accelSteps += this->totalSteps - this->accelSteps - this->decelSteps;
999 this->decelSteps = this->accelSteps;
1000 this->cruiseSteps = this->totalSteps - this->accelSteps - this->decelSteps;
1003 this->exactDelay.setValue(
INTFREQ/sqrt((curVel*curVel) + 2.0*this->acceleration));
1005 if(this->exactDelay.getFloatValue() >= 65535.5)
1007 this->delay = 0xFFFF;
1011 this->delay = (uint16_t)(this->exactDelay.getFloatValue() - 0.5);
1016 if(curVel > this->velocity)
1019 this->initialDecelSteps = (uint32_t)(((this->velocity*this->velocity) - (curVel*curVel))/(-2.0*this->acceleration));
1020 this->accelSteps = 0;
1021 this->decelSteps = (uint32_t)((this->velocity*this->velocity)/(2.0*this->acceleration));
1022 this->exactDelay.setValue((
INTFREQ/sqrt((curVel*curVel) + 2*this->acceleration)));
1024 if(this->totalSteps <= (this->initialDecelSteps + this->decelSteps))
1026 this->cruiseSteps = 0;
1030 this->cruiseSteps = steps - this->initialDecelSteps - this->decelSteps;
1036 else if(curVel < this->velocity)
1038 this->state =
ACCEL;
1039 this->accelSteps = (uint32_t)(((this->velocity*this->velocity) - (curVel*curVel))/(2.0*this->acceleration));
1041 if(this->accelSteps > (this->totalSteps >> 1))
1043 this->accelSteps = this->decelSteps = (this->totalSteps >> 1);
1044 this->accelSteps += this->totalSteps - this->accelSteps - this->decelSteps;
1045 this->cruiseSteps = 0;
1049 this->decelSteps = this->accelSteps;
1050 this->cruiseSteps = this->totalSteps - this->accelSteps - this->decelSteps;
1053 this->cruiseSteps = steps - this->accelSteps - this->decelSteps;
1054 this->initialDecelSteps = 0;
1060 this->decelSteps = (uint32_t)((this->velocity*this->velocity)/(2.0*this->acceleration));
1061 this->accelSteps = 0;
1062 this->initialDecelSteps = 0;
1064 if(this->decelSteps >= this->totalSteps)
1066 this->cruiseSteps = 0;
1070 this->cruiseSteps = steps - this->decelSteps;
1086 this->state =
ACCEL;
1087 this->accelSteps = (uint32_t)((this->velocity * this->velocity)/(2.0*this->acceleration));
1088 this->initialDecelSteps = 0;
1090 if(this->accelSteps > (steps >> 1))
1092 this->cruiseSteps = 0;
1093 this->accelSteps = this->decelSteps = (steps >> 1);
1094 this->accelSteps += steps - this->accelSteps - this->decelSteps;
1099 this->decelSteps = this->accelSteps;
1100 this->cruiseSteps = steps - this->accelSteps - this->decelSteps;
1102 this->exactDelay.setValue(
INTFREQ/sqrt(2.0*this->acceleration));
1104 if(this->exactDelay.getFloatValue() > 65535.0)
1106 this->delay = 0xFFFF;
1110 this->delay = (uint16_t)(this->exactDelay.getFloatValue() - 0.5);
1115 this->enableMotor();
1126 this->hold = holdMode;
1137 if(holdMode ==
SOFT)
1139 this->disableMotor();
1142 else if (holdMode ==
HARD)
1144 this->enableMotor();
1159 this->hold = holdMode;
1163 curVel =
INTFREQ/this->exactDelay.getFloatValue();
1165 this->decelSteps = (uint32_t)((curVel*curVel)/(2.0*this->acceleration));
1166 this->accelSteps = this->initialDecelSteps = this->cruiseSteps = 0;
1167 this->state =
DECEL;
1169 this->exactDelay.setValue(
INTFREQ/sqrt(2.0*this->acceleration));
1171 if(this->exactDelay.getFloatValue() > 65535.0)
1173 this->delay = 0xFFFF;
1177 this->delay = (uint16_t)(this->exactDelay.getFloatValue() - 0.5);
1185 if(holdMode ==
SOFT)
1187 this->disableMotor();
1190 else if (holdMode ==
HARD)
1192 this->enableMotor();
1198 uint8_t microStepping,
1199 float faultTolerance,
1200 float faultHysteresis,
1215 digitalWrite(2,HIGH);
1216 digitalWrite(3,HIGH);
1217 digitalWrite(4,HIGH);
1218 attachInterrupt(digitalPinToInterrupt(2),
interrupt0, CHANGE);
1219 attachInterrupt(digitalPinToInterrupt(3),
interrupt1, FALLING);
1222 this->stepConversion = ((float)(200*microStepping))/4096.0;
1223 this->tolerance = faultTolerance;
1224 this->hysteresis = faultHysteresis;
1225 this->angleToStep = ((float)(200*microStepping))/360.0;
1228 this->pTerm = pTerm/10000.0;
1229 this->iTerm = iTerm/(10000.0*500.0);
1230 this->dTerm = dTerm/(10000.0/500.0);
1232 this->stepsSinceReset=0;
1233 TCCR2B &= ~((1 << CS20) | (1 << CS21) | (1 << CS22) | (1 << WGM22));
1234 TCCR2A &= ~((1 << WGM20) | (1 << WGM21));
1235 TCCR2B |= (1 << CS21)| (1 << WGM22);
1236 TCCR2A |= (1 << WGM21) | (1 << WGM20);
1239 this->enableMotor();
1240 this->encoder.setup(mode);
1247 TIFR2 |= (1 << OCF2A);
1248 TIMSK2 |= (1 << OCIE2A);
1255 TIMSK2 &= ~(1 << OCIE2A);
1270 return this->direction;
1275 if(this->state !=
STOP)
1285 if(this->direction ==
CW)
1287 return this->stepsSinceReset;
1291 return this->stepsSinceReset;
1297 this->pwmD8(current);
1302 if(!(TCCR1A & (1 << COM1B1)))
1305 TCCR1A |= (1 << COM1B1);
1319 OCR1B = (uint16_t)(duty + 0.5);
1326 if(!(TCCR1A & (1 << COM1B1)))
1329 TCCR1A |= (1 << COM1B1);
1335 TCCR1A &= ~(1 << COM1B1);
1341 if(!(TCCR2A & (1 << COM2B1)))
1344 TCCR2A |= (1 << COM2B1);
1358 OCR2B = (uint16_t)(duty + 0.5);
1365 if(!(TCCR2A & (1 << COM2B1)))
1368 TCCR2A |= (1 << COM2B1);
1374 TCCR2A &= ~(1 << COM2B1);
1385 this->stepCnt = (int32_t)(setPoint*this->angleToStep);
1390 static float oldError = 0.0;
1393 static float accumError = 0.0;
1398 static uint32_t speed = 10000;
1399 static uint32_t oldMicros = 0;
1405 error = (float)this->stepCnt;
1406 if(this->speedValue[0] == oldMicros)
1416 speed = this->speedValue[0] - this->speedValue[1];
1420 curAngle = (((uint16_t)data[0]) << 8 ) | (uint16_t)data[1];
1421 this->encoder.angle = curAngle;
1422 curAngle -= this->encoder.encoderOffset;
1428 deltaAngle = (int16_t)this->encoder.oldAngle - (int16_t)curAngle;
1430 if(deltaAngle < -2047)
1432 this->encoder.revolutions--;
1436 else if(deltaAngle > 2047)
1438 this->encoder.revolutions++;
1442 this->encoder.angleMoved = (int32_t)curAngle + (4096*(int32_t)this->encoder.revolutions);
1443 this->encoder.oldAngle = curAngle;
1445 error = (((float)this->encoder.angleMoved * this->stepConversion) - error);
1447 if(error < -this->tolerance)
1450 this->control = (int32_t)error;
1455 integral = error*this->iTerm;
1456 accumError += integral;
1462 output -= this->pTerm*error;
1463 output -= accumError;
1464 output -= this->dTerm*(error - oldError);
1470 output *= (float)speed;
1475 accumError -= integral;
1486 else if(error > this->tolerance)
1489 this->control = (int32_t)error;
1492 integral = error*this->iTerm;
1493 accumError += integral;
1495 output -= this->pTerm*error;
1496 output -= accumError;
1497 output -= this->dTerm*(error - oldError);
1503 output *= (float)speed;
1508 accumError -= integral;
1521 if(error >= -this->hysteresis && error <= this->hysteresis)
1523 PORTB |= (PIND & 0x04) >> 2;
1535 static float oldError = 0.0;
1538 static float accumError = 0.0;
1544 static uint32_t speed = 10000;
1545 static uint32_t oldMicros = 0;
1555 error = (float)this->stepsSinceReset;
1556 if(this->exactDelay.getFloatValue() >= 1.0)
1558 speed = (uint32_t)((this->exactDelay.getFloatValue() *
INTPERIOD));
1571 curAngle = (((uint16_t)data[0]) << 8 ) | (uint16_t)data[1];
1572 this->encoder.angle = curAngle;
1573 curAngle -= this->encoder.encoderOffset;
1579 deltaAngle = (int16_t)this->encoder.oldAngle - (int16_t)curAngle;
1581 if(deltaAngle < -2047)
1583 this->encoder.revolutions--;
1587 else if(deltaAngle > 2047)
1589 this->encoder.revolutions++;
1593 this->encoder.angleMoved = (int32_t)curAngle + (4096*(int32_t)this->encoder.revolutions);
1594 this->encoder.oldAngle = curAngle;
1596 error = (((float)this->encoder.angleMoved * this->stepConversion) - error);
1598 if(error < -this->tolerance)
1601 this->control = (int32_t)error;
1606 integral = error*this->iTerm;
1607 accumError += integral;
1613 output -= this->pTerm*error;
1614 output -= accumError;
1615 output -= this->dTerm*(error - oldError);
1621 output *= (float)speed;
1625 accumError -= integral;
1637 else if(error > this->tolerance)
1640 this->control = (int32_t)error;
1643 integral = error*this->iTerm;
1644 accumError += integral;
1646 output -= this->pTerm*error;
1647 output -= accumError;
1648 output -= this->dTerm*(error - oldError);
1654 output *= (float)speed;
1658 accumError -= integral;
1673 if(error >= -this->hysteresis && error <= this->hysteresis)
1676 if(this->hold || this->state)
1711 while (!(TWCR & (1 << TWINT)))
1721 status = TWSR & 0xF8;
1726 bool i2cMaster::read(uint8_t slaveAddr, uint8_t regAddr, uint8_t numOfBytes, uint8_t *data)
1728 uint8_t i, buff[numOfBytes];
1730 TIMSK1 &= ~(1 << OCIE1A);
1750 for(i = 0; i < (numOfBytes - 1); i++)
1767 TIMSK1 |= (1 << OCIE1A);
1776 TIMSK1 &= ~(1 << OCIE1A);
1790 for(i = 0; i < numOfBytes; i++)
1800 TIMSK1 |= (1 << OCIE1A);
1809 if(this->cmd((1 << TWINT) | (1 << TWEN) | (1 << TWEA)) ==
false)
1818 if(this->cmd((1 << TWINT) | (1 << TWEN)) ==
false)
1832 this->cmd((1<<TWINT) | (1<<TWSTA) | (1<<TWEN));
1834 if (this->getStatus() !=
START && this->getStatus() !=
REPSTART)
1840 TWDR = (addr << 1) | RW;
1841 this->cmd((1 << TWINT) | (1 << TWEN));
1857 return this->start(addr, RW);
1864 this->cmd((1 << TWINT) | (1 << TWEN));
1873 TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWSTO);
1876 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.
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 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.
void moveSteps(uint32_t steps, bool dir, bool holdMode)
Make the motor perform a predefined number of steps.
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 setCurrent(double duty)
Set motor output current.
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.
bool stop(void)
Closes the I2C connection.
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 pwmD3(double duty)
Generate PWM signal on digital output 3.
uStepperTemp(void)
Constructor.
void pwmD8(double current)
Generate PWM signal on digital output 8.
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.
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)
Initializes the different parts of the uStepper object.