File content as of revision 0:88faaa1afb83:

#include "FXAS21002.h"
#include "FXOS8700Q.h"  
#include "mbed.h"
#include "CarPWM.h"
#include "receiver.h"


#define PI 3.141592653589793238462
#define Ts 0.02                         // Seconds
#define INITIAL_P 0.452531214933414
#define INITIAL_I 5.45748932024049
#define INITIAL_D 0.000233453623255507
#define INITIAL_N 51.0605584484153
#define GYRO_OFFSET 0.0152
#define END_THRESH 4
#define START_THRESH 10
#define MINIMUM_VELOCITY 15
Serial ser(USBTX, USBRX);    // Initialize Serial port
PwmOut motor(PTD1);         // Motor connected to pin PTD1
PwmOut servo(PTD3);         // Servo connected to pin PTD3

FXOS8700Q_mag mag(PTE25,PTE24,FXOS8700CQ_SLAVE_ADDR1);
FXAS21002 gyro(PTE25,PTE24);



// PID controller parameters and functions
float e[2], u, up[1],ui[2], ud[2]; // The vector coeficient means a time delay, for exemple e[a] = e(k-a) -> z^(-a)e(k)
float P, I, D, N, reference = 0;
void controlAnglePID(float P, float I, float D, float N);
void initializeController();

// Gyroscope variables and functions
float gyro_data[3], gyro_angle;
Timer t; 
void processGyroAngle();
void startGyro();
void stopGyro();

// Magnetometer variables and functions
float max_x, max_y, min_x, min_y,x,y;
MotionSensorDataUnits mag_data;
float processMagAngle();
void magCal();

// State variables
float sensor, velocity;
void readProtocol();
void brakeMotor();
// Test functions
void debug();

int main(){
    // Initializing serial communication
    ser.baud(9600);
    ser.format(8, SerialBase::None, 1);
    // Initializing controller
    printf("Initializing controller....\r\n\r\n");
    initializeController();
    printf("Controller Initialized. \r\n");
    // Calibrating magnetometer and setting the initial position
    magCal();
    gyro_angle = processMagAngle();
    // Start moving the robot and integrating the gyroscope
    velocity = MINIMUM_VELOCITY;
    setMotorPWM(velocity,motor);
    startGyro();
    // main loop
        while (true){
        processGyroAngle();
        controlAnglePID(P,I,D,N);
        //debug();
        if(t.read_us() < Ts*1000000)
            wait_us(Ts*1000000 - t.read_us());
        if(ser.readable())
            readProtocol();
        }
}

void readProtocol(){
    char msg = ser.getc();
    switch(msg)
    {
        case NONE:
            //ser.printf("sending red signal to led\r\n");
            return;
            break;          
        case BRAKE:
            //ser.printf("sending green signal to led\r\n");
            brakeMotor();
            break;
        case ANG_RST:
            //ser.printf("sending blue signal to led\r\n");
            stopGyro();
            gyro_angle = 0;
            startGyro();
            return;
            break;
        case ANG_REF:
            reference = get_ang_ref(ser);     
            break;
        case GND_SPEED:
            velocity = get_gnd_speed(ser);       
            setMotorPWM(velocity,motor);
            break;
        case PID_PARAMS:
            ser.putc('p');
            get_pid_params(ser, &P, &I, &D, &N);     
            break;
        default:
           // ser.flush();

    }
}
/* Initialize the controller parameter P, I, D and N with the initial values and set the error and input to 0.          */
void initializeController(){
    for(int i =0; i<2; i++){
        e[i] = 0;
        ui[i] = 0;
        ud[i] = 0;
    }
    P= INITIAL_P;
    I= INITIAL_I;
    D= INITIAL_D;
    N= INITIAL_N;
}

/* Start the Gyroscope timer and set the initial configuration */
void startGyro(){
    gyro.gyro_config(); 
    t.start();
}

/* Stop and reset the Gyroscope */
void stopGyro(){
    t.stop();
    t.reset();
    gyro_angle = 0;
}

