File content as of revision 2:1ebd4e9e4218:

#include <mbed.h>
#include "QEI.h"
#include "MPU6050.h"
#include <Timer.h>

MPU6050 MPUR(p9, p10);
MPU6050 MPUL(p28, p27);

Serial pc(USBTX, USBRX); // tx, rx

const float M_PI = 3.14156;
const float ACC_FACTOR = 2.0/32767.0;
const float GYR_FACTOR = (250.0/32767.0);
// Default alpha, for non-dynamic use
const float ALPHA = 0.03;
// Gyro vector magnitude thresholds for dynamic alpha calculation
// With Gyro sens of +/- 250, the max magnitude is slightly over 433
const float MAGN_MIN = 5.0;
const float MAGN_MAX = 10.0;
const float ALPHA_MIN = 0.02; 
const float ALPHA_MAX = 0.1; 

float alpha[2];
const float DT = 0.01;
#define LEFT 0
#define RIGHT 1
 
int accSmpCount[2]; 
 
float Acc[2][3];
float Gyr[2][3];

float pitchL, yawL, rollL, pitchR, yawR, rollR;
float pitchAcc[2], rollAcc[2];

QEI encL (p5, p6, NC, 5000);
QEI encR (p7, p8, NC, 5000);

//Timer lowerResTimer; // Software-wise lowers resolution of the encoder, for testing
//float atten = 1.0;
DigitalOut myLed1(LED1);

void GetEncoderAngles(){
    /*lowerResTimer.start();
    // 40s which is 8 1.8
    if (lowerResTimer.read_ms() > 5000.0){
          atten += 0.1;
          lowerResTimer.reset();  
    }*/
    float atten = 5.0;
    yawL = floor((float)encL.getPulses()/atten)*180.0/(5000.0/atten);
    yawR = floor((float)encR.getPulses()/atten)*180.0/(5000.0/atten);
}

void GetIMUAnglesAvg(char sensor){
    //Low Pass Filter
    if (sensor == LEFT){
        Acc[LEFT][0] += (double)MPUL.getAcceleroRawX()*ACC_FACTOR * 0.33;
        Acc[LEFT][1] += (double)MPUL.getAcceleroRawY()*ACC_FACTOR * 0.33;
        Acc[LEFT][2] += (double)MPUL.getAcceleroRawZ()*ACC_FACTOR * 0.33;
        Gyr[LEFT][0] += (double)MPUL.getGyroRawX()*GYR_FACTOR * 0.33;
        Gyr[LEFT][1] += (double)MPUL.getGyroRawY()*GYR_FACTOR * 0.33;
        Gyr[LEFT][2] += (double)MPUL.getGyroRawZ()*GYR_FACTOR * 0.33;
    }
    //Low Pass Filter
    if (sensor == RIGHT){
        Acc[RIGHT][0] += (double)MPUR.getAcceleroRawX()*ACC_FACTOR * 0.33;
        Acc[RIGHT][1] += (double)MPUR.getAcceleroRawY()*ACC_FACTOR * 0.33;
        Acc[RIGHT][2] += (double)MPUR.getAcceleroRawZ()*ACC_FACTOR * 0.33;
        Gyr[RIGHT][0] += (double)MPUR.getGyroRawX()*GYR_FACTOR * 0.33;
        Gyr[RIGHT][1] += (double)MPUR.getGyroRawY()*GYR_FACTOR * 0.33;
        Gyr[RIGHT][2] += (double)MPUR.getGyroRawZ()*GYR_FACTOR * 0.33;
    }
}

void Purge(){
    for (int i = 0; i < 2; i++){
        for (int j = 0; j < 3; j++){
            Acc[i][j] = 0;
            Gyr[i][j] = 0;
        }    
    }
}

void CalcAccAngles(){
    //Roll & Pitch Equations
    rollAcc[LEFT]  = (atan2(-Acc[LEFT][1], Acc[LEFT][2])*180.0)/M_PI;
    pitchAcc[LEFT] = (atan2(Acc[LEFT][0], sqrt(Acc[LEFT][1]*Acc[LEFT][1] + Acc[LEFT][2]*Acc[LEFT][2]))*180.0)/M_PI;        
    //Roll & Pitch Equations
    rollAcc[RIGHT]  = (atan2(-Acc[RIGHT][1], Acc[RIGHT][2])*180.0)/M_PI;
    pitchAcc[RIGHT] = (atan2(Acc[RIGHT][0], sqrt(Acc[RIGHT][1]*Acc[RIGHT][1] + Acc[RIGHT][2]*Acc[RIGHT][2]))*180.0)/M_PI; 
}

void SendPrintDebug(){
    pc.printf("PitchL=%6.2f, YawL=%6.2f, RollL=%6.2f, PitchR=%6.2f, YawR=%6.2f, RollR=%6.2f\n\r",pitchL,yawL,rollL,pitchR,yawR,rollR);
}

