Hshine
/
wheel_legged
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Compare commits

base: Hshine:main
Branches Tags
Hshine:main
Hshine:cpp
Hshine:master
..
compare: Hshine:cpp
Branches Tags
Hshine:main
Hshine:cpp
Hshine:master

2 Commits
main ... cpp

Author SHA1 Message Date
宋家齐 58c1d73ca6 更换cpp架构 有标准冲突待解决 2024-03-09 22:56:07 +08:00
宋家齐 3e54d14737 更换cpp架构 有标准冲突待解决 2024-03-09 22:32:37 +08:00
35 changed files with 927 additions and 1099 deletions
Show Stats Expand all files Collapse all files

1
CMakeLists.txt
View File

@ -35,7 +35,6 @@ 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)

1
Src/main.c → Src/main.cpp
View File

@ -120,7 +120,6 @@ 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 */

336
application/balance/balance.c
View File

@ -1,336 +0,0 @@
//
// 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;
}

115
application/balance/balance.cpp Normal file
View File

@ -0,0 +1,115 @@
//
// 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];
}
}

72
application/balance/balance.h
View File

@ -1,62 +1,42 @@
//
// Created by SJQ on 2023/12/30.
// Created by SJQ on 2024/3/2.
//
#ifndef WHEEL_LEGGED_BALANCE_H
#define WHEEL_LEGGED_BALANCE_H
#include "leg.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;
#include "LQR.h"
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,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);
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_;
};
#endif //WHEEL_LEGGED_BALANCE_H

67
application/balance/estimator.c
View File

@ -1,67 +0,0 @@
//
// 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];
}
}
}

62
application/balance/estimator.cpp Normal file
View File

@ -0,0 +1,62 @@
//
// 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];
}

14
application/balance/estimator.h
View File

@ -6,15 +6,19 @@
#ifndef WHEEL_LEGGED_ESTIMATOR_H
#define WHEEL_LEGGED_ESTIMATOR_H
typedef struct
{
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:
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

156
application/balance/leg.cpp Normal file
View File

@ -0,0 +1,156 @@
//
// 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;
}

74
application/balance/leg.h
View File

@ -1,9 +1,11 @@
//
// Created by SJQ on 2023/12/25.
// Created by SJQ on 2024/3/1.
//
#ifndef WHEEL_LEGGED_LEG_H
#define WHEEL_LEGGED_LEG_H
#include <matrix.h>
#include "controller.h"
typedef struct
@ -19,31 +21,13 @@ typedef struct
uint32_t DWT_CNT;
float dt; //用于计算时间并求导
float last_dPhi0;
float last_phi0;
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;
@ -51,36 +35,36 @@ typedef struct
float init_target_L0; //初始腿长目标值
float F_feedforward;
}Leg_init_config_s;
typedef struct
class leg
{
Leg_State_t legState;
Leg_Input_t legInput;
Leg_Input_t legFeedback;
Leg_Output_t legOutput;
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);
float target_L0;
float target_phi0;
PIDInstance Length_PID;
PIDInstance Phi0_PID;
float F_feedforward_; //支持力前馈补偿
private:
Leg_State_t legState_;
Matrixf<2,1> legInput_;
Matrixf<2,1> legOutput_;
Matrixf<2,1> legFeedback_;
float F_feedforward; //支持力前馈补偿
Matrixf<2,1> State_dot_; // L0 和 phi0 的导数
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;
float target_L0_;
float target_phi0_;
PIDInstance Length_PID_;
PIDInstance Phi0_PID_;
Matrixf<2,2> VMC_Jacobi_;
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);
void get_dot(float phi1_dot,float phi4_dot);
};
#endif //WHEEL_LEGGED_LEG_H

177
application/chassis/balance_old.c Normal file
View File

@ -0,0 +1,177 @@
//
// 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;
}

43
application/chassis/balance_old.h Normal file
View File

