11
Diff: main.cpp
- Revision:
- 2:9418258519ea
- Parent:
- 0:dd5d4837292c
diff -r b7cd6d22f277 -r 9418258519ea main.cpp --- a/main.cpp Thu Aug 05 02:11:41 2021 +0000 +++ b/main.cpp Fri Aug 06 09:03:02 2021 +0000 @@ -14,17 +14,6 @@ #include "timer.h" -float ankp = 0.0f; -float ankv = 0.0f; -float ankt = 0.0f; - -float knp = 0.0f; -float knv = 0.0f; -float knt = 0.0f; - -float count=0.0f; -float result_knee=0.0f; -float result_ankle=0.0f; //////////////////////////////////////////////////////////////////////////////// // PID调试 // @@ -33,94 +22,37 @@ int main() { - + // Timer ttt; + + ////////////////////////初始化////////////////////////////////////// pc.baud(115200); // U2 - pc.attach(&serial_pc_isr); + foot.baud(115200); // U1 foot.attach(&serial_board_isr); - command.baud(115200); // U3 - command.attach(&serial_command_isr); + // command.baud(115200); // U3 + // command.attach(&serial_command_isr); + - ankle_can.frequency(1000000); - ankle_can.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0); - ankle_rxMsg.len = 6; - ankle_txMsg.len = 8; - ankle_txMsg.id = 0x01; - - knee_can.frequency(1000000); - knee_can.filter(CAN_ID<<21, 0xFFE00004, CANStandard, 0); - knee_rxMsg.len = 6; - knee_txMsg.len = 8; - knee_txMsg.id = 0x02; + //////////////////////////////////////////////////////////////////////////////// - wait(4); - EnterMotorMode(&knee_txMsg); - EnterMotorMode(&ankle_txMsg); - Zero(&knee_txMsg); - Zero(&ankle_txMsg); + NVIC_SetPriority(USART1_IRQn, 1); // command中断优先级高于board + NVIC_SetPriority(USART3_IRQn, 3); - tim.start(); - - while(1) { - ankle_can.read(ankle_rxMsg); - unpack_reply(ankle_rxMsg, &a_state); - wait_us(100); - knee_can.read(knee_rxMsg); - unpack_reply(knee_rxMsg, &a_state); - - ankp = a_state.ankle_state.p; // 从CAN获得的当前位置 - ankv = a_state.ankle_state.v; - ankt = a_state.ankle_state.t; - - knp = a_state.knee_state.p; // 从CAN获得的当前位置 - knv = a_state.knee_state.v; - knt = a_state.knee_state.t; - control(ankp, knp); - - if(send_enable == 1){ - PackAll(); - WriteAll(); - } -// 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); -// 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()); -// 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); - - pc.printf("AB"); - if(a_state.knee_state.p >= 0) - pc.printf("+%.3f\t",a_state.knee_state.p); - else - pc.printf("%.3f\t",a_state.knee_state.p); - - if(a_control.knee.p_des >= 0) - pc.printf("+%.3f\t",a_control.knee.p_des); - else - pc.printf("%.3f\t",a_control.knee.p_des); - - if(a_state.knee_state.t >= 0) - pc.printf("+%.3f\t",a_state.knee_state.t); - else - pc.printf("%.3f\t",a_state.knee_state.t); - - if(a_state.ankle_state.p >= 0) - pc.printf("+%.3f\t",a_state.ankle_state.p); - else - pc.printf("%.3f\t",a_state.ankle_state.p); - - if(a_control.ankle.p_des >= 0) - pc.printf("+%.3f\t",a_control.ankle.p_des); - else - pc.printf("%.3f\t",a_control.ankle.p_des); - - if(a_state.ankle_state.t >= 0) - pc.printf("+%.3f\n",a_state.ankle_state.t); - else - pc.printf("%.3f\n",a_state.ankle_state.t); - - + + while(1) { + /* + ttt.start(); + ttt.read(); + if(ttt.read()>2) + { + pc.printf("Helloee\n"); + ttt.reset(); + } + */ } } \ No newline at end of file