Diff: main.cpp

Revision:

21:e8da3b351cd0

Parent:

20:198dc13777eb

Child:

22:9cf274ffe1de

--- a/main.cpp	Fri Mar 20 11:40:57 2015 +0000
+++ b/main.cpp	Fri Mar 20 14:56:13 2015 +0000
@@ -59,8 +59,8 @@
 
 void printBluetooth() { //for debugging
     
-    pc.printf("LLU%i  LRU%i               rlu%i rru%i\n",sensorArray[1]->state,sensorArray[0]->state,sensorArray[1]->state,sensorArray[0]->state);
-    pc.printf("LLD%i  LRD%i               rld%i rrd%i\n\n",sensorArray[3]->state,sensorArray[2]->state,sensorArray[3]->state,sensorArray[2]->state);
+    pc.printf("LLU%i  LRU%i               rlu%i rru%i\n",sensorArray[7]->state,sensorArray[6]->state,sensorArray[1]->state,sensorArray[0]->state);
+    pc.printf("LLD%i  LRD%i               rld%i rrd%i\n\n",sensorArray[5]->state,sensorArray[4]->state,sensorArray[3]->state,sensorArray[2]->state);
     //HC06.printf("%i  %i               %i %i",sensorArray[NUMBER_SENSORS_REGULAR-3]->state,sensorArray[NUMBER_SENSORS_REGULAR-4]->state,sensorArray[3]->state,sensorArray[2]->state);
     //HC06.printf("%i      %i/n%i      %i,sensorArray[NUMBER_SENSORS_REGULAR]->state,sensorArray[NUMBER_SENSORS_REGULAR+1]->state,sensorArray[NUMBER_SENSORS_REGULAR+2]->state,sensorArray[NUMBER_SENSORS_REGULAR+3]->state);        
     //HC06.printf("%f                      %f",motor.getLeftSpeed(),motor.getRightSpeed());
@@ -149,10 +149,8 @@
         }
         
     }
-    
-   
-    //HC06.printf("AT");
-    //HC06.printf("AT+PIN5555");
+//HC06.printf("AT");
+//HC06.printf("AT+PIN5555");
     
 
    // pc.printf("Start...");
@@ -163,26 +161,28 @@
        if (driveMode == REGULAR) {
           measureSensors();
           switch (sensorsCheckSum) {
-            case 94: //go straight
-                break;
-            case 95: //Turn right seriously
-                break;
-            case 104: //Turn right as well
-                break;
-            
-            case 158: //Turn left seriously
-                pc.printf ("only Right is white \n");
-                break;
-            case 194 :   //Turn left seriously again
-                pc.printf ("only Left is white \n");
-                break;
-            case 103 :  //RRD is white all else normal
-                pc.printf ("both are white \n");
-                break;
-            
-//          case 204: 
-//            driveMode = SQUARE; //if all sensors are white you're in the square
-//            break;
+        case 0: // all black, turn around by 180 degrees
+            motors.setSteeringMode(PIVOT_CCW);
+            motors.setLeftSpeed(0.2f);
+            motors.setRightSpeed(0.1f); 
+            break;
+        case 30: //all right are white, left all black >> turn right(move left wheel)
+            break;
+        case 46: //left 5 white, right only 3 black >> turn right
+            break;
+        case 94: //normal startting position, half of right and half of left are white, (move right wheel)
+            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; 
             break;