87 lines
1.8 KiB
C
87 lines
1.8 KiB
C
//
|
|
// Created by SJQ on 2023/12/25.
|
|
//
|
|
|
|
#ifndef WHEEL_LEGGED_LEG_H
|
|
#define WHEEL_LEGGED_LEG_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_dPhi0;
|
|
float phi0_dot;
|
|
float phi0_dot2;
|
|
|
|
float last_dL0;
|
|
float L0_dot;
|
|
float L0_dot2;
|
|
|
|
float position[6]; //腿部关节位置 用于UI绘制
|
|
}Leg_State_t;
|
|
|
|
typedef struct
|
|
{
|
|
float F;
|
|
float Tp;
|
|
}Leg_Input_t;
|
|
|
|
typedef struct
|
|
{
|
|
float T1;
|
|
float T2;
|
|
}Leg_Output_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;
|
|
typedef struct
|
|
{
|
|
Leg_State_t legState;
|
|
Leg_Input_t legInput;
|
|
Leg_Input_t legFeedback;
|
|
Leg_Output_t legOutput;
|
|
|
|
float target_L0;
|
|
float target_phi0;
|
|
PIDInstance Length_PID;
|
|
PIDInstance Phi0_PID;
|
|
|
|
float F_feedforward; //支持力前馈补偿
|
|
|
|
arm_matrix_instance_f32 VMC_Jacobi;
|
|
float VMC_Jacobi_data[4];
|
|
arm_matrix_instance_f32 inv_VMC_Jacobi;
|
|
float inv_VMC_Jacobi_data[4];
|
|
}LegInstance;
|
|
|
|
|
|
|
|
void Leg_State_update(LegInstance* legInstance, float phi1,float phi4 ,float phi1_dot,float phi4_dot);
|
|
void Leg_calc_output(Leg_State_t* leg_state,Leg_Input_t* leg_input,Leg_Output_t* leg_output);
|
|
void Leg_Input_update(Leg_Input_t* leg_Input, float F,float Tp);
|
|
void Leg_feedback_update(LegInstance* legInstance,float T1_fb,float T2_fb);
|
|
|
|
void Leg_Init(LegInstance* legInstance, Leg_init_config_s* legInitConfig);
|
|
void Leg_length_control_loop(LegInstance* legInstance);
|
|
void VMC_getT(LegInstance* legInstance);
|
|
|
|
|
|
#endif //WHEEL_LEGGED_LEG_H
|