calculate.cpp

00001 #include "calculate.h"
00002 
00003 
00004 ///////////计时函数////////////////
00005 float timeRecord()
00006 {
00007     float s =tim.read();
00008     if(mode_flag == 8) {
00009         if(s >= 2.0f) {
00010             tim.reset();
00011         }
00012     } else if(mode_flag==1){
00013         if(s >= 4.0f) {
00014             tim.reset();
00015             wait_ms(20);
00016             mode_flag = 2;
00017             send_enable = 0;
00018         }
00019     }else if(mode_flag==2){
00020         if(s >= 4.0f) {
00021             tim.reset();
00022             wait_ms(20);
00023             mode_flag = 1;
00024             send_enable = 0;
00025         }
00026     }else{
00027         if(s >= 3.0f) {
00028             tim.reset();
00029         }
00030     }
00031     return s;
00032 }
00033 
00034 
00035 
00036 unsigned int send_enable = 0;
00037 float pos_ankle = 0.0f;
00038 float pos_knee = 0.0f;
00039 
00040 
00041 float position_knee(float res)
00042 {
00043     float x = res*100/3.0f;      //可能不太合适
00044     float a = 0.0f;
00045 
00046     if(mode_flag==1) {
00047         //坐
00048         x = res*100/4.0f;
00049         a = 0.8308f - 0.7549f*cos(0.04647f*x) - 0.555f*sin(0.04647f*x) \
00050             - 0.01085f*cos(2*0.04647f*x) - 0.06257f*sin(2*0.04647f*x) \
00051             - 0.0555f*cos(3*0.04647f*x) + 0.1742f*sin(3*0.04647f*x) \
00052             - 0.0227f*cos(4*0.04647f*x) + 0.01162f*sin(4*0.04647f*x) \
00053             + 0.0292f*cos(5*0.04647f*x) - 0.0004315f*sin(5*0.04647f*x);
00054     } else if(mode_flag==2) {
00055         //起
00056         x = res*100/4.0f;
00057         a =  0.8303f + 0.5947f*cos(0.04688f*x) + 0.7276f*sin(0.04688f*x) \
00058             + 0.007792f*cos(2*0.04688f*x) - 0.0688f*sin(2*0.04688f*x) \
00059             + 0.1629f*cos(3*0.04688f*x) - 0.0822f*sin(3*0.04688f*x) \
00060             - 0.02656f*cos(4*0.04688f*x) - 0.008309f*sin(4*0.04688f*x) \
00061             - 0.007829f*cos(5*0.04688f*x) - 0.02764f*sin(5*0.04688f*x);
00062     } else if(mode_flag==3) {
00063         //平地
00064         a = 0.3713f - 0.01673f*cos(x*0.06085f) - 0.3617f*sin(x*0.06085f) \
00065             - 0.26f*cos(2*x*0.06085f) + 0.09466f*sin(2*x*0.06085f)      \
00066             - 0.01627f*cos(3*x*0.06085f) + 0.07981f*sin(3*x*0.06085f);
00067     } else if(mode_flag==4) {
00068         //上坡
00069         a = 3.099f + 1.654f*cos(0.03788f*x) - 4.737f*sin(0.03788f*x) \
00070             - 2.759f*cos(2*0.03788f*x) - 2.703f*sin(2*0.03788f*x) \
00071             - 2.541f*cos(3*0.03788f*x) + 1.4f*sin(3*0.03788f*x) \
00072             + 0.1508f*cos(4*0.03788f*x) + 1.524f*sin(4*0.03788f*x)\
00073             + 0.6595f*cos(5*0.03788f*x) + 0.2162f*sin(5*0.03788f*x)\
00074             +  0.1359f*cos(6*0.03788f*x) - 0.1806f*sin(6*0.03788f*x)\
00075             - 0.02243f*cos(7*0.03788f*x) - 0.04809f*sin(7*0.03788f*x);
00076     } else if(mode_flag==5) {
00077         //下坡
00078         a = 0.6109f + -0.2748f*cos(0.06062f*x) -  0.2946f*sin(0.06062f*x) \
00079             - 0.2302f*cos(2*0.06062f*x) + 0.1319f*sin(2*0.06062f*x) \
00080             + 0.02774f*cos(3*0.06062f*x) + 0.04863f*sin(3*0.06062f*x) \
00081             + 0.01865f*cos(4*0.06062f*x) -  0.01827f*sin(4*0.06062f*x) \
00082             + 0.009155f*cos(5*0.06062f*x) -  0.008681f*sin(5*0.06062f*x) \
00083             + 0.008983f*cos(6*0.06062f*x) -  0.01089f*sin(6*0.06062f*x) \
00084             - 0.00243f*cos(7*0.06062f*x) -  0.004621f*sin(7*0.06062f*x);
00085 
00086     } else if(mode_flag==6) {
00087         //上楼
00088         a = 0.6949f + 0.2931f*cos(0.064f*x) -  0.4634f*sin(0.064f*x) \
00089              - 0.1563f*cos(2*0.064f*x) + 0.01809f*sin(2*0.064f*x) \
00090              + 0.01518f*cos(3*0.064f*x) + 0.02992f*sin(3*0.064f*x);
00091 
00092     } else if(mode_flag==7) {
00093         //下楼
00094         a = 0.5695f - 0.2778f*cos(x*0.0614f) - 0.3586f*sin(x*0.0614f) \
00095             - 0.2202f*cos(2*x*0.0614f) + 0.2438f*sin(2*x*0.0614f)    \
00096             + 0.05218f*cos(3*x*0.0614f) + 0.03495f*sin(3*x*0.0614f)  \
00097             - 0.00463f*cos(4*x*0.0614f) - 0.05175f*sin(4*x*0.0614f);
00098     } else if(mode_flag==8) {
00099         x = res*100/2.0f;
00100         a = 0.3713f - 0.01673f*cos(x*0.06085f) - 0.3617f*sin(x*0.06085f) \
00101             - 0.26f*cos(2*x*0.06085f) + 0.09466f*sin(2*x*0.06085f)      \
00102             - 0.01627f*cos(3*x*0.06085f) + 0.07981f*sin(3*x*0.06085f);
00103     }
00104 
00105     return a;
00106 }
00107 
00108 float position_ankle(float res)
00109 {
00110     float x = res*100/3.0f;  //可能不太合适
00111     float a = 0.0f;
00112     if(mode_flag == 1) {
00113         //坐
00114         a = 0.0f;
00115     } else if(mode_flag == 2) {
00116         //起
00117         a = 0.0f;
00118     } else if(mode_flag == 3) {
00119         //平地
00120         if( x <= 72.54f){
00121             a = 0 - 0.154f + -0.2819f*cos(0.05937f*x) + 0.5131f*sin(0.05937f*x) \
00122                 + 0.2204f*cos(2*0.05937f*x) + 0.263f*sin(2*0.05937f*x) \
00123                 + 0.21f*cos(3*0.05937f*x) + 0.008273f*sin(3*0.05937f*x) \
00124                 + 0.1431f*cos(4*0.05937f*x) - 0.1179f*sin(4*0.05937f*x) \
00125                 - 0.005844f*cos(5*0.05937f*x) - 0.1348f*sin(5*0.05937f*x) \
00126                 - 0.05481f*cos(6*0.05937f*x) - 0.02825f*sin(6*0.05937f*x) \
00127                 - 0.01024f*cos(7*0.05937f*x) + 0.008747f*sin(7*0.05937f*x);
00128         }else{
00129             a = 0.01567f + 0.05387f*cos(0.1041f*x) - 0.2137f*sin(0.1041f*x) \
00130                 + 0.08837f*cos(2*0.1041f*x) - 0.08757f*sin(2*0.1041f*x) \
00131                 + 0.00966f*cos(3*0.1041f*x) - 0.02229f*sin(3*0.1041f*x);
00132         }
00133         
00134         
00135 //        a = 0.07088f - 0.02461f*cos(x*0.06048f) + 0.1543f *sin(x*0.06048f)\
00136 //            + 0.05244f*cos(2*x*0.06048f) - 0.09977f*sin(2*x*0.06048f)    \
00137 //            - 0.07853f*cos(3*x*0.06048f) - 0.01301f*sin(3*x*0.06048f)    \
00138 //            + 0.02483f*cos(4*x*0.06048f) + 0.01296f*sin(4*x*0.06048f)    \
00139 //            + 0.003777f*cos(5*x*0.06048f) - 0.0268f*sin(5*x*0.06048f);      
00140     } else if(mode_flag == 4) {
00141         //上坡
00142         a = 0.2081f + 0.1834f*cos(x*0.05377f) + 0.0957f*sin(x*0.05377f) \
00143             - 0.125f*cos(2*x*0.05377f) - 0.1071f*sin(2*x*0.05377f)      \
00144             + 0.0755f*cos(3*x*0.05377f) + 0.01942f*sin(3*x*0.05377f)    \
00145             - 0.04593f*cos(4*x*0.05377f) - 0.04182f*sin(4*x*0.05377f)   \
00146             + 0.02662f*cos(5*x*0.05377f) - 0.005419f*sin(5*x*0.05377f)  \
00147             - 0.01759f*cos(6*x*0.05377f) - 0.006702f*sin(6*x*0.05377f)  \
00148             + 0.003964f*cos(7*x*0.05377f) - 0.004773f*sin(7*x*0.05377f) \
00149             - 0.007487f*cos(8*x*0.05377f) + 0.0009378f*sin(8*x*0.05377f);
00150 
00151     } else if(mode_flag == 5) {
00152         //下坡
00153         a = 0.07331f - 0.1611f*cos(0.06333f*x) - 0.01543f*sin(0.06333f*x) \
00154             +  0.05741f*cos(2*0.06333f*x) - 0.0004983f*sin(2*0.06333f*x) \
00155             + 0.002168f*cos(3*0.06333f*x) - 0.07856f*sin(3*0.06333f*x) \
00156             - 0.01604f*cos(4*0.06333f*x) + 0.0007893f*sin(4*0.06333f*x) \
00157             + 0.02003f*cos(5*0.06333f*x) + 0.005234f*sin(5*0.06333f*x) \
00158             + 0.00913f*cos(6*0.06333f*x) + 0.0001917f*sin(6*0.06333f*x) \
00159             + 0.0007703f*cos(7*0.06333f*x) + 0.002357f*sin(7*0.06333f*x);
00160 
00161     } else if(mode_flag == 6) {
00162         //上楼
00163         a = 0.3354f + 0.1043f*cos(0.04022f*x) - 0.2226f*sin(0.04022f*x) \
00164             - 0.01181f*cos(2*0.04022f*x) - 0.1101f*sin(2*0.04022f*x) \
00165             - 0.2017f*cos(3*0.04022f*x) + 0.003176f*sin(3*0.04022f*x) \
00166             - 0.03876f*cos(4*0.04022f*x) + 0.09576f*sin(4*0.04022f*x) \
00167             + 0.04748f*cos(5*0.04022f*x) + 0.09573f*sin(5*0.04022f*x) \
00168             + 0.0475f*cos(6*0.04022f*x) - 0.009034f*sin(6*0.04022f*x) \
00169             + 0.01196f*cos(7*0.04022f*x) - 0.02833f*sin(7*0.04022f*x) \
00170             - 0.007736f*cos(8*0.04022f*x) + 0.001967f*sin(8*0.04022f*x);
00171     } else if(mode_flag == 7) {
00172         //下楼
00173         a = 0.1254f - 0.3401f*cos(x*0.0573f) + 0.1949f*sin(x*0.0573f) \
00174             - 0.01378f*cos(2*x*0.0573f) + 0.0842f*sin(2*x*0.0573f)     \
00175             - 0.08164f*cos(3*x*0.0573f) + 0.05474f*sin(3*x*0.0573f)    \
00176             + 0.01607f*cos(4*x*0.0573f) - 0.01914f*sin(4*x*0.0573f)    \
00177             - 0.04177f*cos(5*x*0.0573f) - 0.01208f*sin(5*x*0.0573f);
00178     } else if(mode_flag == 8) {
00179         //快速
00180         x = res*100/2.0f;
00181         if( x <= 72.54f){
00182             a = 0 - 0.154f + -0.2819f*cos(0.05937f*x) + 0.5131f*sin(0.05937f*x) \
00183                 + 0.2204f*cos(2*0.05937f*x) + 0.263f*sin(2*0.05937f*x) \
00184                 + 0.21f*cos(3*0.05937f*x) + 0.008273f*sin(3*0.05937f*x) \
00185                 + 0.1431f*cos(4*0.05937f*x) - 0.1179f*sin(4*0.05937f*x) \
00186                 - 0.005844f*cos(5*0.05937f*x) - 0.1348f*sin(5*0.05937f*x) \
00187                 - 0.05481f*cos(6*0.05937f*x) - 0.02825f*sin(6*0.05937f*x) \
00188                 - 0.01024f*cos(7*0.05937f*x) + 0.008747f*sin(7*0.05937f*x);
00189         }else{
00190             a = 0.01567f + 0.05387f*cos(0.1041f*x) - 0.2137f*sin(0.1041f*x) \
00191                 + 0.08837f*cos(2*0.1041f*x) - 0.08757f*sin(2*0.1041f*x) \
00192                 + 0.00966f*cos(3*0.1041f*x) - 0.02229f*sin(3*0.1041f*x);
00193         }
00194     }
00195     return -a;
00196 }
00197 
00198 unsigned int init_joint_angles_flag = 0;
00199 
00200 void calculate_fuzzy(float ankp, float knp)
00201 {
00202     send_enable = 1;    // can可以发送数据
00203     
00204         //初始化关节角度
00205     if(init_joint_angles_flag == 0){
00206         init_joint_angles_flag = 1;
00207         init_joint_angles();
00208     }
00209     
00210     calculate_ankle_fuzzy(ankp,knp);
00211 }
00212 
00213 ////////////////////////踝关节模糊控制//////////////////////////////
00214 //    Gait_per_now     当前步态百分比
00215 
00216 float ank_e = 0.0f, ank_before_e = 0.0f, ank_ec = 0.0f;
00217 unsigned int fuzzy50 = 1;
00218 
00219 void calculate_ankle_fuzzy(float ankp, float knp)
00220 {
00221 
00222 
00223     float s = timeRecord();
00224     pos_knee = position_knee(s);                                   //用于测试模糊PID参数
00225     pos_ankle = position_ankle(s);  
00226 
00227 //    pc.printf("%.3f\t%.3f\t%.3f\t%.3f\t%.3f\n", a_state.knee_state.p, pos_knee, a_state.ankle_state.p, pos_ankle, s);  
00228 
00229 
00230     cal_command.q_des_ankle = pos_ankle;
00231     cal_command.qd_des_ankle = 0;
00232     cal_command.kp_ankle = 280;
00233     cal_command.kd_ankle = 2;
00234     cal_command.tau_ankle_ff = 0;
00235 
00236     cal_command.q_des_knee = pos_knee;
00237     cal_command.qd_des_knee = 0;
00238     cal_command.kp_knee = 180;
00239     cal_command.kd_knee = 0;
00240     cal_command.tau_knee_ff = 0;
00241 }
00242 
00243 
00244 void init_joint_angles(){
00245     
00246     a_control.ankle.p_des = position_ankle(0);
00247     a_control.ankle.v_des = 0;
00248     a_control.ankle.kp    = 60;
00249     a_control.ankle.kd    = 0;
00250     a_control.ankle.t_ff  = 0;
00251         
00252     a_control.knee.p_des = position_knee(0.01);
00253     a_control.knee.v_des = 0;
00254     a_control.knee.kp    = 60;
00255     a_control.knee.kd    = 0;
00256     a_control.knee.t_ff  = 0;
00257     
00258     PackAll();
00259     WriteAll(); 
00260     
00261     wait(2);
00262 }
00263 
00264 
00265