Diff: main.cpp

Revision:

22:9cf274ffe1de

Parent:

21:e8da3b351cd0

Child:

23:902c3086394e

--- a/main.cpp	Fri Mar 20 14:56:13 2015 +0000
+++ b/main.cpp	Fri Mar 20 15:49:31 2015 +0000
@@ -40,6 +40,8 @@
 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 k = 0;
 
 
 void measureSensors () {
@@ -53,12 +55,17 @@
         if (measure(sensorArray[i]) == 1) {//if sensor is white 
             sensorsCheckSum += (i+1)*(i+1);
         }
-    }    
+    }
+    if (values_old[0] != sensorsCheckSum) {
+        for (k = 5; k > 0; k--) {
+            values_old[k] = values_old[k-1];
+        }
+        values_old[0] = sensorsCheckSum;
+    }
     //pc.printf("sensorsCheckSum is %i",sensorsCheckSum);
 }
 
 void printBluetooth() { //for debugging
-    
     pc.printf("LLU%i  LRU%i               rlu%i rru%i\n",sensorArray[7]->state,sensorArray[6]->state,sensorArray[1]->state,sensorArray[0]->state);
     pc.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);
@@ -69,9 +76,7 @@
 }
 
 int main() {
-    
-    
-    
+        
     Motor_Driver motors(PTA5, PTC9, PTC8,PTD4, PTA12, PTA4, PWM_PERIOD_US);
     
   //  motors.setSpeed(0.1f);
@@ -105,27 +110,26 @@
 //    motors.reverse();
     
    
-    
-    //motors.start();
-    
- //   setup_counter(1000, 0);
-  //  float frequency = measure_frequency(CHANNEL_1);
-    measureTimer.start();
-    driveMode = REGULAR; //initialise drivemoder
-    sensor_initialise(); // initialise sensor values
-    wait(1); //give time to set up the system
+//motors.start();
     
-    sensorTimer.start(); // start timer for sensors
+//    setup_counter(1000, 0);
+//    float frequency = measure_frequency(CHANNEL_1);
+//    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
     float normalSpeed = 0.01f;
-    HC06.baud(9600);
-    HC06.printf("working..");
-    motors.setSpeed(normalSpeed);
-    motors.forward();
-    motors.start();
-    
-    
-    
-    wait(3);
+//    HC06.baud(9600);
+//    HC06.printf("working..");
+//    motors.setSpeed(normalSpeed);
+//    motors.forward();
+//    motors.start();
+//    
+//    
+//    
+//    wait(3);
     while(1){
         
         measureSensors();