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];
731 this->angleMoved = 0;
732 this->revolutions = 0;
738 return (
float)this->angle*0.087890625;
747 return (((uint16_t)data[0]) << 8 )| (uint16_t)data[1];
772 else if(data == 0x10)
777 else if(data == 0x20)
790 this->setMaxAcceleration(1000.0);
791 this->setMaxVelocity(1000.0);
793 p = &(this->control);
805 this->setMaxVelocity(vel);
806 this->setMaxAcceleration(accel);
808 p = &(this->control);
818 this->acceleration = accel;
821 this->multiplier.setValue((this->acceleration/(
INTFREQ*
INTFREQ)));
823 if(this->state !=
STOP)
825 if(this->continous == 1)
827 this->runContinous(this->direction);
831 this->moveSteps(this->totalSteps - this->currentStep + 1, this->direction, this->hold);
838 return this->acceleration;
845 this->velocity = 0.5005;
848 else if(vel > 28000.0)
850 this->velocity = 28000.0;
855 this->velocity = vel;
859 this->cruiseDelay = (uint16_t)((
INTFREQ/this->velocity) - 0.5);
861 if(this->state !=
STOP)
863 if(this->continous == 1)
865 this->runContinous(this->direction);
869 this->moveSteps(this->totalSteps - this->currentStep + 1, this->direction, this->hold);
876 return this->velocity;
893 curVel =
INTFREQ/this->exactDelay.getFloatValue();
894 if(dir != digitalRead(DIR))
896 this->direction = dir;
898 this->initialDecelSteps = (uint32_t)(((curVel*curVel))/(2.0*this->acceleration));
899 this->accelSteps = (uint32_t)((this->velocity*this->velocity)/(2.0*this->acceleration));
901 this->exactDelay.setValue(
INTFREQ/sqrt((curVel*curVel) + 2.0*this->acceleration));
903 if(this->exactDelay.getFloatValue() >= 65535.5)
905 this->delay = 0xFFFF;
909 this->delay = (uint16_t)(this->exactDelay.getFloatValue() - 0.5);
914 if(curVel > this->velocity)
917 this->initialDecelSteps = (uint32_t)(((this->velocity*this->velocity) - (curVel*curVel))/(-2.0*this->acceleration));
918 this->accelSteps = 0;
921 else if(curVel < this->velocity)
924 this->accelSteps = (uint32_t)(((this->velocity*this->velocity) - (curVel*curVel))/(2.0*this->acceleration));
936 this->direction = dir;
946 this->accelSteps = (velocity*velocity)/(2.0*acceleration);
948 this->exactDelay.setValue(
INTFREQ/sqrt(2.0*this->acceleration));
950 if(this->exactDelay.getFloatValue() > 65535.0)
952 this->delay = 0xFFFF;
956 this->delay = (uint16_t)(this->exactDelay.getFloatValue() - 0.5);
975 this->direction = dir;
976 this->hold = holdMode;
977 this->totalSteps = steps;
982 curVel =
INTFREQ/this->exactDelay.getFloatValue();
984 if(dir != digitalRead(DIR))
987 this->initialDecelSteps = (uint32_t)((curVel*curVel)/(2.0*this->acceleration));
988 this->accelSteps = (uint32_t)((this->velocity * this->velocity)/(2.0*this->acceleration));
989 this->totalSteps += this->initialDecelSteps;
991 if(this->accelSteps > (this->totalSteps >> 1))
993 this->accelSteps = this->decelSteps = (this->totalSteps >> 1);
994 this->accelSteps += this->totalSteps - this->accelSteps - this->decelSteps;
998 this->decelSteps = this->accelSteps;
999 this->cruiseSteps = this->totalSteps - this->accelSteps - this->decelSteps;
1002 this->exactDelay.setValue(
INTFREQ/sqrt((curVel*curVel) + 2.0*this->acceleration));
1004 if(this->exactDelay.getFloatValue() >= 65535.5)
1006 this->delay = 0xFFFF;
1010 this->delay = (uint16_t)(this->exactDelay.getFloatValue() - 0.5);
1015 if(curVel > this->velocity)
1018 this->initialDecelSteps = (uint32_t)(((this->velocity*this->velocity) - (curVel*curVel))/(-2.0*this->acceleration));
1019 this->accelSteps = 0;
1020 this->decelSteps = (uint32_t)((this->velocity*this->velocity)/(2.0*this->acceleration));
1021 this->exactDelay.setValue((
INTFREQ/sqrt((curVel*curVel) + 2*this->acceleration)));
1023 if(this->totalSteps <= (this->initialDecelSteps + this->decelSteps))
1025 this->cruiseSteps = 0;
1029 this->cruiseSteps = steps - this->initialDecelSteps - this->decelSteps;
1035 else if(curVel < this->velocity)
1037 this->state =
ACCEL;
1038 this->accelSteps = (uint32_t)(((this->velocity*this->velocity) - (curVel*curVel))/(2.0*this->acceleration));
1040 if(this->accelSteps > (this->totalSteps >> 1))
1042 this->accelSteps = this->decelSteps = (this->totalSteps >> 1);
1043 this->accelSteps += this->totalSteps - this->accelSteps - this->decelSteps;
1044 this->cruiseSteps = 0;
1048 this->decelSteps = this->accelSteps;
1049 this->cruiseSteps = this->totalSteps - this->accelSteps - this->decelSteps;
1052 this->cruiseSteps = steps - this->accelSteps - this->decelSteps;
1053 this->initialDecelSteps = 0;
1059 this->decelSteps = (uint32_t)((this->velocity*this->velocity)/(2.0*this->acceleration));
1060 this->accelSteps = 0;
1061 this->initialDecelSteps = 0;
1063 if(this->decelSteps >= this->totalSteps)
1065 this->cruiseSteps = 0;
1069 this->cruiseSteps = steps - this->decelSteps;
1085 this->state =
ACCEL;
1086 this->accelSteps = (uint32_t)((this->velocity * this->velocity)/(2.0*this->acceleration));
1087 this->initialDecelSteps = 0;
1089 if(this->accelSteps > (steps >> 1))
1091 this->cruiseSteps = 0;
1092 this->accelSteps = this->decelSteps = (steps >> 1);
1093 this->accelSteps += steps - this->accelSteps - this->decelSteps;
1098 this->decelSteps = this->accelSteps;
1099 this->cruiseSteps = steps - this->accelSteps - this->decelSteps;
1101 this->exactDelay.setValue(
INTFREQ/sqrt(2.0*this->acceleration));
1103 if(this->exactDelay.getFloatValue() > 65535.0)
1105 this->delay = 0xFFFF;
1109 this->delay = (uint16_t)(this->exactDelay.getFloatValue() - 0.5);
1114 this->enableMotor();
1125 this->hold = holdMode;
1136 if(holdMode ==
SOFT)
1138 this->disableMotor();
1141 else if (holdMode ==
HARD)
1143 this->enableMotor();
1158 this->hold = holdMode;
1162 curVel =
INTFREQ/this->exactDelay.getFloatValue();
1164 this->decelSteps = (uint32_t)((curVel*curVel)/(2.0*this->acceleration));
1165 this->accelSteps = this->initialDecelSteps = this->cruiseSteps = 0;
1166 this->state =
DECEL;
1168 this->exactDelay.setValue(
INTFREQ/sqrt(2.0*this->acceleration));
1170 if(this->exactDelay.getFloatValue() > 65535.0)
1172 this->delay = 0xFFFF;
1176 this->delay = (uint16_t)(this->exactDelay.getFloatValue() - 0.5);
1184 if(holdMode ==
SOFT)
1186 this->disableMotor();
1189 else if (holdMode ==
HARD)
1191 this->enableMotor();
1197 uint8_t microStepping,
1198 float faultTolerance,
1199 float faultHysteresis,
1214 digitalWrite(2,HIGH);
1215 digitalWrite(3,HIGH);
1216 digitalWrite(4,HIGH);
1217 attachInterrupt(digitalPinToInterrupt(2),
interrupt0, CHANGE);
1218 attachInterrupt(digitalPinToInterrupt(3),
interrupt1, FALLING);
1221 this->stepConversion = ((float)(200*microStepping))/4096.0;
1222 this->tolerance = faultTolerance;
1223 this->hysteresis = faultHysteresis;
1224 this->angleToStep = ((float)(200*microStepping))/360.0;
1227 this->pTerm = pTerm/10000.0;
1228 this->iTerm = iTerm/10000.0;
1229 this->dTerm = dTerm/10000.0;
1232 TCCR2B &= ~((1 << CS20) | (1 << CS21) | (1 << CS22) | (1 << WGM22));
1233 TCCR2A &= ~((1 << WGM20) | (1 << WGM21));
1234 TCCR2B |= (1 << CS21)| (1 << WGM22);
1235 TCCR2A |= (1 << WGM21) | (1 << WGM20);
1238 this->enableMotor();
1239 this->encoder.setup(mode);
1246 TIFR2 |= (1 << OCF2A);
1247 TIMSK2 |= (1 << OCIE2A);
1254 TIMSK2 &= ~(1 << OCIE2A);
1269 return this->direction;
1274 if(this->state !=
STOP)
1284 if(this->direction ==
CW)
1286 return this->stepsSinceReset;
1290 return this->stepsSinceReset;
1296 if(!(TCCR1A & (1 << COM1B1)))
1299 TCCR1A |= (1 << COM1B1);
1313 OCR1B = (uint16_t)(duty + 0.5);
1320 if(!(TCCR1A & (1 << COM1B1)))
1323 TCCR1A |= (1 << COM1B1);
1329 TCCR1A &= ~(1 << COM1B1);
1335 if(!(TCCR2A & (1 << COM2B1)))
1338 TCCR2A |= (1 << COM2B1);
1352 OCR2B = (uint16_t)(duty + 0.5);
1359 if(!(TCCR2A & (1 << COM2B1)))
1362 TCCR2A |= (1 << COM2B1);
1368 TCCR2A &= ~(1 << COM2B1);
1379 this->stepCnt = (int32_t)(setPoint*this->angleToStep);
1384 static float oldError = 0.0;
1387 static float accumError = 0.0;
1392 static uint32_t speed = 10000;
1393 static uint32_t oldMicros = 0;
1399 error = (float)this->stepCnt;
1400 if(this->speedValue[0] == oldMicros)
1410 speed = this->speedValue[0] - this->speedValue[1];
1414 curAngle = (((uint16_t)data[0]) << 8 ) | (uint16_t)data[1];
1415 this->encoder.angle = curAngle;
1416 curAngle -= this->encoder.encoderOffset;
1422 deltaAngle = (int16_t)this->encoder.oldAngle - (int16_t)curAngle;
1424 if(deltaAngle < -2047)
1426 this->encoder.revolutions--;
1430 else if(deltaAngle > 2047)
1432 this->encoder.revolutions++;
1436 this->encoder.angleMoved = (int32_t)curAngle + (4096*(int32_t)this->encoder.revolutions);
1437 this->encoder.oldAngle = curAngle;
1439 error = (((float)this->encoder.angleMoved * this->stepConversion) - error);
1441 if(error < -this->tolerance)
1444 this->control = (int32_t)error;
1449 integral = error*this->iTerm;
1450 accumError += integral;
1456 output -= this->pTerm*error;
1457 output -= accumError;
1458 output -= this->dTerm*(error - oldError);
1464 output *= (float)speed;
1469 accumError -= integral;
1480 else if(error > this->tolerance)
1483 this->control = (int32_t)error;
1486 integral = error*this->iTerm;
1487 accumError += integral;
1489 output -= this->pTerm*error;
1490 output -= accumError;
1491 output -= this->dTerm*(error - oldError);
1496 output *= (float)speed;
1501 accumError -= integral;
1514 if(error >= -this->hysteresis && error <= this->hysteresis)
1516 PORTB |= (PIND & 0x04) >> 2;
1528 static float oldError = 0.0;
1531 static float accumError = 0.0;
1537 static uint32_t speed = 10000;
1538 static uint32_t oldMicros = 0;
1548 error = (float)this->stepsSinceReset;
1549 if(this->exactDelay.getFloatValue() >= 1.0)
1551 speed = (uint32_t)((this->exactDelay.getFloatValue() *
INTPERIOD));
1564 curAngle = (((uint16_t)data[0]) << 8 ) | (uint16_t)data[1];
1565 this->encoder.angle = curAngle;
1566 curAngle -= this->encoder.encoderOffset;
1572 deltaAngle = (int16_t)this->encoder.oldAngle - (int16_t)curAngle;
1574 if(deltaAngle < -2047)
1576 this->encoder.revolutions--;
1580 else if(deltaAngle > 2047)
1582 this->encoder.revolutions++;
1586 this->encoder.angleMoved = (int32_t)curAngle + (4096*(int32_t)this->encoder.revolutions);
1587 this->encoder.oldAngle = curAngle;
1589 error = (((float)this->encoder.angleMoved * this->stepConversion) - error);
1591 if(error < -this->tolerance)
1594 this->control = (int32_t)error;
1599 integral = error*this->iTerm;
1600 accumError += integral;
1606 output -= this->pTerm*error;
1607 output -= accumError;
1608 output -= this->dTerm*(error - oldError);
1614 output *= (float)speed;
1619 accumError -= integral;
1630 else if(error > this->tolerance)
1633 this->control = (int32_t)error;
1636 integral = error*this->iTerm;
1637 accumError += integral;
1639 output -= this->pTerm*error;
1640 output -= accumError;
1641 output -= this->dTerm*(error - oldError);
1646 output *= (float)speed;
1651 accumError -= integral;
1664 if(error >= -this->hysteresis && error <= this->hysteresis)
1667 if(this->hold || this->state)
1702 while (!(TWCR & (1 << TWINT)))
1712 status = TWSR & 0xF8;
1717 bool i2cMaster::read(uint8_t slaveAddr, uint8_t regAddr, uint8_t numOfBytes, uint8_t *data)
1719 uint8_t i, buff[numOfBytes];
1721 TIMSK1 &= ~(1 << OCIE1A);
1741 for(i = 0; i < (numOfBytes - 1); i++)
1758 TIMSK1 |= (1 << OCIE1A);
1767 TIMSK1 &= ~(1 << OCIE1A);
1781 for(i = 0; i < numOfBytes; i++)
1791 TIMSK1 |= (1 << OCIE1A);
1800 if(this->cmd((1 << TWINT) | (1 << TWEN) | (1 << TWEA)) ==
false)
1809 if(this->cmd((1 << TWINT) | (1 << TWEN)) ==
false)
1823 this->cmd((1<<TWINT) | (1<<TWSTA) | (1<<TWEN));
1825 if (this->getStatus() !=
START && this->getStatus() !=
REPSTART)
1831 TWDR = (addr << 1) | RW;
1832 this->cmd((1 << TWINT) | (1 << TWEN));
1848 return this->start(addr, RW);
1855 this->cmd((1 << TWINT) | (1 << TWEN));
1864 TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWSTO);
1867 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 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.
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.
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.