scribe robot working with stepper motors

Dependencies:   BLE_nRF8001 BNO055 HC_SR04_Ultrasonic_Library localization mbed-rtos mbed

Revision:
4:1da4d4050306
Parent:
2:63aef644e6d2
Child:
5:2c196f871096
diff -r 63aef644e6d2 -r 1da4d4050306 stepper.cpp
--- a/stepper.cpp	Sat Apr 23 02:46:30 2016 +0000
+++ b/stepper.cpp	Mon Apr 25 05:38:48 2016 +0000
@@ -11,60 +11,75 @@
 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(){
-    //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);
-
+    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;
 }
 
@@ -251,3 +266,54 @@
     }
     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;
+}