Weiqian Xu / motion_tracking2

for led strip

motion_tracking.cpp@0:da91c8ed4a98, 2018-06-11 (annotated)

Committer:
xuweiqian9999
Date:
Mon Jun 11 20:33:26 2018 +0000
Revision:
0:da91c8ed4a98
Child:
1:b90027de2bdb
added to a new proj

Who changed what in which revision?

UserRevisionLine numberNew contents of line
xuweiqian9999 0:da91c8ed4a98 1 #include "motion_tracking.h"
xuweiqian9999 0:da91c8ed4a98 2
xuweiqian9999 0:da91c8ed4a98 3 void acquire_new_speed(float *cur_speed, float duration, float *instant_accel) {
xuweiqian9999 0:da91c8ed4a98 4 cur_speed[0] = cur_speed[0] + duration * instant_accel[0];
xuweiqian9999 0:da91c8ed4a98 5 cur_speed[1] = cur_speed[1] + duration * instant_accel[1];
xuweiqian9999 0:da91c8ed4a98 6 cur_speed[2] = cur_speed[2] + duration * instant_accel[2];
xuweiqian9999 0:da91c8ed4a98 7 }
xuweiqian9999 0:da91c8ed4a98 8
xuweiqian9999 0:da91c8ed4a98 9 void acquire_sensor_data(float *accel_data, float *gyro_data, float *calib_data,
xuweiqian9999 0:da91c8ed4a98 10 int calibFlag, FXOS8700 *accel, FXAS21002 *gyro) {
xuweiqian9999 0:da91c8ed4a98 11 accel->acquire_accel_data_g(accel_data);
xuweiqian9999 0:da91c8ed4a98 12 gyro->acquire_gyro_data_dps(gyro_data);
xuweiqian9999 0:da91c8ed4a98 13 int precision = 1000;
xuweiqian9999 0:da91c8ed4a98 14
xuweiqian9999 0:da91c8ed4a98 15 float temp_y = accel_data[1];
xuweiqian9999 0:da91c8ed4a98 16 accel_data[1] = accel_data[0];
xuweiqian9999 0:da91c8ed4a98 17 accel_data[0] = temp_y;
xuweiqian9999 0:da91c8ed4a98 18
xuweiqian9999 0:da91c8ed4a98 19 if (calibFlag && (calib_data != NULL)) {
xuweiqian9999 0:da91c8ed4a98 20 int i;
xuweiqian9999 0:da91c8ed4a98 21 for (i = 0; i < 3; i++) {
xuweiqian9999 0:da91c8ed4a98 22 accel_data[i] -= calib_data[i];
xuweiqian9999 0:da91c8ed4a98 23 gyro_data[i] -= calib_data[i + 3];
xuweiqian9999 0:da91c8ed4a98 24
xuweiqian9999 0:da91c8ed4a98 25 if (accel_data[i] < 0) {
xuweiqian9999 0:da91c8ed4a98 26 accel_data[i] = floor(-1*precision*accel_data[i])/(-1 * precision);
xuweiqian9999 0:da91c8ed4a98 27 } else {
xuweiqian9999 0:da91c8ed4a98 28 accel_data[i] = floor(precision*accel_data[i])/precision;
xuweiqian9999 0:da91c8ed4a98 29 }
xuweiqian9999 0:da91c8ed4a98 30 if (gyro_data[i] < 0) {
xuweiqian9999 0:da91c8ed4a98 31 gyro_data[i] = floor(-1*precision*gyro_data[i])/(-1*precision);
xuweiqian9999 0:da91c8ed4a98 32 } else {
xuweiqian9999 0:da91c8ed4a98 33 gyro_data[i] = floor(precision*gyro_data[i])/precision;
xuweiqian9999 0:da91c8ed4a98 34 }
xuweiqian9999 0:da91c8ed4a98 35 }
xuweiqian9999 0:da91c8ed4a98 36 }
xuweiqian9999 0:da91c8ed4a98 37 }
xuweiqian9999 0:da91c8ed4a98 38
xuweiqian9999 0:da91c8ed4a98 39 void get_caliberate_data(float *caliberate, FXOS8700 *accel, FXAS21002 *gyro) {
xuweiqian9999 0:da91c8ed4a98 40 int i;
xuweiqian9999 0:da91c8ed4a98 41 int j;
xuweiqian9999 0:da91c8ed4a98 42 float accel_data[3];
xuweiqian9999 0:da91c8ed4a98 43 float gyro_data[3];
xuweiqian9999 0:da91c8ed4a98 44
xuweiqian9999 0:da91c8ed4a98 45 double temp_calib[6] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f};
xuweiqian9999 0:da91c8ed4a98 46
xuweiqian9999 0:da91c8ed4a98 47 for (i = 0; i < CALIBTIMES; i++) {
xuweiqian9999 0:da91c8ed4a98 48 acquire_sensor_data(accel_data, gyro_data, NULL, 0, accel, gyro);
xuweiqian9999 0:da91c8ed4a98 49 for (j = 0; j < 3; j++) {
xuweiqian9999 0:da91c8ed4a98 50 temp_calib[j] += accel_data[j];
xuweiqian9999 0:da91c8ed4a98 51 temp_calib[j + 3] += gyro_data[j];
xuweiqian9999 0:da91c8ed4a98 52 }
xuweiqian9999 0:da91c8ed4a98 53 wait_ms(MAINWAIT);
xuweiqian9999 0:da91c8ed4a98 54 }
xuweiqian9999 0:da91c8ed4a98 55
xuweiqian9999 0:da91c8ed4a98 56 for (i = 0; i < 6; i++) {
xuweiqian9999 0:da91c8ed4a98 57 temp_calib[i] /= (double)CALIBTIMES;
xuweiqian9999 0:da91c8ed4a98 58 caliberate[i] = (float)temp_calib[i];
xuweiqian9999 0:da91c8ed4a98 59 }
xuweiqian9999 0:da91c8ed4a98 60 }
xuweiqian9999 0:da91c8ed4a98 61
xuweiqian9999 0:da91c8ed4a98 62 void load_buffer(float **all_data, float *accel_data, float time, int i) {
xuweiqian9999 0:da91c8ed4a98 63 if (i == MAXBUFFERSIZE) {
xuweiqian9999 0:da91c8ed4a98 64 printf("Buffer full!\n\r");
xuweiqian9999 0:da91c8ed4a98 65 exit(1);
xuweiqian9999 0:da91c8ed4a98 66 }
xuweiqian9999 0:da91c8ed4a98 67
xuweiqian9999 0:da91c8ed4a98 68 all_data[0][i] = time;
xuweiqian9999 0:da91c8ed4a98 69 all_data[1][i] = accel_data[0];
xuweiqian9999 0:da91c8ed4a98 70 all_data[2][i] = accel_data[1];
xuweiqian9999 0:da91c8ed4a98 71 all_data[3][i] = accel_data[2];
xuweiqian9999 0:da91c8ed4a98 72 }
xuweiqian9999 0:da91c8ed4a98 73
xuweiqian9999 0:da91c8ed4a98 74 void init_moving_avg_buf(float moving_avg_buf[][MOVINGAVRBUFSIZE], int *num_samples,
xuweiqian9999 0:da91c8ed4a98 75 float *last_total, float **all_data, float *caliberate,
xuweiqian9999 0:da91c8ed4a98 76 Timer *t, FXOS8700 *accel, FXAS21002 *gyro) {
xuweiqian9999 0:da91c8ed4a98 77 int i;
xuweiqian9999 0:da91c8ed4a98 78 float accel_data[3];
xuweiqian9999 0:da91c8ed4a98 79 float gyro_data[3];
xuweiqian9999 0:da91c8ed4a98 80 float time;
xuweiqian9999 0:da91c8ed4a98 81 float total_accelx = 0.0f;
xuweiqian9999 0:da91c8ed4a98 82 float total_accely = 0.0f;
xuweiqian9999 0:da91c8ed4a98 83 float total_accelz = 0.0f;
xuweiqian9999 0:da91c8ed4a98 84
xuweiqian9999 0:da91c8ed4a98 85 for (i = 0; i < (int)MOVINGAVRBUFSIZE; i++) {
xuweiqian9999 0:da91c8ed4a98 86 acquire_sensor_data(accel_data, gyro_data, caliberate, 1, accel, gyro);
xuweiqian9999 0:da91c8ed4a98 87 time = t->read();
xuweiqian9999 0:da91c8ed4a98 88 moving_avg_buf[0][i] = accel_data[0];
xuweiqian9999 0:da91c8ed4a98 89 moving_avg_buf[1][i] = accel_data[1];
xuweiqian9999 0:da91c8ed4a98 90 moving_avg_buf[2][i] = accel_data[2];
xuweiqian9999 0:da91c8ed4a98 91 moving_avg_buf[3][i] = time;
xuweiqian9999 0:da91c8ed4a98 92 wait_ms(MAINWAIT);
xuweiqian9999 0:da91c8ed4a98 93 }
xuweiqian9999 0:da91c8ed4a98 94
xuweiqian9999 0:da91c8ed4a98 95 for (i = 0; i < (int)MOVINGAVRBUFSIZE; i++) {
xuweiqian9999 0:da91c8ed4a98 96 total_accelx += moving_avg_buf[0][i];
xuweiqian9999 0:da91c8ed4a98 97 total_accely += moving_avg_buf[1][i];
xuweiqian9999 0:da91c8ed4a98 98 total_accelz += moving_avg_buf[2][i];
xuweiqian9999 0:da91c8ed4a98 99 }
xuweiqian9999 0:da91c8ed4a98 100
xuweiqian9999 0:da91c8ed4a98 101 last_total[0] = total_accelx;
xuweiqian9999 0:da91c8ed4a98 102 last_total[1] = total_accely;
xuweiqian9999 0:da91c8ed4a98 103 last_total[2] = total_accelz;
xuweiqian9999 0:da91c8ed4a98 104
xuweiqian9999 0:da91c8ed4a98 105 all_data[0][0] = moving_avg_buf[3][0];
xuweiqian9999 0:da91c8ed4a98 106 all_data[1][0] = total_accelx/(float) MOVINGAVRBUFSIZE;
xuweiqian9999 0:da91c8ed4a98 107 all_data[2][0] = total_accely/(float) MOVINGAVRBUFSIZE;
xuweiqian9999 0:da91c8ed4a98 108 all_data[3][0] = total_accelz/(float) MOVINGAVRBUFSIZE;
xuweiqian9999 0:da91c8ed4a98 109
xuweiqian9999 0:da91c8ed4a98 110 (*num_samples)++;
xuweiqian9999 0:da91c8ed4a98 111 }
xuweiqian9999 0:da91c8ed4a98 112
xuweiqian9999 0:da91c8ed4a98 113 void get_new_moving_average_point(float **all_data, float *last_total,
xuweiqian9999 0:da91c8ed4a98 114 float *accel_data, float moving_avg_buf[][MOVINGAVRBUFSIZE],
xuweiqian9999 0:da91c8ed4a98 115 float time, int *num_samples, int *start,
xuweiqian9999 0:da91c8ed4a98 116 int *end) {
xuweiqian9999 0:da91c8ed4a98 117
xuweiqian9999 0:da91c8ed4a98 118 last_total[0] = last_total[0] - moving_avg_buf[0][*start] + accel_data[0];
xuweiqian9999 0:da91c8ed4a98 119 last_total[1] = last_total[1] - moving_avg_buf[1][*start] + accel_data[1];
xuweiqian9999 0:da91c8ed4a98 120 last_total[2] = last_total[2] - moving_avg_buf[2][*start] + accel_data[2];
xuweiqian9999 0:da91c8ed4a98 121
xuweiqian9999 0:da91c8ed4a98 122 all_data[0][*num_samples] = moving_avg_buf[3][*start];
xuweiqian9999 0:da91c8ed4a98 123
xuweiqian9999 0:da91c8ed4a98 124 float temp_x = last_total[0] / (float)MOVINGAVRBUFSIZE;
xuweiqian9999 0:da91c8ed4a98 125 float temp_y = last_total[1] / (float)MOVINGAVRBUFSIZE;
xuweiqian9999 0:da91c8ed4a98 126 float temp_z = last_total[2] / (float)MOVINGAVRBUFSIZE;
xuweiqian9999 0:da91c8ed4a98 127
xuweiqian9999 0:da91c8ed4a98 128 if (abs(temp_x) > (float) XMECHANICAL) {
xuweiqian9999 0:da91c8ed4a98 129 all_data[1][*num_samples] = temp_x;
xuweiqian9999 0:da91c8ed4a98 130 } else {
xuweiqian9999 0:da91c8ed4a98 131 all_data[1][*num_samples] = 0.0f;
xuweiqian9999 0:da91c8ed4a98 132 }
xuweiqian9999 0:da91c8ed4a98 133
xuweiqian9999 0:da91c8ed4a98 134 if (abs(temp_y) > (float) YMECHANICAL) {
xuweiqian9999 0:da91c8ed4a98 135 all_data[2][*num_samples] = temp_y;
xuweiqian9999 0:da91c8ed4a98 136 } else {
xuweiqian9999 0:da91c8ed4a98 137 all_data[2][*num_samples] = 0.0f;
xuweiqian9999 0:da91c8ed4a98 138 }
xuweiqian9999 0:da91c8ed4a98 139
xuweiqian9999 0:da91c8ed4a98 140 if (abs(temp_z) > (float) ZMECHANICAL) {
xuweiqian9999 0:da91c8ed4a98 141 all_data[3][*num_samples] = temp_z;
xuweiqian9999 0:da91c8ed4a98 142 } else {
xuweiqian9999 0:da91c8ed4a98 143 all_data[3][*num_samples] = 0.0f;
xuweiqian9999 0:da91c8ed4a98 144 }
xuweiqian9999 0:da91c8ed4a98 145
xuweiqian9999 0:da91c8ed4a98 146 *start = (*start + 1) % (int) MOVINGAVRBUFSIZE;
xuweiqian9999 0:da91c8ed4a98 147 *end = (*end + 1) % (int) MOVINGAVRBUFSIZE;
xuweiqian9999 0:da91c8ed4a98 148
xuweiqian9999 0:da91c8ed4a98 149 moving_avg_buf[0][*end] = accel_data[0];
xuweiqian9999 0:da91c8ed4a98 150 moving_avg_buf[1][*end] = accel_data[1];
xuweiqian9999 0:da91c8ed4a98 151 moving_avg_buf[2][*end] = accel_data[2];
xuweiqian9999 0:da91c8ed4a98 152 moving_avg_buf[3][*end] = time;
xuweiqian9999 0:da91c8ed4a98 153
xuweiqian9999 0:da91c8ed4a98 154 (*num_samples) += 1;
xuweiqian9999 0:da91c8ed4a98 155 }
xuweiqian9999 0:da91c8ed4a98 156
xuweiqian9999 0:da91c8ed4a98 157 void apply_trend_protect(float **all_data, int num_samples, float *total_diff,
xuweiqian9999 0:da91c8ed4a98 158 float *additional_to_vel, float duration) {
xuweiqian9999 0:da91c8ed4a98 159
xuweiqian9999 0:da91c8ed4a98 160 if (num_samples > TRENDPROTECTBUFSIZE) {
xuweiqian9999 0:da91c8ed4a98 161 total_diff[0] -= all_data[2][num_samples-TRENDPROTECTBUFSIZE] - all_data[2][num_samples-TRENDPROTECTBUFSIZE - 1];
xuweiqian9999 0:da91c8ed4a98 162 total_diff[1] -= all_data[3][num_samples-TRENDPROTECTBUFSIZE] - all_data[3][num_samples-TRENDPROTECTBUFSIZE - 1];
xuweiqian9999 0:da91c8ed4a98 163
xuweiqian9999 0:da91c8ed4a98 164 total_diff[0] += all_data[2][num_samples-1] - all_data[2][num_samples-2];
xuweiqian9999 0:da91c8ed4a98 165 total_diff[1] += all_data[3][num_samples-1] - all_data[3][num_samples-2];
xuweiqian9999 0:da91c8ed4a98 166
xuweiqian9999 0:da91c8ed4a98 167 if (abs(total_diff[0]) <= (float)XTRENDPROTECTTHRESHOLD) {
xuweiqian9999 0:da91c8ed4a98 168 additional_to_vel[0] = -1000.0f;
xuweiqian9999 0:da91c8ed4a98 169 } else {
xuweiqian9999 0:da91c8ed4a98 170 float avg_accel = all_data[2][num_samples-1] - ((all_data[2][num_samples-1] - all_data[2][num_samples-2])/2.0f);
xuweiqian9999 0:da91c8ed4a98 171 additional_to_vel[0] = avg_accel * duration;
xuweiqian9999 0:da91c8ed4a98 172 }
xuweiqian9999 0:da91c8ed4a98 173
xuweiqian9999 0:da91c8ed4a98 174 if (abs(total_diff[1]) <= (float)YTRENDPROTECTTHRESHOLD) {
xuweiqian9999 0:da91c8ed4a98 175 additional_to_vel[1] = -1000.0f;
xuweiqian9999 0:da91c8ed4a98 176 } else {
xuweiqian9999 0:da91c8ed4a98 177 float avg_accel = all_data[3][num_samples-1] - ((all_data[3][num_samples-1] - all_data[3][num_samples-2])/2.0f);
xuweiqian9999 0:da91c8ed4a98 178 additional_to_vel[1] = avg_accel * duration;
xuweiqian9999 0:da91c8ed4a98 179 }
xuweiqian9999 0:da91c8ed4a98 180
xuweiqian9999 0:da91c8ed4a98 181 } else {
xuweiqian9999 0:da91c8ed4a98 182 if(num_samples == 1){
xuweiqian9999 0:da91c8ed4a98 183 } else {
xuweiqian9999 0:da91c8ed4a98 184 total_diff[0] += all_data[2][num_samples-1] - all_data[2][num_samples-2];
xuweiqian9999 0:da91c8ed4a98 185 total_diff[1] += all_data[3][num_samples-1] - all_data[3][num_samples-2];
xuweiqian9999 0:da91c8ed4a98 186 }
xuweiqian9999 0:da91c8ed4a98 187 }
xuweiqian9999 0:da91c8ed4a98 188 }
xuweiqian9999 0:da91c8ed4a98 189
xuweiqian9999 0:da91c8ed4a98 190
xuweiqian9999 0:da91c8ed4a98 191
xuweiqian9999 0:da91c8ed4a98 192 void apply_move_end_check(float **all_data, int num_samples,
xuweiqian9999 0:da91c8ed4a98 193 int moving_end_buf[][MOVINGENDBUFSIZE],
xuweiqian9999 0:da91c8ed4a98 194 int *num_unqualified, int *start, int *end,
xuweiqian9999 0:da91c8ed4a98 195 float *addition_to_vel, float duration,
xuweiqian9999 0:da91c8ed4a98 196 float *total_diff) {
xuweiqian9999 0:da91c8ed4a98 197
xuweiqian9999 0:da91c8ed4a98 198 if (num_samples > MOVINGENDBUFSIZE) {
xuweiqian9999 0:da91c8ed4a98 199 num_unqualified[0] -= moving_end_buf[0][*start];
xuweiqian9999 0:da91c8ed4a98 200 num_unqualified[1] -= moving_end_buf[1][*start];
xuweiqian9999 0:da91c8ed4a98 201 *start = (*start + 1) % MOVINGENDBUFSIZE;
xuweiqian9999 0:da91c8ed4a98 202 *end = (*end + 1) % MOVINGENDBUFSIZE;
xuweiqian9999 0:da91c8ed4a98 203
xuweiqian9999 0:da91c8ed4a98 204 if (abs(all_data[2][num_samples-1]) <= (float) YMOVENDTHRESHOLD ){
xuweiqian9999 0:da91c8ed4a98 205 num_unqualified[0] += 1;
xuweiqian9999 0:da91c8ed4a98 206 moving_end_buf[0][*end] = 1;
xuweiqian9999 0:da91c8ed4a98 207 } else {
xuweiqian9999 0:da91c8ed4a98 208 moving_end_buf[0][*end] = 0;
xuweiqian9999 0:da91c8ed4a98 209 }
xuweiqian9999 0:da91c8ed4a98 210
xuweiqian9999 0:da91c8ed4a98 211 if (abs(all_data[3][num_samples-1]) <= (float) ZMOVENDTHRESHOLD ){
xuweiqian9999 0:da91c8ed4a98 212 num_unqualified[1] += 1;
xuweiqian9999 0:da91c8ed4a98 213 moving_end_buf[1][*end] = 1;
xuweiqian9999 0:da91c8ed4a98 214 } else {
xuweiqian9999 0:da91c8ed4a98 215 moving_end_buf[1][*end] = 0;
xuweiqian9999 0:da91c8ed4a98 216 }
xuweiqian9999 0:da91c8ed4a98 217
xuweiqian9999 0:da91c8ed4a98 218 if (num_unqualified[0] >= (int)YMOVENDBIASNUM){
xuweiqian9999 0:da91c8ed4a98 219 addition_to_vel[0] = -1000.0f;
xuweiqian9999 0:da91c8ed4a98 220 }
xuweiqian9999 0:da91c8ed4a98 221
xuweiqian9999 0:da91c8ed4a98 222 if (num_unqualified[1] >= (int)ZMOVENDBIASNUM){
xuweiqian9999 0:da91c8ed4a98 223 addition_to_vel[1] = -1000.0f;
xuweiqian9999 0:da91c8ed4a98 224 }
xuweiqian9999 0:da91c8ed4a98 225
xuweiqian9999 0:da91c8ed4a98 226 apply_trend_protect(all_data, num_samples, total_diff, addition_to_vel,
xuweiqian9999 0:da91c8ed4a98 227 duration);
xuweiqian9999 0:da91c8ed4a98 228
xuweiqian9999 0:da91c8ed4a98 229 if (num_unqualified[0] < (int)YMOVENDBIASNUM){
xuweiqian9999 0:da91c8ed4a98 230 float avg_accel = all_data[2][num_samples-1] - ((all_data[2][num_samples-1] - all_data[2][num_samples-2])/2.0f);
xuweiqian9999 0:da91c8ed4a98 231 addition_to_vel[0] = avg_accel * duration;
xuweiqian9999 0:da91c8ed4a98 232 }
xuweiqian9999 0:da91c8ed4a98 233
xuweiqian9999 0:da91c8ed4a98 234 if (num_unqualified[1] < (int)ZMOVENDBIASNUM){
xuweiqian9999 0:da91c8ed4a98 235 float avg_accel = all_data[3][num_samples-1] - ((all_data[3][num_samples-1] - all_data[3][num_samples-2])/2.0f);
xuweiqian9999 0:da91c8ed4a98 236 addition_to_vel[1] = avg_accel * duration;
xuweiqian9999 0:da91c8ed4a98 237 }
xuweiqian9999 0:da91c8ed4a98 238
xuweiqian9999 0:da91c8ed4a98 239 } else if (num_samples < MOVINGENDBUFSIZE) {
xuweiqian9999 0:da91c8ed4a98 240 addition_to_vel[0] = -1000.0f;
xuweiqian9999 0:da91c8ed4a98 241 addition_to_vel[1] = -1000.0f;
xuweiqian9999 0:da91c8ed4a98 242 apply_trend_protect(all_data, num_samples, total_diff, addition_to_vel,
xuweiqian9999 0:da91c8ed4a98 243 duration);
xuweiqian9999 0:da91c8ed4a98 244 } else {
xuweiqian9999 0:da91c8ed4a98 245 int i;
xuweiqian9999 0:da91c8ed4a98 246 for (i = 0; i < MOVINGENDBUFSIZE; i++) {
xuweiqian9999 0:da91c8ed4a98 247 if (abs(all_data[2][i]) <= (float)YMOVENDTHRESHOLD) {
xuweiqian9999 0:da91c8ed4a98 248 moving_end_buf[0][i] = 1;
xuweiqian9999 0:da91c8ed4a98 249 num_unqualified[0] += 1;
xuweiqian9999 0:da91c8ed4a98 250 } else {
xuweiqian9999 0:da91c8ed4a98 251 moving_end_buf[0][i] = 0;
xuweiqian9999 0:da91c8ed4a98 252 }
xuweiqian9999 0:da91c8ed4a98 253
xuweiqian9999 0:da91c8ed4a98 254 if (abs(all_data[3][i]) <= (float)ZMOVENDTHRESHOLD) {
xuweiqian9999 0:da91c8ed4a98 255 moving_end_buf[1][i] = 1;
xuweiqian9999 0:da91c8ed4a98 256 num_unqualified[1] += 1;
xuweiqian9999 0:da91c8ed4a98 257 } else {
xuweiqian9999 0:da91c8ed4a98 258 moving_end_buf[1][i] = 0;
xuweiqian9999 0:da91c8ed4a98 259 }
xuweiqian9999 0:da91c8ed4a98 260
xuweiqian9999 0:da91c8ed4a98 261 addition_to_vel[0] = -1000.0f;
xuweiqian9999 0:da91c8ed4a98 262 addition_to_vel[1] = -1000.0f;
xuweiqian9999 0:da91c8ed4a98 263 }
xuweiqian9999 0:da91c8ed4a98 264 apply_trend_protect(all_data, num_samples, total_diff, addition_to_vel,
xuweiqian9999 0:da91c8ed4a98 265 duration);
xuweiqian9999 0:da91c8ed4a98 266 }
xuweiqian9999 0:da91c8ed4a98 267 }
xuweiqian9999 0:da91c8ed4a98 268
xuweiqian9999 0:da91c8ed4a98 269 void get_new_velocity (float *original_speed, float *addition_to_vel) {
xuweiqian9999 0:da91c8ed4a98 270 if (addition_to_vel[0] == -1000.0f) {
xuweiqian9999 0:da91c8ed4a98 271 original_speed[0] = 0.0f;
xuweiqian9999 0:da91c8ed4a98 272 } else {
xuweiqian9999 0:da91c8ed4a98 273 original_speed[0] += addition_to_vel[0];
xuweiqian9999 0:da91c8ed4a98 274 }
xuweiqian9999 0:da91c8ed4a98 275
xuweiqian9999 0:da91c8ed4a98 276 if (addition_to_vel[1] == -1000.0f) {
xuweiqian9999 0:da91c8ed4a98 277 original_speed[1] = 0.0f;
xuweiqian9999 0:da91c8ed4a98 278 } else {
xuweiqian9999 0:da91c8ed4a98 279 original_speed[1] += addition_to_vel[1];
xuweiqian9999 0:da91c8ed4a98 280 }
xuweiqian9999 0:da91c8ed4a98 281 }
xuweiqian9999 0:da91c8ed4a98 282
xuweiqian9999 0:da91c8ed4a98 283 void get_new_position (float *original_position, float *cur_speed, float duration) {
xuweiqian9999 0:da91c8ed4a98 284 original_position[0] += cur_speed[0] * duration;
xuweiqian9999 0:da91c8ed4a98 285 original_position[1] -= cur_speed[1] * duration;
xuweiqian9999 0:da91c8ed4a98 286 }
xuweiqian9999 0:da91c8ed4a98 287
xuweiqian9999 0:da91c8ed4a98 288
xuweiqian9999 0:da91c8ed4a98 289 void insert_new_vel_to_buffer(float** vel_buffer, float time, float* cur_vel,
xuweiqian9999 0:da91c8ed4a98 290 int num_samples) {
xuweiqian9999 0:da91c8ed4a98 291
xuweiqian9999 0:da91c8ed4a98 292 vel_buffer[0][num_samples - 1] = time;
xuweiqian9999 0:da91c8ed4a98 293 vel_buffer[1][num_samples - 1] = cur_vel[0];
xuweiqian9999 0:da91c8ed4a98 294 vel_buffer[2][num_samples - 1] = cur_vel[1];
xuweiqian9999 0:da91c8ed4a98 295 }
xuweiqian9999 0:da91c8ed4a98 296
xuweiqian9999 0:da91c8ed4a98 297 void insert_new_pos_to_buffer(float** pos_buffer, float time, float* cur_pos,
xuweiqian9999 0:da91c8ed4a98 298 int num_samples) {
xuweiqian9999 0:da91c8ed4a98 299 pos_buffer[0][num_samples - 1] = time;
xuweiqian9999 0:da91c8ed4a98 300 pos_buffer[1][num_samples - 1] = cur_pos[0];
xuweiqian9999 0:da91c8ed4a98 301 pos_buffer[2][num_samples - 1] = cur_pos[1];
xuweiqian9999 0:da91c8ed4a98 302 }
xuweiqian9999 0:da91c8ed4a98 303
xuweiqian9999 0:da91c8ed4a98 304 void insert_new_color_to_buffer(float **color_buffer, int r, int g, int b,
xuweiqian9999 0:da91c8ed4a98 305 float time, int num_samples) {
xuweiqian9999 0:da91c8ed4a98 306 color_buffer[0][num_samples - 1] = time;
xuweiqian9999 0:da91c8ed4a98 307 color_buffer[1][num_samples - 1] = (float) r;
xuweiqian9999 0:da91c8ed4a98 308 color_buffer[2][num_samples - 1] = (float) g;
xuweiqian9999 0:da91c8ed4a98 309 color_buffer[3][num_samples - 1] = (float) b;
xuweiqian9999 0:da91c8ed4a98 310 }
xuweiqian9999 0:da91c8ed4a98 311
xuweiqian9999 0:da91c8ed4a98 312 void initialize_color_table(RGB rgb_table[][HORIZONTALNUMCOLOR]){
xuweiqian9999 0:da91c8ed4a98 313 rgb_table[0][0] = (RGB){255, 77, 210};
xuweiqian9999 0:da91c8ed4a98 314 rgb_table[0][1] = (RGB){255, 77, 77};
xuweiqian9999 0:da91c8ed4a98 315 rgb_table[0][2] = (RGB){255, 179, 102};
xuweiqian9999 0:da91c8ed4a98 316 rgb_table[0][3] = (RGB){133, 224, 133};
xuweiqian9999 0:da91c8ed4a98 317 rgb_table[0][4] = (RGB){77, 121, 255};
xuweiqian9999 0:da91c8ed4a98 318
xuweiqian9999 0:da91c8ed4a98 319 rgb_table[1][0] = (RGB){255, 0, 191};
xuweiqian9999 0:da91c8ed4a98 320 rgb_table[1][1] = (RGB){255, 0, 0};
xuweiqian9999 0:da91c8ed4a98 321 rgb_table[1][2] = (RGB){255, 128, 0};
xuweiqian9999 0:da91c8ed4a98 322 rgb_table[1][3] = (RGB){51, 204, 51};
xuweiqian9999 0:da91c8ed4a98 323 rgb_table[1][4] = (RGB){0, 64, 255};
xuweiqian9999 0:da91c8ed4a98 324
xuweiqian9999 0:da91c8ed4a98 325 rgb_table[2][0] = (RGB){179, 0, 134};
xuweiqian9999 0:da91c8ed4a98 326 rgb_table[2][1] = (RGB){179, 0, 0};
xuweiqian9999 0:da91c8ed4a98 327 rgb_table[2][2] = (RGB){204, 102, 0};
xuweiqian9999 0:da91c8ed4a98 328 rgb_table[2][3] = (RGB){31, 122, 31};
xuweiqian9999 0:da91c8ed4a98 329 rgb_table[2][4] = (RGB){0, 45, 179};
xuweiqian9999 0:da91c8ed4a98 330 }
xuweiqian9999 0:da91c8ed4a98 331
xuweiqian9999 0:da91c8ed4a98 332 RGB get_new_color(float *cur_location, RGB rgb_table[][HORIZONTALNUMCOLOR]) {
xuweiqian9999 0:da91c8ed4a98 333 int x;
xuweiqian9999 0:da91c8ed4a98 334 int y;
xuweiqian9999 0:da91c8ed4a98 335
xuweiqian9999 0:da91c8ed4a98 336 if (cur_location[0] < -0.03f) {
xuweiqian9999 0:da91c8ed4a98 337 x = 0;
xuweiqian9999 0:da91c8ed4a98 338 } else if (cur_location[0] >= -0.03f && cur_location[0] < -0.01f) {
xuweiqian9999 0:da91c8ed4a98 339 x = 1;
xuweiqian9999 0:da91c8ed4a98 340 } else if (cur_location[0] >= -0.01f && cur_location[0] < 0.01f) {
xuweiqian9999 0:da91c8ed4a98 341 x = 2;
xuweiqian9999 0:da91c8ed4a98 342 } else if (cur_location[0] >= 0.01f && cur_location[0] < 0.03f) {
xuweiqian9999 0:da91c8ed4a98 343 x = 3;
xuweiqian9999 0:da91c8ed4a98 344 } else {
xuweiqian9999 0:da91c8ed4a98 345 x = 4;
xuweiqian9999 0:da91c8ed4a98 346 }
xuweiqian9999 0:da91c8ed4a98 347
xuweiqian9999 0:da91c8ed4a98 348 if (cur_location[1] > 0.0165f) {
xuweiqian9999 0:da91c8ed4a98 349 y = 0;
xuweiqian9999 0:da91c8ed4a98 350 } else if (cur_location[1] > -0.0165f && cur_location[1] <= 0.0165f) {
xuweiqian9999 0:da91c8ed4a98 351 y = 1;
xuweiqian9999 0:da91c8ed4a98 352 } else {
xuweiqian9999 0:da91c8ed4a98 353 y = 2;
xuweiqian9999 0:da91c8ed4a98 354 }
xuweiqian9999 0:da91c8ed4a98 355
xuweiqian9999 0:da91c8ed4a98 356 return rgb_table[y][x];
xuweiqian9999 0:da91c8ed4a98 357 }
xuweiqian9999 0:da91c8ed4a98 358
xuweiqian9999 0:da91c8ed4a98 359 void output_all_to_serial(float **all_data, int num_samples) {
xuweiqian9999 0:da91c8ed4a98 360 int i;
xuweiqian9999 0:da91c8ed4a98 361 for (i = 0; i < num_samples; i++) {
xuweiqian9999 0:da91c8ed4a98 362 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]);
xuweiqian9999 0:da91c8ed4a98 363 wait(0.01);
xuweiqian9999 0:da91c8ed4a98 364 }
xuweiqian9999 0:da91c8ed4a98 365 printf("Number of samples = %d\n", num_samples);
xuweiqian9999 0:da91c8ed4a98 366 printf ("End Transmission!\n");
xuweiqian9999 0:da91c8ed4a98 367 }
xuweiqian9999 0:da91c8ed4a98 368
xuweiqian9999 0:da91c8ed4a98 369
xuweiqian9999 0:da91c8ed4a98 370 void output_color_to_serial(float **color_data, int num_samples) {
xuweiqian9999 0:da91c8ed4a98 371 int i;
xuweiqian9999 0:da91c8ed4a98 372 for (i = 0; i < num_samples; i++) {
xuweiqian9999 0:da91c8ed4a98 373 printf("%6.3f,%d,%d,%d\n", color_data[0][i], (int)color_data[1][i],
xuweiqian9999 0:da91c8ed4a98 374 (int)color_data[2][i],(int)color_data[3][i]);
xuweiqian9999 0:da91c8ed4a98 375 wait(0.01);
xuweiqian9999 0:da91c8ed4a98 376 }
xuweiqian9999 0:da91c8ed4a98 377 printf("Number of samples = %d\n", num_samples);
xuweiqian9999 0:da91c8ed4a98 378 printf ("End Transmission!\n");
xuweiqian9999 0:da91c8ed4a98 379 }
xuweiqian9999 0:da91c8ed4a98 380
xuweiqian9999 0:da91c8ed4a98 381 void output_to_serial(float **vel_data, int num_samples) {
xuweiqian9999 0:da91c8ed4a98 382 int i;
xuweiqian9999 0:da91c8ed4a98 383 for (i = 0; i < num_samples; i++) {
xuweiqian9999 0:da91c8ed4a98 384 printf("%6.3f,%7.5f,%7.5f\n", vel_data[0][i], vel_data[1][i],vel_data[2][i]);
xuweiqian9999 0:da91c8ed4a98 385 wait(0.01);
xuweiqian9999 0:da91c8ed4a98 386 }
xuweiqian9999 0:da91c8ed4a98 387 printf("Number of samples = %d\n", num_samples);
xuweiqian9999 0:da91c8ed4a98 388 printf ("End Transmission!\n");
xuweiqian9999 0:da91c8ed4a98 389 }