@ -0,0 +1,43 @@
//
// 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

0
application/balance/cal_phi0_dot.c → application/chassis/cal_phi0_dot.c
View File

0
application/balance/cal_phi0_dot.h → application/chassis/cal_phi0_dot.h
View File

364
application/chassis/chassis.c → application/chassis/chassis.cpp
View File

@ -24,7 +24,6 @@
#include "balance.h"
#include "observer.h"
#include "estimator.h"
#include "general_def.h"
#include "bsp_dwt.h"
@ -32,12 +31,14 @@
#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"
@ -66,61 +67,45 @@ static LKMotorInstance *wheel_motor_r,*wheel_motor_l;
static float chassis_vx, chassis_vy; // 将云台系的速度投影到底盘
static float vt_lf, vt_rf, vt_lb, vt_rb; // 底盘速度解算后的临时输出,待进行限幅
static LegInstance legInstance;
Balance_Control_t BalanceControl;
balance balance_chassis;
BalanceObserver Observer;
PIDInstance Turn_Loop_PID , Roll_Loop_PID;
Chassis_state_e ChassisState,last_ChassisState;
chassis_mode_e last_chassis_mode;
Chassis_state_e ChassisState;
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 = {
.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,
};
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,
},
.motor_type = HT04,
.can_init_config = {.can_handle = &hcan2},
};
Motor_Init_Config_s wheel_motor_config = {
.controller_setting_init_config = {
.outer_loop_type = OPEN_LOOP,
.close_loop_type = OPEN_LOOP,
.close_loop_type = OPEN_LOOP,//| CURRENT_LOOP,
.angle_feedback_source = MOTOR_FEED,
.speed_feedback_source = MOTOR_FEED,
},
.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);
@ -130,7 +115,6 @@ 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);
@ -138,26 +122,24 @@ void ChassisInit()
//@todo:瓴控驱动代码有些问题 1号电机必须先初始化
wheel_motor_config.can_init_config.tx_id = 1;
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_REVERSE;
wheel_motor_r = 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);
referee_data = UITaskInit(&huart6,&ui_data); // 裁判系统初始化,会同时初始化UI
//超级电容
SuperCap_Init_Config_s cap_conf = {
.can_config = {
.can_handle = &hcan1,
.tx_id = 0x210,
.rx_id = 0x211,
}};
.can_config = {
.can_handle = &hcan2,
.tx_id = 0x302, // 超级电容默认接收id
.rx_id = 0x301, // 超级电容默认发送id,注意tx和rx在其他人看来是反的
}};
cap = SuperCapInit(&cap_conf); // 超级电容初始化
SuperCapSetPower(cap,70); // 超级电容限制功率
PowerMeter_Init_Config_s power_conf = {
.can_config = {
.can_handle = &hcan1,
@ -170,57 +152,57 @@ void ChassisInit()
Balance_Init_config_s balanceInitConfig = {
.legInitConfig = {
.length_PID_conf = {
.Kp = 300,//500,//180 ,//50,
.Ki = 0,//5,
.Kp = 500,//180 ,//50,
.Ki = 100,//5,
.Kd = 20,//10,//6,//6,
.MaxOut = 100,
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement | PID_DerivativeFilter,
.Derivative_LPF_RC = 0.001f,//1/(2*PI*10),
.Improve = static_cast<PID_Improvement_e>(PID_Trapezoid_Intergral | PID_Integral_Limit |
PID_Derivative_On_Measurement |
PID_DerivativeFilter),
.IntegralLimit = 100,
.Derivative_LPF_RC = 0.001f,//1/(2*PI*10),
},
.phi0_PID_conf = {
.Kp = 80,
.Ki = 0,
.Kd = 5,
.MaxOut = 5,
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement| PID_DerivativeFilter,
.Derivative_LPF_RC = 1/(2*PI*10),
.Improve = static_cast<PID_Improvement_e>(PID_Trapezoid_Intergral | PID_Integral_Limit |
PID_Derivative_On_Measurement |
PID_DerivativeFilter),
.IntegralLimit = 100,
.Derivative_LPF_RC = 1/(2*PI*10),
},
.init_target_L0 = 0.15f,
.F_feedforward = 0,
},
.leg_cor_PID_config = {
.Kp = 25.0f,//25,//25,
.Kp = 25,
.Ki = 0,
.Kd = 3.0f,//3,
.Kd = 3,
.MaxOut = 3,
.Improve = PID_Derivative_On_Measurement| PID_DerivativeFilter,
.Improve = static_cast<PID_Improvement_e>(PID_Derivative_On_Measurement | PID_DerivativeFilter),
.Derivative_LPF_RC = 0.01f,
},
.LQR_state_num = 6,
.LQR_control_num = 2,
};
//Leg_Init(&legInstance,&leg_r_init_conf);
Balance_Control_Init(&BalanceControl,balanceInitConfig);
balance_chassis = balance(&balanceInitConfig);
Chassis_IMU_data = INS_Init(); // 底盘IMU初始化
//转向环
PID_Init_Config_s turn_pid_cfg = {
.Kp = 12.0f,//10.0f,//1,
.Kp = 10.0f,//1,
.Ki = 0,
.Kd = 0.8f,
.MaxOut = 5.0f,
.Improve = PID_Derivative_On_Measurement|PID_DerivativeFilter,
.Kd = 0.0f,
.MaxOut = 2,
.Improve = static_cast<PID_Improvement_e>(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 = 12.0f,
.Kp = 8.0f,
.Ki = 0,
.Kd = 0.5f,
.MaxOut = 100,
@ -250,17 +232,6 @@ 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);
}
@ -268,16 +239,14 @@ 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 - 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.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 + 0.81007f);
@ -291,9 +260,11 @@ static void Chassis_State_update()
leg_phi_dot[3] = motor_lb->measure.speed_rads;
float body_phi = (Chassis_IMU_data->Pitch) * DEGREE_2_RAD; //+ 2
float body_phi = Chassis_IMU_data->Pitch * DEGREE_2_RAD;
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;
@ -308,47 +279,22 @@ 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_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 x = (total_angle * DEGREE_2_RAD ) * 0.135f/2;
float speed = (- wheel_motor_r->measure.speed_rads + wheel_motor_l->measure.speed_rads)/2;
float x_dot = speed * RADIUS_WHEEL;
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);
float x_dot = speed * 0.135f/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_chassis.feedback_update(leg_phi,leg_phi_dot,body_phi,body_phi_dot,x,x_dot,Chassis_IMU_data->MotionAccel_n[2]);
//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;
}
/* 机器人底盘控制核心任务 */
@ -363,16 +309,7 @@ void ChassisTask()
chassis_cmd_recv = *(Chassis_Ctrl_Cmd_s *)CANCommGet(chasiss_can_comm);
#endif // CHASSIS_BOARD
//死亡情况下 关闭电机
if(referee_data->GameRobotState.power_management_chassis_output == 0)
chassis_cmd_recv.chassis_mode = CHASSIS_ZERO_FORCE;
//设置超电上限功率
float power_limit = referee_data->GameRobotState.chassis_power_limit;
power_limit = float_constrain(power_limit,40,120);
SuperCapSetPower(cap,power_limit);
if (chassis_cmd_recv.chassis_mode == CHASSIS_ZERO_FORCE)
if (chassis_cmd_recv.chassis_mode == CHASSIS_NO_FOLLOW)
{ // 如果出现重要模块离线或遥控器设置为急停,让电机停止
HTMotorStop(motor_lf);
HTMotorStop(motor_rf);
@ -380,15 +317,6 @@ 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
{ // 正常工作
@ -402,8 +330,6 @@ 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)
{
@ -412,36 +338,10 @@ 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 = 2.0f;
chassis_cmd_recv.wz = 1.0f * chassis_cmd_recv.offset_angle* abs(chassis_cmd_recv.offset_angle);
break;
default:
break;
@ -452,12 +352,9 @@ 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;
first_order_filter_cali(&vx_filter,chassis_cmd_recv.vx/5280 * 3);
chassis_vx = vx_filter.out;
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;
// // 获取裁判系统数据 建议将裁判系统与底盘分离，所以此处数据应使用消息中心发送
// // 我方颜色id小于7是红色,大于7是蓝色,注意这里发送的是对方的颜色, 0:blue , 1:red
@ -468,73 +365,82 @@ 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 turn_T = 0;
//离地情况下把转向环和roll轴补偿关掉
if(BalanceControl.fly_flag_r == FALSE || BalanceControl.fly_flag_l == FALSE)
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 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) // 倒地 未起身
{
float Roll_pid_out = PIDCalculate(&Roll_Loop_PID,-Chassis_IMU_data->Roll,0);
//将roll轴自平衡PID输出直接叠加到前馈支持力上
BalanceControl.leg_right.F_feedforward = Roll_pid_out;
BalanceControl.leg_left.F_feedforward = -Roll_pid_out;
turn_T = PIDCalculate(&Turn_Loop_PID,Chassis_IMU_data->Gyro[Z],chassis_cmd_recv.wz);
HTMotorSetRef(motor_rf,0);
HTMotorSetRef(motor_rb,0);
HTMotorSetRef(motor_lf,0);
HTMotorSetRef(motor_lb,0);
PIDClear(&BalanceControl.leg_length_PID);
PIDClear(&Turn_Loop_PID);
PIDClear(&Roll_Loop_PID);
PIDClear(&BalanceControl.leg_coordination_PID);
}
else
{
turn_T = 0;
BalanceControl.leg_right.F_feedforward = 0;
BalanceControl.leg_left.F_feedforward = 0;
}
HTMotorSetRef(motor_rf,-BalanceControl.leg_right.legOutput.T1 * HT_CMD_COEF);
HTMotorSetRef(motor_rb,-BalanceControl.leg_right.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_lf,BalanceControl.leg_left.legOutput.T1 * HT_CMD_COEF);
HTMotorSetRef(motor_lb,BalanceControl.leg_left.legOutput.T2 * HT_CMD_COEF);
// HTMotorSetRef(motor_rf,0);
// HTMotorSetRef(motor_rb,0);
//
// HTMotorSetRef(motor_lf,0);
// HTMotorSetRef(motor_lb,0);
}
LKMotorSetRef(wheel_motor_r,wheel_current_r);
LKMotorSetRef(wheel_motor_l,wheel_current_l);
// LKMotorSetRef(wheel_motor_r,0);
// LKMotorSetRef(wheel_motor_l,0);
// LKMotorSetRef(wheel_motor_r,100);
// LKMotorSetRef(wheel_motor_l,100);
// 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);
@ -543,31 +449,11 @@ 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

