yezhong yezhong / panzhan_main_controller_continuous_LX

11

Dependencies:   mbed-dev-f303

CONTROL/control.cpp

Committer:
yezhong
Date:
2021-08-06
Revision:
2:9418258519ea
Parent:
0:dd5d4837292c

File content as of revision 2:9418258519ea:

#include "control.h"


void control(float ankp, float knp)
{
    if(command_control_flag == 1)
        command_control();
            
    
    if(c_lock == 0)
    {
        calculate_fuzzy(ankp,knp);
    }
    
    // 将计算得到的数值赋给控制器
    
    a_control.ankle.p_des = cal_command.q_des_ankle;
    a_control.ankle.v_des = cal_command.qd_des_ankle;
    a_control.ankle.kp    = cal_command.kp_ankle;
    a_control.ankle.kd    = cal_command.kd_ankle;
    a_control.ankle.t_ff  = cal_command.tau_ankle_ff;
        
    a_control.knee.p_des = cal_command.q_des_knee;
    a_control.knee.v_des = cal_command.qd_des_knee;
    a_control.knee.kp    = cal_command.kp_knee;
    a_control.knee.kd    = cal_command.kd_knee;
    a_control.knee.t_ff  = cal_command.tau_knee_ff;
    
}