Hybrid sEMG + IMU activated controller for Galileo Bionic Hand Prosthesis
Dependencies: FXAS21000 FXOS8700Q kalman mbed-dsp mbed-rtos mbed
main.cpp
00001 #include "mbed.h" 00002 #include "FXOS8700Q.h" 00003 #include "FXAS21000.h" 00004 #include "rtos.h" 00005 #include "kalman.c" 00006 #include "arm_math.h" 00007 #include "rtwtypes.h" 00008 00009 #define TRUE 1 00010 #define FALSE 0 00011 #define RAW 0 00012 #define RECTIFIED 1 00013 #define SMOOTH 2 00014 #define THRESHOLDF 0.25f 00015 #define THRESHOLDE 0.25f 00016 #define FLEXION2 0.0f 00017 #define Rad2Dree 57.295779513082320876798154814105f 00018 00019 Serial pc(USBTX, USBRX); 00020 AnalogIn Ref(A0); 00021 AnalogIn E1(A1); 00022 AnalogIn E2(A2); 00023 AnalogIn E3(A3); 00024 FXOS8700Q_acc combo_acc(D14, D15, FXOS8700CQ_SLAVE_ADDR1); 00025 FXOS8700Q_mag combo_mag(D14, D15, FXOS8700CQ_SLAVE_ADDR1); 00026 FXAS21000 gyro(D14, D15); 00027 DigitalOut led1(LED1); 00028 DigitalOut led2(LED2); 00029 InterruptIn sw2(SW2); 00030 Timer GlobalTime; 00031 Timer ProgramTimer; 00032 00033 //Threadas 00034 uint32_t button_pressed; 00035 Thread *thread2; 00036 Thread *thread3; 00037 Thread *thread4; 00038 00039 //EMG samples and DSP variables 00040 float32_t EMG1, EMG2, EMG3; 00041 float32_t samples[25]; 00042 float32_t samples2[25]; 00043 float32_t samples3[25]; 00044 float32_t abs_output[25]; 00045 float32_t abs_output2[25]; 00046 float32_t abs_output3[25]; 00047 float32_t MAV1 = 0.0f; 00048 float32_t MAV2 = 0.0f; 00049 float32_t MAV3 = 0.0f; 00050 00051 typedef struct { 00052 float32_t iemg; 00053 float32_t wl; 00054 float32_t var; 00055 int32_t wamp; 00056 int32_t zc; 00057 int32_t ssc; 00058 } emg_features; 00059 00060 emg_features feat1 = {0.0f,0.0f,0.0f,0,0,0}; 00061 emg_features feat2 = {0.0f,0.0f,0.0f,0,0,0}; 00062 00063 //IMU 00064 float32_t gyro_data[3]; 00065 MotionSensorDataUnits adata; 00066 MotionSensorDataUnits mdata; 00067 float32_t R; 00068 float32_t R_XY; 00069 float32_t R_mean; 00070 float32_t angle[3]; 00071 float32_t roll; 00072 float32_t pitch; 00073 kalman filter_pitch; 00074 kalman filter_roll; 00075 kalman filter_yaw; 00076 float32_t imu_ones[25]; 00077 float32_t accx_samples[25]; 00078 float32_t accy_samples[25]; 00079 float32_t accz_samples[25]; 00080 float32_t roll_samples[25]; 00081 float32_t pitch_samples[25]; 00082 float32_t R_samples[25]; 00083 float32_t imu_abs_samples[25]; 00084 00085 00086 typedef struct { 00087 float32_t l1; 00088 float32_t mean; 00089 float32_t var; 00090 } imu_features; 00091 00092 imu_features ax_feat = {0.0f,0.0f,0.0f}; 00093 imu_features ay_feat = {0.0f,0.0f,0.0f}; 00094 imu_features az_feat = {0.0f,0.0f,0.0f}; 00095 imu_features pitch_feat = {0.0f,0.0f,0.0f}; 00096 imu_features roll_feat = {0.0f,0.0f,0.0f}; 00097 imu_features R_feat = {0.0f,0.0f,0.0f}; 00098 00099 float32_t imu_y[4] = {0.0f,0.0f,0.0f,0.0f}; //neural network output 00100 float32_t imu_x[18] = {0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f}; //imu neural network input 00101 float32_t imu_max = 0.0f; //max accuracy 00102 uint32_t imu_class = 0; //class 00103 00104 //OS 00105 Mutex imu_mutex; 00106 Mutex emg_mutex; 00107 00108 //time 00109 unsigned long timer; 00110 long loopStartTime; 00111 00112 void sw2_press(void); 00113 void button_thread(void const *argument); 00114 void imu_thread(void const *argument); 00115 void sampler_thread(void const *argument); 00116 void imu_proc_thread(void const *argument); 00117 00118 imu_features imuGetFeatures(float32_t samples[50]); 00119 void Serial_Oscilloscope(uint8_t activation, uint8_t type); 00120 void IMU_Logger(void); 00121 void IMU_Classifier(const float x1[18], float b_y1[4]); 00122 boolean_T rtIsNaNF(real32_T value); 00123 00124 int main(void) 00125 { 00126 pc.baud(115200); //Serial com at 115200 bauds 00127 led1 = 1; led2 = 1; 00128 Thread thread(imu_thread); 00129 thread2 = new Thread(button_thread); 00130 thread3 = new Thread(sampler_thread); 00131 thread4 = new Thread(imu_proc_thread); 00132 00133 arm_fill_f32(1.0f, imu_ones, 50); 00134 00135 GlobalTime.start(); 00136 00137 combo_acc.enable(); 00138 combo_mag.enable(); 00139 //pc.printf("FXOS8700 Combo = %X\r\n", combo_acc.whoAmI()); 00140 //pc.printf("FXAS21000 Gyro = %X\r\n", gyro.getWhoAmI()); 00141 00142 kalman_init(&filter_pitch, R_matrix, Q_Gyro_matrix, Q_Accel_matrix); 00143 kalman_init(&filter_roll, R_matrix, Q_Gyro_matrix, Q_Accel_matrix); 00144 00145 button_pressed = 0; 00146 sw2.fall(&sw2_press); 00147 00148 ProgramTimer.start(); 00149 loopStartTime = ProgramTimer.read_us(); 00150 timer = loopStartTime; 00151 00152 while (true) { 00153 led1 = !led1; 00154 //pc.printf("SW2 was pressed (last 5 seconds): %d \n\r", button_pressed); 00155 //pc.printf("Acc Vector: %.6f XY Acc Vector: %.6f Roll Angle X: %.6f Pitch Angle Y: %.6f \r\n", R, R_XY, Rad2Dree * angle[1], Rad2Dree * angle[0]); 00156 //fflush(stdout); 00157 //pc.printf("%.6f,%.6f,%.6f,%.6f,%.6f,%.6f\r",adata.x,adata.y,adata.z,R_XY,angle[0],angle[1]); 00158 00159 button_pressed = 0; 00160 Thread::wait(100); 00161 00162 } 00163 } 00164 00165 void sw2_press(void){ 00166 thread2->signal_set(0x1); 00167 } 00168 00169 void button_thread(void const *argument){ 00170 while (true) { 00171 Thread::signal_wait(0x1); 00172 button_pressed++; 00173 } 00174 } 00175 00176 void imu_thread(void const *argument){ 00177 while (true) { 00178 00179 combo_acc.getAxis(adata); 00180 gyro.ReadXYZ(gyro_data); 00181 00182 R = sqrt(std::pow(adata.x, 2) + std::pow(adata.y, 2) + std::pow(adata.z, 2)); 00183 R_XY = sqrt(std::pow(adata.x, 2) + std::pow(adata.y, 2)); 00184 00185 kalman_predict(&filter_pitch, gyro_data[0], (ProgramTimer.read_us() - timer)); 00186 kalman_update(&filter_pitch, acos(adata.x/R)); 00187 kalman_predict(&filter_roll, gyro_data[1], (ProgramTimer.read_us() - timer)); 00188 kalman_update(&filter_roll, acos(adata.y/R)); 00189 00190 angle[0] = kalman_get_angle(&filter_pitch); 00191 angle[1] = kalman_get_angle(&filter_roll); 00192 00193 imu_mutex.lock(); 00194 for(int j=25-1;j>0;j--) { 00195 accx_samples[j]=accx_samples[j-1]; 00196 accy_samples[j]=accy_samples[j-1]; 00197 accz_samples[j]=accz_samples[j-1]; 00198 roll_samples[j]=roll_samples[j-1]; 00199 pitch_samples[j]=pitch_samples[j-1]; 00200 R_samples[j]=R_samples[j-1]; 00201 } 00202 accx_samples[0]=adata.x; 00203 accy_samples[0]=adata.y; 00204 accz_samples[0]=adata.z; 00205 roll_samples[0]=angle[1]; 00206 pitch_samples[0]=angle[0]; 00207 R_samples[0]=R_XY; 00208 imu_mutex.unlock(); 00209 00210 thread4->signal_set(0x1); 00211 Thread::wait(10); 00212 } 00213 } 00214 00215 void sampler_thread(void const *argument){ 00216 while (true) { 00217 EMG1 = (E1.read()-Ref.read())*3.3f; 00218 EMG2 = (E2.read()-Ref.read())*3.3f; 00219 EMG3 = (E3.read()-Ref.read())*3.3f; 00220 for(int j=24;j>0;j--) { 00221 samples[j]=samples[j-1]; //Fill Array 00222 samples2[j]=samples2[j-1]; //Fill Array 00223 samples3[j]=samples3[j-1]; //Fill Array 00224 } 00225 samples[0]=EMG1; 00226 samples2[0]=EMG2; 00227 samples3[0]=EMG3; 00228 Thread::wait(1); 00229 } 00230 } 00231 00232 00233 void imu_proc_thread(void const *argument){ 00234 while (true) { 00235 //int i; 00236 Thread::signal_wait(0x1); 00237 imu_mutex.lock(); 00238 arm_mean_f32(R_samples, 25, &R_mean); 00239 if(R_mean>=1.125f){ 00240 //for(i=0;i<25;i++) pc.printf("%.6f,",accx_samples[i]); 00241 ax_feat = imuGetFeatures(accx_samples); 00242 ay_feat = imuGetFeatures(accy_samples); 00243 az_feat = imuGetFeatures(accz_samples); 00244 R_feat = imuGetFeatures(R_samples); 00245 pitch_feat = imuGetFeatures(pitch_samples); 00246 roll_feat = imuGetFeatures(roll_samples); 00247 //IMU_Logger(); 00248 imu_x[0] = ax_feat.l1; 00249 imu_x[1] = ax_feat.mean; 00250 imu_x[2] = ax_feat.var; 00251 imu_x[3] = ay_feat.l1; 00252 imu_x[4] = ay_feat.mean; 00253 imu_x[5] = ay_feat.var; 00254 imu_x[6] = az_feat.l1; 00255 imu_x[7] = az_feat.mean; 00256 imu_x[8] = az_feat.var; 00257 imu_x[9] = R_feat.l1; 00258 imu_x[10] = R_feat.mean; 00259 imu_x[11] = R_feat.var; 00260 imu_x[12] = pitch_feat.l1; 00261 imu_x[13] = pitch_feat.mean; 00262 imu_x[14] = pitch_feat.var; 00263 imu_x[15] = roll_feat.l1; 00264 imu_x[16] = roll_feat.mean; 00265 imu_x[17] = roll_feat.var; 00266 IMU_Classifier(imu_x,imu_y); 00267 arm_max_f32(imu_y,4,&imu_max,&imu_class); 00268 pc.printf("%d ",imu_class); 00269 switch(imu_class){ 00270 case 0: pc.printf("Up\n\r"); break; 00271 case 1: pc.printf("Down\n\r"); break; 00272 case 2: pc.printf("Left\n\r"); break; 00273 case 3: pc.printf("Right\n\r"); break; 00274 } 00275 } 00276 imu_mutex.unlock(); 00277 Thread::wait(500); 00278 } 00279 } 00280 00281 void Serial_Oscilloscope(uint8_t activation, uint8_t type){ 00282 if (activation){ 00283 switch(type){ 00284 case RAW: pc.printf("%.10f,%.10f,%.10f\n\r",EMG1,EMG2,EMG3); break; 00285 case RECTIFIED: pc.printf("%.10f,%.10f,%.10f\n\r",abs_output[0],abs_output2[0],abs_output3[0]);break; 00286 case SMOOTH: pc.printf("%.10f,%.10f,%.10f\n\r",MAV1,MAV2,MAV3); break; 00287 default: pc.printf("%.10f,%.10f,%.10f\n\r",EMG1,EMG2,EMG3); 00288 } 00289 } 00290 } 00291 00292 void IMU_Logger(void){ 00293 pc.printf("%.10f,%.10f,%.10f,%.10f,%.10f,%.10f,%.10f,%.10f,%.10f\r",ax_feat.l1,ax_feat.mean,ax_feat.var,ay_feat.l1,ay_feat.mean,ay_feat.var,az_feat.l1,az_feat.mean,az_feat.var); 00294 pc.printf("%.10f,%.10f,%.10f,%.10f,%.10f,%.10f,%.10f,%.10f,%.10f\r",R_feat.l1,R_feat.mean,R_feat.var,pitch_feat.l1,pitch_feat.mean,pitch_feat.var,roll_feat.l1,roll_feat.mean,roll_feat.var); 00295 } 00296 00297 imu_features imuGetFeatures(float32_t samples[25]){ 00298 imu_features feat; 00299 float32_t l1, mean, var; 00300 00301 /*L1*/ 00302 arm_abs_f32(samples, imu_abs_samples, 25); 00303 arm_dot_prod_f32(imu_abs_samples, imu_ones, 25, &l1); 00304 00305 /*MAV*/ 00306 arm_mean_f32(imu_abs_samples, 25, &mean); 00307 /*variance*/ 00308 arm_var_f32(samples, 25, &var); 00309 00310 feat.l1 = l1; 00311 feat.mean = mean; 00312 feat.var = var; 00313 00314 return feat; 00315 } 00316 00317 void IMU_Classifier(const float x1[18], float b_y1[4]){ 00318 float xp1[18]; 00319 int ixstart; 00320 static const double settings_gain[18] = { 0.0615861197203374, 1.53965323006161, 00321 0.857469392512132, 0.0600806679095755, 1.50201694590539, 1.23879030268757, 00322 0.0663679885953248, 1.65919957723595, 0.599294154820359, 0.113465456292823, 00323 2.83663399337079, 2.21904272848878, 0.053290743371231, 1.33226929425699, 00324 2.74309649260372, 0.0572933421985231, 1.43233365754206, 2.73777852926756 }; 00325 00326 static const double settings_xoffset[18] = { 3.13208, 0.1252832, 0.01800423, 00327 9.610352, 0.3844141, 0.06231475, 16.18701, 0.6474805, 0.008212686, 28.19626, 00328 1.12785, 0.06004226, 23.18947, 0.927579, 0.006299257, 12.31171, 0.4924683, 00329 0.02629572 }; 00330 00331 double b[45]; 00332 static const double a[45] = { -1.7580289769551161, -1.3423592326953466, 00333 1.5454917034757418, 1.6192469035519192, 1.1088397665812808, 00334 -1.3294886162934598, -1.5402696585790496, -1.0670236222833622, 00335 0.79657239712499928, -0.67248449362717955, -0.93265917892314076, 00336 0.51780467293444077, 0.75531292620200852, 0.68952452013799781, 00337 -0.9167788050264305, 0.40679456848166357, -0.78466204076547852, 00338 0.0746048711915274, -0.352749928885278, 0.017737649172833974, 00339 -0.038510072629115363, -0.20772701105519994, 0.45325062847723907, 00340 0.26044952985819775, 0.39677487553689345, -0.078712365946250917, 00341 -0.0825777988627018, -0.4727716865854944, 0.527910227195504, 00342 -0.74851602503119774, 0.63024557363169653, -0.58582825900364521, 00343 0.68849439469421991, -0.84750206038432641, -1.2000641131247758, 00344 -0.832240518854391, 1.0889450399496252, -1.3059529805809877, 00345 -1.4651416897453897, 1.3742855905702005, 1.2927860034088354, 00346 1.60109353014912, 1.4478939752003774, -1.6843870868642861, 00347 1.8617437390920597 }; 00348 00349 float b_b[45]; 00350 int ix; 00351 float denom; 00352 static const float fv0[810] = { 0.459632903F, 0.0611374F, -0.421426982F, 00353 0.148853645F, -0.177980483F, 0.247128889F, 0.214431226F, 0.451934367F, 00354 -0.532601058F, 0.0238353293F, 0.497370124F, -0.575586F, -0.499140948F, 00355 -0.0641773343F, 0.53166759F, 0.0778795481F, 0.225708947F, -0.498524398F, 00356 0.694334269F, -0.60842818F, 0.325595617F, 0.252601981F, 0.365996331F, 00357 0.384501338F, 0.00875493512F, -0.4245179F, -0.668958485F, -0.683882117F, 00358 0.181472883F, -0.426831633F, 0.297112852F, -0.583310187F, 0.456262171F, 00359 -0.459338129F, -0.641833425F, -0.459789753F, 0.204319239F, -0.186706707F, 00360 -0.285366446F, 0.640228748F, 0.0229709782F, -0.0642111823F, 0.553962111F, 00361 -0.33109349F, 0.247776791F, 0.443146557F, -0.542193532F, 0.119367629F, 00362 -0.451677382F, -0.786408F, 0.0928186F, 0.316778958F, 0.707230628F, 00363 -0.746820033F, -0.147853762F, 0.219747201F, -0.79193157F, -0.194468275F, 00364 -0.499102324F, 0.274141163F, 0.022611469F, 0.128222346F, -0.607686698F, 00365 0.48310256F, -0.593217313F, 0.536703944F, 0.2398123F, 0.150423944F, 00366 -0.336881399F, -0.769578874F, -0.308003962F, 0.29553327F, -0.83044821F, 00367 0.161142096F, -0.0343337208F, 0.581449211F, -0.873495698F, -0.28423211F, 00368 -0.78573F, 0.383646518F, -0.753481686F, 0.452582896F, 0.226794824F, 00369 0.375832409F, -0.498845309F, -0.258810967F, -0.499766558F, 0.404769868F, 00370 -0.0505502634F, 0.461976469F, -0.389700621F, -0.713240385F, -0.481977165F, 00371 -0.441101372F, 0.769881368F, 0.228649467F, 0.113842353F, -0.513090611F, 00372 0.57655704F, 0.00201666565F, -0.0780342743F, -0.0782211572F, -0.263174862F, 00373 -0.454693228F, -0.401941508F, -0.28711313F, -0.51956F, -0.413541466F, 00374 -0.0222236887F, -0.825855196F, 0.663189113F, 0.648147404F, -0.120558769F, 00375 -0.216713682F, -0.42545253F, 0.63155061F, -0.410026342F, -0.737376034F, 00376 0.199930415F, -0.214675143F, -0.65693754F, -0.323603958F, -0.831128061F, 00377 -0.548523843F, 0.398182899F, 0.475979269F, -0.111701392F, -0.558011353F, 00378 -0.246838376F, -0.176599175F, 0.494953543F, -0.930735171F, -0.485517114F, 00379 -0.660200715F, -0.151493624F, 0.239350051F, 0.50181222F, -0.0828042775F, 00380 -0.0873984F, -0.215416417F, 0.192907035F, -0.3374F, 0.213343263F, 00381 -0.400835454F, -0.206182152F, -0.0448735021F, 0.67194277F, -0.42755118F, 00382 1.01745749F, 0.661765158F, -0.494972438F, 0.0940658301F, 0.00391307333F, 00383 -0.407710791F, 0.106923632F, -0.206497744F, 0.754692852F, 0.176430732F, 00384 0.385327101F, 0.0846962854F, 0.323739618F, -0.151615232F, 0.174422458F, 00385 0.740742445F, 0.591877937F, 0.223434791F, 0.334855318F, 0.282185555F, 00386 -0.214522555F, -0.0105024902F, 0.273585677F, 0.114028879F, 0.543323517F, 00387 -0.291319072F, -0.432583898F, -0.821406F, -0.251906455F, -0.0459434502F, 00388 -0.455817074F, 0.483560562F, -0.182349116F, -0.171598256F, 0.561632752F, 00389 0.540451586F, 0.0060411403F, -0.653444648F, -0.236067981F, -0.182513028F, 00390 0.202278212F, -0.370738387F, -0.0789217F, 0.572875738F, -0.231134444F, 00391 -0.0330054797F, -0.0838245675F, -0.813296914F, 0.0739755705F, 0.118539378F, 00392 -0.72018075F, -0.368218392F, 0.207165778F, -0.267799765F, 0.378820896F, 00393 0.507289112F, 0.269455492F, 0.0232036524F, -0.524693549F, 0.325159252F, 00394 0.773608327F, 0.137095451F, 0.178058892F, -0.351133883F, 0.132823557F, 00395 0.34390527F, 0.495856106F, 0.183722347F, 0.328040898F, 0.716017127F, 00396 -0.492127508F, 0.573495567F, 0.306710184F, 0.602395654F, -0.475191712F, 00397 -0.550692558F, -0.337273896F, 0.232871071F, -0.414121956F, 0.338607788F, 00398 -0.689573646F, 0.449012369F, 0.100024827F, 0.379222423F, 0.189025134F, 00399 0.843987286F, 0.693846166F, -0.185747191F, 0.0946606621F, -0.453255206F, 00400 -1.33637404F, 0.19069238F, 0.126079515F, -0.0326298513F, 0.788161218F, 00401 0.135455295F, 0.336440802F, 0.425412089F, 0.102825224F, 0.63407582F, 00402 -0.270895481F, -0.720058799F, 0.695959747F, -0.023164358F, -0.0242635924F, 00403 -0.671196F, 0.554490209F, 0.190925688F, 0.272486627F, -0.0107945595F, 00404 -0.742727757F, 0.13697502F, -0.775040329F, -0.71145767F, 0.26798436F, 00405 -0.377285868F, 0.252919018F, 1.06671643F, -0.13240622F, -0.494904369F, 00406 0.453439087F, 0.132181391F, 0.668464959F, 0.501227736F, 0.424547732F, 00407 -0.280139863F, -0.019443851F, 0.542961895F, -0.512611568F, -0.559954345F, 00408 -0.339779437F, 0.0962056518F, 0.247770652F, 0.970269084F, -0.098800607F, 00409 0.0772119F, 0.165748924F, -0.425046772F, 0.372346252F, 0.561903596F, 00410 -0.465825468F, -0.00290585565F, -0.882256508F, 1.38634479F, -0.292974889F, 00411 -0.342612416F, 0.0741659775F, -0.297257066F, 0.423533231F, 0.0936212242F, 00412 0.603145778F, 0.517324328F, -0.739661038F, 0.644301593F, -0.291442573F, 00413 0.0685671866F, 0.0421827547F, 0.463616759F, 0.574609816F, 0.732411F, 00414 -0.103880793F, 0.14776741F, 0.350388736F, 0.034289483F, 0.228362814F, 00415 0.623231232F, -0.854294896F, -0.513297081F, 1.01049197F, -0.468677133F, 00416 -0.89124316F, 0.180666521F, 0.618507206F, 0.34617874F, -0.478432477F, 00417 -0.0531243756F, 0.204267785F, 0.472398907F, 0.0415514335F, 1.01171207F, 00418 0.394830078F, -0.0686556473F, -0.852999389F, -0.0204684064F, 0.2879785F, 00419 -0.70830971F, 0.301956505F, -0.581959546F, 0.608920038F, 0.334958166F, 00420 -0.136173159F, -0.063511923F, 0.243027285F, 0.140252739F, 0.769615531F, 00421 0.645489156F, 0.254678458F, -0.284347504F, 0.374404907F, -0.536988378F, 00422 0.717072845F, 0.486760736F, 0.31468308F, 0.329237908F, 0.265202075F, 00423 -0.460599929F, -0.273192674F, -0.18682602F, -0.533184588F, -0.433163792F, 00424 0.153697655F, -0.971417725F, 0.0716149211F, 0.388068318F, 0.304798931F, 00425 0.0493364856F, 0.525654316F, -0.734183729F, -0.418183059F, -0.391056359F, 00426 0.349859178F, 0.295310169F, -0.162787989F, 0.986191332F, 0.893915057F, 00427 0.468418807F, 0.227301195F, -0.893956F, -0.0392929763F, 0.547721744F, 00428 0.277446F, 0.0684096217F, -0.817378163F, 0.294014931F, -0.789526939F, 00429 -0.101080045F, 0.1247008F, 0.390003234F, 0.123767406F, 0.819431722F, 00430 -0.075544F, 0.692092419F, -0.023473734F, 1.12595487F, -0.0727373213F, 00431 0.185063079F, -0.127668485F, 0.0907674953F, 1.01107F, 0.103436872F, 00432 -0.104357071F, -0.612592816F, 0.118782118F, 0.294045866F, -0.182440594F, 00433 0.632002592F, 0.443847477F, -0.648681045F, 0.0449045971F, -0.132676274F, 00434 0.650167644F, -0.520409822F, -0.00697870459F, -0.0560558848F, -0.0724578276F, 00435 0.488136113F, -0.304092556F, 0.362224847F, 0.15520215F, -0.527926445F, 00436 -0.353179634F, 0.369286567F, -0.05207555F, -0.510196149F, -0.129422769F, 00437 0.190302268F, -0.544416964F, 0.247901723F, -0.173602447F, 0.65681529F, 00438 -0.372931182F, 0.239432991F, -0.3728562F, -0.289556086F, -0.465267807F, 00439 -0.0267323405F, 0.211196959F, -0.0809709206F, -0.211201906F, 0.0164073836F, 00440 0.211801484F, 0.28273347F, 0.24367331F, 0.712016702F, -0.232072771F, 00441 0.352156729F, -0.413937926F, -0.67757833F, -0.0740010217F, 0.0034886F, 00442 0.10386055F, 0.491702259F, 0.419249F, -0.321603328F, 0.258051783F, 00443 -0.0750256628F, 0.785783052F, 0.487906694F, -0.302251101F, 0.144713297F, 00444 0.212649152F, 0.0338810794F, 0.489597201F, -0.141343772F, -0.133599311F, 00445 -0.426484972F, 0.708699524F, 0.27392745F, 0.05124259F, 0.430232316F, 00446 0.101622045F, 0.335402131F, -0.0293640438F, 0.446573496F, 0.0604294352F, 00447 0.654577732F, -0.67903477F, -0.479712456F, -0.514000893F, -0.50363785F, 00448 -0.260914654F, -0.113499187F, -0.363629967F, 0.381589234F, -0.00877389684F, 00449 0.378615469F, 0.622865498F, 0.64521867F, -0.464941829F, -0.316285223F, 00450 0.33551228F, -0.631272078F, -0.0607743897F, -0.189647585F, 0.527016699F, 00451 0.154212683F, 0.116914578F, 0.183713183F, 0.346713036F, 0.0502914973F, 00452 -0.170981154F, -0.47873193F, -0.103974499F, -0.296331912F, -0.682696223F, 00453 0.418706745F, -0.702610731F, 0.113047369F, 0.11163982F, -0.219719663F, 00454 -0.265369058F, -0.304860383F, 0.247510567F, 0.605953753F, -0.221037015F, 00455 -0.479578376F, -0.364096701F, 0.232404232F, -0.923531771F, -0.607895613F, 00456 0.379731268F, -0.63160032F, 0.704049F, -0.623289347F, 0.211085469F, 00457 -0.201172546F, 0.496017128F, 0.111459091F, -0.216925532F, -0.456628412F, 00458 0.264619768F, 0.478987068F, -0.304841787F, 0.143561065F, 0.12293458F, 00459 0.41051206F, 0.242302909F, -0.755550683F, 0.268951803F, 0.669802606F, 00460 -0.444868356F, 1.0796355F, 0.0833751F, -0.494852066F, -0.434321523F, 00461 -0.0662497878F, 0.282435954F, 0.518994629F, 0.440311342F, -0.314596683F, 00462 -0.00901326071F, -0.16180931F, -0.645285F, -0.405436337F, 0.182191074F, 00463 0.295300335F, -0.322578758F, 0.196225166F, -0.502243817F, -0.786416531F, 00464 0.523073316F, 0.118213534F, 0.408718884F, -0.063704811F, 0.104132973F, 00465 0.582743F, 0.927500665F, 0.20945847F, -0.929123461F, 0.632320702F, 00466 0.172647551F, 0.653148234F, -0.099445492F, -0.0184663199F, 0.563922763F, 00467 -0.247307703F, 0.08971598F, 0.270614028F, 0.235931277F, 0.593309343F, 00468 0.293943584F, 0.189385429F, -0.65996927F, -0.21901688F, -0.583070755F, 00469 0.341855168F, 1.1340152F, 0.281593F, 0.575785697F, 0.676429689F, 0.459074F, 00470 0.0225899909F, 0.656224668F, 0.132585585F, -0.743888676F, -0.614794552F, 00471 1.03475749F, -0.0882811099F, 0.611714F, -0.473244578F, 0.369189858F, 00472 0.362521559F, -0.681019485F, 0.300381541F, -0.209409952F, -0.610560894F, 00473 -0.0188860334F, -0.641817153F, -0.807590604F, -0.465627879F, -0.365699679F, 00474 -0.231352642F, -1.01078808F, -0.127937913F, -0.181259409F, 0.0717183352F, 00475 0.24582836F, -0.144107684F, 0.0802998692F, 0.0219049808F, -0.386688113F, 00476 0.0793484673F, 0.213585958F, 0.703600287F, -0.0976952165F, -0.3840639F, 00477 0.157325551F, -0.0620508F, -0.593766451F, -0.427041501F, 0.962344646F, 00478 0.325952917F, -0.481346279F, -0.883274317F, -0.448941231F, 0.281245708F, 00479 0.416657F, -0.52693814F, 0.689424396F, 0.296314508F, 0.432173222F, 00480 -0.828454316F, 0.726714194F, 0.523919344F, 0.516204655F, 0.442432255F, 00481 0.45585233F, 0.376238436F, -0.461082667F, -0.151951969F, -0.449687511F, 00482 -0.836503446F, 0.659184694F, -0.29375577F, -0.535523951F, -0.468601644F, 00483 -0.293337F, 0.365438163F, -0.737874627F, 0.190893024F, -0.0833086F, 00484 0.492872238F, -0.434281528F, -0.282535583F, -0.805321336F, -0.503223121F, 00485 0.0819781274F, -0.249333F, -0.0285112094F, -0.352597207F, 0.724770725F, 00486 0.15054968F, 0.319918931F, 0.62603879F, -0.160482243F, -0.16062668F, 00487 -0.27161333F, 0.346072078F, 0.537718773F, -0.552904546F, -0.494646609F, 00488 -0.336943179F, -0.669887364F, -0.00652140286F, -0.222761601F, -0.604594052F, 00489 -0.723421872F, 0.733910799F, 0.463824034F, 0.26406005F, 0.419488341F, 00490 -0.69037348F, 0.441993326F, 0.737772286F, 0.243240789F, -0.262164325F, 00491 0.143315554F, -0.142399684F, -0.079456754F, -0.746082723F, 0.56054467F, 00492 -0.576788604F, -0.162682116F, -0.661055088F, -0.0100884913F, -0.84846276F, 00493 0.502910495F, -0.556128383F, -0.381545156F, -0.226393521F, -0.290395468F, 00494 -0.638160288F, 0.405303419F, 0.45708254F, -0.231018662F, -0.600171506F, 00495 -0.317634284F, -0.796222687F, -0.0962729752F, 0.302519232F, 0.227096081F, 00496 -0.697582F, -0.609304726F, 0.478130311F, 0.589749038F, 0.0213635098F, 00497 -0.726078153F, 0.357511342F, -0.27600044F, -0.282378614F, 0.232002884F, 00498 -1.03095603F, -0.546820879F, 0.453093469F, 0.452624887F, -0.0799274892F, 00499 0.28348428F, 0.36944288F, 0.815283835F, -0.270792842F, 0.45711121F, 00500 0.0650191829F, -0.00474550668F, -0.173524231F, 0.111019306F, -0.22976172F, 00501 0.324191928F, 0.480501801F, -0.596600354F, -0.44761616F, -0.116759337F, 00502 -0.0663144886F, 0.528708041F, 0.85675323F, 0.423383653F, -0.833274245F, 00503 -0.676730037F, -0.675398231F, 1.18571389F, 0.545059323F, 0.194837719F, 00504 -0.571213663F, 0.231315166F, -0.689943135F, -0.517713F, 0.245689139F, 00505 -0.36406526F, 0.159462124F, 0.377790302F, 0.15022406F, 0.203641087F, 00506 -0.231097415F, 0.311318755F, 0.771822393F, 0.402989596F, -0.383682787F, 00507 -0.434375495F, -0.156969324F, 0.271495759F, 0.100708269F, 0.0158944763F, 00508 -0.182064518F, -0.46497798F, -0.53833F, 0.636102617F, -0.416234672F, 00509 -0.0785247087F, -0.0448715724F, -0.431249201F, 0.146780565F, 0.031037325F, 00510 -0.75488F, 0.38843447F, -0.295456231F, -0.127952173F, -0.481856853F, 00511 0.130730167F, -0.694406748F, 0.0459047593F, -0.53592F, -0.676697969F, 00512 0.458953559F, -0.456679523F, 0.348396122F, 0.657496393F, 0.0483298F, 00513 0.143341869F, 0.715741456F, -0.366335869F, 0.218585283F, -0.794284F, 00514 0.618138731F }; 00515 00516 float x[45]; 00517 double c_b[4]; 00518 static const double b_a[4] = { 0.843573525672487, 0.52887395783527291, 00519 0.67012100803296748, 0.49760939396991682 }; 00520 00521 float numer[4]; 00522 static const float fv1[180] = { -0.576368749F, -0.461289287F, 0.359091967F, 00523 0.156713441F, 1.09660339F, -0.57649368F, -0.868483543F, -0.701349378F, 00524 0.437389016F, -0.0926702321F, 0.103165969F, -0.118973181F, -0.839078665F, 00525 -0.364395678F, 0.698953509F, 0.193260536F, -0.757779062F, -0.571485519F, 00526 -0.66037178F, -1.29905105F, -0.201243833F, 0.485816419F, 0.798221F, 00527 -0.417164803F, -0.831456721F, 0.145439208F, -0.624888301F, -0.686319172F, 00528 -1.01671267F, -0.952691317F, -0.346321493F, -0.474031448F, -0.466582507F, 00529 -0.434489608F, 0.0789648071F, -0.972864866F, 0.628803194F, 0.82423836F, 00530 0.392369598F, -1.1727767F, -0.51170367F, -0.79655838F, 0.487405777F, 00531 0.640642822F, 1.03092039F, -0.718461812F, -0.919755638F, -0.072865434F, 00532 1.09984982F, -0.727400839F, -0.270503879F, -0.476028502F, -0.577538967F, 00533 0.310665369F, -0.786185086F, 1.33968735F, -0.261592031F, 0.159126773F, 00534 -0.615301311F, -0.165091559F, -0.919524F, 0.589730442F, 0.887396574F, 00535 -0.275334418F, 0.369588107F, -0.644339323F, 0.131432697F, 0.828792572F, 00536 0.634907126F, -0.218690842F, -0.779631317F, -0.26734677F, -0.51404953F, 00537 -0.538019836F, 0.493474483F, 0.724827945F, -0.0821629688F, 0.126185417F, 00538 -0.86919862F, -1.1688292F, -0.788445175F, -0.743515372F, 1.08614969F, 00539 0.882474661F, 0.943030596F, 0.0328377932F, -0.917530715F, 0.737649441F, 00540 0.37147674F, 0.572349489F, 0.534060121F, -0.778717935F, 0.111810587F, 00541 -0.219216034F, -0.667796552F, -0.313792109F, 0.438846558F, -0.316282123F, 00542 -1.13036656F, 0.664084792F, 1.1458832F, -0.0636171848F, 0.464452535F, 00543 -1.01223803F, 0.634164453F, 0.317785263F, 0.492384404F, -1.02305794F, 00544 1.06032813F, 0.77377373F, -1.05769241F, -0.370299608F, -0.465102851F, 00545 0.62622422F, -0.640314877F, -0.148292065F, -0.208885193F, -0.709490895F, 00546 -1.15019488F, 0.829242766F, 0.120800421F, 0.694289267F, -0.382296205F, 00547 0.90905267F, 1.55740833F, -0.13578631F, -0.692770541F, -0.589448631F, 00548 0.397961974F, -0.523246586F, -0.814948142F, 0.564497709F, 0.0221325234F, 00549 1.3606391F, -0.130946055F, -0.356777936F, -0.273988575F, 0.733114958F, 00550 -0.371666F, 0.590279281F, 1.07341897F, 0.694256663F, -0.389202297F, 00551 0.186797455F, 1.00938177F, -0.705201328F, -0.808631361F, 1.14770544F, 00552 0.814412832F, -0.400106907F, 0.402769178F, -0.402200758F, 0.122468233F, 00553 0.0504386574F, -0.452429116F, -0.146729171F, -0.967114568F, -0.130777434F, 00554 -0.37407282F, 0.184210435F, -0.377413243F, 0.806694806F, 1.11887777F, 00555 -0.513802588F, -0.530572236F, 0.836555302F, -0.410351485F, 0.752652407F, 00556 -0.644252837F, -0.311244279F, -0.628207624F, 0.233606949F, -0.431879401F, 00557 -0.998878F, 0.732248843F, -0.708933234F, 0.0570094101F, -0.292068034F, 00558 1.09782875F, -1.47580659F }; 00559 00560 boolean_T exitg1; 00561 float fv2[1]; 00562 boolean_T b_x; 00563 00564 /* MYNEURALNETWORKFUNCTION neural network simulation function. */ 00565 /* */ 00566 /* Generated by Neural Network Toolbox function genFunction, 30-Oct-2015 21:46:01. */ 00567 /* */ 00568 /* [y1] = myNeuralNetworkFunction(x1) takes these arguments: */ 00569 /* x = 18xQ matrix, input #1 */ 00570 /* and returns: */ 00571 /* y = 4xQ matrix, output #1 */ 00572 /* where Q is the number of samples. */ 00573 /* ===== NEURAL NETWORK CONSTANTS ===== */ 00574 /* Input 1 */ 00575 /* Layer 1 */ 00576 /* Layer 2 */ 00577 /* ===== SIMULATION ======== */ 00578 /* Dimensions */ 00579 /* samples */ 00580 /* Input 1 */ 00581 /* ===== MODULE FUNCTIONS ======== */ 00582 /* Map Minimum and Maximum Input Processing Function */ 00583 for (ixstart = 0; ixstart < 18; ixstart++) { 00584 xp1[ixstart] = (x1[ixstart] - (float)settings_xoffset[ixstart]) * (float) 00585 settings_gain[ixstart] + -1.0F; 00586 } 00587 00588 /* Layer 1 */ 00589 memcpy(&b[0], &a[0], 45U * sizeof(double)); 00590 00591 /* Sigmoid Symmetric Transfer Function */ 00592 for (ix = 0; ix < 45; ix++) { 00593 denom = 0.0F; 00594 for (ixstart = 0; ixstart < 18; ixstart++) { 00595 denom += fv0[ix + 45 * ixstart] * xp1[ixstart]; 00596 } 00597 00598 b_b[ix] = (float)b[ix] + denom; 00599 } 00600 00601 for (ix = 0; ix < 45; ix++) { 00602 x[ix] = (real32_T)exp(-2.0F * b_b[ix]); 00603 } 00604 00605 /* Layer 2 */ 00606 for (ixstart = 0; ixstart < 4; ixstart++) { 00607 c_b[ixstart] = b_a[ixstart]; 00608 } 00609 00610 for (ix = 0; ix < 45; ix++) { 00611 b_b[ix] = 2.0F / (1.0F + x[ix]) - 1.0F; 00612 } 00613 00614 for (ix = 0; ix < 4; ix++) { 00615 denom = 0.0F; 00616 for (ixstart = 0; ixstart < 45; ixstart++) { 00617 denom += fv1[ix + (ixstart << 2)] * b_b[ixstart]; 00618 } 00619 00620 numer[ix] = (float)c_b[ix] + denom; 00621 } 00622 00623 /* Competitive Soft Transfer Function */ 00624 ixstart = 1; 00625 denom = numer[0]; 00626 if (rtIsNaNF(numer[0])) { 00627 ix = 2; 00628 exitg1 = false; 00629 while ((!exitg1) && (ix < 5)) { 00630 ixstart = ix; 00631 if (!rtIsNaNF(numer[ix - 1])) { 00632 denom = numer[ix - 1]; 00633 exitg1 = true; 00634 } else { 00635 ix++; 00636 } 00637 } 00638 } 00639 00640 if (ixstart < 4) { 00641 while (ixstart + 1 < 5) { 00642 if (numer[ixstart] > denom) { 00643 denom = numer[ixstart]; 00644 } 00645 00646 ixstart++; 00647 } 00648 } 00649 00650 for (ixstart = 0; ixstart < 4; ixstart++) { 00651 numer[ixstart] = (real32_T)exp(numer[ixstart] - denom); 00652 } 00653 00654 denom = numer[0]; 00655 for (ixstart = 0; ixstart < 3; ixstart++) { 00656 denom += numer[ixstart + 1]; 00657 } 00658 00659 fv2[0] = denom; 00660 b_x = (denom == 0.0F); 00661 ixstart = 0; 00662 if (b_x) { 00663 ixstart = 1; 00664 } 00665 00666 ix = 0; 00667 while (ix <= ixstart - 1) { 00668 fv2[0] = 1.0F; 00669 ix = 1; 00670 } 00671 00672 for (ixstart = 0; ixstart < 4; ixstart++) { 00673 b_y1[ixstart] = numer[ixstart] / fv2[0]; 00674 } 00675 00676 /* Output 1 */ 00677 } 00678 00679 boolean_T rtIsNaNF(real32_T value){ 00680 #if defined(_MSC_VER) && (_MSC_VER <= 1200) 00681 return _isnan((real_T)value)? true:false; 00682 #else 00683 return (value!=value)? 1U:0U; 00684 #endif 00685 }
Generated on Fri Jul 15 2022 02:31:30 by 1.7.2