Diff: main.cpp

Revision:

2:1ebd4e9e4218

Parent:

1:94e29063fb0d

--- a/main.cpp	Sat Sep 06 00:47:38 2014 +0000
+++ b/main.cpp	Fri Apr 15 11:30:51 2016 +0000
@@ -1,20 +1,215 @@
 
 #include <mbed.h>
-#include "MMA7660.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;
+        }    
+    }
+}
 
-MMA7660 accelemeter;
+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()
 {
-    int8_t x, y, z;
-    float ax,ay,az;
-    accelemeter.init();  
+    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){
-        accelemeter.getXYZ(&x,&y,&z);
-        printf("X=%d, Y=%d, Z=%d, ",x,y,z);
+        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();
+        }
         
-        accelemeter.getAcceleration(&ax,&ay,&az);
-        printf("Accleration of X=%2.2fg, Y=%2.2fg, Z=%2.2fg\n\r",ax,ay,az);
-        wait(.5);
+        //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);
     }
 }
\ No newline at end of file