yezhong yezhong / yezhong_main_controller_copy-

1

Dependencies:   mcp2515 mbed-dev-f303

main.cpp@6:7d2743feaf23, 2022-06-24 (annotated)

Committer:
yezhong
Date:
Fri Jun 24 01:13:03 2022 +0000
Revision:
6:7d2743feaf23
Parent:
5:902ba9999d6c 
1

Who changed what in which revision?

UserRevisionLine numberNew contents of line
panzhan 0:d80c66cb1b3a 1 #include "mbed.h"
panzhan 0:d80c66cb1b3a 2 #include <cstring>
panzhan 0:d80c66cb1b3a 3 #include "math_ops.h"
panzhan 0:d80c66cb1b3a 4 #include "leg_message.h"
panzhan 0:d80c66cb1b3a 5 #include "CAN.h"
panzhan 0:d80c66cb1b3a 6 #include "used_leg_message.h"
panzhan 0:d80c66cb1b3a 7 #include "data_pc.h"
panzhan 0:d80c66cb1b3a 8 #include "data_board.h"
panzhan 0:d80c66cb1b3a 9 #include "mode.h"
panzhan 0:d80c66cb1b3a 10 #include "data_command.h"
yezhong 4:2503c88a564f 11 #include "Moving_Average.h"
yezhong 4:2503c88a564f 12
yezhong 4:2503c88a564f 13 #include "CAN3.h"
yezhong 4:2503c88a564f 14 #include "mcp2515.h"
yezhong 4:2503c88a564f 15 #include <sstream>
yezhong 4:2503c88a564f 16 #include <algorithm>
panzhan 0:d80c66cb1b3a 17
panzhan 0:d80c66cb1b3a 18
panzhan 0:d80c66cb1b3a 19 ////////////////////////////////////////////////////////////////////////////////
panzhan 0:d80c66cb1b3a 20 // 框架搭建完毕 //
panzhan 0:d80c66cb1b3a 21 ////////////////////////////////////////////////////////////////////////////////
yezhong 4:2503c88a564f 22
panzhan 0:d80c66cb1b3a 23
panzhan 0:d80c66cb1b3a 24 int main()
panzhan 0:d80c66cb1b3a 25 {
yezhong 4:2503c88a564f 26 Timer t;
yezhong 3:940a9e40d327 27 //float a=PI/8;
yezhong 3:940a9e40d327 28 //float j=0.558,P=0;
yezhong 4:2503c88a564f 29
yezhong 4:2503c88a564f 30
panzhan 0:d80c66cb1b3a 31 ////////////////////////初始化//////////////////////////////////////
yezhong 4:2503c88a564f 32 pc.baud(115200); //串口打印信息
panzhan 0:d80c66cb1b3a 33 pc.attach(&serial_pc_isr);
panzhan 0:d80c66cb1b3a 34
yezhong 4:2503c88a564f 35 // foot.baud(115200); //接收鞋垫信息
yezhong 2:cd74a8cb03b0 36 // foot.attach(&serial_board_isr);
panzhan 0:d80c66cb1b3a 37
yezhong 6:7d2743feaf23 38 // command.baud(115200); //
yezhong 2:cd74a8cb03b0 39 // command.attach(&serial_command_isr);
panzhan 0:d80c66cb1b3a 40
yezhong 6:7d2743feaf23 41 ///////////////////////
yezhong 2:cd74a8cb03b0 42
yezhong 6:7d2743feaf23 43
yezhong 6:7d2743feaf23 44
yezhong 6:7d2743feaf23 45
yezhong 6:7d2743feaf23 46
yezhong 6:7d2743feaf23 47
yezhong 6:7d2743feaf23 48
yezhong 6:7d2743feaf23 49 ////////////////////////////
yezhong 5:902ba9999d6c 50
yezhong 5:902ba9999d6c 51
yezhong 4:2503c88a564f 52 pf_can.frequency(800000);
yezhong 2:cd74a8cb03b0 53 pf_can.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0);
yezhong 2:cd74a8cb03b0 54 pf_rxMsg.len = 6;
yezhong 4:2503c88a564f 55 pf_txMsg.len = 8;
yezhong 4:2503c88a564f 56 pf_txMsg.id = 0x01;
yezhong 2:cd74a8cb03b0 57
yezhong 4:2503c88a564f 58 df_can.frequency(800000);
yezhong 2:cd74a8cb03b0 59 df_can.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0);
yezhong 2:cd74a8cb03b0 60 df_rxMsg.len = 6;
yezhong 4:2503c88a564f 61 df_txMsg.len = 8;
yezhong 4:2503c88a564f 62 df_txMsg.id = 0x02;
yezhong 4:2503c88a564f 63
yezhong 4:2503c88a564f 64 df1_rxMsg.len = 6;
yezhong 4:2503c88a564f 65 df1_txMsg.len =8;
yezhong 4:2503c88a564f 66 df1_txMsg.id =0x03;
yezhong 4:2503c88a564f 67 df1_can.frequency(800000);
yezhong 2:cd74a8cb03b0 68
yezhong 2:cd74a8cb03b0 69 /////////////////////////////////////position///////////////////////////////////////////
yezhong 6:7d2743feaf23 70 wait(4);
yezhong 4:2503c88a564f 71 Zero(&pf_txMsg);
yezhong 4:2503c88a564f 72 // EnterSPEEDMode(&pf_txMsg);
yezhong 6:7d2743feaf23 73 //EnterPositionMode(&pf_txMsg);
yezhong 6:7d2743feaf23 74 EnterMotorMode(&pf_txMsg);
yezhong 4:2503c88a564f 75
yezhong 6:7d2743feaf23 76 Zero(&df_txMsg);
yezhong 6:7d2743feaf23 77 //EnterSPEEDMode(&df_txMsg);
yezhong 6:7d2743feaf23 78 //EnterPositionMode(&df_txMsg);
yezhong 6:7d2743feaf23 79 EnterMotorMode(&df_txMsg);
yezhong 4:2503c88a564f 80
yezhong 4:2503c88a564f 81 Zero(&df1_txMsg);
yezhong 6:7d2743feaf23 82 // EnterSPEEDMode(&df1_txMsg);
yezhong 6:7d2743feaf23 83 EnterPositionMode(&df1_txMsg);
yezhong 6:7d2743feaf23 84 //EnterMotorMode(&df1_txMsg);
yezhong 3:940a9e40d327 85
yezhong 6:7d2743feaf23 86
yezhong 4:2503c88a564f 87
yezhong 6:7d2743feaf23 88
yezhong 4:2503c88a564f 89
panzhan 1:a71791b81b8a 90
panzhan 0:d80c66cb1b3a 91
panzhan 0:d80c66cb1b3a 92
panzhan 0:d80c66cb1b3a 93 while(1) {
yezhong 2:cd74a8cb03b0 94
yezhong 2:cd74a8cb03b0 95 pf_can.read(pf_rxMsg);
yezhong 2:cd74a8cb03b0 96 unpack_reply(pf_rxMsg, &a_state);
panzhan 0:d80c66cb1b3a 97 wait_us(10);
yezhong 2:cd74a8cb03b0 98 df_can.read(df_rxMsg);
yezhong 2:cd74a8cb03b0 99 unpack_reply(df_rxMsg, &a_state);
yezhong 4:2503c88a564f 100 wait_us(10);
yezhong 4:2503c88a564f 101 df1_can.read(&df1_rxMsg);
yezhong 4:2503c88a564f 102 unpack_reply(df1_rxMsg, &a_state);
panzhan 0:d80c66cb1b3a 103
yezhong 4:2503c88a564f 104 float pfp = a_state.pf.p; // 从CAN获得的当前位置
yezhong 4:2503c88a564f 105 float pfv = a_state.pf.v;
yezhong 4:2503c88a564f 106 float pft = a_state.pf.t;
panzhan 0:d80c66cb1b3a 107
yezhong 4:2503c88a564f 108 float dfp = a_state.df.p;
yezhong 4:2503c88a564f 109 float dfv = a_state.df.v;
yezhong 4:2503c88a564f 110 float dft = a_state.df.t;
yezhong 4:2503c88a564f 111
yezhong 4:2503c88a564f 112 float df1p = a_state.df1.p;
yezhong 4:2503c88a564f 113 float df1v = a_state.df1.v;
yezhong 4:2503c88a564f 114 float df1t = a_state.df1.t;
yezhong 4:2503c88a564f 115
yezhong 4:2503c88a564f 116
yezhong 4:2503c88a564f 117
yezhong 4:2503c88a564f 118 ///////////////////////pf拉力//////////////////////////
yezhong 4:2503c88a564f 119 La_pf_real = LaLi_pf.read()*3.3f*1000; //读取电压值；单位为mV;
yezhong 4:2503c88a564f 120 pf_filter = Moving_Average(10, Ffilter_pf, La_pf_real);
yezhong 4:2503c88a564f 121 F_pf=0.1125f*pf_filter+2.141f;
yezhong 4:2503c88a564f 122 //F_pf=0.1139*pf_filter;
yezhong 4:2503c88a564f 123 La_df_real = LaLi_df.read();
yezhong 4:2503c88a564f 124 La_df1_real = LaLi_df1.read();
yezhong 3:940a9e40d327 125
yezhong 4:2503c88a564f 126 pc.printf("%.3fa%.3f\r\n",F_pf,pfp); //拉力
yezhong 4:2503c88a564f 127
yezhong 4:2503c88a564f 128
yezhong 4:2503c88a564f 129 //pc.printf("%.3f\r\n",dfp);
yezhong 4:2503c88a564f 130 //wait(0.1);
yezhong 3:940a9e40d327 131 // pc.printf("%.3f--%.3f--%.3f==================%.3f--%.3f--%.3f\n",pfkp,pfkv,pfkt,dfp,dfv,dft);
yezhong 3:940a9e40d327 132 // pc.printf("%.4f,%.3f,%.3f,%.4f,%.3f,%.4f\n",pfkp,pfkv,pfkt,a_control.pf.p_des,j,P);
yezhong 4:2503c88a564f 133 //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);
yezhong 4:2503c88a564f 134 //pc.printf("%.3fa%.3f\n\r",df1p,a_control.df1.p_des);
yezhong 3:940a9e40d327 135 /////////////////////////////////////////////////trajectory/////////////////////////////////////////////////////
yezhong 3:940a9e40d327 136 /*
yezhong 3:940a9e40d327 137 j=j+0.001;
yezhong 3:940a9e40d327 138 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);
yezhong 3:940a9e40d327 139 if(j>=0.558&&j<=0.85)
yezhong 3:940a9e40d327 140 {
yezhong 3:940a9e40d327 141 a_control.pf.p_des=P;
yezhong 3:940a9e40d327 142 a_control.pf.v_des=0;
yezhong 3:940a9e40d327 143 a_control.pf.kp=80;
yezhong 3:940a9e40d327 144 a_control.pf.kd=0;
yezhong 3:940a9e40d327 145 a_control.pf.t_ff=0;
yezhong 3:940a9e40d327 146 }
yezhong 3:940a9e40d327 147
yezhong 3:940a9e40d327 148
yezhong 3:940a9e40d327 149 PackAll();
yezhong 3:940a9e40d327 150 WriteAll();
yezhong 3:940a9e40d327 151 */
yezhong 4:2503c88a564f 152 t.start();
yezhong 4:2503c88a564f 153 t.read();
yezhong 4:2503c88a564f 154 /*
yezhong 4:2503c88a564f 155
yezhong 4:2503c88a564f 156 if(t.read()<2)
yezhong 4:2503c88a564f 157 {
yezhong 4:2503c88a564f 158 a_control.pf.p_des=-PI/32;
yezhong 4:2503c88a564f 159 a_control.pf.v_des=0;
yezhong 4:2503c88a564f 160 a_control.pf.kp=10;
yezhong 4:2503c88a564f 161 a_control.pf.kd=5;
yezhong 4:2503c88a564f 162 a_control.pf.t_ff=0;
yezhong 4:2503c88a564f 163 }
yezhong 4:2503c88a564f 164 if(t.read()>2&&t.read()<4)
yezhong 4:2503c88a564f 165 {
yezhong 4:2503c88a564f 166 a_control.pf.p_des=0;
yezhong 4:2503c88a564f 167 a_control.pf.v_des=0;
yezhong 4:2503c88a564f 168 a_control.pf.kp=10;
yezhong 4:2503c88a564f 169 a_control.pf.kd=5;
yezhong 4:2503c88a564f 170 a_control.pf.t_ff=0;
yezhong 4:2503c88a564f 171 }
yezhong 4:2503c88a564f 172 */
yezhong 6:7d2743feaf23 173 a_control.pf.p_des=-PI/4;
yezhong 4:2503c88a564f 174 a_control.pf.v_des=0;
yezhong 4:2503c88a564f 175 a_control.pf.kp=10;
yezhong 4:2503c88a564f 176 a_control.pf.kd=5;
yezhong 4:2503c88a564f 177 a_control.pf.t_ff=0;
yezhong 4:2503c88a564f 178
yezhong 4:2503c88a564f 179
yezhong 4:2503c88a564f 180
yezhong 6:7d2743feaf23 181 a_control.df.p_des=0;
yezhong 6:7d2743feaf23 182 a_control.df.v_des=-500*2*3.14/60/50;
yezhong 6:7d2743feaf23 183 a_control.df.kp=0;
yezhong 4:2503c88a564f 184 a_control.df.kd=5;
yezhong 4:2503c88a564f 185 a_control.df.t_ff=0;
yezhong 4:2503c88a564f 186
yezhong 6:7d2743feaf23 187
yezhong 4:2503c88a564f 188 a_control.df1.p_des=PI/8;
yezhong 4:2503c88a564f 189 a_control.df1.v_des=0;
yezhong 4:2503c88a564f 190 a_control.df1.kp=35;
yezhong 4:2503c88a564f 191 a_control.df1.kd=5;
yezhong 4:2503c88a564f 192 a_control.df1.t_ff=0;
yezhong 6:7d2743feaf23 193
yezhong 4:2503c88a564f 194 /*
yezhong 4:2503c88a564f 195 if(t.read()>4&&t.read()<6)
yezhong 4:2503c88a564f 196 {
yezhong 4:2503c88a564f 197 t.reset();
yezhong 4:2503c88a564f 198 }
yezhong 4:2503c88a564f 199 */
yezhong 4:2503c88a564f 200
yezhong 4:2503c88a564f 201 PackAll();
yezhong 4:2503c88a564f 202 WriteAll();
yezhong 4:2503c88a564f 203
yezhong 4:2503c88a564f 204
yezhong 4:2503c88a564f 205
yezhong 4:2503c88a564f 206
yezhong 4:2503c88a564f 207
yezhong 4:2503c88a564f 208
yezhong 4:2503c88a564f 209
panzhan 0:d80c66cb1b3a 210
yezhong 3:940a9e40d327 211
yezhong 3:940a9e40d327 212 /////////////////////////////////////position///////////////////////////////////////////
yezhong 3:940a9e40d327 213 /*
yezhong 3:940a9e40d327 214 a_control.pf.p_des=PI/8;
yezhong 3:940a9e40d327 215 a_control.pf.v_des=0;
yezhong 3:940a9e40d327 216 a_control.pf.kp=10;
yezhong 3:940a9e40d327 217 a_control.pf.kd=0;
yezhong 3:940a9e40d327 218 a_control.pf.t_ff=0;
yezhong 3:940a9e40d327 219 PackAll();
yezhong 3:940a9e40d327 220 WriteAll();
yezhong 3:940a9e40d327 221 */
yezhong 3:940a9e40d327 222
yezhong 3:940a9e40d327 223 /*
yezhong 3:940a9e40d327 224 t.start();
yezhong 3:940a9e40d327 225 t.read();
yezhong 4:2503c88a564f 226
yezhong 3:940a9e40d327 227 if(t.read()<2)
yezhong 3:940a9e40d327 228 {
yezhong 3:940a9e40d327 229 a_control.pf.p_des=a;
yezhong 3:940a9e40d327 230 a_control.pf.v_des=0;
yezhong 3:940a9e40d327 231 a_control.pf.kp=10;
yezhong 3:940a9e40d327 232 a_control.pf.kd=0;
yezhong 3:940a9e40d327 233 a_control.pf.t_ff=0;
yezhong 3:940a9e40d327 234 }
yezhong 3:940a9e40d327 235 if(t.read()>3)
yezhong 3:940a9e40d327 236 {
yezhong 3:940a9e40d327 237 t.reset();
yezhong 3:940a9e40d327 238 Zero(&PF_can);
yezhong 3:940a9e40d327 239 a=-a;
yezhong 3:940a9e40d327 240 }
yezhong 3:940a9e40d327 241
yezhong 3:940a9e40d327 242 PackAll();
yezhong 3:940a9e40d327 243 WriteAll();
yezhong 3:940a9e40d327 244
yezhong 3:940a9e40d327 245 */
yezhong 3:940a9e40d327 246
yezhong 3:940a9e40d327 247
yezhong 3:940a9e40d327 248
yezhong 3:940a9e40d327 249
yezhong 3:940a9e40d327 250 /////////////////////////////////////////////////////////////////////////////////////////////
panzhan 0:d80c66cb1b3a 251
panzhan 0:d80c66cb1b3a 252 ///////////////////////////////////////velocity///////////////////////////////////////////////
yezhong 4:2503c88a564f 253 /*
yezhong 4:2503c88a564f 254 t.start();
yezhong 4:2503c88a564f 255 t.read();
yezhong 4:2503c88a564f 256
panzhan 0:d80c66cb1b3a 257
yezhong 3:940a9e40d327 258 a_control.pf.p_des=0;
yezhong 4:2503c88a564f 259 a_control.pf.v_des=-500*2*3.14/60/50;
yezhong 4:2503c88a564f 260 a_control.pf.kp=10;
yezhong 3:940a9e40d327 261 a_control.pf.kd=5;
yezhong 3:940a9e40d327 262 a_control.pf.t_ff=0;
yezhong 3:940a9e40d327 263
yezhong 3:940a9e40d327 264 a_control.df.p_des=0;
yezhong 4:2503c88a564f 265 a_control.df.v_des=-500*2*3.14/60/50;
yezhong 4:2503c88a564f 266 a_control.df.kp=10;
yezhong 3:940a9e40d327 267 a_control.df.kd=5;
yezhong 3:940a9e40d327 268 a_control.df.t_ff=0;
yezhong 4:2503c88a564f 269
yezhong 4:2503c88a564f 270 a_control.df1.p_des=0;
yezhong 4:2503c88a564f 271 a_control.df1.v_des=500*2*3.14/60/50;
yezhong 4:2503c88a564f 272 //a_control.df1.v_des=(500*2*3.14/60/50)*sin(0.5f*t);
yezhong 4:2503c88a564f 273 a_control.df1.kp=10;
yezhong 4:2503c88a564f 274 a_control.df1.kd=5;
yezhong 4:2503c88a564f 275 a_control.df1.t_ff=0;
yezhong 3:940a9e40d327 276 PackAll();
yezhong 3:940a9e40d327 277 WriteAll();
yezhong 4:2503c88a564f 278 */
panzhan 0:d80c66cb1b3a 279 ////////////////////////////////////////////////////////////////////////////////////////////////
panzhan 0:d80c66cb1b3a 280 }
panzhan 0:d80c66cb1b3a 281 }