75 volatile int32_t *p __attribute__((used));
129 asm volatile(
"push r16 \n\t");
130 asm volatile(
"in r16,0x3F \n\t");
131 asm volatile(
"push r16 \n\t");
132 asm volatile(
"push r30 \n\t");
133 asm volatile(
"push r31 \n\t");
134 asm volatile(
"lds r30,p \n\t");
135 asm volatile(
"lds r31,p+1 \n\t");
137 asm volatile(
"ldd r16,z+24 \n\t");
138 asm volatile(
"cpi r16,0x01 \n\t");
139 asm volatile(
"breq _pid \n\t");
141 asm volatile(
"ldd r16,z+0 \n\t");
142 asm volatile(
"cpi r16,0x00 \n\t");
143 asm volatile(
"brne _pid \n\t");
145 asm volatile(
"ldd r16,z+1 \n\t");
146 asm volatile(
"cpi r16,0x00 \n\t");
147 asm volatile(
"brne _pid \n\t");
149 asm volatile(
"ldd r16,z+2 \n\t");
150 asm volatile(
"cpi r16,0x00 \n\t");
151 asm volatile(
"brne _pid \n\t");
153 asm volatile(
"ldd r16,z+3 \n\t");
154 asm volatile(
"cpi r16,0x00 \n\t");
155 asm volatile(
"brne _pid \n\t");
157 asm volatile(
"subi r30,83 \n\t");
158 asm volatile(
"sbci r31,0 \n\t");
160 asm volatile(
"jmp _AccelerationAlgorithm \n\t");
162 asm volatile(
"_pid: \n\t");
163 asm volatile(
"push r0 \n\t");
164 asm volatile(
"push r1 \n\t");
165 asm volatile(
"push r2 \n\t");
166 asm volatile(
"push r3 \n\t");
167 asm volatile(
"push r4 \n\t");
168 asm volatile(
"push r5 \n\t");
169 asm volatile(
"push r6 \n\t");
170 asm volatile(
"push r7 \n\t");
171 asm volatile(
"push r8 \n\t");
172 asm volatile(
"push r9 \n\t");
173 asm volatile(
"push r10 \n\t");
174 asm volatile(
"push r11 \n\t");
175 asm volatile(
"push r12 \n\t");
176 asm volatile(
"push r13 \n\t");
177 asm volatile(
"push r14 \n\t");
178 asm volatile(
"push r15 \n\t");
179 asm volatile(
"push r17 \n\t");
180 asm volatile(
"push r18 \n\t");
181 asm volatile(
"push r19 \n\t");
182 asm volatile(
"push r20 \n\t");
183 asm volatile(
"push r21 \n\t");
184 asm volatile(
"push r22 \n\t");
185 asm volatile(
"push r23 \n\t");
186 asm volatile(
"push r24 \n\t");
187 asm volatile(
"push r25 \n\t");
188 asm volatile(
"push r26 \n\t");
189 asm volatile(
"push r27 \n\t");
190 asm volatile(
"push r28 \n\t");
191 asm volatile(
"push r29 \n\t");
200 asm volatile(
"nop \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");
233 asm volatile(
"pop r29 \n\t");
234 asm volatile(
"pop r28 \n\t");
235 asm volatile(
"pop r27 \n\t");
236 asm volatile(
"pop r26 \n\t");
237 asm volatile(
"pop r25 \n\t");
238 asm volatile(
"pop r24 \n\t");
239 asm volatile(
"pop r23 \n\t");
240 asm volatile(
"pop r22 \n\t");
241 asm volatile(
"pop r21 \n\t");
242 asm volatile(
"pop r20 \n\t");
243 asm volatile(
"pop r19 \n\t");
244 asm volatile(
"pop r18 \n\t");
245 asm volatile(
"pop r17 \n\t");
246 asm volatile(
"pop r15 \n\t");
247 asm volatile(
"pop r14 \n\t");
248 asm volatile(
"pop r13 \n\t");
249 asm volatile(
"pop r12 \n\t");
250 asm volatile(
"pop r11 \n\t");
251 asm volatile(
"pop r10 \n\t");
252 asm volatile(
"pop r9 \n\t");
253 asm volatile(
"pop r8 \n\t");
254 asm volatile(
"pop r7 \n\t");
255 asm volatile(
"pop r6 \n\t");
256 asm volatile(
"pop r5 \n\t");
257 asm volatile(
"pop r4 \n\t");
258 asm volatile(
"pop r3 \n\t");
259 asm volatile(
"pop r2 \n\t");
260 asm volatile(
"pop r1 \n\t");
261 asm volatile(
"pop r0 \n\t");
262 asm volatile(
"pop r31 \n\t");
263 asm volatile(
"pop r30 \n\t");
264 asm volatile(
"pop r16 \n\t");
265 asm volatile(
"out 0x3F,r16 \n\t");
266 asm volatile(
"pop r16 \n\t");
267 asm volatile(
"reti \n\t");
401 static float deltaSpeedAngle = 0.0;
402 static uint8_t loops = 0;
403 static uint16_t oldAngle = 0;
404 static int16_t revolutions = 0;
425 curAngle = (((uint16_t)data[0]) << 8 ) | (uint16_t)data[1];
426 pointer->
encoder.angle = curAngle;
433 deltaAngle = (int16_t)oldAngle - (int16_t)curAngle;
435 if(deltaAngle < -2047)
441 else if(deltaAngle > 2047)
450 deltaSpeedAngle += (float)deltaAngle;
461 deltaSpeedAngle = 0.0;
474 float float2::getFloatValue(
void)
482 a.i = (uint32_t)(this->value >> 25);
487 uint64_t float2::getRawValue(
void)
492 void float2::setValue(
float val)
502 this->value = ((uint64_t)a.i) << 25;
505 bool float2::operator<=(
const float &value)
507 if(this->getFloatValue() > value)
512 if(this->getFloatValue() == value)
514 if((this->value & 0x0000000000007FFF) > 0)
523 bool float2::operator<=(
const float2 &value)
525 if((this->value >> 56) > (value.value >> 56))
530 if((this->value >> 56) == (value.value >> 56))
532 if( (this->value >> 48) < (value.value >> 48) )
537 if( (this->value >> 48) == (value.value >> 48) )
539 if((this->value & 0x0000FFFFFFFFFFFF) <= (value.value & 0x0000FFFFFFFFFFFF))
549 float2 & float2::operator=(
const float &value)
551 this->setValue(value);
556 float2 & float2::operator+=(
const float &value)
564 uint64_t tempMant, tempExp;
567 if((this->value >> 56) == (temp.value >> 56))
571 cnt = (temp.value >> 48) - (this->value >> 48);
574 tempExp = (temp.value >> 48);
576 this->value &= 0x0000FFFFFFFFFFFF;
577 this->value |= 0x0001000000000000;
580 tempMant = (temp.value & 0x0000FFFFFFFFFFFF) | 0x0001000000000000;
581 tempMant += this->value;
583 while(tempMant > 0x2000000000000)
589 tempMant &= 0x0000FFFFFFFFFFFF;
590 this->value = (tempExp << 48) | tempMant;
594 this->value = temp.value;
600 cnt = (this->value >> 48) - (temp.value >> 48);
604 tempExp = (this->value >> 48);
606 temp.value &= 0x0000FFFFFFFFFFFF;
607 temp.value |= 0x0001000000000000;
610 tempMant = (this->value & 0x0000FFFFFFFFFFFF) | 0x0001000000000000;
611 tempMant += temp.value;
613 while(tempMant > 0x2000000000000)
619 tempMant &= 0x0000FFFFFFFFFFFF;
620 this->value = (tempExp << 48) | tempMant;
625 else if((this->value >> 56) == 1)
627 this->value &= 0x00FFFFFFFFFFFFFF;
631 cnt = (temp.value >> 48) - (this->value >> 48);
635 tempExp = (temp.value >> 48);
637 this->value &= 0x0000FFFFFFFFFFFF;
638 this->value |= 0x0001000000000000;
641 tempMant = (temp.value & 0x0000FFFFFFFFFFFF) | 0x0001000000000000;
643 tempMant -= this->value;
645 if(tempMant > 0x8000000000000000)
648 tempMant &= 0x0000FFFFFFFFFFFF;
652 while(tempMant < 0x1000000000000)
658 tempMant &= 0x0000FFFFFFFFFFFF;
660 this->value = (tempExp << 48) | tempMant;
665 this->value = temp.value;
671 cnt = (this->value >> 48) - (temp.value >> 48);
674 tempExp = (this->value >> 48);
676 temp.value &= 0x0000FFFFFFFFFFFF;
677 temp.value |= 0x0001000000000000;
680 tempMant = (this->value & 0x0000FFFFFFFFFFFF) | 0x0001000000000000;
682 tempMant -= temp.value;
684 if(tempMant > 0x8000000000000000)
686 tempMant &= 0x0000FFFFFFFFFFFF;
690 while(tempMant < 0x1000000000000)
696 tempMant &= 0x0000FFFFFFFFFFFF;
698 this->value = (tempExp << 48) | tempMant;
699 this->value |= 0x0100000000000000;
706 temp.value &= 0x00FFFFFFFFFFFFFF;
710 cnt = (this->value >> 48) - (temp.value >> 48);
713 tempExp = (this->value >> 48);
715 temp.value &= 0x0000FFFFFFFFFFFF;
716 temp.value |= 0x0001000000000000;
719 tempMant = (this->value & 0x0000FFFFFFFFFFFF) | 0x0001000000000000;
721 tempMant -= temp.value;
723 if(tempMant > 0x8000000000000000)
725 tempMant &= 0x0000FFFFFFFFFFFF;
729 while(tempMant < 0x1000000000000)
735 tempMant &= 0x0000FFFFFFFFFFFF;
737 this->value = (tempExp << 48) | tempMant;
743 cnt = (temp.value >> 48) - (this->value >> 48);
746 tempExp = (temp.value >> 48);
748 this->value &= 0x0000FFFFFFFFFFFF;
749 this->value |= 0x0001000000000000;
752 tempMant = (temp.value & 0x0000FFFFFFFFFFFF) | 0x0001000000000000;
754 tempMant -= this->value;
756 if(tempMant > 0x8000000000000000)
758 tempMant &= 0x0000FFFFFFFFFFFF;
762 while(tempMant < 0x1000000000000)
768 tempMant &= 0x0000FFFFFFFFFFFF;
770 this->value = (tempExp << 48) | tempMant;
771 this->value |= 0x0100000000000000;
776 this->value = temp.value;
777 this->value |= 0x0100000000000000;
797 Vout = analogRead(TEMP)*0.0048828125;
798 NTC = ((
R*5.0)/Vout)-
R;
800 T =
A +
B*NTC +
C*NTC*NTC*NTC;
813 return (
float)this->angleMoved*0.087890625;
818 return this->curSpeed;
836 TIMSK1 = (1 << OCIE1A);
837 TCCR1A = (1 << WGM11);
838 TCCR1B = (1 << WGM12) | (1 << WGM13) | (1 << CS10);
840 this->encoderOffset = (((uint16_t)data[0]) << 8 ) | (uint16_t)data[1];
843 this->angleMoved = 0;
853 this->encoderOffset = (((uint16_t)data[0]) << 8 ) | (uint16_t)data[1];
857 this->angleMoved = 0;
863 return (
float)this->angle*0.087890625;
872 return (((uint16_t)data[0]) << 8 )| (uint16_t)data[1];
897 else if(data == 0x10)
902 else if(data == 0x20)
915 this->setMaxAcceleration(1000.0);
916 this->setMaxVelocity(1000.0);
918 p = &(this->control);
930 this->setMaxVelocity(vel);
931 this->setMaxAcceleration(accel);
933 p = &(this->control);
943 this->acceleration = accel;
946 this->multiplier.setValue((this->acceleration/(
INTFREQ*
INTFREQ)));
948 if(this->state !=
STOP)
950 if(this->continous == 1)
952 this->runContinous(this->direction);
956 this->moveSteps(this->totalSteps - this->currentStep + 1, this->direction, this->hold);
963 return this->acceleration;
970 this->velocity = 0.5005;
973 else if(vel > 28000.0)
975 this->velocity = 28000.0;
980 this->velocity = vel;
984 this->cruiseDelay = (uint16_t)((
INTFREQ/this->velocity) - 0.5);
986 if(this->state !=
STOP)
988 if(this->continous == 1)
990 this->runContinous(this->direction);
994 this->moveSteps(this->totalSteps - this->currentStep + 1, this->direction, this->hold);
1001 return this->velocity;
1014 this->continous = 1;
1018 curVel =
INTFREQ/this->exactDelay.getFloatValue();
1019 if(dir != digitalRead(DIR))
1021 this->direction = dir;
1023 this->initialDecelSteps = (uint32_t)(((curVel*curVel))/(2.0*this->acceleration));
1024 this->accelSteps = (uint32_t)((this->velocity*this->velocity)/(2.0*this->acceleration));
1026 this->exactDelay.setValue(
INTFREQ/sqrt((curVel*curVel) + 2.0*this->acceleration));
1028 if(this->exactDelay.getFloatValue() >= 65535.5)
1030 this->delay = 0xFFFF;
1034 this->delay = (uint16_t)(this->exactDelay.getFloatValue() - 0.5);
1039 if(curVel > this->velocity)
1042 this->initialDecelSteps = (uint32_t)(((this->velocity*this->velocity) - (curVel*curVel))/(-2.0*this->acceleration));
1043 this->accelSteps = 0;
1046 else if(curVel < this->velocity)
1048 this->state =
ACCEL;
1049 this->accelSteps = (uint32_t)(((this->velocity*this->velocity) - (curVel*curVel))/(2.0*this->acceleration));
1061 this->direction = dir;
1062 this->state =
ACCEL;
1071 this->accelSteps = (velocity*velocity)/(2.0*acceleration);
1073 this->exactDelay.setValue(
INTFREQ/sqrt(2.0*this->acceleration));
1075 if(this->exactDelay.getFloatValue() > 65535.0)
1077 this->delay = 0xFFFF;
1081 this->delay = (uint16_t)(this->exactDelay.getFloatValue() - 0.5);
1086 this->enableMotor();
1100 this->direction = dir;
1101 this->hold = holdMode;
1102 this->totalSteps = steps;
1103 this->continous = 0;
1107 curVel =
INTFREQ/this->exactDelay.getFloatValue();
1109 if(dir != digitalRead(DIR))
1112 this->initialDecelSteps = (uint32_t)((curVel*curVel)/(2.0*this->acceleration));
1113 this->accelSteps = (uint32_t)((this->velocity * this->velocity)/(2.0*this->acceleration));
1114 this->totalSteps += this->initialDecelSteps;
1116 if(this->accelSteps > (this->totalSteps >> 1))
1118 this->accelSteps = this->decelSteps = (this->totalSteps >> 1);
1119 this->accelSteps += this->totalSteps - this->accelSteps - this->decelSteps;
1123 this->decelSteps = this->accelSteps;
1124 this->cruiseSteps = this->totalSteps - this->accelSteps - this->decelSteps;
1127 this->exactDelay.setValue(
INTFREQ/sqrt((curVel*curVel) + 2.0*this->acceleration));
1129 if(this->exactDelay.getFloatValue() >= 65535.5)
1131 this->delay = 0xFFFF;
1135 this->delay = (uint16_t)(this->exactDelay.getFloatValue() - 0.5);
1140 if(curVel > this->velocity)
1143 this->initialDecelSteps = (uint32_t)(((this->velocity*this->velocity) - (curVel*curVel))/(-2.0*this->acceleration));
1144 this->accelSteps = 0;
1145 this->decelSteps = (uint32_t)((this->velocity*this->velocity)/(2.0*this->acceleration));
1146 this->exactDelay.setValue((
INTFREQ/sqrt((curVel*curVel) + 2*this->acceleration)));
1148 if(this->totalSteps <= (this->initialDecelSteps + this->decelSteps))
1150 this->cruiseSteps = 0;
1154 this->cruiseSteps = steps - this->initialDecelSteps - this->decelSteps;
1160 else if(curVel < this->velocity)
1162 this->state =
ACCEL;
1163 this->accelSteps = (uint32_t)(((this->velocity*this->velocity) - (curVel*curVel))/(2.0*this->acceleration));
1165 if(this->accelSteps > (this->totalSteps >> 1))
1167 this->accelSteps = this->decelSteps = (this->totalSteps >> 1);
1168 this->accelSteps += this->totalSteps - this->accelSteps - this->decelSteps;
1169 this->cruiseSteps = 0;
1173 this->decelSteps = this->accelSteps;
1174 this->cruiseSteps = this->totalSteps - this->accelSteps - this->decelSteps;
1177 this->cruiseSteps = steps - this->accelSteps - this->decelSteps;
1178 this->initialDecelSteps = 0;
1184 this->decelSteps = (uint32_t)((this->velocity*this->velocity)/(2.0*this->acceleration));
1185 this->accelSteps = 0;
1186 this->initialDecelSteps = 0;
1188 if(this->decelSteps >= this->totalSteps)
1190 this->cruiseSteps = 0;
1194 this->cruiseSteps = steps - this->decelSteps;
1210 this->state =
ACCEL;
1211 this->accelSteps = (uint32_t)((this->velocity * this->velocity)/(2.0*this->acceleration));
1212 this->initialDecelSteps = 0;
1214 if(this->accelSteps > (steps >> 1))
1216 this->cruiseSteps = 0;
1217 this->accelSteps = this->decelSteps = (steps >> 1);
1218 this->accelSteps += steps - this->accelSteps - this->decelSteps;
1223 this->decelSteps = this->accelSteps;
1224 this->cruiseSteps = steps - this->accelSteps - this->decelSteps;
1226 this->exactDelay.setValue(
INTFREQ/sqrt(2.0*this->acceleration));
1228 if(this->exactDelay.getFloatValue() > 65535.0)
1230 this->delay = 0xFFFF;
1234 this->delay = (uint16_t)(this->exactDelay.getFloatValue() - 0.5);
1239 this->enableMotor();
1250 this->hold = holdMode;
1261 if(holdMode ==
SOFT)
1263 this->disableMotor();
1266 else if (holdMode ==
HARD)
1268 this->enableMotor();
1283 this->hold = holdMode;
1287 curVel =
INTFREQ/this->exactDelay.getFloatValue();
1289 this->decelSteps = (uint32_t)((curVel*curVel)/(2.0*this->acceleration));
1290 this->accelSteps = this->initialDecelSteps = this->cruiseSteps = 0;
1291 this->state =
DECEL;
1293 this->exactDelay.setValue(
INTFREQ/sqrt(2.0*this->acceleration));
1295 if(this->exactDelay.getFloatValue() > 65535.0)
1297 this->delay = 0xFFFF;
1301 this->delay = (uint16_t)(this->exactDelay.getFloatValue() - 0.5);
1309 if(holdMode ==
SOFT)
1311 this->disableMotor();
1314 else if (holdMode ==
HARD)
1316 this->enableMotor();
1322 uint8_t microStepping,
1323 float faultTolerance,
1324 float faultHysteresis,
1339 digitalWrite(2,HIGH);
1340 digitalWrite(3,HIGH);
1341 digitalWrite(4,HIGH);
1342 attachInterrupt(digitalPinToInterrupt(2),
interrupt0, CHANGE);
1343 attachInterrupt(digitalPinToInterrupt(3),
interrupt1, FALLING);
1346 this->stepConversion = ((float)(200*microStepping))/4096.0;
1347 this->tolerance = faultTolerance;
1348 this->hysteresis = faultHysteresis;
1349 this->angleToStep = ((float)(200*microStepping))/360.0;
1352 this->pTerm = pTerm/10000.0;
1353 this->iTerm = iTerm/10000.0;
1354 this->dTerm = dTerm/10000.0;
1357 TCCR2B &= ~((1 << CS20) | (1 << CS21) | (1 << CS22) | (1 << WGM22));
1358 TCCR2A &= ~((1 << WGM20) | (1 << WGM21));
1359 TCCR2B |= (1 << CS21)| (1 << WGM22);
1360 TCCR2A |= (1 << WGM21) | (1 << WGM20);
1362 this->enableMotor();
1363 this->encoder.setup(mode);
1370 TIFR2 |= (1 << OCF2A);
1371 TIMSK2 |= (1 << OCIE2A);
1378 TIMSK2 &= ~(1 << OCIE2A);
1393 return this->direction;
1398 if(this->state !=
STOP)
1408 if(this->direction ==
CW)
1410 return this->stepsSinceReset + this->currentStep;
1414 return this->stepsSinceReset - this->currentStep;
1421 TCCR1A |= (1 << COM1B1);
1434 OCR1B = (uint16_t)(duty + 0.5);
1440 TCCR2A |= (1 << COM2B1);
1453 OCR2B = (uint16_t)(duty + 0.5);
1463 this->stepCnt = (int32_t)(setPoint*this->angleToStep);
1468 static float oldError = 0.0;
1471 static float accumError = 0.0;
1475 static uint16_t oldAngle = 0;
1476 static int16_t revolutions = 0;
1478 static uint32_t speed = 10000;
1479 static uint32_t oldMicros = 0;
1485 error = (float)this->stepCnt;
1486 if(this->speedValue[0] == oldMicros)
1496 speed = this->speedValue[0] - this->speedValue[1];
1500 curAngle = (((uint16_t)data[0]) << 8 ) | (uint16_t)data[1];
1501 this->encoder.angle = curAngle;
1502 curAngle -= this->encoder.encoderOffset;
1508 deltaAngle = (int16_t)oldAngle - (int16_t)curAngle;
1510 if(deltaAngle < -2047)
1516 else if(deltaAngle > 2047)
1522 this->encoder.angleMoved = (int32_t)curAngle + (4096*(int32_t)revolutions);
1523 oldAngle = curAngle;
1525 error = (((float)this->encoder.angleMoved * this->stepConversion) - error);
1527 if(error < -this->tolerance)
1530 this->control = (int32_t)error;
1535 integral = error*this->iTerm;
1536 accumError += integral;
1542 output -= this->pTerm*error;
1543 output -= accumError;
1544 output -= this->dTerm*(error - oldError);
1550 output *= (float)speed;
1555 accumError -= integral;
1566 else if(error > this->tolerance)
1569 this->control = (int32_t)error;
1572 integral = error*this->iTerm;
1573 accumError += integral;
1575 output -= this->pTerm*error;
1576 output -= accumError;
1577 output -= this->dTerm*(error - oldError);
1582 output *= (float)speed;
1587 accumError -= integral;
1600 if(error >= -this->hysteresis && error <= this->hysteresis)
1602 PORTB |= (PIND & 0x04) >> 2;
1614 static float oldError = 0.0;
1617 static float accumError = 0.0;
1622 static uint16_t oldAngle = 0;
1623 static int16_t revolutions = 0;
1625 static uint32_t speed = 10000;
1626 static uint32_t oldMicros = 0;
1636 error = (float)this->stepCnt;
1637 speed = (uint32_t)(this->exactDelay.getFloatValue() * INTPERIOD);
1640 curAngle = (((uint16_t)data[0]) << 8 ) | (uint16_t)data[1];
1641 this->encoder.angle = curAngle;
1642 curAngle -= this->encoder.encoderOffset;
1648 deltaAngle = (int16_t)oldAngle - (int16_t)curAngle;
1650 if(deltaAngle < -2047)
1656 else if(deltaAngle > 2047)
1662 this->encoder.angleMoved = (int32_t)curAngle + (4096*(int32_t)revolutions);
1663 oldAngle = curAngle;
1665 error = (((float)this->encoder.angleMoved * this->stepConversion) - error);
1667 if(error < -this->tolerance)
1670 this->control = (int32_t)error;
1675 integral = error*this->iTerm;
1676 accumError += integral;
1682 output -= this->pTerm*error;
1683 output -= accumError;
1684 output -= this->dTerm*(error - oldError);
1690 output *= (float)speed;
1695 accumError -= integral;
1706 else if(error > this->tolerance)
1709 this->control = (int32_t)error;
1712 integral = error*this->iTerm;
1713 accumError += integral;
1715 output -= this->pTerm*error;
1716 output -= accumError;
1717 output -= this->dTerm*(error - oldError);
1722 output *= (float)speed;
1727 accumError -= integral;
1740 if(error >= -this->hysteresis && error <= this->hysteresis)
1743 if(this->hold || this->state)
1778 while (!(TWCR & (1 << TWINT)));
1781 status = TWSR & 0xF8;
1784 bool i2cMaster::read(uint8_t slaveAddr, uint8_t regAddr, uint8_t numOfBytes, uint8_t *data)
1786 uint8_t i, buff[numOfBytes];
1788 TIMSK1 &= ~(1 << OCIE1A);
1796 for(i = 0; i < (numOfBytes - 1); i++)
1805 TIMSK1 |= (1 << OCIE1A);
1814 TIMSK1 &= ~(1 << OCIE1A);
1819 for(i = 0; i < numOfBytes; i++)
1825 TIMSK1 |= (1 << OCIE1A);
1834 this->cmd((1 << TWINT) | (1 << TWEN) | (1 << TWEA));
1839 this->cmd((1 << TWINT) | (1 << TWEN));
1850 this->cmd((1<<TWINT) | (1<<TWSTA) | (1<<TWEN));
1852 if (this->getStatus() !=
START && this->getStatus() !=
REPSTART)
1858 TWDR = (addr << 1) | RW;
1859 this->cmd((1 << TWINT) | (1 << TWEN));
1874 return this->start(addr, RW);
1881 this->cmd((1 << TWINT) | (1 << TWEN));
1890 TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWSTO);
1893 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.
void pwmD8(float duty)
Generate PWM signal on digital output 8.
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 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.
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 pwmD3(float duty)
Generate PWM signal on digital output 3.
void softStop(bool holdMode)
Stop the motor with deceleration.
uint8_t detectMagnet(void)
Detect if magnet is present and within range.
uint8_t getStatus(void)
Get current I2C status.
void cmd(uint8_t cmd)
Sends commands over the I2C bus.
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]
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.
float getAngle(void)
Measure the current shaft angle.
bool stop(void)
Closes the I2C connection.
void TIMER2_COMPA_vect(void)
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.
void setup(uint8_t mode)
Setup the encoder.
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.
int64_t getStepsSinceReset(void)
Get the number of steps applied since reset.
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.