File content as of revision 2:9418258519ea:

#include "calculate.h"


///////////计时函数////////////////
float timeRecord()
{
    float s =tim.read();
    if(mode_flag == 8) {
        if(s >= 2.0f) {
            tim.reset();
        }
    } else if(mode_flag==1){
        if(s >= 4.0f) {
            tim.reset();
            wait_ms(20);
            mode_flag = 2;
            send_enable = 0;
        }
    }else if(mode_flag==2){
        if(s >= 4.0f) {
            tim.reset();
            wait_ms(20);
            mode_flag = 1;
            send_enable = 0;
        }
    }else{
        if(s >= 3.0f) {
            tim.reset();
        }
    }
    return s;
}



unsigned int send_enable = 0;
float pos_ankle = 0.0f;
float pos_knee = 0.0f;


float position_knee(float res)
{
    float x = res*100/3.0f;      //可能不太合适
    float a = 0.0f;

    if(mode_flag==1) {
        //坐
        x = res*100/4.0f;
        a = 0.8308f - 0.7549f*cos(0.04647f*x) - 0.555f*sin(0.04647f*x) \
            - 0.01085f*cos(2*0.04647f*x) - 0.06257f*sin(2*0.04647f*x) \
            - 0.0555f*cos(3*0.04647f*x) + 0.1742f*sin(3*0.04647f*x) \
            - 0.0227f*cos(4*0.04647f*x) + 0.01162f*sin(4*0.04647f*x) \
            + 0.0292f*cos(5*0.04647f*x) - 0.0004315f*sin(5*0.04647f*x);
    } else if(mode_flag==2) {
        //起
        x = res*100/4.0f;
        a =  0.8303f + 0.5947f*cos(0.04688f*x) + 0.7276f*sin(0.04688f*x) \
            + 0.007792f*cos(2*0.04688f*x) - 0.0688f*sin(2*0.04688f*x) \
            + 0.1629f*cos(3*0.04688f*x) - 0.0822f*sin(3*0.04688f*x) \
            - 0.02656f*cos(4*0.04688f*x) - 0.008309f*sin(4*0.04688f*x) \
            - 0.007829f*cos(5*0.04688f*x) - 0.02764f*sin(5*0.04688f*x);
    } else if(mode_flag==3) {
        //平地
        a = 0.3713f - 0.01673f*cos(x*0.06085f) - 0.3617f*sin(x*0.06085f) \
            - 0.26f*cos(2*x*0.06085f) + 0.09466f*sin(2*x*0.06085f)      \
            - 0.01627f*cos(3*x*0.06085f) + 0.07981f*sin(3*x*0.06085f);
    } else if(mode_flag==4) {
        //上坡
        a = 3.099f + 1.654f*cos(0.03788f*x) - 4.737f*sin(0.03788f*x) \
            - 2.759f*cos(2*0.03788f*x) - 2.703f*sin(2*0.03788f*x) \
            - 2.541f*cos(3*0.03788f*x) + 1.4f*sin(3*0.03788f*x) \
            + 0.1508f*cos(4*0.03788f*x) + 1.524f*sin(4*0.03788f*x)\
            + 0.6595f*cos(5*0.03788f*x) + 0.2162f*sin(5*0.03788f*x)\
            +  0.1359f*cos(6*0.03788f*x) - 0.1806f*sin(6*0.03788f*x)\
            - 0.02243f*cos(7*0.03788f*x) - 0.04809f*sin(7*0.03788f*x);
    } else if(mode_flag==5) {
        //下坡
        a = 0.6109f + -0.2748f*cos(0.06062f*x) -  0.2946f*sin(0.06062f*x) \
            - 0.2302f*cos(2*0.06062f*x) + 0.1319f*sin(2*0.06062f*x) \
            + 0.02774f*cos(3*0.06062f*x) + 0.04863f*sin(3*0.06062f*x) \
            + 0.01865f*cos(4*0.06062f*x) -  0.01827f*sin(4*0.06062f*x) \
            + 0.009155f*cos(5*0.06062f*x) -  0.008681f*sin(5*0.06062f*x) \
            + 0.008983f*cos(6*0.06062f*x) -  0.01089f*sin(6*0.06062f*x) \
            - 0.00243f*cos(7*0.06062f*x) -  0.004621f*sin(7*0.06062f*x);

    } else if(mode_flag==6) {
        //上楼
        a = 0.6949f + 0.2931f*cos(0.064f*x) -  0.4634f*sin(0.064f*x) \
             - 0.1563f*cos(2*0.064f*x) + 0.01809f*sin(2*0.064f*x) \
             + 0.01518f*cos(3*0.064f*x) + 0.02992f*sin(3*0.064f*x);

    } else if(mode_flag==7) {
        //下楼
        a = 0.5695f - 0.2778f*cos(x*0.0614f) - 0.3586f*sin(x*0.0614f) \
            - 0.2202f*cos(2*x*0.0614f) + 0.2438f*sin(2*x*0.0614f)    \
            + 0.05218f*cos(3*x*0.0614f) + 0.03495f*sin(3*x*0.0614f)  \
            - 0.00463f*cos(4*x*0.0614f) - 0.05175f*sin(4*x*0.0614f);
    } else if(mode_flag==8) {
        x = res*100/2.0f;
        a = 0.3713f - 0.01673f*cos(x*0.06085f) - 0.3617f*sin(x*0.06085f) \
            - 0.26f*cos(2*x*0.06085f) + 0.09466f*sin(2*x*0.06085f)      \
            - 0.01627f*cos(3*x*0.06085f) + 0.07981f*sin(3*x*0.06085f);
    }

    return a;
}

