111
Dependencies: yezhong_main_controller_copy mbed1-dev
main.cpp@10:f83b0935a8f0, 2022-07-09 (annotated)
- Committer:
- ganlikun
- Date:
- Sat Jul 09 11:58:13 2022 +0000
- Revision:
- 10:f83b0935a8f0
- Parent:
- 9:c082f1c52936
00000
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" |
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 | |
ganlikun | 9:c082f1c52936 | 18 | int main() |
panzhan | 0:d80c66cb1b3a | 19 | { |
ganlikun | 10:f83b0935a8f0 | 20 | Timer tim; |
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 | |
ganlikun | 4:3f7a59212142 | 34 | pf_can.frequency(800000); |
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; |
ganlikun | 10:f83b0935a8f0 | 38 | PF_can.id = 0x02; |
yezhong | 2:cd74a8cb03b0 | 39 | |
ganlikun | 4:3f7a59212142 | 40 | df_can.frequency(800000); |
yezhong | 2:cd74a8cb03b0 | 41 | df_can.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0); |
yezhong | 2:cd74a8cb03b0 | 42 | df_rxMsg.len = 6; |
ganlikun | 9:c082f1c52936 | 43 | DF_can.len = 8; |
ganlikun | 4:3f7a59212142 | 44 | DF_can.id = 0x02; |
yezhong | 2:cd74a8cb03b0 | 45 | |
yezhong | 2:cd74a8cb03b0 | 46 | /////////////////////////////////////position/////////////////////////////////////////// |
ganlikun | 4:3f7a59212142 | 47 | wait(6); |
yezhong | 3:940a9e40d327 | 48 | Zero(&PF_can); |
ganlikun | 9:c082f1c52936 | 49 | EnterMotorMode(&PF_can); |
ganlikun | 9:c082f1c52936 | 50 | |
ganlikun | 4:3f7a59212142 | 51 | Zero(&DF_can);//0位置 |
ganlikun | 9:c082f1c52936 | 52 | EnterMotorMode(&DF_can);//电机模式 |
yezhong | 3:940a9e40d327 | 53 | |
yezhong | 3:940a9e40d327 | 54 | /* |
yezhong | 2:cd74a8cb03b0 | 55 | a_control.pf.p_des=PI/8; |
yezhong | 2:cd74a8cb03b0 | 56 | a_control.pf.v_des=0; |
yezhong | 2:cd74a8cb03b0 | 57 | a_control.pf.kp=7; |
yezhong | 2:cd74a8cb03b0 | 58 | a_control.pf.kd=0; |
yezhong | 2:cd74a8cb03b0 | 59 | a_control.pf.t_ff=0; |
panzhan | 1:a71791b81b8a | 60 | |
yezhong | 2:cd74a8cb03b0 | 61 | a_control.df.p_des=PI/8; |
yezhong | 2:cd74a8cb03b0 | 62 | a_control.df.v_des=0; |
yezhong | 2:cd74a8cb03b0 | 63 | a_control.df.kp=7; |
yezhong | 2:cd74a8cb03b0 | 64 | a_control.df.kd=0; |
yezhong | 2:cd74a8cb03b0 | 65 | a_control.df.t_ff=0; |
yezhong | 2:cd74a8cb03b0 | 66 | |
yezhong | 2:cd74a8cb03b0 | 67 | |
panzhan | 1:a71791b81b8a | 68 | PackAll(); |
yezhong | 3:940a9e40d327 | 69 | WriteAll(); */ |
panzhan | 0:d80c66cb1b3a | 70 | |
panzhan | 0:d80c66cb1b3a | 71 | |
panzhan | 0:d80c66cb1b3a | 72 | while(1) { |
yezhong | 2:cd74a8cb03b0 | 73 | |
yezhong | 2:cd74a8cb03b0 | 74 | pf_can.read(pf_rxMsg); |
yezhong | 2:cd74a8cb03b0 | 75 | unpack_reply(pf_rxMsg, &a_state); |
panzhan | 0:d80c66cb1b3a | 76 | wait_us(10); |
yezhong | 2:cd74a8cb03b0 | 77 | df_can.read(df_rxMsg); |
yezhong | 2:cd74a8cb03b0 | 78 | unpack_reply(df_rxMsg, &a_state); |
panzhan | 0:d80c66cb1b3a | 79 | |
yezhong | 2:cd74a8cb03b0 | 80 | float pfkp = a_state.pf.p; // 从CAN获得的当前位置 |
yezhong | 2:cd74a8cb03b0 | 81 | float pfkv = a_state.pf.v; |
yezhong | 2:cd74a8cb03b0 | 82 | float pfkt = a_state.pf.t; |
panzhan | 0:d80c66cb1b3a | 83 | |
ganlikun | 9:c082f1c52936 | 84 | float dfp = a_state.df.p; |
ganlikun | 9:c082f1c52936 | 85 | float dfv = a_state.df.v; |
ganlikun | 9:c082f1c52936 | 86 | float dft = a_state.df.t; |
yezhong | 3:940a9e40d327 | 87 | |
yezhong | 3:940a9e40d327 | 88 | // pc.printf("%.3f--%.3f--%.3f==================%.3f--%.3f--%.3f\n",pfkp,pfkv,pfkt,dfp,dfv,dft); |
yezhong | 3:940a9e40d327 | 89 | // pc.printf("%.4f,%.3f,%.3f,%.4f,%.3f,%.4f\n",pfkp,pfkv,pfkt,a_control.pf.p_des,j,P); |
yezhong | 3:940a9e40d327 | 90 | |
yezhong | 3:940a9e40d327 | 91 | pc.printf("%.3f,%.3f,%.3f,%.3f\n",pfkp,pfkv,pfkt,a_control.pf.v_des); |
yezhong | 3:940a9e40d327 | 92 | /////////////////////////////////////////////////trajectory///////////////////////////////////////////////////// |
yezhong | 3:940a9e40d327 | 93 | /* |
yezhong | 3:940a9e40d327 | 94 | j=j+0.001; |
yezhong | 3:940a9e40d327 | 95 | 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 | 96 | if(j>=0.558&&j<=0.85) |
yezhong | 3:940a9e40d327 | 97 | { |
yezhong | 3:940a9e40d327 | 98 | a_control.pf.p_des=P; |
yezhong | 3:940a9e40d327 | 99 | a_control.pf.v_des=0; |
yezhong | 3:940a9e40d327 | 100 | a_control.pf.kp=80; |
yezhong | 3:940a9e40d327 | 101 | a_control.pf.kd=0; |
yezhong | 3:940a9e40d327 | 102 | a_control.pf.t_ff=0; |
yezhong | 3:940a9e40d327 | 103 | } |
yezhong | 3:940a9e40d327 | 104 | |
yezhong | 3:940a9e40d327 | 105 | |
yezhong | 3:940a9e40d327 | 106 | PackAll(); |
yezhong | 3:940a9e40d327 | 107 | WriteAll(); |
yezhong | 3:940a9e40d327 | 108 | */ |
panzhan | 0:d80c66cb1b3a | 109 | |
yezhong | 3:940a9e40d327 | 110 | |
yezhong | 3:940a9e40d327 | 111 | /////////////////////////////////////position/////////////////////////////////////////// |
yezhong | 3:940a9e40d327 | 112 | /* |
yezhong | 3:940a9e40d327 | 113 | a_control.pf.p_des=PI/8; |
yezhong | 3:940a9e40d327 | 114 | a_control.pf.v_des=0; |
yezhong | 3:940a9e40d327 | 115 | a_control.pf.kp=10; |
yezhong | 3:940a9e40d327 | 116 | a_control.pf.kd=0; |
yezhong | 3:940a9e40d327 | 117 | a_control.pf.t_ff=0; |
yezhong | 3:940a9e40d327 | 118 | PackAll(); |
yezhong | 3:940a9e40d327 | 119 | WriteAll(); |
yezhong | 3:940a9e40d327 | 120 | */ |
yezhong | 3:940a9e40d327 | 121 | |
yezhong | 3:940a9e40d327 | 122 | /* |
yezhong | 3:940a9e40d327 | 123 | t.start(); |
yezhong | 3:940a9e40d327 | 124 | t.read(); |
yezhong | 3:940a9e40d327 | 125 | if(t.read()<2) |
yezhong | 3:940a9e40d327 | 126 | { |
yezhong | 3:940a9e40d327 | 127 | a_control.pf.p_des=a; |
yezhong | 3:940a9e40d327 | 128 | a_control.pf.v_des=0; |
yezhong | 3:940a9e40d327 | 129 | a_control.pf.kp=10; |
yezhong | 3:940a9e40d327 | 130 | a_control.pf.kd=0; |
yezhong | 3:940a9e40d327 | 131 | a_control.pf.t_ff=0; |
yezhong | 3:940a9e40d327 | 132 | } |
yezhong | 3:940a9e40d327 | 133 | if(t.read()>3) |
yezhong | 3:940a9e40d327 | 134 | { |
yezhong | 3:940a9e40d327 | 135 | t.reset(); |
yezhong | 3:940a9e40d327 | 136 | Zero(&PF_can); |
yezhong | 3:940a9e40d327 | 137 | a=-a; |
yezhong | 3:940a9e40d327 | 138 | } |
yezhong | 3:940a9e40d327 | 139 | |
yezhong | 3:940a9e40d327 | 140 | PackAll(); |
yezhong | 3:940a9e40d327 | 141 | WriteAll(); |
yezhong | 3:940a9e40d327 | 142 | |
yezhong | 3:940a9e40d327 | 143 | */ |
yezhong | 3:940a9e40d327 | 144 | |
yezhong | 3:940a9e40d327 | 145 | |
yezhong | 3:940a9e40d327 | 146 | |
yezhong | 3:940a9e40d327 | 147 | |
yezhong | 3:940a9e40d327 | 148 | ///////////////////////////////////////////////////////////////////////////////////////////// |
panzhan | 0:d80c66cb1b3a | 149 | |
panzhan | 0:d80c66cb1b3a | 150 | ///////////////////////////////////////velocity/////////////////////////////////////////////// |
ganlikun | 10:f83b0935a8f0 | 151 | tim.start(); |
ganlikun | 10:f83b0935a8f0 | 152 | // float timeRecord() |
ganlikun | 10:f83b0935a8f0 | 153 | float s =tim.read(); |
ganlikun | 10:f83b0935a8f0 | 154 | if(s >= 10.0f) { |
ganlikun | 10:f83b0935a8f0 | 155 | tim.reset(); |
ganlikun | 10:f83b0935a8f0 | 156 | } |
ganlikun | 10:f83b0935a8f0 | 157 | |
yezhong | 3:940a9e40d327 | 158 | a_control.pf.p_des=0; |
ganlikun | 10:f83b0935a8f0 | 159 | a_control.pf.v_des=500*2*3.14/60/50*(sin(3.14/5*s)+1); |
yezhong | 3:940a9e40d327 | 160 | a_control.pf.kp=0; |
yezhong | 3:940a9e40d327 | 161 | a_control.pf.kd=5; |
yezhong | 3:940a9e40d327 | 162 | a_control.pf.t_ff=0; |
yezhong | 3:940a9e40d327 | 163 | |
yezhong | 3:940a9e40d327 | 164 | a_control.df.p_des=0; |
ganlikun | 10:f83b0935a8f0 | 165 | a_control.df.v_des=100*2*3.14/60/50*s; |
ganlikun | 10:f83b0935a8f0 | 166 | // a_control.df.v_des=500*2*3.14/60/50;//50减速比, |
yezhong | 3:940a9e40d327 | 167 | a_control.df.kp=0; |
yezhong | 3:940a9e40d327 | 168 | a_control.df.kd=5; |
yezhong | 3:940a9e40d327 | 169 | a_control.df.t_ff=0; |
ganlikun | 9:c082f1c52936 | 170 | |
yezhong | 3:940a9e40d327 | 171 | PackAll(); |
yezhong | 3:940a9e40d327 | 172 | WriteAll(); |
panzhan | 0:d80c66cb1b3a | 173 | |
panzhan | 0:d80c66cb1b3a | 174 | //////////////////////////////////////////////////////////////////////////////////////////////// |
panzhan | 0:d80c66cb1b3a | 175 | } |
ganlikun | 10:f83b0935a8f0 | 176 | } |