40 lines
1.1 KiB
C
40 lines
1.1 KiB
C
//
|
|
// Created by SJQ on 2023/12/30.
|
|
//
|
|
|
|
#ifndef WHEEL_LEGGED_BALANCE_H
|
|
#define WHEEL_LEGGED_BALANCE_H
|
|
|
|
#include "leg.h"
|
|
#include "LQR.h"
|
|
#include "controller.h"
|
|
typedef struct
|
|
{
|
|
LegInstance leg_right;
|
|
LegInstance leg_left;
|
|
PIDInstance leg_coordination_PID; //双腿协调PD控制
|
|
|
|
float state[6];
|
|
float target_state[6];
|
|
|
|
LQRInstance balance_LQR;
|
|
}Balance_Control_t;
|
|
|
|
typedef struct
|
|
{
|
|
Leg_init_config_s legInitConfig;
|
|
PID_Init_Config_s leg_cor_PID_config;
|
|
|
|
int LQR_state_num;
|
|
int LQR_control_num;
|
|
}Balance_Init_config_s;
|
|
|
|
void Balance_Control_Init(Balance_Control_t* balanceControl,Balance_Init_config_s InitConfig);
|
|
//void Balance_feedback_update(Balance_Control_t* balanceControl,float leg_phi[4],float body_phi,float body_phi_dot,float x,float x_dot);
|
|
|
|
void Balance_feedback_update(Balance_Control_t* balanceControl,float leg_phi[4],float leg_phi_dot[4],float body_phi,float body_phi_dot,float x,float x_dot);
|
|
void Balance_Control_Loop(Balance_Control_t* balanceControl);
|
|
void target_x_set(Balance_Control_t* balanceControl,float x);
|
|
|
|
#endif //WHEEL_LEGGED_BALANCE_H
|