Stepper motion control from signals on SD card (co-author: Mark Flanagan)

Note: This post is co-authored by Mark Flanagan. Mark is blogging about his final-year project here and has also posted a really interesting account of his preliminary investigative work here.

Mark’s project concerns the development of a fascinating robotic simulator for human respiration motion, which will soon be used in series of experiments to validate the tracking and treatment planning systems used in radiotherapy. Mark’s prototype has two stepper driven linear axes, one horizontal and one vertical, each with a slider potentiometer attached for position feedback. The device is controlled by a dsPIC30F4011 microcontroller, which reads a target trajectory as a series of samples (x and y values) from an SD card.

This post concerns the structure of a program that loads samples from the SD card at regular intervals and controls the motion of each stepper accordingly. We discussed several possible approaches to the structure of the program for this device, but this is currently the favoured option. It’s an interesting sort of a design pattern that can probably be usefully applied in many other applications, so that’s why we’re posting it.

In brief, an interrupt driven structure is used with three timers running in parallel, each with its own interrupt service routines (ISR):

  • The Timer 1 ISR increments a sample counter, causing the main program loop to read a new sample from the SD card at a regular interval (40ms).
  • The Timer 2 ISR drives periodic stepping of the x-axis actuator. The stepping frequency is adjusted for each new sample to ensure that the actuator will reach its target position at the end of the current sample period.
  • The Timer 3 ISR drives periodic stepping of the y-axis actuator. As with the x-axis, the stepping frequency varies according to the required speed of movement.

This code currently doesn’t actually read anything from an SD card (see the comments in the code) and rather than controlling real stepper motors, it just varies the duty cycle of two PWM channels to brighten or dim two attached LEDs. Once these features are included, Mark will probably post the whole program on his blog.

This video shows the example code running on a very simple dsPIC30F4011 circuit.

Here’s the full example code.

//
// Sketch of a pattern to make two steppers follow trajectories
// stored as samples on e.g. an SD card. It doesn't actually read
// anything from an SD card right now. Instead, each sample is
// generated by a dummy get_sample function on the fly.
//
// Specifically, what this dummy version actually does is to
// dynamically generate samples of a sin and cos function for the
// x and y axes respectively. Instead of moving a stepper motor,
// a pair of dummy step functions for each axis increase and
// decrease the duty cycle on a PWM channel, varying the light
// intensity of an attached LED.
//
// Mark Flanagan (adapted from example code by Ted Burke)
// Last modified 11-4-2012
//

#include <libpic30.h>
#include <p30f4011.h>
#include <stdio.h>
#include <math.h>

// Configuration settings
_FOSC(CSW_FSCM_OFF & FRC_PLL16); // Fosc=16x7.5MHz, Fcy=30MHz
_FWDT(WDT_OFF);                  // Watchdog timer off
_FBORPOR(MCLR_DIS);              // Disable reset pin

// Function prototypes
void setup();
unsigned int read_analog_channel(int n);
void get_sample(unsigned long sample_number);

// Stepping function prototypes
void x_step_forward();
void x_step_back();
void y_step_forward();
void y_step_back();

// Function prototypes for interrupt service routines for timers 1, 2 and 3
void __attribute__((__interrupt__, __auto_psv__)) _T1Interrupt(void);
void __attribute__((__interrupt__, __auto_psv__)) _T2Interrupt(void);
void __attribute__((__interrupt__, __auto_psv__)) _T3Interrupt(void);

// Global variables
unsigned int x_1, x_2, y_1, y_2;
unsigned long current_sample, sample_counter;

int steps_per_du = 9; // JUST AN EXAMPLE VALUE - DEPENDS ON REAL SYSTEM!!!

