1
Dependencies: mcp2515 mbed-dev-f303
main.cpp@8:2250c7f70748, 2022-06-24 (annotated)
- Committer:
- yezhong
- Date:
- Fri Jun 24 01:24:33 2022 +0000
- Revision:
- 8:2250c7f70748
- Parent:
- 6:7d2743feaf23
1
Who changed what in which revision?
User | Revision | Line number | New 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 | } |