control.cpp

00001 #include "control.h"
00002 
00003 
00004 void control()
00005 {
00006     if(command_control_flag == 1)
00007         command_control();
00008     
00009     if(return_zero == 1)
00010     {
00011 //        gangduceshi();
00012     }
00013          
00014     if(c_lock == 1)                                                             // 处于PC串口锁定模式
00015     {
00016 //        cal_command.q_des_pf  = SP_pf;
00017 //        cal_command.qd_des_pf = 0.0f;
00018 //        cal_command.kp_pf     = 200.0f;
00019 //        cal_command.kd_pf     = 0.0f;
00020 //        cal_command.tau_pf_ff = 0.0f;
00021 //        
00022 //        cal_command.q_des_df  = SP_df;
00023 //        cal_command.qd_des_df = 0.0f;
00024 //        cal_command.kp_df     = 200.0f;
00025 //        cal_command.kd_df     = 0.0f;
00026 //        cal_command.tau_df_ff = 0.0f;
00027     }
00028     
00029     if(c_lock == 0)
00030     {
00031         //calculate_pf_kh();
00032 //        calculate_pf_fuzzy();
00033         
00034         //calculate_df();
00035     }
00036     
00037     // 将计算得到的数值赋给控制器
00038     
00039 //    a_control.pf.p_des = cal_command.q_des_pf;
00040 //    a_control.pf.v_des = cal_command.qd_des_pf;
00041 //    a_control.pf.kp    = cal_command.kp_pf / 7.0f;
00042 //    a_control.pf.kd    = cal_command.kd_pf / 7.0f;
00043 //    a_control.pf.t_ff  = cal_command.tau_pf_ff / 7.0f;
00044 //        
00045 //    a_control.df.p_des = cal_command.q_des_df;
00046 //    a_control.df.v_des = cal_command.qd_des_df;
00047 //    a_control.df.kp    = cal_command.kp_df / 10.0f;
00048 //    a_control.df.kd    = cal_command.kd_df / 10.0f;
00049 //    a_control.df.t_ff  = cal_command.tau_df_ff / 10.0f;
00050     
00051 }
00052   
00053     
00054 
00055 
00056 
00057 
00058 
00059 
00060       
00061         
00062         
00063 
00064 
00065