File content as of revision 3:87a5122682fa:

#include "mbed.h"
#include "TFC.h"
#define CAM_THRESHOLD 128
 
 
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() {
    TFC_Init();
    TFC_InitServos(0.00052,0.00122,0.02);
    TFC_HBRIDGE_ENABLE;
    TFC_SetMotorPWM(0.3,0.3);
    
    
    pc.baud(57600);
    
    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) {       
            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;
                }
            }

            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);
        }
    }
}