2
application/chassis/chassis.h
View File

@ -13,6 +13,8 @@ void ChassisInit();
*/
void ChassisTask();
typedef enum
{
FAIL = 0,

29
application/balance/leg.c → application/chassis/leg_old.c
View File

@ -2,7 +2,7 @@
// Created by SJQ on 2023/12/25.
//
#include "leg.h"
#include "leg_old.h"
#include "math.h"
#include "main.h"
#include "arm_math.h"
@ -52,22 +52,16 @@ 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)
@ -120,11 +114,8 @@ 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;
leg_state->phi3 = phi3;
}
void VMC_getT(LegInstance* legInstance)

81
application/chassis/leg_old.h Normal file
View File

@ -0,0 +1,81 @@
//
// 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

78
application/balance/observer.c → application/chassis/observer.c
View File

@ -7,23 +7,22 @@
#include "arm_math.h"
static const float dt = 1e-3;
static const float p_A[6][3] = {
{-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}
{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}
};
static const float p_B[6][3] = {
{-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},
{-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}
};
@ -45,47 +44,19 @@ 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 = 1;
//kf->SkipEq2 = 1; kf->SkipEq3 = 1; kf->SkipEq4 = 1; kf->SkipEq5 = 1;
kf->UseAutoAdjustment = 0;
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);
}
@ -111,22 +82,11 @@ 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);
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];
memcpy(kf->MeasuredVector,zk,6*4 );
//测量值更新
memcpy(kf->ControlVector,uk,2*4);
Kalman_Filter_Update(kf);
// 提取估计值
for (uint8_t i = 0; i < 2; i++)
{
observer->Estimate_X[i] = kf->FilteredValue[i];
}
memcpy(observer->Estimate_X,kf->xhatminus_data,6*4);
}

