Diff: main.cpp

Revision:

17:de8b3111ddc5

Parent:

16:3649eb1a056d

Child:

18:d947ea4bab72

Child:

19:d277614084bc

--- a/main.cpp	Fri Mar 13 18:19:01 2015 +0000
+++ b/main.cpp	Wed Mar 18 16:55:14 2015 +0000
@@ -49,6 +49,7 @@
           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);
         }
@@ -58,32 +59,54 @@
 
 void printBluetooth() { //for debugging
     
-    HC06.printf("rru%i  rlu%i               rrd%i rld%i\n",sensorArray[0]->state,sensorArray[1]->state,sensorArray[2]->state,sensorArray[3]->state);
-    //HC06.printf("%i  %i               %i %i",sensorArray[NUMBER_SENSORS_REGULAR-1]->state,sensorArray[NUMBER_SENSORS_REGULAR-2]->state,sensorArray[1]->state,sensorArray[0]->state);
+    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("/n/n");
+    HC06.printf("sensorCheckSum%i\n\n",sensorsCheckSum);
 }
 
 int main() {
     
     
-    /*
-    Motor_Driver motors(PTD4, PTA12, PTA4, PTA5, PTC9, PTC8, PWM_PERIOD_US);
     
-    motors.setSpeed(1.0f);
-    motors.forward();
-    motors.start();
+    Motor_Driver motors(PTA5, PTC9, PTC8,PTD4, PTA12, PTA4,  PWM_PERIOD_US);
     
-    wait(3);
+  //  motors.setSpeed(0.1f);
+//    motors.forward();
+//    motors.start();
+//    wait(2);
+//    float x=0.1f;
+//    while (1) {
+//        motors.setLeftSpeed(x);
+//        x = x+0.05;
+//        wait(3);
+//        }
+//    motors.setLeftSpeed(0.1f);
+//    wait(5);
+//    motors.setLeftSpeed(0.2f);
+//    motors.setRightSpeed (0.2f);
+//    wait(3);
+//    motors.setRightSpeed (0.1f);
+//    wait(5);
+//    motors.stop();
     
-    motors.stop()
-    motors.reverse();
-    wait(200ms);
+    //wait(1);
+//    motors.reverse();
+//    wait(5);
+//    motors.stop();
+//    motors.setSpeed(0.5f);
+//    motors.start();
+//    wait(5);
+//    motors.stop();
+//    wait(1);
+//    motors.reverse();
     
-    motors.start();
-    */ 
+   
+    
+    //motors.start();
+    
  //   setup_counter(1000, 0);
   //  float frequency = measure_frequency(CHANNEL_1);
     
@@ -95,11 +118,25 @@
     
     HC06.baud(9600);
     HC06.printf("working..");
+    motors.setSpeed(0.1f);
+    motors.forward();
+    motors.start();
     while(1){
         measureSensors();
         printBluetooth();
-        wait(0.5);
+        if (sensorsCheckSum > 94){
+            motors.setSpeed(0.1f);
+            motors.setLeftSpeed(0.05f);
         }
+        else if (sensorsCheckSum < 94) {
+            motors.setSpeed(0.1f);
+            motors.setRightSpeed(0.05f);
+        }
+        else {
+            motors.setSpeed(0.1f);
+        }
+        wait(0.1);
+    }
     
    
     //HC06.printf("AT");
@@ -107,22 +144,30 @@
     
 
    // pc.printf("Start...");
-//    
-//    
+    
+    
 //    while (1) {
 //       
 //       if (driveMode == REGULAR) {
 //          measureSensors();
 //          switch (sensorsCheckSum) {
-//            case 1: //right right is 1*1 + 0 + 0+ 0+0 +0 
+//            case 74: //all right are white,else is good TURN RIGHT
+//                break;
+//            case 78: //RLD is black, all else good
+//                break;
+//            case 90: //RLU is black,all else good
+//                break;
+//            
+//            case 94: //keep straight
 //                pc.printf ("only Right is white \n");
 //                break;
-//            case 4 :   //0*0 + 2*2 + 0 + 0 + 0
+//            case 95 :   //RRU is white
 //                pc.printf ("only Left is white \n");
 //                break;
-//            case 5 :  //1*1 + 2*2 + 0 + 0 
+//            case 103 :  //RRD is white all else normal
 //                pc.printf ("both are white \n");
 //                break;
+//            
 ////          case 91: 
 ////            driveMode = SQUARE; //if all sensors are white you're in the square
 ////            break;
@@ -149,31 +194,4 @@
 
 
 
-/*    TOBE deleted
-double measure1 (){
 
-
-    
-    sensorTimer.reset();
-    double freq,period = 0.0;
-    int n =0; //number of samples
-    int sensor_old = 0;
-    int sensor_new  = 0;//variable to remember old sensor state
-    double time_us = sensorTimer.read(); 
-    while (n < NUMBER_SAMPLES){
-        sensor_new = pin_right_right.read();
-        if ( sensor_new== 1 && sensor_old == 0){ // detect on rising edge ,sensor_old
-            n++;
-        }
-        sensor_old = sensor_new;
-        
-    }
-    double time_us2 = sensorTimer.read();
-   // pc.printf(" delta time is %f , time 2 is %f " , (time_us2 - time_us), time_us2); 
-    period = time_us2/((double)NUMBER_SAMPLES); // Get time
-    freq = (1/period);   // Convert period (in us) to frequency (Hz). Works up to 100kHz. 
-   // pc.printf(" period is  %f  ", period);
-    
-    return freq;//(freq < sensor.black_value*2)? 1 : 0;
-}
-*/