int main()
{
	// Set up digital i/o, analog input, PWM and interrupts
	setup();

	// Begin by moving both axes to first sample position
	current_sample = 0;
	sample_counter = 0;
	get_sample(current_sample);
	while(1)
	{
		// Check the current position on both axes.
		x_1 = read_analog_channel(0);
		y_1 = read_analog_channel(1);

		// If both axes have reached the position of the
		// first sample, then move on with the program.
		if (x_1 == x_2 && y_1 == y_2) break;

		// If one or both axes still have not reached the
		// position of the first sample, step as required.
		if (x_1 < x_2) x_step_forward();
		if (x_1 > x_2) x_step_back();
		if (y_1 < y_2) y_step_forward();
		if (y_1 > y_2) y_step_back();

		// A short delay is required to ensure that step
		// frequency is not too high for the stepper motors.
		__delay32(30000); // 1ms - CHANGE TO MINIMUM STEP TIME!!!
	}

	// Turn on Timer 1 to get sample counter incrementing
	T1CONbits.TON = 1;    // Turn on Timer 1

	while(1)
	{
		// Wait for timer 1 ISR to increment sample counter
		while (sample_counter == current_sample);

		// Get target position for the next sample period
		current_sample = sample_counter;
		get_sample(current_sample);

		// Get current position
		x_1 = read_analog_channel(0);
		y_1 = read_analog_channel(1);

		// Calculate the stepping frequency required on each
		// axis in order to reach the target position in one
		// sample period.
		if (x_2 > x_1) PR2 = PR1 / (steps_per_du * (x_2 - x_1));
		else if (x_1 > x_2) PR2 = PR1 / (steps_per_du * (x_1 - x_2));
		else PR2 = 0;

		if (y_2 > y_1) PR3 = PR1 / (steps_per_du * (y_2 - y_1));
		else if (y_1 > y_2) PR3 = PR1 / (steps_per_du * (y_1 - y_2));
		else PR3 = 0;
	}

	return 0;
}

// Timer 1 interrupt service routine increments sample count
void __attribute__((__interrupt__, __auto_psv__)) _T1Interrupt(void)
{
	// Clear Timer 1 interrupt flag
	_T1IF = 0;

	// Tell main loop to get next sample from SD card
	sample_counter++;
}

// Timer 2 interrupt service routine performs x steps
void __attribute__((__interrupt__, __auto_psv__)) _T2Interrupt(void)
{
	// Clear Timer 2 interrupt flag
	_T2IF = 0;

	// Step x-axis in required direction
	if (x_2 > x_1) x_step_forward();
	else x_step_back();
}

// Timer 3 interrupt service routine performs y steps
void __attribute__((__interrupt__, __auto_psv__)) _T3Interrupt(void)
{
	// Clear Timer 3 interrupt flag
	_T3IF = 0;

	// Step x-axis in required direction
	if (y_2 > y_1) y_step_forward();
	else y_step_back();
}

// This function sets up digital i/o, analog input, timer 1
void setup()
{
	// Configure all four port D pins (RD0, RD1, RD2, RD3)
	// as digital outputs
	LATD = 0;
	TRISD = 0b1111111111110000;

	// Configure all three port C pins (RC13, RC14, RC15)
	// as digital inputs
	TRISC = 0b1111111111111111;

	// Configure AN0-AN8 as analog inputs
	TRISB = 0x01FF;      // All 9 port B pins are inputs
	ADPCFG = 0xFE00;     // Lowest 9 PORTB pins are analog inputs
	ADCON1 = 0;          // Manually clear SAMP to end sampling, start conversion
	ADCON2 = 0;          // Voltage reference from AVDD and AVSS
	ADCON3 = 0x0005;     // Manual Sample, ADCS=5 -> Tad = 3*Tcy
	ADCON1bits.ADON = 1; // Turn ADC ON

	// Configure PWM for free running mode
	//
	//   PWM period = Tcy * prescale * PTPER
	//   PWM pulse width = (Tcy/2) * prescale * PDC
	//
	PWMCON1 = 0x00FF; // Enable all PWM pairs in complementary mode
	PTCON = 0;
	_PTCKPS = 0;      // prescale=1:64 (0=1:1, 1=1:4, 2=1:16, 3=1:64)
	PTPER = 10000;    // 3kHz
	PDC1 = PTPER;     // 50% duty cycle on PWM channel 1
	PDC2 = PTPER;     // 50% duty cycle on PWM channel 2
	PDC3 = PTPER;     // 50% duty cycle on PWM channel 3
	PTMR = 0;         // Clear 15-bit PWM timer counter
	_PTEN = 1;        // Enable PWM time base

	// Configure Timer 1
	// PR1 and TCKPS are set for 25Hz (at Fcy=30MHz)
	T1CON = 0;            // Clear Timer 1 configuration
	T1CONbits.TCKPS = 2;  // Prescaler (0=1:1, 1=1:8, 2=1:64, 3=1:256)
	PR1 = 18750;          // Set Timer 1 period (max 65535)
	TMR1 = 0;             // Reset Timer 1 counter
	_T1IP = 1;            // Set Timer 1 interrupt priority
	_T1IF = 0;            // Clear Timer 1 interrupt flag
	_T1IE = 1;            // Enable Timer 1 interrupt

	// Configure Timer 2
	T2CON = 0;            // Clear Timer 2 configuration
	T2CONbits.TCKPS = 2;  // Prescaler (0=1:1, 1=1:8, 2=1:64, 3=1:256)
	PR2 = 0;              // Set Timer 2 period (max 65535)
	TMR2 = 0;             // Reset Timer 2 counter
	_T2IP = 1;            // Set Timer 2 interrupt priority
	_T2IF = 0;            // Clear Timer 2 interrupt flag
	_T2IE = 1;            // Enable Timer 2 interrupt
	T2CONbits.TON = 1;    // Turn on Timer 2 (it won't do anything until PR2 > 0)

	// Configure Timer 3
	T3CON = 0;            // Clear Timer 3 configuration
	T3CONbits.TCKPS = 2;  // Prescaler (0=1:1, 1=1:8, 2=1:64, 3=1:256)
	PR3 = 0;              // Set Timer 3 period (max 65535)
	TMR3 = 0;             // Reset Timer 3 counter
	_T3IP = 1;            // Set Timer 3 interrupt priority
	_T3IF = 0;            // Clear Timer 3 interrupt flag
	_T3IE = 1;            // Enable Timer 3 interrupt
	T3CONbits.TON = 1;    // Turn on Timer 3 (it won't do anything until PR3 > 0)
}

