Diff: main.cpp

Revision:

14:c6f0a3c4e222

Parent:

13:9175be9c2b9e

Child:

15:50d5cfa98425

--- a/main.cpp	Wed Mar 22 20:19:03 2017 +0000
+++ b/main.cpp	Wed Mar 22 22:23:18 2017 +0000
@@ -1,13 +1,13 @@
 #include "mbed.h"
 #include "Camera.h"
-#define STRAIGHT 0.00095f
+#define STRAIGHT 0.0009f
 #define FULLRIGHT 0.0013f
-#define FULLLEFT 0.0006
-#define MIN_TURN_RATIO 0.3
-#define MAX_TURN_RATIO 0.7
-#define MIN_SPEED 0.1
-#define MAX_SPEED 0.6
-#define TURN_TIME 50
+#define FULLLEFT 0.0005
+#define MIN_TURN_RATIO 0
+#define MAX_TURN_RATIO 1
+#define MIN_SPEED 0.2
+#define MAX_SPEED 0.4
+#define TURN_TIME 30
 
 PwmOut servo(PTE20);
 PwmOut motor_left(PTA5);
@@ -27,9 +27,9 @@
 void setAccel(float turnAngle){//, float speed){
     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)/4)+MIN_SPEED;
-    motor_left.write(newSpeed + 0.5 * newSpeed * (turnAngle / .00035f));
-    motor_right.write(newSpeed - 0.5 * newSpeed * (turnAngle / .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));
 }//end setAccel
 
 /*
@@ -37,7 +37,7 @@
     Description: Turns the wheels in order to stay between two black lines seen
                  by the camera
 */
-void turnWheels(int frame[]){  //int[][] frame, float speed){
+void turnWheels(int frame[]){
     int positionSum = 0;
     int numDarks = 0;
     for(int i = 0; i < 128; i++){
@@ -48,30 +48,28 @@
     }
     float averagePos = 0;
 
-        if (numDarks == 0 && turnCounter >= (TURN_TIME / 2)) {
-             wheelPos = STRAIGHT;
+        if (numDarks == 0) {
+             if(turnCounter >= (TURN_TIME / 3)) wheelPos = STRAIGHT;
         }
         
         else {
             averagePos = positionSum / numDarks;
-            if(averagePos <= 64 && ((wheelPos >= STRAIGHT) || turnCounter >= TURN_TIME)){
-                float powerRatio = (averagePos / 64) * MAX_TURN_RATIO + MIN_TURN_RATIO;
+            if(averagePos <= 60 && ((wheelPos >= STRAIGHT) || turnCounter >= TURN_TIME)){
+                float powerRatio = (averagePos / 60) * MAX_TURN_RATIO + MIN_TURN_RATIO;
                 if(averagePos >= 45) powerRatio = 1;
                 wheelPos = STRAIGHT + (FULLRIGHT - STRAIGHT) * powerRatio;
                 turnCounter = 0;
             }
             
-            else if(averagePos >= 66 && (wheelPos <= STRAIGHT || turnCounter >= TURN_TIME)){
-                float powerRatio = (1 - (averagePos - 64) / 64) * MAX_TURN_RATIO + MIN_TURN_RATIO;
+            else if(averagePos >= 68 && (wheelPos <= STRAIGHT || turnCounter >= TURN_TIME)){
+                float powerRatio = (1 - (averagePos - 69) / 60) * MAX_TURN_RATIO + MIN_TURN_RATIO;
                 if(averagePos <= 85) powerRatio = 1;
                 wheelPos = STRAIGHT - (STRAIGHT - FULLLEFT) * powerRatio;
                 turnCounter = 0;
             }
+        }
         turnCounter++;
-        }
-        
     servo.pulsewidth(wheelPos);
-    setAccel(wheelPos);
 }
 
 void display(int frame[]){