scribe robot working with stepper motors
Dependencies: BLE_nRF8001 BNO055 HC_SR04_Ultrasonic_Library localization mbed-rtos mbed
stepper.cpp
- Committer:
- manz
- Date:
- 2016-05-04
- Revision:
- 5:2c196f871096
- Parent:
- 4:1da4d4050306
- Child:
- 6:ca112c3083bb
File content as of revision 5:2c196f871096:
// stepper.cpp of SCRIBE stepper -> updated for pololu motors #include "mbed.h"; DigitalOut yellow_l(p5); DigitalOut orange_l(p6); DigitalOut brown_l(p7); DigitalOut black_l(p8); 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_left(){ 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_right(){ 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; }