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 }
void setAcceleration(uint32_t acceleration)
Set motor acceleration.
void setDirection(bool direction)
uint16_t getStallValue(void)
Returns the load measurement used for Stall detection.
void chipSelect(bool state)
void readMotorStatus(void)
volatile int32_t xTarget
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 reset(void)
void setShaftDirection(bool direction)
Set motor driver direction.
uStepperDriver(void)
Constructor.
void stop(void)
Stops any ongoing movement with deceleration.
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.
volatile int32_t xActual
void setHoldCurrent(uint8_t current)
Set motor driver hold current.
void enableStealth(void)
void enableStallguard(int8_t threshold, bool stopOnStall, float rpm)
int32_t readRegister(uint8_t address)
Reads a register from the motor driver.
uStepperS * pointer
void setVelocity(uint32_t velocity)
Set motor velocity.
void clearStall(void)
int32_t getVelocity(void)
Returns the current speed of the motor driver.
void setCurrent(uint8_t current)
Set motor driver current.
void setHome(int32_t initialSteps=0)
Resets the internal position counter 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.
Prototype of class for accessing all features of the uStepper S in a single object.
Definition: uStepperS.h:279
float rpmToVelocity
Definition: uStepperS.h:733
volatile int32_t pidPositionStepsIssued
Definition: uStepperS.h:766
void setSPIMode(uint8_t mode)
Definition: uStepperS.cpp:400
uint8_t SPI(uint8_t data)
Definition: uStepperS.cpp:415
volatile bool shaftDir
Definition: uStepperS.h:779
uStepperS * pointer
Definition: uStepperS.cpp:34
#define RAMP_STAT
#define VACTUAL
#define WRITE_ACCESS
#define TBL(n)
#define SEDN(n)
#define VSTOP_REG
#define PWM_AMPL(n)
#define D1_REG
#define IHOLD(n)
#define TCOOLTHRS
#define POSITIONING_MODE
#define SG_STOP(n)
#define EN_PWM_MODE(n)
#define PWM_GRAD(n)
#define DRIVER_VELOCITY
#define FREEWHEEL(n)
#define TOFF(n)
#define SGT(n)
#define XACTUAL
#define CHOPCONF
#define IHOLDDELAY(n)
#define SEMIN(n)
#define SFILT(n)
#define IRUN(n)
#define HEND(n)
#define PWM_AUTOSCALE(n)
#define VMAX_REG
#define DRV_STATUS
#define THIGH
#define AMAX_REG
#define I_SCALE_ANALOG(n)
#define SEMAX(n)
#define XTARGET
#define A1_REG
#define DRIVER_POSITION
#define RAMPMODE
#define VELOCITY_MODE_NEG
#define PWM_FREQ(n)
#define IHOLD_IRUN
#define HSTRT_TFD(n)
#define PWMCONF
#define VELOCITY_MODE_POS
#define DRIVER_STOP
#define VSTART_REG
#define SW_MODE
#define DMAX_REG
#define TPWMTHRS
#define V1_REG
#define GCONF
#define COOLCONF
#define CS_DRIVER
Definition: uStepperS.h:222