Gan likun / yezhong_main_controller_copy001

111

Dependencies:   yezhong_main_controller_copy mbed1-dev

CONTROL/control.cpp@10:f83b0935a8f0, 2022-07-09 (annotated)

Committer:
ganlikun
Date:
Sat Jul 09 11:58:13 2022 +0000
Revision:
10:f83b0935a8f0
Parent:
0:d80c66cb1b3a 
00000

Who changed what in which revision?

UserRevisionLine numberNew 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