Diff: main.cpp

Revision:

2:cd74a8cb03b0

Parent:

1:a71791b81b8a

Child:

3:940a9e40d327

--- a/main.cpp	Tue Nov 17 11:32:58 2020 +0000
+++ b/main.cpp	Thu Nov 19 07:59:28 2020 +0000
@@ -25,42 +25,46 @@
     pc.baud(115200);
     pc.attach(&serial_pc_isr);
 
-    foot.baud(115200);
-    foot.attach(&serial_board_isr);
+//  foot.baud(115200);
+//  foot.attach(&serial_board_isr);
 
-//    command.baud(115200);
-//    command.attach(&serial_command_isr);
+//   command.baud(115200);
+//   command.attach(&serial_command_isr);
 
-    ankle_can.frequency(1000000);
-    ankle_can.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0);
-    ankle_rxMsg.len = 6;
-    ankle_txMsg.len   = 8;
-    ankle_txMsg.id    = 0x01;
-
-    knee_can.frequency(1000000);
-    knee_can.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0);
-    knee_rxMsg.len = 6;
-    knee_txMsg.len   = 8;
-    knee_txMsg.id    = 0x01;
-////////////////////////////////////////////////////////////////////////////////
+    
+    pf_can.frequency(1000000);
+    pf_can.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0);
+    pf_rxMsg.len = 6;
+    PF_can.len   = 8;
+    PF_can.id    = 0x01;
+    
+    df_can.frequency(1000000);
+    df_can.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0);
+    df_rxMsg.len = 6;
+    DF_can.len   = 8;
+    DF_can.id    = 0x01;
+    
+/////////////////////////////////////position///////////////////////////////////////////
     wait(5);
 
-
-    EnterMotorMode(&knee_txMsg);
-    EnterMotorMode(&ankle_txMsg);
+    EnterMotorMode(&PF_can);
+    EnterMotorMode(&DF_can);
+    Zero(&PF_can);
+    Zero(&DF_can);
 
-//////////////////////////////////////////position/////////////////////////////////////////
-    a_control.ankle.p_des=3.14/8;
-    a_control.ankle.v_des=0;
-    a_control.ankle.kp=5;
-    a_control.ankle.kd=0;
-    a_control.ankle.t_ff=0;
+    a_control.pf.p_des=PI/8;
+    a_control.pf.v_des=0;
+    a_control.pf.kp=7;
+    a_control.pf.kd=0;
+    a_control.pf.t_ff=0;
 
-    a_control.knee.p_des=3.14/8;
-    a_control.knee.v_des=0;
-    a_control.knee.kp=5;
-    a_control.knee.kd=0;
-    a_control.knee.t_ff=0;
+    a_control.df.p_des=PI/8;
+    a_control.df.v_des=0;
+    a_control.df.kp=7;
+    a_control.df.kd=0;
+    a_control.df.t_ff=0;
+
+
     PackAll();
     WriteAll();
 
@@ -68,35 +72,37 @@
     while(1) {
 
 
-        ankle_can.read(ankle_rxMsg);
-        unpack_reply(ankle_rxMsg, &a_state);
+
+        
+        pf_can.read(pf_rxMsg);                                                                            
+        unpack_reply(pf_rxMsg, &a_state);
         wait_us(10);
-        knee_can.read(knee_rxMsg);
-        unpack_reply(knee_rxMsg, &a_state);
+        df_can.read(df_rxMsg);
+        unpack_reply(df_rxMsg, &a_state);
 
-        float ankp = a_state.ankle_state.p;                                                     // 从CAN获得的当前位置
-        float ankv = a_state.ankle_state.v;
-        float ankt = a_state.ankle_state.t;
+        float pfkp = a_state.pf.p;                                                     // 从CAN获得的当前位置
+        float pfkv = a_state.pf.v;
+        float pfkt = a_state.pf.t;
 
-        float knp = a_state.knee_state.p;
-        float knv = a_state.knee_state.v;
-        float knt = a_state.knee_state.t;
+        float dfp = a_state.df.p;
+        float dfv = a_state.df.v;
+        float dft = a_state.df.t;
 
-        pc.printf("%.3f--%.3f--%.3f==================%.3f--%.3f--%.3f\n",ankp,ankv,ankt,knp,knv,knt);
+        pc.printf("%.3f--%.3f--%.3f==================%.3f--%.3f--%.3f\n",pfkp,pfkv,pfkt,dfp,dfv,dft);
 
 ///////////////////////////////////////velocity///////////////////////////////////////////////
 
-//        a_control.ankle.p_des=0;
-//        a_control.ankle.v_des=100*2*3.14/60/49;
-//        a_control.ankle.kp=0;
-//        a_control.ankle.kd=4;
-//        a_control.ankle.t_ff=0;
+//        a_control.pf.p_des=0;
+//        a_control.pf.v_des=100*2*3.14/60/49;
+//        a_control.pf.kp=0;
+//        a_control.pf.kd=4;
+//        a_control.pf.t_ff=0;
 //
-//        a_control.knee.p_des=0;
-//        a_control.knee.v_des=100*2*3.14/60/49;
-//        a_control.knee.kp=0;
-//        a_control.knee.kd=4;
-//        a_control.knee.t_ff=0;
+//        a_control.df.p_des=0;
+//        a_control.df.v_des=100*2*3.14/60/49;
+//        a_control.df.kp=0;
+//        a_control.df.kd=4;
+//        a_control.df.t_ff=0;
 //        PackAll();
 //        WriteAll();