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);
        }
    }
}