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,
172 uint16_t dropinStepSize,
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){
666 uint32_t timeOutStart = micros();
686 if((micros() - timeOutStart) > (timeOut * 1000))
711 static float integral;
712 static bool integralReset = 0;
713 static float errorOld, differential = 0.0;
717 u = error*this->
pTerm;
734 integral += error*this->
iTerm;
736 if(integral > 200000.0)
740 else if(integral < -200000.0)
742 integral = -200000.0;
745 if(error > -10 && error < 10)
761 differential += 0.1*((error - errorOld)*this->
dTerm);
798 if(cmd->charAt(2) ==
';')
800 Serial.println(
"COMMAND NOT ACCEPTED");
809 if(cmd->substring(0,2) == String(
"P="))
813 if(cmd->charAt(i) >=
'0' && cmd->charAt(i) <=
'9')
815 value.concat(cmd->charAt(i));
817 else if(cmd->charAt(i) ==
'.')
819 value.concat(cmd->charAt(i));
823 else if(cmd->charAt(i) ==
';')
829 Serial.println(
"COMMAND NOT ACCEPTED");
836 if(cmd->charAt(i) >=
'0' && cmd->charAt(i) <=
'9')
838 value.concat(cmd->charAt(i));
840 else if(cmd->charAt(i) ==
';')
842 Serial.print(
"COMMAND ACCEPTED. P = ");
843 Serial.println(value.toFloat(),4);
851 Serial.println(
"COMMAND NOT ACCEPTED");
861 else if(cmd->substring(0,2) == String(
"I="))
865 if(cmd->charAt(i) >=
'0' && cmd->charAt(i) <=
'9')
867 value.concat(cmd->charAt(i));
869 else if(cmd->charAt(i) ==
'.')
871 value.concat(cmd->charAt(i));
875 else if(cmd->charAt(i) ==
';')
881 Serial.println(
"COMMAND NOT ACCEPTED");
888 if(cmd->charAt(i) >=
'0' && cmd->charAt(i) <=
'9')
890 value.concat(cmd->charAt(i));
892 else if(cmd->charAt(i) ==
';')
894 Serial.print(
"COMMAND ACCEPTED. I = ");
895 Serial.println(value.toFloat(),4);
903 Serial.println(
"COMMAND NOT ACCEPTED");
913 else if(cmd->substring(0,2) == String(
"D="))
917 if(cmd->charAt(i) >=
'0' && cmd->charAt(i) <=
'9')
919 value.concat(cmd->charAt(i));
921 else if(cmd->charAt(i) ==
'.')
923 value.concat(cmd->charAt(i));
927 else if(cmd->charAt(i) ==
';')
933 Serial.println(
"COMMAND NOT ACCEPTED");
940 if(cmd->charAt(i) >=
'0' && cmd->charAt(i) <=
'9')
942 value.concat(cmd->charAt(i));
944 else if(cmd->charAt(i) ==
';')
946 Serial.print(
"COMMAND ACCEPTED. D = ");
947 Serial.println(value.toFloat(),4);
955 Serial.println(
"COMMAND NOT ACCEPTED");
965 else if(cmd->substring(0,6) == String(
"invert"))
967 if(cmd->charAt(6) !=
';')
969 Serial.println(
"COMMAND NOT ACCEPTED");
974 Serial.println(F(
"Direction normal!"));
982 Serial.println(F(
"Direction inverted!"));
994 else if(cmd->substring(0,5) == String(
"error"))
996 if(cmd->charAt(5) !=
';')
998 Serial.println(
"COMMAND NOT ACCEPTED");
1001 Serial.print(F(
"Current Error: "));
1003 Serial.println(F(
" Steps"));
1010 else if(cmd->substring(0,7) == String(
"current"))
1012 if(cmd->charAt(7) !=
';')
1014 Serial.println(
"COMMAND NOT ACCEPTED");
1017 Serial.print(F(
"Run Current: "));
1019 Serial.println(F(
" %"));
1020 Serial.print(F(
"Hold Current: "));
1022 Serial.println(F(
" %"));
1029 else if(cmd->substring(0,10) == String(
"parameters"))
1031 if(cmd->charAt(10) !=
';')
1033 Serial.println(
"COMMAND NOT ACCEPTED");
1036 Serial.print(F(
"P: "));
1038 Serial.print(F(
", "));
1039 Serial.print(F(
"I: "));
1041 Serial.print(F(
", "));
1042 Serial.print(F(
"D: "));
1050 else if(cmd->substring(0,4) == String(
"help"))
1052 if(cmd->charAt(4) !=
';')
1054 Serial.println(
"COMMAND NOT ACCEPTED");
1064 else if(cmd->substring(0,11) == String(
"runCurrent="))
1068 if(cmd->charAt(i) >=
'0' && cmd->charAt(i) <=
'9')
1070 value.concat(cmd->charAt(i));
1072 else if(cmd->charAt(i) ==
'.')
1074 value.concat(cmd->charAt(i));
1078 else if(cmd->charAt(i) ==
';')
1084 Serial.println(
"COMMAND NOT ACCEPTED");
1091 if(cmd->charAt(i) >=
'0' && cmd->charAt(i) <=
'9')
1093 value.concat(cmd->charAt(i));
1095 else if(cmd->charAt(i) ==
';')
1097 if(value.toFloat() >= 0.0 && value.toFloat() <= 100.0)
1099 i = (uint8_t)value.toFloat();
1104 Serial.println(
"COMMAND NOT ACCEPTED");
1110 Serial.println(
"COMMAND NOT ACCEPTED");
1115 Serial.print(
"COMMAND ACCEPTED. runCurrent = ");
1117 Serial.println(F(
" %"));
1127 else if(cmd->substring(0,12) == String(
"holdCurrent="))
1131 if(cmd->charAt(i) >=
'0' && cmd->charAt(i) <=
'9')
1133 value.concat(cmd->charAt(i));
1135 else if(cmd->charAt(i) ==
'.')
1137 value.concat(cmd->charAt(i));
1141 else if(cmd->charAt(i) ==
';')
1147 Serial.println(
"COMMAND NOT ACCEPTED");
1154 if(cmd->charAt(i) >=
'0' && cmd->charAt(i) <=
'9')
1156 value.concat(cmd->charAt(i));
1158 else if(cmd->charAt(i) ==
';')
1160 if(value.toFloat() >= 0.0 && value.toFloat() <= 100.0)
1162 i = (uint8_t)value.toFloat();
1167 Serial.println(
"COMMAND NOT ACCEPTED");
1173 Serial.println(
"COMMAND NOT ACCEPTED");
1178 Serial.print(
"COMMAND ACCEPTED. holdCurrent = ");
1180 Serial.println(F(
" %"));
1192 Serial.println(
"COMMAND NOT ACCEPTED");
1200 static String stringInput;
1201 static uint32_t t = millis();
1205 while(!Serial.available())
1208 if((millis() - t) >= 500)
1210 stringInput.remove(0);
1215 stringInput += (char)Serial.read();
1216 if(stringInput.lastIndexOf(
';') > -1)
1219 stringInput.remove(0);
1226 Serial.println(F(
"uStepper S Dropin !"));
1227 Serial.println(F(
""));
1228 Serial.println(F(
"Usage:"));
1229 Serial.println(F(
"Show this command list: 'help;'"));
1230 Serial.println(F(
"Get PID Parameters: 'parameters;'"));
1231 Serial.println(F(
"Set Proportional constant: 'P=10.002;'"));
1232 Serial.println(F(
"Set Integral constant: 'I=10.002;'"));
1233 Serial.println(F(
"Set Differential constant: 'D=10.002;'"));
1234 Serial.println(F(
"Invert Direction: 'invert;'"));
1235 Serial.println(F(
"Get Current PID Error: 'error;'"));
1236 Serial.println(F(
"Get Run/Hold Current Settings: 'current;'"));
1237 Serial.println(F(
"Set Run Current (percent): 'runCurrent=50.0;'"));
1238 Serial.println(F(
"Set Hold Current (percent): 'holdCurrent=50.0;'"));
1239 Serial.println(F(
""));
1240 Serial.println(F(
""));
1247 EEPROM.get(0,tempSettings);
1274 uint8_t checksum = 0xAA;
1275 uint8_t *p = (uint8_t*)settings;
1277 for(i=0; i < 15; i++)
void setAcceleration(uint32_t acceleration)
Set motor acceleration.
void setDirection(bool direction)
void readMotorStatus(void)
void setDeceleration(uint32_t deceleration)
Set motor deceleration.
int32_t getPosition(void)
Returns the current position of the motor driver.
void disableStallguard(void)
void updateCurrent(void)
Writes the current setting registers of the motor driver
void setShaftDirection(bool direction)
Set motor driver direction.
int32_t writeRegister(uint8_t address, uint32_t datagram)
Write a register of the motor driver.
void init(uStepperS *_pointer)
Initiation of the motor driver.
void enableStallguard(int8_t threshold, bool stopOnStall, float rpm)
int32_t readRegister(uint8_t address)
Reads a register from the motor driver.
void setVelocity(uint32_t velocity)
Set motor velocity.
int32_t getVelocity(void)
Returns the current speed of the motor driver.
void setRampMode(uint8_t mode)
Set motor driver to position mode or velocity mode.
void setPosition(int32_t position)
Set the motor position.
volatile int32_t angleMoved
uint16_t captureAngle(void)
Capture the current shaft angle.
float getAngleMoved(bool filtered=true)
Returns the angle moved from reference position in degrees.
void setHome(float initialAngle=0)
Define new reference(home) position.
void init(uStepperS *_pointer)
Initiation of the encoder.
volatile posFilter_t encoderFilter
Prototype of class for accessing all features of the uStepper S in a single object.
void stop(bool mode=HARD)
Stop the motor.
bool loadDropinSettings(void)
void setMaxVelocity(float velocity)
Set the maximum velocity of the stepper motor.
void saveDropinSettings(void)
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.
float angleMoved(void)
Get the angle moved from reference position in degrees.
float moveToEnd(bool dir, float rpm=40.0, int8_t threshold=4, uint32_t timeOut=100000)
Moves the motor to its physical limit, without limit switch.
volatile posFilter_t externalStepInputFilter
volatile float controlThreshold
float getDriverRPM(void)
Get the RPM from driver.
void parseCommand(String *cmd)
This method is used for the dropinCli to take in user commands.
void setDifferential(float D)
This method is used to change the PID differential parameter D.
void dropinCli()
This method is used to tune Drop-in parameters. After tuning uStepper S, the parameters are saved in ...
void setMaxDeceleration(float deceleration)
Set the maximum deceleration of the stepper motor.
float stepsPerSecondToRPM
bool isStalled(void)
This method returns a bool variable indicating wether the motor is stalled or not....
void setMaxAcceleration(float acceleration)
Set the maximum acceleration of the stepper motor.
void setProportional(float P)
This method is used to change the PID proportional parameter P.
bool invertPidDropinDirection
void disableStallguard(void)
Disables the builtin stallguard offered from TMC5130, and reenables StealthChop.
void setIntegral(float I)
This method is used to change the PID integral parameter I.
void setHoldCurrent(double current)
Set motor hold current.
void setRPM(float rpm)
Set the velocity in rpm.
void disablePid(void)
This method disables the PID until calling enablePid.
volatile bool pidDisabled
float RPMToStepsPerSecond
void moveAngle(float angle)
Makes the motor rotate a specific angle relative to the current position.
void enableClosedLoop(void)
This method reenables the closed loop mode after being disabled.
void filterSpeedPos(posFilter_t *filter, int32_t steps)
void setBrakeMode(uint8_t mode, float brakeCurrent=25.0)
friend void interrupt0(void)
Used by dropin feature to take in step pulses.
void enableStallguard(int8_t threshold=4, bool stopOnStall=false, float rpm=10.0)
Enable TMC5130 StallGuard.
void checkOrientation(float distance=10)
This method is used to check the orientation of the motor connector.
void setControlThreshold(float threshold)
This method sets the control threshold for the closed loop position control in microsteps - i....
void setSPIMode(uint8_t mode)
void moveToAngle(float angle)
Makes the motor rotate to a specific absolute angle.
void clearStall(void)
Clear the stallguard, reenabling the motor to return to its previous operation.
dropinCliSettings_t dropinSettings
void init(void)
Internal function to prepare the uStepperS in the constructor.
void runContinous(bool dir)
Make the motor rotate continuously.
uint8_t SPI(uint8_t data)
void enablePid(void)
This method reenables the PID after being disabled.
void setCurrent(double current)
Set motor output current.
void dropinPrintHelp()
This method is used to print the dropinCli menu explainer:
volatile float currentPidError
uint8_t dropinSettingsCalcChecksum(dropinCliSettings_t *settings)
void invertDropinDir(bool invert)
This method is used to invert the drop-in direction pin interpretation.
void disableClosedLoop(void)
This method disables the closed loop mode until calling enableClosedLoop.
uStepperS()
Constructor of uStepper class.
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.
float getPidError(void)
This method returns the current PID error.
Struct to store dropin settings.
Struct for encoder velocity estimator.
#define ACCELERATIONCONVERSION
#define VELOCITY_MODE_POS
#define VELOCITYCONVERSION
#define ENCODERDATATOSTEP
void interrupt0(void)
Used by dropin feature to take in step pulses.
void interrupt1(void)
Used by dropin feature to take in enable signal.
void TIMER1_COMPA_vect(void)