Diff: main.cpp

Revision:

17:846417c48571

Parent:

16:60e70bef7828

Child:

18:8dbd05e65211

--- a/main.cpp	Mon Mar 27 23:36:07 2017 +0000
+++ b/main.cpp	Fri Mar 31 19:31:34 2017 +0000
@@ -1,16 +1,17 @@
 #include "mbed.h"
 #include "Camera.h"
-#define STRAIGHT 0.00094f
+#define STRAIGHT 0.00092f
 #define FULLRIGHT 0.0013f
 #define FULLLEFT 0.0005
 #define MIN_TURN_RATIO 0
 #define MAX_TURN_RATIO 1
 #define MIN_SPEED 0.15
 #define MAX_SPEED 0.4
-#define TURN_TIME 40
-#define STRAIGHT_TIME 15
+#define TURN_TIME 0
+#define STRAIGHT_TIME 10
 #define DEFAULT_THRESHOLD 65
-#define BLIND_LENGTH 20
+#define BLIND_LENGTH 30
+#define DIFF_RATIO 0.5
 
 PwmOut servo(PTE20);
 PwmOut motor_left(PTA5);
@@ -33,10 +34,10 @@
 */
 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 turnRatio = abs(turnAngle)/ (FULLRIGHT - STRAIGHT);
     float newSpeed = ((MAX_SPEED - MIN_SPEED)*(1-turnRatio)/3)+MIN_SPEED;
-    motor_left.write(newSpeed + 0.5 * newSpeed * (turnAngle / .00035f));
-    motor_right.write(newSpeed - 0.5 * newSpeed * (turnAngle / .00035f));
+    motor_left.write(newSpeed + DIFF_RATIO * newSpeed * (turnAngle / (STRAIGHT - FULLLEFT)));
+    motor_right.write(newSpeed - DIFF_RATIO * newSpeed * (turnAngle / (FULLRIGHT - STRAIGHT)));
 }//end setAccel
 
 /*
@@ -92,9 +93,11 @@
 void display(int frame[]){
     char draw = 'x';
     for(int i = 0; i< 128; i++){
-        if (frame[i] <65) draw = '|';
+        
+        if (frame[i] < threshold) draw = '|';
         else draw = '-';
         pc.printf("%c", draw);
+        //pc.printf("%d", frame[i]);
         draw = 'x';
     }
     pc.printf("\r");