Diff: main.cpp

Revision:

8:1ab9699af5ae

Parent:

7:d1b09098579b

--- a/main.cpp	Tue Feb 22 14:26:28 2022 +0000
+++ b/main.cpp	Tue Mar 22 02:03:58 2022 +0000
@@ -67,19 +67,19 @@
     
     
 /////////////////////////////////////position///////////////////////////////////////////
-    wait(8);
+    wait(4);
      Zero(&pf_txMsg);
-     // EnterSPEEDMode(&pf_txMsg);
+     //EnterSPEEDMode(&pf_txMsg);
+      EnterPositionMode(&pf_txMsg);
       
      //Zero(&df_txMsg);
      // EnterSPEEDMode(&df_txMsg);
+     //EnterPositionMode(&df_txMsg);  
+     
       
       Zero(&df1_txMsg);
-      EnterSPEEDMode(&df1_txMsg);
-    
-    EnterPositionMode(&pf_txMsg);
-    //EnterPositionMode(&df_txMsg);  
-    EnterPositionMode(&df1_txMsg);
+     // EnterSPEEDMode(&df1_txMsg);
+      EnterPositionMode(&df1_txMsg);
    
       
     //EnterMotorMode(&pf_txMsg);
@@ -90,7 +90,7 @@
 
 
     while(1) {
-        
+       
         pf_can.read(pf_rxMsg);                                                                            
         unpack_reply(pf_rxMsg, &a_state);
         wait_us(10);
@@ -169,8 +169,8 @@
         a_control.pf.t_ff=0;
        }
         */
-        a_control.pf.p_des=PI/32;
-        a_control.pf.v_des=0;
+        a_control.pf.p_des=0;
+        a_control.pf.v_des=-500*2*3.14/60/50;
         a_control.pf.kp=10;
         a_control.pf.kd=5;
         a_control.pf.t_ff=0;
@@ -183,20 +183,20 @@
         a_control.df.kd=5;
         a_control.df.t_ff=0;
         
-        if(t.read()<2)
+        if(t.read()<13)
         {
-        a_control.df1.p_des=PI/4;
+        a_control.df1.p_des=6*PI;
         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)
+        if(t.read()>13&&t.read()<26)
         {
-        a_control.df1.p_des=PI/8;
+        a_control.df1.p_des=0;
         a_control.df1.v_des=0;
-        a_control.df1.kp=35;
+        a_control.df1.kp=10;
         a_control.df1.kd=5;
         a_control.df1.t_ff=0;
         }