NXP
Contains code to drive a small stepper motor with a Freescale h-bridge driver evaluation board, and a Freedom FRDM-KL25Z
Fork of
LVHB Stepper Motor Drive
by 
Freescale
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 00002 //Libraries needed 00003 #include "mbed.h" 00004 #include "USBHID.h" 00005 00006 // We declare a USBHID device. 00007 // HID In/Out Reports are 64 Bytes long 00008 // Vendor ID (VID): 0x15A2 00009 // Product ID (PID): 0x0138 00010 // Serial Number: 0x0001 00011 USBHID hid(64, 64, 0x15A2, 0x0138, 0x0001, true); 00012 00013 //storage for send and receive data 00014 HID_REPORT send_report; 00015 HID_REPORT recv_report; 00016 00017 //Stepper Motor States 00018 #define STATE1 1 00019 #define STATE2 2 00020 #define STATE3 3 00021 #define STATE4 4 00022 #define STATE5 5 00023 #define STATE6 6 00024 #define STATE7 7 00025 #define STATE8 8 00026 00027 // USB COMMANDS 00028 // These are sent from the PC 00029 #define WRITE_LED 0x20 00030 #define WRITE_OUTPUT_EN 0x30 00031 #define WRITE_STEPS_PER_SEC 0x40 00032 #define WRITE_RUN_STOP 0x70 00033 #define WRITE_DIRECTION 0x71 00034 #define WRITE_ACCEL 0x80 00035 #define WRITE_RESET 0xA0 00036 #define WRITE_STEPMODE 0xB0 00037 00038 00039 // MOTOR STATES 00040 #define STOP 0x00 00041 #define RUN 0x02 00042 #define RAMP 0x03 00043 #define RESET 0x05 00044 00045 // LED CONSTANTS 00046 #define LEDS_OFF 0x00 00047 #define RED 0x01 00048 #define GREEN 0x02 00049 #define BLUE 0x03 00050 #define READY_LED 0x04 00051 00052 // LOGICAL CONSTANTS 00053 #define OFF 0x00 00054 #define ON 0x01 00055 00056 //step modes 00057 #define QTR 0x01 00058 #define HALF 0x02 00059 00060 //FRDM-KL25Z LEDs 00061 DigitalOut red_led(LED1); 00062 DigitalOut green_led(LED2); 00063 DigitalOut blue_led(LED3); 00064 00065 //Input pins on Eval Board 00066 DigitalOut IN1A(PTD4); // Pin IN1A input to EVAL board (FRDM PIN Name) 00067 DigitalOut IN1B(PTA12); // Pin IN1B input to EVAL board (FRDM PIN Name) 00068 DigitalOut IN2A(PTA4); // Pin IN2A input to EVAL board (FRDM PIN Name) 00069 DigitalOut IN2B(PTA5); // Pin IN2B input to EVAL board (FRDM PIN Name) 00070 00071 //Green LED on Eval Board 00072 DigitalOut READY(PTC5); // Pin READY input to EVAL board (FRDM PIN Name) 00073 00074 //These pins are defined differntly on different parts 00075 //OE for FRDM-17529 and FRDM-17533 Eval Boards 00076 //and PSAVE for FRDM-17C724 and FRDM-17531 Eval Boards 00077 //FRDM-34933 Eval Board does not have either of these pins 00078 DigitalOut OE(PTC7); // Pin OE input to MPC17533, MPC17529 (FRDM PIN Name) 00079 00080 //variables 00081 static int stepState = 1; //holds current step state of the step being applied to the stepper 00082 static int stepMode = 0; //This is either 1/2 or 1/4 step 00083 static int dir = 0; //rotational direction of stepper 00084 static int rampCount = 0; //counter value used during acceleration ramp-up 00085 static int initflag = 0; //used upon first entry into main at startup 00086 static int runstop = 0; //holds value of running or stopped of commanded value from PC 00087 static int motorState = 0; //holds state of stepper motor state machine (RUN, STOP, RESET, or RAMP) 00088 static int accel = 0; //holds the value of accceleration enabled from PC 00089 static float numRampSteps = 0; //calculated value that is based on the stepper speed after acceleration 00090 static float stepRate = 10; 00091 static int stepMultiplier = 1; 00092 static float next_interval_us = 1200; 00093 static float stepInterval_us = 1200; 00094 static bool acceleration_start = 0; 00095 static bool runStopChange = 0; 00096 static bool accel_wait_flag = 0; 00097 00098 //static int mode = 0; 00099 00100 void test_state_mach(void); 00101 void adv_state_mach_half(void); 00102 void adv_state_mach_full(void); 00103 void set_speed(float stepRate); 00104 void acceleration_a(void); 00105 void acceleration_b(void); 00106 void set_speed_with_ramp(void); 00107 void null_function(void); 00108 void clear_accel_wait_flag(void); 00109 00110 00111 00112 // 00113 //Create a Timeout function. Used for ramping up the speed at startup 00114 Timeout accel_ramp; 00115 00116 //Create a Ticker function. Called during normal runtime to advance the stepper motor 00117 Ticker advance_state; 00118 00119 00120 int main() 00121 { 00122 //set up storage for incoming/outgoing 00123 send_report.length = 64; 00124 recv_report.length = 64; 00125 00126 red_led = 1; // Red LED = OFF 00127 green_led = 1; // Green LED = OFF 00128 blue_led = 0; // Blue LED = ON 00129 00130 READY = 0; 00131 00132 motorState = RESET; 00133 00134 while(1) 00135 { 00136 //try to read a msg 00137 if(hid.readNB(&recv_report)) 00138 { 00139 if(initflag == true) 00140 { 00141 initflag = false; 00142 blue_led = 1; //turn off blue LED 00143 READY = 0; 00144 } 00145 switch(recv_report.data[0]) //byte 0 of recv_report.data is command 00146 { 00147 //----------------------------------------------------------------------------------------------------------------- 00148 // COMMAND PARSER 00149 //----------------------------------------------------------------------------------------------------------------- 00150 //////// 00151 case WRITE_LED: 00152 switch(recv_report.data[1]) 00153 { 00154 case LEDS_OFF: 00155 red_led = 1; 00156 green_led = 1; 00157 blue_led = 1; 00158 break; 00159 case RED: 00160 if(recv_report.data[2] == 1){red_led = 0;} else {red_led = 1;} 00161 break; 00162 case GREEN: 00163 if(recv_report.data[2] == 1){green_led = 0;} else {green_led = 1;} 00164 break; 00165 case BLUE: 00166 if(recv_report.data[2] == 1){blue_led = 0;} else {blue_led = 1;} 00167 break; 00168 default: 00169 break; 00170 }// End recv report data[1] 00171 break; 00172 ////////end case WRITE_LED 00173 00174 //////// 00175 case WRITE_STEPS_PER_SEC: 00176 00177 stepRate = recv_report.data[1]; 00178 set_speed(stepRate); 00179 break; 00180 //////// 00181 case WRITE_RUN_STOP: 00182 if(recv_report.data[1] == 1) 00183 { 00184 if(runstop != 1) 00185 { 00186 red_led = 1; 00187 blue_led = 1; 00188 green_led = 0; 00189 runstop = 1; 00190 runStopChange = 1; 00191 00192 if(accel == 1) 00193 { 00194 motorState = RAMP; 00195 next_interval_us = stepMultiplier; 00196 acceleration_start = 1; 00197 } 00198 else 00199 { 00200 motorState = RUN; 00201 set_speed(stepRate); 00202 } 00203 } 00204 00205 } 00206 else 00207 { 00208 if(runstop != 0) 00209 { 00210 runstop = 0; 00211 runStopChange = 1; 00212 motorState = STOP; 00213 red_led = 0; 00214 green_led = 1; 00215 blue_led = 1; 00216 } 00217 }//end if(recv_report.data[1] == 1) 00218 break; 00219 //////// 00220 case WRITE_DIRECTION: 00221 00222 if(recv_report.data[1] == 1) 00223 { 00224 dir = 1; 00225 00226 } 00227 else 00228 { 00229 dir = 0; 00230 00231 } 00232 break; 00233 //////// 00234 case WRITE_ACCEL: 00235 if(recv_report.data[1] == 1) 00236 { 00237 accel = 1; 00238 } 00239 else 00240 { 00241 accel = 0; 00242 } 00243 break; 00244 //////// 00245 case WRITE_STEPMODE: 00246 if(recv_report.data[1] == QTR) 00247 { 00248 stepMode = QTR; 00249 stepMultiplier = 8; 00250 00251 } 00252 else 00253 { 00254 stepMode = HALF; 00255 stepMultiplier = 4; 00256 00257 } 00258 break; 00259 //////// 00260 default: 00261 break; 00262 }// End Switch recv report data[0] 00263 00264 00265 //----------------------------------------------------------------------------------------------------------------- 00266 // end command parser 00267 //----------------------------------------------------------------------------------------------------------------- 00268 00269 send_report.data[0] = recv_report.data[0]; // Echo Command 00270 send_report.data[1] = recv_report.data[1]; // Echo Subcommand 1 00271 send_report.data[2] = recv_report.data[2]; // Echo Subcommand 2 00272 send_report.data[3] = 0x00; 00273 send_report.data[4] = 0x00; 00274 send_report.data[5] = 0x00; 00275 send_report.data[6] = 0x00; 00276 send_report.data[7] = 0x00; 00277 00278 //Send the report 00279 hid.send(&send_report); 00280 }// End If(hid.readNB(&recv_report)) 00281 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00282 //End of USB message handling 00283 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00284 00285 00286 /************************************************************************************************************************ 00287 // This is handling of Speed, Acceleration, Direction, and Start/Stop 00288 ***********************************************************************************************************************/ 00289 00290 00291 00292 00293 00294 00295 switch (motorState) 00296 { 00297 case STOP: 00298 if(runStopChange == 1) 00299 { 00300 runStopChange = 0; 00301 OE = 1; 00302 advance_state.detach(); 00303 } 00304 else 00305 { 00306 OE = 1; 00307 } 00308 break; 00309 00310 case RUN: 00311 if(runStopChange != 0) 00312 { 00313 OE = 0; 00314 runStopChange = 0; 00315 } 00316 break; 00317 00318 case RESET: 00319 OE = 1; 00320 runStopChange = false; 00321 motorState = STOP; 00322 break; 00323 00324 case RAMP: 00325 if(acceleration_start == 1) 00326 { 00327 OE = 0; 00328 acceleration_a(); 00329 acceleration_start = 0; 00330 } 00331 if(accel_wait_flag == 0) 00332 { 00333 acceleration_a(); 00334 accel_wait_flag = 1; 00335 accel_ramp.attach_us(&clear_accel_wait_flag, next_interval_us); 00336 } 00337 00338 break; 00339 00340 }// end switch motorState 00341 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00342 00343 00344 }//end while 00345 }//end main 00346 00347 00348 00349 00350 00351 00352 00353 00354 00355 00356 //state machine to advance to next step 00357 //called each time 00358 void adv_state_mach_half() 00359 { 00360 00361 READY = 1; 00362 if(OE == 0) 00363 { 00364 00365 if(dir == 1) 00366 { 00367 switch(stepState) 00368 { 00369 case STATE1: 00370 IN1A = 1; //coil A +, coil B + 00371 IN1B = 0; 00372 IN2A = 1; 00373 IN2B = 0; 00374 stepState = 2; 00375 break; 00376 00377 case STATE2: 00378 IN1A = 0; //coil A 0, coil B + 00379 IN1B = 0; 00380 IN2A = 1; 00381 IN2B = 0; 00382 stepState = 3; 00383 break; 00384 00385 case STATE3: 00386 IN1A = 0; //coil A -, coil B + 00387 IN1B = 1; 00388 IN2A = 1; 00389 IN2B = 0; 00390 stepState = 4; 00391 break; 00392 00393 case STATE4: 00394 IN1A = 0; //coil A -, coil B 0 00395 IN1B = 1; 00396 IN2A = 0; 00397 IN2B = 0; 00398 stepState = 5; 00399 break; 00400 00401 case STATE5: 00402 IN1A = 0; //coil A -, coil B - 00403 IN1B = 1; 00404 IN2A = 0; 00405 IN2B = 1; 00406 stepState = 6; 00407 break; 00408 00409 case STATE6: 00410 IN1A = 0; //coil A 0, coil B - 00411 IN1B = 0; 00412 IN2A = 0; 00413 IN2B = 1; 00414 stepState = 7; 00415 break; 00416 00417 case STATE7: 00418 IN1A = 1; //coil A +, coil B - 00419 IN1B = 0; 00420 IN2A = 0; 00421 IN2B = 1; 00422 stepState = 8; 00423 break; 00424 00425 case STATE8: 00426 IN1A = 1; //coil A +, coil B 0 00427 IN1B = 0; 00428 IN2A = 0; 00429 IN2B = 0; 00430 stepState = 1; 00431 break; 00432 00433 default: 00434 IN1A = 0; 00435 IN1B = 0; 00436 IN2A = 0; 00437 IN2B = 0; 00438 stepState = 1; 00439 break; 00440 00441 00442 } 00443 } 00444 else 00445 { 00446 switch(stepState) 00447 { 00448 case STATE1: 00449 IN1A = 1; //coil A +, coil B 0 00450 IN1B = 0; 00451 IN2A = 0; 00452 IN2B = 0; 00453 stepState = 2; 00454 break; 00455 00456 case STATE2: 00457 IN1A = 1; //coil A +, coil B - 00458 IN1B = 0; 00459 IN2A = 0; 00460 IN2B = 1; 00461 stepState = 3; 00462 break; 00463 00464 case STATE3: 00465 IN1A = 0; //coil A 0 , coil B - 00466 IN1B = 0; 00467 IN2A = 0; 00468 IN2B = 1; 00469 stepState = 4; 00470 break; 00471 00472 case STATE4: 00473 IN1A = 0; //coil A -, coil B - 00474 IN1B = 1; 00475 IN2A = 0; 00476 IN2B = 1; 00477 stepState = 5; 00478 break; 00479 00480 case STATE5: 00481 IN1A = 0; //coil A -, coil B 0 00482 IN1B = 1; 00483 IN2A = 0; 00484 IN2B = 0; 00485 stepState = 6; 00486 break; 00487 00488 case STATE6: 00489 IN1A = 0; //coil A -, coil B + 00490 IN1B = 1; 00491 IN2A = 1; 00492 IN2B = 0; 00493 stepState = 7; 00494 break; 00495 00496 case STATE7: 00497 IN1A = 0; //coil A 0, coil B + 00498 IN1B = 0; 00499 IN2A = 1; 00500 IN2B = 0; 00501 stepState = 8; 00502 break; 00503 00504 case STATE8: 00505 IN1A = 1; //coil A +, coil B + 00506 IN1B = 0; 00507 IN2A = 1; 00508 IN2B = 0; 00509 stepState = 1; 00510 break; 00511 00512 default: 00513 IN1A = 0; 00514 IN1B = 0; 00515 IN2A = 0; 00516 IN2B = 0; 00517 stepState = 1; 00518 break; 00519 00520 00521 } 00522 } 00523 } 00524 00525 } 00526 00527 00528 void adv_state_mach_full() 00529 { 00530 if(OE == 0) 00531 { 00532 if(dir == 1) 00533 { 00534 switch(stepState) 00535 { 00536 case STATE1: 00537 IN1A = 1; //coil A +, coil B + 00538 IN1B = 0; 00539 IN2A = 1; 00540 IN2B = 0; 00541 stepState = 2; 00542 break; 00543 00544 case STATE2: 00545 IN1A = 1; //coil A +, coil B - 00546 IN1B = 0; 00547 IN2A = 0; 00548 IN2B = 1; 00549 stepState = 3; 00550 break; 00551 00552 case STATE3: 00553 IN1A = 0; //coil A -, coil B - 00554 IN1B = 1; 00555 IN2A = 0; 00556 IN2B = 1; 00557 stepState = 4; 00558 break; 00559 00560 00561 case STATE4: 00562 IN1A = 0; //coil A -, coil B + 00563 IN1B = 1; 00564 IN2A = 1; 00565 IN2B = 0; 00566 stepState = 1; 00567 break; 00568 00569 00570 00571 default: 00572 IN1A = 0; 00573 IN1B = 0; 00574 IN2A = 0; 00575 IN2B = 0; 00576 stepState = 1; 00577 break; 00578 00579 00580 } 00581 } 00582 else 00583 { 00584 switch(stepState) 00585 { 00586 case STATE1: 00587 IN1A = 0; //coil A -, coil B + 00588 IN1B = 1; 00589 IN2A = 1; 00590 IN2B = 0; 00591 stepState = 2; 00592 break; 00593 00594 case STATE2: 00595 IN1A = 0; //coil A -, coil B - 00596 IN1B = 1; 00597 IN2A = 0; 00598 IN2B = 1; 00599 stepState = 3; 00600 break; 00601 00602 case STATE3: 00603 IN1A = 1; //coil A +, coil B - 00604 IN1B = 0; 00605 IN2A = 0; 00606 IN2B = 1; 00607 stepState = 4; 00608 break; 00609 00610 case STATE4: 00611 IN1A = 1; //coil A +, coil B + 00612 IN1B = 0; 00613 IN2A = 1; 00614 IN2B = 0; 00615 stepState = 1; 00616 break; 00617 00618 default: 00619 IN1A = 0; 00620 IN1B = 0; 00621 IN2A = 0; 00622 IN2B = 0; 00623 stepState = 1; 00624 break; 00625 00626 00627 } //end switch 00628 } //end else 00629 } 00630 } 00631 ///////////////////////////////////////////////////////////////////////////////// 00632 00633 00634 void clear_accel_wait_flag(void) 00635 { 00636 accel_wait_flag = 0; 00637 } 00638 00639 00640 00641 00642 void acceleration_a(void) 00643 { 00644 00645 rampCount ++; 00646 if(rampCount <= numRampSteps) 00647 { 00648 test_state_mach(); 00649 next_interval_us = 1000000 * (1/float(rampCount * stepMultiplier)); 00650 // accel_ramp.attach_us(&acceleration_b, next_interval_us);//speed); 00651 } 00652 else 00653 { 00654 rampCount = 0; 00655 motorState = RUN; 00656 advance_state.attach_us(&test_state_mach, stepInterval_us); 00657 } 00658 00659 } 00660 ///////////////////////////////////////////////////////////////////////////////// 00661 void acceleration_b(void) 00662 { 00663 READY = 1; 00664 rampCount ++; 00665 if(rampCount <= 5000) //numRampSteps) 00666 { 00667 test_state_mach(); 00668 next_interval_us = 1000000 * (1/(rampCount)); // * stepMultiplier)); 00669 accel_ramp.attach_us(&acceleration_a, next_interval_us); 00670 } 00671 else 00672 { 00673 READY = 0; 00674 rampCount = 0; 00675 motorState = RUN; 00676 advance_state.attach_us(&test_state_mach, stepInterval_us); 00677 } 00678 00679 } 00680 ///////////////////////////////////////////////////////////////////////////////// 00681 00682 void test_state_mach(void) 00683 { 00684 if(stepMode == QTR) 00685 { 00686 adv_state_mach_half(); 00687 } 00688 else 00689 { 00690 adv_state_mach_full(); 00691 } 00692 } 00693 /////////////////////////////////////////////////////////////////////////////////// 00694 00695 00696 void set_speed(float speed) 00697 { 00698 if(motorState == RUN) 00699 { 00700 if(stepMode == QTR) 00701 { 00702 numRampSteps = speed; 00703 stepInterval_us = 1000000*(1.0/(speed * 8)); 00704 advance_state.attach_us(&test_state_mach, stepInterval_us); 00705 } 00706 else 00707 { 00708 numRampSteps = speed; 00709 stepInterval_us = 1000000*(1.0/(speed * 4)); 00710 advance_state.attach_us(&test_state_mach, stepInterval_us); 00711 } 00712 } 00713 else //not in run mode, save the change 00714 { 00715 if(stepMode == QTR) 00716 { 00717 numRampSteps = speed; 00718 stepInterval_us = 1000000*(1.0/(speed * 8)); 00719 } 00720 else 00721 { 00722 stepInterval_us = 1000000*(1.0/(speed * 4)); 00723 numRampSteps = speed; 00724 } 00725 } 00726 }
Generated on Thu Jul 14 2022 05:23:17 by
1.7.2