jim shepard
/
mbed_lvhb_dc_motor
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Diff: main.cpp
- Revision:
- 0:ac34ca83efb9
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Mon Jan 16 23:57:29 2017 +0000
@@ -0,0 +1,729 @@
+#include "mbed.h"
+
+
+//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(PTA5); // Pin IN1A input to EVAL board (FRDM PIN Name)
+DigitalOut IN1B(PTC8); // Pin IN1B input to EVAL board (FRDM PIN Name)
+DigitalOut IN2A(PTA13); // Pin IN2A input to EVAL board (FRDM PIN Name)
+DigitalOut IN2B(PTD5); // 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(PTA2); // 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;
+ }
+ }
+}
+