File content as of revision 7:ad893fc41b95:

#include "mbed.h"
#include "TFC.h"
#include "XBEE.h"
#define CAM_THRESHOLD 109
 
 
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 frame_counter = 0;
 
 int startstop = 0;
bool seen = false;
 
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=125/25.0f;
    Ki=12.0f/25.0f;
    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/25.0f;
                        Ki = i/25.0f;
                        Kd = d/25.0f;
                        pc.putc('E');
                        pc.printf("pid change");
                        pc.putc(0);
                        curr_cmd = 0;        
                    }    
                break;
                
                case 'F':
                    if(xb.cBuffer->available() >= 1) {
                        char a = xb.cBuffer->read();
                        speed = a/256.0f;
                        TFC_SetMotorPWM(speed,speed);  
                        pc.putc('E');
                        pc.printf("s = %u %f",a, speed);
                        pc.putc(0);
                        curr_cmd = 0;   
                        
                    }
                break;
                  
                default:
                break; 
            }
        }
        
        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;
            } 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;
            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) {
                    left = i;
                    break;
                }
                */
                
                diff = prev - curr_left;
                if(abs(diff) >= 10 && curr_left <= 100 && 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 && curr_right <= 100 && prev != -1) {
                    right = i;
                    break;    
                }
                
                prev = curr_right;
            }

            if((frame_counter % 3) == 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;
            
            if(right - left < 60) {
                pc.putc('E');
                pc.printf("START STOP!! &d",startstop);
                pc.putc(0);            
                
                if(seen) {
                    seen = false;
                } else {
                    startstop++;
                    seen = true;    
                }    
            
            if(startstop >= 5) {
                TFC_SetMotorPWM(0.0,0.0);
                TFC_HBRIDGE_DISABLE;
                startstop = 0;      
            }
            
            } 
            
    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(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
    {
          
            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();
    t.reset();
    t.start();
 
            
        

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