Diff: main.cpp

Revision:

0:80f939ca1f14

diff -r 000000000000 -r 80f939ca1f14 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Sep 04 21:37:38 2015 +0000
@@ -0,0 +1,116 @@
+#include "PCA9547/PCA9547.h"
+#include "PowerControl/PowerControl.h"
+#include "PowerControl/EthernetPowerControl.h"
+#define I2C_SDA p9 //28
+#define I2C_SCL p10 //27
+
+I2C i2c(I2C_SDA, I2C_SCL);
+
+PCA9547 mux( i2c, 0xE0 );
+int selectmux = 0;
+
+Serial pc(USBTX, USBRX); // tx, rx
+Timer t;
+int delt_t = 0, count = 0;
+int tpsend = 0;
+
+int IMU_DATA[9][3];
+int IMU_DATA_COMP[9][3];
+
+#include "MPU9250.h"
+
+MPU9250 IMU1(1), IMU2(2), IMU3(3), IMU4(4), IMU5(5), IMU6(6), IMU7(7), IMU8(8), IMU9(9);
+MPU9250 NIMU[9];
+
+void flushSerialBuffer(void) { 
+    char char1 = 0; 
+    while (pc.readable()) { 
+        char1 = pc.getc(); 
+    } 
+    return; 
+}
+
+int main() { 
+    NIMU[0] = IMU1;
+    NIMU[1] = IMU2;
+    NIMU[2] = IMU3;
+    NIMU[3] = IMU4;
+    NIMU[4] = IMU5;
+    NIMU[5] = IMU6;
+    NIMU[6] = IMU7;
+    NIMU[7] = IMU8;
+    NIMU[8] = IMU9;
+    
+    pc.baud(9600);  
+    i2c.frequency(400000);
+    PHY_PowerDown(); //Eteind le module Ethernet du Mbed afin d'économiser l'energie 
+    t.start();
+ 
+    for(int i = 0; i<8; i++){      
+        mux.select( i );
+        wait_us(5);  
+        /*i2c.stop();
+        wait_us(5); 
+        i2c.start();*/
+ 
+        uint8_t whoami_imu = 0;
+        while(whoami_imu != 0x71){
+            whoami_imu = NIMU[i].readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250);  
+        } 
+              
+        NIMU[i].CalibIMU();    
+        NIMU[i].BiasIMU();  
+        
+        pc.printf("Initilisation IMU numero : %d OK\n\r", i+1); 
+        pc.printf("\n\r");
+    }
+      
+    selectmux = 0;
+    mux.select( selectmux ); 
+    wait_us(10);  
+    /*i2c.stop();
+    wait_us(10); 
+    i2c.start();*/
+              
+    while(1){      
+
+        if(NIMU[selectmux].readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01){        
+            NIMU[selectmux].GetQuaternion();
+        }
+               
+        delt_t = t.read_ms() - count;
+        
+        if (delt_t > 20) {   
+            NIMU[selectmux].MadgwickQuaternionUpdate();
+            NIMU[selectmux].FinalQuaternion(); 
+            
+            IMU_DATA[selectmux][0] = NIMU[selectmux].GetPitch();
+            IMU_DATA[selectmux][1] = NIMU[selectmux].GetRoll();
+            IMU_DATA[selectmux][2] = NIMU[selectmux].GetYaw();
+             
+            selectmux++;
+            if(selectmux == 8) selectmux = 0;            
+            mux.select( selectmux );
+            wait_us(10);   
+            /*i2c.stop();
+            wait_us(10); 
+            i2c.start();*/
+            
+            tpsend+=delt_t;
+            count = t.read_ms(); 
+            
+            flushSerialBuffer();
+        }
+
+        if(tpsend > 500){                  
+            tpsend = 0;     
+
+            for(int i=0;i<8;i++){
+                //pc.printf("%d;%d;%d\n\r",IMU_DATA[i][0], IMU_DATA[i][1], IMU_DATA[i][2]);// 0 = Pitch, 1 = Roll et 2 = Yaw
+                pc.printf("IMU n%d, Pitch = %d; Roll = %d; Yaw = %d\n\r",i, IMU_DATA[i][0], IMU_DATA[i][1], IMU_DATA[i][2]);// 0 = Pitch, 1 = Roll et 2 = Yaw
+            }
+            
+            pc.printf("\n"); 
+        }       
+    }
+}