void SendBytes(){
    short Lx, Ly, Lz, Rx, Ry, Rz;
    char buffer[16];
    
    Lx = (short) (pitchL * 32767.0/180.0);
    Ly = (short) (yawL * 32767.0/180.0);
    Lz = (short) ((rollL) * 32767.0/180.0);

    Rx = (short) (pitchR * 32767.0/180.0);
    Ry = (short) (yawR * 32767.0/180.0);
    Rz = (short) ((rollR) * 32767.0/180.0);
    
    buffer[0]  = '#';
    buffer[1]  = '!';
    buffer[3]  = (char) (Lx >> 8 & 0xFF);
    buffer[2]  = (char) (Lx & 0xFF);  
    buffer[5]  = (char) (Ly >> 8 & 0xFF);
    buffer[4]  = (char) (Ly & 0xFF);  
    buffer[7]  = (char) (Lz >> 8 & 0xFF);
    buffer[6]  = (char) (Lz & 0xFF);  
    buffer[9]  = (char) (Rx >> 8 & 0xFF);
    buffer[8]  = (char) (Rx & 0xFF);  
    buffer[11] = (char) (Ry >> 8 & 0xFF);
    buffer[10] = (char) (Ry & 0xFF);  
    buffer[13] = (char) (Rz >> 8 & 0xFF);
    buffer[12] = (char) (Rz & 0xFF);
    
    buffer[14] = '~';
    buffer[15] = '~';
    pc.printf("%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c%c",buffer[0],buffer[1],buffer[2],buffer[3],buffer[4],buffer[5],buffer[6],buffer[7],buffer[8],buffer[9],buffer[10],buffer[11],buffer[12],buffer[13],buffer[14],buffer[15]);
    //pc.write(buffer, sizeof(buffer));
}

void Complementary () {
    // a=tau / (tau + loop time)
    // newAngle = angle measured with atan2 using the accelerometer
    // newRate = angle measured using the gyro
    // dt = loop time in seconds

    // Integrate the gyroscope data -> int(angularSpeed) = angle
    // Only integrate if faster than 0.075 degrees/s, to reduce drift
    // No active person moves that slow, I guess ... This is most important
    // for Yaw (i.e. gyro[2] since that angle cannot be compensated for using acc.)
    //if (fabs(gyro[2] * dt) > 0.075)
    pitchL += Gyr[0][1] * DT; // Angle around the X-axis, roll
    rollL  += Gyr[0][0] * DT; // Angle around the Z-axis, pitch
    
    pitchR += Gyr[1][1] * DT; // Angle around the X-axis, roll
    rollR  -= Gyr[1][0] * DT; // Angle around the Z-axis, pitch

    // Compensate for drift with accelerometer data if !bullshit
    // Sensitivity = -16 to 16 G at 16Bit -> 16G = 32768 && 0.5G = 1024
    pitchL += alpha[LEFT] * (pitchAcc[LEFT] - pitchL);
    rollL  += alpha[LEFT] * (rollAcc[LEFT]  - rollL);

    pitchR += alpha[RIGHT] * (pitchAcc[RIGHT] - pitchR);
    rollR  += alpha[RIGHT] * (rollAcc[RIGHT]  - rollR);
    
    // Override that fucking shit angle, we don't use it now anyway grrrr
    rollL = rollR = 0;
}

void GetDynamicAlpha(int Sensor){
    float magn;
    magn = sqrt(pow(Gyr[Sensor][0],2)+pow(Gyr[Sensor][1],2)+pow(Gyr[Sensor][2],2));
    if (magn > MAGN_MIN && magn < MAGN_MAX) alpha[Sensor] = ALPHA_MAX * (magn / MAGN_MAX);
    else if (magn > MAGN_MAX) alpha[Sensor] = ALPHA_MAX;
    else alpha[Sensor] = ALPHA_MIN;
}

int main()
{
    myLed1=1;
    Timer sendtimer, updateAccTimer;
    pc.baud(115200);
    MPUL.setAcceleroRange(MPU6050_ACCELERO_RANGE_2G);
    MPUR.setAcceleroRange(MPU6050_ACCELERO_RANGE_2G);
    MPUL.setGyroRange(MPU6050_GYRO_RANGE_250);
    MPUR.setGyroRange(MPU6050_GYRO_RANGE_250);
    
    pitchL = rollL = pitchR = rollR = 0;
    alpha[0] = alpha[1] = ALPHA;
    
    sendtimer.start();
    while(1){
        if(MPUL.read(MPU6050_INT_STATUS) & 0x01){ // check if data ready interrupt
            GetIMUAnglesAvg(LEFT);
            accSmpCount[LEFT]++;        
        }
        if(MPUR.read(MPU6050_INT_STATUS) & 0x01){ // check if data ready interrupt
            GetIMUAnglesAvg(RIGHT);
            accSmpCount[RIGHT]++;   
        }

        if (sendtimer.read_ms () >= 10){
            // Dynamic alpha based on amount of motion
            GetDynamicAlpha(LEFT);
            GetDynamicAlpha(RIGHT);
            
            //pc.printf("%d   %d  \n\r", accSmpCount[LEFT], accSmpCount[RIGHT]);
            //pc.printf("%f6.6   %f6.6  \n\r", Gyr[LEFT][0], Gyr[LEFT][2]);
            GetEncoderAngles();
            CalcAccAngles();
            Complementary ();
            SendBytes();
            //SendPrintDebug();
            sendtimer.reset();
            accSmpCount[LEFT] = accSmpCount[RIGHT] = 0;    
            // Purge 
            Purge();
        }
        
        //pc.printf("1  of X=%2.2fg, Y=%2.2fg, Z=%2.2fg",MMA1.x(),MMA1.y(),MMA1.z());
        //pc.printf("  2  of X=%2.2fg, Y=%2.2fg, Z=%2.2fg\n\r",MMA2.x(),MMA2.y(),MMA2.z());
        //wait(.01);
    }
}