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?
User | Revision | Line number | New 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 | } |