NXP / Mbed 2 deprecated LVHB Stepper Motor Drive

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

main.cpp@0:d14cf1e75576, 2014-11-14 (annotated)

Committer:
bdbohn
Date:
Fri Nov 14 21:03:06 2014 +0000
Revision:
0:d14cf1e75576
Initial Releas

Who changed what in which revision?

UserRevisionLine numberNew contents of line
bdbohn 0:d14cf1e75576 1
bdbohn 0:d14cf1e75576 2 //Libraries needed
bdbohn 0:d14cf1e75576 3 #include "mbed.h"
bdbohn 0:d14cf1e75576 4 #include "USBHID.h"
bdbohn 0:d14cf1e75576 5
bdbohn 0:d14cf1e75576 6 // We declare a USBHID device.
bdbohn 0:d14cf1e75576 7 // HID In/Out Reports are 64 Bytes long
bdbohn 0:d14cf1e75576 8 // Vendor ID (VID): 0x15A2
bdbohn 0:d14cf1e75576 9 // Product ID (PID): 0x0138
bdbohn 0:d14cf1e75576 10 // Serial Number: 0x0001
bdbohn 0:d14cf1e75576 11 USBHID hid(64, 64, 0x15A2, 0x0138, 0x0001, true);
bdbohn 0:d14cf1e75576 12
bdbohn 0:d14cf1e75576 13 //storage for send and receive data
bdbohn 0:d14cf1e75576 14 HID_REPORT send_report;
bdbohn 0:d14cf1e75576 15 HID_REPORT recv_report;
bdbohn 0:d14cf1e75576 16
bdbohn 0:d14cf1e75576 17 //Stepper Motor States
bdbohn 0:d14cf1e75576 18 #define STATE1 1
bdbohn 0:d14cf1e75576 19 #define STATE2 2
bdbohn 0:d14cf1e75576 20 #define STATE3 3
bdbohn 0:d14cf1e75576 21 #define STATE4 4
bdbohn 0:d14cf1e75576 22 #define STATE5 5
bdbohn 0:d14cf1e75576 23 #define STATE6 6
bdbohn 0:d14cf1e75576 24 #define STATE7 7
bdbohn 0:d14cf1e75576 25 #define STATE8 8
bdbohn 0:d14cf1e75576 26
bdbohn 0:d14cf1e75576 27 // USB COMMANDS
bdbohn 0:d14cf1e75576 28 // These are sent from the PC
bdbohn 0:d14cf1e75576 29 #define WRITE_LED 0x20
bdbohn 0:d14cf1e75576 30 #define WRITE_OUTPUT_EN 0x30
bdbohn 0:d14cf1e75576 31 #define WRITE_STEPS_PER_SEC 0x40
bdbohn 0:d14cf1e75576 32 #define WRITE_RUN_STOP 0x70
bdbohn 0:d14cf1e75576 33 #define WRITE_DIRECTION 0x71
bdbohn 0:d14cf1e75576 34 #define WRITE_ACCEL 0x80
bdbohn 0:d14cf1e75576 35 #define WRITE_RESET 0xA0
bdbohn 0:d14cf1e75576 36 #define WRITE_STEPMODE 0xB0
bdbohn 0:d14cf1e75576 37
bdbohn 0:d14cf1e75576 38
bdbohn 0:d14cf1e75576 39 // MOTOR STATES
bdbohn 0:d14cf1e75576 40 #define STOP 0x00
bdbohn 0:d14cf1e75576 41 #define RUN 0x02
bdbohn 0:d14cf1e75576 42 #define RAMP 0x03
bdbohn 0:d14cf1e75576 43 #define RESET 0x05
bdbohn 0:d14cf1e75576 44
bdbohn 0:d14cf1e75576 45 // LED CONSTANTS
bdbohn 0:d14cf1e75576 46 #define LEDS_OFF 0x00
bdbohn 0:d14cf1e75576 47 #define RED 0x01
bdbohn 0:d14cf1e75576 48 #define GREEN 0x02
bdbohn 0:d14cf1e75576 49 #define BLUE 0x03
bdbohn 0:d14cf1e75576 50 #define READY_LED 0x04
bdbohn 0:d14cf1e75576 51
bdbohn 0:d14cf1e75576 52 // LOGICAL CONSTANTS
bdbohn 0:d14cf1e75576 53 #define OFF 0x00
bdbohn 0:d14cf1e75576 54 #define ON 0x01
bdbohn 0:d14cf1e75576 55
bdbohn 0:d14cf1e75576 56 //step modes
bdbohn 0:d14cf1e75576 57 #define QTR 0x01
bdbohn 0:d14cf1e75576 58 #define HALF 0x02
bdbohn 0:d14cf1e75576 59
bdbohn 0:d14cf1e75576 60 //FRDM-KL25Z LEDs
bdbohn 0:d14cf1e75576 61 DigitalOut red_led(LED1);
bdbohn 0:d14cf1e75576 62 DigitalOut green_led(LED2);
bdbohn 0:d14cf1e75576 63 DigitalOut blue_led(LED3);
bdbohn 0:d14cf1e75576 64
bdbohn 0:d14cf1e75576 65 //Input pins on Eval Board
bdbohn 0:d14cf1e75576 66 DigitalOut IN1A(PTD4); // Pin IN1A input to EVAL board (FRDM PIN Name)
bdbohn 0:d14cf1e75576 67 DigitalOut IN1B(PTA12); // Pin IN1B input to EVAL board (FRDM PIN Name)
bdbohn 0:d14cf1e75576 68 DigitalOut IN2A(PTA4); // Pin IN2A input to EVAL board (FRDM PIN Name)
bdbohn 0:d14cf1e75576 69 DigitalOut IN2B(PTA5); // Pin IN2B input to EVAL board (FRDM PIN Name)
bdbohn 0:d14cf1e75576 70
bdbohn 0:d14cf1e75576 71 //Green LED on Eval Board
bdbohn 0:d14cf1e75576 72 DigitalOut READY(PTC5); // Pin READY input to EVAL board (FRDM PIN Name)
bdbohn 0:d14cf1e75576 73
bdbohn 0:d14cf1e75576 74 //These pins are defined differntly on different parts
bdbohn 0:d14cf1e75576 75 //OE for FRDM-17529 and FRDM-17533 Eval Boards
bdbohn 0:d14cf1e75576 76 //and PSAVE for FRDM-17C724 and FRDM-17531 Eval Boards
bdbohn 0:d14cf1e75576 77 //FRDM-34933 Eval Board does not have either of these pins
bdbohn 0:d14cf1e75576 78 DigitalOut OE(PTC7); // Pin OE input to MPC17533, MPC17529 (FRDM PIN Name)
bdbohn 0:d14cf1e75576 79
bdbohn 0:d14cf1e75576 80 //variables
bdbohn 0:d14cf1e75576 81 static int stepState = 1; //holds current step state of the step being applied to the stepper
bdbohn 0:d14cf1e75576 82 static int stepMode = 0; //This is either 1/2 or 1/4 step
bdbohn 0:d14cf1e75576 83 static int dir = 0; //rotational direction of stepper
bdbohn 0:d14cf1e75576 84 static int rampCount = 0; //counter value used during acceleration ramp-up
bdbohn 0:d14cf1e75576 85 static int initflag = 0; //used upon first entry into main at startup
bdbohn 0:d14cf1e75576 86 static int runstop = 0; //holds value of running or stopped of commanded value from PC
bdbohn 0:d14cf1e75576 87 static int motorState = 0; //holds state of stepper motor state machine (RUN, STOP, RESET, or RAMP)
bdbohn 0:d14cf1e75576 88 static int accel = 0; //holds the value of accceleration enabled from PC
bdbohn 0:d14cf1e75576 89 static float numRampSteps = 0; //calculated value that is based on the stepper speed after acceleration
bdbohn 0:d14cf1e75576 90 static float stepRate = 10;
bdbohn 0:d14cf1e75576 91 static int stepMultiplier = 1;
bdbohn 0:d14cf1e75576 92 static float next_interval_us = 1200;
bdbohn 0:d14cf1e75576 93 static float stepInterval_us = 1200;
bdbohn 0:d14cf1e75576 94 static bool acceleration_start = 0;
bdbohn 0:d14cf1e75576 95 static bool runStopChange = 0;
bdbohn 0:d14cf1e75576 96 static bool accel_wait_flag = 0;
bdbohn 0:d14cf1e75576 97
bdbohn 0:d14cf1e75576 98 //static int mode = 0;
bdbohn 0:d14cf1e75576 99
bdbohn 0:d14cf1e75576 100 void test_state_mach(void);
bdbohn 0:d14cf1e75576 101 void adv_state_mach_half(void);
bdbohn 0:d14cf1e75576 102 void adv_state_mach_full(void);
bdbohn 0:d14cf1e75576 103 void set_speed(float stepRate);
bdbohn 0:d14cf1e75576 104 void acceleration_a(void);
bdbohn 0:d14cf1e75576 105 void acceleration_b(void);
bdbohn 0:d14cf1e75576 106 void set_speed_with_ramp(void);
bdbohn 0:d14cf1e75576 107 void null_function(void);
bdbohn 0:d14cf1e75576 108 void clear_accel_wait_flag(void);
bdbohn 0:d14cf1e75576 109
bdbohn 0:d14cf1e75576 110
bdbohn 0:d14cf1e75576 111
bdbohn 0:d14cf1e75576 112 //
bdbohn 0:d14cf1e75576 113 //Create a Timeout function. Used for ramping up the speed at startup
bdbohn 0:d14cf1e75576 114 Timeout accel_ramp;
bdbohn 0:d14cf1e75576 115
bdbohn 0:d14cf1e75576 116 //Create a Ticker function. Called during normal runtime to advance the stepper motor
bdbohn 0:d14cf1e75576 117 Ticker advance_state;
bdbohn 0:d14cf1e75576 118
bdbohn 0:d14cf1e75576 119
bdbohn 0:d14cf1e75576 120 int main()
bdbohn 0:d14cf1e75576 121 {
bdbohn 0:d14cf1e75576 122 //set up storage for incoming/outgoing
bdbohn 0:d14cf1e75576 123 send_report.length = 64;
bdbohn 0:d14cf1e75576 124 recv_report.length = 64;
bdbohn 0:d14cf1e75576 125
bdbohn 0:d14cf1e75576 126 red_led = 1; // Red LED = OFF
bdbohn 0:d14cf1e75576 127 green_led = 1; // Green LED = OFF
bdbohn 0:d14cf1e75576 128 blue_led = 0; // Blue LED = ON
bdbohn 0:d14cf1e75576 129
bdbohn 0:d14cf1e75576 130 READY = 0;
bdbohn 0:d14cf1e75576 131
bdbohn 0:d14cf1e75576 132 motorState = RESET;
bdbohn 0:d14cf1e75576 133
bdbohn 0:d14cf1e75576 134 while(1)
bdbohn 0:d14cf1e75576 135 {
bdbohn 0:d14cf1e75576 136 //try to read a msg
bdbohn 0:d14cf1e75576 137 if(hid.readNB(&recv_report))
bdbohn 0:d14cf1e75576 138 {
bdbohn 0:d14cf1e75576 139 if(initflag == true)
bdbohn 0:d14cf1e75576 140 {
bdbohn 0:d14cf1e75576 141 initflag = false;
bdbohn 0:d14cf1e75576 142 blue_led = 1; //turn off blue LED
bdbohn 0:d14cf1e75576 143 READY = 0;
bdbohn 0:d14cf1e75576 144 }
bdbohn 0:d14cf1e75576 145 switch(recv_report.data[0]) //byte 0 of recv_report.data is command
bdbohn 0:d14cf1e75576 146 {
bdbohn 0:d14cf1e75576 147 //-----------------------------------------------------------------------------------------------------------------
bdbohn 0:d14cf1e75576 148 // COMMAND PARSER
bdbohn 0:d14cf1e75576 149 //-----------------------------------------------------------------------------------------------------------------
bdbohn 0:d14cf1e75576 150 ////////
bdbohn 0:d14cf1e75576 151 case WRITE_LED:
bdbohn 0:d14cf1e75576 152 switch(recv_report.data[1])
bdbohn 0:d14cf1e75576 153 {
bdbohn 0:d14cf1e75576 154 case LEDS_OFF:
bdbohn 0:d14cf1e75576 155 red_led = 1;
bdbohn 0:d14cf1e75576 156 green_led = 1;
bdbohn 0:d14cf1e75576 157 blue_led = 1;
bdbohn 0:d14cf1e75576 158 break;
bdbohn 0:d14cf1e75576 159 case RED:
bdbohn 0:d14cf1e75576 160 if(recv_report.data[2] == 1){red_led = 0;} else {red_led = 1;}
bdbohn 0:d14cf1e75576 161 break;
bdbohn 0:d14cf1e75576 162 case GREEN:
bdbohn 0:d14cf1e75576 163 if(recv_report.data[2] == 1){green_led = 0;} else {green_led = 1;}
bdbohn 0:d14cf1e75576 164 break;
bdbohn 0:d14cf1e75576 165 case BLUE:
bdbohn 0:d14cf1e75576 166 if(recv_report.data[2] == 1){blue_led = 0;} else {blue_led = 1;}
bdbohn 0:d14cf1e75576 167 break;
bdbohn 0:d14cf1e75576 168 default:
bdbohn 0:d14cf1e75576 169 break;
bdbohn 0:d14cf1e75576 170 }// End recv report data[1]
bdbohn 0:d14cf1e75576 171 break;
bdbohn 0:d14cf1e75576 172 ////////end case WRITE_LED
bdbohn 0:d14cf1e75576 173
bdbohn 0:d14cf1e75576 174 ////////
bdbohn 0:d14cf1e75576 175 case WRITE_STEPS_PER_SEC:
bdbohn 0:d14cf1e75576 176
bdbohn 0:d14cf1e75576 177 stepRate = recv_report.data[1];
bdbohn 0:d14cf1e75576 178 set_speed(stepRate);
bdbohn 0:d14cf1e75576 179 break;
bdbohn 0:d14cf1e75576 180 ////////
bdbohn 0:d14cf1e75576 181 case WRITE_RUN_STOP:
bdbohn 0:d14cf1e75576 182 if(recv_report.data[1] == 1)
bdbohn 0:d14cf1e75576 183 {
bdbohn 0:d14cf1e75576 184 if(runstop != 1)
bdbohn 0:d14cf1e75576 185 {
bdbohn 0:d14cf1e75576 186 red_led = 1;
bdbohn 0:d14cf1e75576 187 blue_led = 1;
bdbohn 0:d14cf1e75576 188 green_led = 0;
bdbohn 0:d14cf1e75576 189 runstop = 1;
bdbohn 0:d14cf1e75576 190 runStopChange = 1;
bdbohn 0:d14cf1e75576 191
bdbohn 0:d14cf1e75576 192 if(accel == 1)
bdbohn 0:d14cf1e75576 193 {
bdbohn 0:d14cf1e75576 194 motorState = RAMP;
bdbohn 0:d14cf1e75576 195 next_interval_us = stepMultiplier;
bdbohn 0:d14cf1e75576 196 acceleration_start = 1;
bdbohn 0:d14cf1e75576 197 }
bdbohn 0:d14cf1e75576 198 else
bdbohn 0:d14cf1e75576 199 {
bdbohn 0:d14cf1e75576 200 motorState = RUN;
bdbohn 0:d14cf1e75576 201 set_speed(stepRate);
bdbohn 0:d14cf1e75576 202 }
bdbohn 0:d14cf1e75576 203 }
bdbohn 0:d14cf1e75576 204
bdbohn 0:d14cf1e75576 205 }
bdbohn 0:d14cf1e75576 206 else
bdbohn 0:d14cf1e75576 207 {
bdbohn 0:d14cf1e75576 208 if(runstop != 0)
bdbohn 0:d14cf1e75576 209 {
bdbohn 0:d14cf1e75576 210 runstop = 0;
bdbohn 0:d14cf1e75576 211 runStopChange = 1;
bdbohn 0:d14cf1e75576 212 motorState = STOP;
bdbohn 0:d14cf1e75576 213 red_led = 0;
bdbohn 0:d14cf1e75576 214 green_led = 1;
bdbohn 0:d14cf1e75576 215 blue_led = 1;
bdbohn 0:d14cf1e75576 216 }
bdbohn 0:d14cf1e75576 217 }//end if(recv_report.data[1] == 1)
bdbohn 0:d14cf1e75576 218 break;
bdbohn 0:d14cf1e75576 219 ////////
bdbohn 0:d14cf1e75576 220 case WRITE_DIRECTION:
bdbohn 0:d14cf1e75576 221
bdbohn 0:d14cf1e75576 222 if(recv_report.data[1] == 1)
bdbohn 0:d14cf1e75576 223 {
bdbohn 0:d14cf1e75576 224 dir = 1;
bdbohn 0:d14cf1e75576 225
bdbohn 0:d14cf1e75576 226 }
bdbohn 0:d14cf1e75576 227 else
bdbohn 0:d14cf1e75576 228 {
bdbohn 0:d14cf1e75576 229 dir = 0;
bdbohn 0:d14cf1e75576 230
bdbohn 0:d14cf1e75576 231 }
bdbohn 0:d14cf1e75576 232 break;
bdbohn 0:d14cf1e75576 233 ////////
bdbohn 0:d14cf1e75576 234 case WRITE_ACCEL:
bdbohn 0:d14cf1e75576 235 if(recv_report.data[1] == 1)
bdbohn 0:d14cf1e75576 236 {
bdbohn 0:d14cf1e75576 237 accel = 1;
bdbohn 0:d14cf1e75576 238 }
bdbohn 0:d14cf1e75576 239 else
bdbohn 0:d14cf1e75576 240 {
bdbohn 0:d14cf1e75576 241 accel = 0;
bdbohn 0:d14cf1e75576 242 }
bdbohn 0:d14cf1e75576 243 break;
bdbohn 0:d14cf1e75576 244 ////////
bdbohn 0:d14cf1e75576 245 case WRITE_STEPMODE:
bdbohn 0:d14cf1e75576 246 if(recv_report.data[1] == QTR)
bdbohn 0:d14cf1e75576 247 {
bdbohn 0:d14cf1e75576 248 stepMode = QTR;
bdbohn 0:d14cf1e75576 249 stepMultiplier = 8;
bdbohn 0:d14cf1e75576 250
bdbohn 0:d14cf1e75576 251 }
bdbohn 0:d14cf1e75576 252 else
bdbohn 0:d14cf1e75576 253 {
bdbohn 0:d14cf1e75576 254 stepMode = HALF;
bdbohn 0:d14cf1e75576 255 stepMultiplier = 4;
bdbohn 0:d14cf1e75576 256
bdbohn 0:d14cf1e75576 257 }
bdbohn 0:d14cf1e75576 258 break;
bdbohn 0:d14cf1e75576 259 ////////
bdbohn 0:d14cf1e75576 260 default:
bdbohn 0:d14cf1e75576 261 break;
bdbohn 0:d14cf1e75576 262 }// End Switch recv report data[0]
bdbohn 0:d14cf1e75576 263
bdbohn 0:d14cf1e75576 264
bdbohn 0:d14cf1e75576 265 //-----------------------------------------------------------------------------------------------------------------
bdbohn 0:d14cf1e75576 266 // end command parser
bdbohn 0:d14cf1e75576 267 //-----------------------------------------------------------------------------------------------------------------
bdbohn 0:d14cf1e75576 268
bdbohn 0:d14cf1e75576 269 send_report.data[0] = recv_report.data[0]; // Echo Command
bdbohn 0:d14cf1e75576 270 send_report.data[1] = recv_report.data[1]; // Echo Subcommand 1
bdbohn 0:d14cf1e75576 271 send_report.data[2] = recv_report.data[2]; // Echo Subcommand 2
bdbohn 0:d14cf1e75576 272 send_report.data[3] = 0x00;
bdbohn 0:d14cf1e75576 273 send_report.data[4] = 0x00;
bdbohn 0:d14cf1e75576 274 send_report.data[5] = 0x00;
bdbohn 0:d14cf1e75576 275 send_report.data[6] = 0x00;
bdbohn 0:d14cf1e75576 276 send_report.data[7] = 0x00;
bdbohn 0:d14cf1e75576 277
bdbohn 0:d14cf1e75576 278 //Send the report
bdbohn 0:d14cf1e75576 279 hid.send(&send_report);
bdbohn 0:d14cf1e75576 280 }// End If(hid.readNB(&recv_report))
bdbohn 0:d14cf1e75576 281 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bdbohn 0:d14cf1e75576 282 //End of USB message handling
bdbohn 0:d14cf1e75576 283 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bdbohn 0:d14cf1e75576 284
bdbohn 0:d14cf1e75576 285
bdbohn 0:d14cf1e75576 286 /************************************************************************************************************************
bdbohn 0:d14cf1e75576 287 // This is handling of Speed, Acceleration, Direction, and Start/Stop
bdbohn 0:d14cf1e75576 288 ***********************************************************************************************************************/
bdbohn 0:d14cf1e75576 289
bdbohn 0:d14cf1e75576 290
bdbohn 0:d14cf1e75576 291
bdbohn 0:d14cf1e75576 292
bdbohn 0:d14cf1e75576 293
bdbohn 0:d14cf1e75576 294
bdbohn 0:d14cf1e75576 295 switch (motorState)
bdbohn 0:d14cf1e75576 296 {
bdbohn 0:d14cf1e75576 297 case STOP:
bdbohn 0:d14cf1e75576 298 if(runStopChange == 1)
bdbohn 0:d14cf1e75576 299 {
bdbohn 0:d14cf1e75576 300 runStopChange = 0;
bdbohn 0:d14cf1e75576 301 OE = 1;
bdbohn 0:d14cf1e75576 302 advance_state.detach();
bdbohn 0:d14cf1e75576 303 }
bdbohn 0:d14cf1e75576 304 else
bdbohn 0:d14cf1e75576 305 {
bdbohn 0:d14cf1e75576 306 OE = 1;
bdbohn 0:d14cf1e75576 307 }
bdbohn 0:d14cf1e75576 308 break;
bdbohn 0:d14cf1e75576 309
bdbohn 0:d14cf1e75576 310 case RUN:
bdbohn 0:d14cf1e75576 311 if(runStopChange != 0)
bdbohn 0:d14cf1e75576 312 {
bdbohn 0:d14cf1e75576 313 OE = 0;
bdbohn 0:d14cf1e75576 314 runStopChange = 0;
bdbohn 0:d14cf1e75576 315 }
bdbohn 0:d14cf1e75576 316 break;
bdbohn 0:d14cf1e75576 317
bdbohn 0:d14cf1e75576 318 case RESET:
bdbohn 0:d14cf1e75576 319 OE = 1;
bdbohn 0:d14cf1e75576 320 runStopChange = false;
bdbohn 0:d14cf1e75576 321 motorState = STOP;
bdbohn 0:d14cf1e75576 322 break;
bdbohn 0:d14cf1e75576 323
bdbohn 0:d14cf1e75576 324 case RAMP:
bdbohn 0:d14cf1e75576 325 if(acceleration_start == 1)
bdbohn 0:d14cf1e75576 326 {
bdbohn 0:d14cf1e75576 327 OE = 0;
bdbohn 0:d14cf1e75576 328 acceleration_a();
bdbohn 0:d14cf1e75576 329 acceleration_start = 0;
bdbohn 0:d14cf1e75576 330 }
bdbohn 0:d14cf1e75576 331 if(accel_wait_flag == 0)
bdbohn 0:d14cf1e75576 332 {
bdbohn 0:d14cf1e75576 333 acceleration_a();
bdbohn 0:d14cf1e75576 334 accel_wait_flag = 1;
bdbohn 0:d14cf1e75576 335 accel_ramp.attach_us(&clear_accel_wait_flag, next_interval_us);
bdbohn 0:d14cf1e75576 336 }
bdbohn 0:d14cf1e75576 337
bdbohn 0:d14cf1e75576 338 break;
bdbohn 0:d14cf1e75576 339
bdbohn 0:d14cf1e75576 340 }// end switch motorState
bdbohn 0:d14cf1e75576 341 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
bdbohn 0:d14cf1e75576 342
bdbohn 0:d14cf1e75576 343
bdbohn 0:d14cf1e75576 344 }//end while
bdbohn 0:d14cf1e75576 345 }//end main
bdbohn 0:d14cf1e75576 346
bdbohn 0:d14cf1e75576 347
bdbohn 0:d14cf1e75576 348
bdbohn 0:d14cf1e75576 349
bdbohn 0:d14cf1e75576 350
bdbohn 0:d14cf1e75576 351
bdbohn 0:d14cf1e75576 352
bdbohn 0:d14cf1e75576 353
bdbohn 0:d14cf1e75576 354
bdbohn 0:d14cf1e75576 355
bdbohn 0:d14cf1e75576 356 //state machine to advance to next step
bdbohn 0:d14cf1e75576 357 //called each time
bdbohn 0:d14cf1e75576 358 void adv_state_mach_half()
bdbohn 0:d14cf1e75576 359 {
bdbohn 0:d14cf1e75576 360
bdbohn 0:d14cf1e75576 361 READY = 1;
bdbohn 0:d14cf1e75576 362 if(OE == 0)
bdbohn 0:d14cf1e75576 363 {
bdbohn 0:d14cf1e75576 364
bdbohn 0:d14cf1e75576 365 if(dir == 1)
bdbohn 0:d14cf1e75576 366 {
bdbohn 0:d14cf1e75576 367 switch(stepState)
bdbohn 0:d14cf1e75576 368 {
bdbohn 0:d14cf1e75576 369 case STATE1:
bdbohn 0:d14cf1e75576 370 IN1A = 1; //coil A +, coil B +
bdbohn 0:d14cf1e75576 371 IN1B = 0;
bdbohn 0:d14cf1e75576 372 IN2A = 1;
bdbohn 0:d14cf1e75576 373 IN2B = 0;
bdbohn 0:d14cf1e75576 374 stepState = 2;
bdbohn 0:d14cf1e75576 375 break;
bdbohn 0:d14cf1e75576 376
bdbohn 0:d14cf1e75576 377 case STATE2:
bdbohn 0:d14cf1e75576 378 IN1A = 0; //coil A 0, coil B +
bdbohn 0:d14cf1e75576 379 IN1B = 0;
bdbohn 0:d14cf1e75576 380 IN2A = 1;
bdbohn 0:d14cf1e75576 381 IN2B = 0;
bdbohn 0:d14cf1e75576 382 stepState = 3;
bdbohn 0:d14cf1e75576 383 break;
bdbohn 0:d14cf1e75576 384
bdbohn 0:d14cf1e75576 385 case STATE3:
bdbohn 0:d14cf1e75576 386 IN1A = 0; //coil A -, coil B +
bdbohn 0:d14cf1e75576 387 IN1B = 1;
bdbohn 0:d14cf1e75576 388 IN2A = 1;
bdbohn 0:d14cf1e75576 389 IN2B = 0;
bdbohn 0:d14cf1e75576 390 stepState = 4;
bdbohn 0:d14cf1e75576 391 break;
bdbohn 0:d14cf1e75576 392
bdbohn 0:d14cf1e75576 393 case STATE4:
bdbohn 0:d14cf1e75576 394 IN1A = 0; //coil A -, coil B 0
bdbohn 0:d14cf1e75576 395 IN1B = 1;
bdbohn 0:d14cf1e75576 396 IN2A = 0;
bdbohn 0:d14cf1e75576 397 IN2B = 0;
bdbohn 0:d14cf1e75576 398 stepState = 5;
bdbohn 0:d14cf1e75576 399 break;
bdbohn 0:d14cf1e75576 400
bdbohn 0:d14cf1e75576 401 case STATE5:
bdbohn 0:d14cf1e75576 402 IN1A = 0; //coil A -, coil B -
bdbohn 0:d14cf1e75576 403 IN1B = 1;
bdbohn 0:d14cf1e75576 404 IN2A = 0;
bdbohn 0:d14cf1e75576 405 IN2B = 1;
bdbohn 0:d14cf1e75576 406 stepState = 6;
bdbohn 0:d14cf1e75576 407 break;
bdbohn 0:d14cf1e75576 408
bdbohn 0:d14cf1e75576 409 case STATE6:
bdbohn 0:d14cf1e75576 410 IN1A = 0; //coil A 0, coil B -
bdbohn 0:d14cf1e75576 411 IN1B = 0;
bdbohn 0:d14cf1e75576 412 IN2A = 0;
bdbohn 0:d14cf1e75576 413 IN2B = 1;
bdbohn 0:d14cf1e75576 414 stepState = 7;
bdbohn 0:d14cf1e75576 415 break;
bdbohn 0:d14cf1e75576 416
bdbohn 0:d14cf1e75576 417 case STATE7:
bdbohn 0:d14cf1e75576 418 IN1A = 1; //coil A +, coil B -
bdbohn 0:d14cf1e75576 419 IN1B = 0;
bdbohn 0:d14cf1e75576 420 IN2A = 0;
bdbohn 0:d14cf1e75576 421 IN2B = 1;
bdbohn 0:d14cf1e75576 422 stepState = 8;
bdbohn 0:d14cf1e75576 423 break;
bdbohn 0:d14cf1e75576 424
bdbohn 0:d14cf1e75576 425 case STATE8:
bdbohn 0:d14cf1e75576 426 IN1A = 1; //coil A +, coil B 0
bdbohn 0:d14cf1e75576 427 IN1B = 0;
bdbohn 0:d14cf1e75576 428 IN2A = 0;
bdbohn 0:d14cf1e75576 429 IN2B = 0;
bdbohn 0:d14cf1e75576 430 stepState = 1;
bdbohn 0:d14cf1e75576 431 break;
bdbohn 0:d14cf1e75576 432
bdbohn 0:d14cf1e75576 433 default:
bdbohn 0:d14cf1e75576 434 IN1A = 0;
bdbohn 0:d14cf1e75576 435 IN1B = 0;
bdbohn 0:d14cf1e75576 436 IN2A = 0;
bdbohn 0:d14cf1e75576 437 IN2B = 0;
bdbohn 0:d14cf1e75576 438 stepState = 1;
bdbohn 0:d14cf1e75576 439 break;
bdbohn 0:d14cf1e75576 440
bdbohn 0:d14cf1e75576 441
bdbohn 0:d14cf1e75576 442 }
bdbohn 0:d14cf1e75576 443 }
bdbohn 0:d14cf1e75576 444 else
bdbohn 0:d14cf1e75576 445 {
bdbohn 0:d14cf1e75576 446 switch(stepState)
bdbohn 0:d14cf1e75576 447 {
bdbohn 0:d14cf1e75576 448 case STATE1:
bdbohn 0:d14cf1e75576 449 IN1A = 1; //coil A +, coil B 0
bdbohn 0:d14cf1e75576 450 IN1B = 0;
bdbohn 0:d14cf1e75576 451 IN2A = 0;
bdbohn 0:d14cf1e75576 452 IN2B = 0;
bdbohn 0:d14cf1e75576 453 stepState = 2;
bdbohn 0:d14cf1e75576 454 break;
bdbohn 0:d14cf1e75576 455
bdbohn 0:d14cf1e75576 456 case STATE2:
bdbohn 0:d14cf1e75576 457 IN1A = 1; //coil A +, coil B -
bdbohn 0:d14cf1e75576 458 IN1B = 0;
bdbohn 0:d14cf1e75576 459 IN2A = 0;
bdbohn 0:d14cf1e75576 460 IN2B = 1;
bdbohn 0:d14cf1e75576 461 stepState = 3;
bdbohn 0:d14cf1e75576 462 break;
bdbohn 0:d14cf1e75576 463
bdbohn 0:d14cf1e75576 464 case STATE3:
bdbohn 0:d14cf1e75576 465 IN1A = 0; //coil A 0 , coil B -
bdbohn 0:d14cf1e75576 466 IN1B = 0;
bdbohn 0:d14cf1e75576 467 IN2A = 0;
bdbohn 0:d14cf1e75576 468 IN2B = 1;
bdbohn 0:d14cf1e75576 469 stepState = 4;
bdbohn 0:d14cf1e75576 470 break;
bdbohn 0:d14cf1e75576 471
bdbohn 0:d14cf1e75576 472 case STATE4:
bdbohn 0:d14cf1e75576 473 IN1A = 0; //coil A -, coil B -
bdbohn 0:d14cf1e75576 474 IN1B = 1;
bdbohn 0:d14cf1e75576 475 IN2A = 0;
bdbohn 0:d14cf1e75576 476 IN2B = 1;
bdbohn 0:d14cf1e75576 477 stepState = 5;
bdbohn 0:d14cf1e75576 478 break;
bdbohn 0:d14cf1e75576 479
bdbohn 0:d14cf1e75576 480 case STATE5:
bdbohn 0:d14cf1e75576 481 IN1A = 0; //coil A -, coil B 0
bdbohn 0:d14cf1e75576 482 IN1B = 1;
bdbohn 0:d14cf1e75576 483 IN2A = 0;
bdbohn 0:d14cf1e75576 484 IN2B = 0;
bdbohn 0:d14cf1e75576 485 stepState = 6;
bdbohn 0:d14cf1e75576 486 break;
bdbohn 0:d14cf1e75576 487
bdbohn 0:d14cf1e75576 488 case STATE6:
bdbohn 0:d14cf1e75576 489 IN1A = 0; //coil A -, coil B +
bdbohn 0:d14cf1e75576 490 IN1B = 1;
bdbohn 0:d14cf1e75576 491 IN2A = 1;
bdbohn 0:d14cf1e75576 492 IN2B = 0;
bdbohn 0:d14cf1e75576 493 stepState = 7;
bdbohn 0:d14cf1e75576 494 break;
bdbohn 0:d14cf1e75576 495
bdbohn 0:d14cf1e75576 496 case STATE7:
bdbohn 0:d14cf1e75576 497 IN1A = 0; //coil A 0, coil B +
bdbohn 0:d14cf1e75576 498 IN1B = 0;
bdbohn 0:d14cf1e75576 499 IN2A = 1;
bdbohn 0:d14cf1e75576 500 IN2B = 0;
bdbohn 0:d14cf1e75576 501 stepState = 8;
bdbohn 0:d14cf1e75576 502 break;
bdbohn 0:d14cf1e75576 503
bdbohn 0:d14cf1e75576 504 case STATE8:
bdbohn 0:d14cf1e75576 505 IN1A = 1; //coil A +, coil B +
bdbohn 0:d14cf1e75576 506 IN1B = 0;
bdbohn 0:d14cf1e75576 507 IN2A = 1;
bdbohn 0:d14cf1e75576 508 IN2B = 0;
bdbohn 0:d14cf1e75576 509 stepState = 1;
bdbohn 0:d14cf1e75576 510 break;
bdbohn 0:d14cf1e75576 511
bdbohn 0:d14cf1e75576 512 default:
bdbohn 0:d14cf1e75576 513 IN1A = 0;
bdbohn 0:d14cf1e75576 514 IN1B = 0;
bdbohn 0:d14cf1e75576 515 IN2A = 0;
bdbohn 0:d14cf1e75576 516 IN2B = 0;
bdbohn 0:d14cf1e75576 517 stepState = 1;
bdbohn 0:d14cf1e75576 518 break;
bdbohn 0:d14cf1e75576 519
bdbohn 0:d14cf1e75576 520
bdbohn 0:d14cf1e75576 521 }
bdbohn 0:d14cf1e75576 522 }
bdbohn 0:d14cf1e75576 523 }
bdbohn 0:d14cf1e75576 524
bdbohn 0:d14cf1e75576 525 }
bdbohn 0:d14cf1e75576 526
bdbohn 0:d14cf1e75576 527
bdbohn 0:d14cf1e75576 528 void adv_state_mach_full()
bdbohn 0:d14cf1e75576 529 {
bdbohn 0:d14cf1e75576 530 if(OE == 0)
bdbohn 0:d14cf1e75576 531 {
bdbohn 0:d14cf1e75576 532 if(dir == 1)
bdbohn 0:d14cf1e75576 533 {
bdbohn 0:d14cf1e75576 534 switch(stepState)
bdbohn 0:d14cf1e75576 535 {
bdbohn 0:d14cf1e75576 536 case STATE1:
bdbohn 0:d14cf1e75576 537 IN1A = 1; //coil A +, coil B +
bdbohn 0:d14cf1e75576 538 IN1B = 0;
bdbohn 0:d14cf1e75576 539 IN2A = 1;
bdbohn 0:d14cf1e75576 540 IN2B = 0;
bdbohn 0:d14cf1e75576 541 stepState = 2;
bdbohn 0:d14cf1e75576 542 break;
bdbohn 0:d14cf1e75576 543
bdbohn 0:d14cf1e75576 544 case STATE2:
bdbohn 0:d14cf1e75576 545 IN1A = 1; //coil A +, coil B -
bdbohn 0:d14cf1e75576 546 IN1B = 0;
bdbohn 0:d14cf1e75576 547 IN2A = 0;
bdbohn 0:d14cf1e75576 548 IN2B = 1;
bdbohn 0:d14cf1e75576 549 stepState = 3;
bdbohn 0:d14cf1e75576 550 break;
bdbohn 0:d14cf1e75576 551
bdbohn 0:d14cf1e75576 552 case STATE3:
bdbohn 0:d14cf1e75576 553 IN1A = 0; //coil A -, coil B -
bdbohn 0:d14cf1e75576 554 IN1B = 1;
bdbohn 0:d14cf1e75576 555 IN2A = 0;
bdbohn 0:d14cf1e75576 556 IN2B = 1;
bdbohn 0:d14cf1e75576 557 stepState = 4;
bdbohn 0:d14cf1e75576 558 break;
bdbohn 0:d14cf1e75576 559
bdbohn 0:d14cf1e75576 560
bdbohn 0:d14cf1e75576 561 case STATE4:
bdbohn 0:d14cf1e75576 562 IN1A = 0; //coil A -, coil B +
bdbohn 0:d14cf1e75576 563 IN1B = 1;
bdbohn 0:d14cf1e75576 564 IN2A = 1;
bdbohn 0:d14cf1e75576 565 IN2B = 0;
bdbohn 0:d14cf1e75576 566 stepState = 1;
bdbohn 0:d14cf1e75576 567 break;
bdbohn 0:d14cf1e75576 568
bdbohn 0:d14cf1e75576 569
bdbohn 0:d14cf1e75576 570
bdbohn 0:d14cf1e75576 571 default:
bdbohn 0:d14cf1e75576 572 IN1A = 0;
bdbohn 0:d14cf1e75576 573 IN1B = 0;
bdbohn 0:d14cf1e75576 574 IN2A = 0;
bdbohn 0:d14cf1e75576 575 IN2B = 0;
bdbohn 0:d14cf1e75576 576 stepState = 1;
bdbohn 0:d14cf1e75576 577 break;
bdbohn 0:d14cf1e75576 578
bdbohn 0:d14cf1e75576 579
bdbohn 0:d14cf1e75576 580 }
bdbohn 0:d14cf1e75576 581 }
bdbohn 0:d14cf1e75576 582 else
bdbohn 0:d14cf1e75576 583 {
bdbohn 0:d14cf1e75576 584 switch(stepState)
bdbohn 0:d14cf1e75576 585 {
bdbohn 0:d14cf1e75576 586 case STATE1:
bdbohn 0:d14cf1e75576 587 IN1A = 0; //coil A -, coil B +
bdbohn 0:d14cf1e75576 588 IN1B = 1;
bdbohn 0:d14cf1e75576 589 IN2A = 1;
bdbohn 0:d14cf1e75576 590 IN2B = 0;
bdbohn 0:d14cf1e75576 591 stepState = 2;
bdbohn 0:d14cf1e75576 592 break;
bdbohn 0:d14cf1e75576 593
bdbohn 0:d14cf1e75576 594 case STATE2:
bdbohn 0:d14cf1e75576 595 IN1A = 0; //coil A -, coil B -
bdbohn 0:d14cf1e75576 596 IN1B = 1;
bdbohn 0:d14cf1e75576 597 IN2A = 0;
bdbohn 0:d14cf1e75576 598 IN2B = 1;
bdbohn 0:d14cf1e75576 599 stepState = 3;
bdbohn 0:d14cf1e75576 600 break;
bdbohn 0:d14cf1e75576 601
bdbohn 0:d14cf1e75576 602 case STATE3:
bdbohn 0:d14cf1e75576 603 IN1A = 1; //coil A +, coil B -
bdbohn 0:d14cf1e75576 604 IN1B = 0;
bdbohn 0:d14cf1e75576 605 IN2A = 0;
bdbohn 0:d14cf1e75576 606 IN2B = 1;
bdbohn 0:d14cf1e75576 607 stepState = 4;
bdbohn 0:d14cf1e75576 608 break;
bdbohn 0:d14cf1e75576 609
bdbohn 0:d14cf1e75576 610 case STATE4:
bdbohn 0:d14cf1e75576 611 IN1A = 1; //coil A +, coil B +
bdbohn 0:d14cf1e75576 612 IN1B = 0;
bdbohn 0:d14cf1e75576 613 IN2A = 1;
bdbohn 0:d14cf1e75576 614 IN2B = 0;
bdbohn 0:d14cf1e75576 615 stepState = 1;
bdbohn 0:d14cf1e75576 616 break;
bdbohn 0:d14cf1e75576 617
bdbohn 0:d14cf1e75576 618 default:
bdbohn 0:d14cf1e75576 619 IN1A = 0;
bdbohn 0:d14cf1e75576 620 IN1B = 0;
bdbohn 0:d14cf1e75576 621 IN2A = 0;
bdbohn 0:d14cf1e75576 622 IN2B = 0;
bdbohn 0:d14cf1e75576 623 stepState = 1;
bdbohn 0:d14cf1e75576 624 break;
bdbohn 0:d14cf1e75576 625
bdbohn 0:d14cf1e75576 626
bdbohn 0:d14cf1e75576 627 } //end switch
bdbohn 0:d14cf1e75576 628 } //end else
bdbohn 0:d14cf1e75576 629 }
bdbohn 0:d14cf1e75576 630 }
bdbohn 0:d14cf1e75576 631 /////////////////////////////////////////////////////////////////////////////////
bdbohn 0:d14cf1e75576 632
bdbohn 0:d14cf1e75576 633
bdbohn 0:d14cf1e75576 634 void clear_accel_wait_flag(void)
bdbohn 0:d14cf1e75576 635 {
bdbohn 0:d14cf1e75576 636 accel_wait_flag = 0;
bdbohn 0:d14cf1e75576 637 }
bdbohn 0:d14cf1e75576 638
bdbohn 0:d14cf1e75576 639
bdbohn 0:d14cf1e75576 640
bdbohn 0:d14cf1e75576 641
bdbohn 0:d14cf1e75576 642 void acceleration_a(void)
bdbohn 0:d14cf1e75576 643 {
bdbohn 0:d14cf1e75576 644
bdbohn 0:d14cf1e75576 645 rampCount ++;
bdbohn 0:d14cf1e75576 646 if(rampCount <= numRampSteps)
bdbohn 0:d14cf1e75576 647 {
bdbohn 0:d14cf1e75576 648 test_state_mach();
bdbohn 0:d14cf1e75576 649 next_interval_us = 1000000 * (1/float(rampCount * stepMultiplier));
bdbohn 0:d14cf1e75576 650 // accel_ramp.attach_us(&acceleration_b, next_interval_us);//speed);
bdbohn 0:d14cf1e75576 651 }
bdbohn 0:d14cf1e75576 652 else
bdbohn 0:d14cf1e75576 653 {
bdbohn 0:d14cf1e75576 654 rampCount = 0;
bdbohn 0:d14cf1e75576 655 motorState = RUN;
bdbohn 0:d14cf1e75576 656 advance_state.attach_us(&test_state_mach, stepInterval_us);
bdbohn 0:d14cf1e75576 657 }
bdbohn 0:d14cf1e75576 658
bdbohn 0:d14cf1e75576 659 }
bdbohn 0:d14cf1e75576 660 /////////////////////////////////////////////////////////////////////////////////
bdbohn 0:d14cf1e75576 661 void acceleration_b(void)
bdbohn 0:d14cf1e75576 662 {
bdbohn 0:d14cf1e75576 663 READY = 1;
bdbohn 0:d14cf1e75576 664 rampCount ++;
bdbohn 0:d14cf1e75576 665 if(rampCount <= 5000) //numRampSteps)
bdbohn 0:d14cf1e75576 666 {
bdbohn 0:d14cf1e75576 667 test_state_mach();
bdbohn 0:d14cf1e75576 668 next_interval_us = 1000000 * (1/(rampCount)); // * stepMultiplier));
bdbohn 0:d14cf1e75576 669 accel_ramp.attach_us(&acceleration_a, next_interval_us);
bdbohn 0:d14cf1e75576 670 }
bdbohn 0:d14cf1e75576 671 else
bdbohn 0:d14cf1e75576 672 {
bdbohn 0:d14cf1e75576 673 READY = 0;
bdbohn 0:d14cf1e75576 674 rampCount = 0;
bdbohn 0:d14cf1e75576 675 motorState = RUN;
bdbohn 0:d14cf1e75576 676 advance_state.attach_us(&test_state_mach, stepInterval_us);
bdbohn 0:d14cf1e75576 677 }
bdbohn 0:d14cf1e75576 678
bdbohn 0:d14cf1e75576 679 }
bdbohn 0:d14cf1e75576 680 /////////////////////////////////////////////////////////////////////////////////
bdbohn 0:d14cf1e75576 681
bdbohn 0:d14cf1e75576 682 void test_state_mach(void)
bdbohn 0:d14cf1e75576 683 {
bdbohn 0:d14cf1e75576 684 if(stepMode == QTR)
bdbohn 0:d14cf1e75576 685 {
bdbohn 0:d14cf1e75576 686 adv_state_mach_half();
bdbohn 0:d14cf1e75576 687 }
bdbohn 0:d14cf1e75576 688 else
bdbohn 0:d14cf1e75576 689 {
bdbohn 0:d14cf1e75576 690 adv_state_mach_full();
bdbohn 0:d14cf1e75576 691 }
bdbohn 0:d14cf1e75576 692 }
bdbohn 0:d14cf1e75576 693 ///////////////////////////////////////////////////////////////////////////////////
bdbohn 0:d14cf1e75576 694
bdbohn 0:d14cf1e75576 695
bdbohn 0:d14cf1e75576 696 void set_speed(float speed)
bdbohn 0:d14cf1e75576 697 {
bdbohn 0:d14cf1e75576 698 if(motorState == RUN)
bdbohn 0:d14cf1e75576 699 {
bdbohn 0:d14cf1e75576 700 if(stepMode == QTR)
bdbohn 0:d14cf1e75576 701 {
bdbohn 0:d14cf1e75576 702 numRampSteps = speed;
bdbohn 0:d14cf1e75576 703 stepInterval_us = 1000000*(1.0/(speed * 8));
bdbohn 0:d14cf1e75576 704 advance_state.attach_us(&test_state_mach, stepInterval_us);
bdbohn 0:d14cf1e75576 705 }
bdbohn 0:d14cf1e75576 706 else
bdbohn 0:d14cf1e75576 707 {
bdbohn 0:d14cf1e75576 708 numRampSteps = speed;
bdbohn 0:d14cf1e75576 709 stepInterval_us = 1000000*(1.0/(speed * 4));
bdbohn 0:d14cf1e75576 710 advance_state.attach_us(&test_state_mach, stepInterval_us);
bdbohn 0:d14cf1e75576 711 }
bdbohn 0:d14cf1e75576 712 }
bdbohn 0:d14cf1e75576 713 else //not in run mode, save the change
bdbohn 0:d14cf1e75576 714 {
bdbohn 0:d14cf1e75576 715 if(stepMode == QTR)
bdbohn 0:d14cf1e75576 716 {
bdbohn 0:d14cf1e75576 717 numRampSteps = speed;
bdbohn 0:d14cf1e75576 718 stepInterval_us = 1000000*(1.0/(speed * 8));
bdbohn 0:d14cf1e75576 719 }
bdbohn 0:d14cf1e75576 720 else
bdbohn 0:d14cf1e75576 721 {
bdbohn 0:d14cf1e75576 722 stepInterval_us = 1000000*(1.0/(speed * 4));
bdbohn 0:d14cf1e75576 723 numRampSteps = speed;
bdbohn 0:d14cf1e75576 724 }
bdbohn 0:d14cf1e75576 725 }
bdbohn 0:d14cf1e75576 726 }