assignment Dogpremetter
Dependencies: A2_DogPreMetter1 SDFileSystem mbed
Fork of MPU6050IMU by
main.cpp
- Committer:
- Unknowplayer
- Date:
- 2015-12-08
- Revision:
- 5:5f74d3265c84
- Parent:
- 4:5be0ca4af5b7
File content as of revision 5:5f74d3265c84:
#include "mbed.h" #include "SDFileSystem.h" #include "Rtc_Ds1307.h" #include <string> #include "debug.h" #include "MPU6050.h" #ifndef DEBUG #define DEBUG #endif float sum = 0; uint32_t sumCount = 0; SDFileSystem sd(D11, D12, D13, D10, "sd"); MPU6050 mpu6050; Serial pc(D1,D0); // tx, rx Rtc_Ds1307 rtc(D14, D15); Serial device(D8, D2); Timer t; bool storeData(float, float, float); bool sendDataTime(int); bool clearData(); bool setTimeData(Rtc_Ds1307::Time_rtc); char buffer[128]; int readptr = 0; char j[] = "strongaaa"; int k = 0; int x,numcount = 0; int l[13] = {0}; int m[13] = {0}; int main() { pc.baud(9600); //Set up I2C i2c.frequency(400000); // use fast (400 kHz) I2C t.start(); pc.printf("DogPremetter\n"); //First Rtc_Ds1307::Time_rtc tm; rtc.startClock(); //startClock // setTimeData(tm); //setTimeData mkdir("/sd/dataimu", 0777); rtc.getTime(tm); pc.printf("Hello\n"); pc.printf("The current time is : %02d:%02d:%02d\n", tm.hour, tm.min, tm.sec); pc.printf("The current date is : %02d/%02d/%04d\n", tm.mon, tm.date, tm.year); mkdir("/sd/dataimu", 0777); // ##############################IMU############################################################################################################ // Read the WHO_AM_I register, this is a good test of communication uint8_t whoami = mpu6050.readByte(MPU6050_ADDRESS, WHO_AM_I_MPU6050); // Read WHO_AM_I register for MPU-6050 pc.printf("I AM 0x%x\n\r", whoami); pc.printf("I SHOULD BE 0x68\n\r"); if (whoami == 0x68) { // WHO_AM_I should always be 0x68 pc.printf("MPU6050 is online..."); wait(1); 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) { mpu6050.resetMPU6050(); // Reset registers to default in preparation for device calibration mpu6050.calibrateMPU6050(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers mpu6050.initMPU6050(); pc.printf("MPU6050 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature wait(2); } else { pc.printf("Device did not the pass self-test!\n\r"); } } else { pc.printf("Could not connect to MPU6050: \n\r"); pc.printf("%#x \n", whoami); while(1) ; // Loop forever if communication doesn't happen } while(1) { // If data ready bit set, all data registers have new data if(mpu6050.readByte(MPU6050_ADDRESS, INT_STATUS) & 0x01) { // check if data ready interrupt mpu6050.readAccelData(accelCount); // Read the x/y/z adc values mpu6050.getAres(); // Now we'll calculate the accleration value into actual g's ax = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set ay = (float)accelCount[1]*aRes - accelBias[1]; az = (float)accelCount[2]*aRes - accelBias[2]; mpu6050.readGyroData(gyroCount); // Read the x/y/z adc values mpu6050.getGres(); // Calculate the gyro value into actual degrees per second gx = (float)gyroCount[0]*gRes; // - gyroBias[0]; // get actual gyro value, this depends on scale being set gy = (float)gyroCount[1]*gRes; // - gyroBias[1]; gz = (float)gyroCount[2]*gRes; // - gyroBias[2]; tempCount = mpu6050.readTempData(); // Read the x/y/z adc values temperature = (tempCount) / 340. + 36.53; // Temperature in degrees Centigrade } Now = t.read_us(); deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update lastUpdate = Now; sum += deltat; sumCount++; if(lastUpdate - firstUpdate > 10000000.0f) { beta = 0.04; // decrease filter gain after stabilized zeta = 0.015; // increasey bias drift gain after stabilized } // Pass gyro rate as rad/s mpu6050.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f); // Serial print and/or display at 0.5 s rate independent of data rates delt_t = t.read_ms() - count; if (delt_t > 500) { // update LCD once per half-second independent of read rate 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]); pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2])); 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]); pitch *= 180.0f / PI; yaw *= 180.0f / PI; roll *= 180.0f / PI; rtc.getTime(tm); // ##############################IMU+BLUETOOTH+DATALOGGER####################################################################################### pc.printf("Yaw, Pitch, Roll: %f %f %f\n\r", yaw, pitch, roll); storeData(yaw,pitch,roll); if((yaw > -100 && yaw < 79) && (pitch > -88 && pitch < 45) && (roll > -178 && roll < 178)) { numcount++; sendDataTime(tm.hour); rtc.getTime(tm); pc.printf("num = %d\n",numcount); } storeData(yaw,pitch,roll); if(device.readable()) { if( device.getc() == '#' ) { for(x=1; x<13; x++) { device.printf("%d\n",m[x]); pc.printf("%d\n",m[x]); wait(0.2); } 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; device.printf("%d\n",k); pc.printf("%d\n",k); } else if( device.getc() == '$' ) { clearData(); m[13] = 0; l[13] = 0; numcount = 0; k = 0; } } // pc.printf("average rate = %f\n\r", (float) sumCount/sum); // ############################################################################################################################################# myled= !myled; count = t.read_ms(); sum = 0; sumCount = 0; } } } bool storeData(float x, float y, float z) { FILE *fp = fopen("/sd/dataimu/logger.txt", "a"); if(fp == NULL) { error("Could not open file for write\n"); return false; } else { //sprintf(&input[0],"x=%f, y=%f, z=%f\n ",x,y,z); pc.printf("InreturnData x,y,z\n"); fprintf(fp, "yaw=%f , pitch=%f , roll=%f\n ",x,y,z); 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]); fprintf(fp, "Sum = %d\n",k); pc.printf("yaw=%f , pitch=%f , roll=%f\n ",x,y,z); } pc.printf("OutreturnData\n"); fclose(fp); return true; } bool sendDataTime(int data) { if(data == 00 || data == 01) { l[1]++; m[1] = l[1]+10000 ; wait(0.5); } else if(data == 02 || data == 03) { l[2]++; m[2] = l[2]+20000 ; wait(0.5); } else if(data == 04 || data == 05) { l[3]++; m[3] = l[3]+30000 ; wait(0.5); } else if(data == 06 || data == 07) { l[4]++; m[4] = l[4]+40000 ; wait(0.5); } else if(data > 07 && data < 10) { l[5]++; m[5] = l[5]+50000 ; wait(0.5); } else if(data == 10 || data == 11) { l[5]++; m[6] = l[6]+60000 ; wait(0.5); } else if(data == 12 || data == 13) { l[7]++; m[7] = l[7]+70000 ; wait(0.5); } else if(data == 14 || data == 15) { l[8]++; m[8] = l[8]+80000 ; wait(0.5); } else if(data == 16 || data == 17) { l[9]++; m[9] = l[9]+90000 ; wait(0.5); } else if(data == 18 || data == 19) { l[10]++; m[10] = l[10]+100000 ; wait(0.5); } else if(data == 20 || data == 21) { l[11]++; m[11] = l[11]+110000 ; wait(0.5); } else if(data == 22 || data == 23) { l[12]++; m[12] = l[12]+120000 ; wait(0.5); } return true; } bool clearData() { //device.printf("clear Data \n"); pc.printf("clear Data \n "); remove("/sd/dataimu/logger.txt"); return true; } bool setTimeData(Rtc_Ds1307::Time_rtc tm) { tm.hour=14; tm.min=50; tm.sec=00; tm.mon=12; tm.date=6; tm.year=2015; rtc.setTime(tm,true,false); }