File content as of revision 12:da96e2f87465:

//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"
#include "motor.h"
#include "main.h"

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

#define RIGHT_MOTOR_COMPENSATION_RATIO 1.1586276

//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
pid_instance servo_pid;
Timer t;

float right_p;
float right_error;
float right_perror;
float right_measured_value, right_desired_value;
float right_output;

float left_p;
float left_error;
float left_perror;
float left_measured_value, left_desired_value;
float left_output;

//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) {       
            servo_pid.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
    servo_pid.Kp = 125   / 25.0f;
    servo_pid.Ki = 12.0f / 25.0f;
    servo_pid.Kd = 0.0f;
    servo_pid.dt = 0;
    servo_pid.p_error = 0;
    servo_pid.pid_error = 0;
    servo_pid.integral = 0;
    servo_pid.measured_value = 0;
    servo_pid.desired_value = 0;
    servo_pid.derivative = 0;
    
    valBufferIndex = 0;
    
    // motor p controller init
    right_p = 1.0;
    right_error = 0.0;
    right_perror = 0.0;
    right_measured_value = 0.0;
    right_desired_value = 0.0;
    right_output = 0.0;
    left_p = 1.0;
    left_error = 0.0;
    left_perror = 0.0;
    left_measured_value = 0.0;
    left_desired_value = 0.0;
    left_output = 0.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 = wL/3.0f;
        wR= wR/3.0f;
        sendString("wL = %f, wR = %f",wL,wR);
        wL = 0;
        wR = 0;
        
             union {
        float a;
        unsigned char bytes[4];
    } thing;
    thing.a = TFC_ReadBatteryVoltage();
    pc.putc('J');        
    pc.putc(thing.bytes[0]);
    pc.putc(thing.bytes[1]);
    pc.putc(thing.bytes[2]);
    pc.putc(thing.bytes[3]);
    }
        wL+=Get_Speed(Time_L);
        wR+=Get_Speed(Time_R);
    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();
                        servo_pid.Kp = p/25.0f;
                        servo_pid.Ki = i/25.0f;
                        servo_pid.Kd = d/25.0f;
                        pc.putc('E');
                        pc.printf("pid change, Kp: %f, Ki: %f, Kd: %f, p: %u, i: %u, d: %u", servo_pid.Kp, servo_pid.Ki, servo_pid.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(RIGHT_MOTOR_COMPENSATION_RATIO*speed,speed);
                servo_pid.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;
}

void handlePID(pid_instance *pid) {
    pid->dt = t.read();
    pid->pid_error = pid->desired_value - pid->measured_value;
    pid->integral = pid->integral + pid->pid_error * pid->dt;
    pid->derivative = (pid->pid_error - pid->p_error) / pid->dt;
    pid->output = pid->Kp * pid->pid_error + pid->Ki * pid->integral + pid->Kd * pid->derivative;
    pid->p_error = pid->pid_error;
    
    if(pid->integral > 1.0f) {
        pid->integral = 1.0f;   
    }
    if(pid->integral < -1.0f) {
        pid->integral = -1.0f;   
    }
}

inline void PIDController() {
    //PID Stuff!
    t.start();
    handlePID(&servo_pid);

    if((-1.0 <= servo_pid.output) && (servo_pid.output <= 1.0))
    {
        TFC_SetServo(0, servo_pid.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", servo_pid.output, servo_pid.p_error);
        pc.putc(0);
        TFC_InitServos(0.00052, 0.00122, 0.02);
        //output, pid_error, p_error, integral, derivative = 0;
        
        if(servo_pid.output >= 1.0f) {
            TFC_SetServo(0, 0.9f);
            servo_pid.output = 1.0f;
        } else {
            TFC_SetServo(0, -0.9f);
            servo_pid.output = -1.0f;
        }
    }
   
    /*
    right_error = right_desired_value - right_measured_value;
    right_output = right_p * right_error;
    right_perror = right_error;
    
    left_error = left_desired_value - left_measured_value;
    left_output = left_p * left_error;
    left_perror = left_error;
    
    TFC_SetMotorPWM(right_output,left_output);
    
    dutyCycleCorner(speed,output);
    */
    
    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();
}