pan zhan / panzhan_main_controller_continuous

12

Dependencies:   mbed-dev_spine

main.cpp@0:dd5d4837292c, 2021-06-09 (annotated)

Committer:
panzhan
Date:
Wed Jun 09 01:41:45 2021 +0000
Revision:
0:dd5d4837292c
continuous motion

Who changed what in which revision?

UserRevisionLine numberNew contents of line
panzhan 0:dd5d4837292c 1 #include "mbed.h"
panzhan 0:dd5d4837292c 2 #include <cstring>
panzhan 0:dd5d4837292c 3 #include "math_ops.h"
panzhan 0:dd5d4837292c 4 #include "leg_message.h"
panzhan 0:dd5d4837292c 5 #include "CAN.h"
panzhan 0:dd5d4837292c 6 #include "used_leg_message.h"
panzhan 0:dd5d4837292c 7 #include "data_pc.h"
panzhan 0:dd5d4837292c 8 #include "data_board.h"
panzhan 0:dd5d4837292c 9 #include "mode.h"
panzhan 0:dd5d4837292c 10 #include "data_command.h"
panzhan 0:dd5d4837292c 11 #include "math_ops.h"
panzhan 0:dd5d4837292c 12 #include "fuzzy.h"
panzhan 0:dd5d4837292c 13 #include "calculate.h"
panzhan 0:dd5d4837292c 14 #include "timer.h"
panzhan 0:dd5d4837292c 15
panzhan 0:dd5d4837292c 16
panzhan 0:dd5d4837292c 17 float ankp = 0.0f;
panzhan 0:dd5d4837292c 18 float ankv = 0.0f;
panzhan 0:dd5d4837292c 19 float ankt = 0.0f;
panzhan 0:dd5d4837292c 20
panzhan 0:dd5d4837292c 21 float knp = 0.0f;
panzhan 0:dd5d4837292c 22 float knv = 0.0f;
panzhan 0:dd5d4837292c 23 float knt = 0.0f;
panzhan 0:dd5d4837292c 24
panzhan 0:dd5d4837292c 25 float count=0.0f;
panzhan 0:dd5d4837292c 26 float result_knee=0.0f;
panzhan 0:dd5d4837292c 27 float result_ankle=0.0f;
panzhan 0:dd5d4837292c 28
panzhan 0:dd5d4837292c 29 ////////////////////////////////////////////////////////////////////////////////
panzhan 0:dd5d4837292c 30 // PID调试 //
panzhan 0:dd5d4837292c 31 ////////////////////////////////////////////////////////////////////////////////
panzhan 0:dd5d4837292c 32
panzhan 0:dd5d4837292c 33
panzhan 0:dd5d4837292c 34 int main()
panzhan 0:dd5d4837292c 35 {
panzhan 0:dd5d4837292c 36
panzhan 0:dd5d4837292c 37 ////////////////////////初始化//////////////////////////////////////
panzhan 0:dd5d4837292c 38 pc.baud(115200); // U2
panzhan 0:dd5d4837292c 39 pc.attach(&serial_pc_isr);
panzhan 0:dd5d4837292c 40
panzhan 0:dd5d4837292c 41 foot.baud(115200); // U1
panzhan 0:dd5d4837292c 42 foot.attach(&serial_board_isr);
panzhan 0:dd5d4837292c 43
panzhan 0:dd5d4837292c 44 command.baud(115200); // U3
panzhan 0:dd5d4837292c 45 command.attach(&serial_command_isr);
panzhan 0:dd5d4837292c 46
panzhan 0:dd5d4837292c 47 ankle_can.frequency(1000000);
panzhan 0:dd5d4837292c 48 ankle_can.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0);
panzhan 0:dd5d4837292c 49 ankle_rxMsg.len = 6;
panzhan 0:dd5d4837292c 50 ankle_txMsg.len = 8;
panzhan 0:dd5d4837292c 51 ankle_txMsg.id = 0x01;
panzhan 0:dd5d4837292c 52
panzhan 0:dd5d4837292c 53 knee_can.frequency(1000000);
panzhan 0:dd5d4837292c 54 knee_can.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0);
panzhan 0:dd5d4837292c 55 knee_rxMsg.len = 6;
panzhan 0:dd5d4837292c 56 knee_txMsg.len = 8;
panzhan 0:dd5d4837292c 57 knee_txMsg.id = 0x02;
panzhan 0:dd5d4837292c 58 ////////////////////////////////////////////////////////////////////////////////
panzhan 0:dd5d4837292c 59
panzhan 0:dd5d4837292c 60 wait(4);
panzhan 0:dd5d4837292c 61 EnterMotorMode(&knee_txMsg);
panzhan 0:dd5d4837292c 62 EnterMotorMode(&ankle_txMsg);
panzhan 0:dd5d4837292c 63 Zero(&knee_txMsg);
panzhan 0:dd5d4837292c 64 Zero(&ankle_txMsg);
panzhan 0:dd5d4837292c 65
panzhan 0:dd5d4837292c 66 tim.start();
panzhan 0:dd5d4837292c 67
panzhan 0:dd5d4837292c 68 while(1) {
panzhan 0:dd5d4837292c 69 ankle_can.read(ankle_rxMsg);
panzhan 0:dd5d4837292c 70 unpack_reply(ankle_rxMsg, &a_state);
panzhan 0:dd5d4837292c 71 wait_us(100);
panzhan 0:dd5d4837292c 72 knee_can.read(knee_rxMsg);
panzhan 0:dd5d4837292c 73 unpack_reply(knee_rxMsg, &a_state);
panzhan 0:dd5d4837292c 74
panzhan 0:dd5d4837292c 75 ankp = a_state.ankle_state.p; // 从CAN获得的当前位置
panzhan 0:dd5d4837292c 76 ankv = a_state.ankle_state.v;
panzhan 0:dd5d4837292c 77 ankt = a_state.ankle_state.t;
panzhan 0:dd5d4837292c 78
panzhan 0:dd5d4837292c 79 knp = a_state.knee_state.p; // 从CAN获得的当前位置
panzhan 0:dd5d4837292c 80 knv = a_state.knee_state.v;
panzhan 0:dd5d4837292c 81 knt = a_state.knee_state.t;
panzhan 0:dd5d4837292c 82
panzhan 0:dd5d4837292c 83 control(ankp, knp);
panzhan 0:dd5d4837292c 84
panzhan 0:dd5d4837292c 85 if(send_enable == 1){
panzhan 0:dd5d4837292c 86 PackAll();
panzhan 0:dd5d4837292c 87 WriteAll();
panzhan 0:dd5d4837292c 88 }
panzhan 0:dd5d4837292c 89 // pc.printf("p:%.3f\tv:%.3f\tKp:%.3f\tKd:%.3f\ttff:%.3f\n",a_control.ankle.p_des, a_control.ankle.v_des, a_control.ankle.kp, a_control.ankle.kd, a_control.ankle.t_ff);
panzhan 0:dd5d4837292c 90 // pc.printf("%.3f\t%.3f\t%.3f\t%.3f\t%.3f\n", a_state.knee_state.p, a_control.knee.p_des, a_state.ankle_state.p, a_control.ankle.p_des,tim.read());
panzhan 0:dd5d4837292c 91 // pc.printf("AB%.3f\t%.3f\t%.3f\t%.3f\t%.3f\t%.3f\n", a_state.knee_state.p, a_control.knee.p_des, a_state.knee_state.t, a_state.ankle_state.p, a_control.ankle.p_des, a_state.ankle_state.t);
panzhan 0:dd5d4837292c 92
panzhan 0:dd5d4837292c 93 pc.printf("AB");
panzhan 0:dd5d4837292c 94 if(a_state.knee_state.p >= 0)
panzhan 0:dd5d4837292c 95 pc.printf("+%.3f\t",a_state.knee_state.p);
panzhan 0:dd5d4837292c 96 else
panzhan 0:dd5d4837292c 97 pc.printf("%.3f\t",a_state.knee_state.p);
panzhan 0:dd5d4837292c 98
panzhan 0:dd5d4837292c 99 if(a_control.knee.p_des >= 0)
panzhan 0:dd5d4837292c 100 pc.printf("+%.3f\t",a_control.knee.p_des);
panzhan 0:dd5d4837292c 101 else
panzhan 0:dd5d4837292c 102 pc.printf("%.3f\t",a_control.knee.p_des);
panzhan 0:dd5d4837292c 103
panzhan 0:dd5d4837292c 104 if(a_state.knee_state.t >= 0)
panzhan 0:dd5d4837292c 105 pc.printf("+%.3f\t",a_state.knee_state.t);
panzhan 0:dd5d4837292c 106 else
panzhan 0:dd5d4837292c 107 pc.printf("%.3f\t",a_state.knee_state.t);
panzhan 0:dd5d4837292c 108
panzhan 0:dd5d4837292c 109 if(a_state.ankle_state.p >= 0)
panzhan 0:dd5d4837292c 110 pc.printf("+%.3f\t",a_state.ankle_state.p);
panzhan 0:dd5d4837292c 111 else
panzhan 0:dd5d4837292c 112 pc.printf("%.3f\t",a_state.ankle_state.p);
panzhan 0:dd5d4837292c 113
panzhan 0:dd5d4837292c 114 if(a_control.ankle.p_des >= 0)
panzhan 0:dd5d4837292c 115 pc.printf("+%.3f\t",a_control.ankle.p_des);
panzhan 0:dd5d4837292c 116 else
panzhan 0:dd5d4837292c 117 pc.printf("%.3f\t",a_control.ankle.p_des);
panzhan 0:dd5d4837292c 118
panzhan 0:dd5d4837292c 119 if(a_state.ankle_state.t >= 0)
panzhan 0:dd5d4837292c 120 pc.printf("+%.3f\n",a_state.ankle_state.t);
panzhan 0:dd5d4837292c 121 else
panzhan 0:dd5d4837292c 122 pc.printf("%.3f\n",a_state.ankle_state.t);
panzhan 0:dd5d4837292c 123
panzhan 0:dd5d4837292c 124
panzhan 0:dd5d4837292c 125 }
panzhan 0:dd5d4837292c 126 }