yezhong yezhong
1
Dependencies: mcp2515 mbed-dev-f303
Diff: main.cpp
- Revision:
- 4:2503c88a564f
- Parent:
- 3:940a9e40d327
- Child:
- 5: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