calculate.cpp

00001 #include "calculate.h"
00002 
00003 
00004 unsigned int send_enable = 0;
00005 unsigned int init_joint_angles_flag = 0;
00006 float pos_ankle = 0.0f;
00007 float pos_knee = 0.0f;
00008 target_pid tagertPID;
00009 
00010 
00011 float position_knee(float res)
00012 {   
00013     float x = res*100/3.0f;      //计算步态周期百分比   每一个步态周期长度为3s
00014     
00015     if (mode_flag==8){      
00016         x = res*100/2.0f;        //快速行走步态周期长度为2s
00017     }
00018     
00019     
00020     float a = 0.0f;
00021 
00022 
00023     /*
00024       判断运动模式
00025     */
00026     if(mode_flag==1) {
00027         //坐
00028         tagertPID.knee_target_Kp = 150; 
00029         tagertPID.knee_target_Kd = 0;
00030         tagertPID.knee_target_tff = 0;
00031         a = 0.8308f - 0.7549f*cos(0.04647f*x) - 0.555f*sin(0.04647f*x) \
00032             - 0.01085f*cos(2*0.04647f*x) - 0.06257f*sin(2*0.04647f*x) \
00033             - 0.0555f*cos(3*0.04647f*x) + 0.1742f*sin(3*0.04647f*x) \
00034             - 0.0227f*cos(4*0.04647f*x) + 0.01162f*sin(4*0.04647f*x) \
00035             + 0.0292f*cos(5*0.04647f*x) - 0.0004315f*sin(5*0.04647f*x);
00036     } else if(mode_flag==2) {
00037         //起
00038         tagertPID.knee_target_Kp = 150; 
00039         tagertPID.knee_target_Kd = 0;
00040         tagertPID.knee_target_tff = 0;
00041         a =  0.9133f + 0.5947f*cos(0.04688f*x) + 0.7276f*sin(0.04688f*x) \
00042             + 0.007792f*cos(2*0.04688f*x) - 0.0688f*sin(2*0.04688f*x) \
00043             + 0.1629f*cos(3*0.04688f*x) - 0.0822f*sin(3*0.04688f*x) \
00044             - 0.02656f*cos(4*0.04688f*x) - 0.008309f*sin(4*0.04688f*x) \
00045             - 0.007829f*cos(5*0.04688f*x) - 0.02764f*sin(5*0.04688f*x);
00046     } else if(mode_flag==3) {
00047         //平地
00048         tagertPID.knee_target_Kp = 180; 
00049         tagertPID.knee_target_Kd = 0;
00050         tagertPID.knee_target_tff = 0;
00051         
00052         a = 0.2718f - 0.03479f*cos(0.06021f*x) - 0.2809f*sin(0.06021f*x) \
00053             - 0.1804f*cos(2*0.06021f*x) + 0.09041f*sin(2*0.06021f*x) \
00054             + 0.001454f*cos(3*0.06021f*x) + 0.06005f*sin(3*0.06021f*x);
00055             
00056 
00057     } else if(mode_flag==4) {
00058         //上坡
00059         tagertPID.knee_target_Kp = 180; 
00060         tagertPID.knee_target_Kd = 0;
00061         tagertPID.knee_target_tff = 0;
00062         a = 3.099f + 1.654f*cos(0.03788f*x) - 4.737f*sin(0.03788f*x) \
00063             - 2.759f*cos(2*0.03788f*x) - 2.703f*sin(2*0.03788f*x) \
00064             - 2.541f*cos(3*0.03788f*x) + 1.4f*sin(3*0.03788f*x) \
00065             + 0.1508f*cos(4*0.03788f*x) + 1.524f*sin(4*0.03788f*x)\
00066             + 0.6595f*cos(5*0.03788f*x) + 0.2162f*sin(5*0.03788f*x)\
00067             +  0.1359f*cos(6*0.03788f*x) - 0.1806f*sin(6*0.03788f*x)\
00068             - 0.02243f*cos(7*0.03788f*x) - 0.04809f*sin(7*0.03788f*x);
00069     } else if(mode_flag==5) {
00070         //下坡
00071         tagertPID.knee_target_Kp = 180; 
00072         tagertPID.knee_target_Kd = 0;
00073         tagertPID.knee_target_tff = 0;        
00074         a = 0.6109f - 0.2748f*cos(0.06062f*x) -  0.2946f*sin(0.06062f*x) \
00075             - 0.2302f*cos(2*0.06062f*x) + 0.1319f*sin(2*0.06062f*x) \
00076             + 0.02774f*cos(3*0.06062f*x) + 0.04863f*sin(3*0.06062f*x) \
00077             + 0.01865f*cos(4*0.06062f*x) -  0.01827f*sin(4*0.06062f*x) \
00078             + 0.009155f*cos(5*0.06062f*x) -  0.008681f*sin(5*0.06062f*x) \
00079             + 0.008983f*cos(6*0.06062f*x) -  0.01089f*sin(6*0.06062f*x) \
00080             - 0.00243f*cos(7*0.06062f*x) -  0.004621f*sin(7*0.06062f*x);
00081 
00082     } else if(mode_flag==6) {
00083         //上楼
00084         tagertPID.knee_target_Kp = 180; 
00085         tagertPID.knee_target_Kd = 0;
00086         tagertPID.knee_target_tff = 0;         
00087         a = 0.6949f + 0.2931f*cos(0.064f*x) -  0.4634f*sin(0.064f*x) \
00088              - 0.1563f*cos(2*0.064f*x) + 0.01809f*sin(2*0.064f*x) \
00089              + 0.01518f*cos(3*0.064f*x) + 0.02992f*sin(3*0.064f*x);
00090 
00091     } else if(mode_flag==7) {
00092         //下楼
00093         tagertPID.knee_target_Kp = 180; 
00094         tagertPID.knee_target_Kd = 0;
00095         tagertPID.knee_target_tff = 0;         
00096         a = 0.5695f - 0.2778f*cos(x*0.0614f) - 0.3586f*sin(x*0.0614f) \
00097             - 0.2202f*cos(2*x*0.0614f) + 0.2438f*sin(2*x*0.0614f)    \
00098             + 0.05218f*cos(3*x*0.0614f) + 0.03495f*sin(3*x*0.0614f)  \
00099             - 0.00463f*cos(4*x*0.0614f) - 0.05175f*sin(4*x*0.0614f);
00100     } else if(mode_flag==8) {
00101         tagertPID.knee_target_Kp = 180; 
00102         tagertPID.knee_target_Kd = 0;
00103         tagertPID.knee_target_tff = 0; 
00104         a = 0.3713f - 0.01673f*cos(x*0.06085f) - 0.3617f*sin(x*0.06085f) \
00105             - 0.26f*cos(2*x*0.06085f) + 0.09466f*sin(2*x*0.06085f)      \
00106             - 0.01627f*cos(3*x*0.06085f) + 0.07981f*sin(3*x*0.06085f);
00107     }
00108 
00109     return a;
00110 }
00111 
00112 float position_ankle(float res)
00113 {
00114 
00115     float a = 0.0f;
00116     float x = res*100/3.0f;      
00117     
00118     if (mode_flag==8){
00119         x = res*100/2.0f;
00120     }
00121     
00122     if(mode_flag == 1) {
00123         //坐
00124         tagertPID.ankle_target_Kp = 0; 
00125         tagertPID.ankle_target_Kd = 0;
00126         tagertPID.ankle_target_tff = 0;         
00127         a = 0.0f;
00128     } else if(mode_flag == 2) {
00129         //起
00130         tagertPID.ankle_target_Kp = 0; 
00131         tagertPID.ankle_target_Kd = 0;
00132         tagertPID.ankle_target_tff = 0;   
00133         a = 0.0f;
00134     } else if(mode_flag == 3) {
00135         //平地
00136         tagertPID.ankle_target_Kp = 220; 
00137         tagertPID.ankle_target_Kd = 1;
00138         tagertPID.ankle_target_tff = 0; 
00139         a =  0.05373f - 0.005853f*cos(0.05922f*x) + 0.1164f*sin(0.05922f*x) \
00140                 + 0.02177f*cos(2*0.05922f*x) - 0.08126f*sin(2*0.05922f*x) \
00141                 - 0.05912f*cos(3*0.05922f*x) + 0.01009f*sin(3*0.05922f*x) \
00142                 + 0.02115f*cos(4*0.05922f*x) + 0.001152f*sin(4*0.05922f*x) \
00143                 - 0.00824f*cos(5*0.*x) - 0.01898f*sin(5*0.05922f*x);
00144 
00145     } else if(mode_flag == 4) {
00146         //上坡
00147         tagertPID.ankle_target_Kp = 220; 
00148         tagertPID.ankle_target_Kd = 0;
00149         tagertPID.ankle_target_tff = 0; 
00150         a = 0.2081f + 0.1834f*cos(x*0.05377f) + 0.0957f*sin(x*0.05377f) \
00151             - 0.125f*cos(2*x*0.05377f) - 0.1071f*sin(2*x*0.05377f)      \
00152             + 0.0755f*cos(3*x*0.05377f) + 0.01942f*sin(3*x*0.05377f)    \
00153             - 0.04593f*cos(4*x*0.05377f) - 0.04182f*sin(4*x*0.05377f)   \
00154             + 0.02662f*cos(5*x*0.05377f) - 0.005419f*sin(5*x*0.05377f)  \
00155             - 0.01759f*cos(6*x*0.05377f) - 0.006702f*sin(6*x*0.05377f)  \
00156             + 0.003964f*cos(7*x*0.05377f) - 0.004773f*sin(7*x*0.05377f) \
00157             - 0.007487f*cos(8*x*0.05377f) + 0.0009378f*sin(8*x*0.05377f);
00158 
00159     } else if(mode_flag == 5) {
00160         //下坡
00161         tagertPID.ankle_target_Kp = 220; 
00162         tagertPID.ankle_target_Kd = 0;
00163         tagertPID.ankle_target_tff = 0; 
00164         a = 0.07331f + -0.1611f*cos(0.06333f*x) - 0.01543f*sin(0.06333f*x) \
00165             +  0.05741f*cos(2*0.06333f*x) - 0.0004983f*sin(2*0.06333f*x) \
00166             + 0.002168f*cos(3*0.06333f*x) - 0.07856f*sin(3*0.06333f*x) \
00167             - 0.01604f*cos(4*0.06333f*x) + 0.0007893f*sin(4*0.06333f*x) \
00168             + 0.02003f*cos(5*0.06333f*x) + 0.005234f*sin(5*0.06333f*x) \
00169             + 0.00913f*cos(6*0.06333f*x) + 0.0001917f*sin(6*0.06333f*x) \
00170             + 0.0007703f*cos(7*0.06333f*x) + 0.002357f*sin(7*0.06333f*x);
00171 
00172     } else if(mode_flag == 6) {
00173         //上楼
00174         tagertPID.ankle_target_Kp = 280; 
00175         tagertPID.ankle_target_Kd = 1;
00176         tagertPID.ankle_target_tff = 0; 
00177         a = 0.3354f + 0.1043f*cos(0.04022f*x) - 0.2226f*sin(0.04022f*x) \
00178             - 0.01181f*cos(2*0.04022f*x) - 0.1101f*sin(2*0.04022f*x) \
00179             - 0.2017f*cos(3*0.04022f*x) + 0.003176f*sin(3*0.04022f*x) \
00180             - 0.03876f*cos(4*0.04022f*x) + 0.09576f*sin(4*0.04022f*x) \
00181             + 0.04748f*cos(5*0.04022f*x) + 0.09573f*sin(5*0.04022f*x) \
00182             + 0.0475f*cos(6*0.04022f*x) - 0.009034f*sin(6*0.04022f*x) \
00183             + 0.01196f*cos(7*0.04022f*x) - 0.02833f*sin(7*0.04022f*x) \
00184             - 0.007736f*cos(8*0.04022f*x) + 0.001967f*sin(8*0.04022f*x);
00185     } else if(mode_flag == 7) {
00186         //下楼
00187         tagertPID.ankle_target_Kp = 220; 
00188         tagertPID.ankle_target_Kd = 1;
00189         tagertPID.ankle_target_tff = 0; 
00190         a = 0.1254f - 0.3401f*cos(x*0.0573f) + 0.1949f*sin(x*0.0573f) \
00191             - 0.01378f*cos(2*x*0.0573f) + 0.0842f*sin(2*x*0.0573f)     \
00192             - 0.08164f*cos(3*x*0.0573f) + 0.05474f*sin(3*x*0.0573f)    \
00193             + 0.01607f*cos(4*x*0.0573f) - 0.01914f*sin(4*x*0.0573f)    \
00194             - 0.04177f*cos(5*x*0.0573f) - 0.01208f*sin(5*x*0.0573f);
00195     } else if(mode_flag == 8) {
00196         //快速
00197         tagertPID.ankle_target_Kp = 300; 
00198         tagertPID.ankle_target_Kd = 1;
00199         tagertPID.ankle_target_tff = 0; 
00200         if( x <= 72.54f){
00201             a = 0 - 0.154f + -0.2819f*cos(0.05937f*x) + 0.5131f*sin(0.05937f*x) \
00202                 + 0.2204f*cos(2*0.05937f*x) + 0.263f*sin(2*0.05937f*x) \
00203                 + 0.21f*cos(3*0.05937f*x) + 0.008273f*sin(3*0.05937f*x) \
00204                 + 0.1431f*cos(4*0.05937f*x) - 0.1179f*sin(4*0.05937f*x) \
00205                 - 0.005844f*cos(5*0.05937f*x) - 0.1348f*sin(5*0.05937f*x) \
00206                 - 0.05481f*cos(6*0.05937f*x) - 0.02825f*sin(6*0.05937f*x) \
00207                 - 0.01024f*cos(7*0.05937f*x) + 0.008747f*sin(7*0.05937f*x);
00208         }else{
00209             a = 0.01567f + 0.05387f*cos(0.1041f*x) - 0.2137f*sin(0.1041f*x) \
00210                 + 0.08837f*cos(2*0.1041f*x) - 0.08757f*sin(2*0.1041f*x) \
00211                 + 0.00966f*cos(3*0.1041f*x) - 0.02229f*sin(3*0.1041f*x);
00212         }
00213     }
00214     return a;
00215 }
00216 
00217 
00218 
00219 void calculate_fuzzy(float ankle_real_p, float knee_real_p)
00220 {
00221     send_enable = 1;    // can可以发送数据
00222     
00223     //初始化关节角度
00224     if(init_joint_angles_flag == 0){
00225         init_joint_angles_flag = 1;
00226         init_joint_angles();
00227     }
00228     
00229     
00230     tim_time =  tim.read();
00231 //    pc.printf("%.3f\n",tim_time);    
00232     
00233     
00234     if(Gait_reset_flag == 1){
00235         Gait_reset_flag = 0;       
00236         tim.reset();
00237     }
00238     
00239     if (mode_flag==8){
00240        if(tim_time >= 2){
00241             send_enable = 0;
00242         }else{        
00243             calculate_ankle_fuzzy(ankle_real_p, knee_real_p);        
00244         } 
00245     }else if (mode_flag!=8){
00246        if(tim_time >= 3){
00247             send_enable = 0;
00248         }else{        
00249             calculate_ankle_fuzzy(ankle_real_p, knee_real_p);        
00250         } 
00251     }
00252 }
00253 
00254 
00255 ////////////////////////////////////////////////////////////////////////////////
00256 //                              初始化关节角度                                 //
00257 ////////////////////////////////////////////////////////////////////////////////
00258 
00259 void init_joint_angles(){
00260     
00261     a_control.ankle.p_des = position_ankle(3.0f);
00262     a_control.ankle.v_des = 0;
00263     a_control.ankle.kp    = 80;
00264     a_control.ankle.kd    = 0;
00265     a_control.ankle.t_ff  = 0;
00266         
00267     a_control.knee.p_des = position_knee(3.0f);
00268     a_control.knee.v_des = 0;
00269     a_control.knee.kp    = 80;
00270     a_control.knee.kd    = 0;
00271     a_control.knee.t_ff  = 0;
00272     
00273     PackAll();
00274     WriteAll(); 
00275     
00276     wait(3);
00277 }
00278 
00279 
00280 
00281 //    Gait_per_now_real     当前步态百分比
00282 
00283 float ank_e = 0.0f, ank_before_e = 0.0f, ank_ec = 0.0f;
00284 float kn_e = 0.0f, kn_before_e = 0.0f, kn_ec = 0.0f;
00285 
00286 unsigned int fuzzy50 = 1;
00287 void calculate_ankle_fuzzy(float ankle_real_p, float knee_real_p)
00288 {
00289     pos_ankle = position_ankle(tim.read());
00290     pos_knee = position_knee(tim.read());
00291     //膝关节
00292     if(Gait_now_real==1 || Gait_now_real==4) {
00293         fuzzy_position_control_ankle(ankle_real_p, pos_ankle);
00294         fuzzy_position_control_knee(knee_real_p, pos_knee);
00295         //膝关节
00296     } else if(Gait_now_real==2 || Gait_now_real==3) {
00297         fuzzy_position_control_ankle(ankle_real_p, pos_ankle);
00298         fuzzy_position_control_knee(knee_real_p, pos_knee);
00299     }
00300 }
00301 
00302 
00303 
00304 ////////////////////////////////////////////////////////////////////////////////
00305 //////////////////////////////////踝关节模糊控制/////////////////////////////////
00306 void fuzzy_position_control_ankle(float ankle_real_p, float pos_ankle)
00307 {
00308 
00309     ank_e = ankle_real_p - pos_ankle;    // 实际值 - 参考值
00310 
00311     if(ank_e > 0.3f*ankleFuzzy.quantied_e)
00312         ank_e = 0.3f*ankleFuzzy.quantied_e;
00313     if(ank_e <= (0.0f - 0.3f*ankleFuzzy.quantied_e))
00314         ank_e = 0.0f - 0.3f*ankleFuzzy.quantied_e;
00315 
00316     ank_ec =  ank_e - ank_before_e;
00317 
00318     if(ank_ec > 0.3f*ankleFuzzy.quantied_ec)
00319         ank_ec = 0.3f*ankleFuzzy.quantied_ec;
00320     if(ank_ec <= (0.0f - 0.3f*ankleFuzzy.quantied_ec))
00321         ank_ec = 0.0f - 0.3f*ankleFuzzy.quantied_ec;
00322 
00323     ank_before_e = ank_ec;
00324 
00325 
00326     if(fuzzy50 == 1) {                                                          // 进入模糊计算，每10ms计算一次，设初始值为1，则可以第一次进入
00327         fuzzytimer.reset();                                                     // 计时清0
00328         fuzzy_control(ank_e, ank_ec, &ankleFuzzy,&FuzzyRuleKp[0], &FuzzyRuleKd[0]);
00329     }//出模糊计算
00330     cal_command.q_des_ankle = pos_ankle;
00331     cal_command.qd_des_ankle = 0;
00332     cal_command.kp_ankle = ankleFuzzy.outKp + tagertPID.ankle_target_Kp;
00333     cal_command.kd_ankle = ankleFuzzy.outKd + tagertPID.ankle_target_Kd;
00334     cal_command.tau_ankle_ff = 0;
00335     pc.printf("outKp:%f\toutKd:%f\tpos_ankle:%f\tankle_real_p:%f\ttime:%f\n", cal_command.kp_ankle, cal_command.kd_ankle, pos_ankle, ankle_real_p, tim.read());
00336 }
00337 
00338 void torque_control_ankle(){
00339 
00340 }
00341 
00342 ////////////////////////////////////////////////////////////////////////////////
00343 
00344 ////////////////////////膝关节模糊控制//////////////////////////////
00345 void fuzzy_position_control_knee(float knee_real_p, float pos_knee){
00346 
00347     kn_e = knee_real_p - pos_knee;    // 实际值 - 参考值
00348 
00349     if(kn_e > 0.3f*kneeFuzzy.quantied_e)
00350         kn_e = 0.3f*kneeFuzzy.quantied_e;
00351     if(kn_e <= (0.0f - 0.3f*kneeFuzzy.quantied_e))
00352         kn_e = 0.0f - 0.3f*kneeFuzzy.quantied_e;
00353 
00354     kn_ec =  kn_e - kn_before_e;
00355 
00356     if(kn_ec > 0.3f*kneeFuzzy.quantied_ec)
00357         kn_ec = 0.3f*kneeFuzzy.quantied_ec;
00358     if(kn_ec <= (0.0f - 0.3f*kneeFuzzy.quantied_ec))
00359         kn_ec = 0.0f - 0.3f*kneeFuzzy.quantied_ec;
00360 
00361     kn_before_e = kn_ec;
00362 
00363     if(fuzzy50 == 1) {                                              // 进入模糊计算，每10ms计算一次，设初始值为1，则可以第一次进入
00364         fuzzy50 = 0;
00365         fuzzy_control(kn_e, kn_ec, &kneeFuzzy, &FuzzyRuleKp[0], &FuzzyRuleKd[0]);
00366     }//出模糊计算
00367     
00368     Tfuzzy = fuzzytimer.read_ms();
00369     if(Tfuzzy >= CT)
00370         fuzzy50 = 1;            // 间隔10ms后可进入
00371     cal_command.q_des_knee = pos_knee;
00372     cal_command.qd_des_knee = 0;
00373     cal_command.kp_knee = kneeFuzzy.outKp + tagertPID.knee_target_Kp;
00374     cal_command.kd_knee = kneeFuzzy.outKd + tagertPID.knee_target_Kd;
00375     cal_command.tau_knee_ff = 0;
00376     //pc.printf("outKp:%f\toutKd:%f\tpos_knee:%f\tknee_real_p:%f\ttime:%f\n", cal_command.kp_knee, cal_command.kd_knee, pos_knee, knee_real_p, tim.read());
00377 }
00378 
00379 void torque_control_knee(){
00380 
00381 }
00382 
00383 
00384 
00385