//
// 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