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

Join the conversation

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

Guest
Reply to this topic...

×   Pasted as rich text.   Paste as plain text instead

  Only 75 emoji are allowed.

×   Your link has been automatically embedded.   Display as a link instead

×   Your previous content has been restored.   Clear editor

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

Sign in to follow this  

×
×
  • Create New...