Code.cpp

00001 /*****
00002         Algorithm based on MPU-9250_Snowda program. It has been modified by Josu? Olmeda Castell? for imasD Tecnolog?a.
00003 
00004         This algorithm calibrates and reads data from accelerometer, gyroscope, magnetometer and the
00005         internal temperature sensor. The lecture is made each time has a new mesured value (both gyro and accel data).
00006         A comunication with a computer is made using serial interface. The user can see the data measured with 1 second update rate.
00007 
00008         This algorithm uses the STM32L152 development board and the MPU-9250 9-axis InvenSense movement sensor. The communication
00009         between both devices is made through I2C serial interface.
00010 
00011         AD0 should be connected to GND.
00012 
00013                                                                 04/05/2015
00014 *****/
00015 
00016 #include "mbed.h"
00017 #include "mpu9250.h"
00018 #include "DHT.h"
00019 
00020 Serial pc(D8,D2); // Huyperterminal default config: 9600 bauds, 8-bit data, 1 stop bit, no parity
00021 Serial bell(PA_11,PA_12);
00022 MPU9250 mpu9250;
00023 Timer t;
00024 Timer t2;
00025 DigitalOut myled1(LED1);
00026 DigitalOut myledred(D10);
00027 DigitalOut myledblue(D9);
00028 DigitalOut myledbuzzer(D7);
00029 DHT sensor(D4,22); // Use the SEN11301P sensor
00030 
00031 void StopCon(float,float,float);
00032 void WalkCon(float);
00033 void RunCon(float);
00034 void checkTemp(float);
00035 void timer2(float,float,float);
00036 
00037 float sum = 0;
00038 uint32_t sumCount = 0;
00039 char buffer[14];
00040 uint8_t dato_leido[2];
00041 uint8_t whoami;
00042 char showtimez[10];
00043 int timez=0;
00044 int err;
00045 int cont = 0;
00046 int sw = 1;
00047 int check = 0;
00048 int a=0;
00049 float y1 = 0;
00050 float y2 = 0;
00051 float newyaw = 0;
00052 
00053 uint8_t state_menu = 0;
00054 uint8_t state_show = 0;
00055 uint8_t state_exit = 0;
00056 uint8_t data;
00057 
00058 int main()
00059 {
00060 
00061     //___ Set up I2C: use fast (400 kHz) I2C ___
00062     i2c.frequency(400000);
00063 
00064     pc.printf("CPU SystemCoreClock is %d Hz\r\n", SystemCoreClock);
00065 
00066     t.start(); // Timer ON
00067 
00068     // Read the WHO_AM_I register, this is a good test of communication
00069     whoami = mpu9250.readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250);
00070 
00071     pc.printf("I AM 0x%x\n\r", whoami);
00072     pc.printf("I SHOULD BE 0x71\n\r");
00073     if (I2Cstate != 0) // error on I2C
00074         pc.printf("I2C failure while reading WHO_AM_I register");
00075 
00076     if (whoami == 0x71) { // WHO_AM_I should always be 0x71
00077         pc.printf("MPU9250 WHO_AM_I is 0x%x\n\r", whoami);
00078         pc.printf("MPU9250 is online...\n\r");
00079         sprintf(buffer, "0x%x", whoami);
00080         wait(1);
00081 
00082         mpu9250.resetMPU9250(); // Reset registers to default in preparation for device calibration
00083 
00084         mpu9250.MPU9250SelfTest(SelfTest); // Start by performing self test and reporting values (accelerometer and gyroscope self test)
00085         pc.printf("x-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[0]);
00086         pc.printf("y-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[1]);
00087         pc.printf("z-axis self test: acceleration trim within : %f % of factory value\n\r", SelfTest[2]);
00088         pc.printf("x-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[3]);
00089         pc.printf("y-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[4]);
00090         pc.printf("z-axis self test: gyration trim within : %f % of factory value\n\r", SelfTest[5]);
00091 
00092         mpu9250.calibrateMPU9250(gyroBias, accelBias); // Calibrate gyro and accelerometer, load biases in bias registers
00093         pc.printf("x gyro bias = %f\n\r", gyroBias[0]);
00094         pc.printf("y gyro bias = %f\n\r", gyroBias[1]);
00095         pc.printf("z gyro bias = %f\n\r", gyroBias[2]);
00096         pc.printf("x accel bias = %f\n\r", accelBias[0]);
00097         pc.printf("y accel bias = %f\n\r", accelBias[1]);
00098         pc.printf("z accel bias = %f\n\r", accelBias[2]);
00099         wait(2);
00100 
00101         // Initialize device for active mode read of acclerometer, gyroscope, and temperature
00102         mpu9250.initMPU9250();
00103         //pc.printf("MPU9250 initialized for active data mode....\n\r");
00104 
00105         // Initialize device for active mode read of magnetometer, 16 bit resolution, 100Hz.
00106         mpu9250.initAK8963(magCalibration);
00107         pc.printf("AK8963 initialized for active data mode....\n\r");
00108         pc.printf("Accelerometer full-scale range = %f  g\n\r", 2.0f*(float)(1<<Ascale));
00109         pc.printf("Gyroscope full-scale range = %f  deg/s\n\r", 250.0f*(float)(1<<Gscale));
00110         if(Mscale == 0) pc.printf("Magnetometer resolution = 14  bits\n\r");
00111         if(Mscale == 1) pc.printf("Magnetometer resolution = 16  bits\n\r");
00112         if(Mmode == 2) pc.printf("Magnetometer ODR = 8 Hz\n\r");
00113         if(Mmode == 6) pc.printf("Magnetometer ODR = 100 Hz\n\r");
00114         wait(1);
00115     }
00116 
00117     else { // Connection failure
00118         pc.printf("Could not connect to MPU9250: \n\r");
00119         pc.printf("%#x \n",  whoami);
00120         sprintf(buffer, "WHO_AM_I 0x%x", whoami);
00121         while(1) ; // Loop forever if communication doesn't happen
00122     }
00123 
00124     mpu9250.getAres(); // Get accelerometer sensitivity
00125     mpu9250.getGres(); // Get gyro sensitivity
00126     mpu9250.getMres(); // Get magnetometer sensitivity
00127     pc.printf("Accelerometer sensitivity is %f LSB/g \n\r", 1.0f/aRes);
00128     pc.printf("Gyroscope sensitivity is %f LSB/deg/s \n\r", 1.0f/gRes);
00129     pc.printf("Magnetometer sensitivity is %f LSB/G \n\r", 1.0f/mRes);
00130     magbias[0] = +470.;  // User environmental x-axis correction in milliGauss, should be automatically calculated
00131     magbias[1] = +120.;  // User environmental x-axis correction in milliGauss
00132     magbias[2] = +125.;  // User environmental x-axis correction in milliGauss
00133 
00134     while(1) {
00135 
00136         // If intPin goes high, all data registers have new data
00137         if(mpu9250.readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) {  // On interrupt, check if data ready interrupt
00138 
00139             mpu9250.readAccelData(accelCount);  // Read the x/y/z adc values
00140             // Now we'll calculate the accleration value into actual g's
00141             if (I2Cstate != 0) //error on I2C
00142                 pc.printf("I2C error ocurred while reading accelerometer data. I2Cstate = %d \n\r", I2Cstate);
00143             else { // I2C read or write ok
00144                 I2Cstate = 1;
00145                 ax = (float)accelCount[0]*aRes - accelBias[0];  // get actual g value, this depends on scale being set
00146                 ay = (float)accelCount[1]*aRes - accelBias[1];
00147                 az = (float)accelCount[2]*aRes - accelBias[2];
00148             }
00149 
00150             mpu9250.readGyroData(gyroCount);  // Read the x/y/z adc values
00151             // Calculate the gyro value into actual degrees per second
00152             if (I2Cstate != 0) //error on I2C
00153                 pc.printf("I2C error ocurred while reading gyrometer data. I2Cstate = %d \n\r", I2Cstate);
00154             else { // I2C read or write ok
00155                 I2Cstate = 1;
00156                 gx = (float)gyroCount[0]*gRes - gyroBias[0];  // get actual gyro value, this depends on scale being set
00157                 gy = (float)gyroCount[1]*gRes - gyroBias[1];
00158                 gz = (float)gyroCount[2]*gRes - gyroBias[2];
00159             }
00160 
00161             mpu9250.readMagData(magCount);  // Read the x/y/z adc values
00162             // Calculate the magnetometer values in milliGauss
00163             // Include factory calibration per data sheet and user environmental corrections
00164             if (I2Cstate != 0) //error on I2C
00165                 pc.printf("I2C error ocurred while reading magnetometer data. I2Cstate = %d \n\r", I2Cstate);
00166             else { // I2C read or write ok
00167                 I2Cstate = 1;
00168                 mx = (float)magCount[0]*mRes*magCalibration[0] - magbias[0];  // get actual magnetometer value, this depends on scale being set
00169                 my = (float)magCount[1]*mRes*magCalibration[1] - magbias[1];
00170                 mz = (float)magCount[2]*mRes*magCalibration[2] - magbias[2];
00171             }
00172 
00173             mpu9250.getCompassOrientation(orientation);
00174         }
00175 
00176         Now = t.read_us();
00177         deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
00178         lastUpdate = Now;
00179         sum += deltat;
00180         sumCount++;
00181 
00182         // Pass gyro rate as rad/s
00183         // mpu9250.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f,  my,  mx, mz);
00184         mpu9250.MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
00185 
00186 
00187         // Serial print and/or display at 1.5 s rate independent of data rates
00188         delt_t = t.read_ms() - count;
00189         if (delt_t > 100) { // update LCD once per half-second independent of read rate
00190 
00191 
00192 
00193             tempCount = mpu9250.readTempData();  // Read the adc values
00194             if (I2Cstate != 0) //error on I2C
00195                 pc.printf("I2C error ocurred while reading sensor temp. I2Cstate = %d \n\r", I2Cstate);
00196             else { // I2C read or write ok
00197                 I2Cstate = 1;
00198                 temperature = ((float) tempCount) / 333.87f + 21.0f; // Temperature in degrees Centigrade
00199 
00200 
00201 
00202 
00203                 // Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation.
00204                 // In this coordinate system, the positive z-axis is down toward Earth.
00205                 // Yaw is the angle between Sensor x-axis and Earth magnetic North (or true North if corrected for local declination, looking down on the sensor positive yaw is counterclockwise.
00206                 // Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative.
00207                 // Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll.
00208                 // These arise from the definition of the homogeneous rotation matrix constructed from quaternions.
00209                 // Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be
00210                 // applied in the correct order which for this configuration is yaw, pitch, and then roll.
00211                 // For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links.
00212 
00213                 yaw   = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);
00214                 pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
00215                 roll  = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
00216                 pitch *= 180.0f / PI;
00217                 yaw   *= 180.0f / PI;
00218                 yaw   -= 13.8f; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04
00219                 roll  *= 180.0f / PI;
00220 
00221                 timer2(yaw,pitch,roll);
00222                 sensor.readData();
00223                 checkTemp(sensor.ReadTemperature(CELCIUS));
00224 
00225                 if(state_show == 0) {
00226                     pc.printf("\nWelcome to Project Digital B16\nplease select 1 or 2\n");
00227                     pc.printf("1. Mode a\n");
00228                     pc.printf("2. Mode b\n");
00229                     state_show = 1;
00230                 }
00231                 if(pc.readable()) {
00232                     data = pc.getc();
00233                     pc.printf("\n");
00234                     state_show = 0;
00235                     state_exit = 0;
00236                 }
00237                 switch(data) {
00238                     case '1':
00239                         do {
00240                             if(state_menu == 0) {
00241                                 pc.printf("Temperature is %4.2f C \r\n",sensor.ReadTemperature(CELCIUS));
00242                                 pc.printf("Temperature is %4.2f F \r\n",sensor.ReadTemperature(FARENHEIT));
00243                                 pc.printf("Temperature is %4.2f K \r\n",sensor.ReadTemperature(KELVIN));
00244                                 pc.printf("Humidity is %4.2f \r\n",sensor.ReadHumidity());
00245                                 pc.printf("x.Exit\n");
00246                                 state_menu = 1;
00247                             }
00248                             if(pc.readable()) {
00249                                 data = pc.getc();
00250                                 pc.printf("\n");
00251                                 state_menu=0;
00252 
00253                                 switch(data) {
00254                                     case'x':
00255                                         state_exit = 1;
00256                                         break;
00257 
00258                                     default:
00259                                         pc.printf("plz select x for exit\n");
00260                                         pc.printf("\n\n");
00261                                         break;
00262                                 }
00263                             }
00264                         } while(state_exit ==0);
00265                         pc.printf("\n\n");
00266                         break;
00267 
00268                     case'2':
00269                         do {
00270                             if(state_menu == 0) {
00271                                 pc.printf("Check Status\n");
00272                                 StopCon(yaw,pitch,roll);
00273                                 WalkCon(az);
00274                                 RunCon(roll);
00275                                 pc.printf("x.Exit\n");
00276                                 state_menu = 1;
00277                             }
00278                             if(pc.readable()) {
00279                                 data = pc.getc();
00280                                 pc.printf("\n");
00281                                 state_menu=0;
00282 
00283                                 switch(data) {
00284                                     case'x':
00285                                         state_exit = 1;
00286                                         break;
00287 
00288                                     default:
00289                                         pc.printf("plz select x for exit\n");
00290                                         pc.printf("\n\n");
00291                                         break;
00292                                 }
00293                             }
00294                         } while(state_exit ==0);
00295                         pc.printf("\n\n");
00296                         break;
00297                 }
00298 
00299                 //pc.printf("Ax , Ay , Az :         %f              %f              %f\n\r", 10*ax , 10*ay , 10*az);
00300                 pc.printf("  %f  %f  %f\n\r", yaw, pitch, roll);
00301 
00302             }
00303         }
00304     }
00305 }
00306 
00307 void StopCon(float Y,float P,float R)
00308 {
00309     while((Y == Y)&&(P >= -3.0f) && (P <= 3.0f) && (R >= -3.0f) && (R <= 3.0f)) {
00310         cont++;
00311         wait(1);
00312         if(cont == 10) {
00313             pc.printf("Stopping\n");
00314             cont = 0;
00315             /*myledblue = 1;
00316             wait(3);
00317             myledblue = 0;*/
00318             break;
00319         }
00320     }
00321 
00322 }
00323 void WalkCon(float Z)
00324 {
00325     Z = Z*10;
00326     while(Z >= 11.0f) {
00327         cont++;
00328         wait(1);
00329         if(cont == 10) {
00330             pc.printf("Walking\n");
00331             /*myledred = 1;
00332             wait(3);
00333             myledred = 0;*/
00334             cont = 0;
00335             break;
00336         }
00337     }
00338 }
00339 void RunCon(float R)
00340 {
00341     while(R >= 10) {
00342         cont++;
00343         wait(1);
00344         if(cont == 10) {
00345             pc.printf("Running\n");
00346             cont = 0;
00347             /*myledblue = 1;
00348             myledred = 1;
00349             wait(0.25);
00350             myledblue = 0;
00351             myledred = 0;
00352             myledblue = 1;
00353             myledred = 1;
00354             wait(0.25);
00355             myledblue = 0;
00356             myledred = 0;
00357             wait(0.25);
00358             myledblue = 1;
00359             myledred = 1;
00360             wait(0.25);
00361             myledblue = 0;
00362             myledred = 0;
00363             myledblue = 1;
00364             myledred = 1;
00365             wait(0.25);
00366             myledblue = 0;
00367             myledred = 0;
00368             myledblue = 1;
00369             myledred = 1;
00370             wait(0.25);
00371             myledblue = 0;
00372             myledred = 0;
00373             wait(0.25);
00374             myledblue = 1;
00375             myledred = 1;
00376             wait(0.25);
00377             myledblue = 0;
00378             myledred = 0;*/
00379             break;
00380         }
00381     }
00382 }
00383 void checkTemp(float T)
00384 {
00385     if(T >= 38 && T <= 39) {
00386         myledblue = 1;
00387     } else myledblue = 0;
00388     if(T >= 40) {
00389         myledred = 1;
00390     } else myledred = 0;
00391 }
00392 void timer2(float y,float p,float r)
00393 {
00394     y2 = y;
00395     newyaw = y1 - y2;
00396     y1 = y2;
00397 
00398     if((newyaw <= -10 || newyaw >= 10) && (p <= -13 || p >= 13)&& (r <=  -15 || r >= 5 ) && sw == 1) {
00399         pc.printf("  *****444444******* \n ");
00400         check=1;
00401         t2.start();
00402         sw=0;
00403     }
00404     if(t2.read() < 30 &&(newyaw <= 5 && newyaw >= -5)&&  (p <= 5 && p >= -5) &&(r <= 5 && r >= -5) && sw == 0) {
00405         pc.printf("  *****6666666*******  ");
00406         a++;
00407         pc.printf("\t\t****Tend : %d****\t\t\n",a);
00408         sw=1;
00409 
00410     }
00411     if(t2.read()>30 && check==1) {
00412         if(a==4) {
00413             // pc.printf(" You stumbles LV1. ");
00414             myledred =1;
00415             wait(1);
00416             myledred =0;
00417             myledblue =1;
00418             wait(1);
00419             myledblue =0;
00420             myledbuzzer =1;
00421             wait(2);
00422             myledbuzzer=0;
00423             wait(0.5);
00424             myledbuzzer=1;
00425             wait(2);
00426             myledbuzzer=0;
00427             myledred =1;
00428             wait(1);
00429             myledred =0;
00430             myledblue =1;
00431             wait(1);
00432             myledblue =0;
00433         }
00434         if(a>=5) {
00435             myledbuzzer =1;
00436             wait(0.5);
00437             myledbuzzer =0;
00438             wait(0.5);
00439             myledbuzzer =1;
00440             wait(0.5);
00441             myledbuzzer =0;
00442             wait(0.5);
00443             myledbuzzer =1;
00444             wait(0.5);
00445             myledbuzzer =0;
00446             wait(0.5);
00447             myledbuzzer =1;
00448             wait(0.5);
00449             myledbuzzer =0;
00450             wait(0.5);
00451             myledbuzzer =1;
00452             wait(20);
00453             myledbuzzer =0;
00454         }
00455 
00456         check=0;
00457         a=0;
00458         t2.reset();
00459 
00460 
00461     }
00462 
00463     // pc.printf("\tTend : %d\t",a);
00464 
00465 
00466 }