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

Committer:
WXD
Date:
2019-10-08
Revision:
5:6a95726e45b0
Child:
6:aad89fd109c2

File content as of revision 5:6a95726e45b0:

#include "control.h"


unsigned int send_enable = 0;


void control()
{
    if(c_control == 1)
        command_control();
        
    if(c_lock == 1)                                                             // 处于PC串口锁定模式
    {
        cal_command.q_des_pf  = SP_pf;
        cal_command.qd_des_pf = 0.0f;
        cal_command.kp_pf     = 800.0f;
        cal_command.kd_pf     = 100.0f;
        cal_command.tau_pf_ff = 0.0f;
        
        cal_command.q_des_df  = SP_df;
        cal_command.qd_des_df = 0.0f;
        cal_command.kp_df     = 800.0f;
        cal_command.kd_df     = 100.0f;
        cal_command.tau_df_ff = 0.0f;
    }
    
    if(c_lock == 0)
    {
        calculate_pf();
        calculate_df();
    }
    
    // 将计算得到的数值赋给控制器
    a_control.pf.p_des = cal_command.q_des_pf;
    a_control.pf.v_des = cal_command.qd_des_pf;
    a_control.pf.kp    = cal_command.kp_pf;
    a_control.pf.kd    = cal_command.kd_pf;
    a_control.pf.t_ff  = cal_command.tau_pf_ff;
        
    a_control.df.p_des = cal_command.q_des_df;
    a_control.df.v_des = cal_command.qd_des_df;
    a_control.df.kp    = cal_command.kp_df;
    a_control.df.kd    = cal_command.kd_df;
    a_control.df.t_ff  = cal_command.tau_df_ff;
}