uStepper S
uStepperDriver.cpp
Go to the documentation of this file.
1 /********************************************************************************************
2 * File: uStepperDriver.cpp *
3 * Version: 2.1.0 *
4 * Date: July 11th, 2020 *
5 * Author: Thomas Hørring Olsen *
6 * *
7 *********************************************************************************************
8 * (C) 2020 *
9 * *
10 * uStepper ApS *
11 * www.ustepper.com *
12 * administration@ustepper.com *
13 * *
14 * The code contained in this file is released under the following open source license: *
15 * *
16 * Creative Commons Attribution-NonCommercial-ShareAlike 4.0 International *
17 * *
18 * The code in this file is provided without warranty of any kind - use at own risk! *
19 * neither uStepper ApS nor the author, can be held responsible for any damage *
20 * caused by the use of the code contained in this file ! *
21 * *
22 * TEST *
23 ********************************************************************************************/
33 #include <uStepperS.h>
34 
35 extern uStepperS * pointer;
36 
38 }
39 
40 
41 void uStepperDriver::reset( void ){
42 
43  // Reset stallguard
44  this->writeRegister( TCOOLTHRS, 0 );
45  this->writeRegister( THIGH, 0);
46  this->writeRegister( COOLCONF, 0 );
47  this->writeRegister( SW_MODE, 0 );
48  this->clearStall();
49 
50  this->writeRegister(XACTUAL, 0);
51  this->writeRegister(XTARGET, 0);
52 
53  this->writeRegister( IHOLD_IRUN,0 );
54  this->writeRegister( CHOPCONF, 0 );
55  this->writeRegister( GCONF, 0 );
56  this->writeRegister( PWMCONF, 0 );
57  this->writeRegister( TPWMTHRS, 0 );
58 
59  this->writeRegister( RAMPMODE, 0 );
60  this->writeRegister( VSTART, 0 );
61  this->writeRegister( A1, 0 );
62  this->writeRegister( V1, 0 );
63  this->writeRegister( AMAX, 0 );
64  this->writeRegister( VMAX, 0 );
65  this->writeRegister( D1, 0 );
66  this->writeRegister( VSTOP, 0 );
67 
68 }
69 
70 void uStepperDriver::init( uStepperS * _pointer ){
71 
72  this->pointer = _pointer;
73  this->chipSelect(true); // Set CS HIGH
74 
75  // First clear previous defined registers
76  this->reset();
77 
78  /* Set motor current */
79  this->writeRegister( IHOLD_IRUN, IHOLD( this->holdCurrent) | IRUN( this->current) | IHOLDDELAY( this->holdDelay) );
80 
81  this->enableStealth();
82 
83  /* Set all-round chopper configuration */
84  this->writeRegister( CHOPCONF, TOFF(2) | TBL(2) | HSTRT_TFD(4) | HEND(0));
85 
86  /* Set startup ramp mode */
88 
89  /* Reset position */
90  this->writeRegister(XACTUAL, 0);
91  this->writeRegister(XTARGET, 0);
92 
93  this->setDeceleration( 0xFFFE );
94  this->setAcceleration( 0xFFFE );
95 
96  this->stop();
97 
98  while(this->readRegister(VACTUAL) != 0);
99 }
100 
102 {
103  while(TCNT1 > 15900); //If interrupt is just about to happen, wait for it to finish
104  this->readRegister(XACTUAL);
105 }
106 
107 void uStepperDriver::setVelocity( uint32_t velocity )
108 {
109  this->VMAX = velocity;
110 
111  if(this->VMAX > 0x7FFE00)
112  {
113  this->VMAX = 0x7FFE00;
114  }
115 
116  this->writeRegister(VMAX_REG, this->VMAX);
117 }
118 
119 void uStepperDriver::setAcceleration( uint32_t acceleration )
120 {
121  this->AMAX = acceleration;
122 
123  if(this->AMAX > 0xFFFE)
124  {
125  this->AMAX = 0xFFFE;
126  }
127 
128  this->writeRegister(AMAX_REG, this->AMAX);
129 }
130 
131 void uStepperDriver::setDeceleration( uint32_t deceleration )
132 {
133  this->DMAX = deceleration;
134 
135  if(this->DMAX > 0xFFFE)
136  {
137  this->DMAX = 0xFFFE;
138  }
139 
140  this->writeRegister(DMAX_REG, this->DMAX);
141 }
142 
143 void uStepperDriver::setCurrent( uint8_t current )
144 {
145  this->current = current;
146  this->updateCurrent();
147 }
148 
149 void uStepperDriver::setHoldCurrent( uint8_t current )
150 {
151  this->holdCurrent = current;
152  this->updateCurrent();
153 }
154 
156 {
157  this->writeRegister( IHOLD_IRUN, IHOLD( this->holdCurrent) | IRUN( this->current) | IHOLDDELAY( this->holdDelay) );
158 }
159 
160 void uStepperDriver::setPosition( int32_t position )
161 {
162  this->mode = DRIVER_POSITION;
164  this->writeRegister(XTARGET, position);
165  this->xTarget = position;
166 }
167 
168 void uStepperDriver::setShaftDirection( bool direction )
169 {
170  // Read the register to save the settings
171  int32_t value = this->readRegister( GCONF );
172  // Update the direction bit
173  if(direction == 1){
174  value |= (0x01 << 4);
175  }else{
176  value &= ~(0x01 << 4);
177  }
178  this->writeRegister( GCONF, value );
179 }
180 
181 void uStepperDriver::setDirection( bool direction )
182 {
183  this->mode = DRIVER_VELOCITY;
184  if(direction){
186  }else{
188  }
189 }
190 
191 void uStepperDriver::setRampMode( uint8_t mode ){
192 
193  switch(mode){
194  case POSITIONING_MODE:
195  // Positioning mode
196  this->writeRegister(VSTART_REG, this->VSTART);
197  this->writeRegister(A1_REG, this->A1);
198  this->writeRegister(V1_REG, this->V1);
199  this->writeRegister(AMAX_REG, this->AMAX);
200  this->writeRegister(VMAX_REG, this->VMAX);
201  this->writeRegister(DMAX_REG, this->DMAX);
202  this->writeRegister(D1_REG, this->D1);
203  this->writeRegister(VSTOP_REG, this->VSTOP); /* Minimum 10 in POSITIONING_MODE */
204  this->writeRegister(RAMPMODE, POSITIONING_MODE); /* RAMPMODE = POSITIONING_MODE */
205  break;
206 
207  case VELOCITY_MODE_POS:
208  // Velocity mode (only AMAX and VMAX is used)
209  this->writeRegister(VSTART_REG, this->VSTART);
210  this->writeRegister(A1_REG, 0);
211  this->writeRegister(V1_REG, 0);
212  this->writeRegister(AMAX_REG, this->AMAX);
213  this->writeRegister(VMAX_REG, this->VMAX);
214  this->writeRegister(DMAX_REG, 0);
215  this->writeRegister(D1_REG, 0);
216  this->writeRegister(VSTOP_REG, 0);
217  this->writeRegister(RAMPMODE, VELOCITY_MODE_POS); /* RAMPMODE = VELOCITY_MODE_POS */
218  break;
219  }
220 }
221 
223 {
224  /* Set GCONF and enable stealthChop */
225  this->writeRegister( GCONF, EN_PWM_MODE(1) | I_SCALE_ANALOG(1) );
226  this->setShaftDirection(pointer->shaftDir);
227 
228  /* Set PWMCONF for StealthChop */
229  this->writeRegister( PWMCONF, PWM_AUTOSCALE(1) | PWM_GRAD(1) | PWM_AMPL(128) | PWM_FREQ(0) | FREEWHEEL(2) );
230 
231  /* Specifies the upper velocity (lower time delay) for operation in stealthChop voltage PWM mode */
232  this->writeRegister( TPWMTHRS, 5000 );
233 }
234 
236 {
237  return this->readRegister(VACTUAL);
238 }
239 
241 {
242  return this->readRegister(XACTUAL);
243 }
244 
246 {
247  this->mode = DRIVER_STOP;
248  this->setVelocity(0);
249 }
250 
251 void uStepperDriver::setHome(int32_t initialSteps)
252 {
253  int32_t xActual, xTarget;
254 
255  if(this->mode == DRIVER_POSITION)
256  {
257  xActual = this->getPosition();
258  xTarget = this->readRegister(XTARGET);
259 
260  xTarget -= xActual;
261  this->xTarget = xTarget + initialSteps;
262  this->xActual = initialSteps;
263  this->writeRegister(XACTUAL, initialSteps);
264  this->writeRegister(XTARGET, this->xTarget);
265  }
266  else
267  {
268  this->xTarget = initialSteps;
269  this->xActual = initialSteps;
270  this->writeRegister(XACTUAL, initialSteps);
271  this->writeRegister(XTARGET, initialSteps);
272  }
273 
274  pointer->pidPositionStepsIssued = initialSteps;
275 }
276 
277 int32_t uStepperDriver::writeRegister( uint8_t address, uint32_t datagram ){
278 
279  // Disabled interrupts until write is complete
280  //cli();
281  TIMSK1 &= ~(1 << OCIE1A);
282  // Enable SPI mode 3 to use TMC5130
283  this->pointer->setSPIMode(3);
284 
285  uint32_t package = 0;
286 
287  // Add the value of WRITE_ACCESS to enable register write
288  address += WRITE_ACCESS;
289 
290  this->chipSelect(false);
291 
292  this->status = this->pointer->SPI(address);
293 
294  package |= this->pointer->SPI((datagram >> 24) & 0xff);
295  package <<= 8;
296  package |= this->pointer->SPI((datagram >> 16) & 0xff);
297  package <<= 8;
298  package |= this->pointer->SPI((datagram >> 8) & 0xff);
299  package <<= 8;
300  package |= this->pointer->SPI((datagram) & 0xff);
301 
302  this->chipSelect(true); // Set CS HIGH
303 
304  //sei();
305  TIMSK1 |= (1 << OCIE1A);
306  return package;
307 }
308 
309 int32_t uStepperDriver::readRegister( uint8_t address )
310 {
311  // Disabled interrupts until write is complete
312  //cli();
313  TIMSK1 &= ~(1 << OCIE1A);
314 
315  // Enable SPI mode 3 to use TMC5130
316  this->pointer->setSPIMode(3);
317 
318  // Request a reading on address
319  this->chipSelect(false);
320  this->status = this->pointer->SPI(address);
321  this->pointer->SPI(0x00);
322  this->pointer->SPI(0x00);
323  this->pointer->SPI(0x00);
324  this->pointer->SPI(0x00);
325  this->chipSelect(true);
326 
327  // Read the actual value on second request
328  int32_t value = 0;
329 
330  this->chipSelect(false);
331  this->status = this->pointer->SPI(address);
332  value |= this->pointer->SPI(0x00);
333  value <<= 8;
334  value |= this->pointer->SPI(0x00);
335  value <<= 8;
336  value |= this->pointer->SPI(0x00);
337  value <<= 8;
338  value |= this->pointer->SPI(0x00);
339  this->chipSelect(true);
340 
341  //sei();
342  TIMSK1 |= (1 << OCIE1A);
343 
344  return value;
345 }
346 
348 {
349  if(state == false)
350  PORTE &= ~(1 << CS_DRIVER); // Set CS LOW
351  else
352  PORTE |= (1 << CS_DRIVER); // Set CS HIGH
353 }
354 
355 void uStepperDriver::enableStallguard( int8_t threshold, bool stopOnStall, float rpm)
356 {
357  // Limit threshold
358  if( threshold > 63)
359  threshold = 63;
360  else if( threshold < -64)
361  threshold = -64;
362 
363  rpm = abs(rpm);
364  // Limit rpm
365  if( rpm > 1000)
366  rpm = 1000;
367  else if( rpm < 2)
368  rpm = 2;
369 
370  /* Disable StealthChop for stallguard operation */
371  this->writeRegister( GCONF, EN_PWM_MODE(0) | I_SCALE_ANALOG(1) );
372  this->setShaftDirection(pointer->shaftDir);
373 
374  // Configure COOLCONF for stallguard
375  this->writeRegister( COOLCONF, SGT(threshold) | SFILT(1) | SEMIN(5) | SEMAX(2) | SEDN(1) );
376 
377  //int32_t stall_speed = 1048576 / pointer->rpmToVelocity * speed // 1048576 = 2^20. See TSTEP in datasheet p.33
378  int32_t stall_speed = 1048576 / pointer->rpmToVelocity * (rpm/2); //Should be 1048576 = 2^20.
379  stall_speed = stall_speed * 1.2; // // Activate stallGuard sligthly below desired homing velocity (provide 20% tolerance)
380 
381  // Set TCOOLTHRS to max speed value (enable stallguard for all speeds)
382  this->writeRegister( TCOOLTHRS, stall_speed ); // Max value is 20bit = 0xFFFFF
383  this->writeRegister( THIGH, 0);
384 
385  // Enable automatic stop on stall dectection
386  if( stopOnStall )
387  this->writeRegister( SW_MODE, SG_STOP(1) );
388  else
389  this->writeRegister( SW_MODE, SG_STOP(0) );
390 }
391 
393 {
394  // Reenable stealthchop
395  this->writeRegister( GCONF, EN_PWM_MODE(1) | I_SCALE_ANALOG(1) );
396  this->setShaftDirection(pointer->shaftDir);
397 
398  // Disable all stallguard configuration
399  this->writeRegister( COOLCONF, 0 );
400  this->writeRegister( TCOOLTHRS, 0 );
401  this->writeRegister( THIGH, 0);
402  this->writeRegister( SW_MODE, 0 );
403 }
404 
406 {
407  // Reading the RAMP_STAT register clears the stallguard flag, telling the driver to continue.
408  this->readRegister( RAMP_STAT );
409 }
410 
412 {
413  // Get the SG_RESULT from DRV_STATUS.
414  return this->readRegister(DRV_STATUS) & 0x3FF;
415 }
XTARGET
#define XTARGET
Definition: uStepperDriver.h:66
uStepperS
Prototype of class for accessing all features of the uStepper S in a single object.
Definition: uStepperS.h:277
uStepperDriver::init
void init(uStepperS *_pointer)
Initiation of the motor driver.
Definition: uStepperDriver.cpp:70
uStepperDriver::disableStallguard
void disableStallguard(void)
Definition: uStepperDriver.cpp:392
VELOCITY_MODE_NEG
#define VELOCITY_MODE_NEG
Definition: uStepperDriver.h:134
uStepperDriver::setAcceleration
void setAcceleration(uint32_t acceleration)
Set motor acceleration.
Definition: uStepperDriver.cpp:119
uStepperDriver::VSTART
uint32_t VSTART
Definition: uStepperDriver.h:324
uStepperDriver::readRegister
int32_t readRegister(uint8_t address)
Reads a register from the motor driver.
Definition: uStepperDriver.cpp:309
IRUN
#define IRUN(n)
Definition: uStepperDriver.h:124
HSTRT_TFD
#define HSTRT_TFD(n)
Definition: uStepperDriver.h:101
uStepperDriver::mode
uint8_t mode
Definition: uStepperDriver.h:315
uStepperDriver::AMAX
uint16_t AMAX
Definition: uStepperDriver.h:329
uStepperS.h
uStepperDriver::readMotorStatus
void readMotorStatus(void)
Definition: uStepperDriver.cpp:101
DRIVER_VELOCITY
#define DRIVER_VELOCITY
Definition: uStepperDriver.h:138
uStepperDriver::getVelocity
int32_t getVelocity(void)
Returns the current speed of the motor driver.
Definition: uStepperDriver.cpp:235
uStepperDriver::pointer
uStepperS * pointer
Definition: uStepperDriver.h:317
uStepperS::rpmToVelocity
float rpmToVelocity
Definition: uStepperS.h:717
TPWMTHRS
#define TPWMTHRS
Definition: uStepperDriver.h:49
uStepperDriver::setVelocity
void setVelocity(uint32_t velocity)
Set motor velocity.
Definition: uStepperDriver.cpp:107
uStepperS::setSPIMode
void setSPIMode(uint8_t mode)
Definition: uStepperS.cpp:386
HEND
#define HEND(n)
Definition: uStepperDriver.h:100
V1_REG
#define V1_REG
Definition: uStepperDriver.h:59
uStepperDriver::clearStall
void clearStall(void)
Definition: uStepperDriver.cpp:405
CHOPCONF
#define CHOPCONF
Definition: uStepperDriver.h:85
uStepperDriver::setHome
void setHome(int32_t initialSteps=0)
Resets the internal position counter of the motor driver.
Definition: uStepperDriver.cpp:251
uStepperDriver::A1
uint16_t A1
Definition: uStepperDriver.h:328
A1_REG
#define A1_REG
Definition: uStepperDriver.h:58
EN_PWM_MODE
#define EN_PWM_MODE(n)
Definition: uStepperDriver.h:41
RAMP_STAT
#define RAMP_STAT
Definition: uStepperDriver.h:70
GCONF
#define GCONF
Definition: uStepperDriver.h:38
uStepperDriver::updateCurrent
void updateCurrent(void)
Writes the current setting registers of the motor driver
Definition: uStepperDriver.cpp:155
uStepperDriver::current
uint8_t current
Definition: uStepperDriver.h:319
uStepperDriver::VSTOP
uint32_t VSTOP
Definition: uStepperDriver.h:327
IHOLDDELAY
#define IHOLDDELAY(n)
Definition: uStepperDriver.h:123
uStepperDriver::D1
uint16_t D1
Definition: uStepperDriver.h:331
uStepperDriver::chipSelect
void chipSelect(bool state)
Definition: uStepperDriver.cpp:347
D1_REG
#define D1_REG
Definition: uStepperDriver.h:63
TCOOLTHRS
#define TCOOLTHRS
Definition: uStepperDriver.h:50
VMAX_REG
#define VMAX_REG
Definition: uStepperDriver.h:61
TOFF
#define TOFF(n)
Definition: uStepperDriver.h:102
uStepperDriver::V1
uint32_t V1
Definition: uStepperDriver.h:325
VSTOP_REG
#define VSTOP_REG
Definition: uStepperDriver.h:64
uStepperDriver::setCurrent
void setCurrent(uint8_t current)
Set motor driver current.
Definition: uStepperDriver.cpp:143
PWM_FREQ
#define PWM_FREQ(n)
Definition: uStepperDriver.h:78
uStepperDriver::holdDelay
uint8_t holdDelay
Definition: uStepperDriver.h:321
uStepperDriver::setPosition
void setPosition(int32_t position)
Set the motor position.
Definition: uStepperDriver.cpp:160
RAMPMODE
#define RAMPMODE
Definition: uStepperDriver.h:52
PWMCONF
#define PWMCONF
Definition: uStepperDriver.h:74
WRITE_ACCESS
#define WRITE_ACCESS
Definition: uStepperDriver.h:128
PWM_GRAD
#define PWM_GRAD(n)
Definition: uStepperDriver.h:79
IHOLD_IRUN
#define IHOLD_IRUN
Definition: uStepperDriver.h:46
uStepperDriver::getPosition
int32_t getPosition(void)
Returns the current position of the motor driver.
Definition: uStepperDriver.cpp:240
DRV_STATUS
#define DRV_STATUS
Definition: uStepperDriver.h:121
SEMAX
#define SEMAX(n)
Definition: uStepperDriver.h:112
DRIVER_POSITION
#define DRIVER_POSITION
Definition: uStepperDriver.h:139
AMAX_REG
#define AMAX_REG
Definition: uStepperDriver.h:60
COOLCONF
#define COOLCONF
Definition: uStepperDriver.h:107
uStepperDriver::uStepperDriver
uStepperDriver(void)
Constructor.
Definition: uStepperDriver.cpp:37
IHOLD
#define IHOLD(n)
Definition: uStepperDriver.h:125
PWM_AMPL
#define PWM_AMPL(n)
Definition: uStepperDriver.h:80
XACTUAL
#define XACTUAL
Definition: uStepperDriver.h:53
SGT
#define SGT(n)
Definition: uStepperDriver.h:109
uStepperDriver::holdCurrent
uint8_t holdCurrent
Definition: uStepperDriver.h:320
TBL
#define TBL(n)
Definition: uStepperDriver.h:95
uStepperDriver::enableStallguard
void enableStallguard(int8_t threshold, bool stopOnStall, float rpm)
Definition: uStepperDriver.cpp:355
CS_DRIVER
#define CS_DRIVER
Definition: uStepperS.h:221
FREEWHEEL
#define FREEWHEEL(n)
Definition: uStepperDriver.h:76
SG_STOP
#define SG_STOP(n)
Definition: uStepperDriver.h:69
uStepperDriver::status
uint8_t status
Definition: uStepperDriver.h:312
pointer
uStepperS * pointer
Definition: uStepperS.cpp:33
uStepperDriver::getStallValue
uint16_t getStallValue(void)
Returns the load measurement used for Stall detection.
Definition: uStepperDriver.cpp:411
uStepperS::shaftDir
volatile bool shaftDir
Definition: uStepperS.h:762
SEDN
#define SEDN(n)
Definition: uStepperDriver.h:111
DMAX_REG
#define DMAX_REG
Definition: uStepperDriver.h:62
uStepperDriver::stop
void stop(void)
Stops any ongoing movement with deceleration.
Definition: uStepperDriver.cpp:245
THIGH
#define THIGH
Definition: uStepperDriver.h:51
uStepperS::SPI
uint8_t SPI(uint8_t data)
Definition: uStepperS.cpp:401
uStepperDriver::reset
void reset(void)
Definition: uStepperDriver.cpp:41
uStepperDriver::enableStealth
void enableStealth(void)
Definition: uStepperDriver.cpp:222
VSTART_REG
#define VSTART_REG
Definition: uStepperDriver.h:57
uStepperDriver::setDirection
void setDirection(bool direction)
Definition: uStepperDriver.cpp:181
uStepperS::pidPositionStepsIssued
volatile int32_t pidPositionStepsIssued
Definition: uStepperS.h:749
VELOCITY_MODE_POS
#define VELOCITY_MODE_POS
Definition: uStepperDriver.h:133
uStepperDriver::xActual
volatile int32_t xActual
Definition: uStepperDriver.h:307
uStepperDriver::VMAX
uint32_t VMAX
Definition: uStepperDriver.h:326
SFILT
#define SFILT(n)
Definition: uStepperDriver.h:108
SEMIN
#define SEMIN(n)
Definition: uStepperDriver.h:114
uStepperDriver::DMAX
uint16_t DMAX
Definition: uStepperDriver.h:330
POSITIONING_MODE
#define POSITIONING_MODE
Definition: uStepperDriver.h:132
SW_MODE
#define SW_MODE
Definition: uStepperDriver.h:68
uStepperDriver::setShaftDirection
void setShaftDirection(bool direction)
Set motor driver direction.
Definition: uStepperDriver.cpp:168
VACTUAL
#define VACTUAL
Definition: uStepperDriver.h:54
uStepperDriver::setHoldCurrent
void setHoldCurrent(uint8_t current)
Set motor driver hold current.
Definition: uStepperDriver.cpp:149
uStepperDriver::setDeceleration
void setDeceleration(uint32_t deceleration)
Set motor deceleration.
Definition: uStepperDriver.cpp:131
I_SCALE_ANALOG
#define I_SCALE_ANALOG(n)
Definition: uStepperDriver.h:42
PWM_AUTOSCALE
#define PWM_AUTOSCALE(n)
Definition: uStepperDriver.h:77
DRIVER_STOP
#define DRIVER_STOP
Definition: uStepperDriver.h:137
uStepperDriver::setRampMode
void setRampMode(uint8_t mode)
Set motor driver to position mode or velocity mode.
Definition: uStepperDriver.cpp:191
uStepperDriver::xTarget
volatile int32_t xTarget
Definition: uStepperDriver.h:304
uStepperDriver::writeRegister
int32_t writeRegister(uint8_t address, uint32_t datagram)
Write a register of the motor driver.
Definition: uStepperDriver.cpp:277