39 this->microSteps = 256;
50 this->microSteps = 256;
61 this->pidDisabled = 1;
76 SPCR1 = (1<<SPE1)|(1<<MSTR1);
84 this->
driver.readMotorStatus();
93 uint16_t stepsPerRevolution,
97 uint16_t dropinStepSize,
104 this->pidDisabled = 1;
107 this->fullSteps = stepsPerRevolution;
108 this->dropinStepSize = 256/dropinStepSize;
109 this->angleToStep = (float)this->fullSteps * (
float)this->microSteps / 360.0;
110 this->rpmToVelocity = (float)(279620.267 * fullSteps * microSteps)/(
CLOCKFREQ);
111 this->stepsPerSecondToRPM = 60.0/(this->microSteps*this->fullSteps);
112 this->RPMToStepsPerSecond = (this->microSteps*this->fullSteps)/60.0;
138 digitalWrite(2,HIGH);
139 digitalWrite(3,HIGH);
140 digitalWrite(4,HIGH);
148 tempSettings.
P.
f = pTerm;
150 tempSettings.
D.
f = dTerm;
151 tempSettings.
invert = invert;
154 tempSettings.
checksum = this->dropinSettingsCalcChecksum(&tempSettings);
158 this->dropinSettings = tempSettings;
159 this->saveDropinSettings();
161 this->loadDropinSettings();
165 if(!this->loadDropinSettings())
167 this->dropinSettings = tempSettings;
168 this->saveDropinSettings();
170 this->loadDropinSettings();
201 this->pidDisabled = 0;
227 steps = (int32_t)((angle * angleToStep) - 0.5);
232 steps = (int32_t)((angle * angleToStep) + 0.5);
244 steps = (int32_t)( (abs(diff) * angleToStep) + 0.5);
256 bool uStepperS::detectStall(int32_t stepsMoved)
258 static float oldTargetPosition;
259 static float oldEncoderPosition;
260 static float encoderPositionChange;
261 static float targetPositionChange;
263 static float internalStall = 0.0;
265 encoderPositionChange *= 0.99;
266 encoderPositionChange += 0.01*(oldEncoderPosition - encoderPosition);
267 oldEncoderPosition = encoderPosition;
269 targetPositionChange *= 0.99;
270 targetPositionChange += 0.01*(oldTargetPosition - stepsMoved);
271 oldTargetPosition = stepsMoved;
273 if(abs(encoderPositionChange) < abs(targetPositionChange)*0.5)
275 internalStall *= this->stallSensitivity;
276 internalStall += 1.0-this->stallSensitivity;
280 internalStall *= this->stallSensitivity;
283 if(internalStall >= 0.95)
295 if(this->stallSensitivity > 1.0)
297 this->stallSensitivity = 1.0;
299 else if(this->stallSensitivity < 0.0)
301 this->stallSensitivity = 0.0;
304 this->stallSensitivity = stallSensitivity;
310 void uStepperS::brakeMotor(
bool brake )
317 int32_t velocityDir = rpmToVelocity * rpm;
326 uint32_t velocity = abs(velocityDir);
332 void uStepperS::setSPIMode( uint8_t mode ){
337 SPCR1 &= ~(1<<CPHA1);
347 uint8_t uStepperS::SPI(uint8_t data){
352 while(!( SPSR1 & (1 << SPIF1) ));
360 velocity *= (float)this->microSteps;
371 acceleration *= (float)this->microSteps;
383 deceleration *= (float)this->microSteps;
386 this->maxDeceleration = deceleration;
394 if( current <= 100.0 && current >= 0.0){
396 this->
driver.current = ceil(0.31 * current);
399 this->
driver.current = 16;
408 if( current <= 100.0 && current >= 0.0){
410 this->
driver.holdCurrent = ceil(0.31 * current);
413 this->
driver.holdCurrent = 16;
429 this->
driver.setDirection( direction );
463 void uStepperS::filterSpeedPos(
posFilter_t *filter, int32_t steps)
495 if(!pointer->invertPidDropinDirection)
497 pointer->stepCnt-=pointer->dropinStepSize;
501 pointer->stepCnt+=pointer->dropinStepSize;
507 if(!pointer->invertPidDropinDirection)
509 pointer->stepCnt+=pointer->dropinStepSize;
513 pointer->stepCnt-=pointer->dropinStepSize;
537 if(deltaAngle < -32768)
541 else if(deltaAngle > 32768)
550 stepCntTemp = pointer->stepCnt;
553 pointer->filterSpeedPos(&pointer->externalStepInputFilter, stepCntTemp);
555 if(!pointer->pidDisabled)
558 pointer->currentPidSpeed = pointer->externalStepInputFilter.
velIntegrator;
566 if(!pointer->pidDisabled)
569 if(abs(pointer->currentPidError) >= 10.0 )
579 pointer->detectStall(stepsMoved);
585 this->pidDisabled = 0;
592 this->pidDisabled = 1;
609 while(!this->
isStalled(stallSensitivity))
620 return this->currentPidError;
623 float uStepperS::pid(
float error)
626 float limit = abs(this->currentPidSpeed) + 150000.0;
627 static float integral;
628 static bool integralReset = 0;
629 static float errorOld, differential = 0.0;
631 this->currentPidError = error;
633 u = error*this->pTerm;
650 integral += error*this->
iTerm;
652 if(integral > 3000000.0)
654 integral = 3000000.0;
656 else if(integral < -3000000.0)
658 integral = -3000000.0;
661 if(error > -100 && error < 100)
677 differential += 0.1*((error - errorOld)*this->dTerm);
683 u *= this->stepsPerSecondToRPM;
706 this->invertPidDropinDirection = invert;
714 if(cmd->charAt(2) ==
';')
716 Serial.println(
"COMMAND NOT ACCEPTED");
725 if(cmd->substring(0,2) == String(
"P="))
729 if(cmd->charAt(i) >=
'0' && cmd->charAt(i) <=
'9')
731 value.concat(cmd->charAt(i));
733 else if(cmd->charAt(i) ==
'.')
735 value.concat(cmd->charAt(i));
739 else if(cmd->charAt(i) ==
';')
745 Serial.println(
"COMMAND NOT ACCEPTED");
752 if(cmd->charAt(i) >=
'0' && cmd->charAt(i) <=
'9')
754 value.concat(cmd->charAt(i));
756 else if(cmd->charAt(i) ==
';')
758 Serial.print(
"COMMAND ACCEPTED. P = ");
759 Serial.println(value.toFloat(),4);
760 this->dropinSettings.
P.
f = value.toFloat();
761 this->saveDropinSettings();
767 Serial.println(
"COMMAND NOT ACCEPTED");
777 else if(cmd->substring(0,2) == String(
"I="))
781 if(cmd->charAt(i) >=
'0' && cmd->charAt(i) <=
'9')
783 value.concat(cmd->charAt(i));
785 else if(cmd->charAt(i) ==
'.')
787 value.concat(cmd->charAt(i));
791 else if(cmd->charAt(i) ==
';')
797 Serial.println(
"COMMAND NOT ACCEPTED");
804 if(cmd->charAt(i) >=
'0' && cmd->charAt(i) <=
'9')
806 value.concat(cmd->charAt(i));
808 else if(cmd->charAt(i) ==
';')
810 Serial.print(
"COMMAND ACCEPTED. I = ");
811 Serial.println(value.toFloat(),4);
812 this->dropinSettings.
I.
f = value.toFloat();
813 this->saveDropinSettings();
819 Serial.println(
"COMMAND NOT ACCEPTED");
829 else if(cmd->substring(0,2) == String(
"D="))
833 if(cmd->charAt(i) >=
'0' && cmd->charAt(i) <=
'9')
835 value.concat(cmd->charAt(i));
837 else if(cmd->charAt(i) ==
'.')
839 value.concat(cmd->charAt(i));
843 else if(cmd->charAt(i) ==
';')
849 Serial.println(
"COMMAND NOT ACCEPTED");
856 if(cmd->charAt(i) >=
'0' && cmd->charAt(i) <=
'9')
858 value.concat(cmd->charAt(i));
860 else if(cmd->charAt(i) ==
';')
862 Serial.print(
"COMMAND ACCEPTED. D = ");
863 Serial.println(value.toFloat(),4);
864 this->dropinSettings.
D.
f = value.toFloat();
865 this->saveDropinSettings();
871 Serial.println(
"COMMAND NOT ACCEPTED");
881 else if(cmd->substring(0,6) == String(
"invert"))
883 if(cmd->charAt(6) !=
';')
885 Serial.println(
"COMMAND NOT ACCEPTED");
888 if(this->invertPidDropinDirection)
890 Serial.println(F(
"Direction normal!"));
891 this->dropinSettings.
invert = 0;
892 this->saveDropinSettings();
898 Serial.println(F(
"Direction inverted!"));
899 this->dropinSettings.
invert = 1;
900 this->saveDropinSettings();
910 else if(cmd->substring(0,5) == String(
"error"))
912 if(cmd->charAt(5) !=
';')
914 Serial.println(
"COMMAND NOT ACCEPTED");
917 Serial.print(F(
"Current Error: "));
919 Serial.println(F(
" Steps"));
926 else if(cmd->substring(0,7) == String(
"current"))
928 if(cmd->charAt(7) !=
';')
930 Serial.println(
"COMMAND NOT ACCEPTED");
933 Serial.print(F(
"Run Current: "));
934 Serial.print(ceil(((
float)this->
driver.current)/0.31));
935 Serial.println(F(
" %"));
936 Serial.print(F(
"Hold Current: "));
937 Serial.print(ceil(((
float)this->
driver.holdCurrent)/0.31));
938 Serial.println(F(
" %"));
945 else if(cmd->substring(0,10) == String(
"parameters"))
947 if(cmd->charAt(10) !=
';')
949 Serial.println(
"COMMAND NOT ACCEPTED");
952 Serial.print(F(
"P: "));
953 Serial.print(this->dropinSettings.
P.
f,4);
954 Serial.print(F(
", "));
955 Serial.print(F(
"I: "));
956 Serial.print(this->dropinSettings.
I.
f,4);
957 Serial.print(F(
", "));
958 Serial.print(F(
"D: "));
959 Serial.println(this->dropinSettings.
D.
f,4);
966 else if(cmd->substring(0,4) == String(
"help"))
968 if(cmd->charAt(4) !=
';')
970 Serial.println(
"COMMAND NOT ACCEPTED");
980 else if(cmd->substring(0,11) == String(
"runCurrent="))
984 if(cmd->charAt(i) >=
'0' && cmd->charAt(i) <=
'9')
986 value.concat(cmd->charAt(i));
988 else if(cmd->charAt(i) ==
'.')
990 value.concat(cmd->charAt(i));
994 else if(cmd->charAt(i) ==
';')
1000 Serial.println(
"COMMAND NOT ACCEPTED");
1007 if(cmd->charAt(i) >=
'0' && cmd->charAt(i) <=
'9')
1009 value.concat(cmd->charAt(i));
1011 else if(cmd->charAt(i) ==
';')
1013 if(value.toFloat() >= 0.0 && value.toFloat() <= 100.0)
1015 i = (uint8_t)value.toFloat();
1020 Serial.println(
"COMMAND NOT ACCEPTED");
1026 Serial.println(
"COMMAND NOT ACCEPTED");
1031 Serial.print(
"COMMAND ACCEPTED. runCurrent = ");
1033 Serial.println(F(
" %"));
1035 this->saveDropinSettings();
1043 else if(cmd->substring(0,12) == String(
"holdCurrent="))
1047 if(cmd->charAt(i) >=
'0' && cmd->charAt(i) <=
'9')
1049 value.concat(cmd->charAt(i));
1051 else if(cmd->charAt(i) ==
'.')
1053 value.concat(cmd->charAt(i));
1057 else if(cmd->charAt(i) ==
';')
1063 Serial.println(
"COMMAND NOT ACCEPTED");
1070 if(cmd->charAt(i) >=
'0' && cmd->charAt(i) <=
'9')
1072 value.concat(cmd->charAt(i));
1074 else if(cmd->charAt(i) ==
';')
1076 if(value.toFloat() >= 0.0 && value.toFloat() <= 100.0)
1078 i = (uint8_t)value.toFloat();
1083 Serial.println(
"COMMAND NOT ACCEPTED");
1089 Serial.println(
"COMMAND NOT ACCEPTED");
1094 Serial.print(
"COMMAND ACCEPTED. holdCurrent = ");
1096 Serial.println(F(
" %"));
1098 this->saveDropinSettings();
1108 Serial.println(
"COMMAND NOT ACCEPTED");
1116 static String stringInput;
1117 static uint32_t t = millis();
1121 while(!Serial.available())
1124 if((millis() - t) >= 500)
1126 stringInput.remove(0);
1131 stringInput += (char)Serial.read();
1132 if(stringInput.lastIndexOf(
';') > -1)
1135 stringInput.remove(0);
1142 Serial.println(F(
"uStepper S Dropin !"));
1143 Serial.println(F(
""));
1144 Serial.println(F(
"Usage:"));
1145 Serial.println(F(
"Show this command list: 'help;'"));
1146 Serial.println(F(
"Get PID Parameters: 'parameters;'"));
1147 Serial.println(F(
"Set Proportional constant: 'P=10.002;'"));
1148 Serial.println(F(
"Set Integral constant: 'I=10.002;'"));
1149 Serial.println(F(
"Set Differential constant: 'D=10.002;'"));
1150 Serial.println(F(
"Invert Direction: 'invert;'"));
1151 Serial.println(F(
"Get Current PID Error: 'error;'"));
1152 Serial.println(F(
"Get Run/Hold Current Settings: 'current;'"));
1153 Serial.println(F(
"Set Run Current (percent): 'runCurrent=50.0;'"));
1154 Serial.println(F(
"Set Hold Current (percent): 'holdCurrent=50.0;'"));
1155 Serial.println(F(
""));
1156 Serial.println(F(
""));
1159 bool uStepperS::loadDropinSettings(
void)
1163 EEPROM.get(0,tempSettings);
1165 if(this->dropinSettingsCalcChecksum(&tempSettings) != tempSettings.
checksum)
1170 this->dropinSettings = tempSettings;
1180 void uStepperS::saveDropinSettings(
void)
1182 this->dropinSettings.
checksum = this->dropinSettingsCalcChecksum(&this->dropinSettings);
1184 EEPROM.put(0,this->dropinSettings);
1190 uint8_t checksum = 0xAA;
1191 uint8_t *p = (uint8_t*)settings;
1193 for(i=0; i < 15; i++)
#define ENCODERDATATOSTEP
volatile posFilter_t encoderFilter
void dropinCli()
This method is used to tune Drop-in parameters. After tuning uStepper S, the parameters are saved in ...
int32_t readRegister(uint8_t address)
Reads a register from the motor driver.
void setRampMode(uint8_t mode)
Set motor driver to position mode or velocity mode.
void init(uStepperS *_pointer)
Initiation of the motor driver.
void moveAngle(float angle)
Moves the motor rotate a specific angle relative to the current position.
void setVelocity(uint32_t velocity)
Set motor velocity.
Prototype of class for accessing all features of the uStepper S in a single object.
void setHoldCurrent(double current)
Set motor hold current.
Struct to store dropin settings.
void dropinPrintHelp()
This method is used to print the dropinCli menu explainer:
void setMaxVelocity(float velocity)
Set the maximum velocity of the stepper motor.
float getAngleMoved(void)
Returns the angle moved from reference position in degrees.
Struct for encoder velocity estimator.
void setPosition(int32_t position)
Set the motor position.
void updateCurrent(void)
Writes the current setting registers of the motor driver.
float getPidError(void)
This method returns the current PID error.
void runContinous(bool dir)
Make the motor rotate continuously.
#define VELOCITY_MODE_POS
volatile int32_t angleMoved
uint16_t captureAngle(void)
Capture the current shaft angle.
void parseCommand(String *cmd)
This method is used for the dropinCli to take in user commands.
#define ACCELERATIONCONVERSION
volatile uint16_t oldAngle
void setHome(void)
Define new reference(home) position.
void setMaxDeceleration(float deceleration)
Set the maximum deceleration of the stepper motor.
uStepperS()
Constructor of uStepper class.
void moveToAngle(float angle)
Moves the motor rotate a specific angle relative to the current position.
void TIMER1_COMPA_vect(void) __attribute__((signal
Interrupt routine for critical tasks.
int32_t getPosition(void)
Returns the current position of the motor driver.
void init(uStepperS *_pointer)
Initiation of the encoder.
void enablePid(void)
This method enables the PID after being disabled (disablePid).
void setRPM(float rpm)
Set the velocity in rpm.
void stop(bool mode=HARD)
Stop the motor.
void moveSteps(int32_t steps)
Make the motor perform a predefined number of steps.
void init(void)
Internal function to prepare the uStepperS in the constructor.
void setShaftDirection(bool direction)
Set motor driver direction.
void interrupt1(void)
Used by dropin feature to take in enable signal.
bool isStalled(float stallSensitivity=0.992)
This method returns a bool variable indicating wether the motor is stalled or not.
void setIntegral(float I)
This method is used to change the PID integral parameter I.
void setProportional(float P)
This method is used to change the PID proportional parameter P.
void invertDropinDir(bool invert)
This method is used to invert the drop-in direction pin interpretation.
float moveToEnd(bool dir, float stallSensitivity=0.992)
Moves the motor to its physical limit, without limit switch.
void setDeceleration(uint32_t deceleration)
Set motor deceleration.
void interrupt0(void)
Used by dropin feature to take in step pulses.
void setup(uint8_t mode=NORMAL, uint16_t stepsPerRevolution=200, float pTerm=10.0, float iTerm=0.0, float dTerm=0.0, uint16_t dropinStepSize=16, bool setHome=true, uint8_t invert=0, uint8_t runCurrent=50, uint8_t holdCurrent=30)
Initializes the different parts of the uStepper S object.
#define VELOCITYCONVERSION
void disablePid(void)
This method disables the PID until calling enablePid.
Function prototypes and definitions for the uStepper S library.
void setDifferential(float D)
This method is used to change the PID differential parameter D.
float angleMoved(void)
Get the angle moved from reference position in degrees.
void setAcceleration(uint32_t acceleration)
Set motor acceleration.
void setMaxAcceleration(float acceleration)
Set the maximum acceleration of the stepper motor.
void setCurrent(double current)
Set motor output current.
int32_t writeRegister(uint8_t address, uint32_t datagram)
Write a register of the motor driver.
bool getMotorState(uint8_t statusType=POSITION_REACHED)
Get the current motor driver state.
friend void interrupt0(void)
Used by dropin feature to take in step pulses.