电机输出滤波 裁判系统杀死自动失能
This commit is contained in:
parent
0c508b08cf
commit
6fa3b67cae
|
|
@ -120,6 +120,7 @@ int main(void)
|
|||
MX_CRC_Init();
|
||||
MX_DAC_Init();
|
||||
/* USER CODE BEGIN 2 */
|
||||
HAL_Delay(1500); //delay 一段时间 等待关节电机驱动初始化
|
||||
RobotInit(); // 唯一的初始化函数
|
||||
LOGINFO("[main] SystemInit() and RobotInit() done");
|
||||
/* USER CODE END 2 */
|
||||
|
|
|
|||
|
|
@ -0,0 +1,336 @@
|
|||
//
|
||||
// Created by SJQ on 2023/12/30.
|
||||
//
|
||||
|
||||
#include "balance.h"
|
||||
|
||||
static void mylqr_k(float L0, float K_mat[12]);
|
||||
|
||||
void Balance_Control_Init(Balance_Control_t* balanceControl,Balance_Init_config_s InitConfig)
|
||||
{
|
||||
|
||||
balanceControl->target_L0 = 0.15f;
|
||||
Leg_Init(&balanceControl->leg_right,&InitConfig.legInitConfig);
|
||||
Leg_Init(&balanceControl->leg_left,&InitConfig.legInitConfig);
|
||||
|
||||
LQR_Init(&balanceControl->LQR_l,InitConfig.LQR_state_num,InitConfig.LQR_control_num);
|
||||
LQR_Init(&balanceControl->LQR_r,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);
|
||||
|
||||
estimator_init(&balanceControl->v_estimator,0.001f,10.0f);
|
||||
Observer_Init(&balanceControl->state_observer);
|
||||
memset(balanceControl->state_r,0,6);
|
||||
memset(balanceControl->target_state_r,0,6);
|
||||
memset(balanceControl->state_l,0,6);
|
||||
memset(balanceControl->target_state_l,0,6);
|
||||
}
|
||||
|
||||
//解算地面支持力F
|
||||
float calculate_F(float MotionAccel_z,float theta[3],LegInstance *leg ,uint32_t *cnt_last);
|
||||
|
||||
|
||||
// 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,const float x[2],const float x_dot[2],const float MotionAccel[3])
|
||||
{
|
||||
LegInstance *leg_r = &balanceControl->leg_right;
|
||||
LegInstance *leg_l = &balanceControl->leg_left;
|
||||
|
||||
Leg_State_update(leg_r,leg_phi[0],leg_phi[1],leg_phi_dot[0],leg_phi_dot[1]);
|
||||
Leg_State_update(leg_l,leg_phi[2],leg_phi[3],leg_phi_dot[2],leg_phi_dot[3]);
|
||||
|
||||
float K_matrix_r[12]={0};
|
||||
float K_matrix_l[12]={0};
|
||||
|
||||
//右腿
|
||||
float alpha_r = PI/2 - leg_r->legState.phi0;
|
||||
float theta_r = alpha_r - body_phi;
|
||||
float theta_dot_r = - leg_r->legState.phi0_dot - body_phi_dot;
|
||||
|
||||
static uint32_t dot_cnt = 0;
|
||||
float dot_dt = DWT_GetDeltaT(&dot_cnt);
|
||||
float theta_dot2_r = (balanceControl->state_r[1] - theta_dot_r) / dot_dt;
|
||||
|
||||
//左腿
|
||||
float alpha_l = PI/2 - leg_l->legState.phi0;
|
||||
float theta_l = alpha_l - body_phi;
|
||||
float theta_dot_l = - leg_l->legState.phi0_dot - body_phi_dot;
|
||||
float theta_dot2_l = (balanceControl->state_l[1] - theta_dot_l) / dot_dt;
|
||||
|
||||
//计算机体速度
|
||||
float xb_r = x[0] + leg_r->legState.L0 * arm_sin_f32(theta_r);
|
||||
float xb_dot_r = x_dot[0] + (leg_r->legState.L0_dot* arm_sin_f32(theta_r) + leg_r->legState.L0 * theta_dot_r * arm_cos_f32(theta_r));
|
||||
|
||||
float xb_l = x[1] + leg_l->legState.L0 * arm_sin_f32(theta_l);
|
||||
float xb_dot_l = x_dot[1] + (leg_l->legState.L0_dot* arm_sin_f32(theta_l) + leg_l->legState.L0 * theta_dot_l * arm_cos_f32(theta_l));
|
||||
|
||||
float xb = (xb_r + xb_l)/2;
|
||||
float xb_dot = (xb_dot_r + xb_dot_l)/2;
|
||||
|
||||
estimator_update(&balanceControl->v_estimator,xb,xb_dot,MotionAccel[1]);
|
||||
|
||||
float x_est_r = balanceControl->v_estimator.Estimate_X_[0] - leg_r->legState.L0 * arm_sin_f32(theta_r);
|
||||
float x_dot_est_r = balanceControl->v_estimator.Estimate_X_[1] - (leg_r->legState.L0_dot* arm_sin_f32(theta_r) + leg_r->legState.L0 * theta_dot_r * arm_cos_f32(theta_r));
|
||||
float x_est_l = balanceControl->v_estimator.Estimate_X_[0] - leg_l->legState.L0 * arm_sin_f32(theta_l);
|
||||
float x_dot_est_l = balanceControl->v_estimator.Estimate_X_[1] - (leg_l->legState.L0_dot* arm_sin_f32(theta_l) + leg_r->legState.L0 * theta_dot_l * arm_cos_f32(theta_l));
|
||||
|
||||
|
||||
balanceControl->v_est[0] = x_est_r;
|
||||
balanceControl->v_est[1] = x_dot_est_r;
|
||||
balanceControl->v_est[2] = x_est_l;
|
||||
balanceControl->v_est[3] = x_dot_est_l;
|
||||
|
||||
balanceControl->state_r[0] = theta_r;
|
||||
balanceControl->state_r[1] = theta_dot_r;//(1.0f - 0.75f) * balanceControl->state_r[1] + 0.75f * theta_dot_r;
|
||||
balanceControl->state_r[2] = x_est_r;//x[0];//balanceControl->v_estimator.Estimate_X_[0];
|
||||
balanceControl->state_r[3] = x_dot_est_r;//x_dot[0];//balanceControl->v_estimator.Estimate_X_[1];
|
||||
balanceControl->state_r[4] = body_phi;
|
||||
balanceControl->state_r[5] = body_phi_dot;//(1.0f - 0.75f) * balanceControl->state_r[5] + 0.75f * body_phi_dot;
|
||||
|
||||
balanceControl->state_l[0] = theta_l;
|
||||
balanceControl->state_l[1] = theta_dot_l;//(1.0f - 0.75f) * balanceControl->state_l[1] + 0.75f * theta_dot_l;
|
||||
balanceControl->state_l[2] = x_est_l;//x[0];//balanceControl->v_estimator.Estimate_X_[0];
|
||||
balanceControl->state_l[3] = x_dot_est_l;//x_dot[0];//balanceControl->v_estimator.Estimate_X_[1];
|
||||
balanceControl->state_l[4] = body_phi;
|
||||
balanceControl->state_l[5] = body_phi_dot;//(1.0f - 0.75f) * balanceControl->state_l[5] + 0.75f * body_phi_dot;
|
||||
|
||||
//地面支持力解算
|
||||
float theta_state_r[3] = {theta_r,theta_dot_r,theta_dot2_r};
|
||||
static uint32_t right_cnt = 0,left_cnt = 0;
|
||||
balanceControl->FN_right = calculate_F(MotionAccel[2],theta_state_r,leg_r,&right_cnt);
|
||||
float theta_state_l[3] = {theta_l,theta_dot_l,theta_dot2_l};
|
||||
balanceControl->FN_left = calculate_F(MotionAccel[2],theta_state_l,leg_l,&left_cnt);
|
||||
|
||||
|
||||
mylqr_k(leg_r->legState.L0,K_matrix_r);
|
||||
mylqr_k(leg_l->legState.L0,K_matrix_l);
|
||||
//离地情况
|
||||
//注意电机输出反向力矩也会导致判断离地
|
||||
if((balanceControl->FN_right <= 10.0f || leg_r->legFeedback.F <= 10.0f) ||
|
||||
(balanceControl->jump_flag == 1 ||balanceControl->jump_flag == 2) )//&& leg_r->legState.L0>=0.15f
|
||||
{
|
||||
balanceControl->fly_flag_r = TRUE;
|
||||
float lqr_K[12] = {0};
|
||||
lqr_K[6] = K_matrix_r[6];
|
||||
lqr_K[7] = K_matrix_r[7];
|
||||
LQR_set_K(&balanceControl->LQR_r,lqr_K);
|
||||
}
|
||||
else
|
||||
{
|
||||
balanceControl->fly_flag_r = FALSE;
|
||||
LQR_set_K(&balanceControl->LQR_r,K_matrix_r);
|
||||
}
|
||||
if((balanceControl->FN_left <= 10.0f || leg_l->legFeedback.F <= 10.0f)||
|
||||
(balanceControl->jump_flag == 1 || balanceControl->jump_flag == 2) )//&& leg_l->legState.L0>=0.15f
|
||||
{
|
||||
balanceControl->fly_flag_l = TRUE;
|
||||
float lqr_K[12] = {0};
|
||||
lqr_K[6] = K_matrix_l[6];
|
||||
lqr_K[7] = K_matrix_l[7];
|
||||
LQR_set_K(&balanceControl->LQR_l,lqr_K);
|
||||
}
|
||||
else
|
||||
{
|
||||
balanceControl->fly_flag_l = FALSE;
|
||||
LQR_set_K(&balanceControl->LQR_l,K_matrix_l);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
#define F_FEEDFORWARD 30.0f //腿部支持力前馈
|
||||
#define L_MAX 0.40f
|
||||
#define L_MIN 0.13f //腿长最大最小值
|
||||
|
||||
void Balance_Control_Loop(Balance_Control_t* balanceControl,uint8_t jump_flag)
|
||||
{
|
||||
LQR_update(&balanceControl->LQR_r,balanceControl->state_r,balanceControl->target_state_r);
|
||||
LQR_update(&balanceControl->LQR_l,balanceControl->state_l,balanceControl->target_state_l);
|
||||
|
||||
balanceControl->LQR_r.control_vector[1] = - balanceControl->LQR_r.control_vector[1];
|
||||
float Tp_r = balanceControl->LQR_r.control_vector[1]; //髋关节力矩
|
||||
float T_r = balanceControl->LQR_r.control_vector[0]; //驱动轮力矩
|
||||
|
||||
balanceControl->LQR_l.control_vector[1] = - balanceControl->LQR_l.control_vector[1];
|
||||
float Tp_l = balanceControl->LQR_l.control_vector[1]; //髋关节力矩
|
||||
float T_l = balanceControl->LQR_l.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,balanceControl->target_L0);
|
||||
//float F_pid_out_l = PIDCalculate(&balanceControl->leg_length_PID,average_length,leg_r->target_L0);
|
||||
|
||||
//跳跃控制
|
||||
static float F_jump_l,F_jump_r;
|
||||
float max_scale = 10.0f;
|
||||
float K = (max_scale - 1) * F_FEEDFORWARD / (L_MIN - L_MAX);
|
||||
F_jump_l = K*(leg_l->legState.L0-L_MIN) + max_scale * F_FEEDFORWARD;
|
||||
F_jump_r = K*(leg_r->legState.L0-L_MIN) + max_scale * F_FEEDFORWARD;
|
||||
|
||||
//跳跃支持力
|
||||
F_jump_l = float_constrain(F_jump_l,F_FEEDFORWARD,max_scale * F_FEEDFORWARD);
|
||||
F_jump_r = float_constrain(F_jump_r,F_FEEDFORWARD,max_scale * F_FEEDFORWARD);
|
||||
|
||||
|
||||
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 = 0;//PIDCalculate(&balanceControl->leg_coordination_PID,coordination_err,0);
|
||||
|
||||
if(jump_flag == 1)//起跳阶段
|
||||
{
|
||||
Leg_Input_update(&leg_r->legInput,F_jump_r,Tp_r + cor_Tp);
|
||||
Leg_Input_update(&leg_l->legInput,F_jump_l,Tp_l - cor_Tp);
|
||||
}
|
||||
|
||||
else if(jump_flag == 2) //收腿阶段
|
||||
{
|
||||
Leg_Input_update(&leg_r->legInput,-100,Tp_r + cor_Tp);
|
||||
Leg_Input_update(&leg_l->legInput,-100,Tp_l - cor_Tp);
|
||||
}
|
||||
else //非跳跃阶段
|
||||
{
|
||||
Leg_Input_update(&leg_r->legInput,F_pid_out + leg_r->F_feedforward + 40,Tp_r + cor_Tp);
|
||||
Leg_Input_update(&leg_l->legInput,F_pid_out + leg_l->F_feedforward + 40,Tp_l - cor_Tp);
|
||||
}
|
||||
|
||||
|
||||
// Leg_Input_update(&leg_r->legInput,F_pid_out_r + 20 ,Tp + cor_Tp);
|
||||
// Leg_Input_update(&leg_l->legInput,F_pid_out_l + 20 ,Tp - cor_Tp);
|
||||
|
||||
VMC_getT(leg_r);
|
||||
VMC_getT(leg_l);
|
||||
}
|
||||
|
||||
void target_x_set(Balance_Control_t* balanceControl,float add_x)
|
||||
{
|
||||
balanceControl->target_state_r[3] = add_x;
|
||||
balanceControl->target_state_l[3] = add_x;
|
||||
|
||||
if( fabsf(add_x) >= 0.01f)
|
||||
{
|
||||
balanceControl->target_state_r[2] = balanceControl->state_r[2];
|
||||
balanceControl->target_state_l[2] = balanceControl->state_l[2];
|
||||
}
|
||||
}
|
||||
|
||||
void state_clear(Balance_Control_t* balanceControl)
|
||||
{
|
||||
//清除位移状态量
|
||||
balanceControl->target_state_r[2] = 0;
|
||||
balanceControl->state_r[2] = 0;
|
||||
|
||||
balanceControl->target_state_l[2] = 0;
|
||||
balanceControl->state_l[2] = 0;
|
||||
|
||||
//estimator_init(&balanceControl->v_estimator,0.001f,10.0f);
|
||||
}
|
||||
|
||||
void target_L_set(Balance_Control_t* balanceControl,float add_L)
|
||||
{
|
||||
balanceControl->target_L0 += add_L;
|
||||
balanceControl->target_L0 = float_constrain(balanceControl->target_L0,L_MIN,L_MAX);
|
||||
}
|
||||
|
||||
|
||||
|
||||
/*
|
||||
* MYLQR_K MATLAB生成
|
||||
* K = MYLQR_K(L0)
|
||||
*
|
||||
* Arguments : float L0
|
||||
* float K[12]
|
||||
* Return Type : void
|
||||
*/
|
||||
void mylqr_k(float L0, float K_mat[12])
|
||||
{
|
||||
float t2;
|
||||
float t3;
|
||||
|
||||
float K[12];
|
||||
/* This function was generated by the Symbolic Math Toolbox version 9.2.
|
||||
*/
|
||||
|
||||
|
||||
/* 06-May-2024 17:35:50 */
|
||||
t2 = L0*L0;
|
||||
t3 = t2*L0;
|
||||
K[0] =
|
||||
((L0 * -282.545074F + t2 * 330.464752F) - t3 * 225.209259F) - 21.1259251F;
|
||||
K[1] =
|
||||
((L0 * 191.247147F - t2 * 904.015686F) + t3 * 1025.96033F) + 44.8702393F;
|
||||
K[2] =
|
||||
((L0 * -38.6851654F + t2 * 7.79979753F) - t3 * 7.8749814F) - 5.59480667F;
|
||||
K[3] =
|
||||
((L0 * -10.0553722F - t2 * 60.9720421F) + t3 * 94.1053925F) + 17.016264F;
|
||||
K[4] =
|
||||
((L0 * 8.41862774F - t2 * 39.7602615F) + t3 * 49.5394897F) - 9.96936703F;
|
||||
K[5] =
|
||||
((L0 * 51.7300377F - t2 * 238.936829F) + t3 * 284.121246F) + 3.30276108F;
|
||||
K[6] =
|
||||
((L0 * 58.7921486F - t2 * 213.884598F) + t3 * 239.798187F) - 32.5602646F;
|
||||
K[7] =
|
||||
((L0 * 112.231422F - t2 * 583.832947F) + t3 * 718.246582F) + 13.6089172F;
|
||||
K[8] =
|
||||
((L0 * -198.74147F + t2 * 196.315338F) - t3 * 42.9540405F) + 89.0673752F;
|
||||
K[9] = ((L0 * 348.149323F - t2 * 531.783142F) + t3 * 252.826599F) + 70.4726F;
|
||||
K[10] =
|
||||
((L0 * -30.6359673F + t2 * 40.8268318F) - t3 * 21.4240017F) + 15.1511059F;
|
||||
K[11] =
|
||||
((L0 * 40.7163315F - t2 * 35.2042122F) - t3 * 12.030282F) + 9.66719341F;
|
||||
//matlab赋值顺序不同
|
||||
for (int i = 0; i < 6; ++i) {
|
||||
K_mat[i] = K[2*i];
|
||||
}
|
||||
for (int i = 0; i < 6; ++i) {
|
||||
K_mat[6+i] = K[2*i+1];
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* File trailer for mylqr_k.c
|
||||
*
|
||||
* [EOF]
|
||||
*/
|
||||
|
||||
|
||||
/**
|
||||
* @brief 解算地面支持力 F
|
||||
* @param float theta[3] = {theta,theta_dot,theta_dot2}
|
||||
* @retval none
|
||||
* @attention
|
||||
*/
|
||||
float calculate_F(float MotionAccel_z,float theta[3],LegInstance *leg ,uint32_t *cnt_last)
|
||||
{
|
||||
//求导所用时间
|
||||
//static uint32_t dot_cnt = 0;
|
||||
// static float last_theta_dot,last_L_dot;
|
||||
// float dot_dt = DWT_GetDeltaT(&dot_cnt);
|
||||
// float theta_dot2 = (last_theta_dot - theta_dot) / dot_dt;
|
||||
// float L_dot2 = (last_L_dot - L_dot) / dot_dt;
|
||||
|
||||
float L = leg->legState.L0;
|
||||
float L_dot = leg->legState.L0_dot;
|
||||
float L_dot2 = leg->legState.L0_dot2;
|
||||
|
||||
float cos_theta = arm_cos_f32(theta[0]);
|
||||
float sin_theta = arm_sin_f32(theta[0]);
|
||||
float zw_dot2 = MotionAccel_z - L_dot2 * cos_theta + 2 * L_dot * theta[1] * sin_theta
|
||||
+ L * theta[2] * sin_theta + L * theta[1] * theta[1] * cos_theta;
|
||||
|
||||
float P = leg->legFeedback.F * cos_theta + leg->legFeedback.Tp * sin_theta/L;
|
||||
|
||||
float F_N = P + 0.8f * (zw_dot2 + 9.8f);
|
||||
|
||||
return F_N;
|
||||
|
||||
}
|
||||
|
|
@ -1,106 +0,0 @@
|
|||
//
|
||||
// Created by SJQ on 2024/3/2.
|
||||
//
|
||||
|
||||
#include "balance.h"
|
||||
void get_lqr_k(float L0, float K[12]);
|
||||
|
||||
balance::balance(Balance_Init_config_s *InitConfig):
|
||||
leg_r_(&InitConfig->legInitConfig),leg_l_(&InitConfig->legInitConfig)
|
||||
{
|
||||
PIDInit(&leg_coordination_PID_,&InitConfig->leg_cor_PID_config);
|
||||
PIDInit(&leg_length_PID_,&InitConfig->legInitConfig.length_PID_conf);
|
||||
memset(state_,0,6);
|
||||
memset(target_state_,0,6);
|
||||
}
|
||||
|
||||
void balance::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)
|
||||
{
|
||||
leg_r_.state_update(leg_phi[0],leg_phi[1],leg_phi_dot[0],leg_phi_dot[1]);
|
||||
leg_l_.state_update(leg_phi[2],leg_phi[3],leg_phi_dot[2],leg_phi_dot[3]);
|
||||
|
||||
Matrixf<4,1> leg_r_state = leg_r_.get_state();
|
||||
Matrixf<4,1> leg_l_state = leg_l_.get_state();
|
||||
float L_average = (leg_r_state[0][0] + leg_l_state[0][0])/2;
|
||||
float phi_average = (leg_r_state[1][0] + leg_l_state[1][0])/2;
|
||||
float phi_dot_average = (leg_r_state[3][0] + leg_l_state[3][0])/2;
|
||||
|
||||
float theta = PI/2 - phi_average - body_phi;
|
||||
float theta_dot = -phi_dot_average - body_phi_dot;
|
||||
state_[0] = theta;
|
||||
state_[1] = theta_dot;
|
||||
state_[2] = x; state_[3] = x_dot;
|
||||
state_[4] = body_phi; state_[5] = body_phi_dot;
|
||||
|
||||
float K_mat[12];
|
||||
get_lqr_k(L_average,K_mat);
|
||||
balance_LQR_.set_k(K_mat);
|
||||
}
|
||||
|
||||
void balance::control_loop() {
|
||||
balance_LQR_.update(state_,target_state_);
|
||||
|
||||
Matrixf<2,1> control = balance_LQR_.get_control();
|
||||
Matrixf<4,1> leg_r_state = leg_r_.get_state();
|
||||
Matrixf<4,1> leg_l_state = leg_l_.get_state();
|
||||
|
||||
float L_average = (leg_r_state[0][0] + leg_l_state[0][0])/2;
|
||||
float F_pid_out= PIDCalculate(&leg_length_PID_,L_average,target_length_);
|
||||
float coordination_err = leg_r_state[1][0] - leg_l_state[1][0];
|
||||
float cor_Tp = PIDCalculate(&leg_coordination_PID_,coordination_err,0);
|
||||
|
||||
leg_r_.input_update(F_pid_out + leg_r_.F_feedforward_ + 20,control[1][0] + cor_Tp);
|
||||
leg_l_.input_update(F_pid_out + leg_l_.F_feedforward_ + 20,control[1][0] - cor_Tp);
|
||||
}
|
||||
|
||||
void balance::target_set(float x) {
|
||||
target_state_[2] = x;
|
||||
}
|
||||
|
||||
Matrixf<2,1> balance::get_control() {
|
||||
return balance_LQR_.get_control();
|
||||
}
|
||||
|
||||
void get_lqr_k(float L0, float K[12])
|
||||
{
|
||||
float t2;
|
||||
float t3;
|
||||
|
||||
float K_temp[12];
|
||||
/* This function was generated by the Symbolic Math Toolbox version 9.2.
|
||||
*/
|
||||
t2 = L0 * L0;
|
||||
t3 = t2 * L0;
|
||||
/* 24-Jan-2024 14:05:09 */
|
||||
K_temp[0] = ((L0 * -141.116669F + t2 * 130.64769F) - t3 * 75.0153351F) - 1.856107F;
|
||||
K_temp[1] =
|
||||
((L0 * 61.3573303F - t2 * 179.144104F) + t3 * 174.797318F) + 2.33913779F;
|
||||
K_temp[2] =
|
||||
((L0 * -13.9213181F - t2 * 17.6587467F) + t3 * 15.9025316F) - 0.53222841F;
|
||||
K_temp[3] =
|
||||
((L0 * 2.06703424F - t2 * 7.27073288F) + t3 * 6.41747713F) + 0.598731041F;
|
||||
K_temp[4] =
|
||||
((L0 * -3.48888779F + t2 * 5.95020676F) - t3 * 3.25835323F) - 9.27006531F;
|
||||
K_temp[5] =
|
||||
((L0 * -19.3605652F + t2 * 5.27040243F) + t3 * 19.7889462F) + 8.40925217F;
|
||||
K_temp[6] = ((L0 * -0.848105669F - t2 * 14.6889477F) + t3 * 20.0115433F) -
|
||||
10.4576302F;
|
||||
K_temp[7] =
|
||||
((L0 * -25.0337353F + t2 * 27.6494694F) - t3 * 6.41098928F) + 8.5957756F;
|
||||
K_temp[8] =
|
||||
((L0 * -67.2155914F + t2 * 92.1350479F) - t3 * 50.9733543F) + 29.9440556F;
|
||||
K_temp[9] =
|
||||
((L0 * 74.7469635F - t2 * 150.980896F) + t3 * 115.21946F) + 63.3836441F;
|
||||
K_temp[10] =
|
||||
((L0 * -4.06481314F + t2 * 2.58104968F) + t3 * 0.92904079F) + 4.43995333F;
|
||||
K_temp[11] =
|
||||
((L0 * 4.27840805F + t2 * 2.30248642F) - t3 * 10.0091085F) + 3.51449132F;
|
||||
|
||||
//matlab赋值顺序不同
|
||||
for (int i = 0; i < 6; ++i) {
|
||||
K[i] = K_temp[2*i];
|
||||
}
|
||||
for (int i = 0; i < 6; ++i) {
|
||||
K[6+i] = K_temp[2*i+1];
|
||||
}
|
||||
}
|
||||
|
|
@ -1,38 +1,62 @@
|
|||
//
|
||||
// Created by SJQ on 2024/3/2.
|
||||
// Created by SJQ on 2023/12/30.
|
||||
//
|
||||
|
||||
#ifndef WHEEL_LEGGED_BALANCE_H
|
||||
#define WHEEL_LEGGED_BALANCE_H
|
||||
|
||||
#include "leg.h"
|
||||
#include "LQR.h"
|
||||
#include "LQR_old.h"
|
||||
#include "controller.h"
|
||||
#include "kalman_filter.h"
|
||||
#include "estimator.h"
|
||||
#include "observer.h"
|
||||
typedef struct
|
||||
{
|
||||
LegInstance leg_right;
|
||||
LegInstance leg_left;
|
||||
PIDInstance leg_coordination_PID; //双腿协调PD控制
|
||||
|
||||
PIDInstance leg_length_PID; //平均腿长控制
|
||||
|
||||
float state_r[6];
|
||||
float target_state_r[6];
|
||||
float state_l[6];
|
||||
float target_state_l[6];
|
||||
|
||||
float v_est[4];
|
||||
|
||||
float target_L0; //平均腿长目标值
|
||||
float FN_left; //地面支持力
|
||||
float FN_right;
|
||||
|
||||
//双腿LQR分开计算
|
||||
LQRInstance LQR_l;
|
||||
LQRInstance LQR_r;
|
||||
estimator_t v_estimator;
|
||||
BalanceObserver state_observer;
|
||||
|
||||
uint8_t jump_flag;//跳跃标志
|
||||
uint8_t fly_flag_r; //右腿离地标志
|
||||
uint8_t fly_flag_l; //左腿离地标志
|
||||
|
||||
}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;
|
||||
|
||||
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_;
|
||||
};
|
||||
|
||||
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,const float x[2],const float x_dot[2],const float MotionAccel[3]);
|
||||
void Balance_Control_Loop(Balance_Control_t* balanceControl,uint8_t jump_flag);
|
||||
void target_x_set(Balance_Control_t* balanceControl,float add_x);
|
||||
void target_L_set(Balance_Control_t* balanceControl,float add_L);
|
||||
void state_clear(Balance_Control_t* balanceControl);
|
||||
|
||||
#endif //WHEEL_LEGGED_BALANCE_H
|
||||
|
|
|
|||
|
|
@ -5,20 +5,20 @@
|
|||
#include "estimator.h"
|
||||
static const float dt = 1e-3f;
|
||||
//状态转移矩阵
|
||||
static float F[4] = {0,1,
|
||||
1,0};
|
||||
static float F[4] = {1,dt,
|
||||
0,1};
|
||||
//控制矩阵
|
||||
static float B[2] = {dt,
|
||||
1};
|
||||
static float B[2] = {0.5f*dt*dt,
|
||||
dt};
|
||||
//观测矩阵
|
||||
static float H[4] = {1,0,
|
||||
0,1};
|
||||
//后验估计协方差初始值
|
||||
static float P[4] = {100, 0.1f,
|
||||
0.1f, 100};
|
||||
// R Q矩阵初始值（其实这里设置多少都无所谓）
|
||||
static float Q[4] = {1.0f, 0,
|
||||
0, 1.0f};
|
||||
static float P[4] = {1, 0,
|
||||
0, 1};
|
||||
// R Q矩阵初始值（其实这里设置多少都无所谓） 需要在调用初始化函数时设置
|
||||
static float Q[4] = {1e-3f, 0,
|
||||
0, 1e-3f};
|
||||
static float R[4] = {1000.0f, 0,
|
||||
0, 1000.0f,};
|
||||
void estimator_init(estimator_t *est ,float process_noise, float measure_noise) {
|
||||
|
|
@ -49,8 +49,19 @@ void estimator_update(estimator_t *est ,float x,float x_dot,float ax) {
|
|||
Kalman_Filter_Update(&est->EstimateKF_);
|
||||
|
||||
// 提取估计值
|
||||
|
||||
for (uint8_t i = 0; i < 2; i++)
|
||||
{
|
||||
est->Estimate_X_[i] = est->EstimateKF_.FilteredValue[i];
|
||||
if(isnanf(est->EstimateKF_.FilteredValue[i]))
|
||||
{
|
||||
memset(est->EstimateKF_.xhat_data,0,est->EstimateKF_.xhatSize * 4);
|
||||
memset(est->EstimateKF_.xhatminus_data,0,est->EstimateKF_.xhatSize * 4);
|
||||
memset(est->EstimateKF_.FilteredValue,0,est->EstimateKF_.xhatSize * 4);
|
||||
}
|
||||
else
|
||||
{
|
||||
est->Estimate_X_[i] = est->EstimateKF_.FilteredValue[i];
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -2,7 +2,7 @@
|
|||
// Created by SJQ on 2023/12/25.
|
||||
//
|
||||
|
||||
#include "leg_old.h"
|
||||
#include "leg.h"
|
||||
#include "math.h"
|
||||
#include "main.h"
|
||||
#include "arm_math.h"
|
||||
|
|
@ -120,8 +120,11 @@ void calculate_leg_pos(Leg_State_t* leg_state)
|
|||
leg_state->L0 = L0;
|
||||
leg_state->phi0 = phi0;
|
||||
leg_state->phi2 = phi2;
|
||||
|
||||
leg_state->phi3 = phi3;
|
||||
|
||||
leg_state->position[0] = xb; leg_state->position[1] = yb;
|
||||
leg_state->position[2] = xc; leg_state->position[3] = yc;
|
||||
leg_state->position[4] = xd; leg_state->position[5] = yd;
|
||||
}
|
||||
|
||||
void VMC_getT(LegInstance* legInstance)
|
||||
|
|
@ -1,155 +0,0 @@
|
|||
//
|
||||
// Created by SJQ on 2024/3/1.
|
||||
//
|
||||
|
||||
|
||||
#include "leg.h"
|
||||
#include <cmath>
|
||||
|
||||
|
||||
static const float l[5]={0.15f,0.27f,0.27f,0.15f,0.15f};
|
||||
|
||||
leg::leg(Leg_init_config_s* legInitConfig)
|
||||
{
|
||||
PIDInit(&Length_PID_,&legInitConfig->length_PID_conf);
|
||||
PIDInit(&Phi0_PID_,&legInitConfig->phi0_PID_conf);
|
||||
|
||||
target_L0_ = legInitConfig->init_target_L0;
|
||||
F_feedforward_ = legInitConfig->F_feedforward;
|
||||
}
|
||||
|
||||
void leg::input_update(float F, float Tp)
|
||||
{
|
||||
legInput_ = Matrixf<2,1>(new (float[2]){F,Tp});
|
||||
}
|
||||
|
||||
void leg::state_update(float phi1,float phi4 ,float phi1_dot,float phi4_dot)
|
||||
{
|
||||
if(phi1<=PI/2)
|
||||
phi1 = PI/2;
|
||||
if(phi4>=PI/2)
|
||||
phi4 = PI/2;
|
||||
|
||||
legState_.phi1 = phi1;
|
||||
legState_.phi4 = phi4;
|
||||
|
||||
//以机体中点建立坐标系
|
||||
float xb = -l[4]/2+l[0]* arm_cos_f32(legState_.phi1);
|
||||
float yb = l[0]* arm_sin_f32(legState_.phi1);
|
||||
|
||||
float xd = l[4]/2+l[3]* arm_cos_f32(legState_.phi4);
|
||||
float yd = l[3]* arm_sin_f32(legState_.phi4);
|
||||
|
||||
float A0 = 2*l[1]*(xd-xb);
|
||||
float B0 = 2*l[1]*(yd-yb);
|
||||
|
||||
float lbd2 = (xd-xb)*(xd-xb)+(yd-yb)*(yd-yb);
|
||||
float C0 = l[1]*l[1]+lbd2-l[2]*l[2];
|
||||
|
||||
float tanPhi = 0;
|
||||
|
||||
arm_sqrt_f32((A0*A0 + B0*B0 - C0*C0),&tanPhi);
|
||||
//tanPhi = (B0+tanPhi)/(A0+C0);
|
||||
|
||||
float phi2 = 2* atan2f(B0+tanPhi,A0+C0);
|
||||
float xc = xb + l[1] * arm_cos_f32(phi2);
|
||||
float yc = yb + l[1] * arm_sin_f32(phi2);
|
||||
|
||||
float phi3 = acosf(xc-xd/l[2]);
|
||||
float L0 = 0;
|
||||
arm_sqrt_f32((xc*xc + yc*yc),&L0);
|
||||
float phi0 = atan2f(yc,xc);
|
||||
|
||||
//赋值
|
||||
legState_.L0 = L0;
|
||||
legState_.phi0 = phi0;
|
||||
legState_.phi2 = phi2;
|
||||
legState_.phi3 = phi3;
|
||||
|
||||
get_dot(phi1_dot,phi4_dot);
|
||||
}
|
||||
|
||||
void leg::calculate_output() {
|
||||
float sinPhi23 = arm_sin_f32(legState_.phi2 - legState_.phi3);
|
||||
float sinPhi12 = arm_sin_f32(legState_.phi1 - legState_.phi2);
|
||||
float sinPhi34 = arm_sin_f32(legState_.phi3 - legState_.phi4);
|
||||
|
||||
float sinPhi03 = arm_sin_f32(legState_.phi0 - legState_.phi3);
|
||||
float sinPhi02 = arm_sin_f32(legState_.phi0 - legState_.phi2);
|
||||
float cosPhi03 = arm_cos_f32(legState_.phi0 - legState_.phi3);
|
||||
float cosPhi02 = arm_cos_f32(legState_.phi0 - legState_.phi2);
|
||||
|
||||
float Jacobi_data[4] = {0};
|
||||
Jacobi_data[0] = -(l[0]* sinPhi03*sinPhi12)/sinPhi23;
|
||||
Jacobi_data[1] = -(l[0]* cosPhi03*sinPhi12)/(legState_.L0 * sinPhi23);
|
||||
|
||||
Jacobi_data[2] = -(l[3]* sinPhi02*sinPhi34)/sinPhi23;
|
||||
Jacobi_data[3] = -(l[3]* cosPhi02*sinPhi34)/(legState_.L0 * sinPhi23);
|
||||
|
||||
VMC_Jacobi_ = Matrixf<2,2>(Jacobi_data);
|
||||
legOutput_ = VMC_Jacobi_ * legInput_;
|
||||
}
|
||||
|
||||
void leg::feedback_update(float T1_fb, float T2_fb) {
|
||||
Matrixf<2,1> T12(new (float[2]){T1_fb,T2_fb});
|
||||
legFeedback_ = matrixf::inv(VMC_Jacobi_) * T12;
|
||||
}
|
||||
|
||||
void leg::get_dot(float phi1_dot,float phi4_dot)
|
||||
{
|
||||
//多项式拟合方法获得雅可比矩阵 待测试
|
||||
float phi_1 = legState_.phi1;
|
||||
float phi_4 = legState_.phi4;
|
||||
|
||||
float phi1_sq = phi_1*phi_1;
|
||||
float phi4_sq = phi_4*phi_4;
|
||||
float pose_Jacobi[4] = {0};
|
||||
float temp = sqrtf(powf((- 0.03992f*phi1_sq + 0.0004128f*phi_1*phi_4 + 0.302f*phi_1 + 0.04085f*phi4_sq + 0.04947f*phi_4 - 0.5549f),2) + powf((0.006896f*phi1_sq - 0.03657f*phi_1*phi_4 - 0.1217f*phi_1 + 0.0007632f*phi4_sq + 0.1984f*phi_4 + 0.466f),2));
|
||||
|
||||
pose_Jacobi[0] =
|
||||
(0.5f*(2.0f*(0.0004128f*phi_4 - 0.07984f*phi_1 + 0.302f)*(- 0.03992f*phi1_sq + 0.0004128f*phi_1*phi_4 + 0.302f*phi_1 + 0.04085f*phi4_sq + 0.04947f*phi_4 - 0.5549f) - 2.0f*(0.03657f*phi_4 - 0.01379f*phi_1 + 0.1217f)*(0.006896f*phi1_sq - 0.03657f*phi_1*phi_4 - 0.1217f*phi_1 + 0.0007632f*phi4_sq + 0.1984f*phi_4 + 0.466f)))/temp;
|
||||
pose_Jacobi[1] =
|
||||
(0.5f*(2.0f*(0.0004128f*phi_1 + 0.0817f*phi_4 + 0.04947f)*(- 0.03992f*phi1_sq + 0.0004128f*phi_1*phi_4 + 0.302f*phi_1 + 0.04085f*phi4_sq + 0.04947f*phi_4 - 0.5549f) + 2.0f*(0.001526f*phi_4 - 0.03657f*phi_1 + 0.1984f)*(0.006896f*phi1_sq - 0.03657f*phi_1*phi_4 - 0.1217f*phi_1 + 0.0007632f*phi4_sq + 0.1984f*phi_4 + 0.466f)))/temp;
|
||||
|
||||
float temp2 = (powf((0.006896f*phi1_sq - 0.03657f*phi_1*phi_4 - 0.1217f*phi_1 + 0.0007632f*phi4_sq + 0.1984f*phi_4 + 0.466f),2)/powf((- 0.03992f*phi1_sq + 0.0004128f*phi_1*phi_4 + 0.302f*phi_1 + 0.04085f*phi4_sq + 0.04947f*phi_4 - 0.5549f),2) + 1.0f);
|
||||
|
||||
pose_Jacobi[2] =
|
||||
((0.03657f*phi_4 - 0.01379f*phi_1 + 0.1217f)/(- 0.03992f*phi1_sq + 0.0004128f*phi_1*phi_4 + 0.302f*phi_1 + 0.04085f*phi4_sq + 0.04947f*phi_4 - 0.5549f) + ((0.0004128f*phi_4 - 0.07984f*phi_1 + 0.302f)*(0.006896f*phi1_sq - 0.03657f*phi_1*phi_4 - 0.1217f*phi_1 + 0.0007632f*phi4_sq + 0.1984f*phi_4 + 0.466f))/powf((- 0.03992f*phi1_sq + 0.0004128f*phi_1*phi_4 + 0.302f*phi_1 + 0.04085f*phi4_sq + 0.04947f*phi_4 - 0.5549f),2))/temp2;
|
||||
pose_Jacobi[3] =
|
||||
-(1.0f*((0.001526f*phi_4 - 0.03657f*phi_1 + 0.1984f)/(- 0.03992f*phi1_sq + 0.0004128f*phi_1*phi_4 + 0.302f*phi_1 + 0.04085f*phi4_sq + 0.04947f*phi_4 - 0.5549f) - (1.0f*(0.0004128f*phi_1 + 0.0817f*phi_4 + 0.04947f)*(0.006896f*phi1_sq - 0.03657f*phi_1*phi_4 - 0.1217f*phi_1 + 0.0007632f*phi4_sq + 0.1984f*phi_4 + 0.466f))/powf((- 0.03992f*phi1_sq + 0.0004128f*phi_1*phi_4 + 0.302f*phi_1 + 0.04085f*phi4_sq + 0.04947f*phi_4 - 0.5549f),2)))/temp2;
|
||||
Matrixf<2,2> pose_J(pose_Jacobi);
|
||||
|
||||
State_dot_ = pose_J * Matrixf<2,1>(new (float[2]){phi1_dot,phi4_dot});
|
||||
}
|
||||
|
||||
Matrixf<4, 1> leg::get_state() {
|
||||
return Matrixf<4, 1>(new (float[4]) {legState_.L0,legState_.phi0,*State_dot_[0],*State_dot_[1]});
|
||||
}
|
||||
|
||||
float leg::calculate_FN(float MotionAccel_z,float theta,float theta_dot) {
|
||||
//求导所用时间
|
||||
static uint32_t dot_cnt = 0;
|
||||
static float last_theta_dot,last_L_dot;
|
||||
float L = legState_.L0;
|
||||
float L_dot = State_dot_[0][0];
|
||||
|
||||
float dot_dt = DWT_GetDeltaT(&dot_cnt);
|
||||
float theta_dot2 = (last_theta_dot - theta_dot) / dot_dt;
|
||||
float L_dot2 = (last_L_dot - L_dot) / dot_dt;
|
||||
|
||||
float cos_theta = arm_cos_f32(theta);
|
||||
float sin_theta = arm_sin_f32(theta);
|
||||
float zw_dot2 = MotionAccel_z - L_dot2 * cos_theta + 2 * L_dot * theta_dot * sin_theta
|
||||
+ L * theta_dot2 * sin_theta + L * theta_dot * theta_dot * cos_theta;
|
||||
|
||||
float P = legFeedback_[0][0]*cos_theta + legFeedback_[1][0]*sin_theta/L;
|
||||
|
||||
float F_N = P + 0.8f * (zw_dot2 + 9.8f);
|
||||
|
||||
return F_N;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
|
@ -1,11 +1,9 @@
|
|||
//
|
||||
// Created by SJQ on 2024/3/1.
|
||||
// Created by SJQ on 2023/12/25.
|
||||
//
|
||||
|
||||
#ifndef WHEEL_LEGGED_LEG_H
|
||||
#define WHEEL_LEGGED_LEG_H
|
||||
|
||||
#include <matrix.h>
|
||||
#include "controller.h"
|
||||
|
||||
typedef struct
|
||||
|
|
@ -21,13 +19,31 @@ typedef struct
|
|||
|
||||
uint32_t DWT_CNT;
|
||||
float dt; //用于计算时间并求导
|
||||
float last_phi0;
|
||||
|
||||
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;
|
||||
|
|
@ -35,35 +51,36 @@ typedef struct
|
|||
float init_target_L0; //初始腿长目标值
|
||||
float F_feedforward;
|
||||
}Leg_init_config_s;
|
||||
|
||||
class leg
|
||||
typedef struct
|
||||
{
|
||||
public:
|
||||
leg(Leg_init_config_s* legInitConfig);
|
||||
void state_update(float phi1,float phi4 ,float phi1_dot,float phi4_dot);
|
||||
void input_update(float F,float Tp);
|
||||
void feedback_update(float T1_fb,float T2_fb);
|
||||
void calculate_output();
|
||||
Matrixf<4,1> get_state();
|
||||
float calculate_FN(float MotionAccel_z,float theta,float theta_dot);
|
||||
Leg_State_t legState;
|
||||
Leg_Input_t legInput;
|
||||
Leg_Input_t legFeedback;
|
||||
Leg_Output_t legOutput;
|
||||
|
||||
float F_feedforward_; //支持力前馈补偿
|
||||
private:
|
||||
Leg_State_t legState_;
|
||||
Matrixf<2,1> legInput_;
|
||||
Matrixf<2,1> legOutput_;
|
||||
Matrixf<2,1> legFeedback_;
|
||||
float target_L0;
|
||||
float target_phi0;
|
||||
PIDInstance Length_PID;
|
||||
PIDInstance Phi0_PID;
|
||||
|
||||
Matrixf<2,1> State_dot_; // L0 和 phi0 的导数
|
||||
float F_feedforward; //支持力前馈补偿
|
||||
|
||||
float target_L0_;
|
||||
float target_phi0_;
|
||||
PIDInstance Length_PID_;
|
||||
PIDInstance Phi0_PID_;
|
||||
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;
|
||||
|
||||
Matrixf<2,2> VMC_Jacobi_;
|
||||
|
||||
void get_dot(float phi1_dot,float phi4_dot);
|
||||
};
|
||||
|
||||
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
|
||||
|
|
|
|||
|
|
@ -1,242 +0,0 @@
|
|||
//
|
||||
// Created by SJQ on 2023/12/30.
|
||||
//
|
||||
|
||||
#include "balance_old.h"
|
||||
|
||||
static void mylqr_k(float L0, float K_mat[12]);
|
||||
|
||||
void Balance_Control_Init(Balance_Control_t* balanceControl,Balance_Init_config_s InitConfig)
|
||||
{
|
||||
|
||||
balanceControl->target_L0 = 0.15f;
|
||||
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);
|
||||
|
||||
estimator_init(&balanceControl->v_estimator,1,0.5);
|
||||
Observer_Init(&balanceControl->state_observer);
|
||||
memset(balanceControl->state,0,6);
|
||||
memset(balanceControl->target_state,0,6);
|
||||
}
|
||||
|
||||
//解算地面支持力F
|
||||
float calculate_F(float MotionAccel_z,float theta[3],LegInstance *leg ,uint32_t *cnt_last);
|
||||
|
||||
|
||||
// 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,const float MotionAccel[3])
|
||||
{
|
||||
LegInstance *leg_r = &balanceControl->leg_right;
|
||||
LegInstance *leg_l = &balanceControl->leg_left;
|
||||
|
||||
Leg_State_update(leg_r,leg_phi[0],leg_phi[1],leg_phi_dot[0],leg_phi_dot[1]);
|
||||
Leg_State_update(leg_l,leg_phi[2],leg_phi[3],leg_phi_dot[2],leg_phi_dot[3]);
|
||||
|
||||
float K_matrix[12]={0};
|
||||
float L_average = leg_r->legState.L0; //(leg_r->legState.L0 + leg_l->legState.L0)/2;
|
||||
//(leg_r->legState.L0 + leg_l->legState.L0)/2;
|
||||
float alpha = PI/2 - leg_r->legState.phi0;
|
||||
float theta = alpha - body_phi;
|
||||
float theta_dot = - leg_r->legState.phi0_dot - body_phi_dot;
|
||||
|
||||
static uint32_t dot_cnt = 0;
|
||||
float dot_dt = DWT_GetDeltaT(&dot_cnt);
|
||||
float theta_dot2 = (balanceControl->state[1] - theta_dot) / dot_dt;
|
||||
|
||||
//计算机体速度
|
||||
float xb = x + 2 * leg_r->legState.L0 * arm_sin_f32(theta);
|
||||
float xb_dot = x_dot + 2*(leg_r->legState.L0_dot* arm_sin_f32(theta) + leg_r->legState.L0 * theta_dot * arm_cos_f32(theta));
|
||||
|
||||
estimator_update(&balanceControl->v_estimator,xb,xb_dot,MotionAccel[0]);
|
||||
|
||||
balanceControl->state[0] = theta;
|
||||
balanceControl->state[1] = (1.0f - 0.75f) * balanceControl->state[1] +
|
||||
0.75f * theta_dot;
|
||||
//balanceControl->state[1] = theta_dot;
|
||||
balanceControl->state[2] = x;
|
||||
balanceControl->state[3] = x_dot;
|
||||
|
||||
balanceControl->state[4] = body_phi;
|
||||
balanceControl->state[5] = (1.0f - 0.75f) * balanceControl->state[5] +
|
||||
0.75f * body_phi_dot;
|
||||
// balanceControl->state[5] = body_phi_dot;
|
||||
|
||||
//Observer_Estimate(&balanceControl->state_observer,balanceControl->state,balanceControl->balance_LQR.control_vector,L_average);
|
||||
|
||||
//地面支持力解算
|
||||
//balanceControl->FN_right = calculate_F(MotionAccel[2],theta,theta_dot,leg_r->legState.L0,leg_r->legState.L0_dot,leg_r->legFeedback.F,leg_r->legFeedback.Tp);
|
||||
//balanceControl->FN_left = calculate_F(MotionAccel[2],theta,theta_dot,leg_l->legState.L0,leg_l->legState.L0_dot,leg_l->legFeedback.F,leg_l->legFeedback.Tp);
|
||||
float theta_state[3] = {theta,theta_dot,theta_dot2};
|
||||
static uint32_t right_cnt = 0,left_cnt = 0;
|
||||
balanceControl->FN_right = calculate_F(MotionAccel[2],theta_state,leg_r,&right_cnt);
|
||||
balanceControl->FN_left = calculate_F(MotionAccel[2],theta_state,leg_l,&left_cnt);
|
||||
|
||||
mylqr_k(L_average,K_matrix);
|
||||
|
||||
|
||||
//离地情况
|
||||
if(balanceControl->FN_right <= 20.0f && balanceControl->FN_left <= 20.0f)
|
||||
{
|
||||
float lqr_K[12] = {0};
|
||||
lqr_K[6] = K_matrix[6];
|
||||
lqr_K[7] = K_matrix[7];
|
||||
LQR_set_K(&balanceControl->balance_LQR,lqr_K);
|
||||
}
|
||||
else
|
||||
{
|
||||
LQR_set_K(&balanceControl->balance_LQR,K_matrix);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
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,balanceControl->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 + 30,Tp + cor_Tp);
|
||||
Leg_Input_update(&leg_l->legInput,F_pid_out + leg_l->F_feedforward + 30,Tp - cor_Tp);
|
||||
|
||||
// Leg_Input_update(&leg_r->legInput,F_pid_out_r + 20 ,Tp + cor_Tp);
|
||||
// Leg_Input_update(&leg_l->legInput,F_pid_out_l + 20 ,Tp - cor_Tp);
|
||||
|
||||
VMC_getT(leg_r);
|
||||
VMC_getT(leg_l);
|
||||
}
|
||||
|
||||
void target_x_set(Balance_Control_t* balanceControl,float add_x)
|
||||
{
|
||||
balanceControl->target_state [2] += add_x;
|
||||
}
|
||||
|
||||
void state_clear(Balance_Control_t* balanceControl)
|
||||
{
|
||||
//清除位移状态量
|
||||
balanceControl->target_state [2] = 0;
|
||||
balanceControl->state[2] = 0;
|
||||
}
|
||||
|
||||
void target_L_set(Balance_Control_t* balanceControl,float add_L)
|
||||
{
|
||||
balanceControl->target_L0 += add_L;
|
||||
float_constrain(balanceControl->target_L0,0.15f,0.30f);
|
||||
}
|
||||
|
||||
|
||||
|
||||
/*
|
||||
* MYLQR_K MATLAB生成
|
||||
* K = MYLQR_K(L0)
|
||||
*
|
||||
* Arguments : float L0
|
||||
* float K[12]
|
||||
* Return Type : void
|
||||
*/
|
||||
void mylqr_k(float L0, float K_mat[12])
|
||||
{
|
||||
float t2;
|
||||
float t3;
|
||||
|
||||
float K[12];
|
||||
/* This function was generated by the Symbolic Math Toolbox version 9.2.
|
||||
*/
|
||||
t2 = L0 * L0;
|
||||
t3 = t2 * L0;
|
||||
/* 11-Mar-2024 19:12:17 */
|
||||
K[0] =
|
||||
((L0 * -201.705292F + t2 * 289.584625F) - t3 * 247.715317F) - 14.6884327F;
|
||||
K[1] = ((L0 * 133.66568F - t2 * 384.819702F) + t3 * 354.437653F) + 9.50502F;
|
||||
K[2] =
|
||||
((L0 * -20.6946182F - t2 * 2.9124589F) - t3 * 2.76665568F) - 3.57213783F;
|
||||
K[3] =
|
||||
((L0 * 2.46232843F - t2 * 18.8637314F) + t3 * 18.9820461F) + 4.76390266F;
|
||||
K[4] =
|
||||
((L0 * 16.4841461F - t2 * 49.2097549F) + t3 * 45.4498405F) - 23.5533714F;
|
||||
K[5] = ((L0 * 182.920456F - t2 * 598.236755F) + t3 * 622.566F) - 10.9050007F;
|
||||
K[6] =
|
||||
((L0 * 56.6551895F - t2 * 155.341293F) + t3 * 140.137665F) - 27.346283F;
|
||||
K[7] =
|
||||
((L0 * 165.648911F - t2 * 564.416138F) + t3 * 604.892395F) - 9.52724552F;
|
||||
K[8] =
|
||||
((L0 * -97.1309662F + t2 * 154.570053F) - t3 * 113.610588F) + 42.7205772F;
|
||||
K[9] =
|
||||
((L0 * -20.722168F + t2 * 194.949692F) - t3 * 261.115692F) + 32.8661461F;
|
||||
K[10] =
|
||||
((L0 * -16.3245182F + t2 * 33.2480202F) - t3 * 28.061224F) + 7.589993F;
|
||||
K[11] =
|
||||
((L0 * -17.9091167F + t2 * 72.7530289F) - t3 * 82.8507385F) + 4.82622719F;
|
||||
//matlab赋值顺序不同
|
||||
for (int i = 0; i < 6; ++i) {
|
||||
K_mat[i] = K[2*i];
|
||||
}
|
||||
for (int i = 0; i < 6; ++i) {
|
||||
K_mat[6+i] = K[2*i+1];
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* File trailer for mylqr_k.c
|
||||
*
|
||||
* [EOF]
|
||||
*/
|
||||
|
||||
|
||||
/**
|
||||
* @brief 解算地面支持力 F
|
||||
* @param float theta[3] = {theta,theta_dot,theta_dot2}
|
||||
* @retval none
|
||||
* @attention
|
||||
*/
|
||||
float calculate_F(float MotionAccel_z,float theta[3],LegInstance *leg ,uint32_t *cnt_last)
|
||||
{
|
||||
//求导所用时间
|
||||
//static uint32_t dot_cnt = 0;
|
||||
// static float last_theta_dot,last_L_dot;
|
||||
// float dot_dt = DWT_GetDeltaT(&dot_cnt);
|
||||
// float theta_dot2 = (last_theta_dot - theta_dot) / dot_dt;
|
||||
// float L_dot2 = (last_L_dot - L_dot) / dot_dt;
|
||||
|
||||
float L = leg->legState.L0;
|
||||
float L_dot = leg->legState.L0_dot;
|
||||
float L_dot2 = leg->legState.L0_dot2;
|
||||
|
||||
float cos_theta = arm_cos_f32(theta[0]);
|
||||
float sin_theta = arm_sin_f32(theta[0]);
|
||||
float zw_dot2 = MotionAccel_z - L_dot2 * cos_theta + 2 * L_dot * theta[1] * sin_theta
|
||||
+ L * theta[2] * sin_theta + L * theta[1] * theta[1] * cos_theta;
|
||||
|
||||
float P = leg->legFeedback.F * cos_theta + leg->legFeedback.Tp * sin_theta/L;
|
||||
|
||||
float F_N = P + 0.8f * (zw_dot2 + 9.8f);
|
||||
|
||||
return F_N;
|
||||
|
||||
}
|
||||
|
|
@ -1,51 +0,0 @@
|
|||
//
|
||||
// Created by SJQ on 2023/12/30.
|
||||
//
|
||||
|
||||
#ifndef WHEEL_LEGGED_BALANCE_OLD_H
|
||||
#define WHEEL_LEGGED_BALANCE_OLD_H
|
||||
|
||||
#include "leg_old.h"
|
||||
#include "LQR_old.h"
|
||||
#include "controller.h"
|
||||
#include "kalman_filter.h"
|
||||
#include "estimator.h"
|
||||
#include "observer.h"
|
||||
typedef struct
|
||||
{
|
||||
LegInstance leg_right;
|
||||
LegInstance leg_left;
|
||||
PIDInstance leg_coordination_PID; //双腿协调PD控制
|
||||
|
||||
PIDInstance leg_length_PID; //平均腿长控制
|
||||
|
||||
float state[6];
|
||||
float target_state[6];
|
||||
float target_L0; //平均腿长目标值
|
||||
float FN_left; //地面支持力
|
||||
float FN_right;
|
||||
|
||||
LQRInstance balance_LQR;
|
||||
estimator_t v_estimator;
|
||||
BalanceObserver state_observer;
|
||||
|
||||
}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,const float MotionAccel[3]);
|
||||
void Balance_Control_Loop(Balance_Control_t* balanceControl);
|
||||
void target_x_set(Balance_Control_t* balanceControl,float add_x);
|
||||
void target_L_set(Balance_Control_t* balanceControl,float add_L);
|
||||
void state_clear(Balance_Control_t* balanceControl);
|
||||
|
||||
#endif //WHEEL_LEGGED_BALANCE_OLD_H
|
||||
|
|
@ -22,7 +22,7 @@
|
|||
#include "power_meter.h"
|
||||
#include "ins_task.h"
|
||||
|
||||
#include "balance_old.h"
|
||||
#include "balance.h"
|
||||
#include "observer.h"
|
||||
#include "estimator.h"
|
||||
|
||||
|
|
@ -74,7 +74,13 @@ BalanceObserver Observer;
|
|||
|
||||
PIDInstance Turn_Loop_PID , Roll_Loop_PID;
|
||||
|
||||
Chassis_state_e ChassisState;
|
||||
Chassis_state_e ChassisState,last_ChassisState;
|
||||
chassis_mode_e last_chassis_mode;
|
||||
|
||||
first_order_filter_type_t wheel_r_filter,wheel_l_filter;
|
||||
|
||||
first_order_filter_type_t ht_rf_filter,ht_rb_filter,ht_lf_filter,ht_lb_filter;
|
||||
first_order_filter_type_t vx_filter;
|
||||
|
||||
void ChassisInit()
|
||||
{
|
||||
|
|
@ -115,7 +121,6 @@ void ChassisInit()
|
|||
//chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_NORMAL;
|
||||
motor_rf = HTMotorInit(&chassis_motor_config);
|
||||
|
||||
|
||||
chassis_motor_config.can_init_config.tx_id = 2;
|
||||
//chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE;
|
||||
motor_rb = HTMotorInit(&chassis_motor_config);
|
||||
|
|
@ -125,6 +130,7 @@ void ChassisInit()
|
|||
//chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE;
|
||||
motor_lf = HTMotorInit(&chassis_motor_config);
|
||||
|
||||
|
||||
chassis_motor_config.can_init_config.tx_id = 4;
|
||||
//chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_NORMAL;
|
||||
motor_lb = HTMotorInit(&chassis_motor_config);
|
||||
|
|
@ -142,13 +148,16 @@ void ChassisInit()
|
|||
|
||||
referee_data = UITaskInit(&huart6,&ui_data); // 裁判系统初始化,会同时初始化UI
|
||||
|
||||
//超级电容
|
||||
SuperCap_Init_Config_s cap_conf = {
|
||||
.can_config = {
|
||||
.can_handle = &hcan2,
|
||||
.tx_id = 0x302, // 超级电容默认接收id
|
||||
.rx_id = 0x301, // 超级电容默认发送id,注意tx和rx在其他人看来是反的
|
||||
}};
|
||||
.can_config = {
|
||||
.can_handle = &hcan1,
|
||||
.tx_id = 0x210,
|
||||
.rx_id = 0x211,
|
||||
}};
|
||||
cap = SuperCapInit(&cap_conf); // 超级电容初始化
|
||||
SuperCapSetPower(cap,70); // 超级电容限制功率
|
||||
|
||||
PowerMeter_Init_Config_s power_conf = {
|
||||
.can_config = {
|
||||
.can_handle = &hcan1,
|
||||
|
|
@ -161,8 +170,8 @@ void ChassisInit()
|
|||
Balance_Init_config_s balanceInitConfig = {
|
||||
.legInitConfig = {
|
||||
.length_PID_conf = {
|
||||
.Kp = 500,//180 ,//50,
|
||||
.Ki = 100,//5,
|
||||
.Kp = 300,//500,//180 ,//50,
|
||||
.Ki = 0,//5,
|
||||
.Kd = 20,//10,//6,//6,
|
||||
.MaxOut = 100,
|
||||
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement | PID_DerivativeFilter,
|
||||
|
|
@ -199,10 +208,11 @@ void ChassisInit()
|
|||
Chassis_IMU_data = INS_Init(); // 底盘IMU初始化
|
||||
//转向环
|
||||
PID_Init_Config_s turn_pid_cfg = {
|
||||
.Kp = 10.0f,//10.0f,//1,
|
||||
.Kp = 12.0f,//10.0f,//1,
|
||||
.Ki = 0,
|
||||
.Kd = 0.0f,
|
||||
.MaxOut = 2,
|
||||
.Kd = 0.8f,
|
||||
.MaxOut = 2.0f,
|
||||
|
||||
.Improve = PID_Derivative_On_Measurement|PID_DerivativeFilter,
|
||||
.Derivative_LPF_RC = 0.001f
|
||||
};
|
||||
|
|
@ -210,7 +220,7 @@ void ChassisInit()
|
|||
|
||||
//Roll轴平衡
|
||||
PID_Init_Config_s roll_pid_cfg = {
|
||||
.Kp = 8.0f,
|
||||
.Kp = 12.0f,
|
||||
.Ki = 0,
|
||||
.Kd = 0.5f,
|
||||
.MaxOut = 100,
|
||||
|
|
@ -240,6 +250,17 @@ void ChassisInit()
|
|||
chassis_sub = SubRegister("chassis_cmd", sizeof(Chassis_Ctrl_Cmd_s));
|
||||
chassis_pub = PubRegister("chassis_feed", sizeof(Chassis_Upload_Data_s));
|
||||
#endif // ONE_BOARD
|
||||
const float a = 0.03f;
|
||||
first_order_filter_init(&wheel_r_filter,1e-3f,&a);
|
||||
first_order_filter_init(&wheel_l_filter,1e-3f,&a);
|
||||
|
||||
const float ht_a = 0.05f;
|
||||
first_order_filter_init(&ht_rf_filter,1e-3f,&ht_a);
|
||||
first_order_filter_init(&ht_rb_filter,1e-3f,&ht_a);
|
||||
first_order_filter_init(&ht_lf_filter,1e-3f,&ht_a);
|
||||
first_order_filter_init(&ht_lb_filter,1e-3f,&ht_a);
|
||||
const float vx_a = 0.6f;
|
||||
first_order_filter_init(&vx_filter,1e-3f,&vx_a);
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -247,14 +268,16 @@ void ChassisInit()
|
|||
* @brief 获取数据并计算状态量
|
||||
*
|
||||
*/
|
||||
|
||||
#define HIP_OFFSET 0.49899f//0.23719f //0.119031f
|
||||
static void Chassis_State_update()
|
||||
{
|
||||
float leg_phi[4]={0};
|
||||
float leg_phi_dot[4] = {0};
|
||||
leg_phi[0] = PI-(motor_rf->measure.total_angle - 0.119031f);
|
||||
leg_phi[1] = -(motor_rb->measure.total_angle + 0.119031f);
|
||||
leg_phi[2] = PI-(-motor_lf->measure.total_angle - 0.119031f);
|
||||
leg_phi[3] = -(-motor_lb->measure.total_angle + 0.119031f);
|
||||
leg_phi[0] = PI-(motor_rf->measure.total_angle - HIP_OFFSET);
|
||||
leg_phi[1] = -(motor_rb->measure.total_angle + HIP_OFFSET);
|
||||
leg_phi[2] = PI-(-motor_lf->measure.total_angle - HIP_OFFSET);
|
||||
leg_phi[3] = -(-motor_lb->measure.total_angle + HIP_OFFSET);
|
||||
|
||||
//最高点上电
|
||||
// leg_phi[0] = PI-(motor_rf->measure.total_angle + 0.81007f);
|
||||
|
|
@ -268,7 +291,7 @@ static void Chassis_State_update()
|
|||
leg_phi_dot[3] = motor_lb->measure.speed_rads;
|
||||
|
||||
|
||||
float body_phi = Chassis_IMU_data->Pitch * DEGREE_2_RAD;
|
||||
float body_phi = (Chassis_IMU_data->Pitch) * DEGREE_2_RAD; //+ 2
|
||||
float body_phi_dot = Chassis_IMU_data->Gyro[X];
|
||||
|
||||
//陀螺仪积分获取位移和速度
|
||||
|
|
@ -290,6 +313,7 @@ static void Chassis_State_update()
|
|||
//倒地需要清零总位移数据
|
||||
if(chassis_cmd_recv.chassis_mode == CHASSIS_ZERO_FORCE)
|
||||
{
|
||||
ChassisState = FAIL;
|
||||
left_offset = wheel_motor_l->measure.total_angle;
|
||||
right_offset = wheel_motor_r->measure.total_angle;
|
||||
}
|
||||
|
|
@ -305,14 +329,26 @@ static void Chassis_State_update()
|
|||
Leg_feedback_update(&BalanceControl.leg_right,- motor_rf->measure.real_current / HT_CMD_COEF,- motor_rb->measure.real_current / HT_CMD_COEF);
|
||||
Leg_feedback_update(&BalanceControl.leg_left,motor_lf->measure.real_current / HT_CMD_COEF,motor_lb->measure.real_current / HT_CMD_COEF);
|
||||
|
||||
Balance_feedback_update(&BalanceControl,leg_phi,leg_phi_dot,body_phi,body_phi_dot,x,x_dot,Chassis_IMU_data->MotionAccel_n);
|
||||
float separate_x[2] = { - x_r * DEGREE_2_RAD * RADIUS_WHEEL , x_l * DEGREE_2_RAD * RADIUS_WHEEL};
|
||||
float separate_x_dot[2] = { - wheel_motor_r->measure.speed_rads * RADIUS_WHEEL , wheel_motor_l->measure.speed_rads * RADIUS_WHEEL};
|
||||
// float separate_x[2] = { x,x};
|
||||
// float separate_x_dot[2] = { x_dot,x_dot};
|
||||
|
||||
|
||||
//Balance_feedback_update(&BalanceControl,leg_phi,leg_phi_dot,body_phi,body_phi_dot,x,x_dot,Chassis_IMU_data->MotionAccel_n);
|
||||
Balance_feedback_update(&BalanceControl,leg_phi,leg_phi_dot,body_phi,body_phi_dot,separate_x,separate_x_dot,Chassis_IMU_data->MotionAccel_n);
|
||||
if( ChassisState == FAIL )
|
||||
{
|
||||
if(abs(body_phi) <= 0.05 && abs(body_phi) <= 0.005 )
|
||||
ChassisState = STAND;
|
||||
}
|
||||
if(chassis_cmd_recv.chassis_mode == CHASSIS_ZERO_FORCE && last_chassis_mode != CHASSIS_ZERO_FORCE)
|
||||
{
|
||||
//倒地重新初始化卡尔曼滤波器一次
|
||||
//estimator_init(&BalanceControl.v_estimator,0.001f,10.0f);
|
||||
}
|
||||
|
||||
last_chassis_mode = chassis_cmd_recv.chassis_mode;
|
||||
}
|
||||
|
||||
/* 机器人底盘控制核心任务 */
|
||||
|
|
@ -327,6 +363,15 @@ void ChassisTask()
|
|||
chassis_cmd_recv = *(Chassis_Ctrl_Cmd_s *)CANCommGet(chasiss_can_comm);
|
||||
#endif // CHASSIS_BOARD
|
||||
|
||||
//死亡情况下 关闭电机
|
||||
if(referee_data->GameRobotState.power_management_chassis_output == 0)
|
||||
chassis_cmd_recv.chassis_mode = CHASSIS_ZERO_FORCE;
|
||||
|
||||
//设置超电上限功率
|
||||
float power_limit = referee_data->GameRobotState.chassis_power_limit;
|
||||
power_limit = float_constrain(power_limit,40,120);
|
||||
SuperCapSetPower(cap,power_limit);
|
||||
|
||||
if (chassis_cmd_recv.chassis_mode == CHASSIS_ZERO_FORCE)
|
||||
{ // 如果出现重要模块离线或遥控器设置为急停,让电机停止
|
||||
HTMotorStop(motor_lf);
|
||||
|
|
@ -343,6 +388,7 @@ void ChassisTask()
|
|||
PIDClear(&BalanceControl.leg_coordination_PID);
|
||||
//清除状态量
|
||||
state_clear(&BalanceControl);
|
||||
//estimator_init(&BalanceControl.v_estimator,0.001f,10.0f);
|
||||
}
|
||||
else
|
||||
{ // 正常工作
|
||||
|
|
@ -356,6 +402,8 @@ void ChassisTask()
|
|||
|
||||
|
||||
static int8_t init_flag = FALSE;
|
||||
static uint8_t jump_flag = 0,last_jump_flag = 0;
|
||||
static float jump_time = 0,shrink_time = 0;
|
||||
// 根据控制模式设定旋转速度
|
||||
switch (chassis_cmd_recv.chassis_mode)
|
||||
{
|
||||
|
|
@ -364,11 +412,36 @@ void ChassisTask()
|
|||
break;
|
||||
case CHASSIS_FOLLOW_GIMBAL_YAW: // 跟随云台,不单独设置pid,以误差角度平方为速度输出
|
||||
//chassis_cmd_recv.wz = 1.0f * chassis_cmd_recv.offset_angle * abs(chassis_cmd_recv.offset_angle);
|
||||
chassis_cmd_recv.wz = 0.01f * chassis_cmd_recv.offset_angle* abs(chassis_cmd_recv.offset_angle);
|
||||
chassis_cmd_recv.wz = 0.08f * chassis_cmd_recv.offset_angle;
|
||||
jump_flag = 0;
|
||||
break;
|
||||
case CHASSIS_LATERAL: //侧向对敌
|
||||
chassis_cmd_recv.wz = 0.08f * loop_float_constrain(chassis_cmd_recv.offset_angle + 90,-180,180);
|
||||
break;
|
||||
case CHASSIS_ROTATE: // 自旋,同时保持全向机动;当前wz维持定值,后续增加不规则的变速策略
|
||||
{ //chassis_cmd_recv.wz = 1.0f;
|
||||
if(last_jump_flag == 0)
|
||||
{
|
||||
jump_flag = 1;
|
||||
jump_time = DWT_GetTimeline_ms();
|
||||
}
|
||||
else if(last_jump_flag == 1)
|
||||
{
|
||||
if (jump_time + 150 <= DWT_GetTimeline_ms()) {
|
||||
jump_flag = 2;
|
||||
shrink_time = DWT_GetTimeline_ms();
|
||||
}
|
||||
}
|
||||
else if(last_jump_flag == 2)
|
||||
{
|
||||
if (shrink_time + 200 <= DWT_GetTimeline_ms()) {
|
||||
jump_flag = 3;
|
||||
}
|
||||
}
|
||||
last_jump_flag = jump_flag;
|
||||
}
|
||||
//chassis_cmd_recv.wz = 4000;
|
||||
chassis_cmd_recv.wz = 2.0f;
|
||||
//chassis_cmd_recv.wz = 2.0f;
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
|
|
@ -379,9 +452,12 @@ void ChassisTask()
|
|||
static float sin_theta, cos_theta;
|
||||
// cos_theta = arm_cos_f32(chassis_cmd_recv.offset_angle * DEGREE_2_RAD);
|
||||
// sin_theta = arm_sin_f32(chassis_cmd_recv.offset_angle * DEGREE_2_RAD);
|
||||
sin_theta = 0; cos_theta = 1;
|
||||
chassis_vx = chassis_cmd_recv.vx * cos_theta - chassis_cmd_recv.vy * sin_theta;
|
||||
chassis_vy = chassis_cmd_recv.vx * sin_theta + chassis_cmd_recv.vy * cos_theta;
|
||||
// sin_theta = 0; cos_theta = 1;
|
||||
// chassis_vx = chassis_cmd_recv.vx * cos_theta - chassis_cmd_recv.vy * sin_theta;
|
||||
// chassis_vy = chassis_cmd_recv.vx * sin_theta + chassis_cmd_recv.vy * cos_theta;
|
||||
first_order_filter_cali(&vx_filter,chassis_cmd_recv.vx/5280 * 3);
|
||||
chassis_vx = vx_filter.out;
|
||||
|
||||
|
||||
// // 获取裁判系统数据 建议将裁判系统与底盘分离，所以此处数据应使用消息中心发送
|
||||
// // 我方颜色id小于7是红色,大于7是蓝色,注意这里发送的是对方的颜色, 0:blue , 1:red
|
||||
|
|
@ -392,74 +468,64 @@ void ChassisTask()
|
|||
|
||||
//获取数据并计算状态量
|
||||
Chassis_State_update();
|
||||
|
||||
float vofa_send_data[12];
|
||||
|
||||
// for(int i = 0; i<6;i++)
|
||||
// vofa_send_data[i] = BalanceControl.state[i];
|
||||
// for(int i = 0; i<6;i++)
|
||||
// vofa_send_data[i+6] = BalanceControl.state_observer.Estimate_X[i];
|
||||
vofa_justfloat_output(vofa_send_data,12*4,&huart1);
|
||||
|
||||
static float target_x = 0;
|
||||
static float target_yaw = 0;
|
||||
if(chassis_cmd_recv.chassis_mode != CHASSIS_ZERO_FORCE) // 右侧开关状态[下],底盘跟随云台
|
||||
{
|
||||
float Roll_pid_out = PIDCalculate(&Roll_Loop_PID,-Chassis_IMU_data->Roll,0);
|
||||
//将roll轴自平衡PID输出直接叠加到前馈支持力上
|
||||
BalanceControl.leg_right.F_feedforward = Roll_pid_out;
|
||||
BalanceControl.leg_left.F_feedforward = -Roll_pid_out;
|
||||
|
||||
//target_x += chassis_cmd_recv.vx/5280 * 0.003f;
|
||||
target_x_set(&BalanceControl,chassis_cmd_recv.vx/5280 * 0.003f);
|
||||
target_x_set(&BalanceControl,chassis_vx);
|
||||
target_L_set(&BalanceControl,chassis_cmd_recv.vy/5280 * 0.0003f);
|
||||
Balance_Control_Loop(&BalanceControl);
|
||||
|
||||
float turn_T = 0;
|
||||
//离地情况下把转向环和roll轴补偿关掉
|
||||
if(BalanceControl.fly_flag_r == FALSE || BalanceControl.fly_flag_l == FALSE)
|
||||
{
|
||||
float Roll_pid_out = PIDCalculate(&Roll_Loop_PID,-Chassis_IMU_data->Roll,0);
|
||||
//将roll轴自平衡PID输出直接叠加到前馈支持力上
|
||||
BalanceControl.leg_right.F_feedforward = Roll_pid_out;
|
||||
BalanceControl.leg_left.F_feedforward = -Roll_pid_out;
|
||||
turn_T = PIDCalculate(&Turn_Loop_PID,Chassis_IMU_data->Gyro[Z],chassis_cmd_recv.wz);
|
||||
}
|
||||
else
|
||||
{
|
||||
turn_T = 0;
|
||||
BalanceControl.leg_right.F_feedforward = 0;
|
||||
BalanceControl.leg_left.F_feedforward = 0;
|
||||
}
|
||||
|
||||
Balance_Control_Loop(&BalanceControl,jump_flag);
|
||||
|
||||
static float wz_feedback;
|
||||
wz_feedback = (1.0f - 0.75f) * wz_feedback + 0.75f * Chassis_IMU_data->Gyro[Z];
|
||||
float turn_T = PIDCalculate(&Turn_Loop_PID,Chassis_IMU_data->Gyro[Z],chassis_cmd_recv.wz);
|
||||
wz_feedback = Chassis_IMU_data->Gyro[Z];
|
||||
|
||||
static float T,wheel_current_r,wheel_current_l;
|
||||
static float wheel_current_r,wheel_current_l,Tr,Tl;
|
||||
|
||||
T = BalanceControl.balance_LQR.control_vector[0];//float_constrain(BalanceControl.balance_LQR.control_vector[0],-4,4);
|
||||
//轮电机滤波并输出
|
||||
Tr = BalanceControl.LQR_r.control_vector[0];
|
||||
Tl = BalanceControl.LQR_l.control_vector[0];
|
||||
|
||||
//T = (1.0f - 0.75f) * T + 0.75f * BalanceControl.balance_LQR.control_vector[0];//float_constrain(BalanceControl.balance_LQR.control_vector[0],-4,4);
|
||||
|
||||
// wheel_current_r = (1.0f - 0.75f) * wheel_current_r + 0.75f * (T+turn_T) * LK_TORQUE_2_CMD;//+turn_T
|
||||
// wheel_current_l = (1.0f - 0.75f) * wheel_current_l + 0.75f * (T-turn_T) * LK_TORQUE_2_CMD;//-turn_T
|
||||
|
||||
wheel_current_r = (T+turn_T) * LK_TORQUE_2_CMD;
|
||||
wheel_current_l = (T-turn_T) * LK_TORQUE_2_CMD;
|
||||
first_order_filter_cali(&wheel_r_filter,Tr+turn_T);
|
||||
first_order_filter_cali(&wheel_l_filter,Tl-turn_T);
|
||||
wheel_current_r = (wheel_r_filter.out)*LK_TORQUE_2_CMD;//(T+turn_T) * LK_TORQUE_2_CMD;
|
||||
wheel_current_l = (wheel_l_filter.out)*LK_TORQUE_2_CMD;//(T-turn_T) * LK_TORQUE_2_CMD;
|
||||
|
||||
float_constrain(wheel_current_r,-4000,4000);
|
||||
float_constrain(wheel_current_l,-4000,4000);
|
||||
//关节电机滤波并输出
|
||||
first_order_filter_cali(&ht_rf_filter,-BalanceControl.leg_right.legOutput.T1 * HT_CMD_COEF);
|
||||
first_order_filter_cali(&ht_rb_filter,-BalanceControl.leg_right.legOutput.T2 * HT_CMD_COEF);
|
||||
|
||||
// if (ChassisState == FAIL) // 倒地 未起身
|
||||
// {
|
||||
first_order_filter_cali(&ht_lf_filter,BalanceControl.leg_left.legOutput.T1 * HT_CMD_COEF);
|
||||
first_order_filter_cali(&ht_lb_filter,BalanceControl.leg_left.legOutput.T2 * HT_CMD_COEF);
|
||||
|
||||
HTMotorSetRef(motor_rf,ht_rf_filter.out);
|
||||
HTMotorSetRef(motor_rb,ht_rb_filter.out);
|
||||
|
||||
HTMotorSetRef(motor_lf,ht_lf_filter.out);
|
||||
HTMotorSetRef(motor_lb,ht_lb_filter.out);
|
||||
// HTMotorSetRef(motor_rf,0);
|
||||
// HTMotorSetRef(motor_rb,0);
|
||||
//
|
||||
// HTMotorSetRef(motor_lf,0);
|
||||
// HTMotorSetRef(motor_lb,0);
|
||||
//
|
||||
// PIDClear(&BalanceControl.leg_length_PID);
|
||||
// PIDClear(&Turn_Loop_PID);
|
||||
// PIDClear(&Roll_Loop_PID);
|
||||
// PIDClear(&BalanceControl.leg_coordination_PID);
|
||||
// }
|
||||
// else
|
||||
{
|
||||
HTMotorSetRef(motor_rf,-BalanceControl.leg_right.legOutput.T1 * HT_CMD_COEF);
|
||||
HTMotorSetRef(motor_rb,-BalanceControl.leg_right.legOutput.T2 * HT_CMD_COEF);
|
||||
|
||||
HTMotorSetRef(motor_lf,BalanceControl.leg_left.legOutput.T1 * HT_CMD_COEF);
|
||||
HTMotorSetRef(motor_lb,BalanceControl.leg_left.legOutput.T2 * HT_CMD_COEF);
|
||||
// HTMotorSetRef(motor_rf,0);
|
||||
// HTMotorSetRef(motor_rb,0);
|
||||
//
|
||||
// HTMotorSetRef(motor_lf,0);
|
||||
// HTMotorSetRef(motor_lb,0);
|
||||
}
|
||||
|
||||
LKMotorSetRef(wheel_motor_r,wheel_current_r);
|
||||
LKMotorSetRef(wheel_motor_l,wheel_current_l);
|
||||
|
|
@ -477,11 +543,31 @@ void ChassisTask()
|
|||
PIDClear(&BalanceControl.leg_coordination_PID);
|
||||
}
|
||||
|
||||
static uint8_t last_UI_refresh = 0 ;
|
||||
if(chassis_cmd_recv.UI_refresh != last_UI_refresh)
|
||||
MyUIInit();
|
||||
last_UI_refresh = chassis_cmd_recv.UI_refresh;
|
||||
//推送ui显示界面
|
||||
ui_data.relative_angle = chassis_cmd_recv.offset_angle;
|
||||
|
||||
for (int i = 0; i < 6; ++i) {
|
||||
ui_data.leg_pos_r[i] = BalanceControl.leg_right.legState.position[i];
|
||||
ui_data.leg_pos_l[i] = BalanceControl.leg_left.legState.position[i];
|
||||
}
|
||||
|
||||
ui_data.chassis_mode = chassis_cmd_recv.chassis_mode;
|
||||
ui_data.shoot_mode = chassis_cmd_recv.shoot_mode;
|
||||
ui_data.friction_mode = chassis_cmd_recv.friction_mode;
|
||||
ui_data.Chassis_Power_Data.chassis_power_mx = referee_data->GameRobotState.chassis_power_limit;
|
||||
ui_data.Chassis_Power_Data.cap_vol = (float)cap->cap_msg.cap_vol/100.0f;
|
||||
ui_data.Chassis_Power_Data.input_vol = (float)cap->cap_msg.input_vol/100.0f;
|
||||
|
||||
|
||||
|
||||
// 推送反馈消息
|
||||
chassis_feedback_data.enemy_color = !referee_data->referee_id.Robot_Color;
|
||||
chassis_feedback_data.real_wz = Chassis_IMU_data->Gyro[Z];
|
||||
chassis_feedback_data.power_management_chassis_output = referee_data->GameRobotState.power_management_chassis_output;
|
||||
#ifdef ONE_BOARD
|
||||
PubPushMessage(chassis_pub, (void *)&chassis_feedback_data);
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -1,85 +0,0 @@
|
|||
//
|
||||
// Created by SJQ on 2023/12/25.
|
||||
//
|
||||
|
||||
#ifndef WHEEL_LEGGED_LEG_OLD_H
|
||||
#define WHEEL_LEGGED_LEG_OLD_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;
|
||||
|
||||
}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_OLD_H
|
||||
|
|
@ -86,6 +86,7 @@ typedef enum
|
|||
CHASSIS_ROTATE, // 小陀螺模式
|
||||
CHASSIS_NO_FOLLOW, // 不跟随，允许全向平移
|
||||
CHASSIS_FOLLOW_GIMBAL_YAW, // 跟随模式，底盘叠加角度环控制
|
||||
CHASSIS_LATERAL, // 侧向对敌
|
||||
} chassis_mode_e;
|
||||
|
||||
// 云台模式设置
|
||||
|
|
@ -127,6 +128,8 @@ typedef enum
|
|||
typedef struct
|
||||
{ // 功率控制
|
||||
float chassis_power_mx;
|
||||
float cap_vol;
|
||||
float input_vol;
|
||||
} Chassis_Power_Data_s;
|
||||
|
||||
/* ----------------CMD应用发布的控制数据,应当由gimbal/chassis/shoot订阅---------------- */
|
||||
|
|
@ -142,10 +145,15 @@ typedef struct
|
|||
float vy; // 横移方向速度
|
||||
float wz; // 旋转速度
|
||||
float offset_angle; // 底盘和归中位置的夹角
|
||||
float leg_length; // 腿长
|
||||
chassis_mode_e chassis_mode;
|
||||
int chassis_speed_buff;
|
||||
// UI部分
|
||||
// ...
|
||||
shoot_mode_e shoot_mode; // 发射模式设置
|
||||
friction_mode_e friction_mode; // 摩擦轮关闭
|
||||
|
||||
uint8_t UI_refresh;
|
||||
|
||||
} Chassis_Ctrl_Cmd_s;
|
||||
|
||||
|
|
@ -185,12 +193,11 @@ typedef struct
|
|||
// 后续增加底盘的真实速度
|
||||
// float real_vx;
|
||||
// float real_vy;
|
||||
// float real_wz;
|
||||
// 底盘旋转速度
|
||||
float real_wz;
|
||||
|
||||
uint8_t rest_heat; // 剩余枪口热量
|
||||
Bullet_Speed_e bullet_speed; // 弹速限制
|
||||
Enemy_Color_e enemy_color; // 0 for blue, 1 for red
|
||||
|
||||
uint8_t power_management_chassis_output;
|
||||
} Chassis_Upload_Data_s;
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -253,3 +253,34 @@ void ramp_calc(ramp_function_source_t *ramp_source_type, float input)
|
|||
ramp_source_type->out = ramp_source_type->min_value;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 一阶低通滤波初始化
|
||||
* @author RM
|
||||
* @param[in] 一阶低通滤波结构体
|
||||
* @param[in] 间隔的时间，单位 s
|
||||
* @param[in] 滤波参数
|
||||
* @retval 返回空
|
||||
*/
|
||||
void first_order_filter_init(first_order_filter_type_t *first_order_filter_type, float frame_period, const float num[1])
|
||||
{
|
||||
first_order_filter_type->frame_period = frame_period;
|
||||
first_order_filter_type->num[0] = num[0];
|
||||
first_order_filter_type->input = 0.0f;
|
||||
first_order_filter_type->out = 0.0f;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief 一阶低通滤波计算
|
||||
* @author RM
|
||||
* @param[in] 一阶低通滤波结构体
|
||||
* @param[in] 间隔的时间，单位 s
|
||||
* @retval 返回空
|
||||
*/
|
||||
void first_order_filter_cali(first_order_filter_type_t *first_order_filter_type, float input)
|
||||
{
|
||||
first_order_filter_type->input = input;
|
||||
first_order_filter_type->out =
|
||||
first_order_filter_type->num[0] / (first_order_filter_type->num[0] + first_order_filter_type->frame_period) * first_order_filter_type->out + first_order_filter_type->frame_period / (first_order_filter_type->num[0] + first_order_filter_type->frame_period) * first_order_filter_type->input;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -58,6 +58,20 @@ typedef struct
|
|||
float frame_period; //时间间隔
|
||||
} ramp_function_source_t;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
float input; //输入数据
|
||||
float out; //滤波输出的数据
|
||||
float num[1]; //滤波参数
|
||||
float frame_period; //滤波的时间间隔 单位 s
|
||||
} first_order_filter_type_t;
|
||||
|
||||
//一阶低通滤波初始化
|
||||
void first_order_filter_init(first_order_filter_type_t *first_order_filter_type, float frame_period, const float num[1]);
|
||||
//一阶低通滤波计算
|
||||
void first_order_filter_cali(first_order_filter_type_t *first_order_filter_type, float input);
|
||||
|
||||
|
||||
/* circumference ratio */
|
||||
#ifndef PI
|
||||
#define PI 3.14159265354f
|
||||
|
|
|
|||
|
|
@ -71,7 +71,7 @@ static void HTMotorLostCallback(void *motor_ptr)
|
|||
{
|
||||
HTMotorInstance *motor = (HTMotorInstance *)motor_ptr;
|
||||
LOGWARNING("[ht_motor] motor %d lost\n", motor->motor_can_instace->tx_id);
|
||||
if (++motor->lost_cnt % 10 != 0)
|
||||
if (++motor->lost_cnt % 100 != 0)
|
||||
HTMotorSetMode(CMD_MOTOR_MODE, motor); // 尝试重新让电机进入控制模式
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -198,7 +198,7 @@ void UIOvalDraw(Graph_Data_t *graph, char graphname[3], uint32_t Graph_Operate,
|
|||
|
||||
void UIArcDraw(Graph_Data_t *graph, char graphname[3], uint32_t Graph_Operate, uint32_t Graph_Layer, uint32_t Graph_Color,
|
||||
uint32_t Graph_StartAngle, uint32_t Graph_EndAngle, uint32_t Graph_Width, uint32_t Start_x, uint32_t Start_y,
|
||||
uint32_t end_x, uint32_t end_y)
|
||||
uint32_t x_Length, uint32_t y_Length)
|
||||
{
|
||||
int i;
|
||||
for (i = 0; i < 3 && graphname[i] != '\0'; i++)
|
||||
|
|
@ -217,8 +217,8 @@ void UIArcDraw(Graph_Data_t *graph, char graphname[3], uint32_t Graph_Operate, u
|
|||
graph->start_x = Start_x;
|
||||
graph->start_y = Start_y;
|
||||
graph->radius = 0;
|
||||
graph->end_x = end_x;
|
||||
graph->end_y = end_y;
|
||||
graph->end_x = x_Length;
|
||||
graph->end_y = y_Length;
|
||||
}
|
||||
|
||||
/************************************************绘制浮点型数据*************************************************
|
||||
|
|
|
|||
|
|
@ -54,7 +54,7 @@ void UIOvalDraw(Graph_Data_t *graph, char graphname[3], uint32_t Graph_Operate,
|
|||
|
||||
void UIArcDraw(Graph_Data_t *graph, char graphname[3], uint32_t Graph_Operate, uint32_t Graph_Layer, uint32_t Graph_Color,
|
||||
uint32_t Graph_StartAngle, uint32_t Graph_EndAngle, uint32_t Graph_Width, uint32_t Start_x, uint32_t Start_y,
|
||||
uint32_t end_x, uint32_t end_y);
|
||||
uint32_t x_Length, uint32_t y_Length);
|
||||
|
||||
void UIFloatDraw(Graph_Data_t *graph, char graphname[3], uint32_t Graph_Operate, uint32_t Graph_Layer, uint32_t Graph_Color,
|
||||
uint32_t Graph_Size, uint32_t Graph_Digit, uint32_t Graph_Width, uint32_t Start_x, uint32_t Start_y, int32_t Graph_Float);
|
||||
|
|
|
|||
|
|
@ -83,6 +83,8 @@ typedef enum
|
|||
ID_robot_hurt = 0x0206, // 伤害状态数据
|
||||
ID_shoot_data = 0x0207, // 实时射击数据
|
||||
ID_student_interactive = 0x0301, // 机器人间交互数据
|
||||
ID_custom_robot = 0x302, // 自定义控制器数据（图传链路）
|
||||
ID_remote_control = 0x304, // 键鼠遥控数据（图传链路）
|
||||
} CmdID_e;
|
||||
|
||||
/* 命令码数据段长,根据官方协议来定义长度，还有自定义数据长度 */
|
||||
|
|
@ -102,6 +104,8 @@ typedef enum
|
|||
LEN_shoot_data = 7, // 0x0207
|
||||
LEN_receive_data = 6 + Communicate_Data_LEN, // 0x0301
|
||||
|
||||
LEN_remote_control = 12, // 0x0304
|
||||
|
||||
} JudgeDataLength_e;
|
||||
|
||||
/****************************接收数据的详细说明****************************/
|
||||
|
|
@ -157,37 +161,31 @@ typedef struct
|
|||
uint8_t supply_projectile_num;
|
||||
} ext_supply_projectile_action_t;
|
||||
|
||||
/* ID: 0X0201 Byte: 27 机器人状态数据 */
|
||||
/* ID: 0X0201 Byte: 13 机器人性能体系数据 */
|
||||
typedef struct
|
||||
{
|
||||
uint8_t robot_id;
|
||||
uint8_t robot_level;
|
||||
uint16_t remain_HP;
|
||||
uint16_t max_HP;
|
||||
uint16_t shooter_id1_17mm_cooling_rate;
|
||||
uint16_t shooter_id1_17mm_cooling_limit;
|
||||
uint16_t shooter_id1_17mm_speed_limit;
|
||||
uint16_t shooter_id2_17mm_cooling_rate;
|
||||
uint16_t shooter_id2_17mm_cooling_limit;
|
||||
uint16_t shooter_id2_17mm_speed_limit;
|
||||
uint16_t shooter_id1_42mm_cooling_rate;
|
||||
uint16_t shooter_id1_42mm_cooling_limit;
|
||||
uint16_t shooter_id1_42mm_speed_limit;
|
||||
uint16_t chassis_power_limit;
|
||||
uint8_t mains_power_gimbal_output : 1;
|
||||
uint8_t mains_power_chassis_output : 1;
|
||||
uint8_t mains_power_shooter_output : 1;
|
||||
uint8_t robot_id;
|
||||
uint8_t robot_level;
|
||||
uint16_t current_HP;
|
||||
uint16_t maximum_HP;
|
||||
uint16_t shooter_barrel_cooling_value;
|
||||
uint16_t shooter_barrel_heat_limit;
|
||||
uint16_t chassis_power_limit;
|
||||
uint8_t power_management_gimbal_output : 1;
|
||||
uint8_t power_management_chassis_output : 1;
|
||||
uint8_t power_management_shooter_output : 1;
|
||||
} ext_game_robot_state_t;
|
||||
|
||||
/* ID: 0X0202 Byte: 14 实时功率热量数据 */
|
||||
/* ID: 0X0202 Byte: 16 实时功率热量数据 */
|
||||
typedef struct
|
||||
{
|
||||
uint16_t chassis_volt;
|
||||
uint16_t chassis_current;
|
||||
float chassis_power; // 瞬时功率
|
||||
uint16_t chassis_power_buffer; // 60焦耳缓冲能量
|
||||
uint16_t shooter_heat0; // 17mm
|
||||
uint16_t shooter_heat1;
|
||||
uint16_t chassis_voltage;
|
||||
uint16_t chassis_current;
|
||||
float chassis_power;
|
||||
uint16_t buffer_energy;
|
||||
uint16_t shooter_17mm_1_barrel_heat;
|
||||
uint16_t shooter_17mm_2_barrel_heat;
|
||||
uint16_t shooter_42mm_barrel_heat;
|
||||
} ext_power_heat_data_t;
|
||||
|
||||
/* ID: 0x0203 Byte: 16 机器人位置数据 */
|
||||
|
|
@ -227,6 +225,19 @@ typedef struct
|
|||
float bullet_speed;
|
||||
} ext_shoot_data_t;
|
||||
|
||||
/****************************图传链路数据****************************/
|
||||
/* ID: 0x0304 Byte: 7 图传链路键鼠遥控数据 */
|
||||
typedef struct
|
||||
{
|
||||
int16_t mouse_x;
|
||||
int16_t mouse_y;
|
||||
int16_t mouse_z;
|
||||
int8_t left_button_down;
|
||||
int8_t right_button_down;
|
||||
uint16_t keyboard_value;
|
||||
uint16_t reserved;
|
||||
}remote_control_t;
|
||||
/****************************图传链路数据****************************/
|
||||
/****************************机器人交互数据****************************/
|
||||
/****************************机器人交互数据****************************/
|
||||
/* 发送的内容数据段最大为 113 检测是否超出大小限制?实际上图形段不会超，数据段最多30个，也不会超*/
|
||||
|
|
|
|||
|
|
@ -14,6 +14,7 @@
|
|||
#include "referee_UI.h"
|
||||
#include "string.h"
|
||||
#include "cmsis_os.h"
|
||||
#include "user_lib.h"
|
||||
|
||||
static Referee_Interactive_info_t *Interactive_data; // UI绘制需要的机器人状态数据
|
||||
static referee_info_t *referee_recv_info; // 接收到的裁判系统数据
|
||||
|
|
@ -39,6 +40,14 @@ static void MyUIRefresh(referee_info_t *referee_recv_info, Referee_Interactive_i
|
|||
static void UIChangeCheck(Referee_Interactive_info_t *_Interactive_data); // 模式切换检测
|
||||
static void RobotModeTest(Referee_Interactive_info_t *_Interactive_data); // 测试用函数，实现模式自动变化
|
||||
|
||||
static void DrawArmorPose(float relative_angle);
|
||||
void DrawLegPose(Referee_Interactive_info_t *_Interactive_data,int x_offset,int y_offset,int scale);
|
||||
|
||||
//腿部姿态绘制位置、大小
|
||||
#define X_OFFSET 1560
|
||||
#define Y_OFFSET 440
|
||||
#define SCALE 400
|
||||
|
||||
referee_info_t *UITaskInit(UART_HandleTypeDef *referee_usart_handle, Referee_Interactive_info_t *UI_data)
|
||||
{
|
||||
referee_recv_info = RefereeInit(referee_usart_handle); // 初始化裁判系统的串口,并返回裁判系统反馈数据指针
|
||||
|
|
@ -49,20 +58,28 @@ referee_info_t *UITaskInit(UART_HandleTypeDef *referee_usart_handle, Referee_Int
|
|||
|
||||
void UITask()
|
||||
{
|
||||
RobotModeTest(Interactive_data); // 测试用函数，实现模式自动变化,用于检查该任务和裁判系统是否连接正常
|
||||
//RobotModeTest(Interactive_data); // 测试用函数，实现模式自动变化,用于检查该任务和裁判系统是否连接正常
|
||||
MyUIRefresh(referee_recv_info, Interactive_data);
|
||||
DrawArmorPose(Interactive_data->relative_angle);
|
||||
DrawLegPose(Interactive_data,X_OFFSET,Y_OFFSET,SCALE);
|
||||
}
|
||||
|
||||
static Graph_Data_t UI_shoot_line[10]; // 射击准线
|
||||
static Graph_Data_t UI_Energy[3]; // 电容能量条
|
||||
static String_Data_t UI_State_sta[6]; // 机器人状态,静态只需画一次
|
||||
static Graph_Data_t UI_Energy[5]; // 电容能量条
|
||||
|
||||
static Graph_Data_t UI_ArmorPose[3]; // 前后装甲板位置
|
||||
static Graph_Data_t UI_LegPose_r[5]; // 腿部姿态
|
||||
static Graph_Data_t UI_LegPose_l[5]; // 腿部姿态
|
||||
|
||||
static String_Data_t UI_State_sta[7]; // 机器人状态,静态只需画一次
|
||||
static String_Data_t UI_State_dyn[6]; // 机器人状态,动态先add才能change
|
||||
static uint32_t shoot_line_location[10] = {540, 960, 490, 515, 565};
|
||||
|
||||
|
||||
void MyUIInit()
|
||||
{
|
||||
if (!referee_recv_info->init_flag)
|
||||
vTaskDelete(NULL); // 如果没有初始化裁判系统则直接删除ui任务
|
||||
// if (!referee_recv_info->init_flag)
|
||||
// vTaskDelete(NULL); // 如果没有初始化裁判系统则直接删除ui任务
|
||||
while (referee_recv_info->GameRobotState.robot_id == 0)
|
||||
osDelay(100); // 若还未收到裁判系统数据,等待一段时间后再检查
|
||||
|
||||
|
|
@ -103,8 +120,13 @@ void MyUIInit()
|
|||
UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[4]);
|
||||
|
||||
// 底盘功率显示，静态
|
||||
UICharDraw(&UI_State_sta[5], "ss5", UI_Graph_ADD, 7, UI_Color_Green, 18, 2, 620, 230, "Power:");
|
||||
UICharDraw(&UI_State_sta[5], "ss5", UI_Graph_ADD, 8, UI_Color_Green, 18, 2, 620, 230, "Power:");
|
||||
UICharRefresh(&referee_recv_info->referee_id, UI_State_sta[5]);
|
||||
|
||||
// 电容电压显示，动态
|
||||
UICharDraw(&UI_State_sta[6], "v_cap_str", UI_Graph_ADD, 8, UI_Color_Main, 18, 2, 1000, 230, "V cap:");
|
||||
UICharRefresh(&referee_recv_info->referee_id, UI_State_sta[6]);
|
||||
|
||||
// 能量条框
|
||||
UIRectangleDraw(&UI_Energy[0], "ss6", UI_Graph_ADD, 7, UI_Color_Green, 2, 720, 140, 1220, 180);
|
||||
UIGraphRefresh(&referee_recv_info->referee_id, 1, UI_Energy[0]);
|
||||
|
|
@ -114,6 +136,43 @@ void MyUIInit()
|
|||
// 能量条初始状态
|
||||
UILineDraw(&UI_Energy[2], "sd6", UI_Graph_ADD, 8, UI_Color_Pink, 30, 720, 160, 1020, 160);
|
||||
UIGraphRefresh(&referee_recv_info->referee_id, 2, UI_Energy[1], UI_Energy[2]);
|
||||
// 电容电压显示,动态
|
||||
UIFloatDraw(&UI_Energy[3], "v_cap", UI_Graph_ADD, 8, UI_Color_Main, 18, 2, 2, 1200, 230, 24000);
|
||||
UIGraphRefresh(&referee_recv_info->referee_id, 1, UI_Energy[3]);
|
||||
|
||||
// 装甲板姿态基准线 静态
|
||||
// UICircleDraw(&UI_ArmorPose[0],"armor",UI_Graph_ADD,8,UI_Color_Main,5,960,540,350);
|
||||
// UIGraphRefresh(&referee_recv_info->referee_id, 1, &UI_ArmorPose[0]);
|
||||
//装甲板相对位置 动态
|
||||
UIArcDraw(&UI_ArmorPose[1],"front",UI_Graph_ADD,7,UI_Color_Yellow,60,120,10,960,540,350,350);
|
||||
UIArcDraw(&UI_ArmorPose[2],"back",UI_Graph_ADD,7,UI_Color_Yellow,240,300,10,960,540,350,350);
|
||||
UIGraphRefresh(&referee_recv_info->referee_id,2,UI_ArmorPose[1],UI_ArmorPose[2]);
|
||||
|
||||
UILineDraw(&UI_LegPose_r[0], "AE", UI_Graph_ADD, 7, UI_Color_Main,
|
||||
3, 960-75, 540, 960+75, 540);
|
||||
UILineDraw(&UI_LegPose_r[1], "AB", UI_Graph_ADD, 7, UI_Color_Main,
|
||||
3, 960-75, 540, 960+75, 540);
|
||||
UILineDraw(&UI_LegPose_r[2], "BC", UI_Graph_ADD, 7, UI_Color_Main,
|
||||
3, 960-75, 540, 960+75, 540);
|
||||
UILineDraw(&UI_LegPose_r[3], "CD", UI_Graph_ADD, 7, UI_Color_Main,
|
||||
3, 960-75, 540, 960+75, 540);
|
||||
UILineDraw(&UI_LegPose_r[4], "ED", UI_Graph_ADD, 7, UI_Color_Main,
|
||||
3, 960-75, 540, 960+75, 540);
|
||||
UIGraphRefresh(&referee_recv_info->referee_id, 5, UI_LegPose_r[0], UI_LegPose_r[1], UI_LegPose_r[2], UI_LegPose_r[3], UI_LegPose_r[4]);
|
||||
|
||||
|
||||
UILineDraw(&UI_LegPose_l[0], "AE_l", UI_Graph_ADD, 7, UI_Color_Main,
|
||||
3, 960-75, 540, 960+75, 540);
|
||||
UILineDraw(&UI_LegPose_l[1], "AB_l", UI_Graph_ADD, 7, UI_Color_Main,
|
||||
3, 960-75, 540, 960+75, 540);
|
||||
UILineDraw(&UI_LegPose_l[2], "BC_l", UI_Graph_ADD, 7, UI_Color_Main,
|
||||
3, 960-75, 540, 960+75, 540);
|
||||
UILineDraw(&UI_LegPose_l[3], "CD_l", UI_Graph_ADD, 7, UI_Color_Main,
|
||||
3, 960-75, 540, 960+75, 540);
|
||||
UILineDraw(&UI_LegPose_l[4], "ED_l", UI_Graph_ADD, 7, UI_Color_Main,
|
||||
3, 960-75, 540, 960+75, 540);
|
||||
UIGraphRefresh(&referee_recv_info->referee_id, 5, UI_LegPose_l[0], UI_LegPose_l[1], UI_LegPose_l[2], UI_LegPose_l[3], UI_LegPose_l[4]);
|
||||
|
||||
}
|
||||
|
||||
// 测试用函数，实现模式自动变化,用于检查该任务和裁判系统是否连接正常
|
||||
|
|
@ -247,10 +306,81 @@ static void MyUIRefresh(referee_info_t *referee_recv_info, Referee_Interactive_i
|
|||
if (_Interactive_data->Referee_Interactive_Flag.Power_flag == 1)
|
||||
{
|
||||
UIFloatDraw(&UI_Energy[1], "sd5", UI_Graph_Change, 8, UI_Color_Green, 18, 2, 2, 750, 230, _Interactive_data->Chassis_Power_Data.chassis_power_mx * 1000);
|
||||
UILineDraw(&UI_Energy[2], "sd6", UI_Graph_Change, 8, UI_Color_Pink, 30, 720, 160, (uint32_t)750 + _Interactive_data->Chassis_Power_Data.chassis_power_mx * 30, 160);
|
||||
UIGraphRefresh(&referee_recv_info->referee_id, 2, UI_Energy[1], UI_Energy[2]);
|
||||
//UILineDraw(&UI_Energy[2], "sd6", UI_Graph_Change, 8, UI_Color_Pink, 30, 720, 160, (uint32_t)750 + _Interactive_data->Chassis_Power_Data.chassis_power_mx * 30, 160);
|
||||
UIGraphRefresh(&referee_recv_info->referee_id, 1, UI_Energy[1]);
|
||||
_Interactive_data->Referee_Interactive_Flag.Power_flag = 0;
|
||||
}
|
||||
|
||||
//电容电压始终刷新
|
||||
// 电容电压显示,动态
|
||||
UIFloatDraw(&UI_Energy[3], "v_cap", UI_Graph_Change, 8, UI_Color_Main, 18, 2, 2, 1200, 230, _Interactive_data->Chassis_Power_Data.cap_vol * 1000);
|
||||
//UIGraphRefresh(&referee_recv_info->referee_id, 1, UI_Energy[3]);
|
||||
//电容能量条 720~1220
|
||||
float percent = (_Interactive_data->Chassis_Power_Data.cap_vol - 12)/(_Interactive_data->Chassis_Power_Data.input_vol - 12);
|
||||
float length = percent * (1220 - 720);
|
||||
UILineDraw(&UI_Energy[2], "sd6", UI_Graph_Change, 8, UI_Color_Pink, 30, 720, 160, (int)(720+length), 160);
|
||||
UIGraphRefresh(&referee_recv_info->referee_id, 2, UI_Energy[2], UI_Energy[3]);
|
||||
}
|
||||
|
||||
void DrawArmorPose(float relative_angle)
|
||||
{
|
||||
float start_angle_b = loop_float_constrain(relative_angle - 30,0,360);
|
||||
float end_angle_b = loop_float_constrain(relative_angle + 30,0,360);
|
||||
|
||||
float start_angle_f = loop_float_constrain(start_angle_b + 180,0,360);
|
||||
float end_angle_f = loop_float_constrain(end_angle_b + 180,0,360);
|
||||
|
||||
UIArcDraw(&UI_ArmorPose[1],"front",UI_Graph_Change,7,UI_Color_Yellow,
|
||||
start_angle_f,end_angle_f,10,960,540,350,350);
|
||||
UIArcDraw(&UI_ArmorPose[2],"back",UI_Graph_Change,7,UI_Color_Yellow,
|
||||
start_angle_b,end_angle_b,10,960,540,350,350);
|
||||
UIGraphRefresh(&referee_recv_info->referee_id,2,UI_ArmorPose[1],UI_ArmorPose[2]);
|
||||
}
|
||||
|
||||
|
||||
void DrawLegPose(Referee_Interactive_info_t *_Interactive_data,int x_offset,int y_offset,int scale)
|
||||
{
|
||||
//float *leg_position = _Interactive_data->leg_pos;
|
||||
float xa = 0.075f * scale + x_offset,xe = -0.075f * scale + x_offset;
|
||||
float ya = 0 + y_offset,ye = 0 + y_offset;
|
||||
float leg_position[6] = {0};
|
||||
for (int i = 0; i < 6; i+=2) {
|
||||
leg_position[i] = -_Interactive_data->leg_pos_r[i] * scale + x_offset;
|
||||
leg_position[i+1] = -_Interactive_data->leg_pos_r[i+1] * scale + y_offset;
|
||||
}
|
||||
UILineDraw(&UI_LegPose_r[0], "AE", UI_Graph_Change, 7, UI_Color_Main,
|
||||
3, xa, ya, xe, ye);
|
||||
UILineDraw(&UI_LegPose_r[1], "AB", UI_Graph_Change, 7, UI_Color_Main,
|
||||
3, xa, ya, leg_position[0], leg_position[1]);
|
||||
UILineDraw(&UI_LegPose_r[2], "BC", UI_Graph_Change, 7, UI_Color_Main,
|
||||
3, leg_position[0], leg_position[1], leg_position[2], leg_position[3]);
|
||||
UILineDraw(&UI_LegPose_r[3], "CD", UI_Graph_Change, 7, UI_Color_Main,
|
||||
3, leg_position[2], leg_position[3], leg_position[4], leg_position[5]);
|
||||
UILineDraw(&UI_LegPose_r[4], "ED", UI_Graph_Change, 7, UI_Color_Main,
|
||||
3, xe, ye, leg_position[4], leg_position[5]);
|
||||
UIGraphRefresh(&referee_recv_info->referee_id, 5, UI_LegPose_r[0], UI_LegPose_r[1], UI_LegPose_r[2], UI_LegPose_r[3], UI_LegPose_r[4]);
|
||||
|
||||
x_offset += 0.5 * scale;
|
||||
xa = 0.075f * scale + x_offset,xe = -0.075f * scale + x_offset;
|
||||
ya = 0 + y_offset,ye = 0 + y_offset;
|
||||
|
||||
for (int i = 0; i < 6; i+=2) {
|
||||
leg_position[i] = -_Interactive_data->leg_pos_r[i] * scale + x_offset;
|
||||
leg_position[i+1] = -_Interactive_data->leg_pos_r[i+1] * scale + y_offset;
|
||||
}
|
||||
UILineDraw(&UI_LegPose_l[0], "AE_l", UI_Graph_Change, 7, UI_Color_Main,
|
||||
3, xa, ya, xe, ye);
|
||||
UILineDraw(&UI_LegPose_l[1], "AB_l", UI_Graph_Change, 7, UI_Color_Main,
|
||||
3, xa, ya, leg_position[0], leg_position[1]);
|
||||
UILineDraw(&UI_LegPose_l[2], "BC_l", UI_Graph_Change, 7, UI_Color_Main,
|
||||
3, leg_position[0], leg_position[1], leg_position[2], leg_position[3]);
|
||||
UILineDraw(&UI_LegPose_l[3], "CD_l", UI_Graph_Change, 7, UI_Color_Main,
|
||||
3, leg_position[2], leg_position[3], leg_position[4], leg_position[5]);
|
||||
UILineDraw(&UI_LegPose_l[4], "ED_l", UI_Graph_Change, 7, UI_Color_Main,
|
||||
3, xe, ye, leg_position[4], leg_position[5]);
|
||||
UIGraphRefresh(&referee_recv_info->referee_id, 5, UI_LegPose_l[0], UI_LegPose_l[1], UI_LegPose_l[2], UI_LegPose_l[3], UI_LegPose_l[4]);
|
||||
|
||||
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
|
|||
|
|
@ -95,6 +95,9 @@ static void JudgeReadData(uint8_t *buff)
|
|||
case ID_student_interactive: // 0x0301 syhtodo接收代码未测试
|
||||
memcpy(&referee_info.ReceiveData, (buff + DATA_Offset), LEN_receive_data);
|
||||
break;
|
||||
case ID_remote_control: // 0x0302
|
||||
memcpy(&referee_info.RemoteControl, (buff + DATA_Offset), LEN_remote_control);
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -38,6 +38,8 @@ typedef struct
|
|||
ext_robot_hurt_t RobotHurt; // 0x0206
|
||||
ext_shoot_data_t ShootData; // 0x0207
|
||||
|
||||
remote_control_t RemoteControl; // 0x304
|
||||
|
||||
// 自定义交互数据的接收
|
||||
Communicate_ReceiveData_t ReceiveData;
|
||||
|
||||
|
|
@ -68,6 +70,10 @@ typedef struct
|
|||
lid_mode_e lid_mode; // 弹舱盖打开
|
||||
Chassis_Power_Data_s Chassis_Power_Data; // 功率控制
|
||||
|
||||
float relative_angle; //云台相对角度
|
||||
float leg_pos_r[6]; //腿部姿态
|
||||
float leg_pos_l[6]; //腿部姿态
|
||||
|
||||
// 上一次的模式，用于flag判断
|
||||
chassis_mode_e chassis_last_mode;
|
||||
gimbal_mode_e gimbal_last_mode;
|
||||
|
|
|
|||
|
|
@ -1,14 +1,8 @@
|
|||
/*
|
||||
* @Descripttion:
|
||||
* @version:
|
||||
* @Author: Chenfu
|
||||
* @Date: 2022-12-02 21:32:47
|
||||
* @LastEditTime: 2022-12-05 15:29:49
|
||||
*/
|
||||
#include "super_cap.h"
|
||||
#include "memory.h"
|
||||
#include "stdlib.h"
|
||||
|
||||
|
||||
static SuperCapInstance *super_cap_instance = NULL; // 可以由app保存此指针
|
||||
|
||||
static void SuperCapRxCallback(CANInstance *_instance)
|
||||
|
|
@ -17,16 +11,17 @@ static void SuperCapRxCallback(CANInstance *_instance)
|
|||
SuperCap_Msg_s *Msg;
|
||||
rxbuff = _instance->rx_buff;
|
||||
Msg = &super_cap_instance->cap_msg;
|
||||
Msg->vol = (uint16_t)(rxbuff[0] << 8 | rxbuff[1]);
|
||||
Msg->current = (uint16_t)(rxbuff[2] << 8 | rxbuff[3]);
|
||||
Msg->power = (uint16_t)(rxbuff[4] << 8 | rxbuff[5]);
|
||||
Msg->input_vol = (int16_t)(rxbuff[1] << 8 | rxbuff[0]);
|
||||
Msg->cap_vol = (int16_t)(rxbuff[3] << 8 | rxbuff[2]);
|
||||
Msg->input_cur = (int16_t)(rxbuff[5] << 8 | rxbuff[4]);
|
||||
Msg->power_set = (int16_t)(rxbuff[7] << 8 | rxbuff[6]);
|
||||
}
|
||||
|
||||
SuperCapInstance *SuperCapInit(SuperCap_Init_Config_s *supercap_config)
|
||||
{
|
||||
super_cap_instance = (SuperCapInstance *)malloc(sizeof(SuperCapInstance));
|
||||
memset(super_cap_instance, 0, sizeof(SuperCapInstance));
|
||||
|
||||
|
||||
supercap_config->can_config.can_module_callback = SuperCapRxCallback;
|
||||
super_cap_instance->can_ins = CANRegister(&supercap_config->can_config);
|
||||
return super_cap_instance;
|
||||
|
|
@ -38,6 +33,15 @@ void SuperCapSend(SuperCapInstance *instance, uint8_t *data)
|
|||
CANTransmit(instance->can_ins,1);
|
||||
}
|
||||
|
||||
void SuperCapSetPower(SuperCapInstance *instance, float power_set)
|
||||
{
|
||||
uint16_t tmpPower = (uint16_t)(power_set * 100);
|
||||
uint8_t data[8] = {0};
|
||||
data[0] = tmpPower >> 8;
|
||||
data[1] = tmpPower;
|
||||
SuperCapSend(instance,data);
|
||||
}
|
||||
|
||||
SuperCap_Msg_s SuperCapGet(SuperCapInstance *instance)
|
||||
{
|
||||
return instance->cap_msg;
|
||||
|
|
|
|||
|
|
@ -1,10 +1,3 @@
|
|||
/*
|
||||
* @Descripttion:
|
||||
* @version:
|
||||
* @Author: Chenfu
|
||||
* @Date: 2022-12-02 21:32:47
|
||||
* @LastEditTime: 2022-12-05 15:25:46
|
||||
*/
|
||||
#ifndef SUPER_CAP_H
|
||||
#define SUPER_CAP_H
|
||||
|
||||
|
|
@ -13,9 +6,10 @@
|
|||
#pragma pack(1)
|
||||
typedef struct
|
||||
{
|
||||
uint16_t vol; // 电压
|
||||
uint16_t current; // 电流
|
||||
uint16_t power; // 功率
|
||||
int16_t input_vol; // 输入电压
|
||||
int16_t cap_vol; // 电容电压
|
||||
int16_t input_cur; // 输入电流
|
||||
int16_t power_set; // 设定功率
|
||||
} SuperCap_Msg_s;
|
||||
#pragma pack()
|
||||
|
||||
|
|
@ -34,7 +28,7 @@ typedef struct
|
|||
|
||||
/**
|
||||
* @brief 初始化超级电容
|
||||
*
|
||||
*
|
||||
* @param supercap_config 超级电容初始化配置
|
||||
* @return SuperCapInstance* 超级电容实例指针
|
||||
*/
|
||||
|
|
@ -42,10 +36,18 @@ SuperCapInstance *SuperCapInit(SuperCap_Init_Config_s *supercap_config);
|
|||
|
||||
/**
|
||||
* @brief 发送超级电容控制信息
|
||||
*
|
||||
*
|
||||
* @param instance 超级电容实例
|
||||
* @param data 超级电容控制信息
|
||||
*/
|
||||
void SuperCapSend(SuperCapInstance *instance, uint8_t *data);
|
||||
|
||||
/**
|
||||
* @brief 发送超级电容目标功率
|
||||
*
|
||||
* @param instance 超级电容实例
|
||||
* @param power_set 超级电容目标功率
|
||||
*/
|
||||
void SuperCapSetPower(SuperCapInstance *instance, float power_set);
|
||||
|
||||
#endif // !SUPER_CAP_Hd
|
||||
|
|
|
|||
Loading…
Reference in New Issue