yezhong yezhong / yezhong_main_controller_copy_1

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?

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
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 }