Diff: main.cpp

Revision:

6:b0e160c51013

Parent:

4:4afa448c9cce

Child:

7:ad893fc41b95

--- a/main.cpp	Thu Nov 03 13:07:52 2016 +0000
+++ b/main.cpp	Wed Nov 09 17:54:38 2016 +0000
@@ -1,7 +1,7 @@
 #include "mbed.h"
 #include "TFC.h"
 #include "XBEE.h"
-#define CAM_THRESHOLD 128
+#define CAM_THRESHOLD 109
  
  
 DigitalOut myled(LED1);
@@ -21,6 +21,7 @@
 char curr_cmd = 0;
 
 float speed = 0.3;
+int frame_counter = 0;
  
 int main() {
     TFC_Init();
@@ -37,8 +38,8 @@
     float output;
       //tunable variables
     float Kp, Ki,Kd;
-        Kp=0.8;
-    Ki=0.6;
+        Kp=125/25.0f;
+    Ki=12.0f/25.0f;
     Kd=0.0;
     myled = 0;// Test
         float dt=0;
@@ -61,24 +62,23 @@
                         char p = xb.cBuffer->read();
                         char i = xb.cBuffer->read();
                         char d = xb.cBuffer->read();
-                        Kp = p/256.0f;
-                        Ki = i/256.0f;
-                        Kd = d/256.0f;
+                        Kp = p/25.0f;
+                        Ki = i/25.0f;
+                        Kd = d/25.0f;
                         pc.putc('E');
-                        pc.printf("pid change\0");
+                        pc.printf("pid change");
+                        pc.putc(0);
                         curr_cmd = 0;        
                     }    
                 break;
                 
                 case 'F':
-                    if(xb.cBuffer->available() >= 2) {
+                    if(xb.cBuffer->available() >= 1) {
                         char a = xb.cBuffer->read();
-                        char b = xb.cBuffer->read();
-                        short s = (a << 8) & b;
-                        
-                        speed = s/65536.0f;
+                        speed = a/256.0f;
                         pc.putc('E');
-                        pc.printf("speed change\0");
+                        pc.printf("s = %u %f",a, speed);
+                        pc.putc(0);
                         curr_cmd = 0;   
                         
                     }
@@ -89,12 +89,14 @@
             }
         }
         
-        if(xb.cBuffer->available() > 0) {
+        if(xb.cBuffer->available() > 0 && curr_cmd == 0) {
             char cmd = xb.cBuffer->read();
             if(cmd == 'D') {
                 TFC_InitServos(0.00052,0.00122,0.02);
                 TFC_HBRIDGE_ENABLE;
                 TFC_SetMotorPWM(speed,speed);   
+                integral = 0;
+                
             } else if (cmd == 'C') {
                 TFC_SetMotorPWM(0.0,0.0);
                 TFC_HBRIDGE_DISABLE;
@@ -108,31 +110,60 @@
         
         //If we have an image ready
         if(TFC_LineScanImageReady>0) {       
-            left = 0;
-            right = 0;
+            //left = 0;
+            //right = 0;
+            int diff = 0;
+            int prev = -1;
             for(i = 63; i > 0; i--) {
                 curr_left = (int8_t)(TFC_LineScanImage0[i] >> 4) & 0xFF;
-                if(curr_left < CAM_THRESHOLD) {
+               /* if(curr_left < CAM_THRESHOLD) {
                     left = i;
                     break;
                 }
+                */
+                
+                diff = prev - curr_left;
+                if(abs(diff) >= 10 && prev != -1) {
+                    left = i;
+                    break;    
+                }
+                
+                prev = curr_left;
             }
             
+            prev = -1;
             for(i = 64; i < 128; i++) {
                 curr_right = (int8_t)(TFC_LineScanImage0[i] >> 4) & 0xFF;
+                /*
                 if(curr_right < CAM_THRESHOLD) {
                     right = i;
                     break;
                 }
+                */
+                
+                int diff = prev - curr_right;
+                if(abs(diff) >= 10 && prev != -1) {
+                    right = i;
+                    break;    
+                }
+                
+                prev = curr_right;
             }
 
+            //if((frame_counter % 10) == 0) {
             pc.putc('H');
             for(i = 0; i < 128; i++) {
                 pc.putc((int8_t)(TFC_LineScanImage0[i] >> 4) & 0xFF);    
             }
+            //}
+            //frame_counter++;
             
             measured_value = (64 - ((left+right)/2))/64.f;
             
+            pc.putc('E');
+            pc.printf("left=%u right=%u", left, right);
+            pc.putc(0);            
+            
     t.start();
     dt=t.read();
     error = desired_value - measured_value;
@@ -141,18 +172,42 @@
    output=Kp*error+Ki*integral+Kd*derivative;
    p_error=error;
    
+   
+   if(integral > 1.0f) {
+        integral = 1.0f;   
+    }
+    if(integral < -1.0f) {
+        integral = -1.0f;   
+    }
+   
+    
    if((-1.0<=output)&&(output<=1.0))
    {
       TFC_SetServo(0,output);
  
-   }
+    }
     else
- {  
-    while(1){
-       pc.putc('E');
-       pc.printf("pid has borked :( \0");
-       wait(0.2f);
-   }   
+    {
+          
+            pc.putc('E');
+    pc.printf("pid unhappy\0");
+    pc.putc('E');
+    pc.printf("out = %f p_err = %f\0",output, p_error);
+    TFC_InitServos(0.00052,0.00122,0.02);
+    //output, error, p_error, integral, derivative = 0;
+    
+    if(output >= 1.0f) {
+        TFC_SetServo(0,0.9f);
+        output = 1.0f;
+    } else {
+        TFC_SetServo(0,-0.9f);
+        output = -1.0f;
+    } 
+    
+    
+  
+    
+    
    }
        
     t.stop();
@@ -164,7 +219,7 @@
 
             //Reset image ready flag
             TFC_LineScanImageReady=0;
-            wait(0.1f);
+            wait(0.05f);
         }
     }
 }
\ No newline at end of file