uStepper S
uStepperDriver.cpp
Go to the documentation of this file.
1/********************************************************************************************
2* File: uStepperDriver.cpp *
3* Version: 2.3.0 *
4* Date: December 27th, 2021 *
5* Authors: Thomas Hørring Olsen *
6* Emil Jacobsen *
7* *
8*********************************************************************************************
9* (C) 2021 *
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
36extern uStepperS * pointer;
37
39}
40
41
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
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
108void 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
120void 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
132void 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
144void uStepperDriver::setCurrent( uint8_t current )
145{
146 this->current = current;
147 this->updateCurrent();
148}
149
150void 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
161void uStepperDriver::setPosition( int32_t position )
162{
163 this->mode = DRIVER_POSITION;
165 this->writeRegister(XTARGET, position);
166 this->xTarget = position;
167}
168
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
182void uStepperDriver::setDirection( bool direction )
183{
184 this->mode = DRIVER_VELOCITY;
185 if(direction){
187 }else{
189 }
190}
191
192void 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
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 */
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
258void 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
284int32_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
316int32_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
362void 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 */
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
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
Function prototypes and definitions for the uStepper S library.
#define CS_DRIVER
Definition: uStepperS.h:222