/* Integrate the Gyroscope to get the angular position (Deegre/seconds) */
void processGyroAngle(){
    gyro.acquire_gyro_data_dps(gyro_data);
    t.stop();
    gyro_angle = gyro_angle + (gyro_data[2] + GYRO_OFFSET)*(double)(t.read_us())/1000000;
    t.reset();
    t.start();
    sensor = gyro_angle;
    if(sensor > 180)
        sensor = sensor - 360;
    if(sensor < -180)
        sensor = sensor + 360;
}

/* PID controller for angular position */
void controlAnglePID(float P, float I, float D, float N){ 
    /* Getting error */
    e[1] = e[0];
    e[0] = reference - (sensor*PI/180);
    if(e[0] > PI)
      e[0]= e[0] - 2*PI;
    if(e[0] < -PI)
      e[0] = e[0] + 2*PI;
    /* Proportinal Part */
    up[0] = e[0]*P;  
    /* Integral Part */
    ui[1] = ui[0];
    if(abs(u) < PI/8){
      ui[0] = (P*I*Ts)*e[1] + ui[1];    
    }
    else if(u > 0)
      ui[0] = PI/8 - up[0];
    else if(u < 0)
      ui[0] = -PI/8 - up[0];  
    /* Derivative Part */
    ud[1] = ud[0];
    ud[0] = P*D*N*(e[0] - e[1]) - ud[1]*(N*Ts -1); 
    /** Controller **/ 
    u = up[0] + ud[0] + ui[0];
    setServoPWM(u*100/(PI/8), servo);
}
/* Brake function, braking while the gyroscope is still integrating will cause considerably error in the measurement. */
void brakeMotor(){
    if(velocity > 0)
        setMotorPWM(-30, motor);
    else if(velocity < 0)
        setMotorPWM(30, motor);
    wait(0.5);
    setMotorPWM(0,motor);
}
/* Debug functions that prints the sensor and control inputs values. Since it's time consuming it should not be used    */
/* in the main loop or the controller performance may be affected.                                                      */
void debug(){
    //printf("ERROR: %f Up: %f Ui: %f Ud: %f U: %f \r\n", e[0]*180/3.1415, up[0]*100/(3.1415/8), ui[0]*100/(3.1415/8), ud[0]*100/(3.1415/8),u*100/(3.1415/8));
    //printf("Erro: %f  Sensor: %f Magnetometer: %f \r\n",e[0]*180/PI,sensor,processMagAngle(0)*180/PI);
    printf(" %f \r\n",sensor);
}

/* Function to normalize the magnetometer reading */
void magCal(){
        printf("Starting Calibration");
        mag.enable(); 
        wait(0.01);
        mag.getAxis(mag_data);
        float x0 = max_x = min_y = mag_data.x;
        float y0 = max_y = min_y = mag_data.y;
        bool began = false;
        while(!(began && abs(mag_data.x - x0) < END_THRESH && abs(mag_data.y - y0) < END_THRESH)){
            mag.getAxis(mag_data);
            if(mag_data.x > max_x)
                max_x = mag_data.x;
            if(mag_data.y > max_y)
                max_y = mag_data.y;
            if(mag_data.y < min_y)
                min_y = mag_data.y;
            if(mag_data.x < min_x)
                min_x = mag_data.x;    
            if(abs(mag_data.x-x0)>START_THRESH && abs(mag_data.y-y0) > START_THRESH)
                began = true;   
            printf("began: %d  X-X0: %f , Y-Y0: %f \n\r", began, abs(mag_data.x-x0),abs(mag_data.y-y0));
        }
        printf("Calibration Completed: X_MAX = %f , Y_MAX = %f , X_MIN = %f and Y_MIN = %f \n\r",max_x,max_y,min_x,min_y);
}

/* Function to transform the magnetometer reading in angle(rad/s).*/
float processMagAngle(){
    mag.getAxis(mag_data);
    x = 2*(mag_data.x-min_x)/float(max_x-min_x) - 1;
    y = 2*(mag_data.y-min_y)/float(max_y-min_y) - 1;
    return atan2(y,x);
}