43oh

Algorithm based stepper motor ramping for Tiva C

Recommended Posts

I have started the process of porting my CO2-laser driver from MSP430 to Tiva C and I am switching to an algorithm based approach for the stepper acceleration/deceleration. As part of testing I have (re)written an implementation of David Austins algorithm as documented here:

http://www.embedded.com/design/mcus-processors-and-socs/4006438/Generate-stepper-motor-speed-profiles-in-real-time

The test code may be of use for others so here it is.

PA7: direction

PF1: step pulse

From what I can glean from my scope the interrupt handler executes in about 2us - so can be used for high pulse rates.

//
// Real-time stepper motor ramp control for TIVA C
//
// Based on article & code by David Austin
//
// Terje Io, 2016-03-08
//

#include <stdint.h>
#include <stdbool.h>

#include "inc/hw_gpio.h"
#include "inc/hw_types.h"
#include "inc/hw_ints.h"
#include "inc/hw_memmap.h"

#include "driverlib/sysctl.h"
#include "driverlib/pin_map.h"
#include "driverlib/gpio.h"
#include "driverlib/pwm.h"

#define STATE_IDLE  0
#define STATE_ACCEL 1
#define STATE_RUN   2
#define STATE_DECEL 3

#define STEPPULSE 50

typedef struct motor {
uint8_t busy;				// state machine state
int16_t position; 	        // absolute step number
uint16_t move;       		// total steps to move
uint16_t midpt;      		// midpoint of move
bool odd;      		        // true if odd number of steps
int8_t pos_inc;     		// position increment
uint16_t delay;          	// integer delay count
uint16_t first_delay;       // integer delay count
uint16_t min_delay;         // integer delay count
uint16_t step_no;    		// progress of move
uint16_t step_down;  		// start of down-ramp
uint32_t c32;        		// 24.8 fixed point delay count
int16_t denom; 				// 4.n+1 in ramp algo
} motor;

struct motor motor1;

void pwm_1_2_int_handler (void);

void motor_init (motor *motor) {
motor->position    = 0;
motor->busy        = STATE_IDLE;
motor->pos_inc     = 0;
motor->first_delay = 30000;
motor->min_delay   = 1400;
}

bool motor_calc (motor *motor, int16_t pos_new) {

if (pos_new < motor->position) { // get direction & #steps
motor->move = motor->position - pos_new;
motor->pos_inc = -1;
} else if (pos_new != motor->position) {
motor->move = pos_new - motor->position;
motor->pos_inc = 1;
} else

motor->odd     = motor->move & 0x0001;
motor->midpt   = (motor->move - (motor->odd ? 1 : 0)) >> 1;
motor->step_no = 0; // step counter
motor->delay   = motor->first_delay;
motor->c32     = ((uint32_t)motor->delay) << 8; 	// keep delay in 24.8 fixed-point format for ramp calcs
motor->denom   = 1; 					// 4.n + 1, n = 0

return motor->busy = motor->move == 1 ? STATE_DECEL : STATE_ACCEL;

}

void motor1_run (int16_t pos_new) {

if(motor_calc(&motor1, pos_new)) {
GPIOPinWrite(GPIO_PORTA_BASE, GPIO_PIN_7, motor1.pos_inc > 0 ? GPIO_PIN_7 : 0);
PWMGenPeriodSet(PWM1_BASE, PWM_GEN_2, motor1.delay);
PWMPulseWidthSet(PWM1_BASE, PWM_OUT_5, STEPPULSE);
PWMGenEnable(PWM1_BASE, PWM_GEN_2);
}
}

void mpu_init (void) {

SysCtlPWMClockSet(SYSCTL_PWMDIV_8);

SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOA);
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOF);
SysCtlPeripheralEnable(SYSCTL_PERIPH_PWM1);
SysCtlDelay(26); // wait for modules to start

