jim shepard / Mbed 2 deprecated 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.

Dependencies:   USBDevice mbed

main.cpp@0:ac34ca83efb9, 2017-01-16 (annotated)

Committer:
jimESS
Date:
Mon Jan 16 23:57:29 2017 +0000
Revision:
0:ac34ca83efb9
Changes to align I/O pins to FRDM board

Who changed what in which revision?

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