File content as of revision 0:02dd72d1d465:

#include "mbed.h"
#include "EncoderCounter.h"
#include "IMU.h"
#include "Controller.h"

DigitalOut myled(LED1);
Serial pc1(USBTX, USBRX); // tx, rx

DigitalOut led1(PB_13);
DigitalOut led2(PC_8);
DigitalOut led3(PC_9);
DigitalOut led4(PC_0);
DigitalOut led5(PC_1);
DigitalOut led6(PH_1);
DigitalOut led7(PC_2);
DigitalOut led8(PC_3);
DigitalOut led9(PB_0);

// create motor control objects
DigitalOut enable1(PB_1);
DigitalOut enable2(PB_2);
DigitalOut enable3(PB_3);

PwmOut pwm1(PA_8);
PwmOut pwm2(PA_9);
PwmOut pwm3(PA_10);

// crete Encoder read objects
EncoderCounter counter1(PA_6, PC_7);
EncoderCounter counter2(PB_6, PB_7);
EncoderCounter counter3(PA_1, PA_0);

// create IMU comunication objects
SPI spi(PC_12, PC_11, PC_10); // mosi, miso, sclk
DigitalOut csAG(PA_15);
DigitalOut csM(PD_2);

// escon I/O


//AnalogIn M1_AOUT1(PB_4);
//AnalogIn M1_AOUT2(PB_5);

//AnalogIn M2_AOUT1(PB_14);
//AnalogIn M2_AOUT2(PB_15);

AnalogIn M3_AOUT1(PA_7);
//AnalogIn M3_AOUT2(PA_11);


Thread thread;

int main()
{
    //IMU
    IMU imu(spi, csAG, csM);

    pwm1.period_us(200);
    enable1 = 1;
    //pwm1.write(0.6f);

    pwm2.period_us(200);
    enable2 = 1;
    //pwm2.write(0.6f);

    pwm3.period_us(200);
    enable3 = 1;
    //pwm3.write(0.6f);
    
    //controller
    Controller controller(pwm1,pwm2,pwm3,counter1,counter2,counter3,imu);
    
    pc1.baud(100000);
    
    led1 = 0;
    led2 = 0;
    led3 = 0;
    led4 = 0;

    int i = 1;
    
    float gamma_z = 0;
    float gz_vor = 0;
    
    int t = 0;
    
    while(1) {
        
        led4 = 0;
        led9 = 0;
        led5 = 0;
        led6 = 0;
        led7 = 0;
        led8 = 0;

        switch(i) {
            case 1:
                led4 = 1;
                break;

            case 2:
                led9 = 1;
                break;

            case 3:
                led5 = 1;
                break;

            case 4:
                led6 = 1;
                break;

            case 5:
                led7 = 1;
                break;

            case 6:
                led8 = 1;
                break;

            case 7:
                led7 = 1;
                break;

            case 8:
                led6 = 1;
                break;

            case 9:
                led5 = 1;
                break;
            case 10:
                led9 = 1;
                break;

            default:
                led1 = 0;
                led2 = 0;
                led3 = 0;
                led4 = 0;
                break;
        }

        i++;
        if(i==11) {
            i=1;
        }

        myled = 1; // LED is ON
        thread.wait(50.0);
        
        float gz = imu.readGyroZ();
        gamma_z = (gz-gz_vor)*0.05f + gamma_z;
        float f = t*(-0.0011f);
        
        //printf("counter1: %d counter2: %d counter3: %d\r\n", counter1.read(),counter2.read(),counter3.read());
        //printf("%.5f %.5f\r\n",imu.getGammaX(),imu.getGammaY());
        //printf("%.5f %.5f\r\n",imu.getGammaZ(),imu.getGammaZ());
        //printf("%.5f %.5f\r\n",imu.readMagnetometerX()*166.0045f -550.0f, imu.readMagnetometerY()*140.6528f + 0.0f);
        //printf("%.2f %.6f %.6f %.6f %.6f %.6f %.6f %.6f %.6f %.6f\r\n",time,imu.readAccelerationX(), imu.readAccelerationY(), imu.readAccelerationZ(),imu.readGyroX(), imu.readGyroY(), imu.readGyroZ(),imu.readMagnetometerX(), imu.readMagnetometerY(), imu.readMagnetometerZ());
        
        //printf("%.7f %.7f\r\n",M1_AOUT1.read(),M1_AOUT2.read());
        
        //printf("main\r\n");
        
        t++;

    }
}