![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
scribe robot working with stepper motors
Dependencies: BLE_nRF8001 BNO055 HC_SR04_Ultrasonic_Library localization mbed-rtos mbed
stepper.cpp
- Committer:
- manz
- Date:
- 2016-04-23
- Revision:
- 2:63aef644e6d2
- Parent:
- 0:83acd45a2405
- Child:
- 4:1da4d4050306
File content as of revision 2:63aef644e6d2:
#include "mbed.h"; DigitalOut yellow_l(p9); DigitalOut orange_l(p14); DigitalOut brown_l(p15); DigitalOut black_l(p16); DigitalOut yellow_r(p17); DigitalOut orange_r(p18); DigitalOut brown_r(p19); DigitalOut black_r(p20); int wait_t = 60; int case_right = 0; int case_left = 0; //StepperMotorUni motor( p17, p18, p19, p20); int step_f(){ //step 0101 black_l = 0; orange_l = 0; brown_l = 1; yellow_l = 1; black_r = 1; orange_r = 0; brown_r = 0; yellow_r = 1; wait_ms(wait_t); //step 1100 black_l = 0; orange_l = 1; brown_l = 1; yellow_l = 0; black_r = 1; orange_r = 1; brown_r = 0; yellow_r = 0; wait_ms(wait_t); //step 1010 black_l = 1; orange_l = 1; brown_l = 0; yellow_l = 0; black_r = 0; orange_r = 1; brown_r = 1; yellow_r = 0; wait_ms(wait_t); //step 0011 black_l = 1; orange_l = 0; brown_l = 0; yellow_l = 1; black_r = 0; orange_r = 0; brown_r = 1; yellow_r = 1; wait_ms(wait_t); return 1; } int step_b(){ //step 0101 black_r = 0; orange_r = 0; brown_r = 1; yellow_r = 1; black_l = 1; orange_l = 0; brown_l = 0; yellow_l = 1; wait_ms(wait_t); //step 1100 black_r = 0; orange_r = 1; brown_r = 1; yellow_r = 0; black_l = 1; orange_l = 1; brown_l = 0; yellow_l = 0; wait_ms(wait_t); //step 1010 black_r = 1; orange_r = 1; brown_r = 0; yellow_r = 0; black_l = 0; orange_l = 1; brown_l = 1; yellow_l = 0; wait_ms(wait_t); //step 0011 black_r = 1; orange_r = 0; brown_r = 0; yellow_r = 1; black_l = 0; orange_l = 0; brown_l = 1; yellow_l = 1; wait_ms(wait_t); return 1; } int step_right(){ switch(case_right){ case 0: //step 0101 black_r = 0; orange_r = 0; brown_r = 1; yellow_r = 1; black_l = 0; orange_l = 0; brown_l = 1; yellow_l = 1; wait_ms(wait_t); case_right = 1; break; case 1: //step 1100 black_r = 0; orange_r = 1; brown_r = 1; yellow_r = 0; black_l = 0; orange_l = 1; brown_l = 1; yellow_l = 0; wait_ms(wait_t); case_right = 2; break; case 2: //step 1010 black_r = 1; orange_r = 1; brown_r = 0; yellow_r = 0; black_l = 1; orange_l = 1; brown_l = 0; yellow_l = 0; wait_ms(wait_t); case_right = 3; break; case 3: //step 0011 black_r = 1; orange_r = 0; brown_r = 0; yellow_r = 1; black_l = 1; orange_l = 0; brown_l = 0; yellow_l = 1; wait_ms(wait_t); case_right = 0; break; } return 1; } int step_left(){ switch(case_left){ case 0: //step 0011 black_l = 1; orange_l = 0; brown_l = 0; yellow_l = 1; black_r = 1; orange_r = 0; brown_r = 0; yellow_r = 1; wait_ms(wait_t); case_left = 1; break; case 1: black_l = 1; orange_l = 1; brown_l = 0; yellow_l = 0; black_r = 1; orange_r = 1; brown_r = 0; yellow_r = 0; wait_ms(wait_t); case_left = 2; break; case 2: black_l = 0; orange_l = 1; brown_l = 1; yellow_l = 0; black_r = 0; orange_r = 1; brown_r = 1; yellow_r = 0; wait_ms(wait_t); case_left = 3; break; case 3: black_l = 0; orange_l = 0; brown_l = 1; yellow_l = 1; black_r = 0; orange_r = 0; brown_r = 1; yellow_r = 1; wait_ms(wait_t); case_left = 0; break; } return 1; }