File content as of revision 31:1a06c9e1985e:

//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"

// Serial
#if USE_COMMS
    Serial pc(PTD3,PTD2);
    XBEE xb(&pc);
#endif

// PID Timer
Timer t;

//Speed Sensors interupts and timers
InterruptIn leftHallSensor(D0);
InterruptIn rightHallSensor(D2);
Timer t1;
Timer t2;

//testing timer
Timer lapTimer;


int main() {
    //Set up TFC driver stuff
    TFC_Init();
    ALIGN_SERVO;
   
    #if USE_COMMS
        //Setup baud rate for serial link, do not change!
        pc.baud(BAUD_RATE);
    #endif
    
    //Initialise/reset PID variables
    initVariables();
    initSpeedSensors();
              
    while(1) {
        
        #if USE_COMMS
            handleComms();
        #endif
        
        //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(CLOSE_CAMERA, left, right);
            
            // Check if car is at the stop line
            handleStartStop();
            
            #if USE_COMMS
                // Send the line scan image over serial
                sendImage();
            #endif
            
            
            //Reset image ready flag
            TFC_LineScanImageReady=0;
            
            // Slow down, adjust PID values and enable differential before corners.
            //handleCornering();
            
            // Run the PID controllers and adjust steering/motor accordingly
            PIDController();
            
            
                
            
            #if USE_COMMS
                // Send the wheel speeds over serial
                sendSpeeds();
            #endif
            

            
            
            
        }
    }
}

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, 0.01f, 0.f, 0.f);
    initPID(&right_motor_pid, 0.01f, 0.f, 0.f);
    
    right_motor_pid.desired_value=0;
    left_motor_pid.desired_value=0;
    
    // intialise the maximum speed interms of the angular speed.
    valBufferIndex = 0;
    speed = 50;
    
    //Start stop
    startstop = 0;
    seen = false;
    
    // Turning
    turning = 0;
    keepTurning = 0;
    slow = false;
    
    wL = 0;
    wR = 0;
    prevL = 0;
    prevR = 0;
}

void initPID(pid_instance* pid, float Kp, float Ki, float Kd) {
    pid->Kp = Kp;
    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;
}

    bool leftSeen;
    bool rightSeen;

//Function which calcuates how far to the left/right of the centre of the track the car is
//Takes data from either camera, and passes some variables by reference, which will hold
//the indices holding the locations of the left and right edges of the track
inline float findCentreValue(volatile uint16_t * cam_data, uint8_t &l, uint8_t &r) {
   
    diff = 0; //Holds difference in intensity between consecutive pixels
    prev = -1; //Holds index of last inspected pixel

    //Used for crossroads navigation, holds info on which edges of the track are observed
    leftSeen = false;
    rightSeen = false;
    
    //Starting in the middle index, step left, inspecting the the edge of the track
    for(i = 63; i > 0; i--) {
        curr_left = (uint8_t)(cam_data[i] >> 4) & 0xFF;              
        diff = prev - curr_left;
        //Check incorporates a combination of looking at the difference in intensities
        //and whether the pixels intensity is less than a threshold, corresponding to the black edge
        if(abs(diff) >= CAM_DIFF && curr_left <= CAM_THRESHOLD && prev != -1) {
            l = i; //Record the index where the edge is observed
            leftSeen = true;
            break;
        }
        prev = curr_left; //Update previous value for the loop
    }
    
    prev = -1;
    //As before, start in the middle but this time step rightwards in the image
    for(i = 64; i < 128; i++) {
        curr_right = (uint8_t)(cam_data[i] >> 4) & 0xFF;
        int diff = prev - curr_right;
        if(abs(diff) >= CAM_DIFF && curr_right <= CAM_THRESHOLD && prev != -1) {
            r = i;
            rightSeen = true;
            break;
        }
        prev = curr_right;
    }
    
    //If both edges are not visible, we are likely in a crossroads
    if(!rightSeen && !leftSeen) {
        sendString("lost edges");
        ALIGN_SERVO; //Straighten wheels so we go straight through the crossroads
        servo_pid.integral = 0;
    }
    
    //Calculate how left/right from the centre line we are
    return (64 - ((l+r)/2))/64.f;
}

