Bradley Kohler
/
Pedometer_Smooth
MPU6050 Pedometer
Pedometer using the MPU6050
main.cpp
- Committer:
- kohlerba
- Date:
- 2017-11-21
- Revision:
- 0:93289d2d6bce
File content as of revision 0:93289d2d6bce:
#include "mbed.h" #include "MPU6050.h" InterruptIn button(USER_BUTTON); DigitalOut led(LED1); Serial pc(USBTX, USBRX); // tx, rx MPU6050 mpu6050; Timer t; Timer step_timer; #define AVERAGE_BUFF_SIZE 20 float ax_fl; float ay_fl; float az_fl; int ax_int; int ay_int; int az_int; int ax_avg_buff[AVERAGE_BUFF_SIZE]; int ax_avg_buff_count = 0; int ax_avg; int ay_avg_buff[AVERAGE_BUFF_SIZE]; int ay_avg_buff_count = 0; int ay_avg; int az_avg_buff[AVERAGE_BUFF_SIZE]; int az_avg_buff_count = 0; int az_avg; int now; int step_timer_now; int min_reg_ax; int min_current_ax; int min_reg_ay; int min_current_ay; int min_reg_az; int min_current_az; int max_reg_ax; int max_current_ax; int max_reg_ay; int max_current_ay; int max_reg_az; int max_current_az; int dy_thres_ax; int dy_thres_ay; int dy_thres_az; int dy_chan_ax; int dy_chan_ay; int dy_chan_az; int active_axis; int sample_new; int sample_old; int sample_result; int step_size = 200; int step_count = 0; //sampling variables #define SAMPLE_SIZE 2000 int test_ax[SAMPLE_SIZE]; int test_ay[SAMPLE_SIZE]; int test_az[SAMPLE_SIZE]; int time_ms[SAMPLE_SIZE]; int count_a = 0; int step_times[SAMPLE_SIZE]; int step_sample_count = 0; int return_samples = 0; // void pressed(){ if(count_a == SAMPLE_SIZE){ return_samples = 1; } else{ step_times[step_sample_count] = us_ticker_read() / 1000;; step_sample_count++; } } int main() { //----------Setup----------// pc.baud(115200); //Set up I2C i2c.frequency(400000); // use fast (400 kHz) I2C t.start(); // Read the WHO_AM_I register, this is a good test of communication uint8_t whoami = mpu6050.readByte(MPU6050_ADDRESS, WHO_AM_I_MPU6050); if (whoami == 0x68){ pc.printf("MPU6050 is online...\n\r"); wait(1); } else{ pc.printf("MPU6050 is offline...\n\r"); pc.printf("Value is %d\n\r",whoami); pc.printf("Should be %d\n\r",0x68); while(1){ wait(1); } } mpu6050.MPU6050SelfTest(SelfTest); // Start by performing self test and reporting values pc.printf("x-axis self test: acceleration trim within : "); pc.printf("%f", SelfTest[0]); pc.printf(" percent of factory value \n\r"); pc.printf("y-axis self test: acceleration trim within : "); pc.printf("%f", SelfTest[1]); pc.printf(" percent of factory value \n\r"); pc.printf("z-axis self test: acceleration trim within : "); pc.printf("%f", SelfTest[2]); pc.printf(" percent of factory value \n\r"); pc.printf("x-axis self test: gyration trim within : "); pc.printf("%f", SelfTest[3]); pc.printf(" percent of factory value \n\r"); pc.printf("y-axis self test: gyration trim within : "); pc.printf("%f", SelfTest[4]); pc.printf(" percent of factory value \n\r"); pc.printf("z-axis self test: gyration trim within : "); pc.printf("%f", SelfTest[5]); pc.printf(" percent of factory value \n\r"); 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 } else { pc.printf("Device did not the pass self-test!\n\r"); } wait(1); //----------Loop----------// 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]; if(ax<0){ ax = ax*-1; } if(ay<0){ ay = ay*-1; } if(az<0){ az = az*-1; } 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 // Pass gyro rate as rad/s mpu6050.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f); //accelerometer readings ax_fl = 1000 * ax; ay_fl = 1000 * ay; az_fl = 1000 * az; ax_int = ax_fl; ay_int = ay_fl; az_int = az_fl; //end //average values //ax ax_avg_buff[ax_avg_buff_count] = ax_int; ax_avg_buff_count++; ax_avg_buff_count = ax_avg_buff_count % AVERAGE_BUFF_SIZE; ax_avg = 0; int i; for(i = 0; i < AVERAGE_BUFF_SIZE;i++){ ax_avg = ax_avg + ax_avg_buff[i]; } ax_avg = ax_avg/AVERAGE_BUFF_SIZE; //ay ay_avg_buff[ay_avg_buff_count] = ay_int; ay_avg_buff_count++; ay_avg_buff_count = ay_avg_buff_count % AVERAGE_BUFF_SIZE; ay_avg = 0; for(i = 0; i < AVERAGE_BUFF_SIZE;i++){ ay_avg = ay_avg + ay_avg_buff[i]; } ay_avg = ay_avg/AVERAGE_BUFF_SIZE; //az az_avg_buff[az_avg_buff_count] = az_int; az_avg_buff_count++; az_avg_buff_count = az_avg_buff_count % AVERAGE_BUFF_SIZE; az_avg = 0; for(i = 0; i < AVERAGE_BUFF_SIZE;i++){ az_avg = az_avg + az_avg_buff[i]; } az_avg = az_avg/AVERAGE_BUFF_SIZE; //end //algorithum readings now = t.read_ms(); if(now>=500){ t.stop(); t.reset(); min_current_ax = min_reg_ax; max_current_ax = max_reg_ax; dy_thres_ax = (min_current_ax+max_current_ax)/2; dy_chan_ax = (max_current_ax-min_current_ax); min_reg_ax = ax_avg; max_reg_ax = ax_avg; min_current_ay = min_reg_ay; max_current_ay = max_reg_ay; dy_thres_ay = (min_current_ay+max_current_ay)/2; dy_chan_ay = (max_current_ay-min_current_ay); min_reg_ay = ay_avg; max_reg_ay = ay_avg; min_current_az = min_reg_az; max_current_az = max_reg_az; dy_thres_az = (min_current_az+max_current_az)/2; dy_chan_az = (max_current_az-min_current_az); min_reg_az = az_avg; max_reg_az = az_avg; //active axis switching if(dy_chan_ax>=dy_chan_ay && dy_chan_ax>= dy_chan_az){ if(active_axis!=0){ sample_old = 0; sample_new = ax_avg; } active_axis = 0; } else if(dy_chan_ay>=dy_chan_ax && dy_chan_ay>=dy_chan_az){ if(active_axis!=1){ sample_old = 0; sample_new = ay_avg; } active_axis = 1; } else{ if(active_axis!=2){ sample_old = 0; sample_new = az_avg; } active_axis = 2; } //end t.start(); } else if(now<500){ if(min_reg_ax>ax_avg){ min_reg_ax = ax_avg; } if(max_reg_ax<ax_avg){ max_reg_ax = ax_avg; } if(min_reg_ay>ay_avg){ min_reg_ay = ay_avg; } if(max_reg_ay<ay_avg){ max_reg_ay = ay_avg; } if(min_reg_az>az_avg){ min_reg_az = az_avg; } if(max_reg_az<az_avg){ max_reg_az = az_avg; } } //end //sample sample_old = sample_new; switch(active_axis){ case(0): if(ax_avg-sample_old>step_size || ax_avg-sample_old<-step_size){ sample_new = ax_avg; if(sample_old>dy_thres_ax && sample_new<dy_thres_ax){ step_count++; } } break; case(1): if(ay_avg-sample_old>step_size || ay_avg-sample_old<-step_size){ sample_new = ay_avg; if(sample_old>dy_thres_ay && sample_new<dy_thres_ay){ step_count++; } } break; case(2): if(az_avg-sample_old>step_size || az_avg-sample_old<-step_size){ sample_new = az_avg; if(sample_old>dy_thres_az && sample_new<dy_thres_az){ step_count++; } } break; } //sampling data if(count_a < SAMPLE_SIZE){ test_ax[count_a] = ax_int; test_ay[count_a] = ay_int; test_az[count_a] = az_int; time_ms[count_a] = us_ticker_read() / 1000; count_a++; } else if(count_a == SAMPLE_SIZE && return_samples == 1){ /* int i; //ax samples pc.printf("ax = ["); for(i = 0; i<SAMPLE_SIZE; i++){ pc.printf(" %d ",test_ax[i]); } pc.printf("]\r\n"); //ay samples pc.printf("ay = ["); for(i = 0; i<SAMPLE_SIZE; i++){ pc.printf(" %d ",test_ay[i]); } pc.printf("]\r\n"); //az samples pc.printf("az = ["); for(i = 0; i<SAMPLE_SIZE; i++){ pc.printf(" %d ",test_az[i]); } pc.printf("]\r\n"); //timer samples pc.printf("t = ["); for(i = 0; i<SAMPLE_SIZE; i++){ pc.printf(" %d ",time_ms[i]); } pc.printf("]\r\n"); //step samples pc.printf("s = ["); for(i = 0; i<SAMPLE_SIZE; i++){ pc.printf(" %d ",step_times[i]); } pc.printf("]\r\n"); */ count_a++; } button.fall(&pressed); //end //printing rtd pc.printf("$%d %d %d;", ax_avg, ay_avg, step_count*100); //end //wait wait_ms(10); } } }