Suchakhree Srisukprom / Mbed 2 deprecated Test_Megre

k

Dependencies:   mbed

main.cpp@1:426fbd0d126a, 2015-12-09 (annotated)

Committer:
Suchakhree
Date:
Wed Dec 09 10:19:11 2015 +0000
Revision:
1:426fbd0d126a
Parent:
0:238df339023b 
????????????????? ????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Suchakhree 0:238df339023b 1 #include "mbed.h"
Suchakhree 0:238df339023b 2 #include "MPU9250.h"
Suchakhree 0:238df339023b 3 #include "TextLCD.h"
Suchakhree 0:238df339023b 4
Suchakhree 1:426fbd0d126a 5 Serial pc(USBTX, USBRX); // Huyperterminal default config: 9600 bauds
Suchakhree 0:238df339023b 6 Serial mobile(D8,D2);
Suchakhree 0:238df339023b 7 //mobile.baud(38400);
Suchakhree 0:238df339023b 8
Suchakhree 0:238df339023b 9 MPU9250 mpu9250;
Suchakhree 0:238df339023b 10 Timer t;
Suchakhree 0:238df339023b 11 //DigitalOut myled(LED1);
Suchakhree 0:238df339023b 12
Suchakhree 0:238df339023b 13 I2C i2c_lcd(D14,D15); // SDA, SCL
Suchakhree 0:238df339023b 14 TextLCD_I2C lcd(&i2c_lcd, 0x4E, TextLCD::LCD20x4); // I2C bus, PCF8574 Slaveaddress, LCD Type
Suchakhree 0:238df339023b 15
Suchakhree 1:426fbd0d126a 16 char pack[14]={0};
Suchakhree 0:238df339023b 17
Suchakhree 1:426fbd0d126a 18 int Ay[2]={0};
Suchakhree 0:238df339023b 19 float sum=0;
Suchakhree 0:238df339023b 20 uint32_t sumCount = 0;
Suchakhree 0:238df339023b 21 char buffer[14];
Suchakhree 0:238df339023b 22 uint8_t dato_leido[2];
Suchakhree 0:238df339023b 23 uint8_t whoami;
Suchakhree 1:426fbd0d126a 24 int GetAcceloroY();
Suchakhree 0:238df339023b 25
Suchakhree 1:426fbd0d126a 26 DigitalOut pinled0(PC_8); DigitalOut pinled1(PC_6); DigitalOut pinled2(PC_5);
Suchakhree 1:426fbd0d126a 27 DigitalOut pinled3(PA_12); DigitalOut pinled4(PA_11); DigitalOut pinled5(PB_12);
Suchakhree 1:426fbd0d126a 28 DigitalOut pinled6(PB_2); DigitalOut pinled7(PB_15); DigitalOut pinled8(PB_14);
Suchakhree 1:426fbd0d126a 29 DigitalOut pinled9(PB_13); DigitalOut pinbuzz(D3);
Suchakhree 0:238df339023b 30 void resetled();
Suchakhree 0:238df339023b 31 void getdistance2led(char step);
Suchakhree 0:238df339023b 32
Suchakhree 1:426fbd0d126a 33 InterruptIn switch1(D11);//TR
Suchakhree 1:426fbd0d126a 34 InterruptIn switch2(D10);//TL
Suchakhree 1:426fbd0d126a 35 InterruptIn switch3(D7);//BR
Suchakhree 1:426fbd0d126a 36 void switch_1();
Suchakhree 1:426fbd0d126a 37 void switch_2();
Suchakhree 1:426fbd0d126a 38 void switch_3();
Suchakhree 1:426fbd0d126a 39 int matrix_state=0;
Suchakhree 0:238df339023b 40
Suchakhree 0:238df339023b 41 int main() {
Suchakhree 1:426fbd0d126a 42
Suchakhree 1:426fbd0d126a 43
Suchakhree 0:238df339023b 44 //___ Set up I2C: use fast (400 kHz) I2C ___
Suchakhree 0:238df339023b 45 i2c.frequency(400000);
Suchakhree 0:238df339023b 46 // Read the WHO_AM_I register, this is a good test of communication
Suchakhree 0:238df339023b 47 whoami = mpu9250.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250);
Suchakhree 0:238df339023b 48 pc.printf("I AM 0x%x\n\r", whoami); pc.printf("I SHOULD BE 0x71\n\r");
Suchakhree 0:238df339023b 49 if (I2Cstate != 0) // error on I2C
Suchakhree 0:238df339023b 50 pc.printf("I2C failure while reading WHO_AM_I register");
Suchakhree 0:238df339023b 51
Suchakhree 0:238df339023b 52 if (whoami == 0x71) // WHO_AM_I should always be 0x71
Suchakhree 0:238df339023b 53 {
Suchakhree 0:238df339023b 54 pc.printf("MPU9250 is online...\n\r");
Suchakhree 0:238df339023b 55 sprintf(buffer, "0x%x", whoami);
Suchakhree 0:238df339023b 56 wait(1);
Suchakhree 0:238df339023b 57 mpu9250.resetMPU9250(); // Reset registers to default in preparation for device calibration
Suchakhree 0:238df339023b 58 mpu9250.MPU9250SelfTest(SelfTest); // Start by performing self test and reporting values (accelerometer and gyroscope self test)
Suchakhree 0:238df339023b 59 mpu9250.calibrateMPU9250(gyroBias, accelBias); // Calibrate gyro and accelerometer, load biases in bias registers
Suchakhree 0:238df339023b 60 wait(2);
Suchakhree 1:426fbd0d126a 61 //Initialize device for active mode read of acclerometer, gyroscope, and temperature
Suchakhree 0:238df339023b 62 mpu9250.initMPU9250();
Suchakhree 0:238df339023b 63 pc.printf("MPU9250 initialized for active data mode....\n\r");
Suchakhree 1:426fbd0d126a 64 //Initialize device for active mode read of magnetometer, 16 bit resolution, 100Hz.
Suchakhree 0:238df339023b 65 mpu9250.initAK8963(magCalibration);
Suchakhree 0:238df339023b 66 wait(1);
Suchakhree 0:238df339023b 67 }
Suchakhree 0:238df339023b 68
Suchakhree 0:238df339023b 69 else // Connection failure
Suchakhree 0:238df339023b 70 {
Suchakhree 0:238df339023b 71 pc.printf("Could not connect to MPU9250: \n\r");
Suchakhree 0:238df339023b 72 pc.printf("%#x \n", whoami);
Suchakhree 0:238df339023b 73 sprintf(buffer, "WHO_AM_I 0x%x", whoami);
Suchakhree 1:426fbd0d126a 74 //while(1) ; // Loop forever if communication doesn't happen
Suchakhree 0:238df339023b 75 }
Suchakhree 0:238df339023b 76
Suchakhree 1:426fbd0d126a 77 mpu9250.getAres();
Suchakhree 0:238df339023b 78
Suchakhree 0:238df339023b 79 lcd.locate(0,0);
Suchakhree 1:426fbd0d126a 80 lcd.printf("FRA221 DigitalElec.");
Suchakhree 0:238df339023b 81 lcd.locate(0,1);
Suchakhree 0:238df339023b 82 lcd.printf("Bike_Light");
Suchakhree 0:238df339023b 83 resetled();
Suchakhree 1:426fbd0d126a 84 t.start();
Suchakhree 1:426fbd0d126a 85
Suchakhree 0:238df339023b 86 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
Suchakhree 0:238df339023b 87 while(1)
Suchakhree 1:426fbd0d126a 88 {
Suchakhree 1:426fbd0d126a 89
Suchakhree 1:426fbd0d126a 90 if(t.read_ms()<=50)
Suchakhree 0:238df339023b 91 {
Suchakhree 1:426fbd0d126a 92 //Read data
Suchakhree 1:426fbd0d126a 93 /*if(mobile.readable())
Suchakhree 1:426fbd0d126a 94 {
Suchakhree 1:426fbd0d126a 95 //pack[0] = mobile.getc();
Suchakhree 1:426fbd0d126a 96 for(int i=0;i<13;i++)
Suchakhree 1:426fbd0d126a 97 pack[i] = mobile.getc();
Suchakhree 1:426fbd0d126a 98
Suchakhree 1:426fbd0d126a 99 //Debug_Serial
Suchakhree 1:426fbd0d126a 100 for(int i=0;i<12;i++)
Suchakhree 1:426fbd0d126a 101 pc.printf("%c",pack[i]);
Suchakhree 1:426fbd0d126a 102 pc.printf("\n");
Suchakhree 1:426fbd0d126a 103 //lcd.locate(0,2);
Suchakhree 1:426fbd0d126a 104 //for(int i=0;i<14;i++) lcd.printf("%c",pack[i]);
Suchakhree 1:426fbd0d126a 105
Suchakhree 1:426fbd0d126a 106 if(pack[0] == '#')
Suchakhree 1:426fbd0d126a 107 {
Suchakhree 1:426fbd0d126a 108
Suchakhree 1:426fbd0d126a 109 if(pack[1] == 'D' && pack[4] == 'S' && pack[7] == 'K')
Suchakhree 1:426fbd0d126a 110 {
Suchakhree 1:426fbd0d126a 111 getdistance2led(pack[3]);
Suchakhree 1:426fbd0d126a 112 lcd.locate(0,2);
Suchakhree 1:426fbd0d126a 113 lcd.printf("SPEED : %c%c km/hr",pack[5],pack[6]);
Suchakhree 1:426fbd0d126a 114 lcd.locate(0,3);
Suchakhree 1:426fbd0d126a 115 lcd.printf("Distance: %c.%c%c km.",pack[8],pack[9],pack[10]);
Suchakhree 1:426fbd0d126a 116 //pc.printf("read\n");
Suchakhree 1:426fbd0d126a 117 }
Suchakhree 1:426fbd0d126a 118 }
Suchakhree 1:426fbd0d126a 119 }*/
Suchakhree 1:426fbd0d126a 120
Suchakhree 1:426fbd0d126a 121 Ay[0]=GetAcceloroY(); //pc.printf("1: %d\n",Ay[0]);
Suchakhree 1:426fbd0d126a 122 Ay[1]=GetAcceloroY(); //pc.printf("2: %d\n",Ay[1]);
Suchakhree 1:426fbd0d126a 123 if( Ay[1]+Ay[0]< -150 ) {matrix_state=3; pc.printf("#BR$\n\n\n"); }
Suchakhree 1:426fbd0d126a 124 switch1.rise(&switch_1);
Suchakhree 1:426fbd0d126a 125 switch2.rise(&switch_2);
Suchakhree 1:426fbd0d126a 126 switch3.rise(&switch_3);
Suchakhree 1:426fbd0d126a 127 if(matrix_state == 0){mobile.printf("#MED$"); pc.printf("#MED$");}
Suchakhree 1:426fbd0d126a 128 else if(matrix_state == 1){mobile.printf("#TR$"); pc.printf("#TR$\n");}
Suchakhree 1:426fbd0d126a 129 else if(matrix_state == 2){mobile.printf("#TL$"); pc.printf("#TL$\n");}
Suchakhree 1:426fbd0d126a 130 else if(matrix_state == 3){mobile.printf("#BR$"); pc.printf("#BR$\n");}
Suchakhree 1:426fbd0d126a 131 if(switch1 == 0 && switch2 == 0 && switch3 == 0) matrix_state = 0;
Suchakhree 1:426fbd0d126a 132 //if(switch1 == 0 && switch2 == 0 && switch3 == 0 && matrix_state == 0) {matrix_state = 1; mobile.printf("#MED$"); pc.printf("#MED$");}
Suchakhree 1:426fbd0d126a 133 t.reset();
Suchakhree 1:426fbd0d126a 134 }
Suchakhree 0:238df339023b 135 }
Suchakhree 0:238df339023b 136 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
Suchakhree 0:238df339023b 137 }
Suchakhree 0:238df339023b 138
Suchakhree 1:426fbd0d126a 139 void switch_1()
Suchakhree 1:426fbd0d126a 140 {
Suchakhree 1:426fbd0d126a 141 matrix_state=1;
Suchakhree 1:426fbd0d126a 142 //matrix_state = 0;
Suchakhree 1:426fbd0d126a 143 //mobile.printf("#TR$");
Suchakhree 1:426fbd0d126a 144 }
Suchakhree 1:426fbd0d126a 145 void switch_2()
Suchakhree 1:426fbd0d126a 146 {
Suchakhree 1:426fbd0d126a 147 matrix_state=2;
Suchakhree 1:426fbd0d126a 148 //matrix_state = 0;
Suchakhree 1:426fbd0d126a 149 //mobile.printf("#TL$");
Suchakhree 1:426fbd0d126a 150 }
Suchakhree 1:426fbd0d126a 151 void switch_3()
Suchakhree 1:426fbd0d126a 152 {
Suchakhree 1:426fbd0d126a 153 matrix_state=3;
Suchakhree 1:426fbd0d126a 154 //matrix_state = 0;
Suchakhree 1:426fbd0d126a 155 //mobile.printf("#BR$");
Suchakhree 1:426fbd0d126a 156 }
Suchakhree 1:426fbd0d126a 157
Suchakhree 0:238df339023b 158 void getdistance2led(char step)
Suchakhree 0:238df339023b 159 {
Suchakhree 0:238df339023b 160 resetled();
Suchakhree 0:238df339023b 161 if(step == '0'){
Suchakhree 0:238df339023b 162 pinled0=1; pinled1=0; pinled2=0; pinled3=0; pinled4=0; pinled5=0; pinled6=0; pinled7=0; pinled8=0; pinled9=0;}
Suchakhree 0:238df339023b 163 else if(step == '1'){
Suchakhree 0:238df339023b 164 pinled0=1; pinled1=1; pinled2=0; pinled3=0; pinled4=0; pinled5=0; pinled6=0; pinled7=0; pinled8=0; pinled9=0;}
Suchakhree 0:238df339023b 165 else if(step == '2'){
Suchakhree 0:238df339023b 166 pinled0=1; pinled1=1; pinled2=1; pinled3=0; pinled4=0; pinled5=0; pinled6=0; pinled7=0; pinled8=0; pinled9=0;}
Suchakhree 0:238df339023b 167 else if(step == '3'){
Suchakhree 0:238df339023b 168 pinled0=1; pinled1=1; pinled2=1; pinled3=1; pinled4=0; pinled5=0; pinled6=0; pinled7=0; pinled8=0; pinled9=0;}
Suchakhree 0:238df339023b 169 else if(step == '4'){
Suchakhree 0:238df339023b 170 pinled0=1; pinled1=1; pinled2=1; pinled3=1; pinled4=1; pinled5=0; pinled6=0; pinled7=0; pinled8=0; pinled9=0;}
Suchakhree 0:238df339023b 171 else if(step == '5'){
Suchakhree 0:238df339023b 172 pinled0=1; pinled1=1; pinled2=1; pinled3=1; pinled4=1; pinled5=1; pinled6=0; pinled7=0; pinled8=0; pinled9=0;}
Suchakhree 0:238df339023b 173 else if(step == '6'){
Suchakhree 0:238df339023b 174 pinled0=1; pinled1=1; pinled2=1; pinled3=1; pinled4=1; pinled5=1; pinled6=1; pinled7=0; pinled8=0; pinled9=0;}
Suchakhree 0:238df339023b 175 else if(step == '7'){
Suchakhree 1:426fbd0d126a 176 pinled0=1; pinled1=1; pinled2=1; pinled3=1; pinled4=1; pinled5=1; pinled6=1; pinled7=1; pinled8=0; pinled9=0;}
Suchakhree 0:238df339023b 177 else if(step == '8'){
Suchakhree 1:426fbd0d126a 178 pinled0=1; pinled1=1; pinled2=1; pinled3=1; pinled4=1; pinled5=1; pinled6=1; pinled7=1; pinled8=1; pinled9=0;}
Suchakhree 0:238df339023b 179 else if(step == '9'){
Suchakhree 0:238df339023b 180 pinled0=1; pinled1=1; pinled2=1; pinled3=1; pinled4=1; pinled5=1; pinled6=1; pinled7=1; pinled8=1; pinled9=1; pinbuzz=1;}
Suchakhree 0:238df339023b 181 else { pinled0=0; pinled1=0; pinled2=0; pinled3=0; pinled4=0; pinled5=0; pinled6=0; pinled7=0; pinled8=0; pinled9=0; pinbuzz=0; }
Suchakhree 0:238df339023b 182 }
Suchakhree 0:238df339023b 183 void resetled()
Suchakhree 0:238df339023b 184 {
Suchakhree 0:238df339023b 185 //reset
Suchakhree 0:238df339023b 186 pinled0=0; pinled1=0; pinled2=0; pinled3=0; pinled4=0;
Suchakhree 0:238df339023b 187 pinled5=0; pinled6=0; pinled7=0; pinled8=0; pinled9=0;
Suchakhree 0:238df339023b 188 pinbuzz=0;
Suchakhree 0:238df339023b 189 }
Suchakhree 0:238df339023b 190 //Function 4 SenAcceloroY
Suchakhree 1:426fbd0d126a 191 int GetAcceloroY()
Suchakhree 0:238df339023b 192 {
Suchakhree 0:238df339023b 193 // If intPin goes high, all data registers have new data
Suchakhree 0:238df339023b 194 if(mpu9250.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) { // On interrupt, check if data ready interrupt
Suchakhree 0:238df339023b 195 mpu9250.readAccelData(accelCount); // Read the x/y/z adc values
Suchakhree 0:238df339023b 196 I2Cstate = 1;
Suchakhree 0:238df339023b 197 ay = (float)accelCount[1]*aRes - accelBias[1];
Suchakhree 0:238df339023b 198 }
Suchakhree 1:426fbd0d126a 199
Suchakhree 1:426fbd0d126a 200 mpu9250.MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
Suchakhree 1:426fbd0d126a 201 mpu9250.MadgwickQuaternionUpdate(ax,ay,az,gx,gy,gz,mx,my,mz);
Suchakhree 1:426fbd0d126a 202 //pc.printf(" ay = %.2f\n", 1000*ay);
Suchakhree 1:426fbd0d126a 203
Suchakhree 0:238df339023b 204 return ay*1000;
Suchakhree 1:426fbd0d126a 205 }