Diff: main.cpp

Revision:

9:644102f863a5

Parent:

6:971236e48adc

Child:

10:246782426144

diff -r fa45b344b3f3 -r 644102f863a5 main.cpp
--- a/main.cpp	Mon Mar 13 20:08:19 2017 +0000
+++ b/main.cpp	Mon Mar 13 22:51:41 2017 +0000
@@ -19,36 +19,23 @@
 PwmOut motor_right(PTC8);
 DigitalOut DIR_L(PTD4);
 DigitalOut DIR_R(PTA4);
+Serial pc(USBTX, USBRX);
+Camera cam(PTE23, PTE21, PTB3);
 
 //int start = 0;
 //int end = 0;
 //int rotationTime = 0;
 //float speed = 0;
 //int startFinish = 0;
-//int frame[RESWIDTH][RESHEIGHT];
 
-/*
-int frame(){ //triggered by interrupt from camera or request frame from camera?
-    //get all the pixels
-    int i = 0;
-    int j = 0;
-    for(i; i<RESWIDTH; i++){
-        for(j; j<RESHEIGHT; j++){
-            frame[i][j] = ???;
-        }//j for
-    }//i for
-    return frame;
-}
-*/
 
 void setAccel(float turnAngle){//, float speed){
     //ififififififif
     turnAngle -= STRAIGHT; //this gets a value from -0.00035 and +0.00035
-    turnAngle = abs(turnAngle);
-    float turnRatio = turnAngle/0.00035f;
-    float newSpeed = (0.7*(1-turnRatio)/4)+0.3;
-    motor_left.write(newSpeed);
-    motor_right.write(newSpeed);
+    float turnRatio = abs(turnAngle)/0.00035f;
+    float newSpeed = (0.2*(1-turnRatio)/4)+0.15;  //range of 0.15 - 0.35
+    motor_left.write(newSpeed + 0.5 * newSpeed * (turnAngle / .00035f));
+    motor_right.write(newSpeed - 0.5 * newSpeed * (turnAngle / .00035f));
 }
 
 void turnWheels(){//int[][] frame, float speed){
@@ -87,6 +74,28 @@
     servo.period(0.020f);
     //servo.pulsewidth(STRAIGHT);
     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);
+        
+        //pc.printf("%f\n\n\r\r", wheelPos);
+        
+        
         /*
         if(rotation){
             end = time.read_ms();
@@ -99,7 +108,7 @@
         //frame = frame();
         if(startFinish(frame)){; //frame returns the result 
         */
-            turnWheels();//frame, speed); //includes setAccel();
+        //    turnWheels();//frame, speed); //includes setAccel();
             //setAccel(frame, speed);
         //} //end if(startFinish(frame))        
     }//while 1