File content as of revision 0:fbdae7e6d805:

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

using namespace std;

class Main
{

public:

    Main();
    virtual     ~Main();

private:

    static const uint32_t   STACK_SIZE = 8192;  // stack size of thread, given in [bytes]

    Thread      thread;
    void        run();
};

Main::Main() : thread(osPriorityNormal, STACK_SIZE)
{

    thread.start(callback(this, &Main::run));
}

Main::~Main() {}

void Main::run()
{

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

    DigitalOut led1(PB_13);
    DigitalOut led2(PC_8);
    DigitalOut led3(PC_9);
    DigitalOut led4(PB_14); //neu
    DigitalOut led5(PB_15); // neu
    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(PC_4); // neu
    AnalogIn M1_AOUT2(PA_5); // neu

    AnalogIn M2_AOUT1(PC_1); // neu
    AnalogIn M2_AOUT2(PC_0); // neu

    AnalogIn M3_AOUT1(PA_7); // neu
    AnalogIn M3_AOUT2(PA_4); // neu


    Thread thread;

//IMU
    IMU imu(spi, csAG, csM);

    enable1 = 1;
    enable2 = 1;
    enable3 = 1;

    // create serial server object

    RawSerial serial(PB_10, PC_5);
    serial.baud(38400);
    serial.format(8, SerialBase::None, 1);
    pc1.printf("init serilCom\r\n");
    SerialCom com(serial);
    pc1.printf("init serilCom done\r\n");
    pc1.printf("init Controller\r\n");
    //controller
    Controller controller(pwm1,pwm2,pwm3,counter1,counter2,counter3,imu);
    pc1.printf("init Controller done\r\n");

    led1 = 0;
    led2 = 0;
    led3 = 0;
    led4 = 1;

    int i = 1;



    int t = 0;

    // enter main loop
    pc1.printf("init done\r\n");
    while (true) {

        //pc1.printf("main\r\n");


        if (com.dataRecived) {
            led9 = 1;
            controller.gainG = (1.0f+com.gainG);
            controller.gain_dG = (1.0f+com.gain_dG);
            controller.offsetX = (1.0f+com.offsetX);
            controller.offsetY = (1.0f+com.offsetY);
            com.dataRecived = false;
            pc1.printf("gainG = %.7f gain_dG = %.7f offsetX = %.7f offsetY = %.7f\r\n",controller.gainG,controller.gain_dG,controller.offsetX,controller.offsetY);
            Thread::wait(50.0f);
        } else {
            led9 = 0;

        }

        led7 = !led7;

        if(M1_AOUT1.read() > M1_AOUT2.read()) {
            led1 = 1;
        } else {
            led1 = 0;
        }
        if(M2_AOUT1.read() > M2_AOUT2.read()) {
            led2 = 1;
        } else {
            led2 = 0;
        }
        if(M3_AOUT1.read() > M3_AOUT2.read()) {
            led3 = 1;
        } else {
            led3 = 0;
        }


        // security condition
        if(abs(imu.getGammaX()) > 0.43f || abs(imu.getGammaY()) > 0.43f) {
            enable1 = 0;
            enable2 = 0;
            enable3 = 0;
            }


        Thread::wait(50.0f);
    }
}

/**
 * This is the main program of the ROME2RevG firmware.
 */
int32_t main()
{

    Main main;

    Thread::wait(osWaitForever);
}