39 lines
878 B
C++
39 lines
878 B
C++
//
|
|
// Created by SJQ on 2024/3/2.
|
|
//
|
|
|
|
#ifndef WHEEL_LEGGED_BALANCE_H
|
|
#define WHEEL_LEGGED_BALANCE_H
|
|
|
|
#include "leg.h"
|
|
#include "LQR.h"
|
|
|
|
typedef struct
|
|
{
|
|
Leg_init_config_s legInitConfig;
|
|
PID_Init_Config_s leg_cor_PID_config;
|
|
}Balance_Init_config_s;
|
|
|
|
class balance {
|
|
public:
|
|
balance(Balance_Init_config_s* InitConfig);
|
|
void feedback_update(float leg_phi[4],float leg_phi_dot[4],float body_phi,float body_phi_dot,float x,float x_dot,float MotionAccel_z);
|
|
void control_loop();
|
|
void target_set(float x);
|
|
Matrixf<2,1> get_control();
|
|
|
|
private:
|
|
leg leg_r_;
|
|
leg leg_l_;
|
|
float target_length_; //平均腿长目标值
|
|
PIDInstance leg_coordination_PID_; //双腿协调PD控制
|
|
PIDInstance leg_length_PID_; //平均腿长控制
|
|
|
|
float state_[6];
|
|
float target_state_[6];
|
|
LQR<6,2> balance_LQR_;
|
|
};
|
|
|
|
|
|
#endif //WHEEL_LEGGED_BALANCE_H
|