Diff: main.cpp

Revision:

14:13085e161dd1

Parent:

13:4e77264f254a

Child:

15:ccde02f96449

--- a/main.cpp	Tue Nov 29 13:11:20 2016 +0000
+++ b/main.cpp	Wed Nov 30 14:54:19 2016 +0000
@@ -13,12 +13,13 @@
 #include "TFC.h"
 #include "XBEE.h"
 #include "angular_speed.h"
+#include "main.h"
 #include "motor.h"
-#include "main.h"
 
 #define BAUD_RATE 57600
 #define CAM_THRESHOLD 109
 #define CAM_DIFF 10
+#define WHEEL_RADIUS 0.025f
 
 #define RIGHT_MOTOR_COMPENSATION_RATIO 1.1586276
 
@@ -192,11 +193,17 @@
         }
     } 
     
+    if(turning) {
+        dutyCycleCorner(0.3,servo_pid.output);
+        //sensorCorner(left_motor_pid.desired_value, right_motor_pid.desired_value , servo_pid.output, 50);  
+    }
+    
     if(abs(servo_pid.measured_value) > 0.11f){
         if(!turning) {
             sendString("start turning");
             
             TFC_SetMotorPWM(0.2,0.2);
+            
             turning = 1;
             
         } else {
@@ -238,13 +245,14 @@
         unsigned char bytes[4];
     } thing;
     
+
     pc.putc('B');
-    thing.a = wL;
+    thing.a = wL * WHEEL_RADIUS;
     pc.putc(thing.bytes[0]);
     pc.putc(thing.bytes[1]);
     pc.putc(thing.bytes[2]);
     pc.putc(thing.bytes[3]);
-    thing.a = wR;
+    thing.a = wR * WHEEL_RADIUS;
     pc.putc(thing.bytes[0]);
     pc.putc(thing.bytes[1]);
     pc.putc(thing.bytes[2]);
@@ -403,10 +411,6 @@
         //If we've done 5 laps, stop the car
         if(startstop >= 1) {
             TFC_SetMotorPWM(0.f,0.f);
-            wait(0.08);
-            TFC_SetMotorPWM(-0.6f,-0.6f);
-            wait(0.3);
-            TFC_SetMotorPWM(0.f,0.f);
             TFC_HBRIDGE_DISABLE;
             startstop = 0;      
         }