Diff: main.cpp

Revision:

10:246782426144

Parent:

9:644102f863a5

Child:

11:45f345aad8ba

--- a/main.cpp	Mon Mar 13 22:51:41 2017 +0000
+++ b/main.cpp	Tue Mar 14 22:10:32 2017 +0000
@@ -38,18 +38,52 @@
     motor_right.write(newSpeed - 0.5 * newSpeed * (turnAngle / .00035f));
 }
 
-void turnWheels(){//int[][] frame, float speed){
+void turnWheels(int frame[]){  //int[][] frame, float speed){
+    int positionSum = 0;
+    int numDarks = 0;
+    for(int i = 0; i < 128; i++){
+        //pc.printf("%d ", cam.imageData[i]);
+        if(frame[i] < 80){
+            positionSum += i;
+            numDarks++;
+        }
+    }
+    //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;
+    
+    //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;
+    }
+    //pc.printf("averagePos: %f \n\n\r\r wheelPos: %f \n\n\r\r", averagePos, wheelPos);
+    //turn/change speed
+    servo.pulsewidth(wheelPos);
+    setAccel(wheelPos);
     //ififififififif
-    for(float p=FULLLEFT; p<=FULLRIGHT; p += 0.00005f) {
-        servo.pulsewidth(p);
-        setAccel(p);
-        wait(.1);
-    }//for p increase
-    for(float p=FULLRIGHT; p>=FULLLEFT; p -= 0.00005f) {
-        servo.pulsewidth(p);
-        setAccel(p);
-        wait(.1);
-    }//for p decrease
+//    for(float p=FULLLEFT; p<=FULLRIGHT; p += 0.00005f) {
+//        servo.pulsewidth(p);
+//        setAccel(p);
+//        wait(.1);
+//    }//for p increase
+//    for(float p=FULLRIGHT; p>=FULLLEFT; p -= 0.00005f) {
+//        servo.pulsewidth(p);
+//        setAccel(p);
+//        wait(.1);
+//    }//for p decrease
 }
 
 
@@ -76,22 +110,8 @@
     while(1){
         wait_ms(10);
         cam.capture();
-        int positionSum = 0;
-        int numDarks = 0;
-        for(int i = 0; i < 128; i++){
-            //pc.printf("%d ", cam.imageData[i]);
-            if(cam.imageData[i] < 60){
-                positionSum += i;
-                numDarks++;
-            }
-        }
-        //pc.printf("\n\n\r\r");
-        float averagePos;
-        if(numDarks > 0) averagePos = positionSum / numDarks;
-        else averagePos = 0;
-        float wheelPos = (FULLRIGHT - STRAIGHT) * ((averagePos - 64.0) / 64.0) + STRAIGHT;
-        servo.pulsewidth(wheelPos);
-        setAccel(wheelPos);
+        turnWheels(cam.imageData);
+        
         
         //pc.printf("%f\n\n\r\r", wheelPos);