Diff: main.cpp

Revision:

27:fc0fd2c0eebb

Parent:

25:bec5dc4c9f5e

Child:

28:d1e7daeb240e

--- a/main.cpp	Mon Mar 23 14:03:29 2015 +0000
+++ b/main.cpp	Mon Mar 23 14:07:35 2015 +0000
@@ -29,7 +29,7 @@
 int sensorsCheckSum; //varibale used for sensors mapping access
 int passedTime1,passedTime2;
 int oldValues[5] = {0};
-int cursor = 0;
+int cursor = 0, k = 0;
 
 
 void measureSensors () {
@@ -73,7 +73,7 @@
     wait(1); //give time to set up the system
     
     sensorTimer.start(); // start timer for sensors
-    float normalSpeed = 0.01f;
+//    float normalSpeed = 0.01f;
 /*
     while(1){
         if (pc.getc() == 'r') {
@@ -98,109 +98,135 @@
 //        }
     }
 */
-    int testOtherCases = 0; //needed for control statements
+//    int testOtherCases = 0; //needed for control statements
     while (1) {
        
         if (driveMode == REGULAR) {
-            measureSensors(); 
-            switch (sensorsCheckSum) {
+                measureSensors();
+                switch (sensorsCheckSum) {
                 case 0: // all black, turn around by 180 degrees or a possible turn on the left
-                        for (int k=0;k<5;k++) {        //left turn situation
-                            if (oldValues[k] == 194) {
-                                motors.stop();
-                                motors.setSteeringMode(NORMAL);
-                                motors.reverse();
-                                motors.setSpeed(0.1f);
-                                 motors.start();
-                                wait(1);
-                                motors.stop();
-                            } 
-                            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.setSteeringMode(NORMAL);
-                    motors.forward();
-                    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
-                    motors.setSteeringMode(NORMAL);
-                    motors.forward();
-                    motors.setSpeed(0.1f);
-                    motors.start();
-                    break;
-                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        
-                    break;
-                case 204 : //all sensors are white
-                               
-                    for (int k=0;k<5;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);
+                    for (k=0;k<5;k++) {                             //left turn situation >> stop, go backwards
+                        if (oldValues[k] == 194) {
+                            motors.stop();
+                            motors.setSteeringMode(NORMAL);
+                            motors.reverse();
+                            motors.setSpeed(0.1f);
+                            motors.start();
+                            wait(2);
+                            motors.stop();
+                        } else {                                    //dead end situation >> stop, go a bit backwards so there is space for turning and turn CCW
+                            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 != 94);
+                            } while (sensorsCheckSum != 96);
                             motors.stop();
-                            motors.setSteeringMode(NORMAL);
-                    }            
-                break;              
-                default: //checksum is zero , all are black    
-                testOtherCases = 1;
-                break;
+                        }
+                    } 
+            break;
+          case 30:                                                  //all right are white, left all black >> turn right(move left wheel faster)
+            motors.setSteeringMode(NORMAL);
+            motors.forward();
+            motors.setRightSpeed(0.1f);
+            motors.setLeftSpeed(0.05f);
+            do
+            {   
+            motors.start();
+            measureSensors();
+            } while (sensorsCheckSum == 30);
+            motors.stop();
+            break;
+
+        case 94: //normal <<STARTING POSITION>>, half of right and half of left are white
+            motors.setSteeringMode(NORMAL);
+            motors.forward();
+            motors.setSpeed(0.1f);
+            do
+            {   
+            motors.start();
+            measureSensors();
+            } while (sensorsCheckSum == 94);
+            motors.start();
+            break;
+        case 104: //right all white, left half white >> turn right
+            motors.setSteeringMode(NORMAL);
+            motors.forward();
+            motors.setSpeed(0.1f);
+            motors.start();
+            wait(1);
+            motors.stop();
+            motors.setRightSpeed(0.2f);
+            motors.setLeftSpeed(0.0f);
+            do
+            {   
+            motors.start();
+            measureSensors();
+            } while (sensorsCheckSum <= 94);
+            motors.stop();
+            break;
+        case 174: //left all white, right all black >> turn left (move right wheel)
+            motors.setSteeringMode(NORMAL);
+            motors.forward();
+            motors.setRightSpeed(0.05f);
+            motors.setLeftSpeed(0.1f);
+            do
+            {   
+            motors.start();
+            measureSensors();
+            } while (sensorsCheckSum == 174);
+            break;
+        case 194 : //left all white, right half white >> go straight, turn right if 194 goes to 204
+            motors.setSteeringMode(NORMAL);
+            motors.forward();
+            motors.setRightSpeed(0.05f);
+            motors.setLeftSpeed(0.1f);
+            do
+            {   
+            motors.start();
+            measureSensors();
+            } while (sensorsCheckSum == 194);
+            motors.stop();       //do while left is white
+            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.setSteeringMode(NORMAL);
+                    motors.setRightSpeed(0.05f);
+                    motors.setLeftSpeed(0.1f);
+                    do
+                    {   
+                    motors.start();
+                    measureSensors();
+                    } while (sensorsCheckSum == 204);
+                    motors.stop();
                 }
-            
-            if (testOtherCases == 1) {
-                if (sensorsCheckSum < 96){ // adjust right
-                    }   
-                else   {//adjust left
-                    }
-                testOtherCases = 0;
-            }   
-            
-        }
-        else {// if driveMode == SQUARE)
+                if (oldValues[k] == 104) {          //right turn 90 situation
+                    motors.setSteeringMode(PIVOT_CW);
+                    motors.setRightSpeed(0.1f);
+                    motors.setLeftSpeed(0.1f);
+                    do
+                    {
+                    motors.start();
+                    measureSensors();
+                    } while (sensorsCheckSum != 94);
+                    motors.stop();
+                }            
+        break;              
+        default:
+            measureSensors(); 
+            break;
         }
     }
 }
+}
+}