File content as of revision 51:6a84fbc404c8:

//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"
#include "MMA8451Q.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 for laptimes
Timer lapTimer;

MMA8451Q acc(SDA, SCL, MMA8451_I2C_ADDRESS);



int loop = 0;
float accc = 0.f;

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();
    
    #if !(USE_COMMS)
        buttonStartup();
    #endif
    
              
    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
            
            if(loop % 10 == 0) {
                accc = checkAcc();
            }
            
            if(accc < accTheshold) {
                handleStartStop(); 
            } else {    
                //sendString("up %f", accc);    
            }
            
            
            
            
            #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
            

            
        //wait_ms(25);    
            
        }
    }
}

void buttonStartup() {
    TFC_SetBatteryLED(led_values[b_pressed % 4]);
    while(1) {
        //handleComms();
        
        // 2 bit is broken = max value is 13
        uint8_t dip = TFC_GetDIP_Switch();
        
        // 1 on press, 0 otherwise
        uint8_t button_a = TFC_ReadPushButton(0);
        uint8_t button_b = TFC_ReadPushButton(1);
        
        // -1 to 1, turning clockwise INCREASES value
        float pot_a = TFC_ReadPot(0);
        float pot_b = TFC_ReadPot(1);
        
        if(button_b && !bDown) {
            bDown = true;
            continue;        
        }
        if(!button_b && bDown) {
            b_pressed++;
            TFC_SetBatteryLED(led_values[b_pressed % 4]);
            bDown = false;
            continue;
        }
        
        if(button_a && !aDown) {
            aDown = true;
            continue;        
        }
        if(!button_a && aDown) {
            
            TFC_SetBatteryLED(0);
            aDown = false;
            
            int choice = b_pressed % 4;
            switch(choice) {
                 case 0:
                     initPID(&servo_pid, 2.2f, 0.6f, 0.f);
                     if(!torque){
                     speed = 40;
                     }
                     else{
                        speed = 0.4;
                     }
                     ed_tune = man_tuner;
                    break;
                case 1:
                     initPID(&servo_pid, 2.2f, 0.6f, 0.f);
                     if(!torque){
                     speed = 65;
                     }
                     else{
                        speed = 0.6;
                     }
                     ed_tune = man_tuner;
                    break;
                case 2:
                     initPID(&servo_pid, 2.2f, 0.6f, 0.f);
                     if(!torque){
                     speed = 80;
                     }
                     else{
                        speed= 0.8;
                     }
                     ed_tune = man_tuner;
                    break;
                case 3:
                     initPID(&servo_pid, 2.2f, 0.6f, 0.f);
                     speed = 110;
                     ed_tune = man_tuner;
                    break;
            }
            
            wait(2.f);
                
            ALIGN_SERVO;
            right_motor_pid.desired_value=speed;
            left_motor_pid.desired_value=speed;
            TFC_HBRIDGE_ENABLE;
            servo_pid.integral = 0;        
            //sendString("PID: %f %f %f speed:%f ed:%f",servo_pid.Kp, servo_pid.Ki, servo_pid.Kd, speed, ed_tune);                
            return;
        }
        
        
    }
 
}



