Diff: main.cpp

Revision:

25:8be440e10126

Parent:

24:c1b1b0ea0cb9

Child:

26:cbbfe012a757

--- a/main.cpp	Sun Mar 22 14:57:21 2015 +0000
+++ b/main.cpp	Sun Mar 22 23:55:20 2015 +0000
@@ -40,7 +40,7 @@
 mode driveMode; //declaring the variable for the states
 int sensorsCheckSum; //varibale used for sensors mapping access
 int passedTime1,passedTime2;
-int values_old[5] = {0};
+int oldValues[5] = {0};
 int k = 0;
 
 
@@ -56,11 +56,11 @@
             sensorsCheckSum += (i+1)*(i+1);
         }
     }
-    if (values_old[0] != sensorsCheckSum) {
+    if (oldValues[0] != sensorsCheckSum) {
         for (k = 5; k > 0; k--) {
-            values_old[k] = values_old[k-1];
+            oldValues[k] = oldValues[k-1];
         }
-        values_old[0] = sensorsCheckSum;
+        oldValues[0] = sensorsCheckSum;
     }
     //pc.printf("sensorsCheckSum is %i",sensorsCheckSum);
 }
@@ -120,7 +120,7 @@
     wait(1); //give time to set up the system
     
     sensorTimer.start(); // start timer for sensors
-    float normalSpeed = 0.01f;
+//    float normalSpeed = 0.01f;
 //    HC06.baud(9600);
 //    HC06.printf("working..");
 //    motors.setSpeed(normalSpeed);
@@ -130,11 +130,11 @@
 //    
 //    
 //    wait(3);
-    while(1){
-        if (pc.getc() == 'r') {
-        measureSensors();
+ //   while(1){
+//        if (pc.getc() == 'r') {
+//        measureSensors();
         //measureTimer.reset();
-        printBluetooth();
+//        printBluetooth();
         //passedTime1 = measureTimer.read_ms();
         //if (sensorsCheckSum == 0) {
 //            motors.setSpeed(normalSpeed);
@@ -151,8 +151,8 @@
 //        else {
 //            motors.setSpeed(normalSpeed);
 //        }
-        }
-    }
+//        }
+//    }
 //HC06.printf("AT");
 //HC06.printf("AT+PIN5555");
     
@@ -169,39 +169,91 @@
           measureSensors();
         switch (sensorsCheckSum) {
         case 0: // all black, turn around by 180 degrees
-            goto turnAround; 
+                for (k=0;k<6;k++) {        //right turn situation
+                if (oldValues[k] == 194) {
+                motors.stop();
+                motors.setSteeringMode(NORMAL);
+                motors.reverse();
+                motors.setSpeed(0.1f);
+                motors.start();
+                wait(1);
+            } else {            
+                motors.stop();
+                motors.setSteeringMode(NORMAL);
+                motors.reverse();
+                motors.setSpeed(0.1f);
+                motors.start();
+                wait(2);
+                motors.stop();
+                motors.setSteeringMode(PIVOT_CCW);
+                motors.setSpeed(0.1f);
+                do
+                {
+                motors.start();
+                measureSensors();
+                } while (sensorsCheckSum != 96);
+                motors.stop();
+                motors.setSteeringMode(NORMAL);
+            } 
             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 46: //left 5 white, right only 3 black >> turn right
+//            motors.setRightSpeed(0.15f);
+//            motors.setLeftSpeed(0.1f);
+//            break;
         case 94: //normal starting position, half of right and half of left are white, (move right wheel)
+            motors.setSteeringMode(NORMAL);
             motors.setSpeed(0.1f);
-            motors.forward;
-            motors.start;
+            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
-        
+        case 104: //right all white, left half white >> turn right
+            motors.setRightSpeed(0.1f);
+            motors.setLeftSpeed(0.15f);
             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)
-        
-       
+            motors.setRightSpeed(0.05f);
+            motors.setLeftSpeed(0.15f);
             break;
-        case 194 : //left all white, right half white >> go straight, turn right if 194 goes to 204
-        
+        case 194 : //left all white, right half white >> go straight, turn right if 194 goes to 204        
             break;
         case 204 : //all sensors are white
+                       
+        for (k=0;k<6;k++) {                 //situation when a square is entered, need to follow right line
+            if (oldValues[k] == 194) {      //checks whether  black line on the right was present before
+            motors.setRightSpeed(0.15f);
+            motors.setLeftSpeed(0.05f);
+            }
+        
+            if (oldValues[k] == 104) {          //right turn 90 situation
+            motors.stop();
+            motors.setSteeringMode(PIVOT_CW);
+            motors.setRightSpeed(0.1f);
+            motors.setLeftSpeed(0.1f);
+                do
+                {
+                motors.start();
+                measureSensors();
+                } while (sensorsCheckSum != 94);
+                motors.stop();
+                motors.setSteeringMode(NORMAL);
+        }            
+        break;              
+            
         default: //checksum is zero , all are black
             measureSensors(); 
             break;
         }
+    }
+}
+}
+}
+}
 
 //        if (testOtherCases == 1) {
 //            if (sensorsCheckSum < 96){ // adjust right
@@ -209,8 +261,7 @@
 //            else   {//adjust left
 //                }
 //            testOtherCases = 0;
-//        }    
-          
+//        }           
 //              
 //        
 //           
@@ -223,27 +274,3 @@
 //       
 //    }
 //    
-        
-}
-
-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;
-}
-    
-
-
-
-
-
-