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 if(!(TCCR1A & (1 << COM1B1)))
1300 TCCR1A |= (1 << COM1B1);
1314 OCR1B = (uint16_t)(duty + 0.5);
1321 if(!(TCCR1A & (1 << COM1B1)))
1324 TCCR1A |= (1 << COM1B1);
1330 TCCR1A &= ~(1 << COM1B1);
1336 if(!(TCCR2A & (1 << COM2B1)))
1339 TCCR2A |= (1 << COM2B1);
1353 OCR2B = (uint16_t)(duty + 0.5);
1360 if(!(TCCR2A & (1 << COM2B1)))
1363 TCCR2A |= (1 << COM2B1);
1369 TCCR2A &= ~(1 << COM2B1);
1380 this->stepCnt = (int32_t)(setPoint*this->angleToStep);
1385 static float oldError = 0.0;
1388 static float accumError = 0.0;
1393 static uint32_t speed = 10000;
1394 static uint32_t oldMicros = 0;
1400 error = (float)this->stepCnt;
1401 if(this->speedValue[0] == oldMicros)
1411 speed = this->speedValue[0] - this->speedValue[1];
1415 curAngle = (((uint16_t)data[0]) << 8 ) | (uint16_t)data[1];
1416 this->encoder.angle = curAngle;
1417 curAngle -= this->encoder.encoderOffset;
1423 deltaAngle = (int16_t)this->encoder.oldAngle - (int16_t)curAngle;
1425 if(deltaAngle < -2047)
1427 this->encoder.revolutions--;
1431 else if(deltaAngle > 2047)
1433 this->encoder.revolutions++;
1437 this->encoder.angleMoved = (int32_t)curAngle + (4096*(int32_t)this->encoder.revolutions);
1438 this->encoder.oldAngle = curAngle;
1440 error = (((float)this->encoder.angleMoved * this->stepConversion) - error);
1442 if(error < -this->tolerance)
1445 this->control = (int32_t)error;
1450 integral = error*this->iTerm;
1451 accumError += integral;
1457 output -= this->pTerm*error;
1458 output -= accumError;
1459 output -= this->dTerm*(error - oldError);
1465 output *= (float)speed;
1470 accumError -= integral;
1481 else if(error > this->tolerance)
1484 this->control = (int32_t)error;
1487 integral = error*this->iTerm;
1488 accumError += integral;
1490 output -= this->pTerm*error;
1491 output -= accumError;
1492 output -= this->dTerm*(error - oldError);
1498 output *= (float)speed;
1501 if(output < cruiseDelay)
1503 output = cruiseDelay;
1504 accumError -= integral;
1508 this->delay = output;
1517 if(error >= -this->hysteresis && error <= this->hysteresis)
1519 PORTB |= (PIND & 0x04) >> 2;
1531 static float oldError = 0.0;
1534 static float accumError = 0.0;
1540 static uint32_t speed = 10000;
1541 static uint32_t oldMicros = 0;
1551 error = (float)this->stepsSinceReset;
1552 if(this->exactDelay.getFloatValue() >= 1.0)
1554 speed = (uint32_t)((this->exactDelay.getFloatValue() *
INTPERIOD));
1567 curAngle = (((uint16_t)data[0]) << 8 ) | (uint16_t)data[1];
1568 this->encoder.angle = curAngle;
1569 curAngle -= this->encoder.encoderOffset;
1575 deltaAngle = (int16_t)this->encoder.oldAngle - (int16_t)curAngle;
1577 if(deltaAngle < -2047)
1579 this->encoder.revolutions--;
1583 else if(deltaAngle > 2047)
1585 this->encoder.revolutions++;
1589 this->encoder.angleMoved = (int32_t)curAngle + (4096*(int32_t)this->encoder.revolutions);
1590 this->encoder.oldAngle = curAngle;
1592 error = (((float)this->encoder.angleMoved * this->stepConversion) - error);
1594 if(error < -this->tolerance)
1597 this->control = (int32_t)error;
1602 integral = error*this->iTerm;
1603 accumError += integral;
1609 output -= this->pTerm*error;
1610 output -= accumError;
1611 output -= this->dTerm*(error - oldError);
1617 output *= (float)speed;
1620 if(output < cruiseDelay)
1622 accumError -= cruiseDelay-output;
1623 output = cruiseDelay;
1627 this->delay = output;
1634 else if(error > this->tolerance)
1637 this->control = (int32_t)error;
1640 integral = error*this->iTerm;
1641 accumError += integral;
1643 output -= this->pTerm*error;
1644 output -= accumError;
1645 output -= this->dTerm*(error - oldError);
1651 output *= (float)speed;
1654 if(output < cruiseDelay)
1656 accumError -= cruiseDelay-output;
1657 output = cruiseDelay;
1661 this->delay = output;
1670 if(error >= -this->hysteresis && error <= this->hysteresis)
1673 if(this->hold || this->state)
1708 while (!(TWCR & (1 << TWINT)))
1718 status = TWSR & 0xF8;
1723 bool i2cMaster::read(uint8_t slaveAddr, uint8_t regAddr, uint8_t numOfBytes, uint8_t *data)
1725 uint8_t i, buff[numOfBytes];
1727 TIMSK1 &= ~(1 << OCIE1A);
1747 for(i = 0; i < (numOfBytes - 1); i++)
1764 TIMSK1 |= (1 << OCIE1A);
1773 TIMSK1 &= ~(1 << OCIE1A);
1787 for(i = 0; i < numOfBytes; i++)
1797 TIMSK1 |= (1 << OCIE1A);
1806 if(this->cmd((1 << TWINT) | (1 << TWEN) | (1 << TWEA)) ==
false)
1815 if(this->cmd((1 << TWINT) | (1 << TWEN)) ==
false)
1829 this->cmd((1<<TWINT) | (1<<TWSTA) | (1<<TWEN));
1831 if (this->getStatus() !=
START && this->getStatus() !=
REPSTART)
1837 TWDR = (addr << 1) | RW;
1838 this->cmd((1 << TWINT) | (1 << TWEN));
1854 return this->start(addr, RW);
1861 this->cmd((1 << TWINT) | (1 << TWEN));
1870 TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWSTO);
1873 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.