Diff: main.cpp

Revision:

29:b5b31256572b

Parent:

28:613239f10ba4

Child:

30:6c047af9f0cc

Child:

31:1a06c9e1985e

--- a/main.cpp	Thu Dec 15 16:42:16 2016 +0000
+++ b/main.cpp	Tue Jan 10 11:24:13 2017 +0000
@@ -137,31 +137,48 @@
     pid->derivative = 0;
 }
 
+    bool leftSeen;
+    bool rightSeen;
+    
 inline float findCentreValue(volatile uint16_t * cam_data, uint8_t &l, uint8_t &r) {
    
     diff = 0;
     prev = -1;
+
+    leftSeen = false;
+    rightSeen = false;
+        
     for(i = 63; i > 0; i--) {
         curr_left = (uint8_t)(cam_data[i] >> 4) & 0xFF;              
         diff = prev - curr_left;
         if(abs(diff) >= CAM_DIFF && curr_left <= CAM_THRESHOLD && prev != -1) {
             l = i;
+            leftSeen = true;
             break;
         }
         prev = curr_left;
     }
     
+        
+    
     prev = -1;
     for(i = 64; i < 128; i++) {
         curr_right = (uint8_t)(cam_data[i] >> 4) & 0xFF;
         int diff = prev - curr_right;
         if(abs(diff) >= CAM_DIFF && curr_right <= CAM_THRESHOLD && prev != -1) {
             r = i;
+            rightSeen = true;
             break;
         }
         prev = curr_right;
     }
     
+    if(!rightSeen && !leftSeen) {
+        sendString("lost edges");
+        ALIGN_SERVO;
+        servo_pid.integral = 0;    
+    }
+    
     //Calculate how left/right we are   
     return (64 - ((l+r)/2))/64.f;
 }
@@ -180,11 +197,12 @@
         }
     } 
     
-    if(!turning && abs(lookaheadMeasuredValue) > 0.11f){
+    /*if(!turning && abs(lookaheadMeasuredValue) > 0.11f){
         TFC_SetMotorPWM(0.4,0.4);    
     }
+    */
     
-    if(turning) {
+    if(false) {
         
         //default
         //TFC_SetMotorPWM(0.4,0.4);
@@ -195,9 +213,10 @@
         //dutyCycleCorner(float cornerspeed, servo_pid.output);
         //dutyCycleCorner(speed, servo_pid.output);
         
-        //sensorCorner(left_motor_pid.desired_value, right_motor_pid.desired_value , servo_pid.output, 50);  
+        sensorCorner(left_motor_pid.desired_value, right_motor_pid.desired_value , servo_pid.output, speed);  
     }
     
+    /*
     if(abs(servo_pid.measured_value) > 0.11f){
         if(!turning) {            
             turning = 1;
@@ -218,6 +237,7 @@
                
         }
     }
+    */
     
 }
 
@@ -293,14 +313,14 @@
     if(left_motor_pid.output > 1.0f) {
        left_motor_pid.output = 1.0f; 
     }
-    if(left_motor_pid.output < -1.0f) {
+    if(left_motor_pid.output < 0.0f) {
        left_motor_pid.output = 0.0f; 
     }
     
     if(right_motor_pid.output > 1.0f) {
         right_motor_pid.output = 1.0f;
     }
-    if(right_motor_pid.output < -1.0f) {
+    if(right_motor_pid.output < 0.0f) {
         right_motor_pid.output = 0.0f;
     }
     
@@ -520,6 +540,8 @@
                         speed = a;
                         sendString("s = %u %f",a, speed);
                         curr_cmd = 0;   
+                        right_motor_pid.desired_value=speed;
+                        left_motor_pid.desired_value=speed;
                     }
                 break;
                   
@@ -538,7 +560,7 @@
                 right_motor_pid.desired_value=speed;
                 left_motor_pid.desired_value=speed;
                 TFC_HBRIDGE_ENABLE;
-
+                
                     
                 servo_pid.integral = 0;
                 lapTimer.start();