Diff: main.cpp

Revision:

27:627d67e3b9b0

Parent:

26:f3d770f3eda1

Child:

28:613239f10ba4

--- a/main.cpp	Thu Dec 15 10:05:07 2016 +0000
+++ b/main.cpp	Thu Dec 15 16:39:32 2016 +0000
@@ -32,7 +32,7 @@
 Timer t2;
 
 //testing timer
-Timer test;
+Timer lapTimer;
 
 
 int main() {
@@ -64,8 +64,10 @@
             // Check if car is at the stop line
             handleStartStop();
             
-            // Send the line scan image over serial
-            sendImage();
+            #if USE_COMMS
+                // Send the line scan image over serial
+                sendImage();
+            #endif
             
             
             //Reset image ready flag
@@ -239,33 +241,28 @@
     if(pid->integral > 1.0f) {
         pid->integral = 1.0f;   
     }
-    if(pid->integral < -1.0f) {
+    if(pid->integral < -1.0f    ) {
         pid->integral = -1.0f;   
     }
 }
 
 
-
 inline void PIDController() {
-    // update motor measurements
+     // update motor measurements
     // Read the angular velocity of both wheels
+    
+    prevL = wL;
+    prevR = wR;
+    
     wL=Get_Speed(Time_L);
     wR=Get_Speed(Time_R);
     
     // Check if left wheel is slipping/giving an abnormal reading and ignore reading
     if(wL - prevL < 1.2/0.025) {
         left_motor_pid.measured_value = wL;
-        prevL = wL;  
-    } else {
-        wL = prevL; 
     }
-    
-    // Same as above for right
     if(wR - prevR < 1.2/0.025) {
         right_motor_pid.measured_value = wR;
-        prevR = wR;
-    } else {
-        wR = prevR;
     }
     
     
@@ -297,14 +294,14 @@
        left_motor_pid.output = 1.0f; 
     }
     if(left_motor_pid.output < -1.0f) {
-       left_motor_pid.output = 0.f; 
+       left_motor_pid.output = 0.0f; 
     }
     
     if(right_motor_pid.output > 1.0f) {
         right_motor_pid.output = 1.0f;
     }
     if(right_motor_pid.output < -1.0f) {
-        right_motor_pid.output = 0.f;
+        right_motor_pid.output = 0.0f;
     }
     
     TFC_SetMotorPWM(left_motor_pid.output,right_motor_pid.output);
@@ -370,7 +367,7 @@
     for(int i = 30; i < 98; i++) {
         currentPixel = (int)(CLOSE_CAMERA[i] >> 4) & 0xFF;
         difference = lastPixel - currentPixel;
-        if(abs(difference) > 20 && lastPixel != -1){ //transition seen, increment counter
+        if(abs(difference) > 10 && lastPixel != -1){ //transition seen, increment counter
             transitionsSeen++;
             i+=5;
         }
@@ -384,8 +381,8 @@
         sendString("Start/stop seen");
         TFC_SetMotorPWM(0.f,0.f);
         TFC_HBRIDGE_DISABLE;
+        lapTime();
     }
-    transitionsSeen = 0;
   //  slower++;
     
 }
@@ -416,11 +413,11 @@
     if(frame_counter % 256 == 0) {
         float level = TFC_ReadBatteryVoltage() * 6.25;          
         pc.putc('J');
-        thing.a = level;
-        pc.putc(thing.bytes[0]);
-        pc.putc(thing.bytes[1]);
-        pc.putc(thing.bytes[2]);
-        pc.putc(thing.bytes[3]);
+        byte_float_union.a = level;
+        pc.putc(byte_float_union.bytes[0]);
+        pc.putc(byte_float_union.bytes[1]);
+        pc.putc(byte_float_union.bytes[2]);
+        pc.putc(byte_float_union.bytes[3]);
     }
 }
 
@@ -473,16 +470,17 @@
     
 
     pc.putc('B');
