SCRIBE
The servo version of SCRIBE
Dependencies: BLE_nRF8001 BNO055 HC_SR04_Ultrasonic_Library mbed-rtos mbed
Fork of
SCRIBE_stepper
by 
SCRIBE
Diff: stepper.cpp
- Revision:
- 7:1bb3b5b66fe8
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/stepper.cpp Wed May 04 23:58:00 2016 +0000
@@ -0,0 +1,320 @@
+// 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;
+}