main.cpp

00001 #include "mbed.h"
00002 #include "SDFileSystem.h"
00003 #include "Rtc_Ds1307.h"
00004 #include <string>
00005 #include "debug.h"
00006 #include "MPU6050.h"
00007 #ifndef DEBUG
00008 #define DEBUG
00009 #endif
00010 
00011 float sum = 0;
00012 uint32_t sumCount = 0;
00013 
00014 SDFileSystem sd(D11, D12, D13, D10, "sd");
00015 MPU6050 mpu6050;
00016 Serial pc(D1,D0); // tx, rx
00017 Rtc_Ds1307 rtc(D14, D15);
00018 Serial device(D8, D2);
00019 
00020 Timer t;
00021 
00022 bool storeData(float, float, float);
00023 bool sendDataTime(int);
00024 bool clearData();
00025 bool setTimeData(Rtc_Ds1307::Time_rtc);
00026 
00027 char buffer[128];
00028 int readptr = 0;
00029 char j[] = "strongaaa";
00030 int k = 0;
00031 int x,numcount = 0;
00032 int l[13] = {0};
00033 int m[13] = {0};
00034 int main()
00035 {
00036     pc.baud(9600);
00037     //Set up I2C
00038     i2c.frequency(400000);  // use fast (400 kHz) I2C
00039     t.start();
00040     pc.printf("DogPremetter\n");        
00041                                                  //First
00042     Rtc_Ds1307::Time_rtc tm;
00043     rtc.startClock();                                                                 //startClock
00044     // setTimeData(tm);                                                               //setTimeData
00045     mkdir("/sd/dataimu", 0777);
00046     rtc.getTime(tm);
00047     pc.printf("Hello\n");
00048     pc.printf("The current time is : %02d:%02d:%02d\n", tm.hour, tm.min, tm.sec);
00049     pc.printf("The current date is : %02d/%02d/%04d\n", tm.mon, tm.date, tm.year);
00050     mkdir("/sd/dataimu", 0777);
00051 
00052     //  ##############################IMU############################################################################################################
00053     // Read the WHO_AM_I register, this is a good test of communication
00054     uint8_t whoami = mpu6050.readByte(MPU6050_ADDRESS, WHO_AM_I_MPU6050);  // Read WHO_AM_I register for MPU-6050
00055     pc.printf("I AM 0x%x\n\r", whoami);
00056     pc.printf("I SHOULD BE 0x68\n\r");
00057     if (whoami == 0x68) { // WHO_AM_I should always be 0x68
00058         pc.printf("MPU6050 is online...");
00059         wait(1);
00060         if(SelfTest[0] < 1.0f && SelfTest[1] < 1.0f && SelfTest[2] < 1.0f && SelfTest[3] < 1.0f && SelfTest[4] < 1.0f && SelfTest[5] < 1.0f) {
00061             mpu6050.resetMPU6050(); // Reset registers to default in preparation for device calibration
00062             mpu6050.calibrateMPU6050(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
00063             mpu6050.initMPU6050();
00064             pc.printf("MPU6050 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
00065 
00066             wait(2);
00067         } else {
00068             pc.printf("Device did not the pass self-test!\n\r");
00069         }
00070     } else {
00071         pc.printf("Could not connect to MPU6050: \n\r");
00072         pc.printf("%#x \n",  whoami);
00073 
00074         while(1) ; // Loop forever if communication doesn't happen
00075     }
00076     while(1) {
00077 
00078         // If data ready bit set, all data registers have new data
00079         if(mpu6050.readByte(MPU6050_ADDRESS, INT_STATUS) & 0x01) {  // check if data ready interrupt
00080             mpu6050.readAccelData(accelCount);  // Read the x/y/z adc values
00081             mpu6050.getAres();
00082 
00083             // Now we'll calculate the accleration value into actual g's
00084             ax = (float)accelCount[0]*aRes - accelBias[0];  // get actual g value, this depends on scale being set
00085             ay = (float)accelCount[1]*aRes - accelBias[1];
00086             az = (float)accelCount[2]*aRes - accelBias[2];
00087 
00088             mpu6050.readGyroData(gyroCount);  // Read the x/y/z adc values
00089             mpu6050.getGres();
00090 
00091             // Calculate the gyro value into actual degrees per second
00092             gx = (float)gyroCount[0]*gRes; // - gyroBias[0];  // get actual gyro value, this depends on scale being set
00093             gy = (float)gyroCount[1]*gRes; // - gyroBias[1];
00094             gz = (float)gyroCount[2]*gRes; // - gyroBias[2];
00095 
00096             tempCount = mpu6050.readTempData();  // Read the x/y/z adc values
00097             temperature = (tempCount) / 340. + 36.53; // Temperature in degrees Centigrade
00098         }
00099 
00100         Now = t.read_us();
00101         deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
00102         lastUpdate = Now;
00103 
00104         sum += deltat;
00105         sumCount++;
00106 
00107         if(lastUpdate - firstUpdate > 10000000.0f) {
00108             beta = 0.04;  // decrease filter gain after stabilized
00109             zeta = 0.015; // increasey bias drift gain after stabilized
00110         }
00111 
00112         // Pass gyro rate as rad/s
00113         mpu6050.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f);
00114 
00115         // Serial print and/or display at 0.5 s rate independent of data rates
00116         delt_t = t.read_ms() - count;
00117         if (delt_t > 500) { // update LCD once per half-second independent of read rate
00118 
00119             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]);
00120             pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
00121             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]);
00122             pitch *= 180.0f / PI;
00123             yaw   *= 180.0f / PI;
00124             roll  *= 180.0f / PI;
00125 
00126             rtc.getTime(tm);
00127             //  ##############################IMU+BLUETOOTH+DATALOGGER#######################################################################################
00128             pc.printf("Yaw, Pitch, Roll: %f %f %f\n\r", yaw, pitch, roll);
00129             storeData(yaw,pitch,roll);
00130 
00131 
00132             if((yaw > -100 && yaw < 79) && (pitch > -88 && pitch < 45) && (roll > -178 && roll < 178)) {
00133                 numcount++;
00134                 sendDataTime(tm.hour);
00135                 rtc.getTime(tm);
00136                 pc.printf("num = %d\n",numcount);
00137 
00138 
00139             }
00140             storeData(yaw,pitch,roll);
00141             if(device.readable()) {
00142                 if( device.getc() == '#' ) {
00143 
00144                     for(x=1; x<13; x++) {
00145                         device.printf("%d\n",m[x]);
00146                         pc.printf("%d\n",m[x]);
00147                         wait(0.2);
00148                     }
00149                     k = l[1]+l[2]+l[3]+l[4]+l[5]+l[6]+l[7]+l[8]+l[9]+l[10]+l[11]+l[12]+130000;
00150                     device.printf("%d\n",k);
00151                     pc.printf("%d\n",k);
00152                 } else if( device.getc() == '$' ) {
00153                     clearData();
00154                     m[13] = 0;
00155                     l[13] = 0;
00156                     numcount = 0;
00157                     k = 0;
00158                 }
00159             }
00160 
00161             //   pc.printf("average rate = %f\n\r", (float) sumCount/sum);
00162             //  #############################################################################################################################################
00163             myled= !myled;
00164             count = t.read_ms();
00165             sum = 0;
00166             sumCount = 0;
00167         }
00168     }
00169 
00170 }
00171 
00172 
00173 
00174 bool storeData(float x, float y, float z)
00175 {
00176     FILE *fp = fopen("/sd/dataimu/logger.txt", "a");
00177     if(fp == NULL) {
00178         error("Could not open file for write\n");
00179         return false;
00180     } else {
00181         //sprintf(&input[0],"x=%f, y=%f, z=%f\n ",x,y,z);
00182         pc.printf("InreturnData x,y,z\n");
00183         fprintf(fp, "yaw=%f , pitch=%f , roll=%f\n ",x,y,z);
00184         fprintf(fp, "l[1]= %d l[2]=%d l[3]= %d l[4]=%d l[5]= %d l[6]=%d l[7]= %d l[8]=%d  l[9]= %d l[10]=%d l[11]= %d l[12]=%d ",l[1],l[2],l[3],l[4],l[5],l[6],l[7],l[8],l[9],l[10],l[11],l[12]);
00185         fprintf(fp, "Sum = %d\n",k);
00186         pc.printf("yaw=%f , pitch=%f , roll=%f\n ",x,y,z);
00187 
00188     }
00189     pc.printf("OutreturnData\n");
00190     fclose(fp);
00191     return true;
00192 }
00193 
00194 
00195 
00196 
00197 bool sendDataTime(int data)
00198 {
00199     if(data == 00 || data == 01) {
00200         l[1]++;
00201         m[1] = l[1]+10000 ;
00202         wait(0.5);
00203 
00204 
00205     } else if(data == 02 || data == 03) {
00206         l[2]++;
00207         m[2] = l[2]+20000 ;
00208         wait(0.5);
00209 
00210     } else if(data == 04 || data == 05) {
00211         l[3]++;
00212         m[3] = l[3]+30000 ;
00213         wait(0.5);
00214 
00215     } else if(data == 06 || data == 07) {
00216         l[4]++;
00217         m[4] = l[4]+40000 ;
00218         wait(0.5);
00219 
00220     } else if(data > 07 && data < 10) {
00221         l[5]++;
00222         m[5] = l[5]+50000 ;
00223         wait(0.5);
00224 
00225     } else if(data == 10 || data == 11) {
00226         l[5]++;
00227         m[6] = l[6]+60000 ;
00228         wait(0.5);
00229 
00230     } else if(data == 12 || data == 13) {
00231         l[7]++;
00232         m[7] = l[7]+70000 ;
00233         wait(0.5);
00234 
00235     } else if(data == 14 || data == 15) {
00236         l[8]++;
00237         m[8] = l[8]+80000 ;
00238         wait(0.5);
00239 
00240     } else if(data == 16 || data == 17) {
00241         l[9]++;
00242         m[9] = l[9]+90000 ;
00243         wait(0.5);
00244 
00245     } else if(data == 18 || data == 19) {
00246         l[10]++;
00247         m[10] = l[10]+100000 ;
00248         wait(0.5);
00249 
00250     } else if(data == 20 || data == 21) {
00251         l[11]++;
00252         m[11] = l[11]+110000 ;
00253         wait(0.5);
00254 
00255     } else if(data == 22 || data == 23) {
00256         l[12]++;
00257         m[12] = l[12]+120000 ;
00258         wait(0.5);
00259 
00260     }
00261     return true;
00262 }
00263 
00264 
00265 bool clearData()
00266 {
00267     //device.printf("clear Data \n");
00268     pc.printf("clear Data \n ");
00269     remove("/sd/dataimu/logger.txt");
00270     return true;
00271 }
00272 
00273 bool setTimeData(Rtc_Ds1307::Time_rtc tm)
00274 {
00275     tm.hour=14;
00276     tm.min=50;
00277     tm.sec=00;
00278     tm.mon=12;
00279     tm.date=6;
00280     tm.year=2015;
00281     rtc.setTime(tm,true,false);
00282 }