Bradley Kohler / Mbed 2 deprecated Pedometer_Smooth

MPU6050 Pedometer

Dependencies:   mbed

Pedometer using the MPU6050

main.cpp@0:93289d2d6bce, 2017-11-21 (annotated)

Committer:
kohlerba
Date:
Tue Nov 21 20:37:39 2017 +0000
Revision:
0:93289d2d6bce
A pedometer using the MAX30100 6DOF module

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kohlerba 0:93289d2d6bce 1 #include "mbed.h"
kohlerba 0:93289d2d6bce 2 #include "MPU6050.h"
kohlerba 0:93289d2d6bce 3
kohlerba 0:93289d2d6bce 4 InterruptIn button(USER_BUTTON);
kohlerba 0:93289d2d6bce 5 DigitalOut led(LED1);
kohlerba 0:93289d2d6bce 6
kohlerba 0:93289d2d6bce 7 Serial pc(USBTX, USBRX); // tx, rx
kohlerba 0:93289d2d6bce 8
kohlerba 0:93289d2d6bce 9 MPU6050 mpu6050;
kohlerba 0:93289d2d6bce 10
kohlerba 0:93289d2d6bce 11 Timer t;
kohlerba 0:93289d2d6bce 12 Timer step_timer;
kohlerba 0:93289d2d6bce 13
kohlerba 0:93289d2d6bce 14 #define AVERAGE_BUFF_SIZE 20
kohlerba 0:93289d2d6bce 15
kohlerba 0:93289d2d6bce 16 float ax_fl;
kohlerba 0:93289d2d6bce 17 float ay_fl;
kohlerba 0:93289d2d6bce 18 float az_fl;
kohlerba 0:93289d2d6bce 19
kohlerba 0:93289d2d6bce 20 int ax_int;
kohlerba 0:93289d2d6bce 21 int ay_int;
kohlerba 0:93289d2d6bce 22 int az_int;
kohlerba 0:93289d2d6bce 23
kohlerba 0:93289d2d6bce 24 int ax_avg_buff[AVERAGE_BUFF_SIZE];
kohlerba 0:93289d2d6bce 25 int ax_avg_buff_count = 0;
kohlerba 0:93289d2d6bce 26 int ax_avg;
kohlerba 0:93289d2d6bce 27 int ay_avg_buff[AVERAGE_BUFF_SIZE];
kohlerba 0:93289d2d6bce 28 int ay_avg_buff_count = 0;
kohlerba 0:93289d2d6bce 29 int ay_avg;
kohlerba 0:93289d2d6bce 30 int az_avg_buff[AVERAGE_BUFF_SIZE];
kohlerba 0:93289d2d6bce 31 int az_avg_buff_count = 0;
kohlerba 0:93289d2d6bce 32 int az_avg;
kohlerba 0:93289d2d6bce 33
kohlerba 0:93289d2d6bce 34 int now;
kohlerba 0:93289d2d6bce 35 int step_timer_now;
kohlerba 0:93289d2d6bce 36 int min_reg_ax;
kohlerba 0:93289d2d6bce 37 int min_current_ax;
kohlerba 0:93289d2d6bce 38 int min_reg_ay;
kohlerba 0:93289d2d6bce 39 int min_current_ay;
kohlerba 0:93289d2d6bce 40 int min_reg_az;
kohlerba 0:93289d2d6bce 41 int min_current_az;
kohlerba 0:93289d2d6bce 42 int max_reg_ax;
kohlerba 0:93289d2d6bce 43 int max_current_ax;
kohlerba 0:93289d2d6bce 44 int max_reg_ay;
kohlerba 0:93289d2d6bce 45 int max_current_ay;
kohlerba 0:93289d2d6bce 46 int max_reg_az;
kohlerba 0:93289d2d6bce 47 int max_current_az;
kohlerba 0:93289d2d6bce 48 int dy_thres_ax;
kohlerba 0:93289d2d6bce 49 int dy_thres_ay;
kohlerba 0:93289d2d6bce 50 int dy_thres_az;
kohlerba 0:93289d2d6bce 51 int dy_chan_ax;
kohlerba 0:93289d2d6bce 52 int dy_chan_ay;
kohlerba 0:93289d2d6bce 53 int dy_chan_az;
kohlerba 0:93289d2d6bce 54
kohlerba 0:93289d2d6bce 55 int active_axis;
kohlerba 0:93289d2d6bce 56
kohlerba 0:93289d2d6bce 57 int sample_new;
kohlerba 0:93289d2d6bce 58 int sample_old;
kohlerba 0:93289d2d6bce 59 int sample_result;
kohlerba 0:93289d2d6bce 60
kohlerba 0:93289d2d6bce 61 int step_size = 200;
kohlerba 0:93289d2d6bce 62
kohlerba 0:93289d2d6bce 63 int step_count = 0;
kohlerba 0:93289d2d6bce 64
kohlerba 0:93289d2d6bce 65 //sampling variables
kohlerba 0:93289d2d6bce 66 #define SAMPLE_SIZE 2000
kohlerba 0:93289d2d6bce 67
kohlerba 0:93289d2d6bce 68 int test_ax[SAMPLE_SIZE];
kohlerba 0:93289d2d6bce 69 int test_ay[SAMPLE_SIZE];
kohlerba 0:93289d2d6bce 70 int test_az[SAMPLE_SIZE];
kohlerba 0:93289d2d6bce 71 int time_ms[SAMPLE_SIZE];
kohlerba 0:93289d2d6bce 72 int count_a = 0;
kohlerba 0:93289d2d6bce 73 int step_times[SAMPLE_SIZE];
kohlerba 0:93289d2d6bce 74 int step_sample_count = 0;
kohlerba 0:93289d2d6bce 75
kohlerba 0:93289d2d6bce 76 int return_samples = 0;
kohlerba 0:93289d2d6bce 77 //
kohlerba 0:93289d2d6bce 78
kohlerba 0:93289d2d6bce 79 void pressed(){
kohlerba 0:93289d2d6bce 80 if(count_a == SAMPLE_SIZE){
kohlerba 0:93289d2d6bce 81 return_samples = 1;
kohlerba 0:93289d2d6bce 82 }
kohlerba 0:93289d2d6bce 83 else{
kohlerba 0:93289d2d6bce 84 step_times[step_sample_count] = us_ticker_read() / 1000;;
kohlerba 0:93289d2d6bce 85 step_sample_count++;
kohlerba 0:93289d2d6bce 86 }
kohlerba 0:93289d2d6bce 87 }
kohlerba 0:93289d2d6bce 88
kohlerba 0:93289d2d6bce 89 int main() {
kohlerba 0:93289d2d6bce 90
kohlerba 0:93289d2d6bce 91 //----------Setup----------//
kohlerba 0:93289d2d6bce 92
kohlerba 0:93289d2d6bce 93 pc.baud(115200);
kohlerba 0:93289d2d6bce 94
kohlerba 0:93289d2d6bce 95 //Set up I2C
kohlerba 0:93289d2d6bce 96 i2c.frequency(400000); // use fast (400 kHz) I2C
kohlerba 0:93289d2d6bce 97
kohlerba 0:93289d2d6bce 98 t.start();
kohlerba 0:93289d2d6bce 99
kohlerba 0:93289d2d6bce 100 // Read the WHO_AM_I register, this is a good test of communication
kohlerba 0:93289d2d6bce 101 uint8_t whoami = mpu6050.readByte(MPU6050_ADDRESS, WHO_AM_I_MPU6050);
kohlerba 0:93289d2d6bce 102
kohlerba 0:93289d2d6bce 103 if (whoami == 0x68){
kohlerba 0:93289d2d6bce 104 pc.printf("MPU6050 is online...\n\r");
kohlerba 0:93289d2d6bce 105 wait(1);
kohlerba 0:93289d2d6bce 106 }
kohlerba 0:93289d2d6bce 107
kohlerba 0:93289d2d6bce 108 else{
kohlerba 0:93289d2d6bce 109 pc.printf("MPU6050 is offline...\n\r");
kohlerba 0:93289d2d6bce 110 pc.printf("Value is %d\n\r",whoami);
kohlerba 0:93289d2d6bce 111 pc.printf("Should be %d\n\r",0x68);
kohlerba 0:93289d2d6bce 112 while(1){
kohlerba 0:93289d2d6bce 113 wait(1);
kohlerba 0:93289d2d6bce 114 }
kohlerba 0:93289d2d6bce 115 }
kohlerba 0:93289d2d6bce 116
kohlerba 0:93289d2d6bce 117 mpu6050.MPU6050SelfTest(SelfTest); // Start by performing self test and reporting values
kohlerba 0:93289d2d6bce 118 pc.printf("x-axis self test: acceleration trim within : "); pc.printf("%f", SelfTest[0]); pc.printf(" percent of factory value \n\r");
kohlerba 0:93289d2d6bce 119 pc.printf("y-axis self test: acceleration trim within : "); pc.printf("%f", SelfTest[1]); pc.printf(" percent of factory value \n\r");
kohlerba 0:93289d2d6bce 120 pc.printf("z-axis self test: acceleration trim within : "); pc.printf("%f", SelfTest[2]); pc.printf(" percent of factory value \n\r");
kohlerba 0:93289d2d6bce 121 pc.printf("x-axis self test: gyration trim within : "); pc.printf("%f", SelfTest[3]); pc.printf(" percent of factory value \n\r");
kohlerba 0:93289d2d6bce 122 pc.printf("y-axis self test: gyration trim within : "); pc.printf("%f", SelfTest[4]); pc.printf(" percent of factory value \n\r");
kohlerba 0:93289d2d6bce 123 pc.printf("z-axis self test: gyration trim within : "); pc.printf("%f", SelfTest[5]); pc.printf(" percent of factory value \n\r");
kohlerba 0:93289d2d6bce 124 wait(1);
kohlerba 0:93289d2d6bce 125
kohlerba 0:93289d2d6bce 126 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)
kohlerba 0:93289d2d6bce 127 {
kohlerba 0:93289d2d6bce 128 mpu6050.resetMPU6050(); // Reset registers to default in preparation for device calibration
kohlerba 0:93289d2d6bce 129 mpu6050.calibrateMPU6050(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
kohlerba 0:93289d2d6bce 130 mpu6050.initMPU6050(); pc.printf("MPU6050 initialized for active data mode....\n\r"); // Initialize device for active mode read of acclerometer, gyroscope, and temperature
kohlerba 0:93289d2d6bce 131 }
kohlerba 0:93289d2d6bce 132
kohlerba 0:93289d2d6bce 133 else
kohlerba 0:93289d2d6bce 134 {
kohlerba 0:93289d2d6bce 135 pc.printf("Device did not the pass self-test!\n\r");
kohlerba 0:93289d2d6bce 136 }
kohlerba 0:93289d2d6bce 137 wait(1);
kohlerba 0:93289d2d6bce 138
kohlerba 0:93289d2d6bce 139 //----------Loop----------//
kohlerba 0:93289d2d6bce 140
kohlerba 0:93289d2d6bce 141 while(1){
kohlerba 0:93289d2d6bce 142 // If data ready bit set, all data registers have new data
kohlerba 0:93289d2d6bce 143 if(mpu6050.readByte(MPU6050_ADDRESS, INT_STATUS) & 0x01) { // check if data ready interrupt
kohlerba 0:93289d2d6bce 144 mpu6050.readAccelData(accelCount); // Read the x/y/z adc values
kohlerba 0:93289d2d6bce 145 mpu6050.getAres();
kohlerba 0:93289d2d6bce 146
kohlerba 0:93289d2d6bce 147 // Now we'll calculate the accleration value into actual g's
kohlerba 0:93289d2d6bce 148 ax = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set
kohlerba 0:93289d2d6bce 149 ay = (float)accelCount[1]*aRes - accelBias[1];
kohlerba 0:93289d2d6bce 150 az = (float)accelCount[2]*aRes - accelBias[2];
kohlerba 0:93289d2d6bce 151
kohlerba 0:93289d2d6bce 152 if(ax<0){
kohlerba 0:93289d2d6bce 153 ax = ax*-1;
kohlerba 0:93289d2d6bce 154 }
kohlerba 0:93289d2d6bce 155 if(ay<0){
kohlerba 0:93289d2d6bce 156 ay = ay*-1;
kohlerba 0:93289d2d6bce 157 }
kohlerba 0:93289d2d6bce 158 if(az<0){
kohlerba 0:93289d2d6bce 159 az = az*-1;
kohlerba 0:93289d2d6bce 160 }
kohlerba 0:93289d2d6bce 161
kohlerba 0:93289d2d6bce 162 mpu6050.readGyroData(gyroCount); // Read the x/y/z adc values
kohlerba 0:93289d2d6bce 163 mpu6050.getGres();
kohlerba 0:93289d2d6bce 164
kohlerba 0:93289d2d6bce 165 // Calculate the gyro value into actual degrees per second
kohlerba 0:93289d2d6bce 166 gx = (float)gyroCount[0]*gRes; // - gyroBias[0]; // get actual gyro value, this depends on scale being set
kohlerba 0:93289d2d6bce 167 gy = (float)gyroCount[1]*gRes; // - gyroBias[1];
kohlerba 0:93289d2d6bce 168 gz = (float)gyroCount[2]*gRes; // - gyroBias[2];
kohlerba 0:93289d2d6bce 169
kohlerba 0:93289d2d6bce 170 tempCount = mpu6050.readTempData(); // Read the x/y/z adc values
kohlerba 0:93289d2d6bce 171 temperature = (tempCount) / 340. + 36.53; // Temperature in degrees Centigrade
kohlerba 0:93289d2d6bce 172
kohlerba 0:93289d2d6bce 173 // Pass gyro rate as rad/s
kohlerba 0:93289d2d6bce 174 mpu6050.MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f);
kohlerba 0:93289d2d6bce 175
kohlerba 0:93289d2d6bce 176 //accelerometer readings
kohlerba 0:93289d2d6bce 177 ax_fl = 1000 * ax;
kohlerba 0:93289d2d6bce 178 ay_fl = 1000 * ay;
kohlerba 0:93289d2d6bce 179 az_fl = 1000 * az;
kohlerba 0:93289d2d6bce 180
kohlerba 0:93289d2d6bce 181 ax_int = ax_fl;
kohlerba 0:93289d2d6bce 182 ay_int = ay_fl;
kohlerba 0:93289d2d6bce 183 az_int = az_fl;
kohlerba 0:93289d2d6bce 184 //end
kohlerba 0:93289d2d6bce 185
kohlerba 0:93289d2d6bce 186 //average values
kohlerba 0:93289d2d6bce 187 //ax
kohlerba 0:93289d2d6bce 188 ax_avg_buff[ax_avg_buff_count] = ax_int;
kohlerba 0:93289d2d6bce 189 ax_avg_buff_count++;
kohlerba 0:93289d2d6bce 190 ax_avg_buff_count = ax_avg_buff_count % AVERAGE_BUFF_SIZE;
kohlerba 0:93289d2d6bce 191
kohlerba 0:93289d2d6bce 192 ax_avg = 0;
kohlerba 0:93289d2d6bce 193
kohlerba 0:93289d2d6bce 194 int i;
kohlerba 0:93289d2d6bce 195 for(i = 0; i < AVERAGE_BUFF_SIZE;i++){
kohlerba 0:93289d2d6bce 196 ax_avg = ax_avg + ax_avg_buff[i];
kohlerba 0:93289d2d6bce 197 }
kohlerba 0:93289d2d6bce 198
kohlerba 0:93289d2d6bce 199 ax_avg = ax_avg/AVERAGE_BUFF_SIZE;
kohlerba 0:93289d2d6bce 200
kohlerba 0:93289d2d6bce 201 //ay
kohlerba 0:93289d2d6bce 202 ay_avg_buff[ay_avg_buff_count] = ay_int;
kohlerba 0:93289d2d6bce 203 ay_avg_buff_count++;
kohlerba 0:93289d2d6bce 204 ay_avg_buff_count = ay_avg_buff_count % AVERAGE_BUFF_SIZE;
kohlerba 0:93289d2d6bce 205
kohlerba 0:93289d2d6bce 206 ay_avg = 0;
kohlerba 0:93289d2d6bce 207
kohlerba 0:93289d2d6bce 208 for(i = 0; i < AVERAGE_BUFF_SIZE;i++){
kohlerba 0:93289d2d6bce 209 ay_avg = ay_avg + ay_avg_buff[i];
kohlerba 0:93289d2d6bce 210 }
kohlerba 0:93289d2d6bce 211
kohlerba 0:93289d2d6bce 212 ay_avg = ay_avg/AVERAGE_BUFF_SIZE;
kohlerba 0:93289d2d6bce 213
kohlerba 0:93289d2d6bce 214 //az
kohlerba 0:93289d2d6bce 215 az_avg_buff[az_avg_buff_count] = az_int;
kohlerba 0:93289d2d6bce 216 az_avg_buff_count++;
kohlerba 0:93289d2d6bce 217 az_avg_buff_count = az_avg_buff_count % AVERAGE_BUFF_SIZE;
kohlerba 0:93289d2d6bce 218
kohlerba 0:93289d2d6bce 219 az_avg = 0;
kohlerba 0:93289d2d6bce 220
kohlerba 0:93289d2d6bce 221 for(i = 0; i < AVERAGE_BUFF_SIZE;i++){
kohlerba 0:93289d2d6bce 222 az_avg = az_avg + az_avg_buff[i];
kohlerba 0:93289d2d6bce 223 }
kohlerba 0:93289d2d6bce 224
kohlerba 0:93289d2d6bce 225 az_avg = az_avg/AVERAGE_BUFF_SIZE;
kohlerba 0:93289d2d6bce 226 //end
kohlerba 0:93289d2d6bce 227
kohlerba 0:93289d2d6bce 228 //algorithum readings
kohlerba 0:93289d2d6bce 229 now = t.read_ms();
kohlerba 0:93289d2d6bce 230 if(now>=500){
kohlerba 0:93289d2d6bce 231 t.stop();
kohlerba 0:93289d2d6bce 232 t.reset();
kohlerba 0:93289d2d6bce 233 min_current_ax = min_reg_ax;
kohlerba 0:93289d2d6bce 234 max_current_ax = max_reg_ax;
kohlerba 0:93289d2d6bce 235 dy_thres_ax = (min_current_ax+max_current_ax)/2;
kohlerba 0:93289d2d6bce 236 dy_chan_ax = (max_current_ax-min_current_ax);
kohlerba 0:93289d2d6bce 237 min_reg_ax = ax_avg;
kohlerba 0:93289d2d6bce 238 max_reg_ax = ax_avg;
kohlerba 0:93289d2d6bce 239 min_current_ay = min_reg_ay;
kohlerba 0:93289d2d6bce 240 max_current_ay = max_reg_ay;
kohlerba 0:93289d2d6bce 241 dy_thres_ay = (min_current_ay+max_current_ay)/2;
kohlerba 0:93289d2d6bce 242 dy_chan_ay = (max_current_ay-min_current_ay);
kohlerba 0:93289d2d6bce 243 min_reg_ay = ay_avg;
kohlerba 0:93289d2d6bce 244 max_reg_ay = ay_avg;
kohlerba 0:93289d2d6bce 245 min_current_az = min_reg_az;
kohlerba 0:93289d2d6bce 246 max_current_az = max_reg_az;
kohlerba 0:93289d2d6bce 247 dy_thres_az = (min_current_az+max_current_az)/2;
kohlerba 0:93289d2d6bce 248 dy_chan_az = (max_current_az-min_current_az);
kohlerba 0:93289d2d6bce 249 min_reg_az = az_avg;
kohlerba 0:93289d2d6bce 250 max_reg_az = az_avg;
kohlerba 0:93289d2d6bce 251
kohlerba 0:93289d2d6bce 252 //active axis switching
kohlerba 0:93289d2d6bce 253 if(dy_chan_ax>=dy_chan_ay && dy_chan_ax>= dy_chan_az){
kohlerba 0:93289d2d6bce 254 if(active_axis!=0){
kohlerba 0:93289d2d6bce 255 sample_old = 0;
kohlerba 0:93289d2d6bce 256 sample_new = ax_avg;
kohlerba 0:93289d2d6bce 257 }
kohlerba 0:93289d2d6bce 258 active_axis = 0;
kohlerba 0:93289d2d6bce 259 }
kohlerba 0:93289d2d6bce 260 else if(dy_chan_ay>=dy_chan_ax && dy_chan_ay>=dy_chan_az){
kohlerba 0:93289d2d6bce 261 if(active_axis!=1){
kohlerba 0:93289d2d6bce 262 sample_old = 0;
kohlerba 0:93289d2d6bce 263 sample_new = ay_avg;
kohlerba 0:93289d2d6bce 264 }
kohlerba 0:93289d2d6bce 265 active_axis = 1;
kohlerba 0:93289d2d6bce 266 }
kohlerba 0:93289d2d6bce 267 else{
kohlerba 0:93289d2d6bce 268 if(active_axis!=2){
kohlerba 0:93289d2d6bce 269 sample_old = 0;
kohlerba 0:93289d2d6bce 270 sample_new = az_avg;
kohlerba 0:93289d2d6bce 271 }
kohlerba 0:93289d2d6bce 272 active_axis = 2;
kohlerba 0:93289d2d6bce 273 }
kohlerba 0:93289d2d6bce 274 //end
kohlerba 0:93289d2d6bce 275
kohlerba 0:93289d2d6bce 276 t.start();
kohlerba 0:93289d2d6bce 277 }
kohlerba 0:93289d2d6bce 278 else if(now<500){
kohlerba 0:93289d2d6bce 279 if(min_reg_ax>ax_avg){
kohlerba 0:93289d2d6bce 280 min_reg_ax = ax_avg;
kohlerba 0:93289d2d6bce 281 }
kohlerba 0:93289d2d6bce 282 if(max_reg_ax<ax_avg){
kohlerba 0:93289d2d6bce 283 max_reg_ax = ax_avg;
kohlerba 0:93289d2d6bce 284 }
kohlerba 0:93289d2d6bce 285 if(min_reg_ay>ay_avg){
kohlerba 0:93289d2d6bce 286 min_reg_ay = ay_avg;
kohlerba 0:93289d2d6bce 287 }
kohlerba 0:93289d2d6bce 288 if(max_reg_ay<ay_avg){
kohlerba 0:93289d2d6bce 289 max_reg_ay = ay_avg;
kohlerba 0:93289d2d6bce 290 }
kohlerba 0:93289d2d6bce 291 if(min_reg_az>az_avg){
kohlerba 0:93289d2d6bce 292 min_reg_az = az_avg;
kohlerba 0:93289d2d6bce 293 }
kohlerba 0:93289d2d6bce 294 if(max_reg_az<az_avg){
kohlerba 0:93289d2d6bce 295 max_reg_az = az_avg;
kohlerba 0:93289d2d6bce 296 }
kohlerba 0:93289d2d6bce 297 }
kohlerba 0:93289d2d6bce 298 //end
kohlerba 0:93289d2d6bce 299
kohlerba 0:93289d2d6bce 300 //sample
kohlerba 0:93289d2d6bce 301 sample_old = sample_new;
kohlerba 0:93289d2d6bce 302 switch(active_axis){
kohlerba 0:93289d2d6bce 303 case(0):
kohlerba 0:93289d2d6bce 304 if(ax_avg-sample_old>step_size || ax_avg-sample_old<-step_size){
kohlerba 0:93289d2d6bce 305 sample_new = ax_avg;
kohlerba 0:93289d2d6bce 306 if(sample_old>dy_thres_ax && sample_new<dy_thres_ax){
kohlerba 0:93289d2d6bce 307 step_count++;
kohlerba 0:93289d2d6bce 308 }
kohlerba 0:93289d2d6bce 309 }
kohlerba 0:93289d2d6bce 310 break;
kohlerba 0:93289d2d6bce 311 case(1):
kohlerba 0:93289d2d6bce 312 if(ay_avg-sample_old>step_size || ay_avg-sample_old<-step_size){
kohlerba 0:93289d2d6bce 313 sample_new = ay_avg;
kohlerba 0:93289d2d6bce 314 if(sample_old>dy_thres_ay && sample_new<dy_thres_ay){
kohlerba 0:93289d2d6bce 315 step_count++;
kohlerba 0:93289d2d6bce 316 }
kohlerba 0:93289d2d6bce 317 }
kohlerba 0:93289d2d6bce 318 break;
kohlerba 0:93289d2d6bce 319 case(2):
kohlerba 0:93289d2d6bce 320 if(az_avg-sample_old>step_size || az_avg-sample_old<-step_size){
kohlerba 0:93289d2d6bce 321 sample_new = az_avg;
kohlerba 0:93289d2d6bce 322 if(sample_old>dy_thres_az && sample_new<dy_thres_az){
kohlerba 0:93289d2d6bce 323 step_count++;
kohlerba 0:93289d2d6bce 324 }
kohlerba 0:93289d2d6bce 325 }
kohlerba 0:93289d2d6bce 326 break;
kohlerba 0:93289d2d6bce 327 }
kohlerba 0:93289d2d6bce 328
kohlerba 0:93289d2d6bce 329 //sampling data
kohlerba 0:93289d2d6bce 330 if(count_a < SAMPLE_SIZE){
kohlerba 0:93289d2d6bce 331 test_ax[count_a] = ax_int;
kohlerba 0:93289d2d6bce 332 test_ay[count_a] = ay_int;
kohlerba 0:93289d2d6bce 333 test_az[count_a] = az_int;
kohlerba 0:93289d2d6bce 334 time_ms[count_a] = us_ticker_read() / 1000;
kohlerba 0:93289d2d6bce 335 count_a++;
kohlerba 0:93289d2d6bce 336 }
kohlerba 0:93289d2d6bce 337 else if(count_a == SAMPLE_SIZE && return_samples == 1){
kohlerba 0:93289d2d6bce 338 /*
kohlerba 0:93289d2d6bce 339 int i;
kohlerba 0:93289d2d6bce 340
kohlerba 0:93289d2d6bce 341 //ax samples
kohlerba 0:93289d2d6bce 342 pc.printf("ax = [");
kohlerba 0:93289d2d6bce 343 for(i = 0; i<SAMPLE_SIZE; i++){
kohlerba 0:93289d2d6bce 344 pc.printf(" %d ",test_ax[i]);
kohlerba 0:93289d2d6bce 345 }
kohlerba 0:93289d2d6bce 346 pc.printf("]\r\n");
kohlerba 0:93289d2d6bce 347
kohlerba 0:93289d2d6bce 348 //ay samples
kohlerba 0:93289d2d6bce 349 pc.printf("ay = [");
kohlerba 0:93289d2d6bce 350 for(i = 0; i<SAMPLE_SIZE; i++){
kohlerba 0:93289d2d6bce 351 pc.printf(" %d ",test_ay[i]);
kohlerba 0:93289d2d6bce 352 }
kohlerba 0:93289d2d6bce 353 pc.printf("]\r\n");
kohlerba 0:93289d2d6bce 354
kohlerba 0:93289d2d6bce 355 //az samples
kohlerba 0:93289d2d6bce 356 pc.printf("az = [");
kohlerba 0:93289d2d6bce 357 for(i = 0; i<SAMPLE_SIZE; i++){
kohlerba 0:93289d2d6bce 358 pc.printf(" %d ",test_az[i]);
kohlerba 0:93289d2d6bce 359 }
kohlerba 0:93289d2d6bce 360 pc.printf("]\r\n");
kohlerba 0:93289d2d6bce 361
kohlerba 0:93289d2d6bce 362 //timer samples
kohlerba 0:93289d2d6bce 363 pc.printf("t = [");
kohlerba 0:93289d2d6bce 364 for(i = 0; i<SAMPLE_SIZE; i++){
kohlerba 0:93289d2d6bce 365 pc.printf(" %d ",time_ms[i]);
kohlerba 0:93289d2d6bce 366 }
kohlerba 0:93289d2d6bce 367 pc.printf("]\r\n");
kohlerba 0:93289d2d6bce 368
kohlerba 0:93289d2d6bce 369 //step samples
kohlerba 0:93289d2d6bce 370 pc.printf("s = [");
kohlerba 0:93289d2d6bce 371 for(i = 0; i<SAMPLE_SIZE; i++){
kohlerba 0:93289d2d6bce 372 pc.printf(" %d ",step_times[i]);
kohlerba 0:93289d2d6bce 373 }
kohlerba 0:93289d2d6bce 374 pc.printf("]\r\n");
kohlerba 0:93289d2d6bce 375 */
kohlerba 0:93289d2d6bce 376 count_a++;
kohlerba 0:93289d2d6bce 377 }
kohlerba 0:93289d2d6bce 378
kohlerba 0:93289d2d6bce 379 button.fall(&pressed);
kohlerba 0:93289d2d6bce 380 //end
kohlerba 0:93289d2d6bce 381
kohlerba 0:93289d2d6bce 382 //printing rtd
kohlerba 0:93289d2d6bce 383 pc.printf("$%d %d %d;", ax_avg, ay_avg, step_count*100);
kohlerba 0:93289d2d6bce 384 //end
kohlerba 0:93289d2d6bce 385
kohlerba 0:93289d2d6bce 386 //wait
kohlerba 0:93289d2d6bce 387 wait_ms(10);
kohlerba 0:93289d2d6bce 388 }
kohlerba 0:93289d2d6bce 389 }
kohlerba 0:93289d2d6bce 390 }