Revision 3:ac1062c23c49, committed 2022-02-22

Comitter:

yezhong

Date:

Tue Feb 22 14:31:06 2022 +0000

Parent:

2:e85ebf401989

Commit message:

11

Changed in this revision

diff -r e85ebf401989 -r ac1062c23c49 main.cpp
--- a/main.cpp	Thu Nov 18 03:14:26 2021 +0000
+++ b/main.cpp	Tue Feb 22 14:31:06 2022 +0000
@@ -32,13 +32,13 @@
 //    command.baud(115200);
 //    command.attach(&serial_command_isr);
 
-    ankle_can.frequency(1000000);
+    ankle_can.frequency(800000);
     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.frequency(800000);
     knee_can.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0);
     knee_rxMsg.len = 6;
     knee_txMsg.len   = 8;
@@ -46,12 +46,15 @@
 ////////////////////////////////////////////////////////////////////////////////
     wait(8);
 
+    Zero(&ankle_txMsg);
+    EnterSPEEDMode(&ankle_txMsg);
+    wait_us(100);
     Zero(&knee_txMsg);
-    Zero(&ankle_txMsg);
-    //EnterSPEEDMode(&knee_txMsg);
-  //  EnterSPEEDMode(&ankle_txMsg);
-    EnterPositionMode(&knee_txMsg);
-    EnterPositionMode(&ankle_txMsg);
+    EnterSPEEDMode(&knee_txMsg);
+    
+   
+    //EnterPositionMode(&knee_txMsg);
+   // EnterPositionMode(&ankle_txMsg);
     //EnterMotorMode(&knee_txMsg);
    // EnterMotorMode(&ankle_txMsg);
     
@@ -81,7 +84,7 @@
      
         
 //////////////////////////////////////////position/////////////////////////////////////////
-  
+ /* 
     t.start();
     t.read(); 
     b=t.read(); 
@@ -106,23 +109,23 @@
             
     PackAll();
     WriteAll();
-
+*/
 ///////////////////////////////////////velocity///////////////////////////////////////////////
- /*
+ 
         t.start();
         t.read(); 
         
         a_control.ankle.p_des=0;
-        a_control.ankle.v_des=800*2*3.14/60/50;
+        a_control.ankle.v_des=-500*2*3.14/60/50;
         //a_control.ankle.v_des=(500*2*3.14/60/50)*sin(t);
-        a_control.ankle.kp=0;
-        a_control.ankle.kd=5;
+        a_control.ankle.kp=10;
+        a_control.ankle.kd=1;
         a_control.ankle.t_ff=0;
 
         a_control.knee.p_des=0;
-        //a_control.knee.v_des=800*2*3.14/60/50;
-        a_control.knee.v_des=(800*2*3.14/60/50)*sin(0.5f*t);
-        a_control.knee.kp=20;
+        a_control.knee.v_des=500*2*3.14/60/50;
+        //a_control.knee.v_des=(800*2*3.14/60/50)*sin(0.5f*t);
+        a_control.knee.kp=10;
         a_control.knee.kd=1;
         a_control.knee.t_ff=0;
             
@@ -135,7 +138,7 @@
         WriteAll();     
         
       
-*/
+
 ////////////////////////////////////////////////////////////////////////////////////////////////
     }
 }
\ No newline at end of file