Contains code to drive a small stepper motor with a Freescale h-bridge driver evaluation board, and a Freedom FRDM-KL25Z

Dependencies:   USBDevice mbed

Fork of LVHB Stepper Motor Drive by Freescale

Files at this revision

API Documentation at this revision

Comitter:
bdbohn
Date:
Fri Nov 14 21:03:06 2014 +0000
Commit message:
Initial Releas

Changed in this revision

USBDevice.lib Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
mbed.bld Show annotated file Show diff for this revision Revisions of this file
diff -r 000000000000 -r d14cf1e75576 USBDevice.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/USBDevice.lib	Fri Nov 14 21:03:06 2014 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/USBDevice/#4d3e7f3d5211
diff -r 000000000000 -r d14cf1e75576 main.cpp
--- /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;
+          }   
+        }             
+}
diff -r 000000000000 -r d14cf1e75576 mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Fri Nov 14 21:03:06 2014 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/031413cf7a89
\ No newline at end of file