![](/media/cache/group/NXP_logo_RGB_web.png.50x50_q85.jpg)
Contains code to drive a small stepper motor with a Freescale h-bridge driver evaluation board, and a Freedom FRDM-KL25Z
Fork of LVHB Stepper Motor Drive by
Diff: main.cpp
- Revision:
- 0:d14cf1e75576
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Fri Nov 14 21:03:06 2014 +0000 @@ -0,0 +1,726 @@ + +//Libraries needed +#include "mbed.h" +#include "USBHID.h" + +// We declare a USBHID device. +// HID In/Out Reports are 64 Bytes long +// Vendor ID (VID): 0x15A2 +// Product ID (PID): 0x0138 +// Serial Number: 0x0001 +USBHID hid(64, 64, 0x15A2, 0x0138, 0x0001, true); + +//storage for send and receive data +HID_REPORT send_report; +HID_REPORT recv_report; + +//Stepper Motor States +#define STATE1 1 +#define STATE2 2 +#define STATE3 3 +#define STATE4 4 +#define STATE5 5 +#define STATE6 6 +#define STATE7 7 +#define STATE8 8 + +// USB COMMANDS +// These are sent from the PC +#define WRITE_LED 0x20 +#define WRITE_OUTPUT_EN 0x30 +#define WRITE_STEPS_PER_SEC 0x40 +#define WRITE_RUN_STOP 0x70 +#define WRITE_DIRECTION 0x71 +#define WRITE_ACCEL 0x80 +#define WRITE_RESET 0xA0 +#define WRITE_STEPMODE 0xB0 + + +// MOTOR STATES +#define STOP 0x00 +#define RUN 0x02 +#define RAMP 0x03 +#define RESET 0x05 + +// LED CONSTANTS +#define LEDS_OFF 0x00 +#define RED 0x01 +#define GREEN 0x02 +#define BLUE 0x03 +#define READY_LED 0x04 + +// LOGICAL CONSTANTS +#define OFF 0x00 +#define ON 0x01 + +//step modes +#define QTR 0x01 +#define HALF 0x02 + +//FRDM-KL25Z LEDs +DigitalOut red_led(LED1); +DigitalOut green_led(LED2); +DigitalOut blue_led(LED3); + +//Input pins on Eval Board +DigitalOut IN1A(PTD4); // Pin IN1A input to EVAL board (FRDM PIN Name) +DigitalOut IN1B(PTA12); // Pin IN1B input to EVAL board (FRDM PIN Name) +DigitalOut IN2A(PTA4); // Pin IN2A input to EVAL board (FRDM PIN Name) +DigitalOut IN2B(PTA5); // Pin IN2B input to EVAL board (FRDM PIN Name) + +//Green LED on Eval Board +DigitalOut READY(PTC5); // Pin READY input to EVAL board (FRDM PIN Name) + +//These pins are defined differntly on different parts +//OE for FRDM-17529 and FRDM-17533 Eval Boards +//and PSAVE for FRDM-17C724 and FRDM-17531 Eval Boards +//FRDM-34933 Eval Board does not have either of these pins +DigitalOut OE(PTC7); // Pin OE input to MPC17533, MPC17529 (FRDM PIN Name) + +//variables +static int stepState = 1; //holds current step state of the step being applied to the stepper +static int stepMode = 0; //This is either 1/2 or 1/4 step +static int dir = 0; //rotational direction of stepper +static int rampCount = 0; //counter value used during acceleration ramp-up +static int initflag = 0; //used upon first entry into main at startup +static int runstop = 0; //holds value of running or stopped of commanded value from PC +static int motorState = 0; //holds state of stepper motor state machine (RUN, STOP, RESET, or RAMP) +static int accel = 0; //holds the value of accceleration enabled from PC +static float numRampSteps = 0; //calculated value that is based on the stepper speed after acceleration +static float stepRate = 10; +static int stepMultiplier = 1; +static float next_interval_us = 1200; +static float stepInterval_us = 1200; +static bool acceleration_start = 0; +static bool runStopChange = 0; +static bool accel_wait_flag = 0; + +//static int mode = 0; + +void test_state_mach(void); +void adv_state_mach_half(void); +void adv_state_mach_full(void); +void set_speed(float stepRate); +void acceleration_a(void); +void acceleration_b(void); +void set_speed_with_ramp(void); +void null_function(void); +void clear_accel_wait_flag(void); + + + +// +//Create a Timeout function. Used for ramping up the speed at startup +Timeout accel_ramp; + +//Create a Ticker function. Called during normal runtime to advance the stepper motor +Ticker advance_state; + + +int main() +{ + //set up storage for incoming/outgoing + send_report.length = 64; + recv_report.length = 64; + + red_led = 1; // Red LED = OFF + green_led = 1; // Green LED = OFF + blue_led = 0; // Blue LED = ON + + READY = 0; + + motorState = RESET; + + while(1) + { + //try to read a msg + if(hid.readNB(&recv_report)) + { + if(initflag == true) + { + initflag = false; + blue_led = 1; //turn off blue LED + READY = 0; + } + switch(recv_report.data[0]) //byte 0 of recv_report.data is command + { +//----------------------------------------------------------------------------------------------------------------- +// COMMAND PARSER +//----------------------------------------------------------------------------------------------------------------- +//////// + case WRITE_LED: + switch(recv_report.data[1]) + { + case LEDS_OFF: + red_led = 1; + green_led = 1; + blue_led = 1; + break; + case RED: + if(recv_report.data[2] == 1){red_led = 0;} else {red_led = 1;} + break; + case GREEN: + if(recv_report.data[2] == 1){green_led = 0;} else {green_led = 1;} + break; + case BLUE: + if(recv_report.data[2] == 1){blue_led = 0;} else {blue_led = 1;} + break; + default: + break; + }// End recv report data[1] + break; +////////end case WRITE_LED + +//////// + case WRITE_STEPS_PER_SEC: + + stepRate = recv_report.data[1]; + set_speed(stepRate); + break; +//////// + case WRITE_RUN_STOP: + if(recv_report.data[1] == 1) + { + if(runstop != 1) + { + red_led = 1; + blue_led = 1; + green_led = 0; + runstop = 1; + runStopChange = 1; + + if(accel == 1) + { + motorState = RAMP; + next_interval_us = stepMultiplier; + acceleration_start = 1; + } + else + { + motorState = RUN; + set_speed(stepRate); + } + } + + } + else + { + if(runstop != 0) + { + runstop = 0; + runStopChange = 1; + motorState = STOP; + red_led = 0; + green_led = 1; + blue_led = 1; + } + }//end if(recv_report.data[1] == 1) + break; +//////// + case WRITE_DIRECTION: + + if(recv_report.data[1] == 1) + { + dir = 1; + + } + else + { + dir = 0; + + } + break; +//////// + case WRITE_ACCEL: + if(recv_report.data[1] == 1) + { + accel = 1; + } + else + { + accel = 0; + } + break; +//////// + case WRITE_STEPMODE: + if(recv_report.data[1] == QTR) + { + stepMode = QTR; + stepMultiplier = 8; + + } + else + { + stepMode = HALF; + stepMultiplier = 4; + + } + break; +//////// + default: + break; + }// End Switch recv report data[0] + + +//----------------------------------------------------------------------------------------------------------------- +// end command parser +//----------------------------------------------------------------------------------------------------------------- + + send_report.data[0] = recv_report.data[0]; // Echo Command + send_report.data[1] = recv_report.data[1]; // Echo Subcommand 1 + send_report.data[2] = recv_report.data[2]; // Echo Subcommand 2 + send_report.data[3] = 0x00; + send_report.data[4] = 0x00; + send_report.data[5] = 0x00; + send_report.data[6] = 0x00; + send_report.data[7] = 0x00; + + //Send the report + hid.send(&send_report); + }// End If(hid.readNB(&recv_report)) +///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +//End of USB message handling +///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + + +/************************************************************************************************************************ +// This is handling of Speed, Acceleration, Direction, and Start/Stop +***********************************************************************************************************************/ + + + + + + + switch (motorState) + { + case STOP: + if(runStopChange == 1) + { + runStopChange = 0; + OE = 1; + advance_state.detach(); + } + else + { + OE = 1; + } + break; + + case RUN: + if(runStopChange != 0) + { + OE = 0; + runStopChange = 0; + } + break; + + case RESET: + OE = 1; + runStopChange = false; + motorState = STOP; + break; + + case RAMP: + if(acceleration_start == 1) + { + OE = 0; + acceleration_a(); + acceleration_start = 0; + } + if(accel_wait_flag == 0) + { + acceleration_a(); + accel_wait_flag = 1; + accel_ramp.attach_us(&clear_accel_wait_flag, next_interval_us); + } + + break; + + }// end switch motorState + /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// + + + }//end while +}//end main + + + + + + + + + + +//state machine to advance to next step +//called each time +void adv_state_mach_half() +{ + + READY = 1; + if(OE == 0) + { + + if(dir == 1) + { + switch(stepState) + { + case STATE1: + IN1A = 1; //coil A +, coil B + + IN1B = 0; + IN2A = 1; + IN2B = 0; + stepState = 2; + break; + + case STATE2: + IN1A = 0; //coil A 0, coil B + + IN1B = 0; + IN2A = 1; + IN2B = 0; + stepState = 3; + break; + + case STATE3: + IN1A = 0; //coil A -, coil B + + IN1B = 1; + IN2A = 1; + IN2B = 0; + stepState = 4; + break; + + case STATE4: + IN1A = 0; //coil A -, coil B 0 + IN1B = 1; + IN2A = 0; + IN2B = 0; + stepState = 5; + break; + + case STATE5: + IN1A = 0; //coil A -, coil B - + IN1B = 1; + IN2A = 0; + IN2B = 1; + stepState = 6; + break; + + case STATE6: + IN1A = 0; //coil A 0, coil B - + IN1B = 0; + IN2A = 0; + IN2B = 1; + stepState = 7; + break; + + case STATE7: + IN1A = 1; //coil A +, coil B - + IN1B = 0; + IN2A = 0; + IN2B = 1; + stepState = 8; + break; + + case STATE8: + IN1A = 1; //coil A +, coil B 0 + IN1B = 0; + IN2A = 0; + IN2B = 0; + stepState = 1; + break; + + default: + IN1A = 0; + IN1B = 0; + IN2A = 0; + IN2B = 0; + stepState = 1; + break; + + + } + } + else + { + switch(stepState) + { + case STATE1: + IN1A = 1; //coil A +, coil B 0 + IN1B = 0; + IN2A = 0; + IN2B = 0; + stepState = 2; + break; + + case STATE2: + IN1A = 1; //coil A +, coil B - + IN1B = 0; + IN2A = 0; + IN2B = 1; + stepState = 3; + break; + + case STATE3: + IN1A = 0; //coil A 0 , coil B - + IN1B = 0; + IN2A = 0; + IN2B = 1; + stepState = 4; + break; + + case STATE4: + IN1A = 0; //coil A -, coil B - + IN1B = 1; + IN2A = 0; + IN2B = 1; + stepState = 5; + break; + + case STATE5: + IN1A = 0; //coil A -, coil B 0 + IN1B = 1; + IN2A = 0; + IN2B = 0; + stepState = 6; + break; + + case STATE6: + IN1A = 0; //coil A -, coil B + + IN1B = 1; + IN2A = 1; + IN2B = 0; + stepState = 7; + break; + + case STATE7: + IN1A = 0; //coil A 0, coil B + + IN1B = 0; + IN2A = 1; + IN2B = 0; + stepState = 8; + break; + + case STATE8: + IN1A = 1; //coil A +, coil B + + IN1B = 0; + IN2A = 1; + IN2B = 0; + stepState = 1; + break; + + default: + IN1A = 0; + IN1B = 0; + IN2A = 0; + IN2B = 0; + stepState = 1; + break; + + + } + } + } + +} + + +void adv_state_mach_full() +{ + if(OE == 0) + { + if(dir == 1) + { + switch(stepState) + { + case STATE1: + IN1A = 1; //coil A +, coil B + + IN1B = 0; + IN2A = 1; + IN2B = 0; + stepState = 2; + break; + + case STATE2: + IN1A = 1; //coil A +, coil B - + IN1B = 0; + IN2A = 0; + IN2B = 1; + stepState = 3; + break; + + case STATE3: + IN1A = 0; //coil A -, coil B - + IN1B = 1; + IN2A = 0; + IN2B = 1; + stepState = 4; + break; + + + case STATE4: + IN1A = 0; //coil A -, coil B + + IN1B = 1; + IN2A = 1; + IN2B = 0; + stepState = 1; + break; + + + + default: + IN1A = 0; + IN1B = 0; + IN2A = 0; + IN2B = 0; + stepState = 1; + break; + + + } + } + else + { + switch(stepState) + { + case STATE1: + IN1A = 0; //coil A -, coil B + + IN1B = 1; + IN2A = 1; + IN2B = 0; + stepState = 2; + break; + + case STATE2: + IN1A = 0; //coil A -, coil B - + IN1B = 1; + IN2A = 0; + IN2B = 1; + stepState = 3; + break; + + case STATE3: + IN1A = 1; //coil A +, coil B - + IN1B = 0; + IN2A = 0; + IN2B = 1; + stepState = 4; + break; + + case STATE4: + IN1A = 1; //coil A +, coil B + + IN1B = 0; + IN2A = 1; + IN2B = 0; + stepState = 1; + break; + + default: + IN1A = 0; + IN1B = 0; + IN2A = 0; + IN2B = 0; + stepState = 1; + break; + + + } //end switch + } //end else + } +} +///////////////////////////////////////////////////////////////////////////////// + + +void clear_accel_wait_flag(void) +{ + accel_wait_flag = 0; +} + + + + +void acceleration_a(void) +{ + + rampCount ++; + if(rampCount <= numRampSteps) + { + test_state_mach(); + next_interval_us = 1000000 * (1/float(rampCount * stepMultiplier)); + // accel_ramp.attach_us(&acceleration_b, next_interval_us);//speed); + } + else + { + rampCount = 0; + motorState = RUN; + advance_state.attach_us(&test_state_mach, stepInterval_us); + } + +} +///////////////////////////////////////////////////////////////////////////////// +void acceleration_b(void) +{ + READY = 1; + rampCount ++; + if(rampCount <= 5000) //numRampSteps) + { + test_state_mach(); + next_interval_us = 1000000 * (1/(rampCount)); // * stepMultiplier)); + accel_ramp.attach_us(&acceleration_a, next_interval_us); + } + else + { + READY = 0; + rampCount = 0; + motorState = RUN; + advance_state.attach_us(&test_state_mach, stepInterval_us); + } + +} +///////////////////////////////////////////////////////////////////////////////// + +void test_state_mach(void) +{ + if(stepMode == QTR) + { + adv_state_mach_half(); + } + else + { + adv_state_mach_full(); + } +} +/////////////////////////////////////////////////////////////////////////////////// + + +void set_speed(float speed) +{ + if(motorState == RUN) + { + if(stepMode == QTR) + { + numRampSteps = speed; + stepInterval_us = 1000000*(1.0/(speed * 8)); + advance_state.attach_us(&test_state_mach, stepInterval_us); + } + else + { + numRampSteps = speed; + stepInterval_us = 1000000*(1.0/(speed * 4)); + advance_state.attach_us(&test_state_mach, stepInterval_us); + } + } + else //not in run mode, save the change + { + if(stepMode == QTR) + { + numRampSteps = speed; + stepInterval_us = 1000000*(1.0/(speed * 8)); + } + else + { + stepInterval_us = 1000000*(1.0/(speed * 4)); + numRampSteps = speed; + } + } +}