Diff: main.cpp

Revision:

5:2503c88a564f

Parent:

3:940a9e40d327

Child:

6:902ba9999d6c

--- a/main.cpp	Wed Jun 30 12:15:37 2021 +0000
+++ b/main.cpp	Tue Jan 11 02:19:55 2022 +0000
@@ -8,64 +8,76 @@
 #include "data_board.h"
 #include "mode.h"
 #include "data_command.h"
+#include "Moving_Average.h"
+
+#include "CAN3.h"
+#include "mcp2515.h"
+#include <sstream>
+#include <algorithm>
 
 
 ////////////////////////////////////////////////////////////////////////////////
 //                             框架搭建完毕                                     //
 ////////////////////////////////////////////////////////////////////////////////
+ 
 
 
 int main()
 {
-     //Timer t;
+     Timer t;
      //float a=PI/8;
     //float j=0.558,P=0;
+   
+    
 ////////////////////////初始化//////////////////////////////////////
-    pc.baud(115200);
+    pc.baud(115200);                                           //串口打印信息
     pc.attach(&serial_pc_isr);
 
-//  foot.baud(115200);
+//  foot.baud(115200);                                          //接收鞋垫信息
 //  foot.attach(&serial_board_isr);
 
-//   command.baud(115200);
+//   command.baud(115200);                                      //485通信
 //   command.attach(&serial_command_isr);
 
     
-    pf_can.frequency(1000000);
+    pf_can.frequency(800000);
     pf_can.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0);
     pf_rxMsg.len = 6;
-    PF_can.len   = 8;
-    PF_can.id    = 0x03;
+    pf_txMsg.len   = 8;
+    pf_txMsg.id    = 0x01;
     
-    df_can.frequency(1000000);
+    df_can.frequency(800000);
     df_can.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0);
     df_rxMsg.len = 6;
-    DF_can.len   = 8;
-    DF_can.id    = 0x03;
+    df_txMsg.len   = 8;
+    df_txMsg.id    = 0x02;
+    
+    df1_rxMsg.len = 6;
+    df1_txMsg.len =8;
+    df1_txMsg.id =0x03;
+    df1_can.frequency(800000);
     
 /////////////////////////////////////position///////////////////////////////////////////
-    wait(2);
-      Zero(&PF_can);
-      Zero(&DF_can);
-    EnterMotorMode(&PF_can);
-    EnterMotorMode(&DF_can);
+    wait(8);
+     Zero(&pf_txMsg);
+     // EnterSPEEDMode(&pf_txMsg);
+      
+     //Zero(&df_txMsg);
+     // EnterSPEEDMode(&df_txMsg);
+      
+      Zero(&df1_txMsg);
+      EnterSPEEDMode(&df1_txMsg);
     
-/*
-    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;
+    EnterPositionMode(&pf_txMsg);
+    //EnterPositionMode(&df_txMsg);  
+    EnterPositionMode(&df1_txMsg);
+   
+      
+    //EnterMotorMode(&pf_txMsg);
+    //EnterMotorMode(&df_txMsg);
+      //EnterMotorMode(&df1_txMsg);
+    
 
-    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();   */
 
 
     while(1) {
@@ -75,19 +87,41 @@
         wait_us(10);
         df_can.read(df_rxMsg);
         unpack_reply(df_rxMsg, &a_state);
+        wait_us(10);
+        df1_can.read(&df1_rxMsg);
+        unpack_reply(df1_rxMsg, &a_state);
 
-        float pfkp = a_state.pf.p;                                                     // 从CAN获得的当前位置
-        float pfkv = a_state.pf.v;
-        float pfkt = a_state.pf.t;
+        float pfp = a_state.pf.p;                                                     // 从CAN获得的当前位置
+        float pfv = a_state.pf.v;
+        float pft = 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;
+        
+        float df1p = a_state.df1.p;
+        float df1v = a_state.df1.v;
+        float df1t = a_state.df1.t;
+    
+    
+    
+       ///////////////////////pf拉力////////////////////////// 
+       La_pf_real = LaLi_pf.read()*3.3f*1000;       //读取电压值；单位为mV;
+       pf_filter = Moving_Average(10, Ffilter_pf, La_pf_real);
+       F_pf=0.1125f*pf_filter+2.141f;
+       //F_pf=0.1139*pf_filter;
+       La_df_real = LaLi_df.read();
+       La_df1_real = LaLi_df1.read();
 
+        pc.printf("%.3fa%.3f\r\n",F_pf,pfp);     //拉力
+         
+               
+        //pc.printf("%.3f\r\n",dfp);
+         //wait(0.1);
        // 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); 
+       //pc.printf("%.3fa%.3fa%.3fa%.3fa%.3fa%.3f\n\r",pfv,dfv,df1v,a_control.pf.v_des,a_control.df.v_des,a_control.df1.v_des); 
+       //pc.printf("%.3fa%.3f\n\r",df1p,a_control.df1.p_des); 
  /////////////////////////////////////////////////trajectory/////////////////////////////////////////////////////
      /* 
         j=j+0.001;
@@ -105,6 +139,74 @@
          PackAll();
         WriteAll();
      */
+        t.start();
+        t.read(); 
+    /*
+      
+       if(t.read()<2)
+       {  
+        a_control.pf.p_des=-PI/32;
+        a_control.pf.v_des=0;
+        a_control.pf.kp=10;
+        a_control.pf.kd=5;
+        a_control.pf.t_ff=0;
+       }
+       if(t.read()>2&&t.read()<4)
+        {  
+        a_control.pf.p_des=0;
+        a_control.pf.v_des=0;
+        a_control.pf.kp=10;
+        a_control.pf.kd=5;
+        a_control.pf.t_ff=0;
+       }
+        */
+        a_control.pf.p_des=PI/32;
+        a_control.pf.v_des=0;
+        a_control.pf.kp=10;
+        a_control.pf.kd=5;
+        a_control.pf.t_ff=0;
+        
+        
+        
+        a_control.df.p_des=PI/4;
+        a_control.df.v_des=0;
+        a_control.df.kp=10;
+        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)
+        {
+        t.reset();
+        }
+     */   
+        
+         PackAll();
+        WriteAll();
+
+
+
+
+
+
+
 
         
 /////////////////////////////////////position///////////////////////////////////////////        
@@ -121,6 +223,7 @@
     /*
      t.start();
      t.read(); 
+     
        if(t.read()<2)        
           {
           a_control.pf.p_des=a;
@@ -147,21 +250,32 @@
  /////////////////////////////////////////////////////////////////////////////////////////////       
 
 ///////////////////////////////////////velocity///////////////////////////////////////////////
+  /*
+        t.start();
+        t.read(); 
+
 
         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.v_des=-500*2*3.14/60/50;
+        a_control.pf.kp=10;
         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.v_des=-500*2*3.14/60/50;
+        a_control.df.kp=10;
         a_control.df.kd=5;
         a_control.df.t_ff=0;
+        
+        a_control.df1.p_des=0;
+        a_control.df1.v_des=500*2*3.14/60/50;
+        //a_control.df1.v_des=(500*2*3.14/60/50)*sin(0.5f*t);
+        a_control.df1.kp=10;
+        a_control.df1.kd=5;
+        a_control.df1.t_ff=0;
         PackAll();
         WriteAll();       
-
+*/
 ////////////////////////////////////////////////////////////////////////////////////////////////
     }
 }
\ No newline at end of file