144 lines
5.3 KiB
C
144 lines
5.3 KiB
C
//
|
|
// Created by SJQ on 2023/12/30.
|
|
//
|
|
|
|
#include "balance.h"
|
|
|
|
static void mylqr_k(float L0, float K[12]);
|
|
|
|
void Balance_Control_Init(Balance_Control_t* balanceControl,Balance_Init_config_s InitConfig)
|
|
{
|
|
Leg_Init(&balanceControl->leg_right,&InitConfig.legInitConfig);
|
|
Leg_Init(&balanceControl->leg_left,&InitConfig.legInitConfig);
|
|
|
|
LQR_Init(&balanceControl->balance_LQR,InitConfig.LQR_state_num,InitConfig.LQR_control_num);
|
|
PIDInit(&balanceControl->leg_coordination_PID,&InitConfig.leg_cor_PID_config);
|
|
|
|
PIDInit(&balanceControl->leg_length_PID,&InitConfig.legInitConfig.length_PID_conf);
|
|
memset(balanceControl->state,0,6);
|
|
memset(balanceControl->target_state,0,6);
|
|
}
|
|
// leg_phi[4] = {phi1_r,phi4_r,phi1_l,phi4_l}
|
|
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)
|
|
{
|
|
Leg_State_update(&balanceControl->leg_right,leg_phi[0],leg_phi[1],leg_phi_dot[0],leg_phi_dot[1]);
|
|
Leg_State_update(&balanceControl->leg_left,leg_phi[2],leg_phi[3],leg_phi_dot[2],leg_phi_dot[3]);
|
|
|
|
float K_matrix[12]={0};
|
|
float L_average = balanceControl->leg_right.legState.L0;
|
|
//(balanceControl->leg_right.legState.L0 + balanceControl->leg_left.legState.L0)/2;
|
|
float alpha = PI/2 - balanceControl->leg_right.legState.phi0;
|
|
|
|
float theta = alpha - body_phi;
|
|
|
|
// static uint32_t dot_cnt = 0;
|
|
// float dot_dt = DWT_GetDeltaT(&dot_cnt);
|
|
// float theta_dot1 = (balanceControl->state[0] - theta) / dot_dt;
|
|
|
|
|
|
float theta_dot = - balanceControl->leg_right.legState.phi0_dot - body_phi_dot;
|
|
|
|
balanceControl->state[0] = theta;
|
|
balanceControl->state[1] = theta_dot;
|
|
balanceControl->state[2] = x; balanceControl->state[3] = x_dot;
|
|
balanceControl->state[4] = body_phi; balanceControl->state[5] = body_phi_dot;
|
|
|
|
mylqr_k(L_average,K_matrix);
|
|
float K_matrix_fix[12];
|
|
for (int i = 0; i < 6; ++i) {
|
|
K_matrix_fix[i] = K_matrix[2*i];
|
|
}
|
|
for (int i = 0; i < 6; ++i) {
|
|
K_matrix_fix[6+i] = K_matrix[2*i+1];
|
|
}
|
|
|
|
LQR_set_K(&balanceControl->balance_LQR,K_matrix_fix);
|
|
}
|
|
|
|
void Balance_Control_Loop(Balance_Control_t* balanceControl)
|
|
{
|
|
LQR_update(&balanceControl->balance_LQR,balanceControl->state,balanceControl->target_state);
|
|
|
|
balanceControl->balance_LQR.control_vector[1] = - balanceControl->balance_LQR.control_vector[1];
|
|
float Tp = balanceControl->balance_LQR.control_vector[1]; //髋关节力矩
|
|
float T = balanceControl->balance_LQR.control_vector[0]; //驱动轮力矩
|
|
|
|
LegInstance *leg_r,*leg_l;
|
|
leg_r = &balanceControl->leg_right;
|
|
leg_l = &balanceControl->leg_left;
|
|
//两条腿独立控制腿长
|
|
// float F_pid_out_r = PIDCalculate(&leg_r->Length_PID,leg_r->legState.L0,leg_r->target_L0);
|
|
// float F_pid_out_l = PIDCalculate(&leg_l->Length_PID,leg_l->legState.L0,leg_l->target_L0);
|
|
//控制平均腿长
|
|
float average_length = (leg_r->legState.L0 + leg_l->legState.L0)/2;
|
|
float F_pid_out = PIDCalculate(&balanceControl->leg_length_PID,average_length,leg_r->target_L0);
|
|
//float F_pid_out_l = PIDCalculate(&balanceControl->leg_length_PID,average_length,leg_r->target_L0);
|
|
|
|
float Tp_pid_out_r = PIDCalculate(&leg_r->Phi0_PID,leg_r->legState.phi0,leg_r->target_phi0);
|
|
float Tp_pid_out_l = PIDCalculate(&leg_l->Phi0_PID,leg_l->legState.phi0,leg_l->target_phi0);
|
|
|
|
float coordination_err = leg_r->legState.phi0 - leg_l->legState.phi0;
|
|
|
|
float cor_Tp = PIDCalculate(&balanceControl->leg_coordination_PID,coordination_err,0);
|
|
|
|
Leg_Input_update(&leg_r->legInput,F_pid_out + leg_r->F_feedforward + 15,Tp + cor_Tp);
|
|
Leg_Input_update(&leg_l->legInput,F_pid_out + leg_l->F_feedforward + 15,Tp - cor_Tp);
|
|
|
|
VMC_getT(leg_r);
|
|
VMC_getT(leg_l);
|
|
}
|
|
|
|
void target_x_set(Balance_Control_t* balanceControl,float x)
|
|
{
|
|
balanceControl->target_state [2] = x;
|
|
}
|
|
|
|
|
|
|
|
/*
|
|
* MYLQR_K MATLAB生成
|
|
* K = MYLQR_K(L0)
|
|
*
|
|
* Arguments : float L0
|
|
* float K[12]
|
|
* Return Type : void
|
|
*/
|
|
void mylqr_k(float L0, float K[12])
|
|
{
|
|
float t2;
|
|
float t3;
|
|
/* This function was generated by the Symbolic Math Toolbox version 9.2.
|
|
*/
|
|
/* 10-Jan-2024 15:16:33 */
|
|
t2 = L0 * L0;
|
|
t3 = t2 * L0;
|
|
K[0] =
|
|
((L0 * -97.6700211F + t2 * 46.4782753F) + t3 * 4.51733494F) - 1.61397338F;
|
|
K[1] =
|
|
((L0 * 146.921585F - t2 * 449.3255F) + t3 * 451.086609F) - 0.117263243F;
|
|
K[2] = ((L0 * -6.99885273F - t2 * 27.3483524F) + t3 * 26.8910675F) -
|
|
0.586572289F;
|
|
K[3] =
|
|
((L0 * 12.7336187F - t2 * 28.605896F) + t3 * 23.224308F) + 0.498449594F;
|
|
K[4] =
|
|
((L0 * 6.6503768F - t2 * 45.4196815F) + t3 * 66.2783737F) - 6.39456844F;
|
|
K[5] = ((L0 * 43.2317657F - t2 * 285.767334F) + t3 * 394.4133F) + 9.38568115F;
|
|
K[6] = ((L0 * 17.2884F - t2 * 97.0312653F) + t3 * 130.450256F) - 8.32731533F;
|
|
K[7] =
|
|
((L0 * 27.309267F - t2 * 224.613647F) + t3 * 318.699432F) + 11.2518187F;
|
|
K[8] =
|
|
((L0 * 8.78794384F - t2 * 170.93956F) + t3 * 255.523727F) + 18.6254063F;
|
|
K[9] =
|
|
((L0 * -44.4622231F + t2 * 450.010193F) - t3 * 693.942322F) + 58.9608879F;
|
|
K[10] =
|
|
((L0 * -1.05264759F - t2 * 9.32109547F) + t3 * 14.8346682F) + 3.42239809F;
|
|
K[11] =
|
|
((L0 * -15.4206066F + t2 * 102.133163F) - t3 * 144.315674F) + 3.18063068F;
|
|
}
|
|
|
|
/*
|
|
* File trailer for mylqr_k.c
|
|
*
|
|
* [EOF]
|
|
*/
|