//Unused function currently
//Was used to establish whether we are in a corner, by inspecting a buffer of
//centre line values
inline void handleCornering() {
    
    //Get current value of how left/right of centre line we are on the track
    float lookaheadMeasuredValue = findCentreValue(LOOKAHEAD_CAMERA, farLeft, farRight);
    
    measuredValBuffer[frame_counter % 64] = servo_pid.measured_value;
    
    int count = 0;
    for(i = 0; i < 10; i++) {
        //Step through the buffer, using modulus operator
        float val = abs(measuredValBuffer[(frame_counter - i) % 64]);
        if(val > 0.09) { //If the value exceeds a certain value (obtained experimentally), we are in a corner
            count++;
        }
    } 
    
    /*if(!turning && abs(lookaheadMeasuredValue) > 0.11f){
        TFC_SetMotorPWM(0.4,0.4);    
    }
    */
    
    if(false) {
        
        //default
        //TFC_SetMotorPWM(0.4,0.4);
        
        //dutyCycleCorner(speed,servo_pid.output);
        
        //may want to have just to set cornering speed at different if going to be slowing down for cornering.
        //dutyCycleCorner(float cornerspeed, servo_pid.output);
        //dutyCycleCorner(speed, servo_pid.output);
        
        sensorCorner(left_motor_pid.desired_value, right_motor_pid.desired_value , servo_pid.output, speed);  
    }
    
    /*
    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);
            }
               
        }
    }
    */
    
}

//Unused function currently
//Was used to estimate whether the stop marker was seen
inline float getLineEntropy() {
    float entropy = 0;
    float last = (int8_t)(CLOSE_CAMERA[0] >> 4) & 0xFF;
    for(int i = 1; i < 128; i++) {
        entropy += abs(last - ((int8_t)(CLOSE_CAMERA[i] >> 4) & 0xFF));
    }
    return entropy;
}

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
    // Read the angular velocity of both wheels
    
    prevL = wL;
    prevR = wR;
    
    wL=Get_Speed(Time_L);
    wR=Get_Speed(Time_R);
    
    // Check if left wheel is slipping/giving an abnormal reading and ignore reading
    if(wL - prevL < 1.2/0.025) { //<3 magic numbers: 48....?
        left_motor_pid.measured_value = wL;
    }
    if(wR - prevR < 1.2/0.025) {
        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);
        //ALIGN_SERVO;
        //Could cause the car to be travelling along one side of the track rather than in the middle
        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;
        }
    }    
    
    
    if(left_motor_pid.output > 1.0f) {
       left_motor_pid.output = 1.0f; 
    }
    if(left_motor_pid.output < 0.0f) {
       left_motor_pid.output = 0.0f; 
    }
    
    if(right_motor_pid.output > 1.0f) {
        right_motor_pid.output = 1.0f;
    }
    if(right_motor_pid.output < 0.0f) {
        right_motor_pid.output = 0.0f;
    }
    
    TFC_SetMotorPWM(left_motor_pid.output,right_motor_pid.output);
    
    
    t.stop();
    t.reset();
    t.start();
}

inline void handleStartStop() {

    //Function to detect the NXP cup stop marker
    
    int slower = 0; //Only send a string every few frames
    int difference = 0; //Holds the difference between intensities of consecutive pixels
    int lastPixel, currentPixel, transitionsSeen;
    lastPixel = -1;
    transitionsSeen = 0;
    //Starting  near the left edge, step right, counting transitions.
    //If there are several (exact value varies, best established experimentally), it is the marker
    for(int i = 30; i < 98; i++) {
        currentPixel = (int)(CLOSE_CAMERA[i] >> 4) & 0xFF;
        difference = lastPixel - currentPixel;
        if(abs(difference) > 10 && lastPixel != -1){ //transition seen, increment counter
            transitionsSeen++;
            i+=5;
        }
        lastPixel = currentPixel;
    }
    //Was used to send an indication that the marker was seen, useful for debugging
    //if (slower % 1000 == 0) {
        //sendString("Transitions seen: %d", transitionsSeen);
    //}
    //slower++;
    if(transitionsSeen >= 5) {
        //Stop the car!
        sendString("Start/stop seen");
        TFC_SetMotorPWM(0.f,0.f);
        TFC_HBRIDGE_DISABLE;
        lapTime();
    }
}


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

#if USE_COMMS
void sendBattery() {
    
    if(frame_counter % 256 == 0) {
        float level = TFC_ReadBatteryVoltage() * 6.25;          
        pc.putc('J');
        byte_float_union._float = level;
        pc.putc(byte_float_union.bytes[0]);
        pc.putc(byte_float_union.bytes[1]);
        pc.putc(byte_float_union.bytes[2]);
        pc.putc(byte_float_union.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);
}

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)(CLOSE_CAMERA[i] >> 4) & 0xFF);    
            }    
        } else {
            for(i = 0; i < 128; i++) {
                pc.putc((int8_t)(LOOKAHEAD_CAMERA[i] >> 4) & 0xFF);    
            } 
        }
        sendBattery();
    }    
       
    frame_counter++;  
}

