Shao Rui / CH_Communicatuin_Test

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed-dev_spine

CONTROL/control.cpp@6:aad89fd109c2, 2019-12-02 (annotated)

Committer:
WXD
Date:
Mon Dec 02 14:22:07 2019 +0000
Revision:
6:aad89fd109c2
Parent:
5:6a95726e45b0 
123

Who changed what in which revision?

UserRevisionLine numberNew contents of line
WXD 5:6a95726e45b0 1 #include "control.h"
WXD 5:6a95726e45b0 2
WXD 5:6a95726e45b0 3
WXD 5:6a95726e45b0 4 unsigned int send_enable = 0;
WXD 5:6a95726e45b0 5
WXD 5:6a95726e45b0 6
WXD 5:6a95726e45b0 7 void control()
WXD 5:6a95726e45b0 8 {
WXD 5:6a95726e45b0 9 if(c_control == 1)
WXD 5:6a95726e45b0 10 command_control();
WXD 6:aad89fd109c2 11
WXD 6:aad89fd109c2 12 if(return_zero == 1)
WXD 6:aad89fd109c2 13 {
WXD 6:aad89fd109c2 14 if( abs(ppf) >= radmax )
WXD 6:aad89fd109c2 15 cal_command.q_des_pf = ppf - radmax;
WXD 6:aad89fd109c2 16 if( abs(ppf) < radmax )
WXD 6:aad89fd109c2 17 cal_command.q_des_pf = 0;
WXD 6:aad89fd109c2 18
WXD 6:aad89fd109c2 19 cal_command.qd_des_pf = 0.0f;
WXD 6:aad89fd109c2 20 cal_command.kp_pf = 60.0f;
WXD 6:aad89fd109c2 21 cal_command.kd_pf = 10.0f;
WXD 6:aad89fd109c2 22 cal_command.tau_pf_ff = 0.0f;
WXD 5:6a95726e45b0 23
WXD 6:aad89fd109c2 24
WXD 6:aad89fd109c2 25 if( abs(pdf) >= radmax )
WXD 6:aad89fd109c2 26 cal_command.q_des_df = pdf - radmax;
WXD 6:aad89fd109c2 27 if( abs(pdf) < radmax )
WXD 6:aad89fd109c2 28 cal_command.q_des_df = 0;
WXD 6:aad89fd109c2 29
WXD 6:aad89fd109c2 30 cal_command.qd_des_df = 0.0f;
WXD 6:aad89fd109c2 31 cal_command.kp_df = 60.0f;
WXD 6:aad89fd109c2 32 cal_command.kd_df = 10.0f;
WXD 6:aad89fd109c2 33 cal_command.tau_df_ff = 0.0f;
WXD 6:aad89fd109c2 34 }
WXD 6:aad89fd109c2 35
WXD 5:6a95726e45b0 36 if(c_lock == 1) // 处于PC串口锁定模式
WXD 5:6a95726e45b0 37 {
WXD 5:6a95726e45b0 38 cal_command.q_des_pf = SP_pf;
WXD 5:6a95726e45b0 39 cal_command.qd_des_pf = 0.0f;
WXD 6:aad89fd109c2 40 cal_command.kp_pf = 60.0f;
WXD 6:aad89fd109c2 41 cal_command.kd_pf = 10.0f;
WXD 5:6a95726e45b0 42 cal_command.tau_pf_ff = 0.0f;
WXD 5:6a95726e45b0 43
WXD 5:6a95726e45b0 44 cal_command.q_des_df = SP_df;
WXD 5:6a95726e45b0 45 cal_command.qd_des_df = 0.0f;
WXD 6:aad89fd109c2 46 cal_command.kp_df = 60.0f;
WXD 6:aad89fd109c2 47 cal_command.kd_df = 10.0f;
WXD 5:6a95726e45b0 48 cal_command.tau_df_ff = 0.0f;
WXD 5:6a95726e45b0 49 }
WXD 5:6a95726e45b0 50
WXD 5:6a95726e45b0 51 if(c_lock == 0)
WXD 5:6a95726e45b0 52 {
WXD 5:6a95726e45b0 53 calculate_pf();
WXD 5:6a95726e45b0 54 calculate_df();
WXD 5:6a95726e45b0 55 }
WXD 5:6a95726e45b0 56
WXD 5:6a95726e45b0 57 // 将计算得到的数值赋给控制器
WXD 5:6a95726e45b0 58 a_control.pf.p_des = cal_command.q_des_pf;
WXD 5:6a95726e45b0 59 a_control.pf.v_des = cal_command.qd_des_pf;
WXD 5:6a95726e45b0 60 a_control.pf.kp = cal_command.kp_pf;
WXD 5:6a95726e45b0 61 a_control.pf.kd = cal_command.kd_pf;
WXD 5:6a95726e45b0 62 a_control.pf.t_ff = cal_command.tau_pf_ff;
WXD 5:6a95726e45b0 63
WXD 5:6a95726e45b0 64 a_control.df.p_des = cal_command.q_des_df;
WXD 5:6a95726e45b0 65 a_control.df.v_des = cal_command.qd_des_df;
WXD 5:6a95726e45b0 66 a_control.df.kp = cal_command.kp_df;
WXD 5:6a95726e45b0 67 a_control.df.kd = cal_command.kd_df;
WXD 5:6a95726e45b0 68 a_control.df.t_ff = cal_command.tau_df_ff;
WXD 5:6a95726e45b0 69 }
WXD 5:6a95726e45b0 70
WXD 5:6a95726e45b0 71
WXD 5:6a95726e45b0 72
WXD 5:6a95726e45b0 73
WXD 5:6a95726e45b0 74
WXD 5:6a95726e45b0 75
WXD 5:6a95726e45b0 76
WXD 5:6a95726e45b0 77
WXD 5:6a95726e45b0 78
WXD 5:6a95726e45b0 79
WXD 5:6a95726e45b0 80
WXD 5:6a95726e45b0 81
WXD 5:6a95726e45b0 82
WXD 5:6a95726e45b0 83