File content as of revision 13:f7a7fe9b5c00:

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

#define PI 3.141592653589793238462
#define Ts 0.02                         // Seconds
#define PWM_PERIOD 13.5                 // ms
#define INITIAL_P 0.452531214933414
#define INITIAL_I 5.45748932024049
#define INITIAL_D 0.000233453623255507
#define INITIAL_N 51.0605584484153
#define BRAKE_CONSTANT 40
#define BRAKE_WAIT 0.3
#define END_THRESH 4
#define START_THRESH 10
#define MINIMUM_VELOCITY 15
#define GYRO_PERIOD 1300                //us

Serial ser(USBTX, USBRX);    // Initialize Serial port
PwmOut servo(PTD3);         // Servo connected to pin PTD3
Motor motor;
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();

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

int main(){
    gyro.gyro_config(MODE_2);
    gyro.start_measure(GYRO_PERIOD);
    initializeController();
    while(1){
        controlAnglePID(P,I,D,N);
        printf("%f \r\n",gyro.get_angle());
        wait(Ts);
    }
}
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");
            motor.brakeMotor();
            break;
        case ANG_RST:
            //ser.printf("sending blue signal to led\r\n");
            gyro.stop_measure();
            gyro.start_measure(GYRO_PERIOD);
            return;
            break;
        case ANG_REF:
            reference = get_ang_ref(ser);     
            break;
        case GND_SPEED:
            motor.setVelocity(get_gnd_speed(ser));
            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;
}

/* PID controller for angular position */
void controlAnglePID(float P, float I, float D, float N){ 
    /* Getting error */
    float feedback = gyro.get_angle();
    e[1] = e[0];
    e[0] = reference - (feedback*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. */
/* 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);
}