0
application/balance/observer.h → application/chassis/observer.h
View File

2
application/cmd/robot_cmd.c
View File

@ -73,7 +73,7 @@ void RobotCMDInit()
#endif // GIMBAL_BOARD
gimbal_cmd_send.pitch = 0;
robot_state = ROBOT_STOP; // 轮腿 上电后进入ROBOT_STOP 保证安全(对应倒地状态）
robot_state = ROBOT_READY; // 启动时机器人进入工作模式,后续加入所有应用初始化完成之后再进入
}
/**

0
application/robot.c → application/robot.cpp
View File

21
application/robot_def.h
View File

@ -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.1f // 轮子半径
#define RADIUS_WHEEL 0.06f // 轮子半径
#define REDUCTION_RATIO_WHEEL 19.0f // 电机减速比,因为编码器量测的是转子的速度而不是输出轴的速度故需进行转换
#define GYRO2GIMBAL_DIR_YAW 1 // 陀螺仪数据相较于云台的yaw的方向,1为相同,-1为相反
@ -86,7 +86,6 @@ typedef enum
CHASSIS_ROTATE, // 小陀螺模式
CHASSIS_NO_FOLLOW, // 不跟随，允许全向平移
CHASSIS_FOLLOW_GIMBAL_YAW, // 跟随模式，底盘叠加角度环控制
CHASSIS_LATERAL, // 侧向对敌
} chassis_mode_e;
// 云台模式设置
@ -128,8 +127,6 @@ typedef enum
typedef struct
{ // 功率控制
float chassis_power_mx;
float cap_vol;
float input_vol;
} Chassis_Power_Data_s;
/* ----------------CMD应用发布的控制数据,应当由gimbal/chassis/shoot订阅---------------- */
@ -145,15 +142,10 @@ 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;
@ -193,11 +185,12 @@ 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;

