Program for driving bipolar stepper motor model 17HA0403-32N. Motor can be driven in wave, full step, and half step modes in clockwise and anticlockwise directions.
Fork of LVHB Stepper Motor Drive by
main.cpp
- Committer:
- bdbohn
- Date:
- 2014-11-14
- Revision:
- 0:d14cf1e75576
- Child:
- 1:48b0212c91e9
File content as of revision 0:d14cf1e75576:
//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; } } }