Diff: main.cpp

Revision:

3:940a9e40d327

Parent:

2:cd74a8cb03b0

Child:

4:3f7a59212142

Child:

5:2503c88a564f

--- a/main.cpp	Thu Nov 19 07:59:28 2020 +0000
+++ b/main.cpp	Wed Jun 30 12:15:37 2021 +0000
@@ -14,13 +14,12 @@
 //                             框架搭建完毕                                     //
 ////////////////////////////////////////////////////////////////////////////////
 
-//ankle----pf
-//knee-----df
-
 
 int main()
 {
-
+     //Timer t;
+     //float a=PI/8;
+    //float j=0.558,P=0;
 ////////////////////////初始化//////////////////////////////////////
     pc.baud(115200);
     pc.attach(&serial_pc_isr);
@@ -36,22 +35,22 @@
     pf_can.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0);
     pf_rxMsg.len = 6;
     PF_can.len   = 8;
-    PF_can.id    = 0x01;
+    PF_can.id    = 0x03;
     
     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;
+    DF_can.id    = 0x03;
     
 /////////////////////////////////////position///////////////////////////////////////////
-    wait(5);
-
+    wait(2);
+      Zero(&PF_can);
+      Zero(&DF_can);
     EnterMotorMode(&PF_can);
     EnterMotorMode(&DF_can);
-    Zero(&PF_can);
-    Zero(&DF_can);
-
+    
+/*
     a_control.pf.p_des=PI/8;
     a_control.pf.v_des=0;
     a_control.pf.kp=7;
@@ -66,13 +65,10 @@
 
 
     PackAll();
-    WriteAll();
+    WriteAll();   */
 
 
     while(1) {
-
-
-
         
         pf_can.read(pf_rxMsg);                                                                            
         unpack_reply(pf_rxMsg, &a_state);
@@ -84,27 +80,87 @@
         float pfkv = a_state.pf.v;
         float pfkt = a_state.pf.t;
 
-        float dfp = a_state.df.p;
-        float dfv = a_state.df.v;
-        float dft = a_state.df.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",pfkp,pfkv,pfkt,dfp,dfv,dft);
+       // pc.printf("%.4f,%.3f,%.3f,%.4f,%.3f,%.4f\n",pfkp,pfkv,pfkt,a_control.pf.p_des,j,P);
+        
+       pc.printf("%.3f,%.3f,%.3f,%.3f\n",pfkp,pfkv,pfkt,a_control.pf.v_des); 
+ /////////////////////////////////////////////////trajectory/////////////////////////////////////////////////////
+     /* 
+        j=j+0.001;
+        P=0.33+(-0.2148)*cos((j+0.1)*10.44)+(-0.1549)*sin((j+0.1)*10.44)+(-0.07744)*cos(2*(j+0.1)*10.44)+(-0.04255)*sin(2*(j+0.1)*10.44);       
+        if(j>=0.558&&j<=0.85)
+        {
+             a_control.pf.p_des=P;
+             a_control.pf.v_des=0; 
+             a_control.pf.kp=80;
+             a_control.pf.kd=0;
+             a_control.pf.t_ff=0;
+        }
+       
+        
+         PackAll();
+        WriteAll();
+     */
 
-        pc.printf("%.3f--%.3f--%.3f==================%.3f--%.3f--%.3f\n",pfkp,pfkv,pfkt,dfp,dfv,dft);
+        
+/////////////////////////////////////position///////////////////////////////////////////        
+     /*
+        a_control.pf.p_des=PI/8;
+        a_control.pf.v_des=0;
+        a_control.pf.kp=10;
+        a_control.pf.kd=0;
+        a_control.pf.t_ff=0; 
+        PackAll();
+        WriteAll();
+    */
+        
+    /*
+     t.start();
+     t.read(); 
+       if(t.read()<2)        
+          {
+          a_control.pf.p_des=a;
+          a_control.pf.v_des=0;
+          a_control.pf.kp=10;
+          a_control.pf.kd=0;
+          a_control.pf.t_ff=0;  
+          }                 
+     if(t.read()>3) 
+     {
+          t.reset();
+          Zero(&PF_can);
+          a=-a;
+         }   
+    
+        PackAll();
+        WriteAll();
+        
+        */
+        
+  
+          
+        
+ /////////////////////////////////////////////////////////////////////////////////////////////       
 
 ///////////////////////////////////////velocity///////////////////////////////////////////////
 
-//        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.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();
+        a_control.pf.p_des=0;
+        a_control.pf.v_des=500*2*3.14/60/49;
+        a_control.pf.kp=0;
+        a_control.pf.kd=5;
+        a_control.pf.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=5;
+        a_control.df.t_ff=0;
+        PackAll();
+        WriteAll();       
 
 ////////////////////////////////////////////////////////////////////////////////////////////////
     }