Diff: main.cpp

Revision:

19:d277614084bc

Parent:

17:de8b3111ddc5

Child:

20:198dc13777eb

--- a/main.cpp	Wed Mar 18 16:55:14 2015 +0000
+++ b/main.cpp	Thu Mar 19 16:57:30 2015 +0000
@@ -32,14 +32,14 @@
 Serial HC06(PTE0,PTE1); //TX,RX
 //Serial pc(USBTX, USBRX);
 
-//Timer MeasureTimer; //Timer used for measurement
+Timer measureTimer; //Timer used for measurement
 
 //Motor_Driver motors(PTD4, PTA12, PTA4, PTA5, PTC9, PTC8, PWM_PERIOD_US);
 
 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;
 
 
 void measureSensors () {
@@ -59,19 +59,20 @@
 
 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);
+    pc.printf("LLU%i  LRU%i               rlu%i rru%i\n",sensorArray[1]->state,sensorArray[0]->state,sensorArray[1]->state,sensorArray[0]->state);
+    pc.printf("LLD%i  LRD%i               rld%i rrd%i\n\n",sensorArray[3]->state,sensorArray[2]->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);
+    pc.printf("sensorCheckSum%i\n\n",sensorsCheckSum);
+    //HC06.printf("passedTime1 %i passedTime2 \n\n",passedTime1,passedTime2);
 }
 
 int main() {
     
     
     
-    Motor_Driver motors(PTA5, PTC9, PTC8,PTD4, PTA12, PTA4,  PWM_PERIOD_US);
+    Motor_Driver motors(PTA5, PTC9, PTC8,PTD4, PTA12, PTA4, PWM_PERIOD_US);
     
   //  motors.setSpeed(0.1f);
 //    motors.forward();
@@ -109,33 +110,44 @@
     
  //   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(0.1f);
+    motors.setSpeed(normalSpeed);
     motors.forward();
     motors.start();
+    
+    
+    
+    wait(3);
     while(1){
+        
         measureSensors();
+        //measureTimer.reset();
         printBluetooth();
-        if (sensorsCheckSum > 94){
-            motors.setSpeed(0.1f);
-            motors.setLeftSpeed(0.05f);
+        //passedTime1 = measureTimer.read_ms();
+        if (sensorsCheckSum == 0) {
+            motors.setSpeed(normalSpeed);
+            }
+        else if (sensorsCheckSum == 1 || sensorsCheckSum == 9 || sensorsCheckSum == 10 || sensorsCheckSum == 14 || sensorsCheckSum==26){
+            motors.setLeftSpeed(normalSpeed/2);
+            
+            motors.setRightSpeed(normalSpeed*6);
         }
-        else if (sensorsCheckSum < 94) {
-            motors.setSpeed(0.1f);
-            motors.setRightSpeed(0.05f);
+        else if (sensorsCheckSum == 4 && sensorsCheckSum ==16 || sensorsCheckSum == 20 || sensorsCheckSum ==21 ||sensorsCheckSum== 29) {
+            motors.setRightSpeed(normalSpeed/2);
+            motors.setLeftSpeed(normalSpeed*9);
         }
         else {
-            motors.setSpeed(0.1f);
+            motors.setSpeed(normalSpeed);
         }
-        wait(0.1);
+        
     }