main.cpp

00001 #include "mbed.h"
00002 #include "LSM9DS1.h"
00003 #include "AS5145.h"
00004 
00005 LSM9DS1 imu2(D5, D7);
00006 LSM9DS1 imu3(D3, D6);
00007 LSM9DS1 imu(D14, D15);
00008 AnalogIn sEMG(D13);
00009 AnalogIn sEMG2(D1);
00010 AnalogIn sEMG3(D0);
00011 AnalogIn sEMG4(PC_4);
00012 Serial pc(USBTX, USBRX);
00013 Ticker timer1;
00014 Ticker timer2;
00015 
00016 float T = 0.001;
00017 /********************************************************************/
00018 //function declaration
00019 /********************************************************************/
00020 void init_TIMER(void);
00021 void timer1_interrupt(void);
00022 void setup(void);
00023 void estimator(float axm[3],float aym[3],float azm[3],float w3[3],float w2[3],float w1[3],float alpha);
00024 float lpf(float input, float output_old, float frequency);
00025 void angle_fn(float x1_hat[3],float x2_hat[3]);
00026 void pitch_dot_fn(float w3[3],float w2[3],float w1[3],float sinroll[3],float cosroll[3]);
00027 void pitch_double_dot_fn(float pitch_dot[3],float pitch_dot_old[3]);
00028 /********************************************************************/
00029 // sensor data
00030 /********************************************************************/
00031 int16_t Gyro_axis_data[9] = {0};     // X, Y, Z axis
00032 int16_t Acce_axis_data[9] = {0};     // X, Y, Z axis
00033 float Gyro_axis_data_f[9] = {0};
00034 float Gyro_axis_data_f_old[9] = {0};
00035 float Acce_axis_data_f[9] = {0};
00036 float Acce_axis_data_f_old[9] = {0};
00037 float axm[3] = {0.0f};
00038 float aym[3] = {0.0f};
00039 float azm[3] = {0.0f};
00040 float w1[3] = {0.0f};
00041 float w2[3] = {0.0f};
00042 float w3[3] = {0.0f};
00043 //estimator
00044 float x1_hat[3] = {0.0f};
00045 float x2_hat[3] = {0.0f};
00046 float sinroll[3] = {0.0f};
00047 float cosroll[3] = {0.0f};
00048 float sinpitch[3] = {0.0f};
00049 float pitch_angle[3] = {0.0f};
00050 float roll_angle[3] = {0.0f};
00051 float yaw_dot[3] = {0.0f};
00052 float pitch_dot[3] = {0.0f};
00053 float pitch_double_dot[3] = {0.0f};
00054 float pitch_double_dot_f[3] = {0.0f};
00055 float pitch_double_dot_f_old[3] = {0.0f};
00056 float pitch_dot_old[3] = {0.0f};
00057 float axm_f[3] = {0.0f};
00058 float axm_f_old[3] = {0.0f};
00059 float w3aym_f[3] = {0.0f};
00060 float w3aym_f_old[3] = {0.0f};
00061 float w2azm_f[3] = {0.0f};
00062 float w2azm_f_old[3] = {0.0f};
00063 float aym_f[3] = {0.0f};
00064 float aym_f_old[3] = {0.0f};
00065 float w3axm_f[3] = {0.0f};
00066 float w3axm_f_old[3] = {0.0f};
00067 float w1azm_f[3] = {0.0f};
00068 float w1azm_f_old[3] = {0.0f};
00069 float azm_f[3] = {0.0f};
00070 float azm_f_old[3] = {0.0f};
00071 float w2axm_f[3] = {0.0f};
00072 float w2axm_f_old[3] = {0.0f};
00073 float w1aym_f[3] = {0.0f};
00074 float w1aym_f_old[3] = {0.0f};
00075 //sEMG variable
00076 float emg_value[4] = {0.0f};
00077 
00078 int main()
00079 {
00080     pc.baud(230400);
00081     setup();  //Setup sensors
00082     AS5145_begin(); //begin encoder
00083     init_TIMER();
00084     while (1)
00085     {
00086         //pc.printf("%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%d,%d\n",pitch_angle[0],pitch_dot[0],pitch_angle[1],pitch_dot[1],pitch_angle[2],pitch_dot[2],emg_value[0],emg_value[1],emg_value[2],emg_value[3],position[1]*360/4096, position[0]*360/4096);
00087         wait(0.05);
00088         //pc.printf("%f,%f,%f,%f\n",emg_value[0],emg_value[1],emg_value[2],emg_value[3]);
00089         pc.printf("%f,%f,%f,%f,%f,%d,%d\n", pitch_angle[0], pitch_angle[1], roll_angle[1], pitch_angle[2], roll_angle[2], position[1]*360/4096, position[0]*360/4096);
00090         //pc.printf("IMU: %2f,%2f\r\n", pitch_angle[0], roll_angle[0]);
00091         //pc.printf("IMU2: %2f,%2f\r\n", pitch_angle[1], roll_angle[1]);
00092         //pc.printf("IMU3: %2f,%2f\r\n", pitch_angle[2], roll_angle[2]);
00093         //pc.printf("position: %d,%d\r\n", position[0], position[1]);
00094         //pc.printf("roll_angle: %2f\r\n",roll_angle);
00095         //pc.printf("A: %2f, %2f, %2f\r\n", imu2.ax*Acce_gain_x_2, imu2.ay*Acce_gain_y_2, imu2.az*Acce_gain_z_2);
00096     }
00097 }
00098 void setup()
00099 {
00100     imu.begin();
00101     imu2.begin();
00102     imu3.begin();
00103 }
00104 /********************************************************************/
00105 // init_TIMER
00106 /********************************************************************/
00107 void init_TIMER(void)
00108 {
00109     timer1.attach_us(&timer1_interrupt, 10000);//10ms interrupt period (100 Hz)
00110     timer2.attach_us(&ReadValue, 1000);//1ms interrupt period (1000 Hz)
00111 }
00112 /********************************************************************/
00113 // timer1_interrupt
00114 /********************************************************************/
00115 void timer1_interrupt(void)
00116 {  
00117     int i;
00118     imu.readAccel();
00119     imu.readGyro();
00120     imu2.readAccel();
00121     imu2.readGyro();
00122     imu3.readAccel();
00123     imu3.readGyro();
00124     // sensor raw data
00125     Acce_axis_data[0] = imu.ax*Acce_gain_x;
00126     Acce_axis_data[1] = imu.ay*Acce_gain_y;
00127     Acce_axis_data[2] = imu.az*Acce_gain_z;
00128     Acce_axis_data[3] = -imu2.ax*Acce_gain_x_2;
00129     Acce_axis_data[4] = imu2.az*Acce_gain_y_2;
00130     Acce_axis_data[5] = imu2.ay*Acce_gain_z_2;
00131     Acce_axis_data[6] = -imu3.ax*Acce_gain_x_2;
00132     Acce_axis_data[7] = -imu3.az*Acce_gain_y_2;
00133     Acce_axis_data[8] = -imu3.ay*Acce_gain_z_2;
00134 
00135     Gyro_axis_data[0] = imu.gx*Gyro_gain_x;
00136     Gyro_axis_data[1] = imu.gy*Gyro_gain_y;
00137     Gyro_axis_data[2] = imu.gz*Gyro_gain_z;
00138     Gyro_axis_data[3] = -imu2.gx*Gyro_gain_x_2;
00139     Gyro_axis_data[4] = imu2.gz*Gyro_gain_y_2;
00140     Gyro_axis_data[5] = imu2.gy*Gyro_gain_z_2;
00141     Gyro_axis_data[6] = -imu3.gx*Gyro_gain_x_2;
00142     Gyro_axis_data[7] = -imu3.gz*Gyro_gain_y_2;
00143     Gyro_axis_data[8] = -imu3.gy*Gyro_gain_z_2;
00144     
00145     for(i=0;i<9;i++)
00146     {
00147         Acce_axis_data_f[i] = lpf(Acce_axis_data[i],Acce_axis_data_f_old[i],15);
00148         Acce_axis_data_f_old[i] = Acce_axis_data_f[i];
00149         Gyro_axis_data_f[i] = lpf(Gyro_axis_data[i],Gyro_axis_data_f_old[i],15);     
00150         Gyro_axis_data_f_old[i] = Gyro_axis_data_f[i];
00151     }
00152     
00153     axm[0] = Acce_axis_data_f[0];
00154     aym[0] = Acce_axis_data_f[1];
00155     azm[0] = Acce_axis_data_f[2];
00156     w1[0]  = Gyro_axis_data_f[0];
00157     w2[0]  = Gyro_axis_data_f[1];
00158     w3[0]  = Gyro_axis_data_f[2];
00159     axm[1] = Acce_axis_data_f[3];
00160     aym[1] = Acce_axis_data_f[4];
00161     azm[1] = Acce_axis_data_f[5];
00162     w1[1]  = Gyro_axis_data_f[3];
00163     w2[1]  = Gyro_axis_data_f[4];
00164     w3[1]  = Gyro_axis_data_f[5];
00165     axm[2] = Acce_axis_data_f[6];
00166     aym[2] = Acce_axis_data_f[7];
00167     azm[2] = Acce_axis_data_f[8];
00168     w1[2]  = Gyro_axis_data_f[6];
00169     w2[2]  = Gyro_axis_data_f[7];
00170     w3[2]  = Gyro_axis_data_f[8];
00171     
00172     
00173     estimator(axm,aym,azm,w3,w2,w1,120);
00174     angle_fn(x1_hat,x2_hat);
00175     pitch_dot_fn(w3,w2,w1,sinroll,cosroll);
00176     pitch_double_dot_fn(pitch_dot,pitch_dot_old); 
00177 
00178     for(i=0;i<3;i++)
00179     {
00180         pitch_dot_old[i] = pitch_dot[i];
00181     }
00182     emg_value[0] = sEMG.read(); 
00183     emg_value[1] = sEMG2.read(); 
00184     emg_value[2] = sEMG3.read();
00185     emg_value[3] = sEMG4.read();
00186 }
00187 /********************************************************************/
00188 // estimator
00189 /********************************************************************/
00190 void estimator(float axm[3],float aym[3],float azm[3],float w3[3],float w2[3],float w1[3],float alpha)
00191 {
00192     int i;
00193     for(i=0;i<3;i++)
00194     {
00195         axm_f[i] = lpf(axm[i],axm_f_old[i],alpha);
00196         axm_f_old[i] = axm_f[i];
00197         w3aym_f[i] = lpf(w3[i]*aym[i],w3aym_f_old[i],alpha);
00198         w3aym_f_old[i] = w3aym_f[i];
00199         w2azm_f[i] = lpf(w2[i]*azm[i],w2azm_f_old[i],alpha);
00200         w2azm_f_old[i] = w2azm_f[i];
00201         aym_f[i] = lpf(aym[i],aym_f_old[i],alpha);
00202         aym_f_old[i] = aym_f[i];
00203         w3axm_f[i] = lpf(w3[i]*axm[i],w3axm_f_old[i],alpha);
00204         w3axm_f_old[i] = w3axm_f[i];
00205         w1azm_f[i] = lpf(w1[i]*azm[i],w1azm_f_old[i],alpha);
00206         w1azm_f_old[i] = w1azm_f[i];
00207     
00208         x1_hat[i] = axm_f[i] + w3aym_f[i]/alpha - w2azm_f[i]/alpha;
00209         x2_hat[i] = -w3axm_f[i]/alpha + aym_f[i] + w1azm_f[i]/alpha;
00210     }
00211     
00212 }
00213 /********************************************************************/
00214 // angle_fn
00215 /********************************************************************/
00216 void angle_fn(float x1_hat[3],float x2_hat[3])
00217 {
00218     int i;
00219     for(i=0;i<3;i++)
00220     {
00221         sinroll[i] = x2_hat[i]*(-0.1020);
00222         if(sinroll[i] >= 1.0f)
00223         {
00224             sinroll[i] = 1.0;
00225             cosroll[i] = 0.0;
00226         }
00227         else if(sinroll[i] <= -1.0f)
00228         {
00229             sinroll[i] = -1.0;
00230             cosroll[i] = 0.0;
00231         }
00232         else cosroll[i] = sqrt(1-(sinroll[i]*sinroll[i]));
00233         roll_angle[i] = (asin(sinroll[i]))*180/pi;
00234         sinpitch[i] = x1_hat[i]*(0.1020f)/cosroll[i];
00235         if(sinpitch[i] >= 1.0f)
00236         {
00237             sinpitch[i] = 1.0;
00238         }
00239         else if(sinpitch[i] <= -1.0f)
00240         {
00241             sinpitch[i] = -1.0;
00242         }
00243     
00244         pitch_angle[i] = (asin(sinpitch[i]))*180/pi;
00245     }
00246 }
00247 void pitch_dot_fn(float w3[3],float w2[3],float w1[3],float sinroll[3],float cosroll[3])
00248 {
00249     int i;
00250     for(i=0;i<3;i++)
00251     {
00252         yaw_dot[i] = (w3[i]*cosroll[i] - w1[i]*sinroll[i])/cosroll[i];
00253         pitch_dot[i] = w2[i] - yaw_dot[i]*sinroll[i];
00254     }
00255 }
00256 void pitch_double_dot_fn(float pitch_dot[3],float pitch_dot_old[3])
00257 {
00258     int i;
00259     for(i=0;i<3;i++)
00260     {
00261         pitch_double_dot[i] = (pitch_dot[i] - pitch_dot_old[i])/0.01f;
00262         pitch_double_dot_f[i] = lpf(pitch_double_dot[i],pitch_double_dot_f_old[i],30);
00263         pitch_double_dot_f_old[i] = pitch_double_dot_f[i]; 
00264     }
00265 }
00266 /********************************************************************/
00267 // lpf
00268 /********************************************************************/
00269 float lpf(float input, float output_old, float frequency)
00270 {
00271     float output = 0;
00272     
00273     output = (output_old + frequency*T*input) / (1 + frequency*T);
00274     
00275     return output;
00276 }