Diff: main.cpp

Revision:

25:74c12b0acf0c

Parent:

24:6219b8ce421f

Child:

26:4afa8c5c5156

--- a/main.cpp	Mon Apr 17 23:37:02 2017 +0000
+++ b/main.cpp	Tue Apr 18 17:50:55 2017 +0000
@@ -6,8 +6,10 @@
 #define FULLLEFT 0.0005
 #define MIN_TURN_RATIO 0
 #define MAX_TURN_RATIO 1
-#define MIN_SPEED 0.17  //.15 seems to be optimal
-#define MAX_SPEED 0.45  //.5
+#define MIN_SPEED 0.13  //.15 seems to be optimal
+#define MAX_SPEED 0.4  //.5
+#define SPEED_TRIAL_MIN 0.25
+#define SPEED_TRIAL_MAX 0.55
 #define TURN_TIME 0
 #define STRAIGHT_TIME 20
 #define START_FINISH_TIME 60
@@ -25,7 +27,8 @@
   #error TARGET NOT DEFINED
 #endif
 
-
+float turn_speed = MIN_SPEED;
+float straight_speed = MAX_SPEED;
 PwmOut servo(PTE20);
 PwmOut motor_left(PTA5);
 PwmOut motor_right(PTC8);
@@ -61,7 +64,7 @@
     if(!idle){
         turnAngle -= STRAIGHT; //this gets a value from -0.00035 and +0.00035
         float turnRatio = abs(turnAngle)/ (FULLRIGHT - STRAIGHT);
-        float newSpeed = ((MAX_SPEED - MIN_SPEED)*(1-turnRatio)/3)+MIN_SPEED;
+        float newSpeed = ((straight_speed - turn_speed)*(1-turnRatio)/3)+turn_speed;
         motor_left.write(newSpeed + DIFF_RATIO * newSpeed * (turnAngle / (STRAIGHT - FULLLEFT)));
         motor_right.write(newSpeed - DIFF_RATIO * newSpeed * (turnAngle / (FULLRIGHT - STRAIGHT)));
     }
@@ -139,8 +142,12 @@
     if(tsi.readPercentage() != lastSlide){
         idle = !idle;
         led = 0.0;
+        turn_speed = MIN_SPEED + (SPEED_TRIAL_MIN - MIN_SPEED) *  tsi.readPercentage();
+        straight_speed = MAX_SPEED + (SPEED_TRIAL_MAX - MAX_SPEED) *  tsi.readPercentage();
         wait(0.5);
+        
     }
+    return;
     if(numDarks <= 15) return;
     for(int i = 0; i < 128; i++){
         
@@ -191,8 +198,7 @@
         if(cam.imageData[i] > high) high = cam.imageData[i];
         if(cam.imageData[i] < low) low = cam.imageData[i];
     }
-//    threshold = low + (high - low) * 0.35;        //(high + 2 * low) / 3;
-    threshold = (3 * low + high) / 4;
+    threshold = ((2 * low) + high) / 3;
 }
 
 int main() {