float position_ankle(float res)
{
    float x = res*100/3.0f;  //可能不太合适
    float a = 0.0f;
    if(mode_flag == 1) {
        //坐
        a = 0.0f;
    } else if(mode_flag == 2) {
        //起
        a = 0.0f;
    } else if(mode_flag == 3) {
        //平地
        if( x <= 72.54f){
            a = 0 - 0.154f + -0.2819f*cos(0.05937f*x) + 0.5131f*sin(0.05937f*x) \
                + 0.2204f*cos(2*0.05937f*x) + 0.263f*sin(2*0.05937f*x) \
                + 0.21f*cos(3*0.05937f*x) + 0.008273f*sin(3*0.05937f*x) \
                + 0.1431f*cos(4*0.05937f*x) - 0.1179f*sin(4*0.05937f*x) \
                - 0.005844f*cos(5*0.05937f*x) - 0.1348f*sin(5*0.05937f*x) \
                - 0.05481f*cos(6*0.05937f*x) - 0.02825f*sin(6*0.05937f*x) \
                - 0.01024f*cos(7*0.05937f*x) + 0.008747f*sin(7*0.05937f*x);
        }else{
            a = 0.01567f + 0.05387f*cos(0.1041f*x) - 0.2137f*sin(0.1041f*x) \
                + 0.08837f*cos(2*0.1041f*x) - 0.08757f*sin(2*0.1041f*x) \
                + 0.00966f*cos(3*0.1041f*x) - 0.02229f*sin(3*0.1041f*x);
        }
        
        
//        a = 0.07088f - 0.02461f*cos(x*0.06048f) + 0.1543f *sin(x*0.06048f)\
//            + 0.05244f*cos(2*x*0.06048f) - 0.09977f*sin(2*x*0.06048f)    \
//            - 0.07853f*cos(3*x*0.06048f) - 0.01301f*sin(3*x*0.06048f)    \
//            + 0.02483f*cos(4*x*0.06048f) + 0.01296f*sin(4*x*0.06048f)    \
//            + 0.003777f*cos(5*x*0.06048f) - 0.0268f*sin(5*x*0.06048f);      
    } else if(mode_flag == 4) {
        //上坡
        a = 0.2081f + 0.1834f*cos(x*0.05377f) + 0.0957f*sin(x*0.05377f) \
            - 0.125f*cos(2*x*0.05377f) - 0.1071f*sin(2*x*0.05377f)      \
            + 0.0755f*cos(3*x*0.05377f) + 0.01942f*sin(3*x*0.05377f)    \
            - 0.04593f*cos(4*x*0.05377f) - 0.04182f*sin(4*x*0.05377f)   \
            + 0.02662f*cos(5*x*0.05377f) - 0.005419f*sin(5*x*0.05377f)  \
            - 0.01759f*cos(6*x*0.05377f) - 0.006702f*sin(6*x*0.05377f)  \
            + 0.003964f*cos(7*x*0.05377f) - 0.004773f*sin(7*x*0.05377f) \
            - 0.007487f*cos(8*x*0.05377f) + 0.0009378f*sin(8*x*0.05377f);

    } else if(mode_flag == 5) {
        //下坡
        a = 0.07331f - 0.1611f*cos(0.06333f*x) - 0.01543f*sin(0.06333f*x) \
            +  0.05741f*cos(2*0.06333f*x) - 0.0004983f*sin(2*0.06333f*x) \
            + 0.002168f*cos(3*0.06333f*x) - 0.07856f*sin(3*0.06333f*x) \
            - 0.01604f*cos(4*0.06333f*x) + 0.0007893f*sin(4*0.06333f*x) \
            + 0.02003f*cos(5*0.06333f*x) + 0.005234f*sin(5*0.06333f*x) \
            + 0.00913f*cos(6*0.06333f*x) + 0.0001917f*sin(6*0.06333f*x) \
            + 0.0007703f*cos(7*0.06333f*x) + 0.002357f*sin(7*0.06333f*x);

    } else if(mode_flag == 6) {
        //上楼
        a = 0.3354f + 0.1043f*cos(0.04022f*x) - 0.2226f*sin(0.04022f*x) \
            - 0.01181f*cos(2*0.04022f*x) - 0.1101f*sin(2*0.04022f*x) \
            - 0.2017f*cos(3*0.04022f*x) + 0.003176f*sin(3*0.04022f*x) \
            - 0.03876f*cos(4*0.04022f*x) + 0.09576f*sin(4*0.04022f*x) \
            + 0.04748f*cos(5*0.04022f*x) + 0.09573f*sin(5*0.04022f*x) \
            + 0.0475f*cos(6*0.04022f*x) - 0.009034f*sin(6*0.04022f*x) \
            + 0.01196f*cos(7*0.04022f*x) - 0.02833f*sin(7*0.04022f*x) \
            - 0.007736f*cos(8*0.04022f*x) + 0.001967f*sin(8*0.04022f*x);
    } else if(mode_flag == 7) {
        //下楼
        a = 0.1254f - 0.3401f*cos(x*0.0573f) + 0.1949f*sin(x*0.0573f) \
            - 0.01378f*cos(2*x*0.0573f) + 0.0842f*sin(2*x*0.0573f)     \
            - 0.08164f*cos(3*x*0.0573f) + 0.05474f*sin(3*x*0.0573f)    \
            + 0.01607f*cos(4*x*0.0573f) - 0.01914f*sin(4*x*0.0573f)    \
            - 0.04177f*cos(5*x*0.0573f) - 0.01208f*sin(5*x*0.0573f);
    } else if(mode_flag == 8) {
        //快速
        x = res*100/2.0f;
        if( x <= 72.54f){
            a = 0 - 0.154f + -0.2819f*cos(0.05937f*x) + 0.5131f*sin(0.05937f*x) \
                + 0.2204f*cos(2*0.05937f*x) + 0.263f*sin(2*0.05937f*x) \
                + 0.21f*cos(3*0.05937f*x) + 0.008273f*sin(3*0.05937f*x) \
                + 0.1431f*cos(4*0.05937f*x) - 0.1179f*sin(4*0.05937f*x) \
                - 0.005844f*cos(5*0.05937f*x) - 0.1348f*sin(5*0.05937f*x) \
                - 0.05481f*cos(6*0.05937f*x) - 0.02825f*sin(6*0.05937f*x) \
                - 0.01024f*cos(7*0.05937f*x) + 0.008747f*sin(7*0.05937f*x);
        }else{
            a = 0.01567f + 0.05387f*cos(0.1041f*x) - 0.2137f*sin(0.1041f*x) \
                + 0.08837f*cos(2*0.1041f*x) - 0.08757f*sin(2*0.1041f*x) \
                + 0.00966f*cos(3*0.1041f*x) - 0.02229f*sin(3*0.1041f*x);
        }
    }
    return -a;
}

