Go to the documentation of this file.
77 SPCR1 = (1<<SPE1)|(1<<MSTR1);
98 return (
float)velocity * this->
velToRpm;
104 uint8_t inverted = 0;
105 uint8_t noninverted = 0;
115 startAngle -= distance/2.0;
130 startAngle += distance/2.0;
145 startAngle -= distance/2.0;
159 if(inverted > noninverted)
168 uint16_t stepsPerRevolution,
216 digitalWrite(2,HIGH);
217 digitalWrite(3,HIGH);
218 digitalWrite(4,HIGH);
229 tempSettings.
invert = invert;
308 steps = (int32_t)( (abs(diff) *
angleToStep) + 0.5);
359 return ( stats >> 13 );
365 registerContent &= ~(3UL << 20);
394 uint32_t velocity = abs(velocityDir);
405 SPCR1 &= ~(1<<CPHA1);
420 while(!( SPSR1 & (1 << SPIF1) ));
462 if( current <= 100.0 && current >= 0.0){
476 if( current <= 100.0 && current >= 0.0){
704 static float integral;
705 static bool integralReset = 0;
706 static float errorOld, differential = 0.0;
710 u = error*this->
pTerm;
727 integral += error*this->
iTerm;
729 if(integral > 200000.0)
733 else if(integral < -200000.0)
735 integral = -200000.0;
738 if(error > -10 && error < 10)
754 differential += 0.1*((error - errorOld)*this->
dTerm);
791 if(cmd->charAt(2) ==
';')
793 Serial.println(
"COMMAND NOT ACCEPTED");
802 if(cmd->substring(0,2) == String(
"P="))
806 if(cmd->charAt(i) >=
'0' && cmd->charAt(i) <=
'9')
808 value.concat(cmd->charAt(i));
810 else if(cmd->charAt(i) ==
'.')
812 value.concat(cmd->charAt(i));
816 else if(cmd->charAt(i) ==
';')
822 Serial.println(
"COMMAND NOT ACCEPTED");
829 if(cmd->charAt(i) >=
'0' && cmd->charAt(i) <=
'9')
831 value.concat(cmd->charAt(i));
833 else if(cmd->charAt(i) ==
';')
835 Serial.print(
"COMMAND ACCEPTED. P = ");
836 Serial.println(value.toFloat(),4);
844 Serial.println(
"COMMAND NOT ACCEPTED");
854 else if(cmd->substring(0,2) == String(
"I="))
858 if(cmd->charAt(i) >=
'0' && cmd->charAt(i) <=
'9')
860 value.concat(cmd->charAt(i));
862 else if(cmd->charAt(i) ==
'.')
864 value.concat(cmd->charAt(i));
868 else if(cmd->charAt(i) ==
';')
874 Serial.println(
"COMMAND NOT ACCEPTED");
881 if(cmd->charAt(i) >=
'0' && cmd->charAt(i) <=
'9')
883 value.concat(cmd->charAt(i));
885 else if(cmd->charAt(i) ==
';')
887 Serial.print(
"COMMAND ACCEPTED. I = ");
888 Serial.println(value.toFloat(),4);
896 Serial.println(
"COMMAND NOT ACCEPTED");
906 else if(cmd->substring(0,2) == String(
"D="))
910 if(cmd->charAt(i) >=
'0' && cmd->charAt(i) <=
'9')
912 value.concat(cmd->charAt(i));
914 else if(cmd->charAt(i) ==
'.')
916 value.concat(cmd->charAt(i));
920 else if(cmd->charAt(i) ==
';')
926 Serial.println(
"COMMAND NOT ACCEPTED");
933 if(cmd->charAt(i) >=
'0' && cmd->charAt(i) <=
'9')
935 value.concat(cmd->charAt(i));
937 else if(cmd->charAt(i) ==
';')
939 Serial.print(
"COMMAND ACCEPTED. D = ");
940 Serial.println(value.toFloat(),4);
948 Serial.println(
"COMMAND NOT ACCEPTED");
958 else if(cmd->substring(0,6) == String(
"invert"))
960 if(cmd->charAt(6) !=
';')
962 Serial.println(
"COMMAND NOT ACCEPTED");
967 Serial.println(F(
"Direction normal!"));
975 Serial.println(F(
"Direction inverted!"));
987 else if(cmd->substring(0,5) == String(
"error"))
989 if(cmd->charAt(5) !=
';')
991 Serial.println(
"COMMAND NOT ACCEPTED");
994 Serial.print(F(
"Current Error: "));
996 Serial.println(F(
" Steps"));
1003 else if(cmd->substring(0,7) == String(
"current"))
1005 if(cmd->charAt(7) !=
';')
1007 Serial.println(
"COMMAND NOT ACCEPTED");
1010 Serial.print(F(
"Run Current: "));
1012 Serial.println(F(
" %"));
1013 Serial.print(F(
"Hold Current: "));
1015 Serial.println(F(
" %"));
1022 else if(cmd->substring(0,10) == String(
"parameters"))
1024 if(cmd->charAt(10) !=
';')
1026 Serial.println(
"COMMAND NOT ACCEPTED");
1029 Serial.print(F(
"P: "));
1031 Serial.print(F(
", "));
1032 Serial.print(F(
"I: "));
1034 Serial.print(F(
", "));
1035 Serial.print(F(
"D: "));
1043 else if(cmd->substring(0,4) == String(
"help"))
1045 if(cmd->charAt(4) !=
';')
1047 Serial.println(
"COMMAND NOT ACCEPTED");
1057 else if(cmd->substring(0,11) == String(
"runCurrent="))
1061 if(cmd->charAt(i) >=
'0' && cmd->charAt(i) <=
'9')
1063 value.concat(cmd->charAt(i));
1065 else if(cmd->charAt(i) ==
'.')
1067 value.concat(cmd->charAt(i));
1071 else if(cmd->charAt(i) ==
';')
1077 Serial.println(
"COMMAND NOT ACCEPTED");
1084 if(cmd->charAt(i) >=
'0' && cmd->charAt(i) <=
'9')
1086 value.concat(cmd->charAt(i));
1088 else if(cmd->charAt(i) ==
';')
1090 if(value.toFloat() >= 0.0 && value.toFloat() <= 100.0)
1092 i = (uint8_t)value.toFloat();
1097 Serial.println(
"COMMAND NOT ACCEPTED");
1103 Serial.println(
"COMMAND NOT ACCEPTED");
1108 Serial.print(
"COMMAND ACCEPTED. runCurrent = ");
1110 Serial.println(F(
" %"));
1120 else if(cmd->substring(0,12) == String(
"holdCurrent="))
1124 if(cmd->charAt(i) >=
'0' && cmd->charAt(i) <=
'9')
1126 value.concat(cmd->charAt(i));
1128 else if(cmd->charAt(i) ==
'.')
1130 value.concat(cmd->charAt(i));
1134 else if(cmd->charAt(i) ==
';')
1140 Serial.println(
"COMMAND NOT ACCEPTED");
1147 if(cmd->charAt(i) >=
'0' && cmd->charAt(i) <=
'9')
1149 value.concat(cmd->charAt(i));
1151 else if(cmd->charAt(i) ==
';')
1153 if(value.toFloat() >= 0.0 && value.toFloat() <= 100.0)
1155 i = (uint8_t)value.toFloat();
1160 Serial.println(
"COMMAND NOT ACCEPTED");
1166 Serial.println(
"COMMAND NOT ACCEPTED");
1171 Serial.print(
"COMMAND ACCEPTED. holdCurrent = ");
1173 Serial.println(F(
" %"));
1185 Serial.println(
"COMMAND NOT ACCEPTED");
1193 static String stringInput;
1194 static uint32_t t = millis();
1198 while(!Serial.available())
1201 if((millis() - t) >= 500)
1203 stringInput.remove(0);
1208 stringInput += (char)Serial.read();
1209 if(stringInput.lastIndexOf(
';') > -1)
1212 stringInput.remove(0);
1219 Serial.println(F(
"uStepper S Dropin !"));
1220 Serial.println(F(
""));
1221 Serial.println(F(
"Usage:"));
1222 Serial.println(F(
"Show this command list: 'help;'"));
1223 Serial.println(F(
"Get PID Parameters: 'parameters;'"));
1224 Serial.println(F(
"Set Proportional constant: 'P=10.002;'"));
1225 Serial.println(F(
"Set Integral constant: 'I=10.002;'"));
1226 Serial.println(F(
"Set Differential constant: 'D=10.002;'"));
1227 Serial.println(F(
"Invert Direction: 'invert;'"));
1228 Serial.println(F(
"Get Current PID Error: 'error;'"));
1229 Serial.println(F(
"Get Run/Hold Current Settings: 'current;'"));
1230 Serial.println(F(
"Set Run Current (percent): 'runCurrent=50.0;'"));
1231 Serial.println(F(
"Set Hold Current (percent): 'holdCurrent=50.0;'"));
1232 Serial.println(F(
""));
1233 Serial.println(F(
""));
1240 EEPROM.get(0,tempSettings);
1267 uint8_t checksum = 0xAA;
1268 uint8_t *p = (uint8_t*)settings;
1270 for(i=0; i < 15; i++)
volatile posFilter_t externalStepInputFilter
Prototype of class for accessing all features of the uStepper S in a single object.
void init(uStepperS *_pointer)
Initiation of the motor driver.
void disableStallguard(void)
void moveSteps(int32_t steps)
Make the motor perform a predefined number of steps.
bool getMotorState(uint8_t statusType=POSITION_REACHED)
Get the current motor driver state.
void setAcceleration(uint32_t acceleration)
Set motor acceleration.
int32_t readRegister(uint8_t address)
Reads a register from the motor driver.
void setBrakeMode(uint8_t mode, float brakeCurrent=25.0)
void readMotorStatus(void)
void enableStallguard(int8_t threshold=4, bool stopOnStall=false, float rpm=10.0)
Enable TMC5130 StallGuard.
volatile int32_t angleMoved
int32_t getVelocity(void)
Returns the current speed of the motor driver.
float getAngleMoved(bool filtered=true)
Returns the angle moved from reference position in degrees.
float getDriverRPM(void)
Get the RPM from driver.
Struct to store dropin settings.
void setVelocity(uint32_t velocity)
Set motor velocity.
void setSPIMode(uint8_t mode)
uint16_t captureAngle(void)
Capture the current shaft angle.
#define ACCELERATIONCONVERSION
void disableStallguard(void)
Disables the builtin stallguard offered from TMC5130, and reenables StealthChop.
void runContinous(bool dir)
Make the motor rotate continuously.
void disablePid(void)
This method disables the PID until calling enablePid.
bool isStalled(void)
This method returns a bool variable indicating wether the motor is stalled or not....
float stepsPerSecondToRPM
void dropinCli()
This method is used to tune Drop-in parameters. After tuning uStepper S, the parameters are saved in ...
volatile posFilter_t encoderFilter
volatile float controlThreshold
void stop(bool mode=HARD)
Stop the motor.
void updateCurrent(void)
Writes the current setting registers of the motor driver
void filterSpeedPos(posFilter_t *filter, int32_t steps)
float getPidError(void)
This method returns the current PID error.
#define ENCODERDATATOSTEP
void setRPM(float rpm)
Set the velocity in rpm.
void disableClosedLoop(void)
This method disables the closed loop mode until calling enableClosedLoop.
volatile bool pidDisabled
void parseCommand(String *cmd)
This method is used for the dropinCli to take in user commands.
float moveToEnd(bool dir, float rpm=40.0, int8_t threshold=4)
Moves the motor to its physical limit, without limit switch.
void setIntegral(float I)
This method is used to change the PID integral parameter I.
float RPMToStepsPerSecond
void clearStall(void)
Clear the stallguard, reenabling the motor to return to its previous operation.
void init(void)
Internal function to prepare the uStepperS in the constructor.
bool loadDropinSettings(void)
void setPosition(int32_t position)
Set the motor position.
void setDifferential(float D)
This method is used to change the PID differential parameter D.
void interrupt1(void)
Used by dropin feature to take in enable signal.
void setMaxVelocity(float velocity)
Set the maximum velocity of the stepper motor.
uint8_t dropinSettingsCalcChecksum(dropinCliSettings_t *settings)
void enableClosedLoop(void)
This method reenables the closed loop mode after being disabled.
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.
friend void interrupt0(void)
Used by dropin feature to take in step pulses.
void init(uStepperS *_pointer)
Initiation of the encoder.
int32_t getPosition(void)
Returns the current position of the motor driver.
void saveDropinSettings(void)
void enableStallguard(int8_t threshold, bool stopOnStall, float rpm)
void setMaxAcceleration(float acceleration)
Set the maximum acceleration of the stepper motor.
float angleMoved(void)
Get the angle moved from reference position in degrees.
#define VELOCITYCONVERSION
void invertDropinDir(bool invert)
This method is used to invert the drop-in direction pin interpretation.
void moveAngle(float angle)
Makes the motor rotate a specific angle relative to the current position.
void setHome(float initialAngle=0)
Define new reference(home) position.
dropinCliSettings_t dropinSettings
uint8_t SPI(uint8_t data)
Struct for encoder velocity estimator.
void setDirection(bool direction)
void checkOrientation(float distance=10)
This method is used to check the orientation of the motor connector.
#define VELOCITY_MODE_POS
void setHoldCurrent(double current)
Set motor hold current.
uStepperS()
Constructor of uStepper class.
void moveToAngle(float angle)
Makes the motor rotate to a specific absolute angle.
void dropinPrintHelp()
This method is used to print the dropinCli menu explainer:
void setControlThreshold(float threshold)
This method sets the control threshold for the closed loop position control in microsteps - i....
volatile float currentPidError
void setProportional(float P)
This method is used to change the PID proportional parameter P.
void enablePid(void)
This method reenables the PID after being disabled.
bool invertPidDropinDirection
void setShaftDirection(bool direction)
Set motor driver direction.
void setDeceleration(uint32_t deceleration)
Set motor deceleration.
void setMaxDeceleration(float deceleration)
Set the maximum deceleration of the stepper motor.
void TIMER1_COMPA_vect(void)
void setCurrent(double current)
Set motor output current.
void setRampMode(uint8_t mode)
Set motor driver to position mode or velocity mode.
int32_t writeRegister(uint8_t address, uint32_t datagram)
Write a register of the motor driver.
void interrupt0(void)
Used by dropin feature to take in step pulses.