Stephane Swanepoel / Mbed 2 deprecated mbed_multiplex_matrix

Screen-Puppet

Dependencies:   Matrix MatrixMath PCA9547 PowerControl mbed

Fork of mbed_multiplex by Stephane Swanepoel

main.cpp@1:f0f34b17c4f0, 2015-09-04 (annotated)

Committer:
yenzo
Date:
Fri Sep 04 21:44:01 2015 +0000
Revision:
1:f0f34b17c4f0
Parent:
0:80f939ca1f14 
With matrix

Who changed what in which revision?

UserRevisionLine numberNew 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 }