File content as of revision 16:81cdffd8c5d5:

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

#define BAUD_RATE 57600
#define CAM_THRESHOLD 109
#define CAM_DIFF 10
#define WHEEL_RADIUS 0.025f

#define RIGHT_MOTOR_COMPENSATION_RATIO 1.1586276

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;
uint8_t farRight;
uint8_t farLeft;
int diff;
int prev;
int i = 0;
float measuredValBuffer[64];
uint8_t valBufferIndex;

//Communication Variables
uint8_t sendCam = 0;
int frame_counter = 0;
char curr_cmd = 0; // Current comms command

//Some PID variables
pid_instance servo_pid;
pid_instance right_motor_pid;
pid_instance left_motor_pid;
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();



float speed = 0.3;


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

void sendString(const char *format, ...);
void initVariables();
inline void handleComms();
inline float findCentreValue(volatile uint16_t * cam_data, uint8_t &left, uint8_t &right);
inline void PIDController();
inline void sendImage();
inline void sendSpeeds();
void handlePID(pid_instance *pid);
void initPID(pid_instance* pid);
inline void handleStartStop();
inline void recordMeasuredVal();
void sendBattery();
inline float getLineEntropy();
 

int main() {
    //Set up TFC driver stuff
    TFC_Init();
    TFC_InitServos(0.00052,0.00122,0.02);
   
    //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) {
            /* Find the bounds of the track and calculate how close we are to
             * the centre */        
            servo_pid.measured_value = findCentreValue(TFC_LineScanImage0, left, right);
            recordMeasuredVal();
            
            // Read the angular velocity of both wheels
            wL=Get_Speed(Time_L);
            wR=Get_Speed(Time_R);
            
            // Run the PID controllers and adjust steering/motor accordingly
            PIDController();
            
            // Send the line scan image over serial
            sendImage();
            
            // Send the wheel speeds over serial
            sendSpeeds();
            
            // Check if car is at the stop line
            //handleStartStop();
            
            
            //Reset image ready flag
            TFC_LineScanImageReady=0;
        }
    }
}

void sendBattery() {
    
    if(frame_counter % 256 == 0) {
        float level = TFC_ReadBatteryVoltage() * 6.25;
         union {
            float a;
            unsigned char bytes[4];
        } thing;
        
        pc.putc('J');
        thing.a = level;
        pc.putc(thing.bytes[0]);
        pc.putc(thing.bytes[1]);
        pc.putc(thing.bytes[2]);
        pc.putc(thing.bytes[3]);
    }
}

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 initPID(pid_instance* pid, float Kp, float Ki, float Kd) {
    pid->Kp = Kd;
    pid->Ki = Ki;
    pid->Kd = Kd;
    pid->dt = 0;
    pid->p_error = 0;
    pid->pid_error = 0;
    pid->integral = 0;
    pid->measured_value = 0;
    pid->desired_value = 0;
    pid->derivative = 0;
}

void initVariables() {
    // Initialise three PID controllers for the servo and each wheel.
    initPID(&servo_pid, 2.2f, 0.6f, 0.f);
    initPID(&left_motor_pid, 1.0f, 0.f, 0.f);
    initPID(&right_motor_pid, 1.0f, 0.f, 0.f);
    
    valBufferIndex = 0;
}

int turning = 0;
int keepTurning = 0;
bool slow = false;
inline void recordMeasuredVal() {
    
    float aheadForward = findCentreValue(TFC_LineScanImage1, farLeft, farRight);
    
    measuredValBuffer[frame_counter % 64] = servo_pid.measured_value;
    
    int count = 0;
    for(i = 0; i < 10; i++) {
        float val = abs(measuredValBuffer[(frame_counter - i) % 64]);
        if(val > 0.09) {
            count++;
        }
    } 
    
    if(!turning && abs(aheadForward) > 0.11f){
        TFC_SetMotorPWM(0.4,0.4);    
    }
    
    if(turning) {
        dutyCycleCorner(0.4,servo_pid.output);
        //sensorCorner(left_motor_pid.desired_value, right_motor_pid.desired_value , servo_pid.output, 50);  
    }
    
    if(abs(servo_pid.measured_value) > 0.11f){
        if(!turning) {            
            turning = 1;
        } else {
            turning++;   
        }
        
    } else {
        if(turning) {
            if(keepTurning == 0 || count > 6) {
                keepTurning++;
            } else {
                //sendString("stop turning turned=%d",turning);
                keepTurning = 0;
                turning = 0;    
                TFC_SetMotorPWM(speed,speed);
            }
               
        }
    }
    
}

