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