void initVariables() {
    // Initialise three PID controllers for the servo and each wheel.
    initPID(&servo_pid, 0.f, 0.f, 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 > 2; 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 < 126; 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) {
        #if USE_COMMS
            sendString("lost edges");
        #endif
        ALIGN_SERVO; //Straighten wheels so we go straight through the crossroads
        servo_pid.integral = 0;
        l=0;r=128;
    }
    
    //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);
        
         
        //dutyCycleCorner(float cornerspeed, servo_pid.output);
        //dutyCycleCorner(speed, servo_pid.output);
        // This activates the electronic differential so that it runs whilst cornering. 
        // this changes the desired desired speed of each of the wheels according to the angle of the servo.
        sensorCorner(left_motor_pid.desired_value, right_motor_pid.desired_value , servo_pid.output, speed,ed_tune);  
    }
    
    /*
    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;   
                left_motor_pid.desired_value=speed;
                right_motot_pid.desired_value=speed; 
                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);
    //enables the ED
    //sensorCorner(left_motor_pid.desired_value,right_motor_pid.desired_value, servo_pid.output,speed,ed_tune);
    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;
        }
    }    
    
    // stops the motors from going into reverse and from being set to a value higher than 1.
    if(left_motor_pid.output > 1.0f) {
       left_motor_pid.output = 1.0f; 
    }
    /*if(left_motor_pid.output > 0.75f) {
        left_motor_pid.output = 0.75f;
    }*/
    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.75f) {
        right_motor_pid.output = 0.75f;
    }*/
    if(right_motor_pid.output < 0.0f) {
        right_motor_pid.output = 0.0f;
    }
    if(!torque){
    TFC_SetMotorPWM(left_motor_pid.output,right_motor_pid.output);
    }
    else{
        dutyCycleCorner(speed,servo_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!
        #if USE_COMMS
            //sendString("Start/stop seen");
            //lapTime();
        #endif
        TFC_SetMotorPWM(0.f,0.f);
        TFC_HBRIDGE_DISABLE;
        
    }
}

float checkAcc() {
    return abs(acc.getAccZ());   
}


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

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

    pc.putc(STATUS_STRING);
    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(SEND_LINE);
        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() {
    pc.putc(SEND_SPEEDS);
    writeFloat(wL * WHEEL_RADIUS);
    writeFloat(wR * WHEEL_RADIUS);
}

void writeFloat(float f) {
    byte_float_union._float = f;
    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]);
}

float readFloat() {
    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();
    return byte_float_union._float;    
}

// Listen for incoming commands from telemetry program and change car variables
inline void handleComms() {
    if(curr_cmd != 0) {
            switch(curr_cmd) {
                // Change the PID values of the servo controller
                case CHANGE_PID:
                    if(xb.cBuffer->available() >= 12) {
                        
                        servo_pid.Kp = readFloat();
                        servo_pid.Ki = readFloat();
                        servo_pid.Kd = readFloat();
                        
                        sendString("pid= Kp: %f, Ki: %f, Kd: %f",  servo_pid.Kp, servo_pid.Ki, servo_pid.Kd);
                        
                        curr_cmd = 0;        
                    }    
                break;
                
                // Set the maximum speed that the motor controllers should attempt to drive the car to
                case MAX_SPEED:
                    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;
                
                // Change the electronic differential coefficient
                case CHANGE_ED:
                 if(xb.cBuffer->available() >= 4) {
                        ed_tune = readFloat();
                        curr_cmd = 0;
                    }
                break;
                  
                default:
                // Unrecognised command
                curr_cmd = 0;
                break; 
            }
        }
        
        if(xb.cBuffer->available() > 0 && curr_cmd == 0) {
            
            char cmd = xb.cBuffer->read();
            
            //Start car the car motors
            if(cmd == START) {
                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;
                
            // Stop the car motors
            } else if (cmd == STOP) {
                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();
                
                // Change the PID values of the servo controller
            } else if(cmd == CHANGE_PID) {
                curr_cmd = CHANGE_PID;
                
                // Set the maximum speed of the motor controllers
            } else if(cmd == MAX_SPEED) {
                curr_cmd = MAX_SPEED;    
                
                // Switch the camera data that is sent over telemetry
            } else if(cmd == SWITCH_CAM) {
                sendCam = ~sendCam;    
                
                // Modify the electronic differential coefficient
            } else if(cmd == CHANGE_ED) {
                curr_cmd = CHANGE_ED;
            }
            
        }
}

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.
    //reads the timer
    float newTime= lapTimer.read();
    lapNo += 1;
    float lapTime= newTime-oldTime;
    float avgTime= newTime/lapNo;
    // this calulates the average lap time and the lap time.
    //then sends the information back to the UI.
    sendString("For lap number: %d  Lap Time: %f  Avergae time: %f \n\r", lapNo,lapTime,avgTime);
    
     
    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();
    
    
}                                              
//motor controll specific(newfunction)




#endif