main.cpp

00001 #include "mbed.h"
00002 #include "MPU6050.h"
00003 
00004 InterruptIn button(USER_BUTTON);
00005 DigitalOut led(LED1);
00006 
00007 Serial pc(USBTX, USBRX); // tx, rx
00008 
00009 MPU6050 mpu6050;
00010 
00011 Timer t;
00012 Timer step_timer;
00013 
00014 #define AVERAGE_BUFF_SIZE 20
00015 
00016 float ax_fl;
00017 float ay_fl;
00018 float az_fl;
00019 
00020 int ax_int;
00021 int ay_int;
00022 int az_int;
00023 
00024 int ax_avg_buff[AVERAGE_BUFF_SIZE];
00025 int ax_avg_buff_count = 0;
00026 int ax_avg;
00027 int ay_avg_buff[AVERAGE_BUFF_SIZE];
00028 int ay_avg_buff_count = 0;
00029 int ay_avg;
00030 int az_avg_buff[AVERAGE_BUFF_SIZE];
00031 int az_avg_buff_count = 0;
00032 int az_avg;
00033 
00034 int now;
00035 int step_timer_now;
00036 int min_reg_ax;
00037 int min_current_ax;
00038 int min_reg_ay;
00039 int min_current_ay;
00040 int min_reg_az;
00041 int min_current_az;
00042 int max_reg_ax;
00043 int max_current_ax;
00044 int max_reg_ay;
00045 int max_current_ay;
00046 int max_reg_az;
00047 int max_current_az;
00048 int dy_thres_ax;
00049 int dy_thres_ay;
00050 int dy_thres_az;
00051 int dy_chan_ax;
00052 int dy_chan_ay;
00053 int dy_chan_az;
00054 
00055 int active_axis;
00056 
00057 int sample_new;
00058 int sample_old;
00059 int sample_result;
00060 
00061 int step_size = 200;
00062 
00063 int step_count = 0;
00064 
00065 //sampling variables
00066 #define SAMPLE_SIZE 2000
00067 
00068 int test_ax[SAMPLE_SIZE];
00069 int test_ay[SAMPLE_SIZE];
00070 int test_az[SAMPLE_SIZE];
00071 int time_ms[SAMPLE_SIZE];
00072 int count_a = 0;
00073 int step_times[SAMPLE_SIZE];
00074 int step_sample_count = 0;
00075 
00076 int return_samples = 0;
00077 //
00078 
00079 void pressed(){
00080     if(count_a == SAMPLE_SIZE){
00081         return_samples = 1;
00082     }
00083     else{
00084         step_times[step_sample_count] = us_ticker_read() / 1000;;
00085         step_sample_count++;
00086     }
00087 }
00088 
00089 int main() {
00090     
00091     //----------Setup----------//
00092     
00093     pc.baud(115200);
00094     
00095     //Set up I2C
00096     i2c.frequency(400000);  // use fast (400 kHz) I2C  
00097     
00098     t.start(); 
00099     
00100     // Read the WHO_AM_I register, this is a good test of communication
00101     uint8_t whoami = mpu6050.readByte(MPU6050_ADDRESS, WHO_AM_I_MPU6050);
00102     
00103     if (whoami == 0x68){  
00104         pc.printf("MPU6050 is online...\n\r");
00105         wait(1);
00106     }
00107     
00108     else{
00109         pc.printf("MPU6050 is offline...\n\r");
00110         pc.printf("Value is %d\n\r",whoami);
00111         pc.printf("Should be %d\n\r",0x68);
00112         while(1){
00113             wait(1);
00114         }
00115     }
00116     
00117     mpu6050.MPU6050SelfTest(SelfTest); // Start by performing self test and reporting values
00118     pc.printf("x-axis self test: acceleration trim within : "); pc.printf("%f", SelfTest[0]); pc.printf(" percent of factory value \n\r");
00119     pc.printf("y-axis self test: acceleration trim within : "); pc.printf("%f", SelfTest[1]); pc.printf(" percent of factory value \n\r");
00120     pc.printf("z-axis self test: acceleration trim within : "); pc.printf("%f", SelfTest[2]); pc.printf(" percent of factory value \n\r");
00121     pc.printf("x-axis self test: gyration trim within : "); pc.printf("%f", SelfTest[3]); pc.printf(" percent of factory value \n\r");
00122     pc.printf("y-axis self test: gyration trim within : "); pc.printf("%f", SelfTest[4]); pc.printf(" percent of factory value \n\r");
00123     pc.printf("z-axis self test: gyration trim within : "); pc.printf("%f", SelfTest[5]); pc.printf(" percent of factory value \n\r");
00124     wait(1);
00125     
00126     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) 
00127     {
00128         mpu6050.resetMPU6050(); // Reset registers to default in preparation for device calibration
00129         mpu6050.calibrateMPU6050(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers  
00130         mpu6050.initMPU6050(); pc.printf("MPU6050 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
00131     }
00132     
00133     else
00134     {
00135         pc.printf("Device did not the pass self-test!\n\r");    
00136     }
00137     wait(1);
00138     
00139     //----------Loop----------//
00140     
00141     while(1){
00142         // If data ready bit set, all data registers have new data
00143         if(mpu6050.readByte(MPU6050_ADDRESS, INT_STATUS) & 0x01) {  // check if data ready interrupt
00144             mpu6050.readAccelData(accelCount);  // Read the x/y/z adc values
00145             mpu6050.getAres();
00146         
00147             // Now we'll calculate the accleration value into actual g's
00148             ax = (float)accelCount[0]*aRes - accelBias[0];  // get actual g value, this depends on scale being set
00149             ay = (float)accelCount[1]*aRes - accelBias[1];   
00150             az = (float)accelCount[2]*aRes - accelBias[2];
00151             
00152             if(ax<0){
00153             ax = ax*-1;
00154             }
00155             if(ay<0){
00156             ay = ay*-1;
00157             }
00158             if(az<0){
00159             az = az*-1;
00160             }
00161             
00162             mpu6050.readGyroData(gyroCount);  // Read the x/y/z adc values
00163             mpu6050.getGres();
00164  
00165             // Calculate the gyro value into actual degrees per second
00166             gx = (float)gyroCount[0]*gRes; // - gyroBias[0];  // get actual gyro value, this depends on scale being set
00167             gy = (float)gyroCount[1]*gRes; // - gyroBias[1];  
00168             gz = (float)gyroCount[2]*gRes; // - gyroBias[2];   
00169 
00170             tempCount = mpu6050.readTempData();  // Read the x/y/z adc values
00171             temperature = (tempCount) / 340. + 36.53; // Temperature in degrees Centigrade
00172             
00173             // Pass gyro rate as rad/s
00174             mpu6050.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f);
00175             
00176             //accelerometer readings
00177             ax_fl = 1000 * ax;
00178             ay_fl = 1000 * ay;
00179             az_fl = 1000 * az;
00180             
00181             ax_int = ax_fl;
00182             ay_int = ay_fl;
00183             az_int = az_fl;
00184             //end
00185             
00186             //average values
00187             //ax
00188             ax_avg_buff[ax_avg_buff_count] = ax_int;
00189             ax_avg_buff_count++;
00190             ax_avg_buff_count = ax_avg_buff_count % AVERAGE_BUFF_SIZE;
00191             
00192             ax_avg = 0;
00193             
00194             int i;
00195             for(i = 0; i < AVERAGE_BUFF_SIZE;i++){
00196                 ax_avg = ax_avg + ax_avg_buff[i];
00197             }
00198             
00199             ax_avg = ax_avg/AVERAGE_BUFF_SIZE;
00200             
00201             //ay
00202             ay_avg_buff[ay_avg_buff_count] = ay_int;
00203             ay_avg_buff_count++;
00204             ay_avg_buff_count = ay_avg_buff_count % AVERAGE_BUFF_SIZE;
00205             
00206             ay_avg = 0;
00207             
00208             for(i = 0; i < AVERAGE_BUFF_SIZE;i++){
00209                 ay_avg = ay_avg + ay_avg_buff[i];
00210             }
00211             
00212             ay_avg = ay_avg/AVERAGE_BUFF_SIZE;
00213             
00214             //az
00215             az_avg_buff[az_avg_buff_count] = az_int;
00216             az_avg_buff_count++;
00217             az_avg_buff_count = az_avg_buff_count % AVERAGE_BUFF_SIZE;
00218             
00219             az_avg = 0;
00220             
00221             for(i = 0; i < AVERAGE_BUFF_SIZE;i++){
00222                 az_avg = az_avg + az_avg_buff[i];
00223             }
00224             
00225             az_avg = az_avg/AVERAGE_BUFF_SIZE;
00226             //end
00227             
00228             //algorithum readings
00229             now = t.read_ms();
00230             if(now>=500){
00231                 t.stop();
00232                 t.reset();
00233                 min_current_ax = min_reg_ax;
00234                 max_current_ax = max_reg_ax;
00235                 dy_thres_ax = (min_current_ax+max_current_ax)/2;
00236                 dy_chan_ax = (max_current_ax-min_current_ax);
00237                 min_reg_ax = ax_avg;
00238                 max_reg_ax = ax_avg;
00239                 min_current_ay = min_reg_ay;
00240                 max_current_ay = max_reg_ay;
00241                 dy_thres_ay = (min_current_ay+max_current_ay)/2;
00242                 dy_chan_ay = (max_current_ay-min_current_ay);
00243                 min_reg_ay = ay_avg;
00244                 max_reg_ay = ay_avg;
00245                 min_current_az = min_reg_az;
00246                 max_current_az = max_reg_az;
00247                 dy_thres_az = (min_current_az+max_current_az)/2;
00248                 dy_chan_az = (max_current_az-min_current_az);
00249                 min_reg_az = az_avg;
00250                 max_reg_az = az_avg;
00251                 
00252                 //active axis switching
00253                 if(dy_chan_ax>=dy_chan_ay && dy_chan_ax>= dy_chan_az){
00254                     if(active_axis!=0){
00255                         sample_old = 0;
00256                         sample_new = ax_avg;
00257                     }
00258                     active_axis = 0;
00259                 }
00260                 else if(dy_chan_ay>=dy_chan_ax && dy_chan_ay>=dy_chan_az){
00261                     if(active_axis!=1){
00262                         sample_old = 0;
00263                         sample_new = ay_avg;
00264                     }
00265                     active_axis = 1;
00266                 }
00267                 else{
00268                     if(active_axis!=2){
00269                         sample_old = 0;
00270                         sample_new = az_avg;
00271                     }
00272                     active_axis = 2;
00273                 }
00274                 //end
00275                 
00276                 t.start();
00277             }
00278             else if(now<500){
00279                 if(min_reg_ax>ax_avg){
00280                     min_reg_ax = ax_avg;
00281                 }
00282                 if(max_reg_ax<ax_avg){
00283                     max_reg_ax = ax_avg;
00284                 }
00285                 if(min_reg_ay>ay_avg){
00286                     min_reg_ay = ay_avg;
00287                 }
00288                 if(max_reg_ay<ay_avg){
00289                     max_reg_ay = ay_avg;
00290                 }
00291                 if(min_reg_az>az_avg){
00292                     min_reg_az = az_avg;
00293                 }
00294                 if(max_reg_az<az_avg){
00295                     max_reg_az = az_avg;
00296                 }
00297             }
00298             //end
00299             
00300             //sample
00301             sample_old = sample_new;
00302             switch(active_axis){
00303             case(0): 
00304                 if(ax_avg-sample_old>step_size || ax_avg-sample_old<-step_size){
00305                     sample_new = ax_avg;
00306                     if(sample_old>dy_thres_ax && sample_new<dy_thres_ax){
00307                         step_count++;
00308                     }
00309                 }
00310                 break;
00311             case(1):
00312                 if(ay_avg-sample_old>step_size || ay_avg-sample_old<-step_size){
00313                     sample_new = ay_avg;
00314                     if(sample_old>dy_thres_ay && sample_new<dy_thres_ay){
00315                         step_count++;
00316                     }
00317                 }
00318                 break;
00319             case(2):
00320                 if(az_avg-sample_old>step_size || az_avg-sample_old<-step_size){
00321                     sample_new = az_avg;
00322                     if(sample_old>dy_thres_az && sample_new<dy_thres_az){
00323                         step_count++;
00324                     }
00325                 }
00326                 break;
00327             }
00328             
00329             //sampling data
00330             if(count_a < SAMPLE_SIZE){
00331                 test_ax[count_a] = ax_int;
00332                 test_ay[count_a] = ay_int;
00333                 test_az[count_a] = az_int;
00334                 time_ms[count_a] = us_ticker_read() / 1000;
00335                 count_a++;
00336             }
00337             else if(count_a == SAMPLE_SIZE && return_samples == 1){
00338                 /*
00339                 int i;
00340                 
00341                 //ax samples
00342                 pc.printf("ax = [");
00343                 for(i = 0; i<SAMPLE_SIZE; i++){
00344                     pc.printf(" %d ",test_ax[i]);
00345                 }
00346                 pc.printf("]\r\n");
00347                 
00348                 //ay samples
00349                 pc.printf("ay = [");
00350                 for(i = 0; i<SAMPLE_SIZE; i++){
00351                     pc.printf(" %d ",test_ay[i]);
00352                 }
00353                 pc.printf("]\r\n");
00354                 
00355                 //az samples
00356                 pc.printf("az = [");
00357                 for(i = 0; i<SAMPLE_SIZE; i++){
00358                     pc.printf(" %d ",test_az[i]);
00359                 }
00360                 pc.printf("]\r\n");
00361                 
00362                 //timer samples
00363                 pc.printf("t = [");
00364                 for(i = 0; i<SAMPLE_SIZE; i++){
00365                     pc.printf(" %d ",time_ms[i]);
00366                 }
00367                 pc.printf("]\r\n");
00368                 
00369                 //step samples
00370                 pc.printf("s = [");
00371                 for(i = 0; i<SAMPLE_SIZE; i++){
00372                     pc.printf(" %d ",step_times[i]);
00373                 }
00374                 pc.printf("]\r\n");
00375                 */
00376                 count_a++;        
00377             }
00378             
00379             button.fall(&pressed);
00380             //end
00381             
00382             //printing rtd
00383             pc.printf("$%d %d %d;", ax_avg, ay_avg, step_count*100);
00384             //end
00385             
00386             //wait 
00387             wait_ms(10);
00388         }
00389     }
00390 }