Jump to content
Sign in to follow this  
terjeio

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

#define PWMINT PWM_INT_CNT_AD | PWM_INT_CNT_BD

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
		return false; // already there

	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);
        GPIOPadConfigSet(GPIO_PORTA_BASE, GPIO_PIN_7, GPIO_STRENGTH_12MA, GPIO_PIN_TYPE_STD);

//	PWMClockSet(PWM1_BASE, PWM_SYSCLK_DIV_64);

        GPIOPinTypePWM(GPIO_PORTF_BASE, GPIO_PIN_1);
        GPIOPadConfigSet(GPIO_PORTF_BASE, GPIO_PIN_1, GPIO_STRENGTH_12MA, GPIO_PIN_TYPE_STD);

        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 this post


Link to post
Share on other sites

Create an account or sign in to comment

You need to be a member in order to leave a comment

Create an account

Sign up for a new account in our community. It's easy!

Register a new account

Sign in

Already have an account? Sign in here.

Sign In Now
Sign in to follow this  

×