motion_tracking.h

00001 #ifndef MOTION_TRACKING_H_
00002 #define MOTION_TRACKING_H_
00003 
00004 #include "mbed.h"
00005 #include "FXOS8700.h"
00006 #include "FXAS21002.h"
00007 #include "math.h"
00008 #include "stdio.h"
00009 
00010 #define MAINWAIT 1
00011 #define CALIBTIMES 50
00012 #define SAMPLEPERIOD_S 6
00013 #define MOVINGAVRBUFSIZE 40
00014 #define MOVINGENDBUFSIZE 32
00015 #define TRENDPROTECTBUFSIZE 70
00016 #define YMOVENDTHRESHOLD 0.1
00017 #define ZMOVENDTHRESHOLD 0.1
00018 #define YMOVENDBIASNUM 30
00019 #define ZMOVENDBIASNUM 30
00020 #define XTRENDPROTECTTHRESHOLD 0.06
00021 #define YTRENDPROTECTTHRESHOLD 0.06
00022 #define MAXBUFFERSIZE 3000
00023 #define XMECHANICAL 0.005
00024 #define YMECHANICAL 0.005
00025 #define ZMECHANICAL 0.005
00026 #define VERTICALNUMCOLOR 3
00027 #define HORIZONTALNUMCOLOR 5
00028 
00029 struct RGB {
00030     int R;
00031     int G;
00032     int B;
00033 };
00034 
00035 void acquire_new_speed(float *cur_speed, float duration, float *instant_accel);
00036 
00037 void acquire_sensor_data(float *accel_data, float *gyro_data, float *calib_data,
00038                          int calibFlag, FXOS8700 *accel, FXAS21002 *gyro);
00039 
00040 void get_caliberate_data(float *caliberate, FXOS8700 *accel, FXAS21002 *gyro);
00041 
00042 void load_buffer(float **all_data, float *accel_data, float time, int i);
00043 
00044 void init_moving_avg_buf(float moving_avg_buf[][MOVINGAVRBUFSIZE], int *num_samples, 
00045                          float *last_total, float **all_data, float *caliberate, 
00046                          Timer *t, FXOS8700 *accel, FXAS21002 *gyro);
00047 
00048 void get_new_moving_average_point(float **all_data, float *last_total, 
00049                                 float *accel_data, float moving_avg_buf[][MOVINGAVRBUFSIZE], 
00050                                 float time, int *num_samples, int *start, 
00051                                 int *end);
00052 
00053 void apply_trend_protect(float **all_data, int num_samples, float *total_diff,
00054                         float *additional_to_vel, float duration);
00055 
00056 void apply_move_end_check(float **all_data, int num_samples, 
00057                         int moving_end_buf[][MOVINGENDBUFSIZE],
00058                         int *num_unqualified, int *start, int *end,
00059                         float *addition_to_vel, float duration,
00060                         float *total_diff);
00061 
00062 void get_new_velocity (float *original_speed, float *addition_to_vel);
00063 
00064 void get_new_position (float *original_position, float *cur_speed, float duration);
00065 
00066 void insert_new_vel_to_buffer(float** vel_buffer, float time, float* cur_vel,
00067                               int num_samples);
00068 
00069 void insert_new_pos_to_buffer(float** pos_buffer, float time, float* cur_pos,
00070                               int num_samples);
00071 
00072 void insert_new_color_to_buffer(float **color_buffer, int r, int g, int b, 
00073                                 float time, int num_samples);
00074 
00075 void initialize_color_table(RGB rgb_table[][HORIZONTALNUMCOLOR]);
00076 
00077 RGB get_new_color(float *cur_location, RGB rgb_table[][HORIZONTALNUMCOLOR]);
00078 
00079 void output_all_to_serial(float **all_data, int num_samples);
00080 
00081 void output_color_to_serial(float **color_data, int num_samples);
00082 
00083 void output_to_serial(float **vel_data, int num_samples);
00084 
00085 #endif