Diff: main.cpp

Revision:

12:4ccf304800fe

Parent:

11:45f345aad8ba

Child:

13:9175be9c2b9e

--- a/main.cpp	Wed Mar 15 23:33:30 2017 +0000
+++ b/main.cpp	Tue Mar 21 01:42:19 2017 +0000
@@ -1,21 +1,15 @@
 #include "mbed.h"
 #include "Servo.h"
 #include "Camera.h"
-
-//#define CIRCUMFERENCE SOMEINT //circumference in some unit of measurement (inches perhaps?)
-//#define RESWIDTH 80  //resolution width/height. change these based on actual resolution
-//#define RESHEIGHT 60
 #define STRAIGHT 0.00095f
 #define FULLRIGHT 0.0013f
 #define FULLLEFT 0.0006
-#define MIN_SPEED 0.2
-#define MAX_SPEED 0.4
+#define MIN_TURN_RATIO 0.3
+#define MAX_TURN_RATIO 0.7
+#define MIN_SPEED 0.1
+#define MAX_SPEED 0.6
+#define TURN_TIME 100
 
-//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;
-//DigitalIn rotation(PXXX);
-//camera data pins used are (D0-D7): PTC7, PTC0, PTC3, PTC4, PTC5, PTC6, PTC10, PTC11
-//other camera pins: SCL->PTE5, SDA->PTE4, PCLK->PTE21, VSYNC->PTE30, H->PTE29
 PwmOut servo(PTE20);
 PwmOut motor_left(PTA5);
 PwmOut motor_right(PTC8);
@@ -26,123 +20,86 @@
 
 int turnCounter = 0;
 float wheelPos = STRAIGHT;
-//int start = 0;
-//int end = 0;
-//int rotationTime = 0;
-//float speed = 0;
-//int startFinish = 0;
 
-
+/*
+    Function: setAccel
+    Description: Sets the speed for the right and left motors individually based
+                 on the turning angle. 
+*/
 void setAccel(float turnAngle){//, float speed){
-    //ififififififif
     turnAngle -= STRAIGHT; //this gets a value from -0.00035 and +0.00035
     float turnRatio = abs(turnAngle)/0.00035f;
-    float newSpeed = ((MAX_SPEED - MIN_SPEED)*(1-turnRatio)/4)+MIN_SPEED;  //range of 0.15 - 0.35
+    float newSpeed = ((MAX_SPEED - MIN_SPEED)*(1-turnRatio)/4)+MIN_SPEED;
     motor_left.write(newSpeed + 0.5 * newSpeed * (turnAngle / .00035f));
     motor_right.write(newSpeed - 0.5 * newSpeed * (turnAngle / .00035f));
-}
+}//end setAccel
 
+/*
+    Function: turnWheels
+    Description: Turns the wheels in order to stay between two black lines seen
+                 by the camera
+*/
 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] < 65){
             positionSum += i;
             numDarks++;
         }
     }
-    //pc.printf("\n\n\r\r");
     float averagePos = 0;
-    if(numDarks > 0) averagePos = positionSum / numDarks;
-    float turnPercent = ((averagePos - 64.0)/64.0);
-    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
-    turnCounter++;
-        if (numDarks == 0) { //no darks found, go straight
-            if(turnCounter >= 10){
+
+        if (numDarks == 0) {
              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 {
+            averagePos = positionSum / numDarks;
+            if(averagePos <= 64 && ((wheelPos >= STRAIGHT) || turnCounter >= TURN_TIME)){
+                float powerRatio = (averagePos / 64) * MAX_TURN_RATIO + MIN_TURN_RATIO;
+                if(averagePos >= 45) powerRatio = 1;
+                wheelPos = STRAIGHT + (FULLRIGHT - STRAIGHT) * powerRatio;
+                turnCounter = 0;
             }
-            else{
-                wheelPos = newWheelPos;
+            
+            else if(averagePos >= 66 && (wheelPos <= STRAIGHT || turnCounter >= TURN_TIME)){
+                float powerRatio = (1 - (averagePos - 64) / 64) * MAX_TURN_RATIO + MIN_TURN_RATIO;
+                if(averagePos <= 85) powerRatio = 1;
+                wheelPos = STRAIGHT - (STRAIGHT - FULLLEFT) * powerRatio;
+                turnCounter = 0;
             }
-        if(wheelPos != STRAIGHT) turnCounter = 0;
+        turnCounter++;
         }
-    turnCounter++;
-    //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
 }
 
-
-/*
-int startFinish(int[][] frame){
-    //if start
-    return 1;
-    //if notFinished
-    return 1;
-    //if finished
-    return 0;
+void display(int frame[]){
+    char draw = 'x';
+    for(int i = 0; i< 128; i++){
+        if (frame[i] <65) draw = '|';
+        else draw = '-';
+        pc.printf("%c", draw);
+        draw = 'x';
+    }
+    pc.printf("\r");
 }
-*/
  
 int main() {    
     //timer.start();
     motor_left.period_us(50);
     motor_right.period_us(50);
-    //motor.write(0.25);
     DIR_R = 1;
     DIR_L = 0;
     servo.period(0.020f);
-    //servo.pulsewidth(STRAIGHT);
+    
     while(1){
         wait_ms(10);
         cam.capture();
-        turnWheels(cam.imageData);
-        
-        
-        //pc.printf("%f\n\n\r\r", wheelPos);
-        
-        
-        /*
-        if(rotation){
-            end = time.read_ms();
-            rotationTime = end-start;
-            speed = CIRCUFERENCE / rotationTime; //inches/ms
-            speed *= 56.8182 //convert to miles/hour
-            start = timer.read_ms(); 
-        }
-        //camera.rise(&frame); //idea is that camera tells MCU that it's sending a frame
-        //frame = frame();
-        if(startFinish(frame)){; //frame returns the result 
-        */
-        //    turnWheels();//frame, speed); //includes setAccel();
-            //setAccel(frame, speed);
-        //} //end if(startFinish(frame))        
-    }//while 1
-}//main
\ No newline at end of file
+        //display(cam.imageData);
+        turnWheels(cam.imageData);      
+        setAccel(wheelPos);
+    }
+}
\ No newline at end of file