File content as of revision 9:aa2ce38dec6b:

//Autonomous Car GDP controller
//Written by various group members
//Commented & cleaned up by Max/Adam

//To-do
// -Change xbee transmission to non-blocking
// -Improve start/stop detection and resultant action (setting PID values?)

#include <stdarg.h>
#include <stdio.h>

#include "mbed.h"
#include "TFC.h"
#include "XBEE.h"
#include "angular_speed.h"

#define BAUD_RATE 57600
#define CAM_THRESHOLD 109
#define CAM_DIFF 10

//Serial pc(USBTX,USBRX);
Serial pc(PTD3,PTD2);
XBEE xb(&pc);

//Woo global variables!
bool onTrack;
char curr_line[128];
uint8_t curr_left;
uint8_t curr_right;
uint8_t right;
uint8_t left;
int8_t leftChange;
int8_t rightChange;
int diff;
int prev;
int i = 0;
float measuredValBuffer[5];
uint8_t valBufferIndex;

//Some PID variables
float Kp;
float Ki;
float Kd;
float dt;
float p_error;
float pid_error;
float integral;
float measured_value, desired_value, derivative;
float output;
Timer t;

//Speed Sensors variables
InterruptIn leftHallSensor(D0);
InterruptIn rightHallSensor(D2);
Timer t1;
Timer t2;
volatile float Time_L,Time_R;
float wL, wR;
void GetTime_L();
void GetTime_R();
inline void initSpeedSensors();

// Current comms command
char curr_cmd = 0;

float speed = 0.3;
int frame_counter = 0;

//Hacky start/stop signal detection
int startstop = 0;
bool seen = false;

void sendString(const char *format, ...);
void initVariables();
inline void handleComms();
inline float findCentreValue();
inline void PIDController();
inline void sendImage();

int main() {
    //Set up TFC driver stuff
    TFC_Init();
    TFC_InitServos(0.00052,0.00122,0.02);
    //Old things to make the car move at run-time
    //Should tie these to a button for the actual races
    //TFC_HBRIDGE_ENABLE;
    //TFC_SetMotorPWM(0.3,0.3);
    
    //Setup baud rate for serial link, do not change!
    pc.baud(BAUD_RATE);
    
    //Initialise/reset PID variables
    initVariables();
    initSpeedSensors();
    
    while(1) {
        
        handleComms();
        
        //If we have an image ready
        if(TFC_LineScanImageReady>0) {       
            measured_value = findCentreValue();
            
            PIDController();
            
            sendImage();
            
            //Hacky way to detect the start/stop signal
            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 we've done 5 laps, stop the car
                if(startstop >= 1) {
                    TFC_SetMotorPWM(0.0,0.0);
                    TFC_HBRIDGE_DISABLE;
                    startstop = 0;      
                }
            }
            
            //Reset image ready flag
            TFC_LineScanImageReady=0;
        }
    }
}

void sendString(const char *format, ...) {
    va_list arg;

    pc.putc('E');
    va_start (arg, format); 
    pc.vprintf(format,arg);
    va_end (arg);
    pc.putc(0);
}

void initVariables() {
    //Tunable PID variables
    Kp = 125   / 25.0f;
    Ki = 12.0f / 25.0f;
    Kd = 0.0f;
    dt = 0;
    p_error = 0;
    pid_error = 0;
    integral = 0;
    measured_value = 0;
    desired_value = 0;
    derivative = 0;
    
    valBufferIndex = 0;
    //Measured value is a float between -1.0 and 1.0 (from left to right)
    //Desired value is always 0.0 (as in, car is in the middle of the road)
}

inline void sendImage() {
     //Only send 1/3 of camera frames to GUI program
    if((frame_counter % 3) == 0) {
        pc.putc('H');
        for(i = 0; i < 128; i++) {
            pc.putc((int8_t)(TFC_LineScanImage0[i] >> 4) & 0xFF);    
        }
        
        wL=Get_Speed(Time_L);
        wR=Get_Speed(Time_R);
        sendString("wL = %f, wR = %f",wL,wR);
    }
    frame_counter++;
}

inline void handleComms() {
    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, Kp: %f, Ki: %f, Kd: %f, p: %u, i: %u, d: %u", Kp, Ki, Kd, p, i, d);
                        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';    
            }
            
        }
}
inline float findCentreValue() {
    float measuredValue;
   
    diff = 0;
    prev = -1;
    leftChange = left;
    for(i = 63; i > 0; i--) {
        curr_left = (int8_t)(TFC_LineScanImage0[i] >> 4) & 0xFF;              
        diff = prev - curr_left;
        if(abs(diff) >= 10 && curr_left <= 100 && prev != -1) {
            left = i;
            break;
        }
        prev = curr_left;
    }
    
    prev = -1;
    rightChange = right;
    for(i = 64; i < 128; i++) {
        curr_right = (int8_t)(TFC_LineScanImage0[i] >> 4) & 0xFF;
        int diff = prev - curr_right;
        if(abs(diff) >= 10 && curr_right <= 100 && prev != -1) {
            right = i;
            break;
        }
        prev = curr_right;
    }
    
    //Calculate how left/right we are
    measuredValue = (64 - ((left+right)/2))/64.f;
    measuredValBuffer[valBufferIndex % 5] = measuredValue;
    valBufferIndex++;
    
    return measuredValue;
}

inline void PIDController() {
    //PID Stuff!
    t.start();
    dt = t.read();
    pid_error = desired_value - measured_value;
    integral = integral + pid_error * dt;
    derivative = (pid_error - p_error) / dt;
    output = Kp * pid_error + Ki * integral + Kd * derivative;
    p_error = pid_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 //Unhappy PID state
    {        
        pc.putc('E');
        pc.printf("pid unhappy");
        pc.putc(0);
        pc.putc('E');
        pc.printf("out = %f p_err = %f", output, p_error);
        pc.putc(0);
        TFC_InitServos(0.00052, 0.00122, 0.02);
        //output, pid_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();
}



inline void initSpeedSensors() {
    t1.start();
    t2.start();
    
    //Left and Right are defined looking at the rear of the car, in the direction the camera points at.
    leftHallSensor.rise(&GetTime_L);
    rightHallSensor.rise(&GetTime_R);  
}

void GetTime_L(){
    Time_L=t1.read_us();
    t1.reset();
}

void GetTime_R(){
    Time_R=t2.read_us();
    t2.reset();
}