Go to the documentation of this file.
76 SPCR1 = (1<<SPE1)|(1<<MSTR1);
97 uint8_t noninverted = 0;
107 startAngle -= distance/2.0;
122 startAngle += distance/2.0;
137 startAngle -= distance/2.0;
151 if(inverted > noninverted)
160 uint16_t stepsPerRevolution,
202 digitalWrite(2,HIGH);
203 digitalWrite(3,HIGH);
204 digitalWrite(4,HIGH);
215 tempSettings.
invert = invert;
294 steps = (int32_t)( (abs(diff) *
angleToStep) + 0.5);
345 return ( stats >> 13 );
351 registerContent &= ~(3UL << 20);
380 uint32_t velocity = abs(velocityDir);
391 SPCR1 &= ~(1<<CPHA1);
406 while(!( SPSR1 & (1 << SPIF1) ));
448 if( current <= 100.0 && current >= 0.0){
462 if( current <= 100.0 && current >= 0.0){
690 static float integral;
691 static bool integralReset = 0;
692 static float errorOld, differential = 0.0;
696 u = error*this->
pTerm;
713 integral += error*this->
iTerm;
715 if(integral > 200000.0)
719 else if(integral < -200000.0)
721 integral = -200000.0;
724 if(error > -10 && error < 10)
740 differential += 0.1*((error - errorOld)*this->
dTerm);
777 if(cmd->charAt(2) ==
';')
779 Serial.println(
"COMMAND NOT ACCEPTED");
788 if(cmd->substring(0,2) == String(
"P="))
792 if(cmd->charAt(i) >=
'0' && cmd->charAt(i) <=
'9')
794 value.concat(cmd->charAt(i));
796 else if(cmd->charAt(i) ==
'.')
798 value.concat(cmd->charAt(i));
802 else if(cmd->charAt(i) ==
';')
808 Serial.println(
"COMMAND NOT ACCEPTED");
815 if(cmd->charAt(i) >=
'0' && cmd->charAt(i) <=
'9')
817 value.concat(cmd->charAt(i));
819 else if(cmd->charAt(i) ==
';')
821 Serial.print(
"COMMAND ACCEPTED. P = ");
822 Serial.println(value.toFloat(),4);
830 Serial.println(
"COMMAND NOT ACCEPTED");
840 else if(cmd->substring(0,2) == String(
"I="))
844 if(cmd->charAt(i) >=
'0' && cmd->charAt(i) <=
'9')
846 value.concat(cmd->charAt(i));
848 else if(cmd->charAt(i) ==
'.')
850 value.concat(cmd->charAt(i));
854 else if(cmd->charAt(i) ==
';')
860 Serial.println(
"COMMAND NOT ACCEPTED");
867 if(cmd->charAt(i) >=
'0' && cmd->charAt(i) <=
'9')
869 value.concat(cmd->charAt(i));
871 else if(cmd->charAt(i) ==
';')
873 Serial.print(
"COMMAND ACCEPTED. I = ");
874 Serial.println(value.toFloat(),4);
882 Serial.println(
"COMMAND NOT ACCEPTED");
892 else if(cmd->substring(0,2) == String(
"D="))
896 if(cmd->charAt(i) >=
'0' && cmd->charAt(i) <=
'9')
898 value.concat(cmd->charAt(i));
900 else if(cmd->charAt(i) ==
'.')
902 value.concat(cmd->charAt(i));
906 else if(cmd->charAt(i) ==
';')
912 Serial.println(
"COMMAND NOT ACCEPTED");
919 if(cmd->charAt(i) >=
'0' && cmd->charAt(i) <=
'9')
921 value.concat(cmd->charAt(i));
923 else if(cmd->charAt(i) ==
';')
925 Serial.print(
"COMMAND ACCEPTED. D = ");
926 Serial.println(value.toFloat(),4);
934 Serial.println(
"COMMAND NOT ACCEPTED");
944 else if(cmd->substring(0,6) == String(
"invert"))
946 if(cmd->charAt(6) !=
';')
948 Serial.println(
"COMMAND NOT ACCEPTED");
953 Serial.println(F(
"Direction normal!"));
961 Serial.println(F(
"Direction inverted!"));
973 else if(cmd->substring(0,5) == String(
"error"))
975 if(cmd->charAt(5) !=
';')
977 Serial.println(
"COMMAND NOT ACCEPTED");
980 Serial.print(F(
"Current Error: "));
982 Serial.println(F(
" Steps"));
989 else if(cmd->substring(0,7) == String(
"current"))
991 if(cmd->charAt(7) !=
';')
993 Serial.println(
"COMMAND NOT ACCEPTED");
996 Serial.print(F(
"Run Current: "));
998 Serial.println(F(
" %"));
999 Serial.print(F(
"Hold Current: "));
1001 Serial.println(F(
" %"));
1008 else if(cmd->substring(0,10) == String(
"parameters"))
1010 if(cmd->charAt(10) !=
';')
1012 Serial.println(
"COMMAND NOT ACCEPTED");
1015 Serial.print(F(
"P: "));
1017 Serial.print(F(
", "));
1018 Serial.print(F(
"I: "));
1020 Serial.print(F(
", "));
1021 Serial.print(F(
"D: "));
1029 else if(cmd->substring(0,4) == String(
"help"))
1031 if(cmd->charAt(4) !=
';')
1033 Serial.println(
"COMMAND NOT ACCEPTED");
1043 else if(cmd->substring(0,11) == String(
"runCurrent="))
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. runCurrent = ");
1096 Serial.println(F(
" %"));
1106 else if(cmd->substring(0,12) == String(
"holdCurrent="))
1110 if(cmd->charAt(i) >=
'0' && cmd->charAt(i) <=
'9')
1112 value.concat(cmd->charAt(i));
1114 else if(cmd->charAt(i) ==
'.')
1116 value.concat(cmd->charAt(i));
1120 else if(cmd->charAt(i) ==
';')
1126 Serial.println(
"COMMAND NOT ACCEPTED");
1133 if(cmd->charAt(i) >=
'0' && cmd->charAt(i) <=
'9')
1135 value.concat(cmd->charAt(i));
1137 else if(cmd->charAt(i) ==
';')
1139 if(value.toFloat() >= 0.0 && value.toFloat() <= 100.0)
1141 i = (uint8_t)value.toFloat();
1146 Serial.println(
"COMMAND NOT ACCEPTED");
1152 Serial.println(
"COMMAND NOT ACCEPTED");
1157 Serial.print(
"COMMAND ACCEPTED. holdCurrent = ");
1159 Serial.println(F(
" %"));
1171 Serial.println(
"COMMAND NOT ACCEPTED");
1179 static String stringInput;
1180 static uint32_t t = millis();
1184 while(!Serial.available())
1187 if((millis() - t) >= 500)
1189 stringInput.remove(0);
1194 stringInput += (char)Serial.read();
1195 if(stringInput.lastIndexOf(
';') > -1)
1198 stringInput.remove(0);
1205 Serial.println(F(
"uStepper S Dropin !"));
1206 Serial.println(F(
""));
1207 Serial.println(F(
"Usage:"));
1208 Serial.println(F(
"Show this command list: 'help;'"));
1209 Serial.println(F(
"Get PID Parameters: 'parameters;'"));
1210 Serial.println(F(
"Set Proportional constant: 'P=10.002;'"));
1211 Serial.println(F(
"Set Integral constant: 'I=10.002;'"));
1212 Serial.println(F(
"Set Differential constant: 'D=10.002;'"));
1213 Serial.println(F(
"Invert Direction: 'invert;'"));
1214 Serial.println(F(
"Get Current PID Error: 'error;'"));
1215 Serial.println(F(
"Get Run/Hold Current Settings: 'current;'"));
1216 Serial.println(F(
"Set Run Current (percent): 'runCurrent=50.0;'"));
1217 Serial.println(F(
"Set Hold Current (percent): 'holdCurrent=50.0;'"));
1218 Serial.println(F(
""));
1219 Serial.println(F(
""));
1226 EEPROM.get(0,tempSettings);
1253 uint8_t checksum = 0xAA;
1254 uint8_t *p = (uint8_t*)settings;
1256 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
float getAngleMoved(bool filtered=true)
Returns the angle moved from reference position in degrees.
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.