FRA221_2015 / Mbed 2 deprecated A2_DogPremetter2

assignment Dogpremetter

Dependencies:   A2_DogPreMetter1 SDFileSystem mbed

Fork of MPU6050IMU by 272835 Digital Team2015

main.cpp@5:5f74d3265c84, 2015-12-08 (annotated)

Committer:
Unknowplayer
Date:
Tue Dec 08 19:32:50 2015 +0000
Revision:
5:5f74d3265c84
Parent:
4:5be0ca4af5b7 
A2

Who changed what in which revision?

UserRevisionLine numberNew contents of line
onehorse 1:cea9d83b8636 1 #include "mbed.h"
Unknowplayer 4:5be0ca4af5b7 2 #include "SDFileSystem.h"
Unknowplayer 4:5be0ca4af5b7 3 #include "Rtc_Ds1307.h"
Unknowplayer 4:5be0ca4af5b7 4 #include <string>
Unknowplayer 4:5be0ca4af5b7 5 #include "debug.h"
onehorse 1:cea9d83b8636 6 #include "MPU6050.h"
Unknowplayer 4:5be0ca4af5b7 7 #ifndef DEBUG
Unknowplayer 4:5be0ca4af5b7 8 #define DEBUG
Unknowplayer 4:5be0ca4af5b7 9 #endif
onehorse 0:65aa78c10981 10
onehorse 1:cea9d83b8636 11 float sum = 0;
onehorse 1:cea9d83b8636 12 uint32_t sumCount = 0;
onehorse 1:cea9d83b8636 13
Unknowplayer 4:5be0ca4af5b7 14 SDFileSystem sd(D11, D12, D13, D10, "sd");
thanawatinges 3:bf3448217248 15 MPU6050 mpu6050;
Unknowplayer 4:5be0ca4af5b7 16 Serial pc(D1,D0); // tx, rx
Unknowplayer 4:5be0ca4af5b7 17 Rtc_Ds1307 rtc(D14, D15);
Unknowplayer 4:5be0ca4af5b7 18 Serial device(D8, D2);
onehorse 1:cea9d83b8636 19
thanawatinges 3:bf3448217248 20 Timer t;
onehorse 1:cea9d83b8636 21
Unknowplayer 4:5be0ca4af5b7 22 bool storeData(float, float, float);
Unknowplayer 4:5be0ca4af5b7 23 bool sendDataTime(int);
Unknowplayer 4:5be0ca4af5b7 24 bool clearData();
Unknowplayer 4:5be0ca4af5b7 25 bool setTimeData(Rtc_Ds1307::Time_rtc);
thanawatinges 3:bf3448217248 26
Unknowplayer 4:5be0ca4af5b7 27 char buffer[128];
Unknowplayer 4:5be0ca4af5b7 28 int readptr = 0;
Unknowplayer 4:5be0ca4af5b7 29 char j[] = "strongaaa";
Unknowplayer 4:5be0ca4af5b7 30 int k = 0;
Unknowplayer 4:5be0ca4af5b7 31 int x,numcount = 0;
Unknowplayer 4:5be0ca4af5b7 32 int l[13] = {0};
Unknowplayer 4:5be0ca4af5b7 33 int m[13] = {0};
onehorse 1:cea9d83b8636 34 int main()
onehorse 1:cea9d83b8636 35 {
thanawatinges 3:bf3448217248 36 pc.baud(9600);
thanawatinges 3:bf3448217248 37 //Set up I2C
thanawatinges 3:bf3448217248 38 i2c.frequency(400000); // use fast (400 kHz) I2C
Unknowplayer 4:5be0ca4af5b7 39 t.start();
Unknowplayer 4:5be0ca4af5b7 40 pc.printf("DogPremetter\n");
Unknowplayer 4:5be0ca4af5b7 41 //First
Unknowplayer 4:5be0ca4af5b7 42 Rtc_Ds1307::Time_rtc tm;
Unknowplayer 4:5be0ca4af5b7 43 rtc.startClock(); //startClock
Unknowplayer 4:5be0ca4af5b7 44 // setTimeData(tm); //setTimeData
Unknowplayer 4:5be0ca4af5b7 45 mkdir("/sd/dataimu", 0777);
Unknowplayer 4:5be0ca4af5b7 46 rtc.getTime(tm);
Unknowplayer 4:5be0ca4af5b7 47 pc.printf("Hello\n");
Unknowplayer 4:5be0ca4af5b7 48 pc.printf("The current time is : %02d:%02d:%02d\n", tm.hour, tm.min, tm.sec);
Unknowplayer 4:5be0ca4af5b7 49 pc.printf("The current date is : %02d/%02d/%04d\n", tm.mon, tm.date, tm.year);
Unknowplayer 4:5be0ca4af5b7 50 mkdir("/sd/dataimu", 0777);
onehorse 0:65aa78c10981 51
Unknowplayer 4:5be0ca4af5b7 52 // ##############################IMU############################################################################################################
thanawatinges 3:bf3448217248 53 // Read the WHO_AM_I register, this is a good test of communication
thanawatinges 3:bf3448217248 54 uint8_t whoami = mpu6050.readByte(MPU6050_ADDRESS, WHO_AM_I_MPU6050); // Read WHO_AM_I register for MPU-6050
thanawatinges 3:bf3448217248 55 pc.printf("I AM 0x%x\n\r", whoami);
thanawatinges 3:bf3448217248 56 pc.printf("I SHOULD BE 0x68\n\r");
thanawatinges 3:bf3448217248 57 if (whoami == 0x68) { // WHO_AM_I should always be 0x68
thanawatinges 3:bf3448217248 58 pc.printf("MPU6050 is online...");
thanawatinges 3:bf3448217248 59 wait(1);
thanawatinges 3:bf3448217248 60 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) {
thanawatinges 3:bf3448217248 61 mpu6050.resetMPU6050(); // Reset registers to default in preparation for device calibration
thanawatinges 3:bf3448217248 62 mpu6050.calibrateMPU6050(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
thanawatinges 3:bf3448217248 63 mpu6050.initMPU6050();
thanawatinges 3:bf3448217248 64 pc.printf("MPU6050 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
onehorse 0:65aa78c10981 65
thanawatinges 3:bf3448217248 66 wait(2);
thanawatinges 3:bf3448217248 67 } else {
thanawatinges 3:bf3448217248 68 pc.printf("Device did not the pass self-test!\n\r");
thanawatinges 3:bf3448217248 69 }
thanawatinges 3:bf3448217248 70 } else {
thanawatinges 3:bf3448217248 71 pc.printf("Could not connect to MPU6050: \n\r");
thanawatinges 3:bf3448217248 72 pc.printf("%#x \n", whoami);
thanawatinges 3:bf3448217248 73
thanawatinges 3:bf3448217248 74 while(1) ; // Loop forever if communication doesn't happen
onehorse 0:65aa78c10981 75 }
thanawatinges 3:bf3448217248 76 while(1) {
thanawatinges 3:bf3448217248 77
thanawatinges 3:bf3448217248 78 // If data ready bit set, all data registers have new data
thanawatinges 3:bf3448217248 79 if(mpu6050.readByte(MPU6050_ADDRESS, INT_STATUS) & 0x01) { // check if data ready interrupt
thanawatinges 3:bf3448217248 80 mpu6050.readAccelData(accelCount); // Read the x/y/z adc values
thanawatinges 3:bf3448217248 81 mpu6050.getAres();
onehorse 0:65aa78c10981 82
thanawatinges 3:bf3448217248 83 // Now we'll calculate the accleration value into actual g's
thanawatinges 3:bf3448217248 84 ax = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set
thanawatinges 3:bf3448217248 85 ay = (float)accelCount[1]*aRes - accelBias[1];
thanawatinges 3:bf3448217248 86 az = (float)accelCount[2]*aRes - accelBias[2];
onehorse 1:cea9d83b8636 87
thanawatinges 3:bf3448217248 88 mpu6050.readGyroData(gyroCount); // Read the x/y/z adc values
thanawatinges 3:bf3448217248 89 mpu6050.getGres();
onehorse 0:65aa78c10981 90
thanawatinges 3:bf3448217248 91 // Calculate the gyro value into actual degrees per second
thanawatinges 3:bf3448217248 92 gx = (float)gyroCount[0]*gRes; // - gyroBias[0]; // get actual gyro value, this depends on scale being set
thanawatinges 3:bf3448217248 93 gy = (float)gyroCount[1]*gRes; // - gyroBias[1];
thanawatinges 3:bf3448217248 94 gz = (float)gyroCount[2]*gRes; // - gyroBias[2];
thanawatinges 3:bf3448217248 95
thanawatinges 3:bf3448217248 96 tempCount = mpu6050.readTempData(); // Read the x/y/z adc values
thanawatinges 3:bf3448217248 97 temperature = (tempCount) / 340. + 36.53; // Temperature in degrees Centigrade
thanawatinges 3:bf3448217248 98 }
thanawatinges 3:bf3448217248 99
thanawatinges 3:bf3448217248 100 Now = t.read_us();
thanawatinges 3:bf3448217248 101 deltat = (float)((Now - lastUpdate)/1000000.0f) ; // set integration time by time elapsed since last filter update
thanawatinges 3:bf3448217248 102 lastUpdate = Now;
thanawatinges 3:bf3448217248 103
thanawatinges 3:bf3448217248 104 sum += deltat;
thanawatinges 3:bf3448217248 105 sumCount++;
thanawatinges 3:bf3448217248 106
thanawatinges 3:bf3448217248 107 if(lastUpdate - firstUpdate > 10000000.0f) {
thanawatinges 3:bf3448217248 108 beta = 0.04; // decrease filter gain after stabilized
thanawatinges 3:bf3448217248 109 zeta = 0.015; // increasey bias drift gain after stabilized
thanawatinges 3:bf3448217248 110 }
onehorse 0:65aa78c10981 111
thanawatinges 3:bf3448217248 112 // Pass gyro rate as rad/s
thanawatinges 3:bf3448217248 113 mpu6050.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f);
onehorse 0:65aa78c10981 114
thanawatinges 3:bf3448217248 115 // Serial print and/or display at 0.5 s rate independent of data rates
thanawatinges 3:bf3448217248 116 delt_t = t.read_ms() - count;
thanawatinges 3:bf3448217248 117 if (delt_t > 500) { // update LCD once per half-second independent of read rate
Unknowplayer 4:5be0ca4af5b7 118
thanawatinges 3:bf3448217248 119 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]);
thanawatinges 3:bf3448217248 120 pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
thanawatinges 3:bf3448217248 121 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]);
thanawatinges 3:bf3448217248 122 pitch *= 180.0f / PI;
thanawatinges 3:bf3448217248 123 yaw *= 180.0f / PI;
thanawatinges 3:bf3448217248 124 roll *= 180.0f / PI;
thanawatinges 3:bf3448217248 125
Unknowplayer 4:5be0ca4af5b7 126 rtc.getTime(tm);
Unknowplayer 4:5be0ca4af5b7 127 // ##############################IMU+BLUETOOTH+DATALOGGER#######################################################################################
thanawatinges 3:bf3448217248 128 pc.printf("Yaw, Pitch, Roll: %f %f %f\n\r", yaw, pitch, roll);
Unknowplayer 4:5be0ca4af5b7 129 storeData(yaw,pitch,roll);
Unknowplayer 4:5be0ca4af5b7 130
Unknowplayer 4:5be0ca4af5b7 131
Unknowplayer 4:5be0ca4af5b7 132 if((yaw > -100 && yaw < 79) && (pitch > -88 && pitch < 45) && (roll > -178 && roll < 178)) {
Unknowplayer 4:5be0ca4af5b7 133 numcount++;
Unknowplayer 4:5be0ca4af5b7 134 sendDataTime(tm.hour);
Unknowplayer 4:5be0ca4af5b7 135 rtc.getTime(tm);
Unknowplayer 4:5be0ca4af5b7 136 pc.printf("num = %d\n",numcount);
Unknowplayer 4:5be0ca4af5b7 137
Unknowplayer 4:5be0ca4af5b7 138
Unknowplayer 4:5be0ca4af5b7 139 }
Unknowplayer 4:5be0ca4af5b7 140 storeData(yaw,pitch,roll);
Unknowplayer 4:5be0ca4af5b7 141 if(device.readable()) {
Unknowplayer 4:5be0ca4af5b7 142 if( device.getc() == '#' ) {
thanawatinges 3:bf3448217248 143
Unknowplayer 4:5be0ca4af5b7 144 for(x=1; x<13; x++) {
Unknowplayer 4:5be0ca4af5b7 145 device.printf("%d\n",m[x]);
Unknowplayer 4:5be0ca4af5b7 146 pc.printf("%d\n",m[x]);
Unknowplayer 4:5be0ca4af5b7 147 wait(0.2);
Unknowplayer 4:5be0ca4af5b7 148 }
Unknowplayer 4:5be0ca4af5b7 149 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;
Unknowplayer 4:5be0ca4af5b7 150 device.printf("%d\n",k);
Unknowplayer 4:5be0ca4af5b7 151 pc.printf("%d\n",k);
Unknowplayer 4:5be0ca4af5b7 152 } else if( device.getc() == '$' ) {
Unknowplayer 4:5be0ca4af5b7 153 clearData();
Unknowplayer 4:5be0ca4af5b7 154 m[13] = 0;
Unknowplayer 4:5be0ca4af5b7 155 l[13] = 0;
Unknowplayer 4:5be0ca4af5b7 156 numcount = 0;
Unknowplayer 4:5be0ca4af5b7 157 k = 0;
Unknowplayer 4:5be0ca4af5b7 158 }
Unknowplayer 4:5be0ca4af5b7 159 }
Unknowplayer 4:5be0ca4af5b7 160
Unknowplayer 4:5be0ca4af5b7 161 // pc.printf("average rate = %f\n\r", (float) sumCount/sum);
Unknowplayer 4:5be0ca4af5b7 162 // #############################################################################################################################################
thanawatinges 3:bf3448217248 163 myled= !myled;
thanawatinges 3:bf3448217248 164 count = t.read_ms();
thanawatinges 3:bf3448217248 165 sum = 0;
thanawatinges 3:bf3448217248 166 sumCount = 0;
thanawatinges 3:bf3448217248 167 }
thanawatinges 3:bf3448217248 168 }
thanawatinges 3:bf3448217248 169
Unknowplayer 4:5be0ca4af5b7 170 }
Unknowplayer 4:5be0ca4af5b7 171
Unknowplayer 4:5be0ca4af5b7 172
Unknowplayer 4:5be0ca4af5b7 173
Unknowplayer 4:5be0ca4af5b7 174 bool storeData(float x, float y, float z)
Unknowplayer 4:5be0ca4af5b7 175 {
Unknowplayer 4:5be0ca4af5b7 176 FILE *fp = fopen("/sd/dataimu/logger.txt", "a");
Unknowplayer 4:5be0ca4af5b7 177 if(fp == NULL) {
Unknowplayer 4:5be0ca4af5b7 178 error("Could not open file for write\n");
Unknowplayer 4:5be0ca4af5b7 179 return false;
Unknowplayer 4:5be0ca4af5b7 180 } else {
Unknowplayer 4:5be0ca4af5b7 181 //sprintf(&input[0],"x=%f, y=%f, z=%f\n ",x,y,z);
Unknowplayer 4:5be0ca4af5b7 182 pc.printf("InreturnData x,y,z\n");
Unknowplayer 4:5be0ca4af5b7 183 fprintf(fp, "yaw=%f , pitch=%f , roll=%f\n ",x,y,z);
Unknowplayer 4:5be0ca4af5b7 184 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]);
Unknowplayer 4:5be0ca4af5b7 185 fprintf(fp, "Sum = %d\n",k);
Unknowplayer 4:5be0ca4af5b7 186 pc.printf("yaw=%f , pitch=%f , roll=%f\n ",x,y,z);
Unknowplayer 4:5be0ca4af5b7 187
Unknowplayer 4:5be0ca4af5b7 188 }
Unknowplayer 4:5be0ca4af5b7 189 pc.printf("OutreturnData\n");
Unknowplayer 4:5be0ca4af5b7 190 fclose(fp);
Unknowplayer 4:5be0ca4af5b7 191 return true;
Unknowplayer 4:5be0ca4af5b7 192 }
Unknowplayer 4:5be0ca4af5b7 193
Unknowplayer 4:5be0ca4af5b7 194
Unknowplayer 4:5be0ca4af5b7 195
Unknowplayer 4:5be0ca4af5b7 196
Unknowplayer 4:5be0ca4af5b7 197 bool sendDataTime(int data)
Unknowplayer 4:5be0ca4af5b7 198 {
Unknowplayer 4:5be0ca4af5b7 199 if(data == 00 || data == 01) {
Unknowplayer 4:5be0ca4af5b7 200 l[1]++;
Unknowplayer 4:5be0ca4af5b7 201 m[1] = l[1]+10000 ;
Unknowplayer 4:5be0ca4af5b7 202 wait(0.5);
Unknowplayer 4:5be0ca4af5b7 203
Unknowplayer 4:5be0ca4af5b7 204
Unknowplayer 4:5be0ca4af5b7 205 } else if(data == 02 || data == 03) {
Unknowplayer 4:5be0ca4af5b7 206 l[2]++;
Unknowplayer 4:5be0ca4af5b7 207 m[2] = l[2]+20000 ;
Unknowplayer 4:5be0ca4af5b7 208 wait(0.5);
Unknowplayer 4:5be0ca4af5b7 209
Unknowplayer 4:5be0ca4af5b7 210 } else if(data == 04 || data == 05) {
Unknowplayer 4:5be0ca4af5b7 211 l[3]++;
Unknowplayer 4:5be0ca4af5b7 212 m[3] = l[3]+30000 ;
Unknowplayer 4:5be0ca4af5b7 213 wait(0.5);
Unknowplayer 4:5be0ca4af5b7 214
Unknowplayer 4:5be0ca4af5b7 215 } else if(data == 06 || data == 07) {
Unknowplayer 4:5be0ca4af5b7 216 l[4]++;
Unknowplayer 4:5be0ca4af5b7 217 m[4] = l[4]+40000 ;
Unknowplayer 4:5be0ca4af5b7 218 wait(0.5);
Unknowplayer 4:5be0ca4af5b7 219
Unknowplayer 4:5be0ca4af5b7 220 } else if(data > 07 && data < 10) {
Unknowplayer 4:5be0ca4af5b7 221 l[5]++;
Unknowplayer 4:5be0ca4af5b7 222 m[5] = l[5]+50000 ;
Unknowplayer 4:5be0ca4af5b7 223 wait(0.5);
Unknowplayer 4:5be0ca4af5b7 224
Unknowplayer 4:5be0ca4af5b7 225 } else if(data == 10 || data == 11) {
Unknowplayer 4:5be0ca4af5b7 226 l[5]++;
Unknowplayer 4:5be0ca4af5b7 227 m[6] = l[6]+60000 ;
Unknowplayer 4:5be0ca4af5b7 228 wait(0.5);
Unknowplayer 4:5be0ca4af5b7 229
Unknowplayer 4:5be0ca4af5b7 230 } else if(data == 12 || data == 13) {
Unknowplayer 4:5be0ca4af5b7 231 l[7]++;
Unknowplayer 4:5be0ca4af5b7 232 m[7] = l[7]+70000 ;
Unknowplayer 4:5be0ca4af5b7 233 wait(0.5);
Unknowplayer 4:5be0ca4af5b7 234
Unknowplayer 4:5be0ca4af5b7 235 } else if(data == 14 || data == 15) {
Unknowplayer 4:5be0ca4af5b7 236 l[8]++;
Unknowplayer 4:5be0ca4af5b7 237 m[8] = l[8]+80000 ;
Unknowplayer 4:5be0ca4af5b7 238 wait(0.5);
Unknowplayer 4:5be0ca4af5b7 239
Unknowplayer 4:5be0ca4af5b7 240 } else if(data == 16 || data == 17) {
Unknowplayer 4:5be0ca4af5b7 241 l[9]++;
Unknowplayer 4:5be0ca4af5b7 242 m[9] = l[9]+90000 ;
Unknowplayer 4:5be0ca4af5b7 243 wait(0.5);
Unknowplayer 4:5be0ca4af5b7 244
Unknowplayer 4:5be0ca4af5b7 245 } else if(data == 18 || data == 19) {
Unknowplayer 4:5be0ca4af5b7 246 l[10]++;
Unknowplayer 4:5be0ca4af5b7 247 m[10] = l[10]+100000 ;
Unknowplayer 4:5be0ca4af5b7 248 wait(0.5);
Unknowplayer 4:5be0ca4af5b7 249
Unknowplayer 4:5be0ca4af5b7 250 } else if(data == 20 || data == 21) {
Unknowplayer 4:5be0ca4af5b7 251 l[11]++;
Unknowplayer 4:5be0ca4af5b7 252 m[11] = l[11]+110000 ;
Unknowplayer 4:5be0ca4af5b7 253 wait(0.5);
Unknowplayer 4:5be0ca4af5b7 254
Unknowplayer 4:5be0ca4af5b7 255 } else if(data == 22 || data == 23) {
Unknowplayer 4:5be0ca4af5b7 256 l[12]++;
Unknowplayer 4:5be0ca4af5b7 257 m[12] = l[12]+120000 ;
Unknowplayer 4:5be0ca4af5b7 258 wait(0.5);
Unknowplayer 4:5be0ca4af5b7 259
Unknowplayer 4:5be0ca4af5b7 260 }
Unknowplayer 4:5be0ca4af5b7 261 return true;
Unknowplayer 4:5be0ca4af5b7 262 }
Unknowplayer 4:5be0ca4af5b7 263
Unknowplayer 4:5be0ca4af5b7 264
Unknowplayer 4:5be0ca4af5b7 265 bool clearData()
Unknowplayer 4:5be0ca4af5b7 266 {
Unknowplayer 4:5be0ca4af5b7 267 //device.printf("clear Data \n");
Unknowplayer 4:5be0ca4af5b7 268 pc.printf("clear Data \n ");
Unknowplayer 4:5be0ca4af5b7 269 remove("/sd/dataimu/logger.txt");
Unknowplayer 4:5be0ca4af5b7 270 return true;
Unknowplayer 4:5be0ca4af5b7 271 }
Unknowplayer 4:5be0ca4af5b7 272
Unknowplayer 4:5be0ca4af5b7 273 bool setTimeData(Rtc_Ds1307::Time_rtc tm)
Unknowplayer 4:5be0ca4af5b7 274 {
Unknowplayer 4:5be0ca4af5b7 275 tm.hour=14;
Unknowplayer 4:5be0ca4af5b7 276 tm.min=50;
Unknowplayer 4:5be0ca4af5b7 277 tm.sec=00;
Unknowplayer 4:5be0ca4af5b7 278 tm.mon=12;
Unknowplayer 4:5be0ca4af5b7 279 tm.date=6;
Unknowplayer 4:5be0ca4af5b7 280 tm.year=2015;
Unknowplayer 4:5be0ca4af5b7 281 rtc.setTime(tm,true,false);
Unknowplayer 4:5be0ca4af5b7 282 }