Gan likun / yezhong_main_controller_copy001

111

Dependencies:   yezhong_main_controller_copy mbed1-dev

main.cpp@5:2503c88a564f, 2022-01-11 (annotated)

Committer:
yezhong
Date:
Tue Jan 11 02:19:55 2022 +0000
Revision:
5:2503c88a564f
Parent:
3:940a9e40d327
Child:
6: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 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 5:2503c88a564f 43 pf_can.frequency(800000);
yezhong 2:cd74a8cb03b0 44 pf_can.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0);
yezhong 2:cd74a8cb03b0 45 pf_rxMsg.len = 6;
yezhong 5:2503c88a564f 46 pf_txMsg.len = 8;
yezhong 5:2503c88a564f 47 pf_txMsg.id = 0x01;
yezhong 2:cd74a8cb03b0 48
yezhong 5:2503c88a564f 49 df_can.frequency(800000);
yezhong 2:cd74a8cb03b0 50 df_can.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0);
yezhong 2:cd74a8cb03b0 51 df_rxMsg.len = 6;
yezhong 5:2503c88a564f 52 df_txMsg.len = 8;
yezhong 5:2503c88a564f 53 df_txMsg.id = 0x02;
yezhong 5:2503c88a564f 54
yezhong 5:2503c88a564f 55 df1_rxMsg.len = 6;
yezhong 5:2503c88a564f 56 df1_txMsg.len =8;
yezhong 5:2503c88a564f 57 df1_txMsg.id =0x03;
yezhong 5:2503c88a564f 58 df1_can.frequency(800000);
yezhong 2:cd74a8cb03b0 59
yezhong 2:cd74a8cb03b0 60 /////////////////////////////////////position///////////////////////////////////////////
yezhong 5:2503c88a564f 61 wait(8);
yezhong 5:2503c88a564f 62 Zero(&pf_txMsg);
yezhong 5:2503c88a564f 63 // EnterSPEEDMode(&pf_txMsg);
yezhong 5:2503c88a564f 64
yezhong 5:2503c88a564f 65 //Zero(&df_txMsg);
yezhong 5:2503c88a564f 66 // EnterSPEEDMode(&df_txMsg);
yezhong 5:2503c88a564f 67
yezhong 5:2503c88a564f 68 Zero(&df1_txMsg);
yezhong 5:2503c88a564f 69 EnterSPEEDMode(&df1_txMsg);
yezhong 3:940a9e40d327 70
yezhong 5:2503c88a564f 71 EnterPositionMode(&pf_txMsg);
yezhong 5:2503c88a564f 72 //EnterPositionMode(&df_txMsg);
yezhong 5:2503c88a564f 73 EnterPositionMode(&df1_txMsg);
yezhong 5:2503c88a564f 74
yezhong 5:2503c88a564f 75
yezhong 5:2503c88a564f 76 //EnterMotorMode(&pf_txMsg);
yezhong 5:2503c88a564f 77 //EnterMotorMode(&df_txMsg);
yezhong 5:2503c88a564f 78 //EnterMotorMode(&df1_txMsg);
yezhong 5:2503c88a564f 79
panzhan 1:a71791b81b8a 80
panzhan 0:d80c66cb1b3a 81
panzhan 0:d80c66cb1b3a 82
panzhan 0:d80c66cb1b3a 83 while(1) {
yezhong 2:cd74a8cb03b0 84
yezhong 2:cd74a8cb03b0 85 pf_can.read(pf_rxMsg);
yezhong 2:cd74a8cb03b0 86 unpack_reply(pf_rxMsg, &a_state);
panzhan 0:d80c66cb1b3a 87 wait_us(10);
yezhong 2:cd74a8cb03b0 88 df_can.read(df_rxMsg);
yezhong 2:cd74a8cb03b0 89 unpack_reply(df_rxMsg, &a_state);
yezhong 5:2503c88a564f 90 wait_us(10);
yezhong 5:2503c88a564f 91 df1_can.read(&df1_rxMsg);
yezhong 5:2503c88a564f 92 unpack_reply(df1_rxMsg, &a_state);
panzhan 0:d80c66cb1b3a 93
yezhong 5:2503c88a564f 94 float pfp = a_state.pf.p; // 从CAN获得的当前位置
yezhong 5:2503c88a564f 95 float pfv = a_state.pf.v;
yezhong 5:2503c88a564f 96 float pft = a_state.pf.t;
panzhan 0:d80c66cb1b3a 97
yezhong 5:2503c88a564f 98 float dfp = a_state.df.p;
yezhong 5:2503c88a564f 99 float dfv = a_state.df.v;
yezhong 5:2503c88a564f 100 float dft = a_state.df.t;
yezhong 5:2503c88a564f 101
yezhong 5:2503c88a564f 102 float df1p = a_state.df1.p;
yezhong 5:2503c88a564f 103 float df1v = a_state.df1.v;
yezhong 5:2503c88a564f 104 float df1t = a_state.df1.t;
yezhong 5:2503c88a564f 105
yezhong 5:2503c88a564f 106
yezhong 5:2503c88a564f 107
yezhong 5:2503c88a564f 108 ///////////////////////pf拉力//////////////////////////
yezhong 5:2503c88a564f 109 La_pf_real = LaLi_pf.read()*3.3f*1000; //读取电压值；单位为mV;
yezhong 5:2503c88a564f 110 pf_filter = Moving_Average(10, Ffilter_pf, La_pf_real);
yezhong 5:2503c88a564f 111 F_pf=0.1125f*pf_filter+2.141f;
yezhong 5:2503c88a564f 112 //F_pf=0.1139*pf_filter;
yezhong 5:2503c88a564f 113 La_df_real = LaLi_df.read();
yezhong 5:2503c88a564f 114 La_df1_real = LaLi_df1.read();
yezhong 3:940a9e40d327 115
yezhong 5:2503c88a564f 116 pc.printf("%.3fa%.3f\r\n",F_pf,pfp); //拉力
yezhong 5:2503c88a564f 117
yezhong 5:2503c88a564f 118
yezhong 5:2503c88a564f 119 //pc.printf("%.3f\r\n",dfp);
yezhong 5:2503c88a564f 120 //wait(0.1);
yezhong 3:940a9e40d327 121 // pc.printf("%.3f--%.3f--%.3f==================%.3f--%.3f--%.3f\n",pfkp,pfkv,pfkt,dfp,dfv,dft);
yezhong 3:940a9e40d327 122 // pc.printf("%.4f,%.3f,%.3f,%.4f,%.3f,%.4f\n",pfkp,pfkv,pfkt,a_control.pf.p_des,j,P);
yezhong 5:2503c88a564f 123 //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 124 //pc.printf("%.3fa%.3f\n\r",df1p,a_control.df1.p_des);
yezhong 3:940a9e40d327 125 /////////////////////////////////////////////////trajectory/////////////////////////////////////////////////////
yezhong 3:940a9e40d327 126 /*
yezhong 3:940a9e40d327 127 j=j+0.001;
yezhong 3:940a9e40d327 128 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 129 if(j>=0.558&&j<=0.85)
yezhong 3:940a9e40d327 130 {
yezhong 3:940a9e40d327 131 a_control.pf.p_des=P;
yezhong 3:940a9e40d327 132 a_control.pf.v_des=0;
yezhong 3:940a9e40d327 133 a_control.pf.kp=80;
yezhong 3:940a9e40d327 134 a_control.pf.kd=0;
yezhong 3:940a9e40d327 135 a_control.pf.t_ff=0;
yezhong 3:940a9e40d327 136 }
yezhong 3:940a9e40d327 137
yezhong 3:940a9e40d327 138
yezhong 3:940a9e40d327 139 PackAll();
yezhong 3:940a9e40d327 140 WriteAll();
yezhong 3:940a9e40d327 141 */
yezhong 5:2503c88a564f 142 t.start();
yezhong 5:2503c88a564f 143 t.read();
yezhong 5:2503c88a564f 144 /*
yezhong 5:2503c88a564f 145
yezhong 5:2503c88a564f 146 if(t.read()<2)
yezhong 5:2503c88a564f 147 {
yezhong 5:2503c88a564f 148 a_control.pf.p_des=-PI/32;
yezhong 5:2503c88a564f 149 a_control.pf.v_des=0;
yezhong 5:2503c88a564f 150 a_control.pf.kp=10;
yezhong 5:2503c88a564f 151 a_control.pf.kd=5;
yezhong 5:2503c88a564f 152 a_control.pf.t_ff=0;
yezhong 5:2503c88a564f 153 }
yezhong 5:2503c88a564f 154 if(t.read()>2&&t.read()<4)
yezhong 5:2503c88a564f 155 {
yezhong 5:2503c88a564f 156 a_control.pf.p_des=0;
yezhong 5:2503c88a564f 157 a_control.pf.v_des=0;
yezhong 5:2503c88a564f 158 a_control.pf.kp=10;
yezhong 5:2503c88a564f 159 a_control.pf.kd=5;
yezhong 5:2503c88a564f 160 a_control.pf.t_ff=0;
yezhong 5:2503c88a564f 161 }
yezhong 5:2503c88a564f 162 */
yezhong 5:2503c88a564f 163 a_control.pf.p_des=PI/32;
yezhong 5:2503c88a564f 164 a_control.pf.v_des=0;
yezhong 5:2503c88a564f 165 a_control.pf.kp=10;
yezhong 5:2503c88a564f 166 a_control.pf.kd=5;
yezhong 5:2503c88a564f 167 a_control.pf.t_ff=0;
yezhong 5:2503c88a564f 168
yezhong 5:2503c88a564f 169
yezhong 5:2503c88a564f 170
yezhong 5:2503c88a564f 171 a_control.df.p_des=PI/4;
yezhong 5:2503c88a564f 172 a_control.df.v_des=0;
yezhong 5:2503c88a564f 173 a_control.df.kp=10;
yezhong 5:2503c88a564f 174 a_control.df.kd=5;
yezhong 5:2503c88a564f 175 a_control.df.t_ff=0;
yezhong 5:2503c88a564f 176
yezhong 5:2503c88a564f 177 if(t.read()<2)
yezhong 5:2503c88a564f 178 {
yezhong 5:2503c88a564f 179 a_control.df1.p_des=PI/4;
yezhong 5:2503c88a564f 180 a_control.df1.v_des=0;
yezhong 5:2503c88a564f 181 a_control.df1.kp=30;
yezhong 5:2503c88a564f 182 a_control.df1.kd=5;
yezhong 5:2503c88a564f 183 a_control.df1.t_ff=0;
yezhong 5:2503c88a564f 184 }
yezhong 5:2503c88a564f 185
yezhong 5:2503c88a564f 186 if(t.read()>2&&t.read()<4)
yezhong 5:2503c88a564f 187 {
yezhong 5:2503c88a564f 188 a_control.df1.p_des=PI/8;
yezhong 5:2503c88a564f 189 a_control.df1.v_des=0;
yezhong 5:2503c88a564f 190 a_control.df1.kp=35;
yezhong 5:2503c88a564f 191 a_control.df1.kd=5;
yezhong 5:2503c88a564f 192 a_control.df1.t_ff=0;
yezhong 5:2503c88a564f 193 }
yezhong 5:2503c88a564f 194 /*
yezhong 5:2503c88a564f 195 if(t.read()>4&&t.read()<6)
yezhong 5:2503c88a564f 196 {
yezhong 5:2503c88a564f 197 t.reset();
yezhong 5:2503c88a564f 198 }
yezhong 5:2503c88a564f 199 */
yezhong 5:2503c88a564f 200
yezhong 5:2503c88a564f 201 PackAll();
yezhong 5:2503c88a564f 202 WriteAll();
yezhong 5:2503c88a564f 203
yezhong 5:2503c88a564f 204
yezhong 5:2503c88a564f 205
yezhong 5:2503c88a564f 206
yezhong 5:2503c88a564f 207
yezhong 5:2503c88a564f 208
yezhong 5: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 5: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 5:2503c88a564f 253 /*
yezhong 5:2503c88a564f 254 t.start();
yezhong 5:2503c88a564f 255 t.read();
yezhong 5:2503c88a564f 256
panzhan 0:d80c66cb1b3a 257
yezhong 3:940a9e40d327 258 a_control.pf.p_des=0;
yezhong 5:2503c88a564f 259 a_control.pf.v_des=-500*2*3.14/60/50;
yezhong 5: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 5:2503c88a564f 265 a_control.df.v_des=-500*2*3.14/60/50;
yezhong 5: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 5:2503c88a564f 269
yezhong 5:2503c88a564f 270 a_control.df1.p_des=0;
yezhong 5:2503c88a564f 271 a_control.df1.v_des=500*2*3.14/60/50;
yezhong 5:2503c88a564f 272 //a_control.df1.v_des=(500*2*3.14/60/50)*sin(0.5f*t);
yezhong 5:2503c88a564f 273 a_control.df1.kp=10;
yezhong 5:2503c88a564f 274 a_control.df1.kd=5;
yezhong 5:2503c88a564f 275 a_control.df1.t_ff=0;
yezhong 3:940a9e40d327 276 PackAll();
yezhong 3:940a9e40d327 277 WriteAll();
yezhong 5:2503c88a564f 278 */
panzhan 0:d80c66cb1b3a 279 ////////////////////////////////////////////////////////////////////////////////////////////////
panzhan 0:d80c66cb1b3a 280 }
panzhan 0:d80c66cb1b3a 281 }