File content as of revision 4:4afa448c9cce:

#include "mbed.h"
#include "TFC.h"
#include "XBEE.h"
#define CAM_THRESHOLD 128
 
 
DigitalOut myled(LED1);
//Serial pc(USBTX,USBRX);
Serial pc(PTD3,PTD2);
XBEE xb(&pc);

char curr_line[128];
uint8_t curr_left;
uint8_t curr_right;

uint8_t right;
uint8_t left;

Timer t;

char curr_cmd = 0;

float speed = 0.3;
 
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.8;
    Ki=0.6;
    Kd=0.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(curr_cmd != 0) {
            switch(curr_cmd) {
                case 'A':
                    if(xb.cBuffer->available() >= 3) {
                        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;
                        pc.putc('E');
                        pc.printf("pid change\0");
                        curr_cmd = 0;        
                    }    
                break;
                
                case 'F':
                    if(xb.cBuffer->available() >= 2) {
                        char a = xb.cBuffer->read();
                        char b = xb.cBuffer->read();
                        short s = (a << 8) & b;
                        
                        speed = s/65536.0f;
                        pc.putc('E');
                        pc.printf("speed change\0");
                        curr_cmd = 0;   
                        
                    }
                break;
                  
                default:
                break; 
            }
        }
        
        if(xb.cBuffer->available() > 0) {
            char cmd = xb.cBuffer->read();
            if(cmd == 'D') {
                TFC_InitServos(0.00052,0.00122,0.02);
                TFC_HBRIDGE_ENABLE;
                TFC_SetMotorPWM(speed,speed);   
            } else if (cmd == 'C') {
                TFC_SetMotorPWM(0.0,0.0);
                TFC_HBRIDGE_DISABLE;
            } else if(cmd == 'A') {
                curr_cmd = 'A';
            } else if(cmd == 'F') {
                curr_cmd = 'F';    
            }
            
        }
        
        //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){
       pc.putc('E');
       pc.printf("pid has borked :( \0");
       wait(0.2f);
   }   
   }
       
    t.stop();
    t.reset();
    t.start();
 
            
        

            //Reset image ready flag
            TFC_LineScanImageReady=0;
            wait(0.1f);
        }
    }
}