File content as of revision 7:f33a935eb77a:

#include "mbed.h"
#include "PID.h"
#include "LSM9DS0.h"
#include "QEI.h"
#include "CURRENT_CONTROL.h"
#include "SENSOR_FUSION.h"

#define Ts 0.001
#define pi 3.14159

LSM9DS0 sensor(SPI_MODE, D9, D6);
Serial pc(SERIAL_TX, SERIAL_RX);
Serial blueTooth(D10, D2);
Serial LeftSerial(PC_12, PD_2);
Serial RightSerial(PC_10, PC_11);

DigitalOut debugLed_l(A4);
DigitalOut debugLed_r(A5);

Ticker WIPVTimer;
void WIPVTimerInterrupt();
float saturation(float input, float limit_H, float limit_L);
void SensorAcquisition(float * data, float samplingTime);

void bluetoothRx();
void RXIrqLeft();
void RXIrqRight();

//MOTOR L == MOTOR 1;  MOTOR R = MOTOR 2
CURRENT_CONTROL motorCur_L(PC_3, D8,  A3,CURRENT_CONTROL::PWM2,400, 900.0,0.0,Ts);
CURRENT_CONTROL motorCur_R(PC_2, D7, D11,CURRENT_CONTROL::PWM1,400, 900.0,0.0,Ts);

QEI wheel_L(D13, D12, NC, 280, 200, Ts, QEI::X4_ENCODING);
QEI wheel_R(A1,  A2,  NC, 280, 200, Ts, QEI::X4_ENCODING);


PID balancingPD(20,0.00,0.0,Ts);
LPF sensorFilter1(Ts);
LPF sensorFilter2(Ts);
LPF sensorFilter3(Ts);
LPF sensorFilter4(Ts);





int tim_counter = 0;
float tim = 0.0;
float amp = 0.3;
float omega = 6.0;
float curCmd_L =0.0, curCmd_R =0.0;



float state[4] = {0.0, 0.0, 0.0, 0.0};
float ref[4] = {0.0, 0.0, 0.0, 0.0};

float torque_L = 0.0, torque_R = 0.0;
float KL[4] = {-0.7057 ,  -0.0733 ,  -0.0085 ,  -0.0300};
float KR[4] = {-0.7057 ,  -0.0733 ,  -0.0300 ,  -0.0085};

float Km_L = 1.050*0.163;
float Km_R = 1.187*0.137;

float yawRate = 0.0;

float velocityCmd[2] = {0.0, 0.0};
unsigned int accelerateFlag = 0;


uint8_t cLast_l = 0x00, cLast_r = 0x00;
bool headerCaptured_l = 0, headerCaptured_r = 0;
uint8_t fromPhoton_l[2] = {0,0}, fromPhoton_r[2] = {0,0};
float slipAcceleration_L = 0.1, slipAcceleration_R = 0.1, slipAcceleration_L_f, slipAcceleration_R_f;
int i_l, i_r;

int main()
{

    pc.baud(230400);
    blueTooth.baud(230400);
    LeftSerial.baud(230400); //uart commu with photon left
    RightSerial.baud(230400); //uart commu with photon right

    if( sensor.begin() != 0 ) {
        pc.printf("Problem starting the sensor with CS @ Pin D6.\r\n");
    } else {
        pc.printf("Sensor with CS @ Pin D9&D6 started.\r\n");
    }
    sensor.setGyroOffset(38,-24,-106);
    sensor.setAccelOffset(-793,-511,300);

    motorCur_L.SetParams(3.3*8/0.6, Km_L, 0.04348);
    motorCur_R.SetParams(3.3*8/0.6, Km_R, 0.04348);

    WIPVTimer.attach_us(WIPVTimerInterrupt, 1000.0);
    blueTooth.attach(&bluetoothRx, Serial::RxIrq);
    LeftSerial.attach(&RXIrqLeft, Serial::RxIrq);
    RightSerial.attach(&RXIrqRight, Serial::RxIrq);

    while(true) {
        //pc.printf("%5.4f\t", 10*pitch_angle);
        //pc.printf("%5.3f\n", 10*sensor.pitch*3.14159/180);
        //pc.printf("%5.3f\r\n", 10*curCmd_L);


        pc.printf("%5.4f\t",   slipAcceleration_L_f);
        pc.printf("%5.4f\r\n", slipAcceleration_R_f);

//        pc.putc(fromPhoton_l[0]);
//        pc.putc(fromPhoton_l[1]);
//        pc.putc(fromPhoton_r[0]);
//        pc.putc(fromPhoton_r[1]);

    }
}


