Diff: main.cpp

Revision:

37:c0fddc75c862

Parent:

35:1819c5a8254a

Child:

38:02ef89edd828

--- a/main.cpp	Tue Mar 24 19:51:28 2015 +0000
+++ b/main.cpp	Tue Mar 24 22:34:52 2015 +0000
@@ -1,8 +1,6 @@
 /*
 ****** MAIN PROGRAM ******
 
-Please consider that although it is an embedded envrionment we are NOT creating a HARD-TIME real time system - delays can be dealt with
-
 Sensors are mapped on the global variable sensorsCheckSum,
 which multiplies the sensor number by itself to then decode,
  which sensors are off and which are on
@@ -20,52 +18,222 @@
 
 DigitalOut led(LED_RED);
 Serial HC06(PTE0,PTE1);
+Motor_Driver motors(PTA5, PTC9, PTC8,PTD4, PTA12, PTA4, PWM_PERIOD_US); //motors object
+
+Timer measureTimer; //Timer used for measurements
+
+typedef enum mode {REGULAR,SQUARE} mode;
+mode driveMode; //enumeration for different states
+typedef enum flag {USE, SKIP} flag;
+flag oldValuesFlag;
+
+int sensorsCheckSum; //variable used for sensors mapping access
+int passedTime1,passedTime2;
+int oldValues[5] = {0};
+int cursor = 0, k = 0;
+float normalSpeed = 0.15f;
 
 void turnLeft ();
 void turnRight ();
 void measureSensors ();
 void printBluetooth();
 
-
-Timer measureTimer; //Timer used for measurement
-Motor_Driver motors(PTA5, PTC9, PTC8,PTD4, PTA12, PTA4, PWM_PERIOD_US); //motors object
+int main() {
+    
+    led = 0;        //start LED with beginning of main program    
+    measureTimer.start();
+    sensorTimer.start();        // start timer for sensors
+    sensor_initialise();
+    driveMode = REGULAR;        // initialise sensor values
+    oldValuesFlag = USE;
+    wait(1);        //give time to set up the system
 
-typedef enum  mode {REGULAR,SQUARE} mode; //enumeration for different states
-mode driveMode; //declaring the variable for the states
-int sensorsCheckSum; //varibale used for sensors mapping access
-int passedTime1,passedTime2;
-int oldValues[5] = {0};
-int cursor = 0, k = 0;
-float normalSpeed = 0.15f;
-
-
-int main() {
-        
-   //setup_counter(1000, 0);
-   // float frequency = measure_frequency(CHANNEL_1);
-    led = 0;//start LED with beginning of main program
+    motors.setSteeringMode(NORMAL);
+    motors.disableHardBraking();
+    motors.forward();
+    motors.setSpeed (0.07f);
+    motors.start(); 
+    //motors.enableSlew();
     
-    measureTimer.start();
-    driveMode = REGULAR; //initialise drivemoder
-    sensor_initialise(); // initialise sensor values
-    wait(1); //give time to set up the system
-    
-    sensorTimer.start(); // start timer for sensors
-    
-    motors.setSteeringMode (NORMAL);
-    motors.disableHardBraking();
-    motors.forward ();
-    
-    motors.start (); 
+    while (1) {
+        measureSensors();
+        oldValuesFlag = USE;                
+        switch (sensorsCheckSum) {
+        case 0:     //all black >> turn around by 180 degrees or a possible turn on the left
+                for (k=0;k<5;k++) {                             //left turn situation >> stop, go backwards
+                    if (oldValues[k] == 194) {
+                        motors.stop();
+                        motors.setSteeringMode(NORMAL);
+                        motors.reverse();
+                        motors.setRightSpeed(0.1f);
+                        motors.setLeftSpeed(0.2f);
+                        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.setRightSpeed(0.1f);
+                        motors.setLeftSpeed(0.2f);
+                        motors.start();
+                        wait(2);
+                        motors.stop();
+                        motors.setSteeringMode(PIVOT_CCW);
+                        motors.setRightSpeed(0.1f);
+                        motors.setLeftSpeed(0.2f);
+                        do
+                        {   
+                        motors.start();
+                        measureSensors();
+                        } while (sensorsCheckSum != 96);
+                        motors.stop();
+                    }
+                } 
+            break;
+        case 30:    //all right sensors are white, left all black >> turn right
+            motors.setSteeringMode(NORMAL);
+            motors.forward();
+            motors.setRightSpeed(0.02f);
+            motors.setLeftSpeed(0.07f);
+            do
+            {   
+            motors.start();
+            measureSensors();
+            } while (sensorsCheckSum == 30);
+            motors.stop();
+            break;
+        case 46:    //robot is facing north-west >> turn left
+            motors.setSteeringMode(NORMAL);
+            motors.forward();
+            motors.setRightSpeed(0.03f);
+            motors.setLeftSpeed(0.07f);
+            do
+            {   
+            motors.start();
+            measureSensors();
+            } while (sensorsCheckSum == 46);
+            motors.stop();
+            break;            
+        case 94:    //normal <<STARTING POSITION>>, half of right and half of left are white
+            motors.setSteeringMode(NORMAL);
+            motors.forward();
+            motors.setRightSpeed(0.02f);
+            motors.setLeftSpeed(0.06f);
+            do
+            {   
+            motors.start();
+            measureSensors();
+            } while (sensorsCheckSum == 94);
+            motors.stop();
+            break;
+//        case 95:        //right turn is present always followed by case 104           
+        case 104:   //right all white, left half white >> crossroads, make a right turn
+            motors.stop();
+            wait(0.5);
+            motors.setSteeringMode(NORMAL);
+            motors.forward();
+            motors.setRightSpeed(0.01f);
+            motors.setLeftSpeed(0.09f);
+//            motors.start();
+//            wait(0.5);
+//            motors.stop();
+//            motors.setSteeringMode(PIVOT_CCW);
+//            motors.setRightSpeed(0.07f);
+//            motors.setLeftSpeed(0.07f);
+            do
+            {   
+            motors.start();
+            measureSensors();
+            } while (measure(sensorArray[0]) == 0);
+            motors.stop();
+            break;
+        case 154:   //robot is facing north-east >> turn right
+            motors.setSteeringMode(NORMAL);
+            motors.forward();
+            motors.setRightSpeed(0.07f);
+            motors.setLeftSpeed(0.03f);
+            do
+            {   
+            motors.start();
+            measureSensors();
+            } while (sensorsCheckSum == 154);
+            motors.stop();
+            break;            
+        case 174:   //left all white, right all black >> turn left (move right wheel)
+            motors.setSteeringMode(NORMAL);
+            motors.forward();
+            motors.setRightSpeed(0.07f);
+            motors.setLeftSpeed(0.03f);
+            do
+            {   
+            motors.start();
+            measureSensors();
+            } while (sensorsCheckSum == 174);
+            motors.stop();
+            break;
+//        case 179: //right turn, followed by case 204
+        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.04f);
+            motors.setLeftSpeed(0.07f);
+            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.01f);
+                    motors.setLeftSpeed(0.05f);
+                    do
+                    {   
+                    motors.start();
+                    measureSensors();
+                    } while (sensorsCheckSum == 204);
+                    motors.stop();
+                }
+                if ((oldValues[k] == 30) or (oldValues[k] == 104))  {          //right turn 90 situation
+                    motors.stop();
+                    wait(0.5);
+                    motors.setSteeringMode(NORMAL);
+                    motors.forward();
+                    motors.setRightSpeed(0.01f);
+                    motors.setLeftSpeed(0.09f);
+//                  motors.start();
+//                  wait(0.5);
+//                  motors.stop();
+//                  motors.setSteeringMode(PIVOT_CCW);
+//                  motors.setRightSpeed(0.07f);
+//                  motors.setLeftSpeed(0.07f);
+                    do
+                    {   
+                    motors.start();
+                    measureSensors();
+                    } while (measure(sensorArray[0]) == 0);
+                    motors.stop();
+                    break;
+                    }    
+                }        
+            break;              
+        default:
+            motors.start();
+            oldValuesFlag = SKIP;
+            break;
+        }
+    }   // while() statement
+  
+}   //while loop
+
+/*    
     motors.setSpeed (normalSpeed);
-    
-    //motors.enableSlew();
-
-    // HC06.printf("heello");
-    int testOtherCases = 0;
     while(1){    
        measureSensors();
-      //printBluetooth();
 //        if (HC06.getc() == 'r') {
 //            measureSensors();
 //            printBluetooth();
@@ -103,16 +271,6 @@
     }
 } //int main
 
-void printBluetooth() { //for debugging
-    HC06.printf("LLU%i  LRU%i               rlu%i rru%i\n",sensorArray[7]->state,sensorArray[6]->state,sensorArray[1]->state,sensorArray[0]->state);
-    HC06.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());
-    HC06.printf("sensorCheckSum%i\n\n",sensorsCheckSum);
-    //HC06.printf("passedTime1 %i passedTime2 \n\n",passedTime1,passedTime2);
-}
-
 void turnRight () {
     motors.setSpeed (normalSpeed);
     wait(0.7);
@@ -133,9 +291,17 @@
         }
         exit = 0;
     }
+} */
 
