Diff: main.cpp

Revision:

24:ecc3fbaf2824

Parent:

23:9b53c72fcd38

Child:

25:bec5dc4c9f5e

Child:

26:7b67bcbddde3

diff -r 9b53c72fcd38 -r ecc3fbaf2824 main.cpp
--- a/main.cpp	Sun Mar 22 23:32:36 2015 +0000
+++ b/main.cpp	Mon Mar 23 12:38:53 2015 +0000
@@ -1,13 +1,8 @@
-// TESTING REPO COMMIT
-
 /*
 ****** 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
@@ -16,21 +11,14 @@
     ...
 */
 
-
-
 #include "mbed.h"
 #include "sensor_measure.h"
 #include "Motor_Driver.h"
 #include "shooter.h"
-//#include "Sensors.h"
 
 #define PWM_PERIOD_US 10000
 
-
-//DigitalOut led(LED1);
-
-Serial HC06(PTE0,PTE1); //TX,RX
-//Serial pc(USBTX, USBRX);
+Serial HC06(PTE0,PTE1);
 
 Timer measureTimer; //Timer used for measurement
 
@@ -45,22 +33,22 @@
 
 
 void measureSensors () {
-    sensorsCheckSum = 0; //zero it when first going into the routine
-    int iterationNumber = NUMBER_SENSORS_REGULAR;
-    if (driveMode == SQUARE) {
+     sensorsCheckSum = 0; //zero it when first going into the routine
+     int iterationNumber = NUMBER_SENSORS_REGULAR;
+     if (driveMode == SQUARE) {
           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 
+     }
+     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 (values_old[cursor] != sensorsCheckSum) { //why only if different ??
+     }
+     if (values_old[cursor] != sensorsCheckSum) { //why only if different ??
         values_old[cursor] = sensorsCheckSum;
         cursor = (cursor+1)%5;
-    }
-    //pc.printf("sensorsCheckSum is %i",sensorsCheckSum);
+     }
+     //pc.printf("sensorsCheckSum is %i",sensorsCheckSum);
 }
 
 void printBluetooth() { //for debugging
@@ -107,9 +95,6 @@
 //    wait(1);
 //    motors.reverse();
     
-   
-//motors.start();
-    
    // setup_counter(1000, 0);
    // float frequency = measure_frequency(CHANNEL_1);
     measureTimer.start();
@@ -119,15 +104,7 @@
     
     sensorTimer.start(); // start timer for sensors
     float normalSpeed = 0.01f;
-//    HC06.baud(9600);
-//    HC06.printf("working..");
-//    motors.setSpeed(normalSpeed);
-//    motors.forward();
-//    motors.start();
-//    
-//    
-//    
-//    wait(3);
+
     while(1){
         if (pc.getc() == 'r') {
         measureSensors();
@@ -149,46 +126,107 @@
 //        else {
 //            motors.setSpeed(normalSpeed);
 //        }
-        }
-    }
-//HC06.printf("AT");
-//HC06.printf("AT+PIN5555");
-    
 
-   // pc.printf("Start...");
-    /*
     int testOtherCases = 0; //needed for control statements
     while (1) {
        
-       if (driveMode == REGULAR) {
+              if (driveMode == REGULAR) {
           measureSensors();
           switch (sensorsCheckSum) {
-        case 0: // all black, turn around by 180 degrees
-            motors.setSteeringMode(PIVOT_CCW);
-            motors.setLeftSpeed(0.2f);
-            motors.setRightSpeed(0.1f); 
+         case 0: // all black, turn around by 180 degrees or a possible turn on the left
+                for (k=0;k<6;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)
-            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
+            motors.setSteeringMode(NORMAL);
+            motors.forward();
+            motors.setRightSpeed(0.15f);
+            motors.setLeftSpeed(0.5f);
             break;
-        case 154: //right 4 white, left only 6 black >> turn left
+//        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)
-        pc.printf ("only Right is white \n");
+            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
-        pc.printf ("only Left is white \n");
+        case 194 : //left all white, right half white >> go straight, turn right if 194 goes to 204        
             break;
         case 204 : //all sensors are white
-          default: //checksum is zero , all are black
-            testOtherCases = 1; 
+                       
+        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
+//            } else {//adjust left
+//            }
+//            testOtherCases = 0;
+//        } else { //if (driveMode == SQUARE}  //implement the square searching thing.
+        }
+    }
+ 
           if (testOtherCases == 1) {
             if (sensorsCheckSum < 96){ // adjust right
                 }   
@@ -196,24 +234,11 @@
                 }
             testOtherCases = 0;
           }    
-          */
-//              
-//        
-//           
 //        }
 //       else { //if (driveMode == SQUARE}
 //        //implement the square searching thing..
-//       
-//       }
-//       
-//       
-//    }
-//    
-        
-}
 
 
 
 
 
-