Bradley Kohler
MPU6050 Pedometer
Pedometer using the MPU6050
Diff: main.cpp
- Revision:
- 0:93289d2d6bce
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Tue Nov 21 20:37:39 2017 +0000
@@ -0,0 +1,390 @@
+#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);
+ }
+ }
+}