The servo version of SCRIBE
Dependencies: BLE_nRF8001 BNO055 HC_SR04_Ultrasonic_Library mbed-rtos mbed
Fork of SCRIBE_stepper by
stepper.cpp
- Committer:
- manz
- Date:
- 2016-04-25
- Revision:
- 5:1da4d4050306
- Parent:
- 3:63aef644e6d2
File content as of revision 5:1da4d4050306:
#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_stepf = 0; int case_right = 0; int case_left = 0; int case_rone = 0; //StepperMotorUni motor( p17, p18, p19, p20); int step_f(){ switch(case_stepf){ case 0: //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); case_stepf=1; break; case 1: //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); case_stepf=2; break; case 2: //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); case_stepf = 3; break; case 3: //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); case_stepf = 0; break; } 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; } int step_rone(){ switch(case_rone){ case 0: //step 0101 black_r = 0; orange_r = 0; brown_r = 1; yellow_r = 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; wait_ms(wait_t); case_right = 2; break; case 2: //step 1010 black_r = 1; orange_r = 1; brown_r = 0; yellow_r = 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; wait_ms(wait_t); case_right = 0; break; } return 1; }