File content as of revision 16:a9e0eb97557f:

#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 5000                //us
#define LED_ON 0
#define LED_OFF 1

#define MIN -1.5
#define MAX 1.5

enum{
    BLACK,
    RED,
    GREEN,
    BLUE,
    PURPLE,
    YELLOW,
    AQUA,
    WHITE};

void turn_leds_off(DigitalOut& red, DigitalOut& green, DigitalOut& blue)
{
    red = LED_OFF;
    green = LED_OFF;
    blue = LED_OFF;
}

void set_leds_color(int color, DigitalOut& red, DigitalOut& green, DigitalOut& blue)
{
    turn_leds_off(red, green, blue);

    switch(color)
    {
        case RED:
            red = LED_ON;               
            break;
        case GREEN:
            green = LED_ON;
            break;
        case BLUE:
            blue = LED_ON;
            break;
        case PURPLE:
            red = LED_ON;
            blue = LED_ON;
            break;
        case YELLOW:
            red = LED_ON;
            green = LED_ON;
            break;
        case AQUA:
            blue = LED_ON;
            green = LED_ON;
            break;
        case WHITE:
            red = LED_ON;
            green = LED_ON;
            blue = LED_ON;
            break;
        default:
            break;
    }
}

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);
FXOS8700Q_acc acc( PTE25, PTE24, FXOS8700CQ_SLAVE_ADDR1);
FXAS21002 gyro(PTE25,PTE24);
DigitalOut red_led(LED_RED);
DigitalOut green_led(LED_GREEN);
DigitalOut blue_led(LED_BLUE);
Receiver rcv;
EthernetInterface eth;

// 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();
void control();
Ticker controller_ticker;

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

// Protocol
void readProtocol();

int main(){
    // Initializing sensors:
    acc.enable();
    gyro.gyro_config(MODE_1);
    initializeController();
    
    // Set initial control configurations
    motor.setVelocity(0);
    
    // Protocol parameters
    set_leds_color(RED, red_led, green_led, blue_led);
    eth.init(RECEIVER_IFACE_ADDR, RECEIVER_NETMASK_ADDR, RECEIVER_GATEWAY_ADDR);
    eth.connect();
    set_leds_color(BLUE, red_led, green_led, blue_led);
    rcv.set_socket();
    
    gyro.start_measure(GYRO_PERIOD);
    controller_ticker.attach(&control,Ts);
    //main loop
    while(1){
        readProtocol();
        wait(0.01);
    }
}
void control(){
    controlAnglePID(P,I,D,N);
}
void readProtocol(){
    if(!rcv.receive())
        return;
    char msg = rcv.get_msg();
    switch(msg)
    {
        case NONE:
            //ser.printf("sending red signal to led\r\n");
             red_led = LED_ON;

            printf("NONE\r\n");

            wait(1);
            red_led = LED_OFF;
            break;          
        case BRAKE:
            //ser.printf("sending green signal to led\r\n");
            green_led = LED_ON;
            motor.stopJogging();
            printf("BRAKE\r\n");
            motor.brakeMotor();
            green_led = LED_OFF;
            break;
        case ANG_RST:
            //ser.printf("sending blue signal to led\r\n");
            blue_led = LED_ON;

            printf("ANG_RST\r\n");
            gyro.stop_measure();
            gyro.start_measure(GYRO_PERIOD);
            blue_led = LED_OFF;
            initializeController();
            break;
        case ANG_REF:
            red_led = LED_ON;
            green_led = LED_ON;

            reference = rcv.get_ang_ref();// - processMagAngle();
            printf("New reference: %f \n\r",reference*180/PI);
            if(reference > PI)
                reference = reference - 2*PI;
            if(reference < -PI)
                reference = reference + 2*PI;  
            red_led = LED_OFF;
            green_led = LED_OFF;
            break;
        case GND_VEL:
            red_led = LED_ON;
            blue_led = LED_ON;
            float vel = rcv.get_gnd_vel();
            motor.setVelocity(vel);      
            printf("GND_VEL = %f\r\n", vel);

            red_led = LED_OFF;
            blue_led = LED_OFF;
            break;
        case JOG_VEL:
            red_led = LED_ON;
            blue_led = LED_ON;

            float p, r;
            rcv.get_jog_vel(&p,&r);
            if(p == 0 || r == 0)
                motor.stopJogging();
            else
                motor.startJogging(r,p);
            red_led = LED_OFF;
            blue_led = LED_OFF;
            break;
        case PID_PARAMS:
            blue_led = LED_ON;
            green_led = LED_ON;

            float ar[4];
            rcv.get_pid_params(ar);
            P = ar[0];
            I = ar[1];
            D = ar[2];
            N = ar[3];
            printf("PID_PARAMS | kp=%f, ki=%f, kd=%f, n=%f\r\n", 
                ar[0], ar[1], ar[2], ar[3]);

            wait(1);
            blue_led = LED_OFF;
            green_led = LED_OFF;
            break;
        case MAG_CALIB:
            float mag[4];
            rcv.get_mag_calib(mag);
            max_x=mag[1];
            max_y=mag[3];
            min_x=mag[0];
            min_y=mag[2];
            break;
        default:
            blue_led = LED_ON;
            green_led = LED_ON;
            red_led = LED_ON;
            printf("nothing understood\r\n");
            wait(1);
            blue_led = LED_OFF;
            green_led = LED_OFF;
            red_led = LED_OFF;
            //ser.printf("unknown command!\r\n");
    }
}
/* 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(){
        //red_led = 0;
        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);
}