File content as of revision 22:b7cca3089dfe:

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

#define PI 3.141593
#define Ts 0.02                         // Controller period(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 END_THRESH 4                    //For magnetometer calibration
#define START_THRESH 10                 //For magnetometer calibration
#define MINIMUM_VELOCITY 15
#define MINIMUM_CURVE_VELOCITY 19
#define ERROR_THRESH    PI/5
#define GYRO_PERIOD 15000               //us
#define RGB_LED_ON 0                    //active Low
#define RGB_LED_OFF 1                   //active Low

#define MIN -1.5
#define MAX 1.5

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


//Control Objetcs
PwmOut servo(PTD1);         // 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);
//Leds Objects
DigitalOut red_led(LED_RED);
DigitalOut green_led(LED_GREEN);
DigitalOut blue_led(LED_BLUE);
DigitalOut red_led_s(PTD0);
DigitalOut green_led_s(PTB2); //PTC4
DigitalOut blue_led_s(PTC12);
DigitalOut main_led(PTC4);
//Protocol Objects
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;

// Motor and servo variables
float saved_velocity = 0;
bool brake = false;

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

// Protocol
void readProtocol();
int contp = 0; // for debug only

// NXP RGB_LEDs control
void set_leds_color(int color);
void turn_leds_off();

int main(){
    
    // Initializing sensors:
    main_led = 1;
    acc.enable();
    gyro.gyro_config(MODE_1);
    initializeController();
    
    // Set initial control configurations
    motor.setVelocity(0);
    
    // Protocol initialization
    
    
    
    
    printf("Initializing Ethernet!\r\n");
    set_leds_color(RED);
    eth.init(RECEIVER_IFACE_ADDR, RECEIVER_NETMASK_ADDR, RECEIVER_GATEWAY_ADDR);
    eth.connect();
    printf("Protocol initialized! \r\n");
    gyro.start_measure(GYRO_PERIOD);
    set_leds_color(BLUE);
    rcv.set_socket();
    
    main_led = 0;
    controller_ticker.attach(&control,Ts);
    
    
    
    //main loop
    while(1){
        readProtocol();
      //  printf("%f \r\n",gyro.get_angle());
    }
}
void control(){
    controlAnglePID(P,I,D,N);
}
void readProtocol(){
    if(!rcv.receive())
        return;
    char msg = rcv.get_msg();
    //printf("Message received!");
    //contp++;
    //printf(" %d \r\n",contp);
    switch(msg)
    {
        case NONE:
            break;          
        case BRAKE:
            //printf("BRAKE ");
            float intensity, b_wait;
            rcv.get_brake(&intensity,&b_wait);
            if(!brake){
                set_leds_color(YELLOW);
                motor.stopJogging();
               // printf("BRAKE\r\n");
                setServoPWM(0,servo);
                //reference = 0;
                controller_ticker.detach();
                motor.brakeMotor(intensity,b_wait);
                controller_ticker.attach(&control,Ts);
                saved_velocity = 0;
                brake = true;
                }
            break;
        case GYRO_ZERO:
            gyro.stop_measure();
            wait(0.05);
            gyro.start_measure(GYRO_PERIOD);
            break;
        case ANG_SET:
            set_leds_color(PURPLE);
            //printf("ANG_SET\r\n");
            gyro_reference = gyro.get_angle();
            initializeController();
            break;
        case ANG_RST:
            //printf("ANG_RST\r\n");
            gyro_reference = 0;
            set_leds_color(PURPLE);
            initializeController();
            break;
        case MAG_ANG_REF:
            set_leds_color(BLUE);
            reference = rcv.get_mag_ang_ref() - processMagAngle();
            //printf("New reference: %f \n\r",reference*180/PI);
            if(reference > PI)
                reference = reference - 2*PI;
            else if(reference < -PI)
                reference = reference + 2*PI;  
            break;
        case ABS_ANG_REF:
            set_leds_color(GREEN);
            reference = rcv.get_abs_ang_ref();
            //printf("New reference: %f \n\r",reference*180/PI);
            if(reference > PI)
                reference = reference - 2*PI;
            else if(reference < -PI)
                reference = reference + 2*PI;  
            break;
        case REL_ANG_REF:
            set_leds_color(RED);
            reference = rcv.get_rel_ang_ref() + gyro.get_angle()*PI/180;
            //printf("New reference: %f \n\r",reference*180/PI); 
            if(reference > PI)
                reference = reference - 2*PI;
            else if(reference < -PI)
                reference = reference + 2*PI;
            break;
        case GND_VEL:
            set_leds_color(AQUA);
            saved_velocity = rcv.get_gnd_vel();
            //printf("GND_VEL");
            if(saved_velocity > 0){
                motor.setVelocity(saved_velocity);      
                if(abs(saved_velocity) > MINIMUM_VELOCITY)
                    brake = false;
                //printf("GND_VEL = %f\r\n", saved_velocity);
            }
            break;
        case JOG_VEL:
            set_leds_color(WHITE);
            float p, r;
            rcv.get_jog_vel(&p,&r);
            //printf("Joggin with period %f and duty cycle %f\r\n",p,r);
            if(p == 0 || r == 0)
                motor.stopJogging();
            else
                motor.startJogging(r,p);
            break;
        case PID_PARAMS:
            set_leds_color(WHITE);
            float arr[4];
            rcv.get_pid_params(arr);
            P = arr[0];
            I = arr[1];
            D = arr[2];
            N = arr[3];
           // printf("PID_PARAMS | kp=%f, ki=%f, kd=%f, n=%f\r\n", 
           //     arr[0], arr[1], arr[2], arr[3]);

            break;
        case MAG_CALIB:
            set_leds_color(BLUE);
            printf("MAG_CALIB\r\n");
            float mag[4];
            rcv.get_mag_calib(mag);
            max_x=mag[1];
            max_y=mag[3];
            min_x=mag[0];
            min_y=mag[2];
            //printf(" max_x = %f, max_y= %f, min_x= %f, min_y= %f\r\n",mag[1],mag[3],mag[0],mag[2]);
            break;
        case LED_ON:
            set_leds_color(BLACK);
            main_led = 1;
            break;
        case LED_OFF:
            set_leds_color(BLACK);
            main_led = 0;
            break;
        default:
            //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() - gyro_reference;
    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);
    if(abs(e[0]) >= ERROR_THRESH && motor.getVelocity() > MINIMUM_CURVE_VELOCITY){
        saved_velocity = motor.getVelocity();
        motor.setVelocity(MINIMUM_CURVE_VELOCITY);
        }
    else if(abs(e[0]) < ERROR_THRESH && motor.getVelocity() != saved_velocity){
        motor.setVelocity(saved_velocity);
        }
}
/* 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(){
   // printf("starting ProcessMagAngle()\n\r");
    float mag_lpf = 0;
    Timer t1;
    for(int i = 0; i<20; i++){
        //printf("\r\n");
        //wait(0.1);
        t1.start();
        __disable_irq();
        mag.getAxis(mag_data);
        __enable_irq();
        t1.stop();
        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;
        mag_lpf = mag_lpf + (-atan2(y,x));
        wait(0.015);
        }
   // wait(20*0.01);
   // printf("Finished ProcessMagAngle() %d \n\r",t1.read_us());
    return mag_lpf/20;
}
void turn_leds_off()
{
    red_led = RGB_LED_OFF;
    green_led = RGB_LED_OFF;
    blue_led = RGB_LED_OFF;
}

void set_leds_color(int color)
{
    turn_leds_off();

    switch(color)
    {
        case RED:
            red_led = RGB_LED_ON;
            red_led_s = RGB_LED_ON;                 
            break;
        case GREEN:
            green_led = RGB_LED_ON;
            green_led_s = RGB_LED_ON;
            break;
        case BLUE:
            blue_led = RGB_LED_ON;
            blue_led_s = RGB_LED_ON;
            break;
        case PURPLE:
            red_led = RGB_LED_ON;
            blue_led = RGB_LED_ON;
            red_led_s = RGB_LED_ON;
            blue_led_s = RGB_LED_ON;
            break;
        case YELLOW:
            red_led = RGB_LED_ON;
            green_led = RGB_LED_ON;
            red_led_s = RGB_LED_ON;
            green_led_s = RGB_LED_ON;
            break;
        case AQUA:
            blue_led = RGB_LED_ON;
            green_led = RGB_LED_ON;
            blue_led_s = RGB_LED_ON;
            green_led_s = RGB_LED_ON;
            break;
        case WHITE:
            red_led = RGB_LED_ON;
            green_led = RGB_LED_ON;
            blue_led = RGB_LED_ON;
            red_led_s = RGB_LED_ON;
            green_led_s = RGB_LED_ON;
            blue_led_s = RGB_LED_ON;
            break;
        default:
            break;
    }
}