Revision 6:7d2743feaf23, committed 2022-06-24

Comitter:

yezhong

Date:

Fri Jun 24 01:13:03 2022 +0000

Parent:

5:902ba9999d6c

Commit message:

1

Changed in this revision

diff -r 902ba9999d6c -r 7d2743feaf23 CONTROL/control.cpp
--- a/CONTROL/control.cpp	Tue Jan 11 13:34:14 2022 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,65 +0,0 @@
-#include "control.h"
-
-
-void control()
-{
-    if(command_control_flag == 1)
-        command_control();
-    
-    if(return_zero == 1)
-    {
-//        gangduceshi();
-    }
-         
-    if(c_lock == 1)                                                             // 处于PC串口锁定模式
-    {
-//        cal_command.q_des_pf  = SP_pf;
-//        cal_command.qd_des_pf = 0.0f;
-//        cal_command.kp_pf     = 200.0f;
-//        cal_command.kd_pf     = 0.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     = 200.0f;
-//        cal_command.kd_df     = 0.0f;
-//        cal_command.tau_df_ff = 0.0f;
-    }
-    
-    if(c_lock == 0)
-    {
-        //calculate_pf_kh();
-//        calculate_pf_fuzzy();
-        
-        //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 / 7.0f;
-//    a_control.pf.kd    = cal_command.kd_pf / 7.0f;
-//    a_control.pf.t_ff  = cal_command.tau_pf_ff / 7.0f;
-//        
-//    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 / 10.0f;
-//    a_control.df.kd    = cal_command.kd_df / 10.0f;
-//    a_control.df.t_ff  = cal_command.tau_df_ff / 10.0f;
-    
-}
-  
-    
-
-
-
-
-
-
-      
-        
-        
-
-
-

diff -r 902ba9999d6c -r 7d2743feaf23 CONTROL/control.h
--- a/CONTROL/control.h	Tue Jan 11 13:34:14 2022 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,20 +0,0 @@
-#ifndef _CONTROL_H
-#define _CONTROL_H
-
-
-#include "mbed.h"
-#include "CAN.h"
-#include "mode.h"
-#include "data_command.h"
-#include "data_board.h"
-#include "used_leg_message.h"
-
-
-
-
-void control();
-
-
-
-
-#endif

diff -r 902ba9999d6c -r 7d2743feaf23 DATA_COMMAND/data_command.h
--- a/DATA_COMMAND/data_command.h	Tue Jan 11 13:34:14 2022 +0000
+++ b/DATA_COMMAND/data_command.h	Fri Jun 24 01:13:03 2022 +0000
@@ -6,7 +6,7 @@
 #include "CAN.h"
 #include "used_leg_message.h"
 #include "mode.h"
-#include "control.h"
+
 
 
 extern Serial      command;

diff -r 902ba9999d6c -r 7d2743feaf23 main.cpp
--- a/main.cpp	Tue Jan 11 13:34:14 2022 +0000
+++ b/main.cpp	Fri Jun 24 01:13:03 2022 +0000
@@ -21,7 +21,6 @@
 ////////////////////////////////////////////////////////////////////////////////
  
 
-
 int main()
 {
      Timer t;
@@ -36,10 +35,18 @@
 //  foot.baud(115200);                                          //接收鞋垫信息
 //  foot.attach(&serial_board_isr);
 
-//   command.baud(115200);                                      //485通信
+//   command.baud(115200);                                      //
 //   command.attach(&serial_command_isr);
 
+    ///////////////////////
     
+    
+    
+    
+    
+    
+    
+    ////////////////////////////
      
     
     pf_can.frequency(800000);
@@ -60,24 +67,25 @@
     df1_can.frequency(800000);
     
 /////////////////////////////////////position///////////////////////////////////////////
-    wait(8);
+    wait(4);
      Zero(&pf_txMsg);
      // EnterSPEEDMode(&pf_txMsg);
+     //EnterPositionMode(&pf_txMsg);
+      EnterMotorMode(&pf_txMsg);
       
-     //Zero(&df_txMsg);
-     // EnterSPEEDMode(&df_txMsg);
+     Zero(&df_txMsg);
+      //EnterSPEEDMode(&df_txMsg);
+      //EnterPositionMode(&df_txMsg);  
+      EnterMotorMode(&df_txMsg);
       
       Zero(&df1_txMsg);
-      EnterSPEEDMode(&df1_txMsg);
+     // EnterSPEEDMode(&df1_txMsg);
+      EnterPositionMode(&df1_txMsg);
+    //EnterMotorMode(&df1_txMsg);
     
-    EnterPositionMode(&pf_txMsg);
-    //EnterPositionMode(&df_txMsg);  
-    EnterPositionMode(&df1_txMsg);
+
    
-      
-    //EnterMotorMode(&pf_txMsg);
-    //EnterMotorMode(&df_txMsg);
-      //EnterMotorMode(&df1_txMsg);
+    
     
 
 
@@ -162,7 +170,7 @@
         a_control.pf.t_ff=0;
        }
         */
-        a_control.pf.p_des=PI/32;
+        a_control.pf.p_des=-PI/4;
         a_control.pf.v_des=0;
         a_control.pf.kp=10;
         a_control.pf.kd=5;
@@ -170,29 +178,19 @@
         
         
         
-        a_control.df.p_des=PI/4;
-        a_control.df.v_des=0;
-        a_control.df.kp=10;
+        a_control.df.p_des=0;
+        a_control.df.v_des=-500*2*3.14/60/50;
+        a_control.df.kp=0;
         a_control.df.kd=5;
         a_control.df.t_ff=0;
         
-        if(t.read()<2)
-        {
-        a_control.df1.p_des=PI/4;
-        a_control.df1.v_des=0;
-        a_control.df1.kp=30;
-        a_control.df1.kd=5;
-        a_control.df1.t_ff=0;
-        }
-        
-        if(t.read()>2&&t.read()<4)
-        {
+       
         a_control.df1.p_des=PI/8;
         a_control.df1.v_des=0;
         a_control.df1.kp=35;
         a_control.df1.kd=5;
         a_control.df1.t_ff=0;
-        }
+        
     /*    
         if(t.read()>4&&t.read()<6)
         {