void WIPVTimerInterrupt()
{
    if(tim_counter <100)tim_counter++;
    else if (tim_counter >= 100 && tim_counter <=109) {
        motorCur_L.currentOffset += motorCur_L.currentAnalogIn.read();
        motorCur_R.currentOffset += motorCur_R.currentAnalogIn.read();
        tim_counter++;
        if(tim_counter == 110) {
            motorCur_L.currentOffset = motorCur_L.currentOffset/10;
            motorCur_R.currentOffset = motorCur_R.currentOffset/10;
        }

    } else {

        /*
        int16_t data_array[6];

        data_array[0]  = sensor.readRawAccelX();
        data_array[1]  = sensor.readRawAccelY();
        data_array[2]  = sensor.readRawAccelZ();
        data_array[3]  = sensor.readRawGyroX();
        data_array[4]  = sensor.readRawGyroY();
        data_array[5]  = sensor.readRawGyroZ();

        pc.printf("%d, ", data_array[0]);
        pc.printf("%d, ", data_array[1]);
        pc.printf("%d, ", data_array[2]);
        pc.printf("%d, ", data_array[3]);
        pc.printf("%d, ", data_array[4]);
        pc.printf("%d;\r\n ", data_array[5]);
        */


        float data_array[6];//Gs and deg/s
        data_array[0]  = sensor.readFloatAccelX();
        data_array[1]  = sensor.readFloatAccelY();
        data_array[2]  = sensor.readFloatAccelZ();
        data_array[3]  = sensor.readFloatGyroX();
        data_array[4]  = sensor.readFloatGyroY();
        data_array[5]  = sensor.readFloatGyroZ();
        sensor.complementaryFilter(data_array,Ts);
        //SensorAcquisition(data_array, Ts);

        //===============wheel speed calculation============//
        wheel_L.Calculate();
        wheel_R.Calculate();

        /////////////Balancing Control/////////////////////////
        //SI dimension
        state[0] = sensor.pitch*3.14159/180.0;
        state[1] = sensorFilter1.filter(data_array[5]*3.14159/180.0, 10.0);
        state[2] = sensorFilter2.filter(wheel_L.getAngularSpeed(),60.0);
        state[3] = -sensorFilter3.filter(wheel_R.getAngularSpeed(),60.0);

        if(accelerateFlag == 1) {
            if(velocityCmd[0]>=7 || velocityCmd[1]>=7)accelerateFlag = 0;
            else {
                velocityCmd[0] += 0.004;
                velocityCmd[1] += 0.004;
            }
        }

        ref[2] = velocityCmd[0];
        ref[3] = velocityCmd[1];

        yawRate = sensorFilter4.filter(data_array[4],20);


        torque_L =  (KL[0]*(state[0] - ref[0])+KL[1]*(state[1] - ref[1])+KL[2]*(state[2] - ref[2])+KL[3]*(state[3]-ref[3]));
        torque_R = -(KR[0]*(state[0] - ref[0])+KR[1]*(state[1] - ref[1])+KR[2]*(state[2] - ref[2])+KR[3]*(state[3]-ref[3]));
        //=========================Anti slip control=============================//
        slipAcceleration_L_f = (1 - 10*2*3.141593*Ts)*slipAcceleration_L_f + 10*2*3.141593*Ts*slipAcceleration_L;
        slipAcceleration_R_f = (1 - 10*2*3.141593*Ts)*slipAcceleration_R_f + 10*2*3.141593*Ts*slipAcceleration_R;

        //motorCur_L.Control(saturation(torque_L/Km_L + 0.008*yawRate,1.2,-1.2), state[2]);
//        motorCur_R.Control(saturation(torque_R/Km_R + 0.008*yawRate,1.2,-1.2), -state[3]);


        /*  //PD Balancing Control
                balancingPD.Compute(0.0, sensor.pitch*3.14159/180);
                curCmd_R = sensorFilter.filter(saturation(0.5*( -balancingPD.output + 0.002*data_array[5]),1.0, -1.0),10);
                //======================current control=========================//
                tim += Ts;
                if(tim >= 4*pi/omega)tim = 0.0;
                //curCmd_R = amp*sin(omega*tim); //current command
                //curCmd_L = 0.8;

                motorCur_R.SetPWMDuty(0.75);

                motorCur_L.Control(-curCmd_R + 0.002*data_array[4], wheel_L.getAngularSpeed());
                motorCur_R.Control(curCmd_R  + 0.002*data_array[4], wheel_R.getAngularSpeed());
        */

    }


}


