hpm_mcl_hybrid_ctrl.h

Go to the documentation of this file.

/*
 * Copyright (c) 2025 HPMicro
 *
 * SPDX-License-Identifier: BSD-3-Clause
 *
 */
#ifndef HPM_MCL_HYBRID_CTRL_H
#define HPM_MCL_HYBRID_CTRL_H
 
#include "hpm_mcl_common.h"
 
#ifdef __cplusplus
extern "C" {
#endif
 
typedef struct {
    float kp;           
    float kd;           
    float tau_ff;       
    float q_des;        
    float dq_des;       
    float tau_max;      
    float tau_min;      
    float speed_lpf_alpha;  
    float speed_deadzone;   
} mcl_hybrid_ctrl_cfg_t;
 
typedef struct {
    float q_actual;     
    float dq_actual;    
    float tau_output;   
    float pos_error;    
    float vel_error;    
    float speed_lpf;    
} mcl_hybrid_ctrl_state_t;
 
void mcl_hybrid_ctrl_init(mcl_hybrid_ctrl_cfg_t *cfg);
 
void mcl_hybrid_ctrl_step(const mcl_hybrid_ctrl_cfg_t *cfg, mcl_hybrid_ctrl_state_t *state);
 
static inline void mcl_hybrid_ctrl_set_kp(mcl_hybrid_ctrl_cfg_t *cfg, float kp)
{
    cfg->kp = kp;
}
 
static inline void mcl_hybrid_ctrl_set_kd(mcl_hybrid_ctrl_cfg_t *cfg, float kd)
{
    cfg->kd = kd;
}
 
static inline void mcl_hybrid_ctrl_set_tau_ff(mcl_hybrid_ctrl_cfg_t *cfg, float tau_ff)
{
    cfg->tau_ff = tau_ff;
}
 
static inline void mcl_hybrid_ctrl_set_position(mcl_hybrid_ctrl_cfg_t *cfg, float q_des)
{
    cfg->q_des = q_des;
}
 
static inline void mcl_hybrid_ctrl_set_velocity(mcl_hybrid_ctrl_cfg_t *cfg, float dq_des)
{
    cfg->dq_des = dq_des;
}
 
static inline void mcl_hybrid_ctrl_set_limits(mcl_hybrid_ctrl_cfg_t *cfg, float tau_min, float tau_max)
{
    cfg->tau_min = tau_min;
    cfg->tau_max = tau_max;
}
 
static inline void mcl_hybrid_ctrl_set_speed_filter(mcl_hybrid_ctrl_cfg_t *cfg, float lpf_alpha, float deadzone)
{
    cfg->speed_lpf_alpha = lpf_alpha;
    cfg->speed_deadzone = deadzone;
}
 
static inline void mcl_hybrid_ctrl_set_params(mcl_hybrid_ctrl_cfg_t *cfg, float kp, float kd, float tau_ff)
{
    cfg->kp = kp;
    cfg->kd = kd;
    cfg->tau_ff = tau_ff;
}
 
static inline void mcl_hybrid_ctrl_set_trajectory(mcl_hybrid_ctrl_cfg_t *cfg, float q_des, float dq_des)
{
    cfg->q_des = q_des;
    cfg->dq_des = dq_des;
}
 
static inline float mcl_hybrid_ctrl_get_torque(const mcl_hybrid_ctrl_state_t *state)
{
    return state->tau_output;
}
 
static inline float mcl_hybrid_ctrl_get_pos_error(const mcl_hybrid_ctrl_state_t *state)
{
    return state->pos_error;
}
 
static inline float mcl_hybrid_ctrl_get_vel_error(const mcl_hybrid_ctrl_state_t *state)
{
    return state->vel_error;
}
 
#ifdef __cplusplus
}
#endif
 
#endif /* HPM_MCL_HYBRID_CTRL_H */