31
modules/algorithm/user_lib.c
View File

@ -253,34 +253,3 @@ 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;
}

14
modules/algorithm/user_lib.h
View File

@ -58,20 +58,6 @@ 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

4
modules/motor/HTmotor/HT04.c
View File

@ -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 % 100 != 0)
if (++motor->lost_cnt % 10 != 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 = 100,//20,//5, // 20ms
.reload_count = 20,//5, // 20ms
};
motor->motor_daemon = DaemonRegister(&conf);

6
modules/motor/LKmotor/LK9025.c
View File

@ -143,18 +143,18 @@ void LKMotorControl()
if ((setting->close_loop_type & SPEED_LOOP) && setting->outer_loop_type & (ANGLE_LOOP | SPEED_LOOP))
{
if (setting->speed_feedback_source == OTHER_FEED)
if (setting->angle_feedback_source == OTHER_FEED)
pid_measure = *motor->other_speed_feedback_ptr;
else
pid_measure = measure->speed_rads;
pid_ref = PIDCalculate(&motor->speed_PID, pid_measure, pid_ref);
pid_ref = PIDCalculate(&motor->angle_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;

6
modules/referee/referee_UI.c
View File

@ -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 x_Length, uint32_t y_Length)
uint32_t end_x, uint32_t end_y)
{
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 = x_Length;
graph->end_y = y_Length;
graph->end_x = end_x;
graph->end_y = end_y;
}
/************************************************绘制浮点型数据*************************************************

2
modules/referee/referee_UI.h
View File

@ -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 x_Length, uint32_t y_Length);
uint32_t end_x, uint32_t end_y);
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);

61
modules/referee/referee_protocol.h
View File

@ -83,8 +83,6 @@ typedef enum
ID_robot_hurt = 0x0206, // 伤害状态数据
ID_shoot_data = 0x0207, // 实时射击数据
ID_student_interactive = 0x0301, // 机器人间交互数据
ID_custom_robot = 0x302, // 自定义控制器数据（图传链路）
ID_remote_control = 0x304, // 键鼠遥控数据（图传链路）
} CmdID_e;
/* 命令码数据段长,根据官方协议来定义长度，还有自定义数据长度 */
@ -104,8 +102,6 @@ typedef enum
LEN_shoot_data = 7, // 0x0207
LEN_receive_data = 6 + Communicate_Data_LEN, // 0x0301
LEN_remote_control = 12, // 0x0304
} JudgeDataLength_e;
/****************************接收数据的详细说明****************************/
@ -161,31 +157,37 @@ typedef struct
uint8_t supply_projectile_num;
} ext_supply_projectile_action_t;
/* ID: 0X0201 Byte: 13 机器人性能体系数据 */
/* ID: 0X0201 Byte: 27 机器人状态数据 */
typedef struct
{
uint8_t robot_id;
uint8_t robot_level;
uint16_t current_HP;
uint16_t maximum_HP;
uint16_t shooter_barrel_cooling_value;
uint16_t shooter_barrel_heat_limit;
uint16_t chassis_power_limit;
uint8_t power_management_gimbal_output : 1;
uint8_t power_management_chassis_output : 1;
uint8_t power_management_shooter_output : 1;
uint8_t robot_id;
uint8_t robot_level;
uint16_t remain_HP;
uint16_t max_HP;
uint16_t shooter_id1_17mm_cooling_rate;
uint16_t shooter_id1_17mm_cooling_limit;
uint16_t shooter_id1_17mm_speed_limit;
uint16_t shooter_id2_17mm_cooling_rate;
uint16_t shooter_id2_17mm_cooling_limit;
uint16_t shooter_id2_17mm_speed_limit;
uint16_t shooter_id1_42mm_cooling_rate;
uint16_t shooter_id1_42mm_cooling_limit;
uint16_t shooter_id1_42mm_speed_limit;
uint16_t chassis_power_limit;
uint8_t mains_power_gimbal_output : 1;
uint8_t mains_power_chassis_output : 1;
uint8_t mains_power_shooter_output : 1;
} ext_game_robot_state_t;
/* ID: 0X0202 Byte: 16 实时功率热量数据 */
/* ID: 0X0202 Byte: 14 实时功率热量数据 */
typedef struct
{
uint16_t chassis_voltage;
uint16_t chassis_current;
float chassis_power;
uint16_t buffer_energy;
uint16_t shooter_17mm_1_barrel_heat;
uint16_t shooter_17mm_2_barrel_heat;
uint16_t shooter_42mm_barrel_heat;
uint16_t chassis_volt;
uint16_t chassis_current;
float chassis_power; // 瞬时功率
uint16_t chassis_power_buffer; // 60焦耳缓冲能量
uint16_t shooter_heat0; // 17mm
uint16_t shooter_heat1;
} ext_power_heat_data_t;
/* ID: 0x0203 Byte: 16 机器人位置数据 */
@ -225,19 +227,6 @@ 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个，也不会超*/

