Revision 3:87a5122682fa, committed 2016-11-02

Comitter:

FatCookies

Date:

Wed Nov 02 13:13:13 2016 +0000

Parent:

2:4b6f6fc84793

Child:

4:4afa448c9cce

Commit message:

init pid example using camera input

Changed in this revision

--- a/main.cpp	Fri Oct 28 10:19:09 2016 +0000
+++ b/main.cpp	Wed Nov 02 13:13:13 2016 +0000
@@ -1,120 +1,114 @@
 #include "mbed.h"
 #include "TFC.h"
-#define CAM_THRESHOLD 3000
+#define CAM_THRESHOLD 128
+ 
  
-Serial pc(USBTX,USBRX); 
 DigitalOut myled(LED1);
+//Serial pc(USBTX,USBRX);
+Serial pc(PTD3,PTD2);
+
+char curr_line[128];
+uint8_t curr_left;
+uint8_t curr_right;
+
+uint8_t right;
+uint8_t left;
+
+Timer t;
  
 int main() {
-    //Serial config
-    pc.baud(57600);
-    
-    
     TFC_Init();
-    
-    uint32_t i,t = 0;
-    //uint32_t width = 0;
-    char curr_line[129];
-    int leftSide, rightSide;
-    float centreOffset;
-    
-    /* Ensure string is null terminated */
-    curr_line[128] = 0; 
+    TFC_InitServos(0.00052,0.00122,0.02);
+    TFC_HBRIDGE_ENABLE;
+    TFC_SetMotorPWM(0.3,0.3);
     
     
-    char thres_line[16];
-    for(i = 0; i < 16; i++) {
-        thres_line[i] = 0;
-    }
- 
+    pc.baud(57600);
     
-    pc.printf("Starting camera test program\n");
+    float p_error, error;
+    float integral;
+    float measured_value, desired_value,derivative;
+    float output;
+      //tunable variables
+    float Kp, Ki,Kd;
+        Kp=0.5;
+    Ki=0.1;
+    Kd=0;
+    myled = 0;// Test
+        float dt=0;
+    p_error=0;
+    error=0;
+    integral=0;
+    measured_value= 0;
+    desired_value=0;
+    derivative=0;
+        // measured value is a float between -1.0 and 1.0
+    // desired value is always 0 ( as in car is in the middle of the road)
     
+    uint8_t i = 0;
     while(1) {
+        
         //If we have an image ready
-        if(TFC_LineScanImageReady>0)
-            {
-                //Reset image ready flag
-                TFC_LineScanImageReady=0;
-                pc.printf("\r\n");
-                //pc.printf("L:");
-                
-                //Strange thing that cycles the battery indicator
-                //Leaving in as this is a good indicator that the camera test program is running
-                if(t==4)
-                    t=0;
-                else
-                    t++;
-                TFC_SetBatteryLED_Level(t);
-                
-                //Byte sending
-                //for(i = 0; i < 128; i++) {
-                    //curr_line[i] = (int8_t)(TFC_LineScanImage0[i] >> 4) & 0xFF;
-                    //pc.printf("%02x", curr_line[i]);
-                //}
-                pc.printf("\n");
-                
-                
-                                    //NEW CENTRE DETECTION CODE
-                //Centre of track detection
-                //First, start at the centre of the 128 bit array
-                //Moving left, start at 63 and move 63 steps left (to get to 0)
-                //Moving right, start at 64 and move 63 steps right (to get to 127)
-                
-                //Move leftwards, stopping where we reach the edge of the track, and record the index where this occurred
-                for (i = 63; i > 0; i--) {
-                    if(TFC_LineScanImage0[i] < CAM_THRESHOLD) {
-                        leftSide = i;
-                        break;
-                        }
+        if(TFC_LineScanImageReady>0) {       
+            left = 0;
+            right = 0;
+            for(i = 63; i >= 0; i--) {
+                curr_left = (int8_t)(TFC_LineScanImage0[i] >> 4) & 0xFF;
+                if(curr_left < CAM_THRESHOLD) {
+                    left = i;
+                    break;
+                }
+            }
+            
+            for(i = 64; i < 128; i++) {
+                curr_right = (int8_t)(TFC_LineScanImage0[i] >> 4) & 0xFF;
+                if(curr_right < CAM_THRESHOLD) {
+                    right = i;
+                    break;
                 }
-                
-                //Then, move rightwards, stopping where we reach the edge of the track, record the index where this occurred
-                for (i = 64; i < 127; i++) {
-                    if(TFC_LineScanImage0[i] < CAM_THRESHOLD) {
-                        rightSide = i;
-                        break;
-                        }
-                }
-                
-                //centreOffset = (leftSide + rightSide)/2;
-                centreOffset = (leftSide - rightSide) / 127;
-                
-                //Send offset over serial to hosted program
-                pc.putc('E');
-                pc.printf("%f", centreOffset);
-                //pc.printf("centre: %d\n", centreOffset);
-                                ///END OF NEW CODE
-                
-                
-                //Old version below, we don't threshold on the FRDM board anymore
-                //pc.printf("0x%x\n", curr_line);
-               
-               
-               //Loop through camera pixels
-               for(i=0;i<128;i++)
-               {
-                   //If the pixel value exceeds our threshold, print a 1
-                   if (TFC_LineScanImage0[i] > CAM_THRESHOLD) 
-                   {
-                       //curr_line[i] = '1';
-                       thres_line[i/8] |= 1 << (i%8);
-                   }
-                   //Else, print a 0
-                   else
-                   {
-                      //curr_line[i] = '0';  
-                   }
-               }
-               pc.putc('I');
-                for(i = 0; i < 16; i++) {
-                    pc.putc(thres_line[i]);
-                }
-                
-                //pc.printf("Width: %d\n", width);
-                //width = 0;
-                
-                   
-               }
+            }
+
+            pc.putc('H');
+            for(i = 0; i < 128; i++) {
+                pc.putc((int8_t)(TFC_LineScanImage0[i] >> 4) & 0xFF);    
+            }
+            
+            measured_value = (64 - ((left+right)/2))/64.f;
+            
+    t.start();
+    dt=t.read();
+    error = desired_value - measured_value;
+   integral=integral+error*dt;
+   derivative=(error-p_error)/dt;
+   output=Kp*error+Ki*integral+Kd*derivative;
+   p_error=error;
+   
+   if((-1.0<=output)&&(output<=1.0))
+   {
+      TFC_SetServo(0,output);
+ 
+   }
+    else
+ {  
+    while(1){
+        myled = 1;
+        wait(0.2);
+        myled = 0;
+        wait(0.2);
+        integral=0;
+   }   
+   }
+       
+    t.stop();
+    t.reset();
+    t.start();
+ 
+            
+        
+
+            //Reset image ready flag
+            TFC_LineScanImageReady=0;
+            wait(0.1f);
+        }
     }
 }
\ No newline at end of file