-    thing.a = wL * WHEEL_RADIUS;//left_motor_pid.output; //
-    pc.putc(thing.bytes[0]);
-    pc.putc(thing.bytes[1]);
-    pc.putc(thing.bytes[2]);
-    pc.putc(thing.bytes[3]);
-    thing.a = wR * WHEEL_RADIUS; // right_motor_pid.output; //
-    pc.putc(thing.bytes[0]);
-    pc.putc(thing.bytes[1]);
-    pc.putc(thing.bytes[2]);
-    pc.putc(thing.bytes[3]);    
+    byte_float_union.a = wL * WHEEL_RADIUS;//left_motor_pid.output; //
+    pc.putc(byte_float_union.bytes[0]);
+    pc.putc(byte_float_union.bytes[1]);
+    pc.putc(byte_float_union.bytes[2]);
+    pc.putc(byte_float_union.bytes[3]);
+    byte_float_union.a = wR * WHEEL_RADIUS; // right_motor_pid.output; //
+    pc.putc(byte_float_union.bytes[0]);
+    pc.putc(byte_float_union.bytes[1]);
+    pc.putc(byte_float_union.bytes[2]);
+    pc.putc(byte_float_union.bytes[3]);    
+
 }
 
 
@@ -492,23 +490,23 @@
                 case 'A':
                     if(xb.cBuffer->available() >= 12) {
                     
-                        thing.bytes[0] = xb.cBuffer->read();
-                        thing.bytes[1] = xb.cBuffer->read();
-                        thing.bytes[2] = xb.cBuffer->read();
-                        thing.bytes[3] = xb.cBuffer->read();
-                        servo_pid.Kp = thing.a; 
+                        byte_float_union.bytes[0] = xb.cBuffer->read();
+                        byte_float_union.bytes[1] = xb.cBuffer->read();
+                        byte_float_union.bytes[2] = xb.cBuffer->read();
+                        byte_float_union.bytes[3] = xb.cBuffer->read();
+                        servo_pid.Kp = byte_float_union.a; 
                         
-                        thing.bytes[0] = xb.cBuffer->read();
-                        thing.bytes[1] = xb.cBuffer->read();
-                        thing.bytes[2] = xb.cBuffer->read();
-                        thing.bytes[3] = xb.cBuffer->read();
-                        servo_pid.Ki = thing.a; 
+                        byte_float_union.bytes[0] = xb.cBuffer->read();
+                        byte_float_union.bytes[1] = xb.cBuffer->read();
+                        byte_float_union.bytes[2] = xb.cBuffer->read();
+                        byte_float_union.bytes[3] = xb.cBuffer->read();
+                        servo_pid.Ki = byte_float_union.a; 
                         
-                        thing.bytes[0] = xb.cBuffer->read();
-                        thing.bytes[1] = xb.cBuffer->read();
-                        thing.bytes[2] = xb.cBuffer->read();
-                        thing.bytes[3] = xb.cBuffer->read();
-                        servo_pid.Kd = thing.a; 
+                        byte_float_union.bytes[0] = xb.cBuffer->read();
+                        byte_float_union.bytes[1] = xb.cBuffer->read();
+                        byte_float_union.bytes[2] = xb.cBuffer->read();
+                        byte_float_union.bytes[3] = xb.cBuffer->read();
+                        servo_pid.Kd = byte_float_union.a; 
                         
                         sendString("pid= Kp: %f, Ki: %f, Kd: %f",  servo_pid.Kp, servo_pid.Ki, servo_pid.Kd);
                         
@@ -519,7 +517,7 @@
                 case 'F':
                     if(xb.cBuffer->available() >= 1) {
                         char a = xb.cBuffer->read();
-                        speed = (a/0.025f)/50.f;
+                        speed = a;
                         sendString("s = %u %f",a, speed);
                         curr_cmd = 0;   
                     }
@@ -543,7 +541,7 @@
 
                     
                 servo_pid.integral = 0;
-                test.start();
+                lapTimer.start();
                 lapNo =0;
                 
             } else if (cmd == 'C') {
@@ -599,7 +597,7 @@
 int lapTime()
 {   
     // function which sends the lap time back to the telemetry.
-    float newTime= test.read();
+    float newTime= lapTimer.read();
     lapNo += 1;
     float lapTime= newTime-oldTime;
     float avgTime= newTime/lapNo;
@@ -614,10 +612,10 @@
 void endTest()
 {// This runs when the car has stopped, this should give the final elapsed time and othere things. this also stops the timer
     
-    float time= test.read();
+    float time= lapTimer.read();
     
     sendString("Laps done: %d  Time elapsed: %f   Average time: %f \n\r",lapNo, time,float (time/lapNo));
-    test.stop();
+    lapTimer.stop();
     
     
 }