Diff: main.cpp

Revision:

16:60e70bef7828

Parent:

15:50d5cfa98425

Child:

17:846417c48571

diff -r 50d5cfa98425 -r 60e70bef7828 main.cpp
--- a/main.cpp	Mon Mar 27 00:25:38 2017 +0000
+++ b/main.cpp	Mon Mar 27 23:36:07 2017 +0000
@@ -1,14 +1,16 @@
 #include "mbed.h"
 #include "Camera.h"
-#define STRAIGHT 0.00095f
+#define STRAIGHT 0.00094f
 #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 50
+#define TURN_TIME 40
 #define STRAIGHT_TIME 15
+#define DEFAULT_THRESHOLD 65
+#define BLIND_LENGTH 20
 
 PwmOut servo(PTE20);
 PwmOut motor_left(PTA5);
@@ -18,7 +20,11 @@
 Serial pc(USBTX, USBRX);
 Camera cam(PTE23, PTE21, PTB3);
 int turnCounter = 0;
+int threshold = DEFAULT_THRESHOLD;
 float wheelPos = STRAIGHT;
+bool idle = false;
+int leftBlind = 0;
+int rightBlind = 0;
 
 /*
     Function: setAccel
@@ -42,7 +48,7 @@
     int positionSum = 0;
     int numDarks = 0;
     for(int i = 0; i < 128; i++){
-        if(frame[i] < 65){
+        if(frame[i] < threshold){
             positionSum += i;
             numDarks++;
         }
@@ -53,25 +59,30 @@
              if(turnCounter >= (STRAIGHT_TIME)){
                  wheelPos = STRAIGHT;
                  turnCounter = TURN_TIME;
+                 leftBlind = 0;
+                 rightBlind = 0;
             }
         }
         
         else {
             averagePos = positionSum / numDarks;
-            if(averagePos <= 58 && ((wheelPos >= STRAIGHT) || turnCounter >= TURN_TIME)){
-                float powerRatio = (averagePos / 60) * MAX_TURN_RATIO + MIN_TURN_RATIO;
+            
+            if(((averagePos <= 64 - leftBlind)) && ((wheelPos >= STRAIGHT) || turnCounter >= TURN_TIME)){
+                float powerRatio = (averagePos / (64 - leftBlind)) * MAX_TURN_RATIO + MIN_TURN_RATIO;
                 powerRatio = sqrt(powerRatio);
-                if(averagePos >= 45) powerRatio = 1;
                 wheelPos = STRAIGHT + (FULLRIGHT - STRAIGHT) * powerRatio;
                 turnCounter = 0;
+                leftBlind = 0;
+                rightBlind = BLIND_LENGTH;
             }
             
-            else if(averagePos >= 70 && (wheelPos <= STRAIGHT || turnCounter >= TURN_TIME)){
-                float powerRatio = (1 - (averagePos - 69) / 60) * MAX_TURN_RATIO + MIN_TURN_RATIO;
+            else if((averagePos >= 64 + rightBlind) && (wheelPos <= STRAIGHT || turnCounter >= TURN_TIME)){
+                float powerRatio = (1 - (averagePos - 64 - rightBlind) / (64 - rightBlind)) * MAX_TURN_RATIO + MIN_TURN_RATIO;
                 powerRatio = sqrt(powerRatio);
-                if(averagePos <= 85) powerRatio = 1;
                 wheelPos = STRAIGHT - (STRAIGHT - FULLLEFT) * powerRatio;
                 turnCounter = 0;
+                leftBlind = BLIND_LENGTH;
+                rightBlind = 0;
             }
         }
         turnCounter++;
@@ -88,19 +99,30 @@
     }
     pc.printf("\r");
 }
- 
+
+void setThreshold(){
+    cam.capture();
+    int low = 99;
+    int high = 0;
+    for(int i = 0; i < 128; i++){
+        if(cam.imageData[i] > high) high = cam.imageData[i];
+        if(cam.imageData[i] < low) low = cam.imageData[i];
+    }
+    threshold = (high + 2 * low) / 3;
+}
+
 int main() {    
+    setThreshold();
     motor_left.period_us(50);
     motor_right.period_us(50);
     DIR_R = 1;
     DIR_L = 0;
     servo.period(0.020f);
-    
     while(1){
         wait_ms(10);
         cam.capture();
         //display(cam.imageData);
         turnWheels(cam.imageData);      
-        setAccel(wheelPos);
+        if(!idle) setAccel(wheelPos);
     }
 }
\ No newline at end of file