yezhong yezhong / yezhong_main_controller_copy_3_

1233

Dependencies:   cJSON_lib mcp2515- mbed-dev-f303 esp8266 yezhong_main_controller_copy_3

main.cpp@0:e923de71caa5, 2022-06-24 (annotated)

Committer:
yezhong
Date:
Fri Jun 24 07:58:26 2022 +0000
Revision:
0:e923de71caa5
111111

Who changed what in which revision?

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