inline void sendSpeeds() {
  
    /*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');
    byte_float_union._float = wL * WHEEL_RADIUS;//left_motor_pid.output; //
    pc.putc(byte_float_union.bytes[0]);
    pc.putc(byte_float_union.bytes[1]);
    pc.putc(byte_float_union.bytes[2]);
    pc.putc(byte_float_union.bytes[3]);
    byte_float_union._float = wR * WHEEL_RADIUS; // right_motor_pid.output; //
    pc.putc(byte_float_union.bytes[0]);
    pc.putc(byte_float_union.bytes[1]);
    pc.putc(byte_float_union.bytes[2]);
    pc.putc(byte_float_union.bytes[3]);    

}


inline void handleComms() {
    if(curr_cmd != 0) {
            switch(curr_cmd) {
                case 'A':
                    if(xb.cBuffer->available() >= 12) {
                    
                        byte_float_union.bytes[0] = xb.cBuffer->read();
                        byte_float_union.bytes[1] = xb.cBuffer->read();
                        byte_float_union.bytes[2] = xb.cBuffer->read();
                        byte_float_union.bytes[3] = xb.cBuffer->read();
                        servo_pid.Kp = byte_float_union._float; 
                        
                        byte_float_union.bytes[0] = xb.cBuffer->read();
                        byte_float_union.bytes[1] = xb.cBuffer->read();
                        byte_float_union.bytes[2] = xb.cBuffer->read();
                        byte_float_union.bytes[3] = xb.cBuffer->read();
                        servo_pid.Ki = byte_float_union._float; 
                        
                        byte_float_union.bytes[0] = xb.cBuffer->read();
                        byte_float_union.bytes[1] = xb.cBuffer->read();
                        byte_float_union.bytes[2] = xb.cBuffer->read();
                        byte_float_union.bytes[3] = xb.cBuffer->read();
                        servo_pid.Kd = byte_float_union._float; 
                        
                        sendString("pid= Kp: %f, Ki: %f, Kd: %f",  servo_pid.Kp, servo_pid.Ki, servo_pid.Kd);
                        
                        curr_cmd = 0;        
                    }    
                break;
                
                case 'F':
                    if(xb.cBuffer->available() >= 1) {
                        char a = xb.cBuffer->read();
                        speed = a;
                        sendString("s = %u %f",a, speed);
                        curr_cmd = 0;   
                        right_motor_pid.desired_value=speed;
                        left_motor_pid.desired_value=speed;
                    }
                break;
                  
                default:
                // Unrecognised command
                curr_cmd = 0;
                break; 
            }
        }
        
        if(xb.cBuffer->available() > 0 && curr_cmd == 0) {
            //Start car
            char cmd = xb.cBuffer->read();
            if(cmd == 'D') {
                ALIGN_SERVO;
                right_motor_pid.desired_value=speed;
                left_motor_pid.desired_value=speed;
                TFC_HBRIDGE_ENABLE;
                
                    
                servo_pid.integral = 0;
                lapTimer.start();
                lapNo =0;
                
            } else if (cmd == 'C') {
                TFC_SetMotorPWM(0.0,0.0);
                right_motor_pid.desired_value=0;
                right_motor_pid.measured_value = 0;
                wR = 0;
                prevR = 0;
                
                left_motor_pid.desired_value=0;
                left_motor_pid.measured_value = 0;
                wL = 0;
                prevL = 0;
                
                TFC_HBRIDGE_DISABLE;
                endTest();
            } else if(cmd == 'A') {
                curr_cmd = 'A';
            } else if(cmd == 'F') {
                curr_cmd = 'F';    
            } else if(cmd == 'K') {
                sendCam = ~sendCam;    
            }
            
        }
}

float testSpeed(float speed)
{
    // search: Speed Increase
    // every time the car sees the stop start the speed of the car will increase
    // this can occur on stop start trigger.
    // may need to send the speed back to the telemetry.
    if (speed>0.4)
    {
        speed+=0.05;
     }
     
     else
     {
       speed+=0.1;   
         
     }   
     
     
     sendString("s = %f", speed);
     return speed;
        
}



int lapTime()
{   
    // function which sends the lap time back to the telemetry.
    float newTime= lapTimer.read();
    lapNo += 1;
    float lapTime= newTime-oldTime;
    float avgTime= newTime/lapNo;
    
    sendString("For lap number: %d  Lap Time: %f  Avergae time: %f \n\r", lapNo,lapTime,avgTime);
    
     // OH WHAT UP IT'S DAT BOI!!!!
    return 0;
}


void endTest()
{// This runs when the car has stopped, this should give the final elapsed time and othere things. this also stops the timer
    
    float time= lapTimer.read();
    
    sendString("Laps done: %d  Time elapsed: %f   Average time: %f \n\r",lapNo, time,float (time/lapNo));
    lapTimer.stop();
    
    
}                                              





#endif