Gan likun / yezhong_main_controller_copy001

111

Dependencies:   yezhong_main_controller_copy mbed1-dev

main.cpp@9:c082f1c52936, 2022-07-07 (annotated)

Committer:
ganlikun
Date:
Thu Jul 07 11:38:10 2022 +0000
Revision:
9:c082f1c52936
Parent:
4:3f7a59212142
Child:
10:f83b0935a8f0
112

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
ganlikun 9:c082f1c52936 18 int main()
panzhan 0:d80c66cb1b3a 19 {
yezhong 3:940a9e40d327 20 //Timer t;
yezhong 3:940a9e40d327 21 //float a=PI/8;
yezhong 3:940a9e40d327 22 //float j=0.558,P=0;
panzhan 0:d80c66cb1b3a 23 ////////////////////////初始化//////////////////////////////////////
panzhan 0:d80c66cb1b3a 24 pc.baud(115200);
panzhan 0:d80c66cb1b3a 25 pc.attach(&serial_pc_isr);
panzhan 0:d80c66cb1b3a 26
yezhong 2:cd74a8cb03b0 27 // foot.baud(115200);
yezhong 2:cd74a8cb03b0 28 // foot.attach(&serial_board_isr);
panzhan 0:d80c66cb1b3a 29
yezhong 2:cd74a8cb03b0 30 // command.baud(115200);
yezhong 2:cd74a8cb03b0 31 // command.attach(&serial_command_isr);
panzhan 0:d80c66cb1b3a 32
yezhong 2:cd74a8cb03b0 33
ganlikun 4:3f7a59212142 34 pf_can.frequency(800000);
yezhong 2:cd74a8cb03b0 35 pf_can.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0);
yezhong 2:cd74a8cb03b0 36 pf_rxMsg.len = 6;
yezhong 2:cd74a8cb03b0 37 PF_can.len = 8;
ganlikun 9:c082f1c52936 38 PF_can.id = 0x03;
yezhong 2:cd74a8cb03b0 39
ganlikun 4:3f7a59212142 40 df_can.frequency(800000);
yezhong 2:cd74a8cb03b0 41 df_can.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0);
yezhong 2:cd74a8cb03b0 42 df_rxMsg.len = 6;
ganlikun 9:c082f1c52936 43 DF_can.len = 8;
ganlikun 4:3f7a59212142 44 DF_can.id = 0x02;
yezhong 2:cd74a8cb03b0 45
yezhong 2:cd74a8cb03b0 46 /////////////////////////////////////position///////////////////////////////////////////
ganlikun 4:3f7a59212142 47 wait(6);
yezhong 3:940a9e40d327 48 Zero(&PF_can);
ganlikun 9:c082f1c52936 49 EnterMotorMode(&PF_can);
ganlikun 9:c082f1c52936 50
ganlikun 4:3f7a59212142 51 Zero(&DF_can);//0位置
ganlikun 9:c082f1c52936 52 EnterMotorMode(&DF_can);//电机模式
yezhong 3:940a9e40d327 53
yezhong 3:940a9e40d327 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();
yezhong 3:940a9e40d327 69 WriteAll(); */
panzhan 0:d80c66cb1b3a 70
panzhan 0:d80c66cb1b3a 71
panzhan 0:d80c66cb1b3a 72 while(1) {
yezhong 2:cd74a8cb03b0 73
yezhong 2:cd74a8cb03b0 74 pf_can.read(pf_rxMsg);
yezhong 2:cd74a8cb03b0 75 unpack_reply(pf_rxMsg, &a_state);
panzhan 0:d80c66cb1b3a 76 wait_us(10);
yezhong 2:cd74a8cb03b0 77 df_can.read(df_rxMsg);
yezhong 2:cd74a8cb03b0 78 unpack_reply(df_rxMsg, &a_state);
panzhan 0:d80c66cb1b3a 79
yezhong 2:cd74a8cb03b0 80 float pfkp = a_state.pf.p; // 从CAN获得的当前位置
yezhong 2:cd74a8cb03b0 81 float pfkv = a_state.pf.v;
yezhong 2:cd74a8cb03b0 82 float pfkt = a_state.pf.t;
panzhan 0:d80c66cb1b3a 83
ganlikun 9:c082f1c52936 84 float dfp = a_state.df.p;
ganlikun 9:c082f1c52936 85 float dfv = a_state.df.v;
ganlikun 9:c082f1c52936 86 float dft = a_state.df.t;
yezhong 3:940a9e40d327 87
yezhong 3:940a9e40d327 88 // pc.printf("%.3f--%.3f--%.3f==================%.3f--%.3f--%.3f\n",pfkp,pfkv,pfkt,dfp,dfv,dft);
yezhong 3:940a9e40d327 89 // pc.printf("%.4f,%.3f,%.3f,%.4f,%.3f,%.4f\n",pfkp,pfkv,pfkt,a_control.pf.p_des,j,P);
yezhong 3:940a9e40d327 90
yezhong 3:940a9e40d327 91 pc.printf("%.3f,%.3f,%.3f,%.3f\n",pfkp,pfkv,pfkt,a_control.pf.v_des);
yezhong 3:940a9e40d327 92 /////////////////////////////////////////////////trajectory/////////////////////////////////////////////////////
yezhong 3:940a9e40d327 93 /*
yezhong 3:940a9e40d327 94 j=j+0.001;
yezhong 3:940a9e40d327 95 P=0.33+(-0.2148)*cos((j+0.1)*10.44)+(-0.1549)*sin((j+0.1)*10.44)+(-0.07744)*cos(2*(j+0.1)*10.44)+(-0.04255)*sin(2*(j+0.1)*10.44);
yezhong 3:940a9e40d327 96 if(j>=0.558&&j<=0.85)
yezhong 3:940a9e40d327 97 {
yezhong 3:940a9e40d327 98 a_control.pf.p_des=P;
yezhong 3:940a9e40d327 99 a_control.pf.v_des=0;
yezhong 3:940a9e40d327 100 a_control.pf.kp=80;
yezhong 3:940a9e40d327 101 a_control.pf.kd=0;
yezhong 3:940a9e40d327 102 a_control.pf.t_ff=0;
yezhong 3:940a9e40d327 103 }
yezhong 3:940a9e40d327 104
yezhong 3:940a9e40d327 105
yezhong 3:940a9e40d327 106 PackAll();
yezhong 3:940a9e40d327 107 WriteAll();
yezhong 3:940a9e40d327 108 */
panzhan 0:d80c66cb1b3a 109
yezhong 3:940a9e40d327 110
yezhong 3:940a9e40d327 111 /////////////////////////////////////position///////////////////////////////////////////
yezhong 3:940a9e40d327 112 /*
yezhong 3:940a9e40d327 113 a_control.pf.p_des=PI/8;
yezhong 3:940a9e40d327 114 a_control.pf.v_des=0;
yezhong 3:940a9e40d327 115 a_control.pf.kp=10;
yezhong 3:940a9e40d327 116 a_control.pf.kd=0;
yezhong 3:940a9e40d327 117 a_control.pf.t_ff=0;
yezhong 3:940a9e40d327 118 PackAll();
yezhong 3:940a9e40d327 119 WriteAll();
yezhong 3:940a9e40d327 120 */
yezhong 3:940a9e40d327 121
yezhong 3:940a9e40d327 122 /*
yezhong 3:940a9e40d327 123 t.start();
yezhong 3:940a9e40d327 124 t.read();
yezhong 3:940a9e40d327 125 if(t.read()<2)
yezhong 3:940a9e40d327 126 {
yezhong 3:940a9e40d327 127 a_control.pf.p_des=a;
yezhong 3:940a9e40d327 128 a_control.pf.v_des=0;
yezhong 3:940a9e40d327 129 a_control.pf.kp=10;
yezhong 3:940a9e40d327 130 a_control.pf.kd=0;
yezhong 3:940a9e40d327 131 a_control.pf.t_ff=0;
yezhong 3:940a9e40d327 132 }
yezhong 3:940a9e40d327 133 if(t.read()>3)
yezhong 3:940a9e40d327 134 {
yezhong 3:940a9e40d327 135 t.reset();
yezhong 3:940a9e40d327 136 Zero(&PF_can);
yezhong 3:940a9e40d327 137 a=-a;
yezhong 3:940a9e40d327 138 }
yezhong 3:940a9e40d327 139
yezhong 3:940a9e40d327 140 PackAll();
yezhong 3:940a9e40d327 141 WriteAll();
yezhong 3:940a9e40d327 142
yezhong 3:940a9e40d327 143 */
yezhong 3:940a9e40d327 144
yezhong 3:940a9e40d327 145
yezhong 3:940a9e40d327 146
yezhong 3:940a9e40d327 147
yezhong 3:940a9e40d327 148 /////////////////////////////////////////////////////////////////////////////////////////////
panzhan 0:d80c66cb1b3a 149
panzhan 0:d80c66cb1b3a 150 ///////////////////////////////////////velocity///////////////////////////////////////////////
panzhan 0:d80c66cb1b3a 151
yezhong 3:940a9e40d327 152 a_control.pf.p_des=0;
yezhong 3:940a9e40d327 153 a_control.pf.v_des=500*2*3.14/60/49;
yezhong 3:940a9e40d327 154 a_control.pf.kp=0;
yezhong 3:940a9e40d327 155 a_control.pf.kd=5;
yezhong 3:940a9e40d327 156 a_control.pf.t_ff=0;
yezhong 3:940a9e40d327 157
yezhong 3:940a9e40d327 158 a_control.df.p_des=0;
ganlikun 4:3f7a59212142 159 a_control.df.v_des=500*2*3.14/60/49;//49减速比，
yezhong 3:940a9e40d327 160 a_control.df.kp=0;
yezhong 3:940a9e40d327 161 a_control.df.kd=5;
yezhong 3:940a9e40d327 162 a_control.df.t_ff=0;
ganlikun 9:c082f1c52936 163
yezhong 3:940a9e40d327 164 PackAll();
yezhong 3:940a9e40d327 165 WriteAll();
panzhan 0:d80c66cb1b3a 166
panzhan 0:d80c66cb1b3a 167 ////////////////////////////////////////////////////////////////////////////////////////////////
panzhan 0:d80c66cb1b3a 168 }
panzhan 0:d80c66cb1b3a 169 }