Gan likun
111
Dependencies: yezhong_main_controller_copy mbed1-dev
CONTROL/control.cpp@0:d80c66cb1b3a, 2020-11-10 (annotated)
- Committer:
- panzhan
- Date:
- Tue Nov 10 09:09:58 2020 +0000
- Revision:
- 0:d80c66cb1b3a
panzhan_main_controller_copy2
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| panzhan | 0:d80c66cb1b3a | 1 | #include "control.h" |
| panzhan | 0:d80c66cb1b3a | 2 | |
| panzhan | 0:d80c66cb1b3a | 3 | |
| panzhan | 0:d80c66cb1b3a | 4 | void control() |
| panzhan | 0:d80c66cb1b3a | 5 | { |
| panzhan | 0:d80c66cb1b3a | 6 | if(command_control_flag == 1) |
| panzhan | 0:d80c66cb1b3a | 7 | command_control(); |
| panzhan | 0:d80c66cb1b3a | 8 | |
| panzhan | 0:d80c66cb1b3a | 9 | if(return_zero == 1) |
| panzhan | 0:d80c66cb1b3a | 10 | { |
| panzhan | 0:d80c66cb1b3a | 11 | // gangduceshi(); |
| panzhan | 0:d80c66cb1b3a | 12 | } |
| panzhan | 0:d80c66cb1b3a | 13 | |
| panzhan | 0:d80c66cb1b3a | 14 | if(c_lock == 1) // 处于PC串口锁定模式 |
| panzhan | 0:d80c66cb1b3a | 15 | { |
| panzhan | 0:d80c66cb1b3a | 16 | // cal_command.q_des_pf = SP_pf; |
| panzhan | 0:d80c66cb1b3a | 17 | // cal_command.qd_des_pf = 0.0f; |
| panzhan | 0:d80c66cb1b3a | 18 | // cal_command.kp_pf = 200.0f; |
| panzhan | 0:d80c66cb1b3a | 19 | // cal_command.kd_pf = 0.0f; |
| panzhan | 0:d80c66cb1b3a | 20 | // cal_command.tau_pf_ff = 0.0f; |
| panzhan | 0:d80c66cb1b3a | 21 | // |
| panzhan | 0:d80c66cb1b3a | 22 | // cal_command.q_des_df = SP_df; |
| panzhan | 0:d80c66cb1b3a | 23 | // cal_command.qd_des_df = 0.0f; |
| panzhan | 0:d80c66cb1b3a | 24 | // cal_command.kp_df = 200.0f; |
| panzhan | 0:d80c66cb1b3a | 25 | // cal_command.kd_df = 0.0f; |
| panzhan | 0:d80c66cb1b3a | 26 | // cal_command.tau_df_ff = 0.0f; |
| panzhan | 0:d80c66cb1b3a | 27 | } |
| panzhan | 0:d80c66cb1b3a | 28 | |
| panzhan | 0:d80c66cb1b3a | 29 | if(c_lock == 0) |
| panzhan | 0:d80c66cb1b3a | 30 | { |
| panzhan | 0:d80c66cb1b3a | 31 | //calculate_pf_kh(); |
| panzhan | 0:d80c66cb1b3a | 32 | // calculate_pf_fuzzy(); |
| panzhan | 0:d80c66cb1b3a | 33 | |
| panzhan | 0:d80c66cb1b3a | 34 | //calculate_df(); |
| panzhan | 0:d80c66cb1b3a | 35 | } |
| panzhan | 0:d80c66cb1b3a | 36 | |
| panzhan | 0:d80c66cb1b3a | 37 | // 将计算得到的数值赋给控制器 |
| panzhan | 0:d80c66cb1b3a | 38 | |
| panzhan | 0:d80c66cb1b3a | 39 | // a_control.pf.p_des = cal_command.q_des_pf; |
| panzhan | 0:d80c66cb1b3a | 40 | // a_control.pf.v_des = cal_command.qd_des_pf; |
| panzhan | 0:d80c66cb1b3a | 41 | // a_control.pf.kp = cal_command.kp_pf / 7.0f; |
| panzhan | 0:d80c66cb1b3a | 42 | // a_control.pf.kd = cal_command.kd_pf / 7.0f; |
| panzhan | 0:d80c66cb1b3a | 43 | // a_control.pf.t_ff = cal_command.tau_pf_ff / 7.0f; |
| panzhan | 0:d80c66cb1b3a | 44 | // |
| panzhan | 0:d80c66cb1b3a | 45 | // a_control.df.p_des = cal_command.q_des_df; |
| panzhan | 0:d80c66cb1b3a | 46 | // a_control.df.v_des = cal_command.qd_des_df; |
| panzhan | 0:d80c66cb1b3a | 47 | // a_control.df.kp = cal_command.kp_df / 10.0f; |
| panzhan | 0:d80c66cb1b3a | 48 | // a_control.df.kd = cal_command.kd_df / 10.0f; |
| panzhan | 0:d80c66cb1b3a | 49 | // a_control.df.t_ff = cal_command.tau_df_ff / 10.0f; |
| panzhan | 0:d80c66cb1b3a | 50 | |
| panzhan | 0:d80c66cb1b3a | 51 | } |
| panzhan | 0:d80c66cb1b3a | 52 | |
| panzhan | 0:d80c66cb1b3a | 53 | |
| panzhan | 0:d80c66cb1b3a | 54 | |
| panzhan | 0:d80c66cb1b3a | 55 | |
| panzhan | 0:d80c66cb1b3a | 56 | |
| panzhan | 0:d80c66cb1b3a | 57 | |
| panzhan | 0:d80c66cb1b3a | 58 | |
| panzhan | 0:d80c66cb1b3a | 59 | |
| panzhan | 0:d80c66cb1b3a | 60 | |
| panzhan | 0:d80c66cb1b3a | 61 | |
| panzhan | 0:d80c66cb1b3a | 62 | |
| panzhan | 0:d80c66cb1b3a | 63 | |
| panzhan | 0:d80c66cb1b3a | 64 | |
| panzhan | 0:d80c66cb1b3a | 65 |