Screen-Puppet
Dependencies: PCA9547 PowerControl mbed
Diff: main.cpp
- Revision:
- 0:80f939ca1f14
--- /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"); + } + } +}