Gan likun / yezhong_main_controller_copy001

111

Dependencies:   yezhong_main_controller_copy mbed1-dev

main.cpp@7:d1b09098579b, 2022-02-22 (annotated)

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