Diff: CONTROL/control.cpp

Revision:

5:6a95726e45b0

Child:

6:aad89fd109c2

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/CONTROL/control.cpp	Tue Oct 08 04:35:23 2019 +0000
@@ -0,0 +1,59 @@
+#include "control.h"
+
+
+unsigned int send_enable = 0;
+
+
+void control()
+{
+    if(c_control == 1)
+        command_control();
+        
+    if(c_lock == 1)                                                             // 处于PC串口锁定模式
+    {
+        cal_command.q_des_pf  = SP_pf;
+        cal_command.qd_des_pf = 0.0f;
+        cal_command.kp_pf     = 800.0f;
+        cal_command.kd_pf     = 100.0f;
+        cal_command.tau_pf_ff = 0.0f;
+        
+        cal_command.q_des_df  = SP_df;
+        cal_command.qd_des_df = 0.0f;
+        cal_command.kp_df     = 800.0f;
+        cal_command.kd_df     = 100.0f;
+        cal_command.tau_df_ff = 0.0f;
+    }
+    
+    if(c_lock == 0)
+    {
+        calculate_pf();
+        calculate_df();
+    }
+    
+    // 将计算得到的数值赋给控制器
+    a_control.pf.p_des = cal_command.q_des_pf;
+    a_control.pf.v_des = cal_command.qd_des_pf;
+    a_control.pf.kp    = cal_command.kp_pf;
+    a_control.pf.kd    = cal_command.kd_pf;
+    a_control.pf.t_ff  = cal_command.tau_pf_ff;
+        
+    a_control.df.p_des = cal_command.q_des_df;
+    a_control.df.v_des = cal_command.qd_des_df;
+    a_control.df.kp    = cal_command.kp_df;
+    a_control.df.kd    = cal_command.kd_df;
+    a_control.df.t_ff  = cal_command.tau_df_ff;
+}
+  
+    
+
+
+
+
+
+
+      
+        
+        
+
+
+