Diff: main.cpp

Revision:

24:c1b1b0ea0cb9

Parent:

23:902c3086394e

Child:

25:8be440e10126

--- a/main.cpp	Sat Mar 21 13:08:53 2015 +0000
+++ b/main.cpp	Sun Mar 22 14:57:21 2015 +0000
@@ -158,47 +158,59 @@
     
 
    // pc.printf("Start...");
-    /*
-    int testOtherCases = 0; //needed for control statements
+    
+//    int testOtherCases = 0; //needed for control statements
+
+
+
     while (1) {
        
        if (driveMode == REGULAR) {
           measureSensors();
-          switch (sensorsCheckSum) {
+        switch (sensorsCheckSum) {
         case 0: // all black, turn around by 180 degrees
-            motors.setSteeringMode(PIVOT_CCW);
-            motors.setLeftSpeed(0.2f);
-            motors.setRightSpeed(0.1f); 
+            goto turnAround; 
             break;
         case 30: //all right are white, left all black >> turn right(move left wheel)
+            motors.setRightSpeed(0.15f);
+            motors.setLeftSpeed(0.5f);
             break;
         case 46: //left 5 white, right only 3 black >> turn right
+            motors.setRightSpeed(0.15f);
+            motors.setLeftSpeed(0.5f);
             break;
-        case 94: //normal startting position, half of right and half of left are white, (move right wheel)
+        case 94: //normal starting position, half of right and half of left are white, (move right wheel)
+            motors.setSpeed(0.1f);
+            motors.forward;
+            motors.start;
             break;
         case 104: //right all white, left half white >> turn right 90 degrees
+        
             break;
         case 154: //right 4 white, left only 6 black >> turn left
+        
             break;
         case 174: //left all white, right all black >> turn left (move right wheel)
-        pc.printf ("only Right is white \n");
+        
+       
             break;
         case 194 : //left all white, right half white >> go straight, turn right if 194 goes to 204
-        pc.printf ("only Left is white \n");
+        
             break;
         case 204 : //all sensors are white
-          default: //checksum is zero , all are black
-            testOtherCases = 1; 
+        default: //checksum is zero , all are black
+            measureSensors(); 
             break;
-          } 
-          if (testOtherCases == 1) {
-            if (sensorsCheckSum < 96){ // adjust right
-                }   
-            else   {//adjust left
-                }
-            testOtherCases = 0;
-          }    
-          */
+        }
+
+//        if (testOtherCases == 1) {
+//            if (sensorsCheckSum < 96){ // adjust right
+//                }   
+//            else   {//adjust left
+//                }
+//            testOtherCases = 0;
+//        }    
+          
 //              
 //        
 //           
@@ -214,8 +226,24 @@
         
 }
 
+turnAround:
+    motors.reverse;
+    motors.setSpeed(0.1f);
+    motors.start;
+    wait(2);
+    motors.stop;
+    motors.setSteeringMode(PIVOT_CCW);
+    motors.setSpeed(0.1f);
+    do
+    {
+        motors.start;
+    } while (sensorsCheckSum != 96);
+    return 0;
+}
+    
 
 
 
 
 
+