pan zhan / panzhan_main_controller_continuous

12

Dependencies:   mbed-dev_spine

CONTROL/control.cpp

Committer:
panzhan
Date:
2021-06-09
Revision:
0:dd5d4837292c

File content as of revision 0:dd5d4837292c:

#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;
    
}