Gan likun / yezhong_main_controller_copy001

111

Dependencies:   yezhong_main_controller_copy mbed1-dev

main.cpp@6:902ba9999d6c, 2022-01-11 (annotated)

Committer:
yezhong
Date:
Tue Jan 11 13:34:14 2022 +0000
Revision:
6:902ba9999d6c
Parent:
5:2503c88a564f
Child:
7:d1b09098579b
11

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