motion_tracking.cpp

00001 #include "motion_tracking.h"
00002 
00003 void acquire_new_speed(float *cur_speed, float duration, float *instant_accel) {
00004     cur_speed[0] = cur_speed[0] + duration * instant_accel[0];
00005     cur_speed[1] = cur_speed[1] + duration * instant_accel[1];
00006     cur_speed[2] = cur_speed[2] + duration * instant_accel[2];
00007 }
00008 
00009 void acquire_sensor_data(float *accel_data, float *gyro_data, float *calib_data, 
00010                          int calibFlag, FXOS8700 *accel, FXAS21002 *gyro) {
00011     accel->acquire_accel_data_g(accel_data);
00012     gyro->acquire_gyro_data_dps(gyro_data);
00013     int precision = 1000;
00014     
00015     float temp_y = accel_data[1];
00016     accel_data[1] = accel_data[0];
00017     accel_data[0] = temp_y;
00018     
00019     if (calibFlag && (calib_data != NULL)) {
00020         int i;
00021         for (i = 0; i < 3; i++) {
00022             accel_data[i] -= calib_data[i];
00023             gyro_data[i] -= calib_data[i + 3];
00024 
00025             if (accel_data[i] < 0) {
00026                 accel_data[i] = floor(-1*precision*accel_data[i])/(-1 * precision);
00027             } else {
00028                 accel_data[i] = floor(precision*accel_data[i])/precision;
00029             }
00030             if (gyro_data[i] < 0) {
00031                 gyro_data[i] = floor(-1*precision*gyro_data[i])/(-1*precision);
00032             } else {
00033                 gyro_data[i] = floor(precision*gyro_data[i])/precision;
00034             }
00035         }
00036     }
00037 }
00038     
00039 void get_caliberate_data(float *caliberate, FXOS8700 *accel, FXAS21002 *gyro) {
00040     int i;
00041     int j;
00042     float accel_data[3];
00043     float gyro_data[3];
00044     
00045     double temp_calib[6] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f};
00046     
00047     for (i = 0; i < CALIBTIMES; i++) {
00048         acquire_sensor_data(accel_data, gyro_data, NULL, 0, accel, gyro);
00049         for (j = 0; j < 3; j++) {
00050             temp_calib[j]     += accel_data[j];
00051             temp_calib[j + 3] += gyro_data[j];
00052         }
00053         wait_ms(MAINWAIT);
00054     }
00055     
00056     for (i = 0; i < 6; i++) {
00057            temp_calib[i] /= (double)CALIBTIMES;
00058            caliberate[i] = (float)temp_calib[i];
00059     }
00060 }
00061 
00062 void load_buffer(float **all_data, float *accel_data, float time, int i) {
00063     if (i == MAXBUFFERSIZE) {
00064         printf("Buffer full!\n\r");
00065         exit(1);
00066     }
00067     
00068     all_data[0][i] = time;
00069     all_data[1][i] = accel_data[0];
00070     all_data[2][i] = accel_data[1];
00071     all_data[3][i] = accel_data[2];
00072 }
00073 
00074 void init_moving_avg_buf(float moving_avg_buf[][MOVINGAVRBUFSIZE], int *num_samples, 
00075                          float *last_total, float **all_data, float *caliberate, 
00076                          Timer *t, FXOS8700 *accel, FXAS21002 *gyro) {
00077     int i;
00078     float accel_data[3];
00079     float gyro_data[3];
00080     float time;
00081     float total_accelx = 0.0f;
00082     float total_accely = 0.0f;
00083     float total_accelz = 0.0f;
00084     
00085     for (i = 0; i < (int)MOVINGAVRBUFSIZE; i++) {
00086         acquire_sensor_data(accel_data, gyro_data, caliberate, 1, accel, gyro);
00087         time = t->read();
00088         moving_avg_buf[0][i] = accel_data[0];
00089         moving_avg_buf[1][i] = accel_data[1];
00090         moving_avg_buf[2][i] = accel_data[2];
00091         moving_avg_buf[3][i] = time;
00092         wait_ms(MAINWAIT);
00093     }
00094 
00095     for (i = 0; i < (int)MOVINGAVRBUFSIZE; i++) {
00096         total_accelx += moving_avg_buf[0][i];
00097         total_accely += moving_avg_buf[1][i];
00098         total_accelz += moving_avg_buf[2][i];
00099     }
00100     
00101     last_total[0] = total_accelx;
00102     last_total[1] = total_accely;
00103     last_total[2] = total_accelz;
00104     
00105     all_data[0][0] = moving_avg_buf[3][0];
00106     all_data[1][0] = total_accelx/(float) MOVINGAVRBUFSIZE;
00107     all_data[2][0] = total_accely/(float) MOVINGAVRBUFSIZE;
00108     all_data[3][0] = total_accelz/(float) MOVINGAVRBUFSIZE;
00109 
00110     (*num_samples)++;
00111 }
00112 
00113 void get_new_moving_average_point(float **all_data, float *last_total, 
00114                                 float *accel_data, float moving_avg_buf[][MOVINGAVRBUFSIZE], 
00115                                 float time, int *num_samples, int *start, 
00116                                 int *end) {
00117     
00118     last_total[0] = last_total[0] - moving_avg_buf[0][*start] + accel_data[0];
00119     last_total[1] = last_total[1] - moving_avg_buf[1][*start] + accel_data[1];
00120     last_total[2] = last_total[2] - moving_avg_buf[2][*start] + accel_data[2];
00121     
00122     all_data[0][*num_samples] = moving_avg_buf[3][*start];
00123     
00124     float temp_x = last_total[0] / (float)MOVINGAVRBUFSIZE;
00125     float temp_y = last_total[1] / (float)MOVINGAVRBUFSIZE;
00126     float temp_z = last_total[2] / (float)MOVINGAVRBUFSIZE;
00127     
00128     if (abs(temp_x) > (float) XMECHANICAL) {
00129         all_data[1][*num_samples] = temp_x;
00130     } else {
00131         all_data[1][*num_samples] = 0.0f;
00132     }
00133     
00134     if (abs(temp_y) > (float) YMECHANICAL) {
00135         all_data[2][*num_samples] = temp_y;
00136     } else {
00137         all_data[2][*num_samples] = 0.0f;
00138     }
00139     
00140     if (abs(temp_z) > (float) ZMECHANICAL) {
00141         all_data[3][*num_samples] = temp_z;
00142     } else {
00143         all_data[3][*num_samples] = 0.0f;
00144     }
00145     
00146     *start = (*start + 1) % (int) MOVINGAVRBUFSIZE;
00147     *end = (*end + 1) % (int) MOVINGAVRBUFSIZE;
00148     
00149     moving_avg_buf[0][*end] = accel_data[0];
00150     moving_avg_buf[1][*end] = accel_data[1];
00151     moving_avg_buf[2][*end] = accel_data[2];
00152     moving_avg_buf[3][*end] = time;
00153     
00154     (*num_samples) += 1;
00155 }
00156 
00157 void apply_trend_protect(float **all_data, int num_samples, float *total_diff,
00158                         float *additional_to_vel, float duration) {
00159     
00160     if (num_samples > TRENDPROTECTBUFSIZE) {
00161         total_diff[0] -= all_data[2][num_samples-TRENDPROTECTBUFSIZE] - all_data[2][num_samples-TRENDPROTECTBUFSIZE - 1];
00162         total_diff[1] -= all_data[3][num_samples-TRENDPROTECTBUFSIZE] - all_data[3][num_samples-TRENDPROTECTBUFSIZE - 1];
00163         
00164         total_diff[0] += all_data[2][num_samples-1] - all_data[2][num_samples-2];
00165         total_diff[1] += all_data[3][num_samples-1] - all_data[3][num_samples-2];
00166         
00167         if (abs(total_diff[0]) <= (float)XTRENDPROTECTTHRESHOLD) {
00168             additional_to_vel[0] = -1000.0f;
00169         } else {
00170             float avg_accel = all_data[2][num_samples-1] - ((all_data[2][num_samples-1] - all_data[2][num_samples-2])/2.0f);
00171             additional_to_vel[0] = avg_accel * duration;
00172         }
00173         
00174         if (abs(total_diff[1]) <= (float)YTRENDPROTECTTHRESHOLD) {
00175             additional_to_vel[1] = -1000.0f;
00176         } else {
00177             float avg_accel = all_data[3][num_samples-1] - ((all_data[3][num_samples-1] - all_data[3][num_samples-2])/2.0f);
00178             additional_to_vel[1] = avg_accel * duration;
00179         }
00180         
00181     } else {
00182         if(num_samples == 1){
00183         } else {
00184             total_diff[0] += all_data[2][num_samples-1] - all_data[2][num_samples-2];
00185             total_diff[1] += all_data[3][num_samples-1] - all_data[3][num_samples-2];
00186         }
00187     }
00188 }
00189         
00190     
00191 
00192 void apply_move_end_check(float **all_data, int num_samples, 
00193                         int moving_end_buf[][MOVINGENDBUFSIZE],
00194                         int *num_unqualified, int *start, int *end,
00195                         float *addition_to_vel, float duration,
00196                         float *total_diff) {
00197 
00198     if (num_samples > MOVINGENDBUFSIZE) {
00199         num_unqualified[0] -= moving_end_buf[0][*start];
00200         num_unqualified[1] -= moving_end_buf[1][*start];
00201         *start = (*start + 1) % MOVINGENDBUFSIZE;
00202         *end = (*end + 1) % MOVINGENDBUFSIZE;
00203         
00204         if (abs(all_data[2][num_samples-1]) <= (float) YMOVENDTHRESHOLD ){
00205             num_unqualified[0] += 1;
00206             moving_end_buf[0][*end] = 1;
00207         } else {
00208             moving_end_buf[0][*end] = 0;
00209         }
00210         
00211         if (abs(all_data[3][num_samples-1]) <= (float) ZMOVENDTHRESHOLD ){
00212             num_unqualified[1] += 1;
00213             moving_end_buf[1][*end] = 1;
00214         } else {
00215             moving_end_buf[1][*end] = 0;
00216         }
00217         
00218         if (num_unqualified[0] >= (int)YMOVENDBIASNUM){
00219             addition_to_vel[0] = -1000.0f;
00220         }
00221         
00222         if (num_unqualified[1] >= (int)ZMOVENDBIASNUM){
00223             addition_to_vel[1] = -1000.0f;
00224         }
00225         
00226         apply_trend_protect(all_data, num_samples, total_diff, addition_to_vel,
00227                             duration);
00228         
00229         if (num_unqualified[0] < (int)YMOVENDBIASNUM){
00230             float avg_accel = all_data[2][num_samples-1] - ((all_data[2][num_samples-1] - all_data[2][num_samples-2])/2.0f);
00231             addition_to_vel[0] = avg_accel * duration;
00232         }
00233         
00234         if (num_unqualified[1] < (int)ZMOVENDBIASNUM){
00235             float avg_accel = all_data[3][num_samples-1] - ((all_data[3][num_samples-1] - all_data[3][num_samples-2])/2.0f);
00236             addition_to_vel[1] = avg_accel * duration;
00237         }
00238         
00239     } else if (num_samples < MOVINGENDBUFSIZE) {
00240         addition_to_vel[0] = -1000.0f;
00241         addition_to_vel[1] = -1000.0f;
00242         apply_trend_protect(all_data, num_samples, total_diff, addition_to_vel,
00243                             duration);
00244     } else {
00245         int i;
00246         for (i = 0; i < MOVINGENDBUFSIZE; i++) {
00247             if (abs(all_data[2][i]) <= (float)YMOVENDTHRESHOLD) {
00248                 moving_end_buf[0][i] = 1;
00249                 num_unqualified[0] += 1;
00250             } else {
00251                 moving_end_buf[0][i] = 0;
00252             }
00253             
00254             if (abs(all_data[3][i]) <= (float)ZMOVENDTHRESHOLD) {
00255                 moving_end_buf[1][i] = 1;
00256                 num_unqualified[1] += 1;
00257             } else {
00258                 moving_end_buf[1][i] = 0;
00259             }
00260             
00261             addition_to_vel[0] = -1000.0f;
00262             addition_to_vel[1] = -1000.0f;
00263         }
00264         apply_trend_protect(all_data, num_samples, total_diff, addition_to_vel,
00265                             duration);
00266     }
00267 }
00268 
00269 void get_new_velocity (float *original_speed, float *addition_to_vel) {
00270     if (addition_to_vel[0] == -1000.0f) {
00271         original_speed[0] = 0.0f;
00272     } else {
00273         original_speed[0] += addition_to_vel[0];
00274     }
00275     
00276     if (addition_to_vel[1] == -1000.0f) {
00277         original_speed[1] = 0.0f;
00278     } else {
00279         original_speed[1] += addition_to_vel[1];
00280     }
00281 }
00282 
00283 void get_new_position (float *original_position, float *cur_speed, float duration) {
00284     original_position[0] += cur_speed[0] * duration;
00285     original_position[1] -= cur_speed[1] * duration;
00286 }
00287     
00288 
00289 void insert_new_vel_to_buffer(float** vel_buffer, float time, float* cur_vel,
00290                               int num_samples) {
00291     
00292     vel_buffer[0][num_samples - 1] = time;
00293     vel_buffer[1][num_samples - 1] = cur_vel[0];
00294     vel_buffer[2][num_samples - 1] = cur_vel[1];
00295 }
00296 
00297 void insert_new_pos_to_buffer(float** pos_buffer, float time, float* cur_pos,
00298                               int num_samples) {
00299     pos_buffer[0][num_samples - 1] = time;
00300     pos_buffer[1][num_samples - 1] = cur_pos[0];
00301     pos_buffer[2][num_samples - 1] = cur_pos[1];
00302 }
00303 
00304 void insert_new_color_to_buffer(float **color_buffer, int r, int g, int b, 
00305                                 float time, int num_samples) {
00306     color_buffer[0][num_samples - 1] = time;
00307     color_buffer[1][num_samples - 1] = (float) r;
00308     color_buffer[2][num_samples - 1] = (float) g;
00309     color_buffer[3][num_samples - 1] = (float) b;
00310 }
00311 
00312 void initialize_color_table(RGB rgb_table[][HORIZONTALNUMCOLOR]){
00313     rgb_table[0][0] = (RGB){4, 77, 210};
00314     rgb_table[0][1] = (RGB){5, 77, 77};
00315     rgb_table[0][2] = (RGB){6, 179, 102};
00316     rgb_table[0][3] = (RGB){7, 224, 133};
00317     rgb_table[0][4] = (RGB){8, 121, 255};
00318     
00319     rgb_table[1][0] = (RGB){9, 0, 191};
00320     rgb_table[1][1] = (RGB){10, 0, 0};
00321     rgb_table[1][2] = (RGB){11, 128, 0};
00322     rgb_table[1][3] = (RGB){12, 204, 51};
00323     rgb_table[1][4] = (RGB){13, 64, 255};
00324     
00325     rgb_table[2][0] = (RGB){14, 0, 134};
00326     rgb_table[2][1] = (RGB){15, 0, 0};
00327     rgb_table[2][2] = (RGB){16, 102, 0};
00328     rgb_table[2][3] = (RGB){17, 122, 31};
00329     rgb_table[2][4] = (RGB){18, 45, 179};
00330 }
00331 
00332 RGB get_new_color(float *cur_location, RGB rgb_table[][HORIZONTALNUMCOLOR]) {
00333     int x;
00334     int y;
00335     
00336     if (cur_location[0] < -0.03f) {
00337         x = 0;
00338     } else if (cur_location[0] >= -0.03f && cur_location[0] < -0.01f) {
00339         x = 1;
00340     } else if (cur_location[0] >= -0.01f && cur_location[0] < 0.01f) {
00341         x = 2;
00342     } else if (cur_location[0] >= 0.01f && cur_location[0] < 0.03f) {
00343         x = 3;
00344     } else {
00345         x = 4;
00346     }
00347     
00348     if (cur_location[1] > 0.0165f) {
00349         y = 0;
00350     } else if (cur_location[1] > -0.0165f && cur_location[1] <= 0.0165f) {
00351         y = 1;
00352     } else {
00353         y = 2;
00354     }
00355     
00356     return rgb_table[y][x];
00357 }
00358 
00359 void output_all_to_serial(float **all_data, int num_samples) {
00360     int i;
00361     for (i = 0; i < num_samples; i++) {
00362         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]);
00363         wait(0.01);
00364     }
00365     printf("Number of samples = %d\n", num_samples);
00366     printf ("End Transmission!\n");
00367 }
00368 
00369 
00370 void output_color_to_serial(float **color_data, int num_samples) {
00371     int i;
00372     for (i = 0; i < num_samples; i++) {
00373         printf("%6.3f,%d,%d,%d\n", color_data[0][i], (int)color_data[1][i],
00374                     (int)color_data[2][i],(int)color_data[3][i]);
00375         wait(0.01);
00376     }
00377     printf("Number of samples = %d\n", num_samples);
00378     printf ("End Transmission!\n");
00379 }
00380 
00381 void output_to_serial(float **vel_data, int num_samples) {
00382     int i;
00383     for (i = 0; i < num_samples; i++) {
00384         printf("%6.3f,%7.5f,%7.5f\n", vel_data[0][i], vel_data[1][i],vel_data[2][i]);
00385         wait(0.01);
00386     }
00387     printf("Number of samples = %d\n", num_samples);
00388     printf ("End Transmission!\n");
00389 }