//
// TEMPORARILY COMMENTED OUT SO THAT DUMMY SENSOR FEEDBACK CAN BE
// USED INSTEAD (SEE BELOW).
//
// This function reads a single sample from the specified
// analog input. It should take less than 2.5us if the chip
// is running at about 30 MIPS.
// Because the dsPIC30F4011 has a 10-bit ADC, the value
// returned will be between 0 and 1023.
/*
unsigned int read_analog_channel(int channel)
{
	ADCHS = channel;          // Select the requested channel
	ADCON1bits.SAMP = 1;      // Start sampling
	__delay32(30);            // 1us delay @ 30 MIPS
	ADCON1bits.SAMP = 0;      // Start Converting
	while (!ADCON1bits.DONE); // Should take 12 * Tad = 1.2us
	return ADCBUF0;
}
*/

//
// Dummy read_analog_channel function for channels 0 and 1.
// This just returns an unsigned int between 0 and 1023
// that's a scaled version of either PDC1 or PDC2.
//

unsigned int read_analog_channel(int channel)
{
	unsigned int value;

	if (channel == 0)
	{
		value = PDC1 / 9;
		if (value > 1023) value = 1023;
	}

	if (channel == 1)
	{
		value = PDC2 / 9;
		if (value > 1023) value = 1023;
	}

	return value;
}

//
// Dummy get_sample function.
// Instead of reading the specified sample from an SD card,
// this function just generates sample values for the x and
// y axes on the fly. A sin function is used for the x-axis
// and a cos function is used for the y-axis.
//

#define PI_TIMES_2_OVER_100 0.062831853

void get_sample(unsigned long sample_number)
{
	// For the time being, instead of reading real samples
	// from an SD card, I'm just generating 4Hz sinusoids.
	x_2 = 512 + 510 * sin(PI_TIMES_2_OVER_100 * (sample_number%100));
	y_2 = 512 + 510 * cos(PI_TIMES_2_OVER_100 * (sample_number%100));
}

//
// Dummy stepping functions.
// These just vary the duty cycles on PWM channels 1 and 2.
//

void x_step_forward()
{
	if (PDC1 < 10000) PDC1++;
}

void x_step_back()
{
	if (PDC1 > 0) PDC1--;
}

void y_step_forward()
{
	if (PDC2 < 10000) PDC2++;
}

void y_step_back()
{
	if (PDC2 > 0) PDC2--;
}
This entry was posted in PIC, Uncategorized and tagged , , , , , , , , . Bookmark the permalink.

Leave a Reply

Fill in your details below or click an icon to log in:

WordPress.com Logo

You are commenting using your WordPress.com account. Log Out / Change )

Twitter picture

You are commenting using your Twitter account. Log Out / Change )

Facebook photo

You are commenting using your Facebook account. Log Out / Change )

Google+ photo

You are commenting using your Google+ account. Log Out / Change )

Connecting to %s