Screen-Puppet
Dependencies: Matrix MatrixMath PCA9547 PowerControl mbed
Fork of mbed_multiplex by
main.cpp@0:80f939ca1f14, 2015-09-04 (annotated)
- Committer:
- yenzo
- Date:
- Fri Sep 04 21:37:38 2015 +0000
- Revision:
- 0:80f939ca1f14
Screen-Puppet
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
yenzo | 0:80f939ca1f14 | 1 | #include "PCA9547/PCA9547.h" |
yenzo | 0:80f939ca1f14 | 2 | #include "PowerControl/PowerControl.h" |
yenzo | 0:80f939ca1f14 | 3 | #include "PowerControl/EthernetPowerControl.h" |
yenzo | 0:80f939ca1f14 | 4 | #define I2C_SDA p9 //28 |
yenzo | 0:80f939ca1f14 | 5 | #define I2C_SCL p10 //27 |
yenzo | 0:80f939ca1f14 | 6 | |
yenzo | 0:80f939ca1f14 | 7 | I2C i2c(I2C_SDA, I2C_SCL); |
yenzo | 0:80f939ca1f14 | 8 | |
yenzo | 0:80f939ca1f14 | 9 | PCA9547 mux( i2c, 0xE0 ); |
yenzo | 0:80f939ca1f14 | 10 | int selectmux = 0; |
yenzo | 0:80f939ca1f14 | 11 | |
yenzo | 0:80f939ca1f14 | 12 | Serial pc(USBTX, USBRX); // tx, rx |
yenzo | 0:80f939ca1f14 | 13 | Timer t; |
yenzo | 0:80f939ca1f14 | 14 | int delt_t = 0, count = 0; |
yenzo | 0:80f939ca1f14 | 15 | int tpsend = 0; |
yenzo | 0:80f939ca1f14 | 16 | |
yenzo | 0:80f939ca1f14 | 17 | int IMU_DATA[9][3]; |
yenzo | 0:80f939ca1f14 | 18 | int IMU_DATA_COMP[9][3]; |
yenzo | 0:80f939ca1f14 | 19 | |
yenzo | 0:80f939ca1f14 | 20 | #include "MPU9250.h" |
yenzo | 0:80f939ca1f14 | 21 | |
yenzo | 0:80f939ca1f14 | 22 | MPU9250 IMU1(1), IMU2(2), IMU3(3), IMU4(4), IMU5(5), IMU6(6), IMU7(7), IMU8(8), IMU9(9); |
yenzo | 0:80f939ca1f14 | 23 | MPU9250 NIMU[9]; |
yenzo | 0:80f939ca1f14 | 24 | |
yenzo | 0:80f939ca1f14 | 25 | void flushSerialBuffer(void) { |
yenzo | 0:80f939ca1f14 | 26 | char char1 = 0; |
yenzo | 0:80f939ca1f14 | 27 | while (pc.readable()) { |
yenzo | 0:80f939ca1f14 | 28 | char1 = pc.getc(); |
yenzo | 0:80f939ca1f14 | 29 | } |
yenzo | 0:80f939ca1f14 | 30 | return; |
yenzo | 0:80f939ca1f14 | 31 | } |
yenzo | 0:80f939ca1f14 | 32 | |
yenzo | 0:80f939ca1f14 | 33 | int main() { |
yenzo | 0:80f939ca1f14 | 34 | NIMU[0] = IMU1; |
yenzo | 0:80f939ca1f14 | 35 | NIMU[1] = IMU2; |
yenzo | 0:80f939ca1f14 | 36 | NIMU[2] = IMU3; |
yenzo | 0:80f939ca1f14 | 37 | NIMU[3] = IMU4; |
yenzo | 0:80f939ca1f14 | 38 | NIMU[4] = IMU5; |
yenzo | 0:80f939ca1f14 | 39 | NIMU[5] = IMU6; |
yenzo | 0:80f939ca1f14 | 40 | NIMU[6] = IMU7; |
yenzo | 0:80f939ca1f14 | 41 | NIMU[7] = IMU8; |
yenzo | 0:80f939ca1f14 | 42 | NIMU[8] = IMU9; |
yenzo | 0:80f939ca1f14 | 43 | |
yenzo | 0:80f939ca1f14 | 44 | pc.baud(9600); |
yenzo | 0:80f939ca1f14 | 45 | i2c.frequency(400000); |
yenzo | 0:80f939ca1f14 | 46 | PHY_PowerDown(); //Eteind le module Ethernet du Mbed afin d'économiser l'energie |
yenzo | 0:80f939ca1f14 | 47 | t.start(); |
yenzo | 0:80f939ca1f14 | 48 | |
yenzo | 0:80f939ca1f14 | 49 | for(int i = 0; i<8; i++){ |
yenzo | 0:80f939ca1f14 | 50 | mux.select( i ); |
yenzo | 0:80f939ca1f14 | 51 | wait_us(5); |
yenzo | 0:80f939ca1f14 | 52 | /*i2c.stop(); |
yenzo | 0:80f939ca1f14 | 53 | wait_us(5); |
yenzo | 0:80f939ca1f14 | 54 | i2c.start();*/ |
yenzo | 0:80f939ca1f14 | 55 | |
yenzo | 0:80f939ca1f14 | 56 | uint8_t whoami_imu = 0; |
yenzo | 0:80f939ca1f14 | 57 | while(whoami_imu != 0x71){ |
yenzo | 0:80f939ca1f14 | 58 | whoami_imu = NIMU[i].readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250); |
yenzo | 0:80f939ca1f14 | 59 | } |
yenzo | 0:80f939ca1f14 | 60 | |
yenzo | 0:80f939ca1f14 | 61 | NIMU[i].CalibIMU(); |
yenzo | 0:80f939ca1f14 | 62 | NIMU[i].BiasIMU(); |
yenzo | 0:80f939ca1f14 | 63 | |
yenzo | 0:80f939ca1f14 | 64 | pc.printf("Initilisation IMU numero : %d OK\n\r", i+1); |
yenzo | 0:80f939ca1f14 | 65 | pc.printf("\n\r"); |
yenzo | 0:80f939ca1f14 | 66 | } |
yenzo | 0:80f939ca1f14 | 67 | |
yenzo | 0:80f939ca1f14 | 68 | selectmux = 0; |
yenzo | 0:80f939ca1f14 | 69 | mux.select( selectmux ); |
yenzo | 0:80f939ca1f14 | 70 | wait_us(10); |
yenzo | 0:80f939ca1f14 | 71 | /*i2c.stop(); |
yenzo | 0:80f939ca1f14 | 72 | wait_us(10); |
yenzo | 0:80f939ca1f14 | 73 | i2c.start();*/ |
yenzo | 0:80f939ca1f14 | 74 | |
yenzo | 0:80f939ca1f14 | 75 | while(1){ |
yenzo | 0:80f939ca1f14 | 76 | |
yenzo | 0:80f939ca1f14 | 77 | if(NIMU[selectmux].readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01){ |
yenzo | 0:80f939ca1f14 | 78 | NIMU[selectmux].GetQuaternion(); |
yenzo | 0:80f939ca1f14 | 79 | } |
yenzo | 0:80f939ca1f14 | 80 | |
yenzo | 0:80f939ca1f14 | 81 | delt_t = t.read_ms() - count; |
yenzo | 0:80f939ca1f14 | 82 | |
yenzo | 0:80f939ca1f14 | 83 | if (delt_t > 20) { |
yenzo | 0:80f939ca1f14 | 84 | NIMU[selectmux].MadgwickQuaternionUpdate(); |
yenzo | 0:80f939ca1f14 | 85 | NIMU[selectmux].FinalQuaternion(); |
yenzo | 0:80f939ca1f14 | 86 | |
yenzo | 0:80f939ca1f14 | 87 | IMU_DATA[selectmux][0] = NIMU[selectmux].GetPitch(); |
yenzo | 0:80f939ca1f14 | 88 | IMU_DATA[selectmux][1] = NIMU[selectmux].GetRoll(); |
yenzo | 0:80f939ca1f14 | 89 | IMU_DATA[selectmux][2] = NIMU[selectmux].GetYaw(); |
yenzo | 0:80f939ca1f14 | 90 | |
yenzo | 0:80f939ca1f14 | 91 | selectmux++; |
yenzo | 0:80f939ca1f14 | 92 | if(selectmux == 8) selectmux = 0; |
yenzo | 0:80f939ca1f14 | 93 | mux.select( selectmux ); |
yenzo | 0:80f939ca1f14 | 94 | wait_us(10); |
yenzo | 0:80f939ca1f14 | 95 | /*i2c.stop(); |
yenzo | 0:80f939ca1f14 | 96 | wait_us(10); |
yenzo | 0:80f939ca1f14 | 97 | i2c.start();*/ |
yenzo | 0:80f939ca1f14 | 98 | |
yenzo | 0:80f939ca1f14 | 99 | tpsend+=delt_t; |
yenzo | 0:80f939ca1f14 | 100 | count = t.read_ms(); |
yenzo | 0:80f939ca1f14 | 101 | |
yenzo | 0:80f939ca1f14 | 102 | flushSerialBuffer(); |
yenzo | 0:80f939ca1f14 | 103 | } |
yenzo | 0:80f939ca1f14 | 104 | |
yenzo | 0:80f939ca1f14 | 105 | if(tpsend > 500){ |
yenzo | 0:80f939ca1f14 | 106 | tpsend = 0; |
yenzo | 0:80f939ca1f14 | 107 | |
yenzo | 0:80f939ca1f14 | 108 | for(int i=0;i<8;i++){ |
yenzo | 0:80f939ca1f14 | 109 | //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 |
yenzo | 0:80f939ca1f14 | 110 | 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 |
yenzo | 0:80f939ca1f14 | 111 | } |
yenzo | 0:80f939ca1f14 | 112 | |
yenzo | 0:80f939ca1f14 | 113 | pc.printf("\n"); |
yenzo | 0:80f939ca1f14 | 114 | } |
yenzo | 0:80f939ca1f14 | 115 | } |
yenzo | 0:80f939ca1f14 | 116 | } |