NXP
/
LVHB Stepper Motor Drive
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.
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