Diff: main.cpp

Revision:

11:45f345aad8ba

Parent:

10:246782426144

Child:

12:4ccf304800fe

diff -r 246782426144 -r 45f345aad8ba main.cpp
--- a/main.cpp	Tue Mar 14 22:10:32 2017 +0000
+++ b/main.cpp	Wed Mar 15 23:33:30 2017 +0000
@@ -8,6 +8,8 @@
 #define STRAIGHT 0.00095f
 #define FULLRIGHT 0.0013f
 #define FULLLEFT 0.0006
+#define MIN_SPEED 0.2
+#define MAX_SPEED 0.4
 
 //InterruptIn camera(PXXX); //can set up to get a frame from the camera whenever is sends one? does the camera even have a pin for this?
 //Timer timer;
@@ -22,6 +24,8 @@
 Serial pc(USBTX, USBRX);
 Camera cam(PTE23, PTE21, PTB3);
 
+int turnCounter = 0;
+float wheelPos = STRAIGHT;
 //int start = 0;
 //int end = 0;
 //int rotationTime = 0;
@@ -33,7 +37,7 @@
     //ififififififif
     turnAngle -= STRAIGHT; //this gets a value from -0.00035 and +0.00035
     float turnRatio = abs(turnAngle)/0.00035f;
-    float newSpeed = (0.2*(1-turnRatio)/4)+0.15;  //range of 0.15 - 0.35
+    float newSpeed = ((MAX_SPEED - MIN_SPEED)*(1-turnRatio)/4)+MIN_SPEED;  //range of 0.15 - 0.35
     motor_left.write(newSpeed + 0.5 * newSpeed * (turnAngle / .00035f));
     motor_right.write(newSpeed - 0.5 * newSpeed * (turnAngle / .00035f));
 }
@@ -43,7 +47,7 @@
     int numDarks = 0;
     for(int i = 0; i < 128; i++){
         //pc.printf("%d ", cam.imageData[i]);
-        if(frame[i] < 80){
+        if(frame[i] < 65){
             positionSum += i;
             numDarks++;
         }
@@ -51,24 +55,33 @@
     //pc.printf("\n\n\r\r");
     float averagePos = 0;
     if(numDarks > 0) averagePos = positionSum / numDarks;
-    else averagePos = -1;
-    float wheelPos = STRAIGHT;
     float turnPercent = ((averagePos - 64.0)/64.0);
-    if (turnPercent > 0) turnPercent = 1-turnPercent;
-    else turnPercent = (1 + turnPercent) * -1;
-    
+    if (turnPercent > 0) turnPercent = (1-turnPercent) * 0.4 + 0.4;
+    else turnPercent = (1 + turnPercent) * -0.5 - 0.4;
     //follow right line
     //(amount from full right to straight) * (% distance black line away from middle of camera's view) + STRAIGHT
     //basically figures out how much to turn away from straight to follow line
 //    float wheelPos = (FULLRIGHT - STRAIGHT) * ((averagePos - 64.0) / 64.0) + STRAIGHT;
-
     //stay between lines
-    if (averagePos < 0) { //no darks found, go straight
-        wheelPos = STRAIGHT;
-    }
-    else { //otherwise darks found, turn away
-        wheelPos = STRAIGHT - (FULLRIGHT - STRAIGHT) * turnPercent;
-    }
+    turnCounter++;
+        if (numDarks == 0) { //no darks found, go straight
+            if(turnCounter >= 10){
+             wheelPos = STRAIGHT;
+            } 
+        }
+        else { //otherwise darks found, turn away
+            float newWheelPos = STRAIGHT - (FULLRIGHT - STRAIGHT) * turnPercent;
+            if(((wheelPos > STRAIGHT) && (newWheelPos <= STRAIGHT)) || ((wheelPos < STRAIGHT) && (newWheelPos >= STRAIGHT))){
+                if(turnCounter >= 50){
+                    wheelPos = newWheelPos;
+                }
+            }
+            else{
+                wheelPos = newWheelPos;
+            }
+        if(wheelPos != STRAIGHT) turnCounter = 0;
+        }
+    turnCounter++;
     //pc.printf("averagePos: %f \n\n\r\r wheelPos: %f \n\n\r\r", averagePos, wheelPos);
     //turn/change speed
     servo.pulsewidth(wheelPos);