1233
Dependencies: cJSON_lib mcp2515- mbed-dev-f303 esp8266 yezhong_main_controller_copy_3
Diff: main.cpp
- Revision:
- 0:e923de71caa5
diff -r 000000000000 -r e923de71caa5 main.cpp --- /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