GPIOPinTypeGPIOOutput(GPIO_PORTA_BASE, GPIO_PIN_6 | GPIO_PIN_7);

//	PWMClockSet(PWM1_BASE, PWM_SYSCLK_DIV_64);

GPIOPinTypePWM(GPIO_PORTF_BASE, GPIO_PIN_1);

GPIOPinConfigure(GPIO_PF1_M1PWM5);

PWMGenConfigure(PWM1_BASE, PWM_GEN_2, PWM_GEN_MODE_DOWN | PWM_GEN_MODE_GEN_SYNC_LOCAL);
PWMOutputState(PWM1_BASE, PWM_OUT_5_BIT, true);

PWMGenIntRegister(PWM1_BASE, PWM_GEN_2, pwm_1_2_int_handler);
PWMGenIntClear(PWM1_BASE, PWM_GEN_2, PWMINT);
PWMGenIntTrigEnable(PWM1_BASE, PWM_GEN_2, PWMINT);
PWMIntEnable(PWM1_BASE, PWM_INT_GEN_2);

}

void main(void) {

SysCtlClockSet(SYSCTL_SYSDIV_2_5 | SYSCTL_USE_PLL | SYSCTL_OSC_MAIN | SYSCTL_XTAL_16MHZ);

motor_init(&motor1);

mpu_init();

while (true) { // repeat 5 revs forward & back
motor1_run(8000); // 1.8 deg/step and 8 microsteps
while (motor1.busy);
motor1_run(0);
while (motor1.busy);
}
}

void pwm_1_2_int_handler (void) {

PWMGenIntClear(PWM1_BASE, PWM_GEN_2, PWMINT);

motor1.position += motor1.pos_inc;
motor1.step_no++;

switch (motor1.busy) {

case STATE_ACCEL:
if (motor1.step_no == motor1.midpt) {
motor1.busy = motor1.odd ? STATE_RUN : STATE_DECEL;
motor1.step_down = motor1.step_no + 1;
motor1.denom -= 2;

} else {

motor1.denom += 4;
motor1.c32 -= (motor1.c32 << 1) / motor1.denom;	// ramp algorithm
motor1.delay = (motor1.c32 + 128) >> 8; 	// round 24.8 format -> int16

if (motor1.delay <= motor1.min_delay) { 	// go to constant speed?
motor1.denom -= 6;
motor1.busy = STATE_RUN;
motor1.step_down = motor1.move - motor1.step_no;
motor1.delay = motor1.min_delay;
}

PWMGenPeriodSet(PWM1_BASE, PWM_GEN_2, motor1.delay);
PWMPulseWidthSet(PWM1_BASE, PWM_OUT_5, STEPPULSE);
}
break;

case STATE_RUN:
if (motor1.step_no == motor1.step_down)
motor1.busy = STATE_DECEL;
break;

case STATE_DECEL:
if (motor1.denom < 2)  // done?
motor1.busy = STATE_IDLE;

else {

motor1.c32 += (motor1.c32 << 1) / motor1.denom;	// ramp algorithm
motor1.delay = (motor1.c32 - 128) >> 8; 	// round 24.8 format -> int16
motor1.denom -= 4;

PWMGenPeriodSet(PWM1_BASE, PWM_GEN_2, motor1.delay);
PWMPulseWidthSet(PWM1_BASE, PWM_OUT_5, STEPPULSE);
}
break;

} // switch (busy)

if(!motor1.busy)
PWMGenDisable(PWM1_BASE, PWM_GEN_2);

}

Share on other sites

Nicely done.  I enjoyed the write-up by David Austin also.

Join the conversation

You can post now and register later. If you have an account, sign in now to post with your account.

×   Pasted as rich text.   Paste as plain text instead

Only 75 emoji are allowed.

×   Your previous content has been restored.   Clear editor

×   You cannot paste images directly. Upload or insert images from URL.

×
×
• Blog

• Activity

×
• Create New...