70 lines
1.4 KiB
C++
70 lines
1.4 KiB
C++
//
|
|
// Created by SJQ on 2024/3/1.
|
|
//
|
|
|
|
#ifndef WHEEL_LEGGED_LEG_H
|
|
#define WHEEL_LEGGED_LEG_H
|
|
|
|
#include <matrix.h>
|
|
#include "controller.h"
|
|
|
|
typedef struct
|
|
{
|
|
float L0;
|
|
float phi0;
|
|
|
|
float phi1;
|
|
float phi4;
|
|
|
|
float phi2;
|
|
float phi3;
|
|
|
|
uint32_t DWT_CNT;
|
|
float dt; //用于计算时间并求导
|
|
float last_phi0;
|
|
float phi0_dot;
|
|
|
|
float L0_dot;
|
|
|
|
}Leg_State_t;
|
|
|
|
typedef struct
|
|
{
|
|
PID_Init_Config_s length_PID_conf;
|
|
PID_Init_Config_s phi0_PID_conf;
|
|
float init_target_L0; //初始腿长目标值
|
|
float F_feedforward;
|
|
}Leg_init_config_s;
|
|
|
|
class leg
|
|
{
|
|
public:
|
|
leg(Leg_init_config_s* legInitConfig);
|
|
void state_update(float phi1,float phi4 ,float phi1_dot,float phi4_dot);
|
|
void input_update(float F,float Tp);
|
|
void feedback_update(float T1_fb,float T2_fb);
|
|
void calculate_output();
|
|
Matrixf<4,1> get_state();
|
|
float calculate_FN(float MotionAccel_z,float theta,float theta_dot);
|
|
|
|
float F_feedforward_; //支持力前馈补偿
|
|
private:
|
|
Leg_State_t legState_;
|
|
Matrixf<2,1> legInput_;
|
|
Matrixf<2,1> legOutput_;
|
|
Matrixf<2,1> legFeedback_;
|
|
|
|
Matrixf<2,1> State_dot_; // L0 和 phi0 的导数
|
|
|
|
float target_L0_;
|
|
float target_phi0_;
|
|
PIDInstance Length_PID_;
|
|
PIDInstance Phi0_PID_;
|
|
|
|
Matrixf<2,2> VMC_Jacobi_;
|
|
|
|
void get_dot(float phi1_dot,float phi4_dot);
|
|
};
|
|
|
|
#endif //WHEEL_LEGGED_LEG_H
|