control.cpp

00001 #include "control.h"
00002 
00003 
00004 void control(float ankle_real_p, float knee_real_p)
00005 {
00006     if(command_control_flag == 1)
00007         command_control();
00008             
00009     if(c_lock == 1)                                                             // 处于PC串口锁定模式 
00010     {
00011         cal_command.q_des_ankle  = SP_ankle;
00012         cal_command.qd_des_ankle = 0;
00013         cal_command.kp_ankle     = 20;
00014         cal_command.kd_ankle     = 1;
00015         cal_command.tau_ankle_ff = 0;
00016         
00017         cal_command.q_des_knee  = SP_knee;
00018         cal_command.qd_des_knee = 0;
00019         cal_command.kp_knee     = 20;
00020         cal_command.kd_knee     = 1;
00021         cal_command.tau_knee_ff = 0;        
00022     }
00023     
00024     if(c_lock == 0)
00025     {
00026         calculate_fuzzy(ankle_real_p, knee_real_p);
00027     }
00028     
00029     // 将计算得到的数值赋给控制器
00030     
00031     a_control.ankle.p_des = cal_command.q_des_ankle;
00032     a_control.ankle.v_des = cal_command.qd_des_ankle;
00033     a_control.ankle.kp    = cal_command.kp_ankle;
00034     a_control.ankle.kd    = cal_command.kd_ankle;
00035     a_control.ankle.t_ff  = cal_command.tau_ankle_ff;
00036         
00037     a_control.knee.p_des = cal_command.q_des_knee;
00038     a_control.knee.v_des = cal_command.qd_des_knee;
00039     a_control.knee.kp    = cal_command.kp_knee;
00040     a_control.knee.kd    = cal_command.kd_knee;
00041     a_control.knee.t_ff  = cal_command.tau_knee_ff;
00042     
00043 }
00044   
00045     
00046 
00047 
00048 
00049 
00050 
00051 
00052       
00053         
00054         
00055 
00056 
00057