unsigned int init_joint_angles_flag = 0;

void calculate_fuzzy(float ankp, float knp)
{
    send_enable = 1;    // can可以发送数据
    
        //初始化关节角度
    if(init_joint_angles_flag == 0){
        init_joint_angles_flag = 1;
        init_joint_angles();
    }
    
    calculate_ankle_fuzzy(ankp,knp);
}

////////////////////////踝关节模糊控制//////////////////////////////
//    Gait_per_now     当前步态百分比

float ank_e = 0.0f, ank_before_e = 0.0f, ank_ec = 0.0f;
unsigned int fuzzy50 = 1;

void calculate_ankle_fuzzy(float ankp, float knp)
{


    float s = timeRecord();
    pos_knee = position_knee(s);                                   //用于测试模糊PID参数
    pos_ankle = position_ankle(s);  

//    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);  


    cal_command.q_des_ankle = pos_ankle;
    cal_command.qd_des_ankle = 0;
    cal_command.kp_ankle = 280;
    cal_command.kd_ankle = 2;
    cal_command.tau_ankle_ff = 0;

    cal_command.q_des_knee = pos_knee;
    cal_command.qd_des_knee = 0;
    cal_command.kp_knee = 180;
    cal_command.kd_knee = 0;
    cal_command.tau_knee_ff = 0;
}


void init_joint_angles(){
    
    a_control.ankle.p_des = position_ankle(0);
    a_control.ankle.v_des = 0;
    a_control.ankle.kp    = 60;
    a_control.ankle.kd    = 0;
    a_control.ankle.t_ff  = 0;
        
    a_control.knee.p_des = position_knee(0.01);
    a_control.knee.v_des = 0;
    a_control.knee.kp    = 60;
    a_control.knee.kd    = 0;
    a_control.knee.t_ff  = 0;
    
    PackAll();
    WriteAll(); 
    
    wait(2);
}