Diff: main.cpp

Revision:

15:50d5cfa98425

Parent:

14:c6f0a3c4e222

Child:

16:60e70bef7828

--- a/main.cpp	Wed Mar 22 22:23:18 2017 +0000
+++ b/main.cpp	Mon Mar 27 00:25:38 2017 +0000
@@ -1,13 +1,14 @@
 #include "mbed.h"
 #include "Camera.h"
-#define STRAIGHT 0.0009f
+#define STRAIGHT 0.00095f
 #define FULLRIGHT 0.0013f
 #define FULLLEFT 0.0005
 #define MIN_TURN_RATIO 0
 #define MAX_TURN_RATIO 1
-#define MIN_SPEED 0.2
+#define MIN_SPEED 0.15
 #define MAX_SPEED 0.4
-#define TURN_TIME 30
+#define TURN_TIME 50
+#define STRAIGHT_TIME 15
 
 PwmOut servo(PTE20);
 PwmOut motor_left(PTA5);
@@ -28,8 +29,8 @@
     turnAngle -= STRAIGHT; //this gets a value from -0.00035 and +0.00035
     float turnRatio = abs(turnAngle)/0.00035f;
     float newSpeed = ((MAX_SPEED - MIN_SPEED)*(1-turnRatio)/3)+MIN_SPEED;
-    motor_left.write(newSpeed + 0.4 * newSpeed * (turnAngle / .00035f));
-    motor_right.write(newSpeed - 0.4 * newSpeed * (turnAngle / .00035f));
+    motor_left.write(newSpeed + 0.5 * newSpeed * (turnAngle / .00035f));
+    motor_right.write(newSpeed - 0.5 * newSpeed * (turnAngle / .00035f));
 }//end setAccel
 
 /*
@@ -49,20 +50,25 @@
     float averagePos = 0;
 
         if (numDarks == 0) {
-             if(turnCounter >= (TURN_TIME / 3)) wheelPos = STRAIGHT;
+             if(turnCounter >= (STRAIGHT_TIME)){
+                 wheelPos = STRAIGHT;
+                 turnCounter = TURN_TIME;
+            }
         }
         
         else {
             averagePos = positionSum / numDarks;
-            if(averagePos <= 60 && ((wheelPos >= STRAIGHT) || turnCounter >= TURN_TIME)){
+            if(averagePos <= 58 && ((wheelPos >= STRAIGHT) || turnCounter >= TURN_TIME)){
                 float powerRatio = (averagePos / 60) * MAX_TURN_RATIO + MIN_TURN_RATIO;
+                powerRatio = sqrt(powerRatio);
                 if(averagePos >= 45) powerRatio = 1;
                 wheelPos = STRAIGHT + (FULLRIGHT - STRAIGHT) * powerRatio;
                 turnCounter = 0;
             }
             
-            else if(averagePos >= 68 && (wheelPos <= STRAIGHT || turnCounter >= TURN_TIME)){
+            else if(averagePos >= 70 && (wheelPos <= STRAIGHT || turnCounter >= TURN_TIME)){
                 float powerRatio = (1 - (averagePos - 69) / 60) * MAX_TURN_RATIO + MIN_TURN_RATIO;
+                powerRatio = sqrt(powerRatio);
                 if(averagePos <= 85) powerRatio = 1;
                 wheelPos = STRAIGHT - (STRAIGHT - FULLLEFT) * powerRatio;
                 turnCounter = 0;