+void printBluetooth() { //for debugging
+    HC06.printf("LLU%i  LRU%i               rlu%i rru%i\n",sensorArray[7]->state,sensorArray[6]->state,sensorArray[1]->state,sensorArray[0]->state);
+    HC06.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("%f                      %f",motor.getLeftSpeed(),motor.getRightSpeed());
+    HC06.printf("sensorCheckSum%i\n\n",sensorsCheckSum);
+    //HC06.printf("passedTime1 %i passedTime2 \n\n",passedTime1,passedTime2);
 }
 
+
 void measureSensors () {
      sensorsCheckSum = 0; //zero it when first going into the routine
      int iterationNumber = NUMBER_SENSORS_REGULAR;
@@ -143,14 +309,27 @@
           iterationNumber += NUMBER_SENSORS_SQUARE;
      }
      for (int i = 0; i < iterationNumber;i++){
-         //pc.printf("%i  iteration%i ",i,iterationNumber);
          if (measure(sensorArray[i]) == 1) {//if sensor is white 
             sensorsCheckSum += (i+1)*(i+1);
         }
      }
-     if (oldValues[cursor] != sensorsCheckSum) { //why only if different ??
+/*     if ((oldValues[cursor] != sensorsCheckSum) and (oldValuesFlag == USE)) {
         oldValues[cursor] = sensorsCheckSum;
         cursor = (cursor+1)%5;
+     }*/
+     if ((oldValues[cursor] != sensorsCheckSum) and (oldValuesFlag == USE)) {       // priority queu, ideas to make it faster/shorter?
+        int i = 0;
+        while ((oldValues[cursor] != sensorsCheckSum)) {
+            for (k = 0; k < 5; k++) {
+                if (oldValues[k] == sensorsCheckSum) {
+                    for (i = k; i > 0; i--) {                           
+                       oldValues[i] = oldValues[i-1];
+                   }
+                } else {
+                    oldValues[k] = oldValues[k-1];
+                }
+            oldValues[0] = sensorsCheckSum;    
+            }
+        }
      }
-     //pc.printf("sensorsCheckSum is %i",sensorsCheckSum);
-}
+}  //measureSensors end