Diff: main.cpp

Revision:

0:e923de71caa5

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Jun 24 07:58:26 2022 +0000
@@ -0,0 +1,300 @@
+#include "mbed.h"
+#include <cstring>
+#include "math_ops.h"
+#include "leg_message.h"
+#include "CAN.h"
+#include "used_leg_message.h"
+#include "data_pc.h"
+#include "data_xiedian.h"
+#include "mode.h"
+#include "data_wifi.h"
+#include "Moving_Average.h"
+#include "wifi_example.h"
+#include "esp8266.h"
+
+
+#include "CAN3.h"
+#include "mcp2515.h"
+#include <sstream>
+#include <algorithm>
+
+
+
+
+
+////////////////////////////////////////////////////////////////////////////////
+//                             框架搭建完毕                                     //
+////////////////////////////////////////////////////////////////////////////////
+ 
+
+
+int main()
+{
+     Timer t;
+     //float a=PI/8;
+    //float j=0.558,P=0;
+   
+    
+////////////////////////初始化//////////////////////////////////////
+    pc.baud(115200);                                           //串口打印信息  U2
+    //pc.attach(&serial_pc_isr);
+
+
+
+       xiedian.baud(115200);                                      //   U3
+       xiedian.attach(&serial_xiedian_isr);
+    
+     
+
+     wifi.baud(115200);             
+    // connectInit();                                //U1 wifi接收姿态传感器
+     wifi.attach(&serial_wifi_isr);
+      
+   
+    
+    pf_can.frequency(800000);
+    pf_can.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0);
+    pf_rxMsg.len = 6;
+    pf_txMsg.len   = 8;
+    pf_txMsg.id    = 0x01;
+    
+    df_can.frequency(800000);
+    df_can.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0);
+    df_rxMsg.len = 6;
+    df_txMsg.len   = 8;
+    df_txMsg.id    = 0x02;
+    
+    df1_rxMsg.len = 6;
+    df1_txMsg.len =8;
+    df1_txMsg.id =0x03;
+    df1_can.frequency(800000);
+    
+    NVIC_SetPriority(USART1_IRQn, 3);                                           // command中断优先级高于board
+    NVIC_SetPriority(USART2_IRQn, 1);  
+    //NVIC_SetPriority(USART3_IRQn, 3);
+    
+    
+    
+/////////////////////////////////////position///////////////////////////////////////////
+    //wait(8);
+     Zero(&pf_txMsg);
+     // EnterSPEEDMode(&pf_txMsg);
+      
+     //Zero(&df_txMsg);
+     // EnterSPEEDMode(&df_txMsg);
+      
+      Zero(&df1_txMsg);
+      EnterSPEEDMode(&df1_txMsg);
+    
+    EnterPositionMode(&pf_txMsg);
+    //EnterPositionMode(&df_txMsg);  
+    EnterPositionMode(&df1_txMsg);
+   
+      
+    //EnterMotorMode(&pf_txMsg);
+    //EnterMotorMode(&df_txMsg);
+      //EnterMotorMode(&df1_txMsg);
+    
+
+           
+
+    while(1) {
+      
+      
+        pf_can.read(pf_rxMsg);                                                                            
+        unpack_reply(pf_rxMsg, &a_state);
+        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 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 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("%.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;
+        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();
+     */
+        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///////////////////////////////////////////        
+     /*
+        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///////////////////////////////////////////////
+  /*
+        t.start();
+        t.read(); 
+
+
+        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;
+
+        a_control.df.p_des=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