Gan likun
/
yezhong_main_controller_copy1
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: yezhong_main_controller_copy mbed1-dev
main.cpp
- Committer:
- yezhong
- Date:
- 2022-03-22
- Revision:
- 8:1ab9699af5ae
- Parent:
- 7:d1b09098579b
File content as of revision 8:1ab9699af5ae:
#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_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;
//float a=PI/8;
//float j=0.558,P=0;
////////////////////////初始化//////////////////////////////////////
pc.baud(115200); //串口打印信息 U2
pc.attach(&serial_pc_isr);
//foot.baud(115200); //接收鞋垫信息 U1
// foot.attach(&serial_board_isr);
// command.baud(115200); //485通信 U3
// command.attach(&serial_command_isr);
zitai_foot.baud(115200); //U4
zitai_foot.attach(&serial_zitai_foot_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, 1); // command中断优先级高于board
//NVIC_SetPriority(USART3_IRQn, 3);
/////////////////////////////////////position///////////////////////////////////////////
wait(4);
Zero(&pf_txMsg);
//EnterSPEEDMode(&pf_txMsg);
EnterPositionMode(&pf_txMsg);
//Zero(&df_txMsg);
// EnterSPEEDMode(&df_txMsg);
//EnterPositionMode(&df_txMsg);
Zero(&df1_txMsg);
// EnterSPEEDMode(&df1_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=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=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()<13)
{
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()>13&&t.read()<26)
{
a_control.df1.p_des=0;
a_control.df1.v_des=0;
a_control.df1.kp=10;
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();
*/
////////////////////////////////////////////////////////////////////////////////////////////////
}
}