uStepper S
uStepperS.h
Go to the documentation of this file.
1 /********************************************************************************************
2 * File: uStepperS.h *
3 * Version: 2.2.0 *
4 * Date: September 22nd, 2020 *
5 * Authors: Thomas Hørring Olsen *
6 * Emil Jacobsen *
7 * *
8 *********************************************************************************************
9 * (C) 2020 *
10 * *
11 * uStepper ApS *
12 * www.ustepper.com *
13 * administration@ustepper.com *
14 * *
15 * The code contained in this file is released under the following open source license: *
16 * *
17 * Creative Commons Attribution-NonCommercial-ShareAlike 4.0 International *
18 * *
19 * The code in this file is provided without warranty of any kind - use at own risk! *
20 * neither uStepper ApS nor the author, can be held responsible for any damage *
21 * caused by the use of the code contained in this file ! *
22 * *
23 ********************************************************************************************/
138 #ifndef _USTEPPER_S_H_
139 #define _USTEPPER_S_H_
140 
141 #ifndef ARDUINO_AVR_USTEPPER_S
142  #error !!This library only supports the uStepper S board!!
143 #endif
144 
145 #ifndef __AVR_ATmega328PB__
146  #error !!This library only supports the ATmega328PB MCU!!
147 #endif
148 
149 #include <avr/io.h>
150 #include <avr/interrupt.h>
151 #include <Arduino.h>
152 #include <EEPROM.h>
153 #include <inttypes.h>
154 #include <uStepperServo.h>
155 
156 #define FREEWHEELBRAKE 0
157 #define COOLBRAKE 1
158 #define HARDBRAKE 2
160 #define CW 1
161 #define CCW 0
163 #define POSITION_REACHED 0x20
164 #define VELOCITY_REACHED 0x10
165 #define STANDSTILL 0x08
166 #define STALLGUARD2 0x04
172 typedef union
173 {
174  float f;
175  uint8_t bytes[4];
176 }floatBytes_t;
177 
186 typedef struct
187 {
191  uint8_t invert;
192  uint8_t holdCurrent;
193  uint8_t runCurrent;
194  uint8_t checksum;
196 
202 typedef struct
203 {
204  float posError = 0.0;
205  float posEst = 0.0;
206  float velIntegrator = 0.0;
207  float velEst = 0.0;
208 }posFilter_t;
209 
210 
211 class uStepperS;
212 #include <uStepperEncoder.h>
213 #include <uStepperDriver.h>
214 
215 #define HARD 0
216 #define SOFT 1
218 #define DRV_ENN PD4
219 #define SD_MODE PD5
220 #define SPI_MODE PD6
222 #define CS_DRIVER PE2
223 #define CS_ENCODER PD7
225 #define MOSI1 PE3
226 #define MOSI_ENC PC2
227 #define MISO1 PC0
228 #define SCK1 PC1
231 #define NORMAL 0
232 #define DROPIN 1
233 #define CLOSEDLOOP 2
234 #define PID CLOSEDLOOP
236 #define CLOCKFREQ 16000000.0
242 #define ENCODERINTFREQ 1000
243 #define ENCODERINTPERIOD 1.0/ENCODERINTFREQ
244 #define PULSEFILTERKP 120.0
245 #define PULSEFILTERKI 1900.0*ENCODERINTPERIOD
252 extern "C" void TIMER1_COMPA_vect(void) __attribute__ ((signal,used));
253 
260 void interrupt0(void);
261 
268 void interrupt1(void);
269 
270 
279 {
280 
281 friend class uStepperDriver;
282 friend class uStepperEncoder;
283 friend void interrupt0(void);
284 friend void TIMER1_COMPA_vect(void) __attribute__ ((signal,used));
285 public:
286 
289 
292 
296  uStepperS();
297 
301  uStepperS(float acceleration, float velocity);
302 
306  void init( void );
307 
343  void setup( uint8_t mode = NORMAL,
344  uint16_t stepsPerRevolution = 200,
345  float pTerm = 10.0,
346  float iTerm = 0.0,
347  float dTerm = 0.0,
348  uint16_t dropinStepSize = 16,
349  bool setHome = true,
350  uint8_t invert = 0,
351  uint8_t runCurrent = 50,
352  uint8_t holdCurrent = 30);
353 
362  void setRPM( float rpm );
363 
371  float getDriverRPM( void );
372 
381  void setMaxAcceleration ( float acceleration );
382 
391  void setMaxDeceleration ( float deceleration );
392 
401  void setMaxVelocity ( float velocity );
402 
410  void setCurrent( double current );
411 
419  void setHoldCurrent( double current );
420 
435  void moveSteps( int32_t steps );
436 
452  void moveAngle( float angle );
453 
469  void moveToAngle( float angle );
470 
479  void runContinous( bool dir );
480 
486  float angleMoved( void );
487 
503  bool getMotorState(uint8_t statusType = POSITION_REACHED);
504 
515  void stop( bool mode = HARD );
516 
526  void enableStallguard( int8_t threshold = 4, bool stopOnStall = false, float rpm = 10.0);
527 
531  void disableStallguard( void );
532 
536  void clearStall( void );
537 
544  bool isStalled(void);
545 
546 
559  bool isStalled( int8_t threshold );
560 
573  void setBrakeMode( uint8_t mode, float brakeCurrent = 25.0 );
574 
579  void enablePid(void);
580 
585  void disablePid(void);
586 
591  void enableClosedLoop(void);
592 
597  void disableClosedLoop(void);
598 
603  void setControlThreshold(float threshold);
604 
620  float moveToEnd(bool dir, float rpm = 40.0, int8_t threshold = 4);
621 
626  float getPidError(void);
627 
634  void setProportional(float P);
635 
642  void setIntegral(float I);
643 
650  void setDifferential(float D);
651 
658  void invertDropinDir(bool invert);
659 
675  void dropinCli();
676 
683  void parseCommand(String *cmd);
684 
701  void dropinPrintHelp();
702 
710  void checkOrientation(float distance = 10);
711 
712 private:
713 
714  float stepTime;
715  float rpmToVel;
716  float velToRpm;
717 
722  float maxVelocity;
723 
732  float angleToStep;
733 
734  uint16_t microSteps;
735  uint16_t fullSteps;
736 
737  uint16_t dropinStepSize;
738 
739  int32_t stepCnt;
740 
743 
745 
749  volatile uint8_t mode;
750  float pTerm;
752  float iTerm;
753 
754  float dTerm;
755  bool brake;
756  volatile bool pidDisabled;
758  volatile float controlThreshold = 10;
761  volatile bool stall;
762  // SPI functions
763 
764  volatile int32_t pidPositionStepsIssued = 0;
765  volatile float currentPidError;
766 
768  int8_t stallThreshold = 4;
769 
771  bool stallStop = false;
772 
774  bool stallEnabled = false;
775 
777  volatile bool shaftDir = 0;
778 
779  uint8_t SPI( uint8_t data );
780 
781  void setSPIMode( uint8_t mode );
782 
783  void chipSelect( uint8_t pin , bool state );
784 
785  void filterSpeedPos(posFilter_t *filter, int32_t steps);
786 
787  float pid(float error);
788 
790  bool loadDropinSettings(void);
791  void saveDropinSettings(void);
793 };
794 
795 
796 
797 #endif
uStepperS::externalStepInputFilter
volatile posFilter_t externalStepInputFilter
Definition: uStepperS.h:744
uStepperS
Prototype of class for accessing all features of the uStepper S in a single object.
Definition: uStepperS.h:279
uStepperS::dropinStepSize
uint16_t dropinStepSize
Definition: uStepperS.h:737
uStepperDriver
Prototype of class for the TMC5130 Driver.
Definition: uStepperDriver.h:151
uStepperS::stepTime
float stepTime
Definition: uStepperS.h:714
dropinCliSettings_t::P
floatBytes_t P
Definition: uStepperS.h:188
uStepperS::stall
volatile bool stall
Definition: uStepperS.h:761
uStepperS::velToRpm
float velToRpm
Definition: uStepperS.h:716
uStepperS::moveSteps
void moveSteps(int32_t steps)
Make the motor perform a predefined number of steps.
Definition: uStepperS.cpp:270
uStepperS::getMotorState
bool getMotorState(uint8_t statusType=POSITION_REACHED)
Get the current motor driver state.
Definition: uStepperS.cpp:84
uStepperEncoder::angle
volatile uint16_t angle
Definition: uStepperEncoder.h:209
uStepperS::stallEnabled
bool stallEnabled
Definition: uStepperS.h:774
uStepperS::setBrakeMode
void setBrakeMode(uint8_t mode, float brakeCurrent=25.0)
Definition: uStepperS.cpp:362
uStepperS::TIMER1_COMPA_vect
friend void TIMER1_COMPA_vect(void) __attribute__((signal
Interrupt routine for critical tasks.
Definition: uStepperS.cpp:593
uStepperS::chipSelect
void chipSelect(uint8_t pin, bool state)
uStepperS::enableStallguard
void enableStallguard(int8_t threshold=4, bool stopOnStall=false, float rpm=10.0)
Enable TMC5130 StallGuard.
Definition: uStepperS.cpp:320
uStepperS::dTerm
float dTerm
Definition: uStepperS.h:754
uStepperS::getDriverRPM
float getDriverRPM(void)
Get the RPM from driver.
Definition: uStepperS.cpp:94
uStepperS::rpmToVelocity
float rpmToVelocity
Definition: uStepperS.h:731
dropinCliSettings_t
Struct to store dropin settings.
Definition: uStepperS.h:187
uStepperS::setSPIMode
void setSPIMode(uint8_t mode)
Definition: uStepperS.cpp:400
uStepperS::disableStallguard
void disableStallguard(void)
Disables the builtin stallguard offered from TMC5130, and reenables StealthChop.
Definition: uStepperS.cpp:331
dropinCliSettings_t::D
floatBytes_t D
Definition: uStepperS.h:190
uStepperS::runContinous
void runContinous(bool dir)
Make the motor rotate continuously.
Definition: uStepperS.cpp:487
floatBytes_t
Union to easily split a float into its binary representation.
Definition: uStepperS.h:173
uStepperS::disablePid
void disablePid(void)
This method disables the PID until calling enablePid.
Definition: uStepperS.cpp:647
uStepperS::fullSteps
uint16_t fullSteps
Definition: uStepperS.h:735
uStepperS::pTerm
float pTerm
Definition: uStepperS.h:750
uStepperS::maxAcceleration
float maxAcceleration
Definition: uStepperS.h:728
uStepperS::brake
bool brake
Definition: uStepperS.h:755
uStepperS::isStalled
bool isStalled(void)
This method returns a bool variable indicating wether the motor is stalled or not....
Definition: uStepperS.cpp:343
uStepperS::stepsPerSecondToRPM
float stepsPerSecondToRPM
Definition: uStepperS.h:741
uStepperS::dropinCli
void dropinCli()
This method is used to tune Drop-in parameters. After tuning uStepper S, the parameters are saved in ...
Definition: uStepperS.cpp:1191
uStepperDriver.h
Function prototypes and definitions for the TMC5130 motor driver.
uStepperS::stepCnt
int32_t stepCnt
Definition: uStepperS.h:739
uStepperS::controlThreshold
volatile float controlThreshold
Definition: uStepperS.h:758
uStepperS::stop
void stop(bool mode=HARD)
Stop the motor.
Definition: uStepperS.cpp:505
uStepperS::encoder
uStepperEncoder encoder
Definition: uStepperS.h:291
interrupt1
void interrupt1(void)
Used by dropin feature to take in enable signal.
Definition: uStepperS.cpp:546
uStepperS::filterSpeedPos
void filterSpeedPos(posFilter_t *filter, int32_t steps)
Definition: uStepperS.cpp:522
uStepperS::getPidError
float getPidError(void)
This method returns the current PID error.
Definition: uStepperS.cpp:695
dropinCliSettings_t::checksum
uint8_t checksum
Definition: uStepperS.h:194
NORMAL
#define NORMAL
Definition: uStepperS.h:231
uStepperS::setRPM
void setRPM(float rpm)
Set the velocity in rpm.
Definition: uStepperS.cpp:383
interrupt0
void interrupt0(void)
Used by dropin feature to take in step pulses.
Definition: uStepperS.cpp:558
uStepperS::disableClosedLoop
void disableClosedLoop(void)
This method disables the closed loop mode until calling enableClosedLoop.
Definition: uStepperS.cpp:659
uStepperS::microSteps
uint16_t microSteps
Definition: uStepperS.h:734
uStepperS::iTerm
float iTerm
Definition: uStepperS.h:752
uStepperS::pidDisabled
volatile bool pidDisabled
Definition: uStepperS.h:756
uStepperS::parseCommand
void parseCommand(String *cmd)
This method is used for the dropinCli to take in user commands.
Definition: uStepperS.cpp:786
floatBytes_t::f
float f
Definition: uStepperS.h:174
uStepperS::moveToEnd
float moveToEnd(bool dir, float rpm=40.0, int8_t threshold=4)
Moves the motor to its physical limit, without limit switch.
Definition: uStepperS.cpp:664
uStepperS::setIntegral
void setIntegral(float I)
This method is used to change the PID integral parameter I.
Definition: uStepperS.cpp:771
uStepperS::RPMToStepsPerSecond
float RPMToStepsPerSecond
Definition: uStepperS.h:742
uStepperS::clearStall
void clearStall(void)
Clear the stallguard, reenabling the motor to return to its previous operation.
Definition: uStepperS.cpp:338
dropinCliSettings_t::invert
uint8_t invert
Definition: uStepperS.h:191
uStepperServo.h
Function prototypes and definitions for the uStepper Servo library.
uStepperS::init
void init(void)
Internal function to prepare the uStepperS in the constructor.
Definition: uStepperS.cpp:59
uStepperS::loadDropinSettings
bool loadDropinSettings(void)
Definition: uStepperS.cpp:1236
uStepperS::setDifferential
void setDifferential(float D)
This method is used to change the PID differential parameter D.
Definition: uStepperS.cpp:776
dropinCliSettings_t::I
floatBytes_t I
Definition: uStepperS.h:189
uStepperS::setMaxVelocity
void setMaxVelocity(float velocity)
Set the maximum velocity of the stepper motor.
Definition: uStepperS.cpp:426
uStepperS::dropinSettingsCalcChecksum
uint8_t dropinSettingsCalcChecksum(dropinCliSettings_t *settings)
Definition: uStepperS.cpp:1264
uStepperS::enableClosedLoop
void enableClosedLoop(void)
This method reenables the closed loop mode after being disabled.
Definition: uStepperS.cpp:654
uStepperS::setup
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.
Definition: uStepperS.cpp:167
uStepperS::interrupt0
friend void interrupt0(void)
Used by dropin feature to take in step pulses.
Definition: uStepperS.cpp:558
dropinCliSettings_t::holdCurrent
uint8_t holdCurrent
Definition: uStepperS.h:192
uStepperS::pid
float pid(float error)
Definition: uStepperS.cpp:700
uStepperS::mode
volatile uint8_t mode
Definition: uStepperS.h:749
uStepperS::angleToStep
float angleToStep
Definition: uStepperS.h:732
uStepperS::stallStop
bool stallStop
Definition: uStepperS.h:771
uStepperS::maxVelocity
float maxVelocity
Definition: uStepperS.h:722
dropinCliSettings_t::runCurrent
uint8_t runCurrent
Definition: uStepperS.h:193
uStepperEncoder
Prototype of class for the AEAT8800-Q24 encoder.
Definition: uStepperEncoder.h:52
uStepperS::saveDropinSettings
void saveDropinSettings(void)
Definition: uStepperS.cpp:1257
uStepperS::setMaxAcceleration
void setMaxAcceleration(float acceleration)
Set the maximum acceleration of the stepper motor.
Definition: uStepperS.cpp:437
uStepperS::angleMoved
float angleMoved(void)
Get the angle moved from reference position in degrees.
Definition: uStepperS.cpp:500
uStepperS::driver
uStepperDriver driver
Definition: uStepperS.h:288
uStepperS::invertDropinDir
void invertDropinDir(bool invert)
This method is used to invert the drop-in direction pin interpretation.
Definition: uStepperS.cpp:781
uStepperS::stallThreshold
int8_t stallThreshold
Definition: uStepperS.h:768
uStepperS::rpmToVel
float rpmToVel
Definition: uStepperS.h:715
uStepperS::shaftDir
volatile bool shaftDir
Definition: uStepperS.h:777
uStepperEncoder.h
Function prototypes and definitions for the AEAT8800-Q24 Encoder.
uStepperS::currentPidSpeed
float currentPidSpeed
Definition: uStepperS.h:746
uStepperS::moveAngle
void moveAngle(float angle)
Makes the motor rotate a specific angle relative to the current position.
Definition: uStepperS.cpp:285
uStepperEncoder::setHome
void setHome(float initialAngle=0)
Define new reference(home) position.
Definition: uStepperEncoder.cpp:82
uStepperS::dropinSettings
dropinCliSettings_t dropinSettings
Definition: uStepperS.h:789
uStepperS::SPI
uint8_t SPI(uint8_t data)
Definition: uStepperS.cpp:415
posFilter_t
Struct for encoder velocity estimator.
Definition: uStepperS.h:203
uStepperS::checkOrientation
void checkOrientation(float distance=10)
This method is used to check the orientation of the motor connector.
Definition: uStepperS.cpp:101
uStepperS::pidPositionStepsIssued
volatile int32_t pidPositionStepsIssued
Definition: uStepperS.h:764
uStepperS::setHoldCurrent
void setHoldCurrent(double current)
Set motor hold current.
Definition: uStepperS.cpp:473
uStepperS::uStepperS
uStepperS()
Constructor of uStepper class.
Definition: uStepperS.cpp:36
uStepperS::moveToAngle
void moveToAngle(float angle)
Makes the motor rotate to a specific absolute angle.
Definition: uStepperS.cpp:302
uStepperS::dropinPrintHelp
void dropinPrintHelp()
This method is used to print the dropinCli menu explainer:
Definition: uStepperS.cpp:1217
uStepperS::setControlThreshold
void setControlThreshold(float threshold)
This method sets the control threshold for the closed loop position control in microsteps - i....
Definition: uStepperS.cpp:636
uStepperS::currentPidError
volatile float currentPidError
Definition: uStepperS.h:765
POSITION_REACHED
#define POSITION_REACHED
Definition: uStepperS.h:163
uStepperS::used
friend void used
Definition: uStepperS.h:284
uStepperS::maxDeceleration
float maxDeceleration
Definition: uStepperS.h:729
uStepperS::setProportional
void setProportional(float P)
This method is used to change the PID proportional parameter P.
Definition: uStepperS.cpp:766
uStepperS::enablePid
void enablePid(void)
This method reenables the PID after being disabled.
Definition: uStepperS.cpp:640
uStepperS::invertPidDropinDirection
bool invertPidDropinDirection
Definition: uStepperS.h:730
HARD
#define HARD
Definition: uStepperS.h:215
uStepperS::setMaxDeceleration
void setMaxDeceleration(float deceleration)
Set the maximum deceleration of the stepper motor.
Definition: uStepperS.cpp:449
uStepperS::setCurrent
void setCurrent(double current)
Set motor output current.
Definition: uStepperS.cpp:460