Gan likun / yezhong_main_controller_copy1

111

Dependencies:   yezhong_main_controller_copy mbed1-dev

main.cpp@0:d80c66cb1b3a, 2020-11-10 (annotated)

Committer:
panzhan
Date:
Tue Nov 10 09:09:58 2020 +0000
Revision:
0:d80c66cb1b3a
Child:
1:a71791b81b8a
panzhan_main_controller_copy2

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 //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
panzhan 0:d80c66cb1b3a 28 foot.baud(115200);
panzhan 0:d80c66cb1b3a 29 foot.attach(&serial_board_isr);
panzhan 0:d80c66cb1b3a 30
panzhan 0:d80c66cb1b3a 31 // command.baud(115200);
panzhan 0:d80c66cb1b3a 32 // command.attach(&serial_command_isr);
panzhan 0:d80c66cb1b3a 33
panzhan 0:d80c66cb1b3a 34 ankle_can.frequency(1000000);
panzhan 0:d80c66cb1b3a 35 ankle_can.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0);
panzhan 0:d80c66cb1b3a 36 ankle_rxMsg.len = 6;
panzhan 0:d80c66cb1b3a 37 ankle_txMsg.len = 8;
panzhan 0:d80c66cb1b3a 38 ankle_txMsg.id = 0x01;
panzhan 0:d80c66cb1b3a 39
panzhan 0:d80c66cb1b3a 40 knee_can.frequency(1000000);
panzhan 0:d80c66cb1b3a 41 knee_can.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0);
panzhan 0:d80c66cb1b3a 42 knee_rxMsg.len = 6;
panzhan 0:d80c66cb1b3a 43 knee_txMsg.len = 8;
panzhan 0:d80c66cb1b3a 44 knee_txMsg.id = 0x01;
panzhan 0:d80c66cb1b3a 45 ////////////////////////////////////////////////////////////////////////////////
panzhan 0:d80c66cb1b3a 46 wait(5);
panzhan 0:d80c66cb1b3a 47
panzhan 0:d80c66cb1b3a 48
panzhan 0:d80c66cb1b3a 49 EnterMotorMode(&knee_txMsg);
panzhan 0:d80c66cb1b3a 50 EnterMotorMode(&ankle_txMsg);
panzhan 0:d80c66cb1b3a 51
panzhan 0:d80c66cb1b3a 52 //////////////////////////////////////////position/////////////////////////////////////////
panzhan 0:d80c66cb1b3a 53 // a_control.ankle.p_des=3.14/8;
panzhan 0:d80c66cb1b3a 54 // a_control.ankle.v_des=0;
panzhan 0:d80c66cb1b3a 55 // a_control.ankle.kp=10;
panzhan 0:d80c66cb1b3a 56 // a_control.ankle.kd=0;
panzhan 0:d80c66cb1b3a 57 // a_control.ankle.t_ff=0;
panzhan 0:d80c66cb1b3a 58 //
panzhan 0:d80c66cb1b3a 59 // a_control.knee.p_des=3.14/8;
panzhan 0:d80c66cb1b3a 60 // a_control.knee.v_des=0;
panzhan 0:d80c66cb1b3a 61 // a_control.knee.kp=10;
panzhan 0:d80c66cb1b3a 62 // a_control.knee.kd=0;
panzhan 0:d80c66cb1b3a 63 // a_control.knee.t_ff=0;
panzhan 0:d80c66cb1b3a 64 // PackAll();
panzhan 0:d80c66cb1b3a 65 // WriteAll();
panzhan 0:d80c66cb1b3a 66
panzhan 0:d80c66cb1b3a 67
panzhan 0:d80c66cb1b3a 68 while(1) {
panzhan 0:d80c66cb1b3a 69
panzhan 0:d80c66cb1b3a 70
panzhan 0:d80c66cb1b3a 71 ankle_can.read(ankle_rxMsg);
panzhan 0:d80c66cb1b3a 72 unpack_reply(ankle_rxMsg, &a_state);
panzhan 0:d80c66cb1b3a 73 wait_us(10);
panzhan 0:d80c66cb1b3a 74 knee_can.read(knee_rxMsg);
panzhan 0:d80c66cb1b3a 75 unpack_reply(knee_rxMsg, &a_state);
panzhan 0:d80c66cb1b3a 76
panzhan 0:d80c66cb1b3a 77 ankp = a_state.ankle_state.p; // 从CAN获得的当前位置
panzhan 0:d80c66cb1b3a 78 ankv = a_state.ankle_state.v;
panzhan 0:d80c66cb1b3a 79 ankt = a_state.ankle_state.t;
panzhan 0:d80c66cb1b3a 80
panzhan 0:d80c66cb1b3a 81 knp = a_state.knee_state.p;
panzhan 0:d80c66cb1b3a 82 knv = a_state.knee_state.v;
panzhan 0:d80c66cb1b3a 83 knt = a_state.knee_state.t;
panzhan 0:d80c66cb1b3a 84
panzhan 0:d80c66cb1b3a 85 pc.printf("%.3f--%.3f--%.3f==================%.3f--%.3f--%.3f\n",ankp,ankv,ankt,knp,knv,knt);
panzhan 0:d80c66cb1b3a 86
panzhan 0:d80c66cb1b3a 87 ///////////////////////////////////////velocity///////////////////////////////////////////////
panzhan 0:d80c66cb1b3a 88
panzhan 0:d80c66cb1b3a 89 // a_control.ankle.p_des=0;
panzhan 0:d80c66cb1b3a 90 // a_control.ankle.v_des=500*2*3.14/60/49;
panzhan 0:d80c66cb1b3a 91 // a_control.ankle.kp=0;
panzhan 0:d80c66cb1b3a 92 // a_control.ankle.kd=3;
panzhan 0:d80c66cb1b3a 93 // a_control.ankle.t_ff=0;
panzhan 0:d80c66cb1b3a 94 //
panzhan 0:d80c66cb1b3a 95 // a_control.knee.p_des=0;
panzhan 0:d80c66cb1b3a 96 // a_control.knee.v_des=500*2*3.14/60/49;
panzhan 0:d80c66cb1b3a 97 // a_control.knee.kp=0;
panzhan 0:d80c66cb1b3a 98 // a_control.knee.kd=3;
panzhan 0:d80c66cb1b3a 99 // a_control.knee.t_ff=0;
panzhan 0:d80c66cb1b3a 100 // PackAll();
panzhan 0:d80c66cb1b3a 101 // WriteAll();
panzhan 0:d80c66cb1b3a 102
panzhan 0:d80c66cb1b3a 103 ////////////////////////////////////////////////////////////////////////////////////////////////
panzhan 0:d80c66cb1b3a 104 }
panzhan 0:d80c66cb1b3a 105 }