Gan likun / yezhong_main_controller_copy001

111

Dependencies:   yezhong_main_controller_copy mbed1-dev

main.cpp@3:940a9e40d327, 2021-06-30 (annotated)

Committer:
yezhong
Date:
Wed Jun 30 12:15:37 2021 +0000
Revision:
3:940a9e40d327
Parent:
2:cd74a8cb03b0
Child:
4:3f7a59212142
Child:
5:2503c88a564f
111

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"
panzhan 0:d80c66cb1b3a 11
panzhan 0:d80c66cb1b3a 12
panzhan 0:d80c66cb1b3a 13 ////////////////////////////////////////////////////////////////////////////////
panzhan 0:d80c66cb1b3a 14 // 框架搭建完毕 //
panzhan 0:d80c66cb1b3a 15 ////////////////////////////////////////////////////////////////////////////////
panzhan 0:d80c66cb1b3a 16
panzhan 0:d80c66cb1b3a 17
panzhan 0:d80c66cb1b3a 18 int main()
panzhan 0:d80c66cb1b3a 19 {
yezhong 3:940a9e40d327 20 //Timer t;
yezhong 3:940a9e40d327 21 //float a=PI/8;
yezhong 3:940a9e40d327 22 //float j=0.558,P=0;
panzhan 0:d80c66cb1b3a 23 ////////////////////////初始化//////////////////////////////////////
panzhan 0:d80c66cb1b3a 24 pc.baud(115200);
panzhan 0:d80c66cb1b3a 25 pc.attach(&serial_pc_isr);
panzhan 0:d80c66cb1b3a 26
yezhong 2:cd74a8cb03b0 27 // foot.baud(115200);
yezhong 2:cd74a8cb03b0 28 // foot.attach(&serial_board_isr);
panzhan 0:d80c66cb1b3a 29
yezhong 2:cd74a8cb03b0 30 // command.baud(115200);
yezhong 2:cd74a8cb03b0 31 // command.attach(&serial_command_isr);
panzhan 0:d80c66cb1b3a 32
yezhong 2:cd74a8cb03b0 33
yezhong 2:cd74a8cb03b0 34 pf_can.frequency(1000000);
yezhong 2:cd74a8cb03b0 35 pf_can.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0);
yezhong 2:cd74a8cb03b0 36 pf_rxMsg.len = 6;
yezhong 2:cd74a8cb03b0 37 PF_can.len = 8;
yezhong 3:940a9e40d327 38 PF_can.id = 0x03;
yezhong 2:cd74a8cb03b0 39
yezhong 2:cd74a8cb03b0 40 df_can.frequency(1000000);
yezhong 2:cd74a8cb03b0 41 df_can.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0);
yezhong 2:cd74a8cb03b0 42 df_rxMsg.len = 6;
yezhong 2:cd74a8cb03b0 43 DF_can.len = 8;
yezhong 3:940a9e40d327 44 DF_can.id = 0x03;
yezhong 2:cd74a8cb03b0 45
yezhong 2:cd74a8cb03b0 46 /////////////////////////////////////position///////////////////////////////////////////
yezhong 3:940a9e40d327 47 wait(2);
yezhong 3:940a9e40d327 48 Zero(&PF_can);
yezhong 3:940a9e40d327 49 Zero(&DF_can);
yezhong 2:cd74a8cb03b0 50 EnterMotorMode(&PF_can);
yezhong 2:cd74a8cb03b0 51 EnterMotorMode(&DF_can);
yezhong 3:940a9e40d327 52
yezhong 3:940a9e40d327 53 /*
yezhong 2:cd74a8cb03b0 54 a_control.pf.p_des=PI/8;
yezhong 2:cd74a8cb03b0 55 a_control.pf.v_des=0;
yezhong 2:cd74a8cb03b0 56 a_control.pf.kp=7;
yezhong 2:cd74a8cb03b0 57 a_control.pf.kd=0;
yezhong 2:cd74a8cb03b0 58 a_control.pf.t_ff=0;
panzhan 1:a71791b81b8a 59
yezhong 2:cd74a8cb03b0 60 a_control.df.p_des=PI/8;
yezhong 2:cd74a8cb03b0 61 a_control.df.v_des=0;
yezhong 2:cd74a8cb03b0 62 a_control.df.kp=7;
yezhong 2:cd74a8cb03b0 63 a_control.df.kd=0;
yezhong 2:cd74a8cb03b0 64 a_control.df.t_ff=0;
yezhong 2:cd74a8cb03b0 65
yezhong 2:cd74a8cb03b0 66
panzhan 1:a71791b81b8a 67 PackAll();
yezhong 3:940a9e40d327 68 WriteAll(); */
panzhan 0:d80c66cb1b3a 69
panzhan 0:d80c66cb1b3a 70
panzhan 0:d80c66cb1b3a 71 while(1) {
yezhong 2:cd74a8cb03b0 72
yezhong 2:cd74a8cb03b0 73 pf_can.read(pf_rxMsg);
yezhong 2:cd74a8cb03b0 74 unpack_reply(pf_rxMsg, &a_state);
panzhan 0:d80c66cb1b3a 75 wait_us(10);
yezhong 2:cd74a8cb03b0 76 df_can.read(df_rxMsg);
yezhong 2:cd74a8cb03b0 77 unpack_reply(df_rxMsg, &a_state);
panzhan 0:d80c66cb1b3a 78
yezhong 2:cd74a8cb03b0 79 float pfkp = a_state.pf.p; // 从CAN获得的当前位置
yezhong 2:cd74a8cb03b0 80 float pfkv = a_state.pf.v;
yezhong 2:cd74a8cb03b0 81 float pfkt = a_state.pf.t;
panzhan 0:d80c66cb1b3a 82
yezhong 3:940a9e40d327 83 //float dfp = a_state.df.p;
yezhong 3:940a9e40d327 84 // float dfv = a_state.df.v;
yezhong 3:940a9e40d327 85 // float dft = a_state.df.t;
yezhong 3:940a9e40d327 86
yezhong 3:940a9e40d327 87 // pc.printf("%.3f--%.3f--%.3f==================%.3f--%.3f--%.3f\n",pfkp,pfkv,pfkt,dfp,dfv,dft);
yezhong 3:940a9e40d327 88 // pc.printf("%.4f,%.3f,%.3f,%.4f,%.3f,%.4f\n",pfkp,pfkv,pfkt,a_control.pf.p_des,j,P);
yezhong 3:940a9e40d327 89
yezhong 3:940a9e40d327 90 pc.printf("%.3f,%.3f,%.3f,%.3f\n",pfkp,pfkv,pfkt,a_control.pf.v_des);
yezhong 3:940a9e40d327 91 /////////////////////////////////////////////////trajectory/////////////////////////////////////////////////////
yezhong 3:940a9e40d327 92 /*
yezhong 3:940a9e40d327 93 j=j+0.001;
yezhong 3:940a9e40d327 94 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 95 if(j>=0.558&&j<=0.85)
yezhong 3:940a9e40d327 96 {
yezhong 3:940a9e40d327 97 a_control.pf.p_des=P;
yezhong 3:940a9e40d327 98 a_control.pf.v_des=0;
yezhong 3:940a9e40d327 99 a_control.pf.kp=80;
yezhong 3:940a9e40d327 100 a_control.pf.kd=0;
yezhong 3:940a9e40d327 101 a_control.pf.t_ff=0;
yezhong 3:940a9e40d327 102 }
yezhong 3:940a9e40d327 103
yezhong 3:940a9e40d327 104
yezhong 3:940a9e40d327 105 PackAll();
yezhong 3:940a9e40d327 106 WriteAll();
yezhong 3:940a9e40d327 107 */
panzhan 0:d80c66cb1b3a 108
yezhong 3:940a9e40d327 109
yezhong 3:940a9e40d327 110 /////////////////////////////////////position///////////////////////////////////////////
yezhong 3:940a9e40d327 111 /*
yezhong 3:940a9e40d327 112 a_control.pf.p_des=PI/8;
yezhong 3:940a9e40d327 113 a_control.pf.v_des=0;
yezhong 3:940a9e40d327 114 a_control.pf.kp=10;
yezhong 3:940a9e40d327 115 a_control.pf.kd=0;
yezhong 3:940a9e40d327 116 a_control.pf.t_ff=0;
yezhong 3:940a9e40d327 117 PackAll();
yezhong 3:940a9e40d327 118 WriteAll();
yezhong 3:940a9e40d327 119 */
yezhong 3:940a9e40d327 120
yezhong 3:940a9e40d327 121 /*
yezhong 3:940a9e40d327 122 t.start();
yezhong 3:940a9e40d327 123 t.read();
yezhong 3:940a9e40d327 124 if(t.read()<2)
yezhong 3:940a9e40d327 125 {
yezhong 3:940a9e40d327 126 a_control.pf.p_des=a;
yezhong 3:940a9e40d327 127 a_control.pf.v_des=0;
yezhong 3:940a9e40d327 128 a_control.pf.kp=10;
yezhong 3:940a9e40d327 129 a_control.pf.kd=0;
yezhong 3:940a9e40d327 130 a_control.pf.t_ff=0;
yezhong 3:940a9e40d327 131 }
yezhong 3:940a9e40d327 132 if(t.read()>3)
yezhong 3:940a9e40d327 133 {
yezhong 3:940a9e40d327 134 t.reset();
yezhong 3:940a9e40d327 135 Zero(&PF_can);
yezhong 3:940a9e40d327 136 a=-a;
yezhong 3:940a9e40d327 137 }
yezhong 3:940a9e40d327 138
yezhong 3:940a9e40d327 139 PackAll();
yezhong 3:940a9e40d327 140 WriteAll();
yezhong 3:940a9e40d327 141
yezhong 3:940a9e40d327 142 */
yezhong 3:940a9e40d327 143
yezhong 3:940a9e40d327 144
yezhong 3:940a9e40d327 145
yezhong 3:940a9e40d327 146
yezhong 3:940a9e40d327 147 /////////////////////////////////////////////////////////////////////////////////////////////
panzhan 0:d80c66cb1b3a 148
panzhan 0:d80c66cb1b3a 149 ///////////////////////////////////////velocity///////////////////////////////////////////////
panzhan 0:d80c66cb1b3a 150
yezhong 3:940a9e40d327 151 a_control.pf.p_des=0;
yezhong 3:940a9e40d327 152 a_control.pf.v_des=500*2*3.14/60/49;
yezhong 3:940a9e40d327 153 a_control.pf.kp=0;
yezhong 3:940a9e40d327 154 a_control.pf.kd=5;
yezhong 3:940a9e40d327 155 a_control.pf.t_ff=0;
yezhong 3:940a9e40d327 156
yezhong 3:940a9e40d327 157 a_control.df.p_des=0;
yezhong 3:940a9e40d327 158 a_control.df.v_des=-100*2*3.14/60/49;
yezhong 3:940a9e40d327 159 a_control.df.kp=0;
yezhong 3:940a9e40d327 160 a_control.df.kd=5;
yezhong 3:940a9e40d327 161 a_control.df.t_ff=0;
yezhong 3:940a9e40d327 162 PackAll();
yezhong 3:940a9e40d327 163 WriteAll();
panzhan 0:d80c66cb1b3a 164
panzhan 0:d80c66cb1b3a 165 ////////////////////////////////////////////////////////////////////////////////////////////////
panzhan 0:d80c66cb1b3a 166 }
panzhan 0:d80c66cb1b3a 167 }