Compare commits
4 Commits
| Author | SHA1 | Date |
|---|---|---|
 宋家齐
|
e20ead6ebf | |
|
宋家齐
|
6fa3b67cae | |
|
宋家齐
|
0c508b08cf | |
|
宋家齐
|
23feabe1fc |
|
|
@ -35,6 +35,7 @@ add_compile_options(-ffunction-sections -fdata-sections -fno-common -fmessage-le
|
|||
# Enable assembler files preprocessing
|
||||
add_compile_options($<$<COMPILE_LANGUAGE:ASM>:-x$<SEMICOLON>assembler-with-cpp>)
|
||||
|
||||
|
||||
if ("${CMAKE_BUILD_TYPE}" STREQUAL "Release")
|
||||
message(STATUS "Maximum optimization for speed")
|
||||
add_compile_options(-Ofast)
|
||||
|
|
|
|||
|
|
@ -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,115 +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 balance::L_target_set(float L) {
|
||||
target_length_ = L;
|
||||
}
|
||||
|
||||
void balance::Phi_target_set(float Phi) {
|
||||
|
||||
}
|
||||
|
||||
|
||||
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;
|
||||
/* 06-Mar-2024 21:40:29 */
|
||||
K_temp[0] =
|
||||
((L0 * -161.752151F + t2 * 164.793869F) - t3 * 97.0541687F) - 1.77457297F;
|
||||
K_temp[1] =
|
||||
((L0 * 96.1845703F - t2 * 284.733704F) + t3 * 279.889618F) + 3.29777265F;
|
||||
K_temp[2] =
|
||||
((L0 * -15.8959455F - t2 * 17.009819F) + t3 * 16.7305775F) - 0.452344805F;
|
||||
K_temp[3] =
|
||||
((L0 * 5.66845322F - t2 * 16.5131245F) + t3 * 14.656208F) + 0.677895188F;
|
||||
K_temp[4] = ((L0 * -7.06628323F + t2 * 13.4040155F) - t3 * 8.83661747F) - 8.61461F;
|
||||
K_temp[5] =
|
||||
((L0 * -28.798315F + t2 * 17.1446743F) + t3 * 15.3877935F) + 11.4884624F;
|
||||
K_temp[6] =
|
||||
((L0 * -5.75727034F - t2 * 3.09837651F) + t3 * 10.1307449F) - 9.7942028F;
|
||||
K_temp[7] =
|
||||
((L0 * -33.38451F + t2 * 35.8943558F) - t3 * 7.07260895F) + 11.7050676F;
|
||||
K_temp[8] =
|
||||
((L0 * -83.7100143F + t2 * 109.333092F) - t3 * 54.9969864F) + 36.5699463F;
|
||||
K_temp[9] =
|
||||
((L0 * 124.852882F - t2 * 253.387085F) + t3 * 193.295883F) + 55.7350769F;
|
||||
K_temp[10] = ((L0 * -6.44499636F + t2 * 5.48124027F) - t3 * 0.516800761F) +
|
||||
5.52106953F;
|
||||
K_temp[11] =
|
||||
((L0 * 12.3147469F - t2 * 13.2560177F) + t3 * 1.4054811F) + 2.85080624F;
|
||||
|
||||
//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,42 +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()= default;
|
||||
explicit 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);
|
||||
void L_target_set(float L);
|
||||
void Phi_target_set(float Phi);
|
||||
|
||||
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
|
||||
|
|
|
|||
|
|
@ -0,0 +1,67 @@
|
|||
//
|
||||
// Created by SJQ on 2024/3/7.
|
||||
//
|
||||
|
||||
#include "estimator.h"
|
||||
static const float dt = 1e-3f;
|
||||
//状态转移矩阵
|
||||
static float F[4] = {1,dt,
|
||||
0,1};
|
||||
//控制矩阵
|
||||
static float B[2] = {0.5f*dt*dt,
|
||||
dt};
|
||||
//观测矩阵
|
||||
static float H[4] = {1,0,
|
||||
0,1};
|
||||
//后验估计协方差初始值
|
||||
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) {
|
||||
Kalman_Filter_Init(&est->EstimateKF_,2,1,2);
|
||||
est->EstimateKF_.UseAutoAdjustment = 0;
|
||||
|
||||
for (uint8_t i = 0; i < 4; i += 3)
|
||||
{
|
||||
// 初始化过程噪声与量测噪声
|
||||
Q[i] = process_noise;
|
||||
R[i] = measure_noise;
|
||||
}
|
||||
memcpy(est->EstimateKF_.F_data,F,sizeof(F));
|
||||
memcpy(est->EstimateKF_.B_data,B,sizeof(B));
|
||||
memcpy(est->EstimateKF_.H_data,H,sizeof(H));
|
||||
memcpy(est->EstimateKF_.Q_data,Q,sizeof(Q));
|
||||
memcpy(est->EstimateKF_.R_data,R,sizeof(R));
|
||||
|
||||
DWT_GetDeltaT(&est->DWT_CNT_);
|
||||
}
|
||||
|
||||
void estimator_update(estimator_t *est ,float x,float x_dot,float ax) {
|
||||
est->EstimateKF_.MeasuredVector[0] = x;
|
||||
est->EstimateKF_.MeasuredVector[1] = x_dot;
|
||||
|
||||
est->EstimateKF_.ControlVector[0] = ax;
|
||||
|
||||
Kalman_Filter_Update(&est->EstimateKF_);
|
||||
|
||||
// 提取估计值
|
||||
|
||||
for (uint8_t i = 0; i < 2; 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];
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
|
@ -1,62 +0,0 @@
|
|||
//
|
||||
// Created by SJQ on 2024/3/7.
|
||||
//
|
||||
|
||||
#include "estimator.h"
|
||||
static const float dt = 1e-3;
|
||||
//状态转移矩阵
|
||||
static float F[4] = {0,1,
|
||||
1,0};
|
||||
//控制矩阵
|
||||
static float B[2] = {dt,
|
||||
1};
|
||||
//观测矩阵
|
||||
static float H[4] = {1,0,
|
||||
0,1};
|
||||
//后验估计协方差初始值
|
||||
static float P[4] = {100, 0.1,
|
||||
0.1, 100};
|
||||
// P Q矩阵初始值（其实这里设置多少都无所谓）
|
||||
static float Q[4] = {0.01, 0,
|
||||
0, 0.01};
|
||||
static float R[4] = {100000, 0,
|
||||
0, 100000,};
|
||||
estimator::estimator(float process_noise, float measure_noise) {
|
||||
Kalman_Filter_Init(&EstimateKF_,2,1,2);
|
||||
EstimateKF_.UseAutoAdjustment = 1;
|
||||
|
||||
for (uint8_t i = 0; i < 4; i += 3)
|
||||
{
|
||||
// 初始化过程噪声与量测噪声
|
||||
Q[i] = process_noise;
|
||||
R[i] = measure_noise;
|
||||
}
|
||||
memcpy(EstimateKF_.F_data,F,sizeof(F));
|
||||
memcpy(EstimateKF_.B_data,B,sizeof(B));
|
||||
memcpy(EstimateKF_.H_data,H,sizeof(H));
|
||||
memcpy(EstimateKF_.Q_data,Q,sizeof(Q));
|
||||
memcpy(EstimateKF_.R_data,R,sizeof(R));
|
||||
|
||||
DWT_GetDeltaT(&DWT_CNT_);
|
||||
}
|
||||
|
||||
void estimator::update(float x,float x_dot,float ax) {
|
||||
EstimateKF_.MeasuredVector[0] = x;
|
||||
EstimateKF_.MeasuredVector[1] = x_dot;
|
||||
|
||||
EstimateKF_.ControlVector[0] = ax;
|
||||
|
||||
Kalman_Filter_Update(&EstimateKF_);
|
||||
|
||||
// 提取估计值
|
||||
for (uint8_t i = 0; i < 2; i++)
|
||||
{
|
||||
Estimate_X_[i] = EstimateKF_.FilteredValue[i];
|
||||
}
|
||||
}
|
||||
|
||||
void estimator::get_result(float state[2])
|
||||
{
|
||||
state[0] = Estimate_X_[0];
|
||||
state[1] = Estimate_X_[1];
|
||||
}
|
||||
|
|
@ -6,19 +6,15 @@
|
|||
#ifndef WHEEL_LEGGED_ESTIMATOR_H
|
||||
#define WHEEL_LEGGED_ESTIMATOR_H
|
||||
|
||||
|
||||
class estimator {
|
||||
public:
|
||||
estimator(float process_noise, float measure_noise);
|
||||
void get_result(float state[2]);
|
||||
void update(float x,float x_dot,float ax);
|
||||
|
||||
private:
|
||||
typedef struct
|
||||
{
|
||||
KalmanFilter_t EstimateKF_; //使用KF作为状态观测器
|
||||
float Estimate_X_[2]; //观测器估计的状态量
|
||||
uint32_t DWT_CNT_; //计时用
|
||||
float dt_;
|
||||
};
|
||||
}estimator_t;
|
||||
|
||||
void estimator_init(estimator_t *est ,float process_noise, float measure_noise);
|
||||
void estimator_update(estimator_t *est ,float x,float x_dot,float ax);
|
||||
|
||||
#endif //WHEEL_LEGGED_ESTIMATOR_H
|
||||
|
|
|
|||
|
|
@ -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"
|
||||
|
|
@ -52,16 +52,22 @@ void Leg_State_update(LegInstance* legInstance, float phi1,float phi4 ,float phi
|
|||
//calculate_leg_pos(leg_state->phi1,leg_state->phi4,leg_pos);
|
||||
calculate_leg_pos(leg_state);
|
||||
|
||||
|
||||
//对phi0求导
|
||||
// leg_state->dt = DWT_GetDeltaT(&leg_state->DWT_CNT);
|
||||
// if(leg_state->last_phi0>=0)
|
||||
// leg_state->phi0_dot = (leg_state->phi0-leg_state->last_phi0)/leg_state->dt;
|
||||
// else
|
||||
// leg_state->phi0_dot = 0;
|
||||
// leg_state->last_phi0 = leg_state->phi0;
|
||||
leg_state->phi0_dot = get_dphi0(phi1,phi4,phi1_dot,phi4_dot);
|
||||
leg_state->L0_dot = get_dL0(phi1,phi4,phi1_dot,phi4_dot);
|
||||
|
||||
//对dPhi0 dL0求导(二阶导）
|
||||
leg_state->dt = DWT_GetDeltaT(&leg_state->DWT_CNT);
|
||||
// //if(leg_state->last_phi0>=0)
|
||||
// leg_state->phi0_dot2 = (leg_state->phi0_dot-leg_state->last_dPhi0)/leg_state->dt;
|
||||
// //else
|
||||
// //leg_state->phi0_dot_num = 0;
|
||||
// leg_state->last_dPhi0 = leg_state->phi0_dot;
|
||||
//
|
||||
//if(leg_state->last_phi0>=0)
|
||||
leg_state->L0_dot2 = (leg_state->L0_dot-leg_state->last_dL0)/leg_state->dt;
|
||||
//else
|
||||
//leg_state->phi0_dot_num = 0;
|
||||
leg_state->last_dL0 = leg_state->L0_dot;
|
||||
}
|
||||
|
||||
void Leg_Input_update(Leg_Input_t* leg_Input, float F,float Tp)
|
||||
|
|
@ -114,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,156 +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,36 +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);
|
||||
leg() {};
|
||||
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
|
||||
|
|
|
|||
|
|
@ -7,22 +7,23 @@
|
|||
#include "arm_math.h"
|
||||
|
||||
static const float dt = 1e-3;
|
||||
|
||||
static const float p_A[6][3] = {
|
||||
{110.149443140716,-209.796047546046, 108.212820026855},
|
||||
{6.56756328149907,-12.5089040777703,6.45209383844021},
|
||||
{20.8432584790045,-14.5681682729471,-0.265391946757807},
|
||||
{1.24276088149285,-0.868614169078260,0.164688375466239},
|
||||
{-121.880643460273,85.1871469584697,1.55187545520287},
|
||||
{-7.26702574149722,5.07920841420309,16.7402602760541}
|
||||
{-2089.29388440040,913.777119858069,161.725026756580},
|
||||
{-48.4503739138708,21.1903377794882,3.75037618024515},
|
||||
{349.621231737198,-308.272343710195,14.5058784104114},
|
||||
{8.10765758344078,-7.14878382193259,1.73804857662225},
|
||||
{-126.242793963670,111.312353023678,-5.23784728498450},
|
||||
{-2.92754916727612,2.58131475207839,21.1973941504635}
|
||||
};
|
||||
|
||||
static const float p_B[6][3] = {
|
||||
{-171.924494208671,125.455249123645,-27.5944606199982},
|
||||
{169.057788940511,-119.995181259560,24.7781571289566},
|
||||
{-0.576505386985648,0.0174968125883799,0.781810074568448},
|
||||
{0.0340470941093140,0.361648497135550,-0.267186973741535},
|
||||
{3.37110666237256,-0.102312350965865,-1.14202204587003},
|
||||
{-0.199089875614915,-2.11473419963208,6.60130194247413}
|
||||
{-5.36398695946053,59.5647709213161,-40.3119963098152},
|
||||
{127.834870314177,-113.128844359679,30.8319480335393},
|
||||
{-33.9678204232552,24.3044065007359,0.798747072246912},
|
||||
{13.4736135478509,-6.23400169559697,-0.818937686872829},
|
||||
{12.2652521237935,-8.77594351760660,1.29067140754752},
|
||||
{-4.86511248363210,2.25100114119497,2.36237087051996},
|
||||
};
|
||||
|
||||
|
||||
|
|
@ -44,19 +45,47 @@ const float gEstimateKF_H[36] = {1, 0, 0, 0, 0,0,
|
|||
0, 0, 0, 0, 1,0,
|
||||
0, 0, 0, 0, 0,1}; // 由于不需要异步量测自适应，这里直接设置矩阵H为常量
|
||||
|
||||
|
||||
//后验估计协方差初始值
|
||||
static float P[36] = {1, 0, 0, 0, 0,0,
|
||||
0, 1, 0, 0, 0,0,
|
||||
0, 0, 1, 0, 0,0,
|
||||
0, 0, 0, 1, 0,0,
|
||||
0, 0, 0, 0, 1,0,
|
||||
0, 0, 0, 0, 0,1};
|
||||
// R Q矩阵初始值（其实这里设置多少都无所谓）
|
||||
static float Q[36] = {1, 0, 0, 0, 0,0,
|
||||
0, 1, 0, 0, 0,0,
|
||||
0, 0, 1, 0, 0,0,
|
||||
0, 0, 0, 1, 0,0,
|
||||
0, 0, 0, 0, 1,0,
|
||||
0, 0, 0, 0, 0,1};
|
||||
static float R[36] = {1, 0, 0, 0, 0,0,
|
||||
0, 1, 0, 0, 0,0,
|
||||
0, 0, 1, 0, 0,0,
|
||||
0, 0, 0, 1, 0,0,
|
||||
0, 0, 0, 0, 1,0,
|
||||
0, 0, 0, 0, 0,1};
|
||||
void Observer_Init(BalanceObserver* observer)
|
||||
{
|
||||
KalmanFilter_t *kf = &observer->stateEstimateKF;
|
||||
Kalman_Filter_Init(kf,6,2,6);
|
||||
|
||||
for (uint8_t i = 0; i < 36; i += 7)
|
||||
{
|
||||
// 初始化过程噪声与量测噪声
|
||||
Q[i] = 1.0f;//process_noise;
|
||||
R[i] = 0.01f;//measure_noise;
|
||||
}
|
||||
|
||||
//暂时只使用状态预测一个步骤
|
||||
kf->UseAutoAdjustment = 0;
|
||||
kf->SkipEq2 = 1; kf->SkipEq3 = 1; kf->SkipEq4 = 1; kf->SkipEq5 = 1;
|
||||
//kf->UseAutoAdjustment = 1;
|
||||
//kf->SkipEq2 = 1; kf->SkipEq3 = 1; kf->SkipEq4 = 1; kf->SkipEq5 = 1;
|
||||
|
||||
memcpy(kf->F_data , gEstimateKF_F , sizeof(gEstimateKF_F));
|
||||
memcpy(kf->B_data , gEstimateKF_B , sizeof(gEstimateKF_B));
|
||||
memcpy(kf->H_data , gEstimateKF_H , sizeof(gEstimateKF_H));
|
||||
memcpy(kf->Q_data , Q,sizeof(Q));
|
||||
memcpy(kf->R_data , R,sizeof(R));
|
||||
// set rest of memory to 0
|
||||
DWT_GetDeltaT(&observer->DWT_CNT);
|
||||
}
|
||||
|
|
@ -82,11 +111,22 @@ void Observer_Estimate(BalanceObserver* observer,float* zk,float* uk,float L0)
|
|||
kf->B_data[6] = B[2]; kf->B_data[7] = B[3];
|
||||
kf->B_data[10] = B[4]; kf->B_data[11] = B[5];
|
||||
//测量值更新
|
||||
memcpy(kf->MeasuredVector,zk,6*4 );
|
||||
//测量值更新
|
||||
memcpy(kf->ControlVector,uk,2*4);
|
||||
// memcpy(kf->MeasuredVector,zk,6*4);
|
||||
// //测量值更新
|
||||
// memcpy(kf->ControlVector,uk,2*4);
|
||||
|
||||
for(uint8_t i = 0; i < 6; i++)
|
||||
kf->MeasuredVector[i] = zk[i];
|
||||
for(uint8_t i = 0; i < 2; i++)
|
||||
kf->ControlVector[i] = uk[i];
|
||||
|
||||
Kalman_Filter_Update(kf);
|
||||
|
||||
memcpy(observer->Estimate_X,kf->xhatminus_data,6*4);
|
||||
// 提取估计值
|
||||
for (uint8_t i = 0; i < 2; i++)
|
||||
{
|
||||
observer->Estimate_X[i] = kf->FilteredValue[i];
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
|
@ -1,177 +0,0 @@
|
|||
//
|
||||
// Created by SJQ on 2023/12/30.
|
||||
//
|
||||
|
||||
#include "balance_old.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);
|
||||
}
|
||||
|
||||
//解算地面支持力F
|
||||
float calculate_F(float MotionAccel_z,float theta,float theta_dot,float L,float L_dot,float F_fb,float Tp_fb);
|
||||
|
||||
// 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,float MotionAccel_z)
|
||||
{
|
||||
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_left.legState.L0)/2;
|
||||
//(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);
|
||||
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,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 + 20,Tp + cor_Tp);
|
||||
Leg_Input_update(&leg_l->legInput,F_pid_out + leg_l->F_feedforward + 20,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 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;
|
||||
|
||||
float K_temp[12];
|
||||
/* This function was generated by the Symbolic Math Toolbox version 9.2.
|
||||
*/
|
||||
t2 = L0 * L0;
|
||||
t3 = t2 * L0;
|
||||
/* 06-Mar-2024 21:40:29 */
|
||||
K_temp[0] =
|
||||
((L0 * -161.752151F + t2 * 164.793869F) - t3 * 97.0541687F) - 1.77457297F;
|
||||
K_temp[1] =
|
||||
((L0 * 96.1845703F - t2 * 284.733704F) + t3 * 279.889618F) + 3.29777265F;
|
||||
K_temp[2] =
|
||||
((L0 * -15.8959455F - t2 * 17.009819F) + t3 * 16.7305775F) - 0.452344805F;
|
||||
K_temp[3] =
|
||||
((L0 * 5.66845322F - t2 * 16.5131245F) + t3 * 14.656208F) + 0.677895188F;
|
||||
K_temp[4] = ((L0 * -7.06628323F + t2 * 13.4040155F) - t3 * 8.83661747F) - 8.61461F;
|
||||
K_temp[5] =
|
||||
((L0 * -28.798315F + t2 * 17.1446743F) + t3 * 15.3877935F) + 11.4884624F;
|
||||
K_temp[6] =
|
||||
((L0 * -5.75727034F - t2 * 3.09837651F) + t3 * 10.1307449F) - 9.7942028F;
|
||||
K_temp[7] =
|
||||
((L0 * -33.38451F + t2 * 35.8943558F) - t3 * 7.07260895F) + 11.7050676F;
|
||||
K_temp[8] =
|
||||
((L0 * -83.7100143F + t2 * 109.333092F) - t3 * 54.9969864F) + 36.5699463F;
|
||||
K_temp[9] =
|
||||
((L0 * 124.852882F - t2 * 253.387085F) + t3 * 193.295883F) + 55.7350769F;
|
||||
K_temp[10] = ((L0 * -6.44499636F + t2 * 5.48124027F) - t3 * 0.516800761F) +
|
||||
5.52106953F;
|
||||
K_temp[11] =
|
||||
((L0 * 12.3147469F - t2 * 13.2560177F) + t3 * 1.4054811F) + 2.85080624F;
|
||||
|
||||
//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];
|
||||
}
|
||||
}
|
||||
|
||||
/*
|
||||
* File trailer for mylqr_k.c
|
||||
*
|
||||
* [EOF]
|
||||
*/
|
||||
|
||||
//解算地面支持力F
|
||||
float calculate_F(float MotionAccel_z,float theta,float theta_dot,float L,float L_dot,float F_fb,float Tp_fb)
|
||||
{
|
||||
//求导所用时间
|
||||
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 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 = F_fb*cos_theta + Tp_fb*sin_theta/L;
|
||||
|
||||
float F_N = P + 0.8f * (zw_dot2 + 9.8f);
|
||||
|
||||
return F_N;
|
||||
|
||||
}
|
||||
|
|
@ -1,43 +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"
|
||||
typedef struct
|
||||
{
|
||||
LegInstance leg_right;
|
||||
LegInstance leg_left;
|
||||
PIDInstance leg_coordination_PID; //双腿协调PD控制
|
||||
|
||||
PIDInstance leg_length_PID; //平均腿长控制
|
||||
|
||||
float state[6];
|
||||
float target_state[6];
|
||||
|
||||
LQRInstance balance_LQR;
|
||||
|
||||
|
||||
}Balance_Control_t;
|
||||
|
||||
typedef struct
|
||||
{
|
||||
Leg_init_config_s legInitConfig;
|
||||
PID_Init_Config_s leg_cor_PID_config;
|
||||
|
||||
int LQR_state_num;
|
||||
int LQR_control_num;
|
||||
}Balance_Init_config_s;
|
||||
|
||||
void Balance_Control_Init(Balance_Control_t* balanceControl,Balance_Init_config_s InitConfig);
|
||||
//void Balance_feedback_update(Balance_Control_t* balanceControl,float leg_phi[4],float body_phi,float body_phi_dot,float x,float x_dot);
|
||||
void Balance_feedback_update(Balance_Control_t* balanceControl,float leg_phi[4],float leg_phi_dot[4],float body_phi,float body_phi_dot,float x,float x_dot,float MotionAccel_z);
|
||||
void Balance_Control_Loop(Balance_Control_t* balanceControl);
|
||||
void target_x_set(Balance_Control_t* balanceControl,float x);
|
||||
|
||||
#endif //WHEEL_LEGGED_BALANCE_OLD_H
|
||||
|
|
@ -24,6 +24,7 @@
|
|||
|
||||
#include "balance.h"
|
||||
#include "observer.h"
|
||||
#include "estimator.h"
|
||||
|
||||
#include "general_def.h"
|
||||
#include "bsp_dwt.h"
|
||||
|
|
@ -31,14 +32,12 @@
|
|||
#include "arm_math.h"
|
||||
#include "vofa.h"
|
||||
#include "user_lib.h"
|
||||
#include "balance.h"
|
||||
/* 根据robot_def.h中的macro自动计算的参数 */
|
||||
#define HALF_WHEEL_BASE (WHEEL_BASE / 2.0f) // 半轴距
|
||||
#define HALF_TRACK_WIDTH (TRACK_WIDTH / 2.0f) // 半轮距
|
||||
#define PERIMETER_WHEEL (RADIUS_WHEEL * 2 * PI) // 轮子周长
|
||||
|
||||
/* 底盘应用包含的模块和信息存储,底盘是单例模式,因此不需要为底盘建立单独的结构体 */
|
||||
|
||||
#ifdef CHASSIS_BOARD // 如果是底盘板,使用板载IMU获取底盘转动角速度
|
||||
#include "can_comm.h"
|
||||
#include "ins_task.h"
|
||||
|
|
@ -67,45 +66,61 @@ static LKMotorInstance *wheel_motor_r,*wheel_motor_l;
|
|||
static float chassis_vx, chassis_vy; // 将云台系的速度投影到底盘
|
||||
static float vt_lf, vt_rf, vt_lb, vt_rb; // 底盘速度解算后的临时输出,待进行限幅
|
||||
|
||||
balance balance_chassis;
|
||||
static LegInstance legInstance;
|
||||
|
||||
Balance_Control_t BalanceControl;
|
||||
|
||||
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()
|
||||
{
|
||||
// 四个关节电机的参数,改tx_id和反转标志位即可
|
||||
Motor_Init_Config_s chassis_motor_config = {
|
||||
.can_init_config.can_handle = &hcan2,
|
||||
.controller_setting_init_config = {
|
||||
.outer_loop_type = OPEN_LOOP,
|
||||
.close_loop_type = OPEN_LOOP,//| CURRENT_LOOP,
|
||||
.angle_feedback_source = MOTOR_FEED,
|
||||
.speed_feedback_source = MOTOR_FEED,
|
||||
.outer_loop_type = OPEN_LOOP,
|
||||
.close_loop_type = OPEN_LOOP,//| CURRENT_LOOP,
|
||||
},
|
||||
.motor_type = HT04,
|
||||
.can_init_config = {.can_handle = &hcan2},
|
||||
|
||||
};
|
||||
|
||||
Motor_Init_Config_s wheel_motor_config = {
|
||||
.can_init_config.can_handle = &hcan1,
|
||||
.controller_param_init_config ={
|
||||
.speed_PID = {
|
||||
.Kp = -100, // 50
|
||||
.Ki = 0, // 200
|
||||
.Kd = 0,
|
||||
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
|
||||
.IntegralLimit = 3000,
|
||||
.MaxOut = 20000,
|
||||
},
|
||||
},
|
||||
.controller_setting_init_config = {
|
||||
.outer_loop_type = OPEN_LOOP,
|
||||
.close_loop_type = OPEN_LOOP,//| CURRENT_LOOP,
|
||||
.angle_feedback_source = MOTOR_FEED,
|
||||
.speed_feedback_source = MOTOR_FEED,
|
||||
.outer_loop_type = OPEN_LOOP,
|
||||
.close_loop_type = OPEN_LOOP,
|
||||
},
|
||||
.motor_type = LK9025,
|
||||
.can_init_config = {.can_handle = &hcan1},
|
||||
};
|
||||
// @todo: 当前还没有设置电机的正反转,仍然需要手动添加reference的正负号,需要电机module的支持,待修改.
|
||||
chassis_motor_config.can_init_config.tx_id = 1;
|
||||
//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);
|
||||
|
|
@ -115,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);
|
||||
|
|
@ -122,24 +138,26 @@ void ChassisInit()
|
|||
//@todo:瓴控驱动代码有些问题 1号电机必须先初始化
|
||||
|
||||
wheel_motor_config.can_init_config.tx_id = 1;
|
||||
wheel_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE;
|
||||
wheel_motor_r = LKMotorInit(&wheel_motor_config);
|
||||
wheel_motor_l = LKMotorInit(&wheel_motor_config);
|
||||
|
||||
wheel_motor_config.can_init_config.tx_id = 2;
|
||||
wheel_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_NORMAL;
|
||||
wheel_motor_l = LKMotorInit(&wheel_motor_config);
|
||||
wheel_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_REVERSE;
|
||||
wheel_motor_r = LKMotorInit(&wheel_motor_config);
|
||||
|
||||
|
||||
|
||||
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_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,
|
||||
|
|
@ -152,57 +170,57 @@ 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 = static_cast<PID_Improvement_e>(PID_Trapezoid_Intergral | PID_Integral_Limit |
|
||||
PID_Derivative_On_Measurement |
|
||||
PID_DerivativeFilter),
|
||||
.IntegralLimit = 100,
|
||||
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement | PID_DerivativeFilter,
|
||||
.Derivative_LPF_RC = 0.001f,//1/(2*PI*10),
|
||||
.IntegralLimit = 100,
|
||||
},
|
||||
.phi0_PID_conf = {
|
||||
.Kp = 80,
|
||||
.Ki = 0,
|
||||
.Kd = 5,
|
||||
.MaxOut = 5,
|
||||
.Improve = static_cast<PID_Improvement_e>(PID_Trapezoid_Intergral | PID_Integral_Limit |
|
||||
PID_Derivative_On_Measurement |
|
||||
PID_DerivativeFilter),
|
||||
.IntegralLimit = 100,
|
||||
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement| PID_DerivativeFilter,
|
||||
.Derivative_LPF_RC = 1/(2*PI*10),
|
||||
.IntegralLimit = 100,
|
||||
},
|
||||
.init_target_L0 = 0.15f,
|
||||
.F_feedforward = 0,
|
||||
},
|
||||
.leg_cor_PID_config = {
|
||||
.Kp = 25,
|
||||
.Kp = 25.0f,//25,//25,
|
||||
.Ki = 0,
|
||||
.Kd = 3,
|
||||
.Kd = 3.0f,//3,
|
||||
.MaxOut = 3,
|
||||
.Improve = static_cast<PID_Improvement_e>(PID_Derivative_On_Measurement | PID_DerivativeFilter),
|
||||
.Improve = PID_Derivative_On_Measurement| PID_DerivativeFilter,
|
||||
.Derivative_LPF_RC = 0.01f,
|
||||
},
|
||||
.LQR_state_num = 6,
|
||||
.LQR_control_num = 2,
|
||||
|
||||
};
|
||||
|
||||
balance_chassis = balance(&balanceInitConfig);
|
||||
|
||||
//Leg_Init(&legInstance,&leg_r_init_conf);
|
||||
Balance_Control_Init(&BalanceControl,balanceInitConfig);
|
||||
Chassis_IMU_data = INS_Init(); // 底盘IMU初始化
|
||||
//转向环
|
||||
PID_Init_Config_s turn_pid_cfg = {
|
||||
.Kp = 10.0f,//1,
|
||||
.Kp = 12.0f,//10.0f,//1,
|
||||
.Ki = 0,
|
||||
.Kd = 0.0f,
|
||||
.MaxOut = 2,
|
||||
.Improve = static_cast<PID_Improvement_e>(PID_Derivative_On_Measurement | PID_DerivativeFilter),
|
||||
.Kd = 0.8f,
|
||||
.MaxOut = 5.0f,
|
||||
|
||||
.Improve = PID_Derivative_On_Measurement|PID_DerivativeFilter,
|
||||
.Derivative_LPF_RC = 0.001f
|
||||
};
|
||||
PIDInit(&Turn_Loop_PID,&turn_pid_cfg);
|
||||
|
||||
//Roll轴平衡
|
||||
PID_Init_Config_s roll_pid_cfg = {
|
||||
.Kp = 8.0f,
|
||||
.Kp = 12.0f,
|
||||
.Ki = 0,
|
||||
.Kd = 0.5f,
|
||||
.MaxOut = 100,
|
||||
|
|
@ -232,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);
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -239,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);
|
||||
|
|
@ -260,11 +291,9 @@ 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];
|
||||
|
||||
float total_angle = (-wheel_motor_r->measure.total_angle + wheel_motor_l->measure.total_angle)/2;
|
||||
|
||||
//陀螺仪积分获取位移和速度
|
||||
static uint32_t integral_cnt = 0;
|
||||
static float imu_v,imu_x;
|
||||
|
|
@ -279,22 +308,47 @@ static void Chassis_State_update()
|
|||
|
||||
|
||||
//float theta_dot1 = (balanceControl->state[0] - theta) / dot_dt;
|
||||
static float left_offset = 0;
|
||||
static float right_offset = 0;
|
||||
//倒地需要清零总位移数据
|
||||
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;
|
||||
}
|
||||
|
||||
|
||||
float x = (total_angle * DEGREE_2_RAD ) * 0.135f/2;
|
||||
|
||||
//驱动轮位移与速度
|
||||
float x_l = wheel_motor_l->measure.total_angle - left_offset;
|
||||
float x_r = wheel_motor_r->measure.total_angle - right_offset;
|
||||
float total_angle = (- x_r + x_l)/2;
|
||||
float x = (total_angle * DEGREE_2_RAD ) * RADIUS_WHEEL;
|
||||
float speed = (- wheel_motor_r->measure.speed_rads + wheel_motor_l->measure.speed_rads)/2;
|
||||
float x_dot = speed * RADIUS_WHEEL;
|
||||
|
||||
float x_dot = speed * 0.135f/2;
|
||||
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_chassis.feedback_update(leg_phi,leg_phi_dot,body_phi,body_phi_dot,x,x_dot,Chassis_IMU_data->MotionAccel_n[2]);
|
||||
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;
|
||||
}
|
||||
|
||||
/* 机器人底盘控制核心任务 */
|
||||
|
|
@ -309,7 +363,16 @@ void ChassisTask()
|
|||
chassis_cmd_recv = *(Chassis_Ctrl_Cmd_s *)CANCommGet(chasiss_can_comm);
|
||||
#endif // CHASSIS_BOARD
|
||||
|
||||
if (chassis_cmd_recv.chassis_mode == CHASSIS_NO_FOLLOW)
|
||||
//死亡情况下 关闭电机
|
||||
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);
|
||||
HTMotorStop(motor_rf);
|
||||
|
|
@ -317,6 +380,15 @@ void ChassisTask()
|
|||
HTMotorStop(motor_rb);
|
||||
LKMotorStop(wheel_motor_r);
|
||||
LKMotorStop(wheel_motor_l);
|
||||
|
||||
//清除相关pid控制量
|
||||
PIDClear(&BalanceControl.leg_length_PID);
|
||||
PIDClear(&Turn_Loop_PID);
|
||||
PIDClear(&Roll_Loop_PID);
|
||||
PIDClear(&BalanceControl.leg_coordination_PID);
|
||||
//清除状态量
|
||||
state_clear(&BalanceControl);
|
||||
//estimator_init(&BalanceControl.v_estimator,0.001f,10.0f);
|
||||
}
|
||||
else
|
||||
{ // 正常工作
|
||||
|
|
@ -330,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)
|
||||
{
|
||||
|
|
@ -338,10 +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.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 = 1.0f * chassis_cmd_recv.offset_angle* abs(chassis_cmd_recv.offset_angle);
|
||||
//chassis_cmd_recv.wz = 2.0f;
|
||||
break;
|
||||
default:
|
||||
break;
|
||||
|
|
@ -352,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
|
||||
|
|
@ -365,82 +468,73 @@ void ChassisTask()
|
|||
|
||||
//获取数据并计算状态量
|
||||
Chassis_State_update();
|
||||
if(chassis_cmd_recv.chassis_mode != CHASSIS_ZERO_FORCE) // 右侧开关状态[下],底盘跟随云台
|
||||
{
|
||||
target_x_set(&BalanceControl,chassis_vx);
|
||||
target_L_set(&BalanceControl,chassis_cmd_recv.vy/5280 * 0.0003f);
|
||||
|
||||
float vofa_send_data[8];
|
||||
|
||||
static float L_target = 0;
|
||||
L_target += chassis_cmd_recv.vy/5280 * 0.0003f;
|
||||
L_target = float_constrain(L_target,0.15f,0.30f);
|
||||
|
||||
balance_chassis.L_target_set(L_target);
|
||||
|
||||
//vofa_justfloat_output(vofa_send_data,32,&huart1);
|
||||
|
||||
static float target_x = 0;
|
||||
static float target_yaw = 0;
|
||||
if(chassis_cmd_recv.chassis_mode == CHASSIS_ROTATE) // 右侧开关状态[下],底盘跟随云台
|
||||
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;
|
||||
|
||||
target_x += chassis_cmd_recv.vx/5280 * 0.003f;
|
||||
target_x_set(&BalanceControl,target_x);
|
||||
|
||||
Balance_Control_Loop(&BalanceControl);
|
||||
|
||||
|
||||
|
||||
target_yaw = 0.0f;//chassis_cmd_recv.wz * 0.001f;//Chassis_IMU_data->Gyro[Z]
|
||||
|
||||
//float turn_T = -PIDCalculate(&Turn_Loop_PID,chassis_cmd_recv.offset_angle,0);
|
||||
//float turn_T = -1.0f * chassis_cmd_recv.offset_angle * abs(chassis_cmd_recv.offset_angle);
|
||||
float turn_T = PIDCalculate(&Turn_Loop_PID,Chassis_IMU_data->Gyro[Z],target_yaw);
|
||||
|
||||
float T = float_constrain(BalanceControl.balance_LQR.control_vector[0],-4,4);
|
||||
|
||||
float wheel_current_r = (T+turn_T) * LK_TORQUE_2_CMD;
|
||||
float wheel_current_l = (T-turn_T) * LK_TORQUE_2_CMD;
|
||||
|
||||
float_constrain(wheel_current_r,-2000,2000);
|
||||
float_constrain(wheel_current_l,-2000,2000);
|
||||
|
||||
if (ChassisState == FAIL) // 倒地 未起身
|
||||
{
|
||||
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);
|
||||
turn_T = PIDCalculate(&Turn_Loop_PID,Chassis_IMU_data->Gyro[Z],chassis_cmd_recv.wz);
|
||||
}
|
||||
else
|
||||
{
|
||||
HTMotorSetRef(motor_rf,-BalanceControl.leg_right.legOutput.T1 * HT_CMD_COEF);
|
||||
HTMotorSetRef(motor_rb,-BalanceControl.leg_right.legOutput.T2 * HT_CMD_COEF);
|
||||
turn_T = 0;
|
||||
BalanceControl.leg_right.F_feedforward = 0;
|
||||
BalanceControl.leg_left.F_feedforward = 0;
|
||||
}
|
||||
|
||||
HTMotorSetRef(motor_lf,BalanceControl.leg_left.legOutput.T1 * HT_CMD_COEF);
|
||||
HTMotorSetRef(motor_lb,BalanceControl.leg_left.legOutput.T2 * HT_CMD_COEF);
|
||||
Balance_Control_Loop(&BalanceControl,jump_flag);
|
||||
|
||||
static float wz_feedback;
|
||||
wz_feedback = Chassis_IMU_data->Gyro[Z];
|
||||
|
||||
static float wheel_current_r,wheel_current_l,Tr,Tl;
|
||||
|
||||
//轮电机滤波并输出
|
||||
Tr = BalanceControl.LQR_r.control_vector[0];
|
||||
Tl = BalanceControl.LQR_l.control_vector[0];
|
||||
|
||||
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);
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
|
||||
LKMotorSetRef(wheel_motor_r,wheel_current_r);
|
||||
LKMotorSetRef(wheel_motor_l,wheel_current_l);
|
||||
// LKMotorSetRef(wheel_motor_r,100);
|
||||
// LKMotorSetRef(wheel_motor_l,100);
|
||||
// LKMotorSetRef(wheel_motor_r,0);
|
||||
// LKMotorSetRef(wheel_motor_l,0);
|
||||
|
||||
Observer_Estimate(&Observer,BalanceControl.state,BalanceControl.balance_LQR.control_vector, BalanceControl.leg_right.legState.L0);
|
||||
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);
|
||||
// Observer_Estimate(&Observer,BalanceControl.state,BalanceControl.balance_LQR.control_vector, BalanceControl.leg_right.legState.L0);
|
||||
// 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);
|
||||
}
|
||||
else{
|
||||
PIDClear(&BalanceControl.leg_length_PID);
|
||||
|
|
@ -449,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
|
||||
|
|
@ -13,8 +13,6 @@ void ChassisInit();
|
|||
*/
|
||||
void ChassisTask();
|
||||
|
||||
|
||||
|
||||
typedef enum
|
||||
{
|
||||
FAIL = 0,
|
||||
|
|
|
|||
|
|
@ -1,81 +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_phi0;
|
||||
float phi0_dot;
|
||||
|
||||
float L0_dot;
|
||||
|
||||
}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
|
||||
|
|
@ -73,7 +73,7 @@ void RobotCMDInit()
|
|||
#endif // GIMBAL_BOARD
|
||||
gimbal_cmd_send.pitch = 0;
|
||||
|
||||
robot_state = ROBOT_READY; // 启动时机器人进入工作模式,后续加入所有应用初始化完成之后再进入
|
||||
robot_state = ROBOT_STOP; // 轮腿 上电后进入ROBOT_STOP 保证安全(对应倒地状态）
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
|
|||
|
|
@ -17,8 +17,8 @@
|
|||
#include "stdint.h"
|
||||
|
||||
/* 开发板类型定义,烧录时注意不要弄错对应功能;修改定义后需要重新编译,只能存在一个定义! */
|
||||
#define ONE_BOARD // 单板控制整车
|
||||
//#define CHASSIS_BOARD //底盘板
|
||||
//#define ONE_BOARD // 单板控制整车
|
||||
#define CHASSIS_BOARD //底盘板
|
||||
// #define GIMBAL_BOARD //云台板
|
||||
|
||||
#define VISION_USE_VCP // 使用虚拟串口发送视觉数据
|
||||
|
|
@ -40,7 +40,7 @@
|
|||
#define TRACK_WIDTH 0.3f // 横向轮距(左右平移方向)
|
||||
#define CENTER_GIMBAL_OFFSET_X 0 // 云台旋转中心距底盘几何中心的距离,前后方向,云台位于正中心时默认设为0
|
||||
#define CENTER_GIMBAL_OFFSET_Y 0 // 云台旋转中心距底盘几何中心的距离,左右方向,云台位于正中心时默认设为0
|
||||
#define RADIUS_WHEEL 0.06f // 轮子半径
|
||||
#define RADIUS_WHEEL 0.1f // 轮子半径
|
||||
#define REDUCTION_RATIO_WHEEL 19.0f // 电机减速比,因为编码器量测的是转子的速度而不是输出轴的速度故需进行转换
|
||||
|
||||
#define GYRO2GIMBAL_DIR_YAW 1 // 陀螺仪数据相较于云台的yaw的方向,1为相同,-1为相反
|
||||
|
|
@ -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); // 尝试重新让电机进入控制模式
|
||||
}
|
||||
|
||||
|
|
@ -122,7 +122,7 @@ HTMotorInstance *HTMotorInit(Motor_Init_Config_s *config)
|
|||
Daemon_Init_Config_s conf = {
|
||||
.callback = HTMotorLostCallback,
|
||||
.owner_id = motor,
|
||||
.reload_count = 20,//5, // 20ms
|
||||
.reload_count = 100,//20,//5, // 20ms
|
||||
};
|
||||
motor->motor_daemon = DaemonRegister(&conf);
|
||||
|
||||
|
|
|
|||
|
|
@ -143,18 +143,18 @@ void LKMotorControl()
|
|||
|
||||
if ((setting->close_loop_type & SPEED_LOOP) && setting->outer_loop_type & (ANGLE_LOOP | SPEED_LOOP))
|
||||
{
|
||||
if (setting->angle_feedback_source == OTHER_FEED)
|
||||
if (setting->speed_feedback_source == OTHER_FEED)
|
||||
pid_measure = *motor->other_speed_feedback_ptr;
|
||||
else
|
||||
pid_measure = measure->speed_rads;
|
||||
pid_ref = PIDCalculate(&motor->angle_PID, pid_measure, pid_ref);
|
||||
pid_ref = PIDCalculate(&motor->speed_PID, pid_measure, pid_ref);
|
||||
if (setting->feedforward_flag & CURRENT_FEEDFORWARD)
|
||||
pid_ref += *motor->current_feedforward_ptr;
|
||||
}
|
||||
|
||||
if (setting->close_loop_type & CURRENT_LOOP)
|
||||
{
|
||||
pid_ref = PIDCalculate(&motor->current_PID, measure->real_current, pid_ref);
|
||||
//pid_ref = PIDCalculate(&motor->current_PID, measure->real_current, pid_ref);
|
||||
}
|
||||
|
||||
set = (int16_t)pid_ref;
|
||||
|
|
|
|||
|
|
@ -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 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 mains_power_gimbal_output : 1;
|
||||
uint8_t mains_power_chassis_output : 1;
|
||||
uint8_t mains_power_shooter_output : 1;
|
||||
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_voltage;
|
||||
uint16_t chassis_current;
|
||||
float chassis_power; // 瞬时功率
|
||||
uint16_t chassis_power_buffer; // 60焦耳缓冲能量
|
||||
uint16_t shooter_heat0; // 17mm
|
||||
uint16_t shooter_heat1;
|
||||
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,9 +11,10 @@ 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)
|
||||
|
|
@ -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()
|
||||
|
||||
|
|
@ -48,4 +42,12 @@ SuperCapInstance *SuperCapInit(SuperCap_Init_Config_s *supercap_config);
|
|||
*/
|
||||
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