File content as of revision 1:4062af6224fa:

#include "mbed.h"
#include "rotary_inc.hpp"
#include "matrix.h"
#include "BNO055.h"
#include <cmath>

//Serial pc(USBTX, USBRX);

RotaryInc enc1(PC_3,PC_2,2*M_PI,256,4);
RotaryInc enc2(PC_5,PA_12,2*M_PI,256,4);
RotaryInc enc3(PC_1,PC_0,2*M_PI,256,4);
RotaryInc enc4(PA_13,PC_4,2*M_PI,256,4);

BNO055 bno(PB_3, PB_10);

Timer t;

Matrix<double> Vr(3, 1);
Matrix<double> J(3, 4);
Matrix<double> W(4, 1);

int main(){
    double r = 50.6;
    double R = 2.0*r;
    double theta = 0.0;
    double gyro = 0.0;
    double fgyro = 0.0;
    double x = 0.0;
    double y = 0.0;
    double vx = 0.0;
    double vy = 0.0;
    double old1 = 0.0, old2 = 0.0, old3 = 0.0, old4 = 0.0;
    double cor1 = 0.0, cor2 = 0.0, cor3 = 0.0, cor4 = 0.0;
    bno.setmode(OPERATION_MODE_IMUPLUS);
        bno.get_angles();/*
        while(fgyro == 0){
        bno.get_angles();
        fgyro = bno.euler.yaw * (M_PI / 180.0);
            }
            fgyro = 0.0;*/
    while(fgyro - (bno.euler.yaw * (M_PI / 180.0)) != 0.0){
        fgyro = bno.euler.yaw * (M_PI / 180.0);
        bno.get_angles();
    }
    t.start();                                           
    double start;
    while(1){
        start = t.read();
        
        cor1 = 2*M_PI*enc1.get()/1024.0;
        cor2 = 2*M_PI*enc2.get()/1024.0;
        cor3 = 2*M_PI*enc3.get()/1024.0;
        cor4 = 2*M_PI*enc4.get()/1024.0;
        
        W.set(  4,
                (old1-cor1)/0.01,//enc1.getSpeed(),
                (old2-cor2)/0.01,//enc2.getSpeed(),
                (old3-cor3)/0.01,//enc3.getSpeed(),
                (old4-cor4)/0.01//enc4.getSpeed()
        );
        
        old1 = cor1;
        old2 = cor2;
        old3 = cor3;
        old4 = cor4;
        
        //W.show();
        bno.get_angles();
        gyro = bno.euler.yaw * (M_PI / 180.0);
        theta = abs(fgyro - gyro);  
        J.set(  12,
                -sin(theta), -sin(theta + M_PI/2.0), -sin(theta + M_PI), -sin(theta + 3.0*M_PI/2.0),
                 cos(theta),  cos(theta + M_PI/2.0),  cos(theta + M_PI),  cos(theta + 3.0*M_PI/2.0),
                1.0/(2.0*R),            1.0/(2.0*R),        1.0/(2.0*R),                1.0/(2.0*R)
        );
        //J.show();

        Vr = J*W*(r/2);
        //Vr.show();
        
        x += ((vx + Vr[0][0]) * 0.01) / 2.0;
        y += ((vy + Vr[1][0]) * 0.01) / 2.0;
        vx = Vr[0][0];
        vy = Vr[1][0];
        
        pc.printf("x : %lf y : %lf  : %lf theta : %lf\r\n", x, y, Vr[2][0], theta);
        //pc.printf("vx : %lf vy : %lf\r\n",Vr[0][0], Vr[1][0]);
        
        while(t.read() < start + 0.01);
    }
}