void  bluetoothRx()
{
    while(blueTooth.readable()) {
        char charRx = blueTooth.getc();
        switch(charRx) {
            case 'w'://forward
                velocityCmd[0] = 2.5;
                velocityCmd[1] = 2.5;
                accelerateFlag = 0;
                break;
            case 's'://backward
                velocityCmd[0] = -3.0;
                velocityCmd[1] = -3.0;
                accelerateFlag = 0;
                break;
            case 'a'://turn left
                velocityCmd[0] = -4.0;
                velocityCmd[1] = 4.0;
                accelerateFlag = 0;
                break;
            case 'd'://turn right
                velocityCmd[0] = 4.0;
                velocityCmd[1] = -4.0;
                accelerateFlag = 0;
                break;
            case 'x'://stop
                velocityCmd[0] = 0.0;
                velocityCmd[1] = 0.0;
                accelerateFlag = 0;
                break;

            case 'q'://accelerate
                accelerateFlag = 1;
                break;
        }
    }
}

void RXIrqLeft()
{
    //  slipAcceleration_L += 0.1;
    while(!LeftSerial.readable());
    uint8_t c = LeftSerial.getc();
    if(!headerCaptured_l & cLast_l == 0xAA & c == 0x55) {
        headerCaptured_l = 1;
        i_l = 0;
        debugLed_l = 1;
    } else if(headerCaptured_l) {
        if(c == 0xAA | c == 0x55 ) {
            headerCaptured_l = 0;
            i_l = 0;
            debugLed_l = 0;
        } 
        else {
            fromPhoton_l[i_l] = c;
            i_l++;
            if(i_l == 2) {
                headerCaptured_l = 0;
                debugLed_l = 0;
                slipAcceleration_L = -(float)((int16_t)(fromPhoton_l[0]*256+fromPhoton_l[1]))/100.0;
                
            }
        }
    }
    cLast_l = c;

}

void RXIrqRight()
{
    //  slipAcceleration_L += 0.1;
    while(!RightSerial.readable());
    uint8_t c = RightSerial.getc();
    if(!headerCaptured_r & cLast_r == 0xAA & c == 0x55) {
        headerCaptured_r = 1;
        i_r = 0;
        debugLed_r = 1;
    } else if(headerCaptured_r) {
        if(c == 0xAA | c == 0x55 ) {
            headerCaptured_r = 0;
            i_r = 0;
            debugLed_r = 0;
        } 
        else {
            fromPhoton_r[i_r] = c;
            i_r++;
            if(i_r == 2) {
                headerCaptured_r = 0;
                debugLed_r = 0;
                slipAcceleration_R = (float)((int16_t)(fromPhoton_r[0]*256+fromPhoton_r[1]))/100.0;
            }
        }
    }
    cLast_r = c;

}



float saturation(float input, float limit_H, float limit_L)
{
    float output;
    if(input >= limit_H)output = limit_H;
    else if (input <= limit_L)output = limit_L;
    else output = input;
    return output;
}


void SensorAcquisition(float * data, float samplingTime)
{

    axm =  data[0]*(-1)*9.81;//accelerometer dimension from g to m/s^2
    aym =  data[1]*(-1)*9.81;
    azm =  data[2]*(-1)*9.81;
    u1  = data[0]*3.14159/180;      //gyroscope :deg/s to rad/s
    u2  = data[1]*3.14159/180;
    u3  = data[2]*3.14159/180;


    if(conv_init <= 3) {
        axm_f_old = axm;
        aym_f_old = aym;
        azm_f_old = azm;

        conv_init++;
    } else {
        pitch_fusion(axm,  aym,azm,u3,u2,20, samplingTime);
        roll_fusion(axm,   aym,azm,u3,u1,20, samplingTime);
        x3_hat_estimat(axm,aym,azm,u2,u1,20, samplingTime);
    }

}