Weiqian Xu
for led strip
Diff: motion_tracking.cpp
- Revision:
- 0:da91c8ed4a98
- Child:
- 1:b90027de2bdb
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/motion_tracking.cpp Mon Jun 11 20:33:26 2018 +0000
@@ -0,0 +1,389 @@
+#include "motion_tracking.h"
+
+void acquire_new_speed(float *cur_speed, float duration, float *instant_accel) {
+ cur_speed[0] = cur_speed[0] + duration * instant_accel[0];
+ cur_speed[1] = cur_speed[1] + duration * instant_accel[1];
+ cur_speed[2] = cur_speed[2] + duration * instant_accel[2];
+}
+
+void acquire_sensor_data(float *accel_data, float *gyro_data, float *calib_data,
+ int calibFlag, FXOS8700 *accel, FXAS21002 *gyro) {
+ accel->acquire_accel_data_g(accel_data);
+ gyro->acquire_gyro_data_dps(gyro_data);
+ int precision = 1000;
+
+ float temp_y = accel_data[1];
+ accel_data[1] = accel_data[0];
+ accel_data[0] = temp_y;
+
+ if (calibFlag && (calib_data != NULL)) {
+ int i;
+ for (i = 0; i < 3; i++) {
+ accel_data[i] -= calib_data[i];
+ gyro_data[i] -= calib_data[i + 3];
+
+ if (accel_data[i] < 0) {
+ accel_data[i] = floor(-1*precision*accel_data[i])/(-1 * precision);
+ } else {
+ accel_data[i] = floor(precision*accel_data[i])/precision;
+ }
+ if (gyro_data[i] < 0) {
+ gyro_data[i] = floor(-1*precision*gyro_data[i])/(-1*precision);
+ } else {
+ gyro_data[i] = floor(precision*gyro_data[i])/precision;
+ }
+ }
+ }
+}
+
+void get_caliberate_data(float *caliberate, FXOS8700 *accel, FXAS21002 *gyro) {
+ int i;
+ int j;
+ float accel_data[3];
+ float gyro_data[3];
+
+ double temp_calib[6] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f};
+
+ for (i = 0; i < CALIBTIMES; i++) {
+ acquire_sensor_data(accel_data, gyro_data, NULL, 0, accel, gyro);
+ for (j = 0; j < 3; j++) {
+ temp_calib[j] += accel_data[j];
+ temp_calib[j + 3] += gyro_data[j];
+ }
+ wait_ms(MAINWAIT);
+ }
+
+ for (i = 0; i < 6; i++) {
+ temp_calib[i] /= (double)CALIBTIMES;
+ caliberate[i] = (float)temp_calib[i];
+ }
+}
+
+void load_buffer(float **all_data, float *accel_data, float time, int i) {
+ if (i == MAXBUFFERSIZE) {
+ printf("Buffer full!\n\r");
+ exit(1);
+ }
+
+ all_data[0][i] = time;
+ all_data[1][i] = accel_data[0];
+ all_data[2][i] = accel_data[1];
+ all_data[3][i] = accel_data[2];
+}
+
+void init_moving_avg_buf(float moving_avg_buf[][MOVINGAVRBUFSIZE], int *num_samples,
+ float *last_total, float **all_data, float *caliberate,
+ Timer *t, FXOS8700 *accel, FXAS21002 *gyro) {
+ int i;
+ float accel_data[3];
+ float gyro_data[3];
+ float time;
+ float total_accelx = 0.0f;
+ float total_accely = 0.0f;
+ float total_accelz = 0.0f;
+
+ for (i = 0; i < (int)MOVINGAVRBUFSIZE; i++) {
+ acquire_sensor_data(accel_data, gyro_data, caliberate, 1, accel, gyro);
+ time = t->read();
+ moving_avg_buf[0][i] = accel_data[0];
+ moving_avg_buf[1][i] = accel_data[1];
+ moving_avg_buf[2][i] = accel_data[2];
+ moving_avg_buf[3][i] = time;
+ wait_ms(MAINWAIT);
+ }
+
+ for (i = 0; i < (int)MOVINGAVRBUFSIZE; i++) {
+ total_accelx += moving_avg_buf[0][i];
+ total_accely += moving_avg_buf[1][i];
+ total_accelz += moving_avg_buf[2][i];
+ }
+
+ last_total[0] = total_accelx;
+ last_total[1] = total_accely;
+ last_total[2] = total_accelz;
+
+ all_data[0][0] = moving_avg_buf[3][0];
+ all_data[1][0] = total_accelx/(float) MOVINGAVRBUFSIZE;
+ all_data[2][0] = total_accely/(float) MOVINGAVRBUFSIZE;
+ all_data[3][0] = total_accelz/(float) MOVINGAVRBUFSIZE;
+
+ (*num_samples)++;
+}
+
+void get_new_moving_average_point(float **all_data, float *last_total,
+ float *accel_data, float moving_avg_buf[][MOVINGAVRBUFSIZE],
+ float time, int *num_samples, int *start,
+ int *end) {
+
+ last_total[0] = last_total[0] - moving_avg_buf[0][*start] + accel_data[0];
+ last_total[1] = last_total[1] - moving_avg_buf[1][*start] + accel_data[1];
+ last_total[2] = last_total[2] - moving_avg_buf[2][*start] + accel_data[2];
+
+ all_data[0][*num_samples] = moving_avg_buf[3][*start];
+
+ float temp_x = last_total[0] / (float)MOVINGAVRBUFSIZE;
+ float temp_y = last_total[1] / (float)MOVINGAVRBUFSIZE;
+ float temp_z = last_total[2] / (float)MOVINGAVRBUFSIZE;
+
+ if (abs(temp_x) > (float) XMECHANICAL) {
+ all_data[1][*num_samples] = temp_x;
+ } else {
+ all_data[1][*num_samples] = 0.0f;
+ }
+
+ if (abs(temp_y) > (float) YMECHANICAL) {
+ all_data[2][*num_samples] = temp_y;
+ } else {
+ all_data[2][*num_samples] = 0.0f;
+ }
+
+ if (abs(temp_z) > (float) ZMECHANICAL) {
+ all_data[3][*num_samples] = temp_z;
+ } else {
+ all_data[3][*num_samples] = 0.0f;
+ }
+
+ *start = (*start + 1) % (int) MOVINGAVRBUFSIZE;
+ *end = (*end + 1) % (int) MOVINGAVRBUFSIZE;
+
+ moving_avg_buf[0][*end] = accel_data[0];
+ moving_avg_buf[1][*end] = accel_data[1];
+ moving_avg_buf[2][*end] = accel_data[2];
+ moving_avg_buf[3][*end] = time;
+
+ (*num_samples) += 1;
+}
+
+void apply_trend_protect(float **all_data, int num_samples, float *total_diff,
+ float *additional_to_vel, float duration) {
+
+ if (num_samples > TRENDPROTECTBUFSIZE) {
+ total_diff[0] -= all_data[2][num_samples-TRENDPROTECTBUFSIZE] - all_data[2][num_samples-TRENDPROTECTBUFSIZE - 1];
+ total_diff[1] -= all_data[3][num_samples-TRENDPROTECTBUFSIZE] - all_data[3][num_samples-TRENDPROTECTBUFSIZE - 1];
+
+ total_diff[0] += all_data[2][num_samples-1] - all_data[2][num_samples-2];
+ total_diff[1] += all_data[3][num_samples-1] - all_data[3][num_samples-2];
+
+ if (abs(total_diff[0]) <= (float)XTRENDPROTECTTHRESHOLD) {
+ additional_to_vel[0] = -1000.0f;
+ } else {
+ float avg_accel = all_data[2][num_samples-1] - ((all_data[2][num_samples-1] - all_data[2][num_samples-2])/2.0f);
+ additional_to_vel[0] = avg_accel * duration;
+ }
+
+ if (abs(total_diff[1]) <= (float)YTRENDPROTECTTHRESHOLD) {
+ additional_to_vel[1] = -1000.0f;
+ } else {
+ float avg_accel = all_data[3][num_samples-1] - ((all_data[3][num_samples-1] - all_data[3][num_samples-2])/2.0f);
+ additional_to_vel[1] = avg_accel * duration;
+ }
+
+ } else {
+ if(num_samples == 1){
+ } else {
+ total_diff[0] += all_data[2][num_samples-1] - all_data[2][num_samples-2];
+ total_diff[1] += all_data[3][num_samples-1] - all_data[3][num_samples-2];
+ }
+ }
+}
+
+
+
+void apply_move_end_check(float **all_data, int num_samples,
+ int moving_end_buf[][MOVINGENDBUFSIZE],
+ int *num_unqualified, int *start, int *end,
+ float *addition_to_vel, float duration,
+ float *total_diff) {
+
+ if (num_samples > MOVINGENDBUFSIZE) {
+ num_unqualified[0] -= moving_end_buf[0][*start];
+ num_unqualified[1] -= moving_end_buf[1][*start];
+ *start = (*start + 1) % MOVINGENDBUFSIZE;
+ *end = (*end + 1) % MOVINGENDBUFSIZE;
+
+ if (abs(all_data[2][num_samples-1]) <= (float) YMOVENDTHRESHOLD ){
+ num_unqualified[0] += 1;
+ moving_end_buf[0][*end] = 1;
+ } else {
+ moving_end_buf[0][*end] = 0;
+ }
+
+ if (abs(all_data[3][num_samples-1]) <= (float) ZMOVENDTHRESHOLD ){
+ num_unqualified[1] += 1;
+ moving_end_buf[1][*end] = 1;
+ } else {
+ moving_end_buf[1][*end] = 0;
+ }
+
+ if (num_unqualified[0] >= (int)YMOVENDBIASNUM){
+ addition_to_vel[0] = -1000.0f;
+ }
+
+ if (num_unqualified[1] >= (int)ZMOVENDBIASNUM){
+ addition_to_vel[1] = -1000.0f;
+ }
+
+ apply_trend_protect(all_data, num_samples, total_diff, addition_to_vel,
+ duration);
+
+ if (num_unqualified[0] < (int)YMOVENDBIASNUM){
+ float avg_accel = all_data[2][num_samples-1] - ((all_data[2][num_samples-1] - all_data[2][num_samples-2])/2.0f);
+ addition_to_vel[0] = avg_accel * duration;
+ }
+
+ if (num_unqualified[1] < (int)ZMOVENDBIASNUM){
+ float avg_accel = all_data[3][num_samples-1] - ((all_data[3][num_samples-1] - all_data[3][num_samples-2])/2.0f);
+ addition_to_vel[1] = avg_accel * duration;
+ }
+
+ } else if (num_samples < MOVINGENDBUFSIZE) {
+ addition_to_vel[0] = -1000.0f;
+ addition_to_vel[1] = -1000.0f;
+ apply_trend_protect(all_data, num_samples, total_diff, addition_to_vel,
+ duration);
+ } else {
+ int i;
+ for (i = 0; i < MOVINGENDBUFSIZE; i++) {
+ if (abs(all_data[2][i]) <= (float)YMOVENDTHRESHOLD) {
+ moving_end_buf[0][i] = 1;
+ num_unqualified[0] += 1;
+ } else {
+ moving_end_buf[0][i] = 0;
+ }
+
+ if (abs(all_data[3][i]) <= (float)ZMOVENDTHRESHOLD) {
+ moving_end_buf[1][i] = 1;
+ num_unqualified[1] += 1;
+ } else {
+ moving_end_buf[1][i] = 0;
+ }
+
+ addition_to_vel[0] = -1000.0f;
+ addition_to_vel[1] = -1000.0f;
+ }
+ apply_trend_protect(all_data, num_samples, total_diff, addition_to_vel,
+ duration);
+ }
+}
+
+void get_new_velocity (float *original_speed, float *addition_to_vel) {
+ if (addition_to_vel[0] == -1000.0f) {
+ original_speed[0] = 0.0f;
+ } else {
+ original_speed[0] += addition_to_vel[0];
+ }
+
+ if (addition_to_vel[1] == -1000.0f) {
+ original_speed[1] = 0.0f;
+ } else {
+ original_speed[1] += addition_to_vel[1];
+ }
+}
+
+void get_new_position (float *original_position, float *cur_speed, float duration) {
+ original_position[0] += cur_speed[0] * duration;
+ original_position[1] -= cur_speed[1] * duration;
+}
+
+
+void insert_new_vel_to_buffer(float** vel_buffer, float time, float* cur_vel,
+ int num_samples) {
+
+ vel_buffer[0][num_samples - 1] = time;
+ vel_buffer[1][num_samples - 1] = cur_vel[0];
+ vel_buffer[2][num_samples - 1] = cur_vel[1];
+}
+
+void insert_new_pos_to_buffer(float** pos_buffer, float time, float* cur_pos,
+ int num_samples) {
+ pos_buffer[0][num_samples - 1] = time;
+ pos_buffer[1][num_samples - 1] = cur_pos[0];
+ pos_buffer[2][num_samples - 1] = cur_pos[1];
+}
+
+void insert_new_color_to_buffer(float **color_buffer, int r, int g, int b,
+ float time, int num_samples) {
+ color_buffer[0][num_samples - 1] = time;
+ color_buffer[1][num_samples - 1] = (float) r;
+ color_buffer[2][num_samples - 1] = (float) g;
+ color_buffer[3][num_samples - 1] = (float) b;
+}
+
+void initialize_color_table(RGB rgb_table[][HORIZONTALNUMCOLOR]){
+ rgb_table[0][0] = (RGB){255, 77, 210};
+ rgb_table[0][1] = (RGB){255, 77, 77};
+ rgb_table[0][2] = (RGB){255, 179, 102};
+ rgb_table[0][3] = (RGB){133, 224, 133};
+ rgb_table[0][4] = (RGB){77, 121, 255};
+
+ rgb_table[1][0] = (RGB){255, 0, 191};
+ rgb_table[1][1] = (RGB){255, 0, 0};
+ rgb_table[1][2] = (RGB){255, 128, 0};
+ rgb_table[1][3] = (RGB){51, 204, 51};
+ rgb_table[1][4] = (RGB){0, 64, 255};
+
+ rgb_table[2][0] = (RGB){179, 0, 134};
+ rgb_table[2][1] = (RGB){179, 0, 0};
+ rgb_table[2][2] = (RGB){204, 102, 0};
+ rgb_table[2][3] = (RGB){31, 122, 31};
+ rgb_table[2][4] = (RGB){0, 45, 179};
+}
+
+RGB get_new_color(float *cur_location, RGB rgb_table[][HORIZONTALNUMCOLOR]) {
+ int x;
+ int y;
+
+ if (cur_location[0] < -0.03f) {
+ x = 0;
+ } else if (cur_location[0] >= -0.03f && cur_location[0] < -0.01f) {
+ x = 1;
+ } else if (cur_location[0] >= -0.01f && cur_location[0] < 0.01f) {
+ x = 2;
+ } else if (cur_location[0] >= 0.01f && cur_location[0] < 0.03f) {
+ x = 3;
+ } else {
+ x = 4;
+ }
+
+ if (cur_location[1] > 0.0165f) {
+ y = 0;
+ } else if (cur_location[1] > -0.0165f && cur_location[1] <= 0.0165f) {
+ y = 1;
+ } else {
+ y = 2;
+ }
+
+ return rgb_table[y][x];
+}
+
+void output_all_to_serial(float **all_data, int num_samples) {
+ int i;
+ for (i = 0; i < num_samples; i++) {
+ printf("%6.3f,%7.5f,%7.5f,%7.5f\n",all_data[0][i],all_data[1][i],all_data[2][i],all_data[3][i]);
+ wait(0.01);
+ }
+ printf("Number of samples = %d\n", num_samples);
+ printf ("End Transmission!\n");
+}
+
+
+void output_color_to_serial(float **color_data, int num_samples) {
+ int i;
+ for (i = 0; i < num_samples; i++) {
+ printf("%6.3f,%d,%d,%d\n", color_data[0][i], (int)color_data[1][i],
+ (int)color_data[2][i],(int)color_data[3][i]);
+ wait(0.01);
+ }
+ printf("Number of samples = %d\n", num_samples);
+ printf ("End Transmission!\n");
+}
+
+void output_to_serial(float **vel_data, int num_samples) {
+ int i;
+ for (i = 0; i < num_samples; i++) {
+ printf("%6.3f,%7.5f,%7.5f\n", vel_data[0][i], vel_data[1][i],vel_data[2][i]);
+ wait(0.01);
+ }
+ printf("Number of samples = %d\n", num_samples);
+ printf ("End Transmission!\n");
+}
\ No newline at end of file