inline void sendImage() {
     //Only send 1/3 of camera frames to GUI program
    if((frame_counter % 3) == 0) {
        pc.putc('H');
        if(sendCam == 0) {            
            for(i = 0; i < 128; i++) {
                pc.putc((int8_t)(TFC_LineScanImage0[i] >> 4) & 0xFF);    
            }    
        } else {
            for(i = 0; i < 128; i++) {
                pc.putc((int8_t)(TFC_LineScanImage1[i] >> 4) & 0xFF);    
            } 
        }
        sendBattery();
    }    
       
    frame_counter++;  
}

inline float getLineEntropy() {
    float entropy = 0;
    float last = (int8_t)(TFC_LineScanImage0[0] >> 4) & 0xFF;
    for(int i = 1; i < 128; i++) {
        entropy += abs(last - ((int8_t)(TFC_LineScanImage0[i] >> 4) & 0xFF));
    }
    return entropy;
}

inline void sendSpeeds() {
    union {
        float a;
        unsigned char bytes[4];
    } thing;
    
    float en = getLineEntropy();
    
    if(onTrack) {
        if(en <= 14000) {
            onTrack = false;
            sendString("offfffffffffffff");    
            TFC_SetMotorPWM(0.0,0.0);
            TFC_HBRIDGE_DISABLE;
        }
    } else {
        if(en > 14000) {
            onTrack = true;
            sendString("ON TRACK");
        }    
    }
    

    pc.putc('B');
    thing.a = en;//wL * WHEEL_RADIUS;
    pc.putc(thing.bytes[0]);
    pc.putc(thing.bytes[1]);
    pc.putc(thing.bytes[2]);
    pc.putc(thing.bytes[3]);
    thing.a = en; //wR * WHEEL_RADIUS;
    pc.putc(thing.bytes[0]);
    pc.putc(thing.bytes[1]);
    pc.putc(thing.bytes[2]);
    pc.putc(thing.bytes[3]);    
}


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;
                        sendString("pid= Kp: %f, Ki: %f, Kd: %f, p: %u, i: %u, d: %u", servo_pid.Kp, servo_pid.Ki, servo_pid.Kd, p, i, d);
                        
                        curr_cmd = 0;        
                    }    
                break;
                
                case 'F':
                    if(xb.cBuffer->available() >= 1) {
                        char a = xb.cBuffer->read();
                        speed = a/256.0f;
                        TFC_SetMotorPWM(speed,speed);  
                        sendString("s = %u %f",a, speed);
                        curr_cmd = 0;   
                    }
                break;
                  
                default:
                // Unrecognised command
                curr_cmd = 0;
                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';    
            } else if(cmd == 'K') {
                sendCam = ~sendCam;    
            }
            
        }
}

inline float findCentreValue(volatile uint16_t * cam_data, uint8_t &l, uint8_t &r) {
   
    diff = 0;
    prev = -1;
    for(i = 63; i > 0; i--) {
        curr_left = (int8_t)(cam_data[i] >> 4) & 0xFF;              
        diff = prev - curr_left;
        if(abs(diff) >= 10 && curr_left <= 100 && prev != -1) {
            l = i;
            break;
        }
        prev = curr_left;
    }
    
    prev = -1;
    for(i = 64; i < 128; i++) {
        curr_right = (int8_t)(cam_data[i] >> 4) & 0xFF;
        int diff = prev - curr_right;
        if(abs(diff) >= 10 && curr_right <= 100 && prev != -1) {
            r = i;
            break;
        }
        prev = curr_right;
    }
    
    //Calculate how left/right we are   
    return (64 - ((l+r)/2))/64.f;
}


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() {
    // update motor measurements
    left_motor_pid.measured_value = wL;
    right_motor_pid.measured_value = wR;
    
    //PID Stuff!
    t.start();
    handlePID(&servo_pid);
    handlePID(&left_motor_pid);
    handlePID(&right_motor_pid);

    if((-1.0 <= servo_pid.output) && (servo_pid.output <= 1.0))
    {
        TFC_SetServo(0, servo_pid.output);
    }
    else //Unhappy PID state
    {        
        //sendString("out = %f p_err = %f", servo_pid.output, servo_pid.p_error);
        TFC_InitServos(0.00052, 0.00122, 0.02);
        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;
        }
    }    
    
    
    t.stop();
    t.reset();
    t.start();
}

inline void handleStartStop() {
    //Hacky way to detect the start/stop signal
    if(right - left < 60) {
        sendString("START STOP!! &d",startstop); 
        
        if(seen) {
            seen = false;
        } else {
            startstop++;
            seen = true;    
        }    
        //If we've done 5 laps, stop the car
        if(startstop >= 1) {
            TFC_SetMotorPWM(0.f,0.f);
            TFC_HBRIDGE_DISABLE;
            startstop = 0;      
        }
    }    
}


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