1
Dependencies: mcp2515 mbed-dev-f303
main.cpp@2:cd74a8cb03b0, 2020-11-19 (annotated)
- Committer:
- yezhong
- Date:
- Thu Nov 19 07:59:28 2020 +0000
- Revision:
- 2:cd74a8cb03b0
- Parent:
- 1:a71791b81b8a
- Child:
- 3:940a9e40d327
11
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 | //ankle----pf |
panzhan | 0:d80c66cb1b3a | 18 | //knee-----df |
panzhan | 0:d80c66cb1b3a | 19 | |
panzhan | 0:d80c66cb1b3a | 20 | |
panzhan | 0:d80c66cb1b3a | 21 | int main() |
panzhan | 0:d80c66cb1b3a | 22 | { |
panzhan | 0:d80c66cb1b3a | 23 | |
panzhan | 0:d80c66cb1b3a | 24 | ////////////////////////初始化////////////////////////////////////// |
panzhan | 0:d80c66cb1b3a | 25 | pc.baud(115200); |
panzhan | 0:d80c66cb1b3a | 26 | pc.attach(&serial_pc_isr); |
panzhan | 0:d80c66cb1b3a | 27 | |
yezhong | 2:cd74a8cb03b0 | 28 | // foot.baud(115200); |
yezhong | 2:cd74a8cb03b0 | 29 | // foot.attach(&serial_board_isr); |
panzhan | 0:d80c66cb1b3a | 30 | |
yezhong | 2:cd74a8cb03b0 | 31 | // command.baud(115200); |
yezhong | 2:cd74a8cb03b0 | 32 | // command.attach(&serial_command_isr); |
panzhan | 0:d80c66cb1b3a | 33 | |
yezhong | 2:cd74a8cb03b0 | 34 | |
yezhong | 2:cd74a8cb03b0 | 35 | pf_can.frequency(1000000); |
yezhong | 2:cd74a8cb03b0 | 36 | pf_can.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0); |
yezhong | 2:cd74a8cb03b0 | 37 | pf_rxMsg.len = 6; |
yezhong | 2:cd74a8cb03b0 | 38 | PF_can.len = 8; |
yezhong | 2:cd74a8cb03b0 | 39 | PF_can.id = 0x01; |
yezhong | 2:cd74a8cb03b0 | 40 | |
yezhong | 2:cd74a8cb03b0 | 41 | df_can.frequency(1000000); |
yezhong | 2:cd74a8cb03b0 | 42 | df_can.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0); |
yezhong | 2:cd74a8cb03b0 | 43 | df_rxMsg.len = 6; |
yezhong | 2:cd74a8cb03b0 | 44 | DF_can.len = 8; |
yezhong | 2:cd74a8cb03b0 | 45 | DF_can.id = 0x01; |
yezhong | 2:cd74a8cb03b0 | 46 | |
yezhong | 2:cd74a8cb03b0 | 47 | /////////////////////////////////////position/////////////////////////////////////////// |
panzhan | 0:d80c66cb1b3a | 48 | wait(5); |
panzhan | 0:d80c66cb1b3a | 49 | |
yezhong | 2:cd74a8cb03b0 | 50 | EnterMotorMode(&PF_can); |
yezhong | 2:cd74a8cb03b0 | 51 | EnterMotorMode(&DF_can); |
yezhong | 2:cd74a8cb03b0 | 52 | Zero(&PF_can); |
yezhong | 2:cd74a8cb03b0 | 53 | Zero(&DF_can); |
panzhan | 0:d80c66cb1b3a | 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(); |
panzhan | 1:a71791b81b8a | 69 | WriteAll(); |
panzhan | 0:d80c66cb1b3a | 70 | |
panzhan | 0:d80c66cb1b3a | 71 | |
panzhan | 0:d80c66cb1b3a | 72 | while(1) { |
panzhan | 0:d80c66cb1b3a | 73 | |
panzhan | 0:d80c66cb1b3a | 74 | |
yezhong | 2:cd74a8cb03b0 | 75 | |
yezhong | 2:cd74a8cb03b0 | 76 | |
yezhong | 2:cd74a8cb03b0 | 77 | pf_can.read(pf_rxMsg); |
yezhong | 2:cd74a8cb03b0 | 78 | unpack_reply(pf_rxMsg, &a_state); |
panzhan | 0:d80c66cb1b3a | 79 | wait_us(10); |
yezhong | 2:cd74a8cb03b0 | 80 | df_can.read(df_rxMsg); |
yezhong | 2:cd74a8cb03b0 | 81 | unpack_reply(df_rxMsg, &a_state); |
panzhan | 0:d80c66cb1b3a | 82 | |
yezhong | 2:cd74a8cb03b0 | 83 | float pfkp = a_state.pf.p; // 从CAN获得的当前位置 |
yezhong | 2:cd74a8cb03b0 | 84 | float pfkv = a_state.pf.v; |
yezhong | 2:cd74a8cb03b0 | 85 | float pfkt = a_state.pf.t; |
panzhan | 0:d80c66cb1b3a | 86 | |
yezhong | 2:cd74a8cb03b0 | 87 | float dfp = a_state.df.p; |
yezhong | 2:cd74a8cb03b0 | 88 | float dfv = a_state.df.v; |
yezhong | 2:cd74a8cb03b0 | 89 | float dft = a_state.df.t; |
panzhan | 0:d80c66cb1b3a | 90 | |
yezhong | 2:cd74a8cb03b0 | 91 | pc.printf("%.3f--%.3f--%.3f==================%.3f--%.3f--%.3f\n",pfkp,pfkv,pfkt,dfp,dfv,dft); |
panzhan | 0:d80c66cb1b3a | 92 | |
panzhan | 0:d80c66cb1b3a | 93 | ///////////////////////////////////////velocity/////////////////////////////////////////////// |
panzhan | 0:d80c66cb1b3a | 94 | |
yezhong | 2:cd74a8cb03b0 | 95 | // a_control.pf.p_des=0; |
yezhong | 2:cd74a8cb03b0 | 96 | // a_control.pf.v_des=100*2*3.14/60/49; |
yezhong | 2:cd74a8cb03b0 | 97 | // a_control.pf.kp=0; |
yezhong | 2:cd74a8cb03b0 | 98 | // a_control.pf.kd=4; |
yezhong | 2:cd74a8cb03b0 | 99 | // a_control.pf.t_ff=0; |
panzhan | 0:d80c66cb1b3a | 100 | // |
yezhong | 2:cd74a8cb03b0 | 101 | // a_control.df.p_des=0; |
yezhong | 2:cd74a8cb03b0 | 102 | // a_control.df.v_des=100*2*3.14/60/49; |
yezhong | 2:cd74a8cb03b0 | 103 | // a_control.df.kp=0; |
yezhong | 2:cd74a8cb03b0 | 104 | // a_control.df.kd=4; |
yezhong | 2:cd74a8cb03b0 | 105 | // a_control.df.t_ff=0; |
panzhan | 0:d80c66cb1b3a | 106 | // PackAll(); |
panzhan | 0:d80c66cb1b3a | 107 | // WriteAll(); |
panzhan | 0:d80c66cb1b3a | 108 | |
panzhan | 0:d80c66cb1b3a | 109 | //////////////////////////////////////////////////////////////////////////////////////////////// |
panzhan | 0:d80c66cb1b3a | 110 | } |
panzhan | 0:d80c66cb1b3a | 111 | } |