1
main.cpp@2:e85ebf401989, 2021-11-18 (annotated)
- Committer:
- yezhong
- Date:
- Thu Nov 18 03:14:26 2021 +0000
- Revision:
- 2:e85ebf401989
- Parent:
- 1:a71791b81b8a
- Child:
- 3:ac1062c23c49
1
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 | { |
yezhong | 2:e85ebf401989 | 23 | Timer t; |
yezhong | 2:e85ebf401989 | 24 | float b; |
panzhan | 0:d80c66cb1b3a | 25 | ////////////////////////初始化////////////////////////////////////// |
panzhan | 0:d80c66cb1b3a | 26 | pc.baud(115200); |
panzhan | 0:d80c66cb1b3a | 27 | pc.attach(&serial_pc_isr); |
panzhan | 0:d80c66cb1b3a | 28 | |
yezhong | 2:e85ebf401989 | 29 | //foot.baud(115200); |
yezhong | 2:e85ebf401989 | 30 | // foot.attach(&serial_board_isr); |
panzhan | 0:d80c66cb1b3a | 31 | |
panzhan | 0:d80c66cb1b3a | 32 | // command.baud(115200); |
panzhan | 0:d80c66cb1b3a | 33 | // command.attach(&serial_command_isr); |
panzhan | 0:d80c66cb1b3a | 34 | |
panzhan | 0:d80c66cb1b3a | 35 | ankle_can.frequency(1000000); |
panzhan | 0:d80c66cb1b3a | 36 | ankle_can.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0); |
panzhan | 0:d80c66cb1b3a | 37 | ankle_rxMsg.len = 6; |
panzhan | 0:d80c66cb1b3a | 38 | ankle_txMsg.len = 8; |
panzhan | 0:d80c66cb1b3a | 39 | ankle_txMsg.id = 0x01; |
panzhan | 0:d80c66cb1b3a | 40 | |
panzhan | 0:d80c66cb1b3a | 41 | knee_can.frequency(1000000); |
panzhan | 0:d80c66cb1b3a | 42 | knee_can.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0); |
panzhan | 0:d80c66cb1b3a | 43 | knee_rxMsg.len = 6; |
panzhan | 0:d80c66cb1b3a | 44 | knee_txMsg.len = 8; |
panzhan | 0:d80c66cb1b3a | 45 | knee_txMsg.id = 0x01; |
panzhan | 0:d80c66cb1b3a | 46 | //////////////////////////////////////////////////////////////////////////////// |
yezhong | 2:e85ebf401989 | 47 | wait(8); |
panzhan | 0:d80c66cb1b3a | 48 | |
yezhong | 2:e85ebf401989 | 49 | Zero(&knee_txMsg); |
yezhong | 2:e85ebf401989 | 50 | Zero(&ankle_txMsg); |
yezhong | 2:e85ebf401989 | 51 | //EnterSPEEDMode(&knee_txMsg); |
yezhong | 2:e85ebf401989 | 52 | // EnterSPEEDMode(&ankle_txMsg); |
yezhong | 2:e85ebf401989 | 53 | EnterPositionMode(&knee_txMsg); |
yezhong | 2:e85ebf401989 | 54 | EnterPositionMode(&ankle_txMsg); |
yezhong | 2:e85ebf401989 | 55 | //EnterMotorMode(&knee_txMsg); |
yezhong | 2:e85ebf401989 | 56 | // EnterMotorMode(&ankle_txMsg); |
yezhong | 2:e85ebf401989 | 57 | |
panzhan | 1:a71791b81b8a | 58 | |
panzhan | 0:d80c66cb1b3a | 59 | |
panzhan | 0:d80c66cb1b3a | 60 | |
panzhan | 0:d80c66cb1b3a | 61 | while(1) { |
panzhan | 0:d80c66cb1b3a | 62 | |
panzhan | 0:d80c66cb1b3a | 63 | |
panzhan | 0:d80c66cb1b3a | 64 | ankle_can.read(ankle_rxMsg); |
panzhan | 0:d80c66cb1b3a | 65 | unpack_reply(ankle_rxMsg, &a_state); |
panzhan | 0:d80c66cb1b3a | 66 | wait_us(10); |
panzhan | 0:d80c66cb1b3a | 67 | knee_can.read(knee_rxMsg); |
panzhan | 0:d80c66cb1b3a | 68 | unpack_reply(knee_rxMsg, &a_state); |
panzhan | 0:d80c66cb1b3a | 69 | |
panzhan | 1:a71791b81b8a | 70 | float ankp = a_state.ankle_state.p; // 从CAN获得的当前位置 |
panzhan | 1:a71791b81b8a | 71 | float ankv = a_state.ankle_state.v; |
panzhan | 1:a71791b81b8a | 72 | float ankt = a_state.ankle_state.t; |
panzhan | 0:d80c66cb1b3a | 73 | |
panzhan | 1:a71791b81b8a | 74 | float knp = a_state.knee_state.p; |
panzhan | 1:a71791b81b8a | 75 | float knv = a_state.knee_state.v; |
panzhan | 1:a71791b81b8a | 76 | float knt = a_state.knee_state.t; |
panzhan | 0:d80c66cb1b3a | 77 | |
yezhong | 2:e85ebf401989 | 78 | // pc.printf("%.3f--%.3f--%.3f==================%.3f--%.3f--%.3f\n",ankp,ankv,ankt,knp,knv,knt); |
yezhong | 2:e85ebf401989 | 79 | //pc.printf("%.3fa%.3fa%.3fa%.3fa%.3fa%.3f\n\r",ankp,ankv,ankt,knp,knv,knt); |
yezhong | 2:e85ebf401989 | 80 | pc.printf("%.3fa%.3fa%.3fa%.3fa%.3fa%.3fa%.3f\n\r",a_control.knee.p_des,a_control.knee.v_des,a_control.knee.t_ff,knp,knv,knt,b); |
yezhong | 2:e85ebf401989 | 81 | |
yezhong | 2:e85ebf401989 | 82 | |
yezhong | 2:e85ebf401989 | 83 | //////////////////////////////////////////position///////////////////////////////////////// |
yezhong | 2:e85ebf401989 | 84 | |
yezhong | 2:e85ebf401989 | 85 | t.start(); |
yezhong | 2:e85ebf401989 | 86 | t.read(); |
yezhong | 2:e85ebf401989 | 87 | b=t.read(); |
yezhong | 2:e85ebf401989 | 88 | a_control.ankle.p_des=PI*sin(t); |
yezhong | 2:e85ebf401989 | 89 | //a_control.ankle.p_des=PI/8; |
yezhong | 2:e85ebf401989 | 90 | a_control.ankle.v_des=0; |
yezhong | 2:e85ebf401989 | 91 | a_control.ankle.kp=7; |
yezhong | 2:e85ebf401989 | 92 | a_control.ankle.kd=5; |
yezhong | 2:e85ebf401989 | 93 | a_control.ankle.t_ff=0; |
yezhong | 2:e85ebf401989 | 94 | |
yezhong | 2:e85ebf401989 | 95 | a_control.knee.p_des=PI*sin(0.5f*t); |
yezhong | 2:e85ebf401989 | 96 | //a_control.knee.p_des=PI/4; |
yezhong | 2:e85ebf401989 | 97 | a_control.knee.v_des=0; |
yezhong | 2:e85ebf401989 | 98 | a_control.knee.kp=10.5; |
yezhong | 2:e85ebf401989 | 99 | a_control.knee.kd=1; |
yezhong | 2:e85ebf401989 | 100 | a_control.knee.t_ff=0; |
yezhong | 2:e85ebf401989 | 101 | |
yezhong | 2:e85ebf401989 | 102 | // if(a_control.knee.v_des>=0) |
yezhong | 2:e85ebf401989 | 103 | // a_control.knee.t_ff=2; |
yezhong | 2:e85ebf401989 | 104 | // else |
yezhong | 2:e85ebf401989 | 105 | // a_control.knee.t_ff=-2; |
yezhong | 2:e85ebf401989 | 106 | |
yezhong | 2:e85ebf401989 | 107 | PackAll(); |
yezhong | 2:e85ebf401989 | 108 | WriteAll(); |
panzhan | 0:d80c66cb1b3a | 109 | |
panzhan | 0:d80c66cb1b3a | 110 | ///////////////////////////////////////velocity/////////////////////////////////////////////// |
yezhong | 2:e85ebf401989 | 111 | /* |
yezhong | 2:e85ebf401989 | 112 | t.start(); |
yezhong | 2:e85ebf401989 | 113 | t.read(); |
yezhong | 2:e85ebf401989 | 114 | |
yezhong | 2:e85ebf401989 | 115 | a_control.ankle.p_des=0; |
yezhong | 2:e85ebf401989 | 116 | a_control.ankle.v_des=800*2*3.14/60/50; |
yezhong | 2:e85ebf401989 | 117 | //a_control.ankle.v_des=(500*2*3.14/60/50)*sin(t); |
yezhong | 2:e85ebf401989 | 118 | a_control.ankle.kp=0; |
yezhong | 2:e85ebf401989 | 119 | a_control.ankle.kd=5; |
yezhong | 2:e85ebf401989 | 120 | a_control.ankle.t_ff=0; |
panzhan | 0:d80c66cb1b3a | 121 | |
yezhong | 2:e85ebf401989 | 122 | a_control.knee.p_des=0; |
yezhong | 2:e85ebf401989 | 123 | //a_control.knee.v_des=800*2*3.14/60/50; |
yezhong | 2:e85ebf401989 | 124 | a_control.knee.v_des=(800*2*3.14/60/50)*sin(0.5f*t); |
yezhong | 2:e85ebf401989 | 125 | a_control.knee.kp=20; |
yezhong | 2:e85ebf401989 | 126 | a_control.knee.kd=1; |
yezhong | 2:e85ebf401989 | 127 | a_control.knee.t_ff=0; |
yezhong | 2:e85ebf401989 | 128 | |
yezhong | 2:e85ebf401989 | 129 | //if(a_control.knee.v_des>=0) |
yezhong | 2:e85ebf401989 | 130 | // a_control.knee.t_ff=7.5; |
yezhong | 2:e85ebf401989 | 131 | // else |
yezhong | 2:e85ebf401989 | 132 | // a_control.knee.t_ff=-5; |
yezhong | 2:e85ebf401989 | 133 | |
yezhong | 2:e85ebf401989 | 134 | PackAll(); |
yezhong | 2:e85ebf401989 | 135 | WriteAll(); |
yezhong | 2:e85ebf401989 | 136 | |
yezhong | 2:e85ebf401989 | 137 | |
yezhong | 2:e85ebf401989 | 138 | */ |
panzhan | 0:d80c66cb1b3a | 139 | //////////////////////////////////////////////////////////////////////////////////////////////// |
panzhan | 0:d80c66cb1b3a | 140 | } |
panzhan | 0:d80c66cb1b3a | 141 | } |