File content as of revision 2:53a942d1b1e5:

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

#define Ts 0.001
#define pi 3.14159

LSM9DS0 sensor(SPI_MODE, D9, D6);
Serial pc(SERIAL_TX, SERIAL_RX);

Ticker WIPVTimer;
void WIPVTimerInterrupt();

PwmOut M1C1(D7);
PwmOut M1C2(D11);
PwmOut M2C1(D8);
PwmOut M2C2(A3);

QEI wheel_L(A1, A2, NC, 280, 50, 0.001, QEI::X4_ENCODING);
QEI wheel_R(D13, D12, NC, 280, 50, 0.001, QEI::X4_ENCODING);

AnalogIn current_L(PC_2);
AnalogIn current_R(PC_3);

PID curL_PID(0.6,2,0.0,0.001);


int tim_counter;
float current_L_Offset;
float current_R_Offset;
float tim = 0.0;
float amp = 0.4;
float omega = 2.0;
float curCmd_L;


int main()
{
    //float abias[3], gbias[3];

    pc.baud(115200);
    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.calLSM9DS0(gbias, abias);
    sensor.setGyroOffset(38,-24,-106);
    sensor.setAccelOffset(-793,-511,300);

    M1C1.period_us(50);
    M1C1.write(0.5);
    M2C1.period_us(50);
    M2C1.write(0.5);
    TIM1->CCER |= 4; //enable ch1 complimentary output
    TIM1->CCER |= 64;//enable ch1 complimentary output



    WIPVTimer.attach_us(WIPVTimerInterrupt, 1000.0);
    while(true) {
        //pc.printf("%5f\t%5f\r\n", speed1.getAngularSpeed(), speed2.getAngularSpeed());
    }
}


void WIPVTimerInterrupt()
{
    if(tim_counter <100)tim_counter++;
    else if (tim_counter >= 100 && tim_counter <=109) {
        current_L_Offset += current_L.read();
        current_R_Offset += current_R.read();
        tim_counter++;
        if(tim_counter == 110) {
            current_L_Offset = current_L_Offset/10;
            current_R_Offset = current_R_Offset/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];
        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);
        /*
                pc.printf("%5.4f, ", sensor.pitch);
                pc.printf("%5.4f   ", sensor.roll);
                pc.printf("%5.4f, ", data_array[0]);
                pc.printf("%5.4f, ", data_array[1]);
                pc.printf("%5.4f    ", data_array[2]);
                pc.printf("%5.4f, ", data_array[3]);
                pc.printf("%5.4f, ", data_array[4]);
                pc.printf("%5.4f;\r\n", data_array[5]);
         */
        //*****wheel speed calculation*****//
        wheel_L.Calculate();
        wheel_R.Calculate();
        //*************obtain current values***********//
        //pc.printf("%f\t", current_L_Offset);
        //pc.printf("%f\r\n", current_R_Offset);
        //*************current control********//
        tim += Ts;
        if(tim >= 4*pi/omega)tim = 0.0;
        curCmd_L = amp*sin(omega*tim); //current command
        
        pc.printf("%5.4f\t", 10*curCmd_L);
        float cur_L = (current_L.read() - current_L_Offset)*3.3*8/0.6; //8A when the voltage is 0.6 (assuming linear)
        pc.printf("%5.4f\t", 10*cur_L);
        
        curL_PID.Compute(curCmd_L, cur_L);
        M1C1 = 0.5 - curL_PID.output;
        pc.printf("%5.4f\r\n", curL_PID.output);
        TIM1->CCER |= 4;
        

    }


}