main.cpp

00001 
00002 #include <mbed.h>
00003 #include "QEI.h"
00004 #include "MPU6050.h"
00005 #include <Timer.h>
00006 
00007 MPU6050 MPUR(p9, p10);
00008 MPU6050 MPUL(p28, p27);
00009 
00010 Serial pc(USBTX, USBRX); // tx, rx
00011 
00012 const float M_PI = 3.14156;
00013 const float ACC_FACTOR = 2.0/32767.0;
00014 const float GYR_FACTOR = (250.0/32767.0);
00015 // Default alpha, for non-dynamic use
00016 const float ALPHA = 0.03;
00017 // Gyro vector magnitude thresholds for dynamic alpha calculation
00018 // With Gyro sens of +/- 250, the max magnitude is slightly over 433
00019 const float MAGN_MIN = 5.0;
00020 const float MAGN_MAX = 10.0;
00021 const float ALPHA_MIN = 0.02; 
00022 const float ALPHA_MAX = 0.1; 
00023 
00024 float alpha[2];
00025 const float DT = 0.01;
00026 #define LEFT 0
00027 #define RIGHT 1
00028  
00029 int accSmpCount[2]; 
00030  
00031 float Acc[2][3];
00032 float Gyr[2][3];
00033 
00034 float pitchL, yawL, rollL, pitchR, yawR, rollR;
00035 float pitchAcc[2], rollAcc[2];
00036 
00037 QEI encL (p5, p6, NC, 5000);
00038 QEI encR (p7, p8, NC, 5000);
00039 
00040 //Timer lowerResTimer; // Software-wise lowers resolution of the encoder, for testing
00041 //float atten = 1.0;
00042 DigitalOut myLed1(LED1);
00043 
00044 void GetEncoderAngles(){
00045     /*lowerResTimer.start();
00046     // 40s which is 8 1.8
00047     if (lowerResTimer.read_ms() > 5000.0){
00048           atten += 0.1;
00049           lowerResTimer.reset();  
00050     }*/
00051     float atten = 5.0;
00052     yawL = floor((float)encL.getPulses()/atten)*180.0/(5000.0/atten);
00053     yawR = floor((float)encR.getPulses()/atten)*180.0/(5000.0/atten);
00054 }
00055 
00056 void GetIMUAnglesAvg(char sensor){
00057     //Low Pass Filter
00058     if (sensor == LEFT){
00059         Acc[LEFT][0] += (double)MPUL.getAcceleroRawX()*ACC_FACTOR * 0.33;
00060         Acc[LEFT][1] += (double)MPUL.getAcceleroRawY()*ACC_FACTOR * 0.33;
00061         Acc[LEFT][2] += (double)MPUL.getAcceleroRawZ()*ACC_FACTOR * 0.33;
00062         Gyr[LEFT][0] += (double)MPUL.getGyroRawX()*GYR_FACTOR * 0.33;
00063         Gyr[LEFT][1] += (double)MPUL.getGyroRawY()*GYR_FACTOR * 0.33;
00064         Gyr[LEFT][2] += (double)MPUL.getGyroRawZ()*GYR_FACTOR * 0.33;
00065     }
00066     //Low Pass Filter
00067     if (sensor == RIGHT){
00068         Acc[RIGHT][0] += (double)MPUR.getAcceleroRawX()*ACC_FACTOR * 0.33;
00069         Acc[RIGHT][1] += (double)MPUR.getAcceleroRawY()*ACC_FACTOR * 0.33;
00070         Acc[RIGHT][2] += (double)MPUR.getAcceleroRawZ()*ACC_FACTOR * 0.33;
00071         Gyr[RIGHT][0] += (double)MPUR.getGyroRawX()*GYR_FACTOR * 0.33;
00072         Gyr[RIGHT][1] += (double)MPUR.getGyroRawY()*GYR_FACTOR * 0.33;
00073         Gyr[RIGHT][2] += (double)MPUR.getGyroRawZ()*GYR_FACTOR * 0.33;
00074     }
00075 }
00076 
00077 void Purge(){
00078     for (int i = 0; i < 2; i++){
00079         for (int j = 0; j < 3; j++){
00080             Acc[i][j] = 0;
00081             Gyr[i][j] = 0;
00082         }    
00083     }
00084 }
00085 
00086 void CalcAccAngles(){
00087     //Roll & Pitch Equations
00088     rollAcc[LEFT]  = (atan2(-Acc[LEFT][1], Acc[LEFT][2])*180.0)/M_PI;
00089     pitchAcc[LEFT] = (atan2(Acc[LEFT][0], sqrt(Acc[LEFT][1]*Acc[LEFT][1] + Acc[LEFT][2]*Acc[LEFT][2]))*180.0)/M_PI;        
00090     //Roll & Pitch Equations
00091     rollAcc[RIGHT]  = (atan2(-Acc[RIGHT][1], Acc[RIGHT][2])*180.0)/M_PI;
00092     pitchAcc[RIGHT] = (atan2(Acc[RIGHT][0], sqrt(Acc[RIGHT][1]*Acc[RIGHT][1] + Acc[RIGHT][2]*Acc[RIGHT][2]))*180.0)/M_PI; 
00093 }
00094 
00095 void SendPrintDebug(){
00096     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);
00097 }
00098 
00099 void SendBytes(){
00100     short Lx, Ly, Lz, Rx, Ry, Rz;
00101     char buffer[16];
00102     
00103     Lx = (short) (pitchL * 32767.0/180.0);
00104     Ly = (short) (yawL * 32767.0/180.0);
00105     Lz = (short) ((rollL) * 32767.0/180.0);
00106 
00107     Rx = (short) (pitchR * 32767.0/180.0);
00108     Ry = (short) (yawR * 32767.0/180.0);
00109     Rz = (short) ((rollR) * 32767.0/180.0);
00110     
00111     buffer[0]  = '#';
00112     buffer[1]  = '!';
00113     buffer[3]  = (char) (Lx >> 8 & 0xFF);
00114     buffer[2]  = (char) (Lx & 0xFF);  
00115     buffer[5]  = (char) (Ly >> 8 & 0xFF);
00116     buffer[4]  = (char) (Ly & 0xFF);  
00117     buffer[7]  = (char) (Lz >> 8 & 0xFF);
00118     buffer[6]  = (char) (Lz & 0xFF);  
00119     buffer[9]  = (char) (Rx >> 8 & 0xFF);
00120     buffer[8]  = (char) (Rx & 0xFF);  
00121     buffer[11] = (char) (Ry >> 8 & 0xFF);
00122     buffer[10] = (char) (Ry & 0xFF);  
00123     buffer[13] = (char) (Rz >> 8 & 0xFF);
00124     buffer[12] = (char) (Rz & 0xFF);
00125     
00126     buffer[14] = '~';
00127     buffer[15] = '~';
00128     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]);
00129     //pc.write(buffer, sizeof(buffer));
00130 }
00131 
00132 void Complementary () {
00133     // a=tau / (tau + loop time)
00134     // newAngle = angle measured with atan2 using the accelerometer
00135     // newRate = angle measured using the gyro
00136     // dt = loop time in seconds
00137 
00138     // Integrate the gyroscope data -> int(angularSpeed) = angle
00139     // Only integrate if faster than 0.075 degrees/s, to reduce drift
00140     // No active person moves that slow, I guess ... This is most important
00141     // for Yaw (i.e. gyro[2] since that angle cannot be compensated for using acc.)
00142     //if (fabs(gyro[2] * dt) > 0.075)
00143     pitchL += Gyr[0][1] * DT; // Angle around the X-axis, roll
00144     rollL  += Gyr[0][0] * DT; // Angle around the Z-axis, pitch
00145     
00146     pitchR += Gyr[1][1] * DT; // Angle around the X-axis, roll
00147     rollR  -= Gyr[1][0] * DT; // Angle around the Z-axis, pitch
00148 
00149     // Compensate for drift with accelerometer data if !bullshit
00150     // Sensitivity = -16 to 16 G at 16Bit -> 16G = 32768 && 0.5G = 1024
00151     pitchL += alpha[LEFT] * (pitchAcc[LEFT] - pitchL);
00152     rollL  += alpha[LEFT] * (rollAcc[LEFT]  - rollL);
00153 
00154     pitchR += alpha[RIGHT] * (pitchAcc[RIGHT] - pitchR);
00155     rollR  += alpha[RIGHT] * (rollAcc[RIGHT]  - rollR);
00156     
00157     // Override that fucking shit angle, we don't use it now anyway grrrr
00158     rollL = rollR = 0;
00159 }
00160 
00161 void GetDynamicAlpha(int Sensor){
00162     float magn;
00163     magn = sqrt(pow(Gyr[Sensor][0],2)+pow(Gyr[Sensor][1],2)+pow(Gyr[Sensor][2],2));
00164     if (magn > MAGN_MIN && magn < MAGN_MAX) alpha[Sensor] = ALPHA_MAX * (magn / MAGN_MAX);
00165     else if (magn > MAGN_MAX) alpha[Sensor] = ALPHA_MAX;
00166     else alpha[Sensor] = ALPHA_MIN;
00167 }
00168 
00169 int main()
00170 {
00171     myLed1=1;
00172     Timer sendtimer, updateAccTimer;
00173     pc.baud(115200);
00174     MPUL.setAcceleroRange(MPU6050_ACCELERO_RANGE_2G);
00175     MPUR.setAcceleroRange(MPU6050_ACCELERO_RANGE_2G);
00176     MPUL.setGyroRange(MPU6050_GYRO_RANGE_250);
00177     MPUR.setGyroRange(MPU6050_GYRO_RANGE_250);
00178     
00179     pitchL = rollL = pitchR = rollR = 0;
00180     alpha[0] = alpha[1] = ALPHA;
00181     
00182     sendtimer.start();
00183     while(1){
00184         if(MPUL.read(MPU6050_INT_STATUS) & 0x01){ // check if data ready interrupt
00185             GetIMUAnglesAvg(LEFT);
00186             accSmpCount[LEFT]++;        
00187         }
00188         if(MPUR.read(MPU6050_INT_STATUS) & 0x01){ // check if data ready interrupt
00189             GetIMUAnglesAvg(RIGHT);
00190             accSmpCount[RIGHT]++;   
00191         }
00192 
00193         if (sendtimer.read_ms () >= 10){
00194             // Dynamic alpha based on amount of motion
00195             GetDynamicAlpha(LEFT);
00196             GetDynamicAlpha(RIGHT);
00197             
00198             //pc.printf("%d   %d  \n\r", accSmpCount[LEFT], accSmpCount[RIGHT]);
00199             //pc.printf("%f6.6   %f6.6  \n\r", Gyr[LEFT][0], Gyr[LEFT][2]);
00200             GetEncoderAngles();
00201             CalcAccAngles();
00202             Complementary ();
00203             SendBytes();
00204             //SendPrintDebug();
00205             sendtimer.reset();
00206             accSmpCount[LEFT] = accSmpCount[RIGHT] = 0;    
00207             // Purge 
00208             Purge();
00209         }
00210         
00211         //pc.printf("1  of X=%2.2fg, Y=%2.2fg, Z=%2.2fg",MMA1.x(),MMA1.y(),MMA1.z());
00212         //pc.printf("  2  of X=%2.2fg, Y=%2.2fg, Z=%2.2fg\n\r",MMA2.x(),MMA2.y(),MMA2.z());
00213         //wait(.01);
00214     }
00215 }