Suchakhree Srisukprom / Mbed 2 deprecated Test_Megre

k

Dependencies:   mbed

main.cpp@0:238df339023b, 2015-12-07 (annotated)

Committer:
Suchakhree
Date:
Mon Dec 07 12:20:46 2015 +0000
Revision:
0:238df339023b
Child:
1:426fbd0d126a
nn

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 0:238df339023b 5 int gyrol_Break();
Suchakhree 0:238df339023b 6
Suchakhree 0:238df339023b 7 Serial pc(USBTX, USBRX); // Huyperterminal default config: 9600 bauds, 8-bit data, 1 stop bit, no parity
Suchakhree 0:238df339023b 8 Serial mobile(D8,D2);
Suchakhree 0:238df339023b 9 //mobile.baud(38400);
Suchakhree 0:238df339023b 10
Suchakhree 0:238df339023b 11 MPU9250 mpu9250;
Suchakhree 0:238df339023b 12 Timer t;
Suchakhree 0:238df339023b 13 //DigitalOut myled(LED1);
Suchakhree 0:238df339023b 14
Suchakhree 0:238df339023b 15 I2C i2c_lcd(D14,D15); // SDA, SCL
Suchakhree 0:238df339023b 16 TextLCD_I2C lcd(&i2c_lcd, 0x4E, TextLCD::LCD20x4); // I2C bus, PCF8574 Slaveaddress, LCD Type
Suchakhree 0:238df339023b 17
Suchakhree 0:238df339023b 18 char pack[8]={0};
Suchakhree 0:238df339023b 19
Suchakhree 0:238df339023b 20 int Ay=0;
Suchakhree 0:238df339023b 21 float sum=0;
Suchakhree 0:238df339023b 22 uint32_t sumCount = 0;
Suchakhree 0:238df339023b 23 char buffer[14];
Suchakhree 0:238df339023b 24 uint8_t dato_leido[2];
Suchakhree 0:238df339023b 25 uint8_t whoami;
Suchakhree 0:238df339023b 26 int SendAcceloroY();
Suchakhree 0:238df339023b 27
Suchakhree 0:238df339023b 28 DigitalOut pinled0(PC_3); DigitalOut pinled1(PC_2); DigitalOut pinled2(PC_13);
Suchakhree 0:238df339023b 29 DigitalOut pinled3(PB_7); DigitalOut pinled4(PA_15); DigitalOut pinled5(PA_13);
Suchakhree 0:238df339023b 30 DigitalOut pinled6(PC_12); DigitalOut pinled7(PC_10); DigitalOut pinled8(PB_4);
Suchakhree 0:238df339023b 31 DigitalOut pinled9(PB_10); DigitalOut pinbuzz(PA_8);
Suchakhree 0:238df339023b 32 void resetled();
Suchakhree 0:238df339023b 33 void getdistance2led(char step);
Suchakhree 0:238df339023b 34
Suchakhree 0:238df339023b 35 DigitalIn switch1(D9);
Suchakhree 0:238df339023b 36 DigitalIn switch2(D10);
Suchakhree 0:238df339023b 37 DigitalIn switch3(D11);
Suchakhree 0:238df339023b 38 void switch_turn();
Suchakhree 0:238df339023b 39 int state = 0;
Suchakhree 0:238df339023b 40
Suchakhree 0:238df339023b 41 int main() {
Suchakhree 0:238df339023b 42
Suchakhree 0:238df339023b 43 //___ Set up I2C: use fast (400 kHz) I2C ___
Suchakhree 0:238df339023b 44 i2c.frequency(400000);
Suchakhree 0:238df339023b 45 // Read the WHO_AM_I register, this is a good test of communication
Suchakhree 0:238df339023b 46 whoami = mpu9250.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250);
Suchakhree 0:238df339023b 47 pc.printf("I AM 0x%x\n\r", whoami); pc.printf("I SHOULD BE 0x71\n\r");
Suchakhree 0:238df339023b 48 if (I2Cstate != 0) // error on I2C
Suchakhree 0:238df339023b 49 pc.printf("I2C failure while reading WHO_AM_I register");
Suchakhree 0:238df339023b 50
Suchakhree 0:238df339023b 51 if (whoami == 0x71) // WHO_AM_I should always be 0x71
Suchakhree 0:238df339023b 52 {
Suchakhree 0:238df339023b 53 pc.printf("MPU9250 is online...\n\r");
Suchakhree 0:238df339023b 54 sprintf(buffer, "0x%x", whoami);
Suchakhree 0:238df339023b 55 wait(1);
Suchakhree 0:238df339023b 56 mpu9250.resetMPU9250(); // Reset registers to default in preparation for device calibration
Suchakhree 0:238df339023b 57 mpu9250.MPU9250SelfTest(SelfTest); // Start by performing self test and reporting values (accelerometer and gyroscope self test)
Suchakhree 0:238df339023b 58 mpu9250.calibrateMPU9250(gyroBias, accelBias); // Calibrate gyro and accelerometer, load biases in bias registers
Suchakhree 0:238df339023b 59 wait(2);
Suchakhree 0:238df339023b 60 // Initialize device for active mode read of acclerometer, gyroscope, and temperature
Suchakhree 0:238df339023b 61 mpu9250.initMPU9250();
Suchakhree 0:238df339023b 62 pc.printf("MPU9250 initialized for active data mode....\n\r");
Suchakhree 0:238df339023b 63 // Initialize device for active mode read of magnetometer, 16 bit resolution, 100Hz.
Suchakhree 0:238df339023b 64 mpu9250.initAK8963(magCalibration);
Suchakhree 0:238df339023b 65 wait(1);
Suchakhree 0:238df339023b 66 }
Suchakhree 0:238df339023b 67
Suchakhree 0:238df339023b 68 else // Connection failure
Suchakhree 0:238df339023b 69 {
Suchakhree 0:238df339023b 70 pc.printf("Could not connect to MPU9250: \n\r");
Suchakhree 0:238df339023b 71 pc.printf("%#x \n", whoami);
Suchakhree 0:238df339023b 72 sprintf(buffer, "WHO_AM_I 0x%x", whoami);
Suchakhree 0:238df339023b 73 while(1) ; // Loop forever if communication doesn't happen
Suchakhree 0:238df339023b 74 }
Suchakhree 0:238df339023b 75
Suchakhree 0:238df339023b 76 mpu9250.getAres(); // Get accelerometer sensitivity
Suchakhree 0:238df339023b 77 mpu9250.getGres(); // Get gyro sensitivity
Suchakhree 0:238df339023b 78 mpu9250.getMres(); // Get magnetometer sensitivity
Suchakhree 0:238df339023b 79 magbias[0] = +470.; // User environmental x-axis correction in milliGauss, should be automatically calculated
Suchakhree 0:238df339023b 80 magbias[1] = +120.; // User environmental x-axis correction in milliGauss
Suchakhree 0:238df339023b 81 magbias[2] = +125.; // User environmental x-axis correction in milliGauss
Suchakhree 0:238df339023b 82
Suchakhree 0:238df339023b 83 lcd.locate(0,0);
Suchakhree 0:238df339023b 84 lcd.printf("FRA241 Software-Dev.");
Suchakhree 0:238df339023b 85 lcd.locate(0,1);
Suchakhree 0:238df339023b 86 lcd.printf("Bike_Light");
Suchakhree 0:238df339023b 87 resetled();
Suchakhree 0:238df339023b 88 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
Suchakhree 0:238df339023b 89 while(1)
Suchakhree 0:238df339023b 90 {
Suchakhree 0:238df339023b 91 if(switch1 == 1 || switch2 == 1 || switch3 == 1) mobile.printf("#");
Suchakhree 0:238df339023b 92 if(switch1 == 1) {mobile.printf("#TR$"); lcd.locate(0,3); lcd.printf("TurnRight"); }
Suchakhree 0:238df339023b 93 if(switch2 == 1) {mobile.printf("#TL$"); lcd.locate(0,3); lcd.printf("TurnLeft.");}
Suchakhree 0:238df339023b 94 if(switch3 == 1) {mobile.printf("#BR$"); lcd.locate(0,3); lcd.printf("Break.");}
Suchakhree 0:238df339023b 95 else {mobile.printf("#MD$"); lcd.locate(0,3); lcd.printf("None. ");}
Suchakhree 0:238df339023b 96
Suchakhree 0:238df339023b 97 //switch_turn();
Suchakhree 0:238df339023b 98 //Read data
Suchakhree 0:238df339023b 99 /* if(mobile.readable())
Suchakhree 0:238df339023b 100 {
Suchakhree 0:238df339023b 101 pack[0] = mobile.getc();
Suchakhree 0:238df339023b 102 for(int i=0;i<8;i++)
Suchakhree 0:238df339023b 103 pack[i] = mobile.getc();
Suchakhree 0:238df339023b 104
Suchakhree 0:238df339023b 105 //Debug_Serial
Suchakhree 0:238df339023b 106 //for(int i=0;i<8;i++)
Suchakhree 0:238df339023b 107 // pc.printf("%c",pack[i]);
Suchakhree 0:238df339023b 108 //pc.printf("\n");
Suchakhree 0:238df339023b 109
Suchakhree 0:238df339023b 110 lcd.locate(0,2);
Suchakhree 0:238df339023b 111 for(int i=0;i<8;i++) lcd.printf("%c",pack[i]);
Suchakhree 0:238df339023b 112
Suchakhree 0:238df339023b 113 if(pack[1] == '#')
Suchakhree 0:238df339023b 114 {
Suchakhree 0:238df339023b 115
Suchakhree 0:238df339023b 116 if(pack[2] == 'D' && pack[5] == 'S')
Suchakhree 0:238df339023b 117 {
Suchakhree 0:238df339023b 118 getdistance2led(pack[4]);
Suchakhree 0:238df339023b 119 lcd.locate(0,2);
Suchakhree 0:238df339023b 120 lcd.printf("SPEED : %c%c km/hr",pack[6],pack[7]);
Suchakhree 0:238df339023b 121
Suchakhree 0:238df339023b 122 //switch1.rise(&switch_turn);
Suchakhree 0:238df339023b 123
Suchakhree 0:238df339023b 124 //for(int i; ((switch1 == 1 || switch2 == 1 || switch3 == 1 )&& state == 0);) {mobile.printf("#"); break;}
Suchakhree 0:238df339023b 125 //for(;switch1 == 1 && state == 0;) {mobile.printf("#TR$"); lcd.locate(0,3); lcd.printf("TurnRight"); state=1; break;}
Suchakhree 0:238df339023b 126 //else if(switch2 == 1 && state == 0) {mobile.printf("#TL$"); lcd.locate(0,3); lcd.printf("TurnLeft."); state=1;}
Suchakhree 0:238df339023b 127 //else if(switch3 == 1 && state == 0) {mobile.printf("#BR$"); lcd.locate(0,3); lcd.printf("Break. "); state=1;}
Suchakhree 0:238df339023b 128 //if((switch1 == 0 || switch2 == 0 || switch3 == 0 )&& state == 1){mobile.printf("#"); mobile.printf("#MD$"); lcd.locate(0,3); lcd.printf("Clear."); state=0;}
Suchakhree 0:238df339023b 129
Suchakhree 0:238df339023b 130 }*/
Suchakhree 0:238df339023b 131
Suchakhree 0:238df339023b 132 //}
Suchakhree 0:238df339023b 133
Suchakhree 0:238df339023b 134 }
Suchakhree 0:238df339023b 135
Suchakhree 0:238df339023b 136 }
Suchakhree 0:238df339023b 137 ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
Suchakhree 0:238df339023b 138 }
Suchakhree 0:238df339023b 139
Suchakhree 0:238df339023b 140 void getdistance2led(char step)
Suchakhree 0:238df339023b 141 {
Suchakhree 0:238df339023b 142 resetled();
Suchakhree 0:238df339023b 143 if(step == '0'){
Suchakhree 0:238df339023b 144 pinled0=1; pinled1=0; pinled2=0; pinled3=0; pinled4=0; pinled5=0; pinled6=0; pinled7=0; pinled8=0; pinled9=0;}
Suchakhree 0:238df339023b 145 else if(step == '1'){
Suchakhree 0:238df339023b 146 pinled0=1; pinled1=1; pinled2=0; pinled3=0; pinled4=0; pinled5=0; pinled6=0; pinled7=0; pinled8=0; pinled9=0;}
Suchakhree 0:238df339023b 147 else if(step == '2'){
Suchakhree 0:238df339023b 148 pinled0=1; pinled1=1; pinled2=1; pinled3=0; pinled4=0; pinled5=0; pinled6=0; pinled7=0; pinled8=0; pinled9=0;}
Suchakhree 0:238df339023b 149 else if(step == '3'){
Suchakhree 0:238df339023b 150 pinled0=1; pinled1=1; pinled2=1; pinled3=1; pinled4=0; pinled5=0; pinled6=0; pinled7=0; pinled8=0; pinled9=0;}
Suchakhree 0:238df339023b 151 else if(step == '4'){
Suchakhree 0:238df339023b 152 pinled0=1; pinled1=1; pinled2=1; pinled3=1; pinled4=1; pinled5=0; pinled6=0; pinled7=0; pinled8=0; pinled9=0;}
Suchakhree 0:238df339023b 153 else if(step == '5'){
Suchakhree 0:238df339023b 154 pinled0=1; pinled1=1; pinled2=1; pinled3=1; pinled4=1; pinled5=1; pinled6=0; pinled7=0; pinled8=0; pinled9=0;}
Suchakhree 0:238df339023b 155 else if(step == '6'){
Suchakhree 0:238df339023b 156 pinled0=1; pinled1=1; pinled2=1; pinled3=1; pinled4=1; pinled5=1; pinled6=1; pinled7=0; pinled8=0; pinled9=0;}
Suchakhree 0:238df339023b 157 else if(step == '7'){
Suchakhree 0:238df339023b 158 pinled0=1; pinled1=1; pinled2=1; pinled3=1; pinled4=1; pinled5=1; pinled6=1; pinled7=1; pinled8=0; pinled9=0; pinbuzz=1;}
Suchakhree 0:238df339023b 159 else if(step == '8'){
Suchakhree 0:238df339023b 160 pinled0=1; pinled1=1; pinled2=1; pinled3=1; pinled4=1; pinled5=1; pinled6=1; pinled7=1; pinled8=1; pinled9=0; pinbuzz=1;}
Suchakhree 0:238df339023b 161 else if(step == '9'){
Suchakhree 0:238df339023b 162 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 163 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 164 }
Suchakhree 0:238df339023b 165 void resetled()
Suchakhree 0:238df339023b 166 {
Suchakhree 0:238df339023b 167 //reset
Suchakhree 0:238df339023b 168 pinled0=0; pinled1=0; pinled2=0; pinled3=0; pinled4=0;
Suchakhree 0:238df339023b 169 pinled5=0; pinled6=0; pinled7=0; pinled8=0; pinled9=0;
Suchakhree 0:238df339023b 170 pinbuzz=0;
Suchakhree 0:238df339023b 171 }
Suchakhree 0:238df339023b 172
Suchakhree 0:238df339023b 173
Suchakhree 0:238df339023b 174 //Function 4 SenAcceloroY
Suchakhree 0:238df339023b 175 void switch_turn()
Suchakhree 0:238df339023b 176 {
Suchakhree 0:238df339023b 177 if(switch1 == 1 || switch2 == 1 || switch3 == 1) mobile.printf("#");
Suchakhree 0:238df339023b 178 if(switch1 == 1) {mobile.printf("#TR$"); lcd.locate(0,3); lcd.printf("TurnRight"); }
Suchakhree 0:238df339023b 179 if(switch2 == 1) {mobile.printf("#TL$"); lcd.locate(0,3); lcd.printf("TurnLeft.");}
Suchakhree 0:238df339023b 180 if(switch3 == 1) {mobile.printf("#BR$"); lcd.locate(0,3); lcd.printf("Break.");}
Suchakhree 0:238df339023b 181 else {mobile.printf("#MD$"); lcd.locate(0,3); lcd.printf("None. ");}
Suchakhree 0:238df339023b 182 }
Suchakhree 0:238df339023b 183 int SendAcceloroY()
Suchakhree 0:238df339023b 184 {
Suchakhree 0:238df339023b 185 // If intPin goes high, all data registers have new data
Suchakhree 0:238df339023b 186 if(mpu9250.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) { // On interrupt, check if data ready interrupt
Suchakhree 0:238df339023b 187 mpu9250.readAccelData(accelCount); // Read the x/y/z adc values
Suchakhree 0:238df339023b 188 // Now we'll calculate the accleration value into actual g's
Suchakhree 0:238df339023b 189 if (I2Cstate != 0) //error on I2C
Suchakhree 0:238df339023b 190 pc.printf("I2C error ocurred while reading accelerometer data. I2Cstate = %d \n\r", I2Cstate);
Suchakhree 0:238df339023b 191 else{ // I2C read or write ok
Suchakhree 0:238df339023b 192 I2Cstate = 1;
Suchakhree 0:238df339023b 193 ax = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set
Suchakhree 0:238df339023b 194 ay = (float)accelCount[1]*aRes - accelBias[1];
Suchakhree 0:238df339023b 195 az = (float)accelCount[2]*aRes - accelBias[2];
Suchakhree 0:238df339023b 196 }
Suchakhree 0:238df339023b 197
Suchakhree 0:238df339023b 198 mpu9250.readGyroData(gyroCount); // Read the x/y/z adc values
Suchakhree 0:238df339023b 199 // Calculate the gyro value into actual degrees per second
Suchakhree 0:238df339023b 200 if (I2Cstate != 0) //error on I2C
Suchakhree 0:238df339023b 201 pc.printf("I2C error ocurred while reading gyrometer data. I2Cstate = %d \n\r", I2Cstate);
Suchakhree 0:238df339023b 202 else{ // I2C read or write ok
Suchakhree 0:238df339023b 203 I2Cstate = 1;
Suchakhree 0:238df339023b 204 gx = (float)gyroCount[0]*gRes - gyroBias[0]; // get actual gyro value, this depends on scale being set
Suchakhree 0:238df339023b 205 gy = (float)gyroCount[1]*gRes - gyroBias[1];
Suchakhree 0:238df339023b 206 gz = (float)gyroCount[2]*gRes - gyroBias[2];
Suchakhree 0:238df339023b 207 }
Suchakhree 0:238df339023b 208
Suchakhree 0:238df339023b 209 mpu9250.readMagData(magCount); // Read the x/y/z adc values
Suchakhree 0:238df339023b 210 // Calculate the magnetometer values in milliGauss
Suchakhree 0:238df339023b 211 // Include factory calibration per data sheet and user environmental corrections
Suchakhree 0:238df339023b 212 if (I2Cstate != 0) //error on I2C
Suchakhree 0:238df339023b 213 pc.printf("I2C error ocurred while reading magnetometer data. I2Cstate = %d \n\r", I2Cstate);
Suchakhree 0:238df339023b 214 else{ // I2C read or write ok
Suchakhree 0:238df339023b 215 I2Cstate = 1;
Suchakhree 0:238df339023b 216 mx = (float)magCount[0]*mRes*magCalibration[0] - magbias[0]; // get actual magnetometer value, this depends on scale being set
Suchakhree 0:238df339023b 217 my = (float)magCount[1]*mRes*magCalibration[1] - magbias[1];
Suchakhree 0:238df339023b 218 mz = (float)magCount[2]*mRes*magCalibration[2] - magbias[2];
Suchakhree 0:238df339023b 219 }
Suchakhree 0:238df339023b 220
Suchakhree 0:238df339023b 221 mpu9250.getCompassOrientation(orientation);
Suchakhree 0:238df339023b 222 }
Suchakhree 0:238df339023b 223
Suchakhree 0:238df339023b 224 Now = t.read_us();
Suchakhree 0:238df339023b 225 deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
Suchakhree 0:238df339023b 226 lastUpdate = Now;
Suchakhree 0:238df339023b 227 sum += deltat;
Suchakhree 0:238df339023b 228 sumCount++;
Suchakhree 0:238df339023b 229
Suchakhree 0:238df339023b 230 // Pass gyro rate as rad/s
Suchakhree 0:238df339023b 231 // mpu9250.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
Suchakhree 0:238df339023b 232 mpu9250.MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
Suchakhree 0:238df339023b 233
Suchakhree 0:238df339023b 234 // Serial print and/or display at 1.5 s rate independent of data rates
Suchakhree 0:238df339023b 235 delt_t = t.read_ms() - count;
Suchakhree 0:238df339023b 236 if (delt_t > 500) { // update LCD once per half-second independent of read rate
Suchakhree 0:238df339023b 237 mpu9250.MadgwickQuaternionUpdate(ax,ay,az,gx,gy,gz,mx,my,mz);
Suchakhree 0:238df339023b 238
Suchakhree 0:238df339023b 239 pc.printf(" ay = %.2f\n", 1000*ay);
Suchakhree 0:238df339023b 240
Suchakhree 0:238df339023b 241 tempCount = mpu9250.readTempData(); // Read the adc values
Suchakhree 0:238df339023b 242 if (I2Cstate != 0) //error on I2C
Suchakhree 0:238df339023b 243 pc.printf("I2C error ocurred while reading sensor temp. I2Cstate = %d \n\r", I2Cstate);
Suchakhree 0:238df339023b 244 else{ // I2C read or write ok
Suchakhree 0:238df339023b 245 I2Cstate = 1;
Suchakhree 0:238df339023b 246 temperature = ((float) tempCount) / 333.87f + 21.0f; // Temperature in degrees Centigrade
Suchakhree 0:238df339023b 247 //pc.printf(" temperature = %f C\n\r", temperature);
Suchakhree 0:238df339023b 248 }
Suchakhree 0:238df339023b 249
Suchakhree 0:238df339023b 250
Suchakhree 0:238df339023b 251 myled= !myled;
Suchakhree 0:238df339023b 252 count = t.read_ms();
Suchakhree 0:238df339023b 253
Suchakhree 0:238df339023b 254 if(count > 1<<21) {
Suchakhree 0:238df339023b 255 t.start(); // start the timer over again if ~30 minutes has passed
Suchakhree 0:238df339023b 256 count = 0;
Suchakhree 0:238df339023b 257 deltat= 0;
Suchakhree 0:238df339023b 258 lastUpdate = t.read_us();
Suchakhree 0:238df339023b 259 }
Suchakhree 0:238df339023b 260 sum = 0;
Suchakhree 0:238df339023b 261 sumCount = 0;
Suchakhree 0:238df339023b 262 }
Suchakhree 0:238df339023b 263 return ay*1000;
Suchakhree 0:238df339023b 264 }