yezhong yezhong / yuanshi_main_controller_copy2

1

Dependencies:   mbed-dev-f303

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?

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