71 #include <uStepperSLite.h>
74 volatile int32_t *p __attribute__((used));
125 void TIMER3_COMPA_vect(
void)
127 asm volatile(
"push r16 \n\t");
128 asm volatile(
"in r16,0x3F \n\t");
129 asm volatile(
"push r16 \n\t");
130 asm volatile(
"push r30 \n\t");
131 asm volatile(
"push r31 \n\t");
132 asm volatile(
"lds r30,p \n\t");
133 asm volatile(
"lds r31,p+1 \n\t");
135 asm volatile(
"jmp _stepGenerator \n\t");
139 void TIMER1_COMPA_vect(
void)
144 float posError = 0.0;
145 static float posEst = 0.0;
146 static float velIntegrator = 0.0;
147 static float velEst = 0.0;
150 int32_t pidTargetPositionTruncated;
151 int32_t *stepsSinceResetPointer;
160 I2C.
read(ENCODERADDR, ANGLE, 2, data);
162 curAngle = (((uint16_t)data[0]) << 8 ) | (uint16_t)data[1];
173 if(deltaAngle < -2047)
178 else if(deltaAngle > 2047)
190 if(pointer->
mode == DROPIN)
193 stepCntTemp = pointer->
stepCnt;
197 posEst += velEst * ENCODERINTSAMPLETIME;
198 posError = (float)stepCntTemp - posEst;
199 velIntegrator += posError * PULSEFILTERKI;
200 velEst = (posError * PULSEFILTERKP) + velIntegrator;
226 if(pointer->
mode == PID)
229 stepsSinceResetPointer = &pidTargetPositionTruncated;
259 if(pointer->
state == INITDECEL)
264 if(*stepsSinceResetPointer <= pointer->decelToAccelThreshold)
269 pointer->
state = CRUISE;
273 pointer->
state = ACCEL;
280 if(*stepsSinceResetPointer <= pointer->decelToAccelThreshold)
285 pointer->
state = CRUISE;
289 pointer->
state = ACCEL;
301 pointer->
state = CRUISE;
305 pointer->
state = ACCEL;
317 pointer->
state = CRUISE;
321 pointer->
state = ACCEL;
325 else if(pointer->
direction == CW && *stepsSinceResetPointer > 0 )
333 pointer->
state = CRUISE;
337 pointer->
state = ACCEL;
341 else if(pointer->
direction == CW && *stepsSinceResetPointer < 0)
349 pointer->
state = CRUISE;
353 pointer->
state = ACCEL;
357 else if(pointer->
direction == CCW && *stepsSinceResetPointer < 0)
360 if(*stepsSinceResetPointer <= pointer->decelToAccelThreshold)
365 pointer->
state = CRUISE;
369 pointer->
state = ACCEL;
373 else if(pointer->
direction == CCW && *stepsSinceResetPointer > 0)
376 if(*stepsSinceResetPointer <= pointer->decelToAccelThreshold)
381 pointer->
state = CRUISE;
385 pointer->
state = ACCEL;
391 else if(pointer->
state == ACCEL)
396 if(*stepsSinceResetPointer <= pointer->accelToCruiseThreshold)
398 pointer->
state = CRUISE;
406 pointer->
state = CRUISE;
411 else if(pointer->
state == CRUISE)
416 pointer->
state = CRUISE;
422 if(*stepsSinceResetPointer <= pointer->cruiseToDecelThreshold)
424 pointer->
state = DECEL;
431 pointer->
state = DECEL;
439 else if(pointer->
state == DECEL)
446 pointer->
state = STOP;
452 if(*stepsSinceResetPointer <= pointer->decelToStopThreshold)
454 pointer->
state = STOP;
459 else if(pointer->
state == STOP)
464 TCCR3B &= ~(1 << CS30);
465 if(pointer->
mode == NORMAL)
467 if(pointer->
brake == BRAKEON)
488 temp = (uint32_t)((STEPGENERATORFREQUENCY/(pointer->
currentPidSpeed)) + 0.5);
495 temp = (uint32_t)((STEPGENERATORFREQUENCY/(-pointer->
currentPidSpeed)) + 0.5);
592 TIMSK1 = (1 << OCIE1A);
593 TCCR1A = (1 << WGM11);
594 TCCR1B = (1 << WGM12) | (1 << WGM13) | (1 << CS10);
601 TIMSK1 &= ~(1 << OCIE1A);
602 I2C.
read(ENCODERADDR, ANGLE, 2, data);
603 TIMSK1 |= (1 << OCIE1A);
604 this->
encoderOffset = (((uint16_t)data[0]) << 8 ) | (uint16_t)data[1];
617 return (
float)this->
angle*0.087890625;
624 TIMSK1 &= ~(1 << OCIE1A);
625 I2C.
read(ENCODERADDR, MAGNITUDE, 2, data);
626 TIMSK1 |= (1 << OCIE1A);
628 return (((uint16_t)data[0]) << 8 )| (uint16_t)data[1];
634 TIMSK1 &= ~(1 << OCIE1A);
635 I2C.
read(ENCODERADDR, AGC, 1, &data);
636 TIMSK1 |= (1 << OCIE1A);
643 TIMSK1 &= ~(1 << OCIE1A);
644 I2C.
read(ENCODERADDR, STATUS, 1, &data);
645 TIMSK1 |= (1 << OCIE1A);
653 else if(data == 0x10)
658 else if(data == 0x20)
684 if(this->
state != STOP)
706 else if(vel > 100000.0)
716 if(this->
state != STOP)
732 float curVel, startVelocity = 0;
735 uint32_t initialDecelSteps;
737 if(this->
mode == DROPIN)
746 if(this->
state == STOP)
748 initialDecelSteps = 0;
754 else if((dir == CW && curVel < 0) || (dir == CCW && curVel > 0))
756 tempState = INITDECEL;
757 initialDecelSteps = (uint32_t)((curVel*curVel)/(2.0*this->
acceleration));
759 startVelocity = curVel;
761 else if((dir == CW && curVel > 0) || (dir == CCW && curVel < 0))
763 startVelocity = curVel;
766 tempState = INITDECEL;
771 else if(abs(curVel) < this->
velocity)
775 initialDecelSteps = 0;
780 initialDecelSteps = 0;
787 initialDecelSteps = 0;
809 if(tempState != INITDECEL)
834 if(tempState != INITDECEL)
858 this->
state = tempState;
859 Serial.print(
"\r\nstate: ");
860 Serial.println(this->
state);
862 Serial.print(
"\r\nstate: ");
863 Serial.println(this->
state);
864 Serial.print(
" StepsSinceReset: ");
866 Serial.print(
" CurrentSpeed: ");
868 Serial.print(
" currentPidAcceleration: ");
870 Serial.print(
" Dir: ");
872 Serial.print(
" Dir: ");
874 Serial.print(
" this->decelToAccelThreshold: ");
876 Serial.print(
" this->accelToCruiseThreshold: ");
881 TCCR3B |= (1 << CS30);
886 float curVel, startVelocity = 0;
891 uint32_t initialDecelSteps;
892 uint32_t cruiseSteps = 0;
894 if(this->
mode == DROPIN)
909 initialDecelSteps = 0;
911 if(this->state == STOP)
917 if(accelSteps > (totalSteps >> 1))
920 accelSteps = decelSteps = (totalSteps >> 1);
921 accelSteps += totalSteps - accelSteps - decelSteps;
926 decelSteps = accelSteps;
927 cruiseSteps = totalSteps - accelSteps - decelSteps;
931 else if((dir == CW && curVel < 0) || (dir == CCW && curVel > 0))
934 initialDecelSteps = (uint32_t)((curVel*curVel)/(2.0*this->
acceleration));
936 totalSteps += initialDecelSteps;
938 if(accelSteps > (totalSteps >> 1))
940 accelSteps = decelSteps = (totalSteps >> 1);
941 accelSteps += totalSteps - accelSteps - decelSteps;
946 decelSteps = accelSteps;
947 cruiseSteps = totalSteps - accelSteps - decelSteps;
949 startVelocity = curVel;
951 else if((dir == CW && curVel > 0) || (dir == CCW && curVel < 0))
953 startVelocity = curVel;
959 decelSteps = (uint32_t)((this->velocity*this->velocity)/(2.0*this->
acceleration));
960 if(totalSteps <= (initialDecelSteps + decelSteps))
966 cruiseSteps = steps - initialDecelSteps - decelSteps;
970 else if(abs(curVel) < this->
velocity)
975 if(accelSteps > (totalSteps >> 1))
977 accelSteps = decelSteps = (totalSteps >> 1);
978 accelSteps += totalSteps - accelSteps - decelSteps;
983 decelSteps = accelSteps;
984 cruiseSteps = totalSteps - accelSteps - decelSteps;
987 cruiseSteps = totalSteps - accelSteps - decelSteps;
988 initialDecelSteps = 0;
996 initialDecelSteps = 0;
998 if(decelSteps >= totalSteps)
1001 decelSteps = totalSteps;
1005 cruiseSteps = totalSteps - decelSteps;
1015 if(accelSteps > (totalSteps >> 1))
1018 accelSteps = decelSteps = (totalSteps >> 1);
1019 accelSteps += totalSteps - accelSteps - decelSteps;
1024 decelSteps = accelSteps;
1025 cruiseSteps = totalSteps - accelSteps - decelSteps;
1027 startVelocity = 0.0;
1048 if(
state != INITDECEL)
1070 if(
state != INITDECEL)
1094 this->state =
state;
1096 this->
brake = holdMode;
1098 Serial.print(
"\r\nstate: ");
1099 Serial.println(this->state);
1101 Serial.print(
"\r\nstate: ");
1102 Serial.println(this->state);
1103 Serial.print(
" StepsSinceReset: ");
1105 Serial.print(
" CurrentSpeed: ");
1107 Serial.print(
" currentPidAcceleration: ");
1109 Serial.print(
" Dir: ");
1111 Serial.print(
" Dir: ");
1113 Serial.print(
" this->decelToAccelThreshold: ");
1115 Serial.print(
" this->accelToCruiseThreshold: ");
1120 TCCR3B |= (1 << CS30);
1125 if(this->
mode == DROPIN)
1140 this->
state = DECEL;
1141 this->
brake = holdMode;
1144 Serial.print(
"\r\nstate: ");
1145 Serial.println(this->
state);
1147 Serial.print(
"\r\nstate: ");
1148 Serial.println(this->
state);
1149 Serial.print(
" StepsSinceReset: ");
1151 Serial.print(
" CurrentSpeed: ");
1153 Serial.print(
" currentPidAcceleration: ");
1155 Serial.print(
" Dir: ");
1157 Serial.print(
" Dir: ");
1159 Serial.print(
" this->decelToAccelThreshold: ");
1161 Serial.print(
" this->accelToCruiseThreshold: ");
1165 TCCR3B |= (1 << CS30);
1175 uint32_t decelSteps = 0;
1178 if(this->
mode == DROPIN)
1185 decelSteps = (uint32_t)((curVel*curVel)/(2.0*this->
acceleration));
1201 this->
state = DECEL;
1202 this->
brake = holdMode;
1217 Serial.print(
"\r\nstate: ");
1218 Serial.println(this->
state);
1220 Serial.print(
"\r\nstate: ");
1221 Serial.println(this->
state);
1222 Serial.print(
" StepsSinceReset: ");
1224 Serial.print(
" CurrentSpeed: ");
1226 Serial.print(
" currentPidAcceleration: ");
1228 Serial.print(
" Dir: ");
1230 Serial.print(
" Dir: ");
1232 Serial.print(
" this->decelToAccelThreshold: ");
1234 Serial.print(
" this->accelToCruiseThreshold: ");
1239 TCCR3B |= (1 << CS30);
1246 int16_t angleDiff[3];
1248 I2C.
read(ENCODERADDR, ANGLE, 2, data);
1249 angle = (((uint16_t)data[0]) << 8 ) | (uint16_t)data[1];
1253 for(i = 0; i < 50; i++)
1256 delayMicroseconds(1);
1261 angleDiff[0] = (int16_t)
angle;
1263 I2C.
read(ENCODERADDR, ANGLE, 2, data);
1264 angle = (((uint16_t)data[0]) << 8 ) | (uint16_t)data[1];
1266 angleDiff[0] -= (int16_t)
angle;
1270 for(i = 0; i < 50; i++)
1273 delayMicroseconds(1);
1278 angleDiff[1] = (int16_t)
angle;
1280 I2C.
read(ENCODERADDR, ANGLE, 2, data);
1281 angle = (((uint16_t)data[0]) << 8 ) | (uint16_t)data[1];
1283 angleDiff[1] -= (int16_t)
angle;
1285 for(i = 0; i < 2; i++)
1287 if(angleDiff[i] > 2048)
1289 angleDiff[i] -= 4096;
1291 else if(angleDiff[i] < -2048)
1293 angleDiff[i] += 4096;
1297 if((angleDiff[0] > 2 && angleDiff[1] < -2))
1314 float stepsPerRevolution = 3200.0,
1318 bool setHome =
true,
1321 uint8_t holdCurrent)
1335 if((uint16_t)stepsPerRevolution == FULL)
1337 stepsPerRevolution = 200.0;
1339 else if((uint16_t)stepsPerRevolution == HALF)
1341 stepsPerRevolution = 400.0;
1343 else if((uint16_t)stepsPerRevolution == QUARTER)
1345 stepsPerRevolution = 800.0;
1347 else if((uint16_t)stepsPerRevolution == EIGHT)
1349 stepsPerRevolution = 1600.0;
1351 else if((uint16_t)stepsPerRevolution == SIXTEEN)
1353 stepsPerRevolution = 3200.0;
1357 this->
angleToStep = ((float)(stepsPerRevolution))/360.0;
1358 this->
stepToAngle = 360.0/((float)(stepsPerRevolution));
1366 if(this->
mode == DROPIN)
1373 digitalWrite(2,HIGH);
1374 digitalWrite(3,HIGH);
1375 digitalWrite(4,HIGH);
1384 tempSettings.
invert = invert;
1425 TIMSK3 = (1 << OCIE3A);
1426 TCCR3A = (1 << WGM31);
1427 TCCR3B = (1 << WGM32) | (1 << WGM33);
1448 if(this->
state != STOP)
1483 if(this->
mode == DROPIN)
1494 while(!this->
isStalled(stallSensitivity));
1522 steps = (uint32_t)((abs(diff)*
angleToStep) + 0.5);
1552 float u, uSat, temp;
1554 static float integral;
1555 static bool integralReset = 0;
1562 if(this->
state == STOP)
1564 if(this->
brake == BRAKEON)
1581 u = error*this->
pTerm;
1598 integral += error*this->
iTerm;
1600 if(integral > 10000.0)
1604 else if(integral < -10000.0)
1606 integral = -10000.0;
1611 if(u > 100000 || u < -100000)
1614 uSat = ((u > 0) - (u < 0)) * uSat;
1621 if(error < -2.0 || error > 2.0)
1645 TCCR3B |= (1 << CS30);
1650 if(this->
state == STOP)
1652 TCCR3B &= ~(1 << CS30);
1659 temp = (uint32_t)((STEPGENERATORFREQUENCY/uSat) + 0.5);
1660 this->stepGeneratorDirectionPid = CW;
1666 else if(uSat < -5.0)
1668 this->stepGeneratorDirectionPid = CCW;
1670 temp = (uint32_t)((STEPGENERATORFREQUENCY/-uSat) + 0.5);
1677 this->stepGeneratorDirectionPid = CW;
1684 this->stepGeneratorDirectionPid = CCW;
1724 static float integral;
1725 static bool integralReset = 0;
1731 u = error*this->
pTerm;
1748 integral += error*this->
iTerm;
1750 if(integral > 10000.0)
1754 else if(integral < -10000.0)
1756 integral = -10000.0;
1759 if(error > -10 && error < 10)
1780 static float oldTargetPosition;
1781 static float oldEncoderPosition;
1782 static float encoderPositionChange;
1783 static float targetPositionChange;
1785 static float internalStall = 0.0;
1787 encoderPositionChange *= 0.99;
1788 encoderPositionChange += 0.01*(oldEncoderPosition - encoderPosition);
1789 oldEncoderPosition = encoderPosition;
1791 targetPositionChange *= 0.99;
1795 if(abs(encoderPositionChange) < abs(targetPositionChange)*0.5)
1805 if(internalStall >= 0.95)
1817 if(this->stallSensitivity > 1.0)
1819 this->stallSensitivity = 1.0;
1821 else if(this->stallSensitivity < 0.0)
1823 this->stallSensitivity = 0.0;
1839 this->
iTerm = I * ENCODERINTSAMPLETIME;
1844 this->
dTerm = D * ENCODERINTFREQ;
1857 if(cmd->charAt(2) ==
';')
1859 Serial.println(
"COMMAND NOT ACCEPTED");
1868 if(cmd->substring(0,2) == String(
"P="))
1872 if(cmd->charAt(i) >=
'0' && cmd->charAt(i) <=
'9')
1874 value.concat(cmd->charAt(i));
1876 else if(cmd->charAt(i) ==
'.')
1878 value.concat(cmd->charAt(i));
1882 else if(cmd->charAt(i) ==
';')
1888 Serial.println(
"COMMAND NOT ACCEPTED");
1895 if(cmd->charAt(i) >=
'0' && cmd->charAt(i) <=
'9')
1897 value.concat(cmd->charAt(i));
1899 else if(cmd->charAt(i) ==
';')
1901 Serial.print(
"COMMAND ACCEPTED. P = ");
1902 Serial.println(value.toFloat(),4);
1910 Serial.println(
"COMMAND NOT ACCEPTED");
1920 else if(cmd->substring(0,2) == String(
"I="))
1924 if(cmd->charAt(i) >=
'0' && cmd->charAt(i) <=
'9')
1926 value.concat(cmd->charAt(i));
1928 else if(cmd->charAt(i) ==
'.')
1930 value.concat(cmd->charAt(i));
1934 else if(cmd->charAt(i) ==
';')
1940 Serial.println(
"COMMAND NOT ACCEPTED");
1947 if(cmd->charAt(i) >=
'0' && cmd->charAt(i) <=
'9')
1949 value.concat(cmd->charAt(i));
1951 else if(cmd->charAt(i) ==
';')
1953 Serial.print(
"COMMAND ACCEPTED. I = ");
1954 Serial.println(value.toFloat(),4);
1963 Serial.println(
"COMMAND NOT ACCEPTED");
1973 else if(cmd->substring(0,2) == String(
"D="))
1977 if(cmd->charAt(i) >=
'0' && cmd->charAt(i) <=
'9')
1979 value.concat(cmd->charAt(i));
1981 else if(cmd->charAt(i) ==
'.')
1983 value.concat(cmd->charAt(i));
1987 else if(cmd->charAt(i) ==
';')
1993 Serial.println(
"COMMAND NOT ACCEPTED");
2000 if(cmd->charAt(i) >=
'0' && cmd->charAt(i) <=
'9')
2002 value.concat(cmd->charAt(i));
2004 else if(cmd->charAt(i) ==
';')
2006 Serial.print(
"COMMAND ACCEPTED. D = ");
2007 Serial.println(value.toFloat(),4);
2015 Serial.println(
"COMMAND NOT ACCEPTED");
2025 else if(cmd->substring(0,6) == String(
"invert"))
2027 if(cmd->charAt(6) !=
';')
2029 Serial.println(
"COMMAND NOT ACCEPTED");
2034 Serial.println(F(
"Direction normal!"));
2042 Serial.println(F(
"Direction inverted!"));
2054 else if(cmd->substring(0,5) == String(
"error"))
2056 if(cmd->charAt(5) !=
';')
2058 Serial.println(
"COMMAND NOT ACCEPTED");
2061 Serial.print(F(
"Current Error: "));
2063 Serial.println(F(
" Steps"));
2070 else if(cmd->substring(0,7) == String(
"current"))
2072 if(cmd->charAt(7) !=
';')
2074 Serial.println(
"COMMAND NOT ACCEPTED");
2077 Serial.print(F(
"Run Current: "));
2078 Serial.print(this->
driver.getRunCurrent());
2079 Serial.println(F(
" %"));
2080 Serial.print(F(
"Hold Current: "));
2081 Serial.print(this->
driver.getHoldCurrent());
2082 Serial.println(F(
" %"));
2089 else if(cmd->substring(0,10) == String(
"parameters"))
2091 if(cmd->charAt(10) !=
';')
2093 Serial.println(
"COMMAND NOT ACCEPTED");
2096 Serial.print(F(
"P: "));
2098 Serial.print(F(
", "));
2099 Serial.print(F(
"I: "));
2101 Serial.print(F(
", "));
2102 Serial.print(F(
"D: "));
2110 else if(cmd->substring(0,4) == String(
"help"))
2112 if(cmd->charAt(4) !=
';')
2114 Serial.println(
"COMMAND NOT ACCEPTED");
2124 else if(cmd->substring(0,11) == String(
"runCurrent="))
2128 if(cmd->charAt(i) >=
'0' && cmd->charAt(i) <=
'9')
2130 value.concat(cmd->charAt(i));
2132 else if(cmd->charAt(i) ==
'.')
2134 value.concat(cmd->charAt(i));
2138 else if(cmd->charAt(i) ==
';')
2144 Serial.println(
"COMMAND NOT ACCEPTED");
2151 if(cmd->charAt(i) >=
'0' && cmd->charAt(i) <=
'9')
2153 value.concat(cmd->charAt(i));
2155 else if(cmd->charAt(i) ==
';')
2157 if(value.toFloat() >= 0.0 && value.toFloat() <= 100.0)
2159 i = (uint8_t)value.toFloat();
2164 Serial.println(
"COMMAND NOT ACCEPTED");
2170 Serial.println(
"COMMAND NOT ACCEPTED");
2175 Serial.print(
"COMMAND ACCEPTED. runCurrent = ");
2177 Serial.println(F(
" %"));
2187 else if(cmd->substring(0,12) == String(
"holdCurrent="))
2191 if(cmd->charAt(i) >=
'0' && cmd->charAt(i) <=
'9')
2193 value.concat(cmd->charAt(i));
2195 else if(cmd->charAt(i) ==
'.')
2197 value.concat(cmd->charAt(i));
2201 else if(cmd->charAt(i) ==
';')
2207 Serial.println(F(
"COMMAND NOT ACCEPTED"));
2214 if(cmd->charAt(i) >=
'0' && cmd->charAt(i) <=
'9')
2216 value.concat(cmd->charAt(i));
2218 else if(cmd->charAt(i) ==
';')
2220 if(value.toFloat() >= 0.0 && value.toFloat() <= 100.0)
2222 i = (uint8_t)value.toFloat();
2227 Serial.println(F(
"COMMAND NOT ACCEPTED"));
2233 Serial.println(F(
"COMMAND NOT ACCEPTED"));
2238 Serial.print(F(
"COMMAND ACCEPTED. holdCurrent = "));
2240 Serial.println(F(
" %"));
2252 Serial.println(F(
"COMMAND NOT ACCEPTED"));
2260 static String stringInput;
2261 static uint32_t t = millis();
2265 while(!Serial.available())
2268 if((millis() - t) >= 500)
2270 stringInput.remove(0);
2275 stringInput += (char)Serial.read();
2276 if(stringInput.lastIndexOf(
';') > -1)
2279 stringInput.remove(0);
2286 Serial.println(F(
"uStepper S-lite Dropin !"));
2287 Serial.println(F(
""));
2288 Serial.println(F(
"Usage:"));
2289 Serial.println(F(
"Show this command list: 'help;'"));
2290 Serial.println(F(
"Get PID Parameters: 'parameters;'"));
2291 Serial.println(F(
"Set Proportional constant: 'P=10.002;'"));
2292 Serial.println(F(
"Set Integral constant: 'I=10.002;'"));
2293 Serial.println(F(
"Set Differential constant: 'D=10.002;'"));
2294 Serial.println(F(
"Invert Direction: 'invert;'"));
2295 Serial.println(F(
"Get Current PID Error: 'error;'"));
2296 Serial.println(F(
"Get Run/Hold Current Settings: 'current;'"));
2297 Serial.println(F(
"Set Run Current (percent): 'runCurrent=50.0;'"));
2298 Serial.println(F(
"Set Hold Current (percent): 'holdCurrent=50.0;'"));
2299 Serial.println(F(
""));
2300 Serial.println(F(
""));
2307 EEPROM.get(0,tempSettings);
2333 uint8_t checksum = 0xAA;
2334 uint8_t *p = (uint8_t*)settings;
2336 for(i=0; i < 15; i++)