File content as of revision 11:69a364e391dc:

#include "stm32f103c8t6.h"
#include "mbed.h"
I2C i2c(PB_9, PB_8);
const int address = 0x08 << 1;
Timer t;
int Now = 0 , LastUpdate = 0, deltat = 0;
int main()
{
    confSysClock();     //Configure system clock (72MHz HSE clock, 48MHz USB clock)

    Serial      pc(PA_2, PA_3);
    pc.baud(115200);
    //AnalogIn    nub1(PA_0);
    PwmOut      servo1(PA_6);
    PwmOut      servo2(PA_7);
    PwmOut      servo3(PB_0);
    PwmOut      servo4(PB_1);
    servo1.period_us(20000);
    servo2.period_us(20000);
    servo3.period_us(20000);
    servo4.period_us(20000);

    float pitchrig = 0.0164;//41*8/20000;
    float pitchlft = 0.0276;//69*8/20000;
    float pitchout = 0.0224;//56*8/20000;


    float rollrig = 0.014;//35
    float rolllft = 0.0308;//77
    float rollout = 0.0224;//56

    float Motr_up = 0.0448;//112
    float MotorA  = 0;
    float MotorB  = 0;

    float MotorAi = 0;
//    float MotorBi = 0;

    float diff = 0;

    servo1 = 0.05f + pitchout;
    servo2 = 0.05f + rollout;
    servo3 = 0.05f + MotorA;
    servo4 = 0.05f + MotorB;
    t.start();

    while(1) {
        char data[5];
        i2c.read(address,data,5);
        //pc.printf("%d,%d,%d,%d,%d\r\n", data[0], data[1], data[2], data[3], data[4]);
//        if (pc.readable()) {
//            char channel = pc.getc();
//            if (channel>128) {
//                char control = pc.getc();
//                if (channel == 129 && control<=128) {
                    Now = t.read_us();
                    deltat = Now - LastUpdate; // set integration time by time elapsed since last filter update
                    LastUpdate = Now;
                    
                    pitchout = float(data[0]*4)/20000;

                    if(pitchout>pitchlft) {
                        pitchout = pitchlft;
                    }
                    if(pitchout<pitchrig) {
                        pitchout = pitchrig;
                    }
                    if(deltat > 4000){
                        servo1 = 0.05f + pitchout;
                    }

//                }
//                if (channel == 130 && control<=128) {
                    rollout = float(data[1]*4)/20000;
                    //pc.printf("rollout = %f\n\r", rollout);
                    if(rollout>rolllft) {
                        rollout = rolllft;
                    }
                    if(rollout<rollrig) {
                        rollout = rollrig;
                    }
                    //servo2 = 0.05f + rollout;
                    //servo2 = 0.05f+float(control*8)/20000;

//                }
//                if (channel == 131 && control<=128) {
                    MotorAi = float(data[2]*4)/20000;
                    //pc.printf("MotorAi = %f\n\r", MotorAi);
                    diff = float((int16_t(data[3]/2)-64)*0.0004);
                    pc.printf("%f,%f,%f,%f\n\r", pitchout,rollout,MotorAi,diff);
                    MotorA = MotorAi+diff;
                    if(MotorA>Motr_up) {
                        MotorA = Motr_up;
                    }
                    if(MotorA<0) {
                        MotorA = 0;
                    }

                    MotorB = MotorAi - diff;
                    if(MotorB>Motr_up) {
                        MotorB = Motr_up;
                    }
                    if(MotorB<0) {
                        MotorB = 0;
                    }
                    //servo3 = 0.05f + MotorA;
                    //servo4 = 0.05f + MotorB;
                    //servo2 = 0.05f+float(control*8)/20000;

//                }
/*                if (channel == 132 && control<=128) {
                    diff = float((int16_t(control)-64)*0.0004);
                    //pc.printf("diff = %f\n\r", diff);
                    
                    MotorA = MotorAi+diff;
                    if(MotorA>Motr_up) {
                        MotorA = Motr_up;
                    }
                    if(MotorA<0) {
                        MotorA = 0;
                    }

                    MotorB = MotorAi-diff;
                    if(MotorB>Motr_up) {
                        MotorB = Motr_up;
                    }
                    if(MotorB<0) {
                        MotorB = 0;
                    }
                    servo3 = 0.05f + MotorA;
                    servo4 = 0.05f + MotorB;
                }
            }*/ 
            //servo = float(1000+(c*4))/20000;
        //}
    }
}