146
modules/referee/referee_task.c
View File

@ -14,7 +14,6 @@
#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; // 接收到的裁判系统数据
@ -40,14 +39,6 @@ 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); // 初始化裁判系统的串口,并返回裁判系统反馈数据指针
@ -58,28 +49,20 @@ 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[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 Graph_Data_t UI_Energy[3]; // 电容能量条
static String_Data_t UI_State_sta[6]; // 机器人状态,静态只需画一次
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); // 若还未收到裁判系统数据,等待一段时间后再检查
@ -120,13 +103,8 @@ void MyUIInit()
UICharRefresh(&referee_recv_info->referee_id, UI_State_dyn[4]);
// 底盘功率显示，静态
UICharDraw(&UI_State_sta[5], "ss5", UI_Graph_ADD, 8, UI_Color_Green, 18, 2, 620, 230, "Power:");
UICharDraw(&UI_State_sta[5], "ss5", UI_Graph_ADD, 7, 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]);
@ -136,43 +114,6 @@ 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]);
}
// 测试用函数，实现模式自动变化,用于检查该任务和裁判系统是否连接正常
@ -306,81 +247,10 @@ 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, 1, UI_Energy[1]);
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]);
_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]);
}
/**

3
modules/referee/rm_referee.c
View File

@ -95,9 +95,6 @@ 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;
}
}
}

6
modules/referee/rm_referee.h
View File

@ -38,8 +38,6 @@ typedef struct
ext_robot_hurt_t RobotHurt; // 0x0206
ext_shoot_data_t ShootData; // 0x0207
remote_control_t RemoteControl; // 0x304
// 自定义交互数据的接收
Communicate_ReceiveData_t ReceiveData;
@ -70,10 +68,6 @@ 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;

26
modules/super_cap/super_cap.c
View File

@ -1,8 +1,14 @@
/*
* @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)
@ -11,17 +17,16 @@ static void SuperCapRxCallback(CANInstance *_instance)
SuperCap_Msg_s *Msg;
rxbuff = _instance->rx_buff;
Msg = &super_cap_instance->cap_msg;
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]);
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]);
}
SuperCapInstance *SuperCapInit(SuperCap_Init_Config_s *supercap_config)
{
super_cap_instance = (SuperCapInstance *)malloc(sizeof(SuperCapInstance));
memset(super_cap_instance, 0, sizeof(SuperCapInstance));
supercap_config->can_config.can_module_callback = SuperCapRxCallback;
super_cap_instance->can_ins = CANRegister(&supercap_config->can_config);
return super_cap_instance;
@ -33,15 +38,6 @@ 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;

26
modules/super_cap/super_cap.h
View File

@ -1,3 +1,10 @@
/*
* @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
@ -6,10 +13,9 @@
#pragma pack(1)
typedef struct
{
int16_t input_vol; // 输入电压
int16_t cap_vol; // 电容电压
int16_t input_cur; // 输入电流
int16_t power_set; // 设定功率
uint16_t vol; // 电压
uint16_t current; // 电流
uint16_t power; // 功率
} SuperCap_Msg_s;
#pragma pack()
@ -28,7 +34,7 @@ typedef struct
/**
* @brief 初始化超级电容
*
*
* @param supercap_config 超级电容初始化配置
* @return SuperCapInstance* 超级电容实例指针
*/
@ -36,18 +42,10 @@ SuperCapInstance *SuperCapInit(SuperCap_Init_Config_s *supercap_config);
/**
* @brief 发送超级电容控制信息
*
*
* @param instance 超级电容实例
* @param data 超级电容控制信息
*/
void SuperCapSend(SuperCapInstance *instance, uint8_t *data);
/**
* @brief 发送超级电容目标功率
*
* @param instance 超级电容实例
* @param power_set 超级电容目标功率
*/
void SuperCapSetPower(SuperCapInstance *instance, float power_set);
#endif // !SUPER_CAP_Hd