Bradley Kohler
/
Pedometer_Smooth
MPU6050 Pedometer
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?
User | Revision | Line number | New 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 | } |