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

基本平衡控制框架完成，能跑了

This commit is contained in:
宋家齐 2024-01-10 18:27:25 +08:00
parent 2c0a44e200
commit ead61a9880
20 changed files with 1297 additions and 198 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

136
application/chassis/balance.c Normal file
View File

@ -0,0 +1,136 @@
//
// Created by SJQ on 2023/12/30.
//
#include "balance.h"
static void mylqr_k(float L0, float K[12]);
void Balance_Control_Init(Balance_Control_t* balanceControl,Balance_Init_config_s InitConfig)
{
Leg_Init(&balanceControl->leg_right,&InitConfig.legInitConfig);
Leg_Init(&balanceControl->leg_left,&InitConfig.legInitConfig);
LQR_Init(&balanceControl->balance_LQR,InitConfig.LQR_state_num,InitConfig.LQR_control_num);
PIDInit(&balanceControl->leg_coordination_PID,&InitConfig.leg_cor_PID_config);
memset(balanceControl->state,0,6);
memset(balanceControl->target_state,0,6);
}
// leg_phi[4] = {phi1_r,phi4_r,phi1_l,phi4_l}
void Balance_feedback_update(Balance_Control_t* balanceControl,float leg_phi[4],float leg_phi_dot[4],float body_phi,float body_phi_dot,float x,float x_dot)
{
Leg_State_update(&balanceControl->leg_right,leg_phi[0],leg_phi[1],leg_phi_dot[0],leg_phi_dot[1]);
Leg_State_update(&balanceControl->leg_left,leg_phi[2],leg_phi[3],leg_phi_dot[2],leg_phi_dot[3]);
float K_matrix[12]={0};
float L_average = balanceControl->leg_right.legState.L0;
//(balanceControl->leg_right.legState.L0 + balanceControl->leg_left.legState.L0)/2;
float alpha = PI/2 - balanceControl->leg_right.legState.phi0;
float theta = alpha - body_phi;
// static uint32_t dot_cnt = 0;
// float dot_dt = DWT_GetDeltaT(&dot_cnt);
// float theta_dot1 = (balanceControl->state[0] - theta) / dot_dt;
float theta_dot = - balanceControl->leg_right.legState.phi0_dot - body_phi_dot;
balanceControl->state[0] = theta;
balanceControl->state[1] = theta_dot;
balanceControl->state[2] = x; balanceControl->state[3] = x_dot;
balanceControl->state[4] = body_phi; balanceControl->state[5] = body_phi_dot;
mylqr_k(L_average,K_matrix);
float K_matrix_fix[12];
for (int i = 0; i < 6; ++i) {
K_matrix_fix[i] = K_matrix[2*i];
}
for (int i = 0; i < 6; ++i) {
K_matrix_fix[6+i] = K_matrix[2*i+1];
}
LQR_set_K(&balanceControl->balance_LQR,K_matrix_fix);
}
void Balance_Control_Loop(Balance_Control_t* balanceControl)
{
LQR_update(&balanceControl->balance_LQR,balanceControl->state,balanceControl->target_state);
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 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_r+leg_r->F_feedforward,Tp + cor_Tp);
Leg_Input_update(&leg_l->legInput,F_pid_out_l+leg_l->F_feedforward,Tp - cor_Tp);
VMC_getT(leg_r);
VMC_getT(leg_l);
}
void target_x_set(Balance_Control_t* balanceControl,float x)
{
balanceControl->target_state [2] = x;
}
/*
* MYLQR_K MATLAB生成
* K = MYLQR_K(L0)
*
* Arguments : float L0
* float K[12]
* Return Type : void
*/
void mylqr_k(float L0, float K[12])
{
float t2;
float t3;
/* This function was generated by the Symbolic Math Toolbox version 9.2.
*/
/* 10-Jan-2024 15:16:33 */
t2 = L0 * L0;
t3 = t2 * L0;
K[0] = ((L0 * -79.173111F - t2 * 4.61935F) + t3 * 53.7748F) - 1.68270767F;
K[1] =
((L0 * 163.861588F - t2 * 454.401855F) + t3 * 423.311035F) + 0.115447611F;
K[2] = ((L0 * -5.08536911F - t2 * 30.8126068F) + t3 * 29.5321121F) -
0.590248764F;
K[3] =
((L0 * 14.4037905F - t2 * 24.0141125F) + t3 * 13.8384714F) + 0.648175836F;
K[4] =
((L0 * 10.8009977F - t2 * 72.3231735F) + t3 * 103.75676F) - 5.87192822F;
K[5] =
((L0 * 48.2422218F - t2 * 312.532166F) + t3 * 420.993896F) + 12.3104877F;
K[6] =
((L0 * 22.1635437F - t2 * 126.369873F) + t3 * 170.583176F) - 7.8162446F;
K[7] = ((L0 * 26.8571F - t2 * 228.328079F) + t3 * 316.050446F) + 14.7130146F;
K[8] =
((L0 * 15.8044338F - t2 * 196.455856F) + t3 * 276.879242F) + 20.4759655F;
K[9] =
((L0 * -80.1292953F + t2 * 698.451477F) - t3 * 1040.30969F) + 51.8307953F;
K[10] =
((L0 * -2.62232423F - t2 * 4.29178047F) + t3 * 7.45553255F) + 4.33007193F;
K[11] =
((L0 * -20.6260967F + t2 * 144.286728F) - t3 * 203.665939F) + 2.42666435F;
}
/*
* File trailer for mylqr_k.c
*
* [EOF]
*/

39
application/chassis/balance.h Normal file
View File

@ -0,0 +1,39 @@
//
// Created by SJQ on 2023/12/30.
//
#ifndef WHEEL_LEGGED_BALANCE_H
#define WHEEL_LEGGED_BALANCE_H
#include "leg.h"
#include "LQR.h"
#include "controller.h"
typedef struct
{
LegInstance leg_right;
LegInstance leg_left;
PIDInstance leg_coordination_PID; //双腿协调PD控制
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);
void Balance_Control_Loop(Balance_Control_t* balanceControl);
void target_x_set(Balance_Control_t* balanceControl,float x);
#endif //WHEEL_LEGGED_BALANCE_H

626
application/chassis/cal_phi0_dot.c Normal file
View File

@ -0,0 +1,626 @@
//
// Created by SJQ on 2024/1/3.
//
#include "controller.h"
#include "cal_phi0_dot.h"
#include "arm_math.h"
/* Function Definitions */
/*
* Arguments : float phi_1
* float phi_4
* float d_phi1
* float d_phi4
* Return Type : float
*/
float get_dphi0(float phi_1, float phi_4, float d_phi1, float d_phi4)
{
float a;
float a_tmp;
float ab_a;
float ac_a;
float ad_a;
float b_a;
float b_a_tmp;
float b_d_phi0_tmp;
float b_d_phi0_tmp_tmp;
float b_out;
float bb_a;
float bc_a;
float bd_a;
float c_a;
float c_d_phi0_tmp;
float c_d_phi0_tmp_tmp;
float c_out;
float cb_a;
float cc_a;
float cd_a;
float d_a;
float d_d_phi0_tmp;
float d_d_phi0_tmp_tmp;
float d_out;
float d_phi0_tmp;
float d_phi0_tmp_tmp;
float db_a;
float dc_a;
float dd_a;
float e_a;
float e_d_phi0_tmp;
float e_d_phi0_tmp_tmp;
float e_out;
float eb_a;
float ec_a;
float ed_a;
float f_a;
float f_d_phi0_tmp;
float f_out;
float fb_a;
float fc_a;
float fd_a;
float g_a;
float g_d_phi0_tmp;
float g_out;
float gb_a;
float gc_a;
float gd_a;
float h_a;
float h_d_phi0_tmp;
float h_out;
float hb_a;
float hc_a;
float i_a;
float i_d_phi0_tmp;
float i_out;
float ib_a;
float ic_a;
float j_a;
float j_d_phi0_tmp;
float j_out;
float jb_a;
float jc_a;
float k_a;
float k_d_phi0_tmp;
float k_out;
float kb_a;
float kc_a;
float l_a;
float l_d_phi0_tmp;
float l_out;
float lb_a;
float lc_a;
float m_a;
float m_out;
float mb_a;
float mc_a;
float n_a;
float n_out;
float nb_a;
float nc_a;
float o_a;
float o_out;
float ob_a;
float oc_a;
float out;
float p_a;
float p_out;
float pb_a;
float pc_a;
float q_a;
float q_out;
float qb_a;
float qc_a;
float r_a;
float rb_a;
float rc_a;
float s_a;
float sb_a;
float sc_a;
float t_a;
float tb_a;
float tc_a;
float u_a;
float ub_a;
float uc_a;
float v_a;
float vb_a;
float vc_a;
float w_a;
float wb_a;
float wc_a;
float x_a;
float xb_a;
float xc_a;
float y_a;
float yb_a;
float yc_a;
d_phi0_tmp = arm_cos_f32(phi_1);
d_phi0_tmp_tmp = arm_sin_f32(phi_1);
b_d_phi0_tmp_tmp = 0.15F * d_phi0_tmp_tmp;
c_d_phi0_tmp_tmp = arm_sin_f32(phi_4);
b_d_phi0_tmp = b_d_phi0_tmp_tmp - 0.15F * c_d_phi0_tmp_tmp;
d_d_phi0_tmp_tmp = 0.15F * d_phi0_tmp;
e_d_phi0_tmp_tmp = arm_cos_f32(phi_4);
c_d_phi0_tmp = (0.15F - d_d_phi0_tmp_tmp) + 0.15F * e_d_phi0_tmp_tmp;
d_d_phi0_tmp = 0.54F * c_d_phi0_tmp;
e_d_phi0_tmp = 0.54F * b_d_phi0_tmp;
f_d_phi0_tmp = 0.3F * d_phi0_tmp * b_d_phi0_tmp;
g_d_phi0_tmp = 0.3F * d_phi0_tmp_tmp * c_d_phi0_tmp;
h_d_phi0_tmp = 2.0F * (f_d_phi0_tmp + g_d_phi0_tmp);
i_d_phi0_tmp = 0.0874800086F * d_phi0_tmp * b_d_phi0_tmp;
j_d_phi0_tmp = 0.0874800086F * d_phi0_tmp_tmp * c_d_phi0_tmp;
d_phi0_tmp *= 0.0810000077F;
f_d_phi0_tmp = (0.0810000077F * d_phi0_tmp_tmp + f_d_phi0_tmp) + g_d_phi0_tmp;
g_d_phi0_tmp = 0.3F * e_d_phi0_tmp_tmp * b_d_phi0_tmp;
d_phi0_tmp_tmp = 0.3F * c_d_phi0_tmp_tmp * c_d_phi0_tmp;
k_d_phi0_tmp = 2.0F * (g_d_phi0_tmp + d_phi0_tmp_tmp);
b_d_phi0_tmp *= 0.0874800086F * e_d_phi0_tmp_tmp;
c_d_phi0_tmp *= 0.0874800086F * c_d_phi0_tmp_tmp;
l_d_phi0_tmp = 0.0810000077F * e_d_phi0_tmp_tmp;
g_d_phi0_tmp =
(0.0810000077F * c_d_phi0_tmp_tmp + g_d_phi0_tmp) + d_phi0_tmp_tmp;
d_phi0_tmp_tmp = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
e_d_phi0_tmp_tmp = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
c_d_phi0_tmp_tmp = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
out = (e_d_phi0_tmp_tmp * e_d_phi0_tmp_tmp +
c_d_phi0_tmp_tmp * c_d_phi0_tmp_tmp) +
0.0729000047F;
e_d_phi0_tmp_tmp = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
c_d_phi0_tmp_tmp = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
b_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
c_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
d_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
e_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
f_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
b_out = (e_a * e_a + f_a * f_a) + 0.0729000047F;
e_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
f_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
g_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
h_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
i_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
j_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
c_out = (i_a * i_a + j_a * j_a) + 0.0729000047F;
i_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
j_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
k_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
a_tmp = 0.54F * ((0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4));
j_a = (((j_a * j_a + k_a * k_a) + 0.0729000047F) - 0.0729000047F) + a_tmp;
k_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
l_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
m_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
d_out = (l_a * l_a + m_a * m_a) + 0.0729000047F;
l_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
b_a_tmp = 0.54F * (0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4));
k_a = sqrtf((0.291600019F * (k_a * k_a) -
(d_out - 0.0729000047F) * (d_out - 0.0729000047F)) +
0.291600019F * (l_a * l_a)) -
b_a_tmp;
l_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
m_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
l_a = (((l_a * l_a + m_a * m_a) + 0.0729000047F) - 0.0729000047F) + a_tmp;
m_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
n_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
o_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
d_out = (n_a * n_a + o_a * o_a) + 0.0729000047F;
n_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
o_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
p_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
q_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
r_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
s_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
e_out = (r_a * r_a + s_a * s_a) + 0.0729000047F;
r_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
s_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
t_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
u_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
v_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
w_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
f_out = (v_a * v_a + w_a * w_a) + 0.0729000047F;
v_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
w_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
x_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
y_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
ab_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
bb_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
cb_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
db_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
g_out = (cb_a * cb_a + db_a * db_a) + 0.0729000047F;
cb_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
db_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
eb_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
fb_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
gb_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
hb_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
h_out = (gb_a * gb_a + hb_a * hb_a) + 0.0729000047F;
gb_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
hb_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
ib_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
hb_a =
(((hb_a * hb_a + ib_a * ib_a) + 0.0729000047F) - 0.0729000047F) + a_tmp;
ib_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
jb_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
kb_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
i_out = (jb_a * jb_a + kb_a * kb_a) + 0.0729000047F;
jb_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
ib_a = sqrtf((0.291600019F * (ib_a * ib_a) -
(i_out - 0.0729000047F) * (i_out - 0.0729000047F)) +
0.291600019F * (jb_a * jb_a)) -
b_a_tmp;
jb_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
kb_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
jb_a =
(((jb_a * jb_a + kb_a * kb_a) + 0.0729000047F) - 0.0729000047F) + a_tmp;
kb_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
lb_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
mb_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
i_out = (lb_a * lb_a + mb_a * mb_a) + 0.0729000047F;
lb_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
mb_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
nb_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
kb_a = (0.27F * arm_cos_f32(2.0F *
atanf((sqrtf((0.291600019F * (kb_a * kb_a) -
(i_out - 0.0729000047F) *
(i_out - 0.0729000047F)) +
0.291600019F * (lb_a * lb_a)) -
b_a_tmp) /
((((mb_a * mb_a + nb_a * nb_a) + 0.0729000047F) -
0.0729000047F) +
a_tmp))) -
0.075F) +
0.15F * arm_cos_f32(phi_1);
lb_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
mb_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
nb_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
i_out = (mb_a * mb_a + nb_a * nb_a) + 0.0729000047F;
mb_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
nb_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
ob_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
lb_a = 0.27F *
arm_sin_f32(2.0F * atanf((sqrtf((0.291600019F * (lb_a * lb_a) -
(i_out - 0.0729000047F) *
(i_out - 0.0729000047F)) +
0.291600019F * (mb_a * mb_a)) -
b_a_tmp) /
((((nb_a * nb_a + ob_a * ob_a) + 0.0729000047F) -
0.0729000047F) +
a_tmp))) +
0.15F * arm_sin_f32(phi_1);
mb_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
nb_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
ob_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
i_out = (nb_a * nb_a + ob_a * ob_a) + 0.0729000047F;
nb_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
ob_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
pb_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
mb_a = (0.27F * arm_cos_f32(2.0F *
atanf((sqrtf((0.291600019F * (mb_a * mb_a) -
(i_out - 0.0729000047F) *
(i_out - 0.0729000047F)) +
0.291600019F * (nb_a * nb_a)) -
b_a_tmp) /
((((ob_a * ob_a + pb_a * pb_a) + 0.0729000047F) -
0.0729000047F) +
a_tmp))) -
0.075F) +
0.15F * arm_cos_f32(phi_1);
nb_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
ob_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
pb_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
i_out = (ob_a * ob_a + pb_a * pb_a) + 0.0729000047F;
ob_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
pb_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
qb_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
rb_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
sb_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
tb_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
ub_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
vb_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
j_out = (ub_a * ub_a + vb_a * vb_a) + 0.0729000047F;
ub_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
vb_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
wb_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
xb_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
yb_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
ac_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
k_out = (yb_a * yb_a + ac_a * ac_a) + 0.0729000047F;
yb_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
ac_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
bc_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
ac_a =
(((ac_a * ac_a + bc_a * bc_a) + 0.0729000047F) - 0.0729000047F) + a_tmp;
bc_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
cc_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
dc_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
l_out = (cc_a * cc_a + dc_a * dc_a) + 0.0729000047F;
cc_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
bc_a = sqrtf((0.291600019F * (bc_a * bc_a) -
(l_out - 0.0729000047F) * (l_out - 0.0729000047F)) +
0.291600019F * (cc_a * cc_a)) -
b_a_tmp;
cc_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
dc_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
cc_a =
(((cc_a * cc_a + dc_a * dc_a) + 0.0729000047F) - 0.0729000047F) + a_tmp;
dc_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
ec_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
fc_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
l_out = (ec_a * ec_a + fc_a * fc_a) + 0.0729000047F;
ec_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
fc_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
gc_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
hc_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
ic_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
jc_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
m_out = (ic_a * ic_a + jc_a * jc_a) + 0.0729000047F;
ic_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
jc_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
kc_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
lc_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
mc_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
nc_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
n_out = (mc_a * mc_a + nc_a * nc_a) + 0.0729000047F;
mc_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
nc_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
oc_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
pc_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
qc_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
rc_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
sc_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
tc_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
o_out = (sc_a * sc_a + tc_a * tc_a) + 0.0729000047F;
sc_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
tc_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
uc_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
vc_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
wc_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
xc_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
p_out = (wc_a * wc_a + xc_a * xc_a) + 0.0729000047F;
wc_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
xc_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
yc_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
xc_a =
(((xc_a * xc_a + yc_a * yc_a) + 0.0729000047F) - 0.0729000047F) + a_tmp;
yc_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
ad_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
bd_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
q_out = (ad_a * ad_a + bd_a * bd_a) + 0.0729000047F;
ad_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
yc_a = sqrtf((0.291600019F * (yc_a * yc_a) -
(q_out - 0.0729000047F) * (q_out - 0.0729000047F)) +
0.291600019F * (ad_a * ad_a)) -
b_a_tmp;
ad_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
bd_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
ad_a =
(((ad_a * ad_a + bd_a * bd_a) + 0.0729000047F) - 0.0729000047F) + a_tmp;
bd_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
cd_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
dd_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
q_out = (cd_a * cd_a + dd_a * dd_a) + 0.0729000047F;
cd_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
dd_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
ed_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
bd_a = (0.27F * arm_cos_f32(2.0F *
atanf((sqrtf((0.291600019F * (bd_a * bd_a) -
(q_out - 0.0729000047F) *
(q_out - 0.0729000047F)) +
0.291600019F * (cd_a * cd_a)) -
b_a_tmp) /
((((dd_a * dd_a + ed_a * ed_a) + 0.0729000047F) -
0.0729000047F) +
a_tmp))) -
0.075F) +
0.15F * arm_cos_f32(phi_1);
cd_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
dd_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
ed_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
q_out = (dd_a * dd_a + ed_a * ed_a) + 0.0729000047F;
dd_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
ed_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
fd_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
cd_a = 0.27F *
arm_sin_f32(2.0F * atanf((sqrtf((0.291600019F * (cd_a * cd_a) -
(q_out - 0.0729000047F) *
(q_out - 0.0729000047F)) +
0.291600019F * (dd_a * dd_a)) -
b_a_tmp) /
((((ed_a * ed_a + fd_a * fd_a) + 0.0729000047F) -
0.0729000047F) +
a_tmp))) +
0.15F * arm_sin_f32(phi_1);
dd_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
ed_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
fd_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
q_out = (ed_a * ed_a + fd_a * fd_a) + 0.0729000047F;
ed_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
fd_a = 0.15F * arm_sin_f32(phi_1) - 0.15F * arm_sin_f32(phi_4);
gd_a = (0.15F - 0.15F * arm_cos_f32(phi_1)) + 0.15F * arm_cos_f32(phi_4);
dd_a = (0.27F * arm_cos_f32(2.0F *
atanf((sqrtf((0.291600019F * (dd_a * dd_a) -
(q_out - 0.0729000047F) *
(q_out - 0.0729000047F)) +
0.291600019F * (ed_a * ed_a)) -
b_a_tmp) /
((((fd_a * fd_a + gd_a * gd_a) + 0.0729000047F) -
0.0729000047F) +
a_tmp))) -
0.075F) +
0.15F * arm_cos_f32(phi_1);
return d_phi1 *
((d_d_phi0_tmp_tmp +
0.54F *
arm_cos_f32(2.0F *
atanf((sqrtf((0.291600019F *
(d_phi0_tmp_tmp * d_phi0_tmp_tmp) -
(out - 0.0729000047F) *
(out - 0.0729000047F)) +
0.291600019F * (e_d_phi0_tmp_tmp *
e_d_phi0_tmp_tmp)) -
e_d_phi0_tmp) /
((((c_d_phi0_tmp_tmp * c_d_phi0_tmp_tmp + a * a) +
0.0729000047F) -
0.0729000047F) +
d_d_phi0_tmp))) *
((((i_d_phi0_tmp - h_d_phi0_tmp * (((b_a * b_a + c_a * c_a) +
0.0729000047F) -
0.0729000047F)) +
j_d_phi0_tmp) /
(2.0F * sqrtf((0.291600019F * (d_a * d_a) -
(b_out - 0.0729000047F) *
(b_out - 0.0729000047F)) +
0.291600019F * (e_a * e_a))) -
d_phi0_tmp) /
((((f_a * f_a + g_a * g_a) + 0.0729000047F) -
0.0729000047F) +
d_d_phi0_tmp) -
(sqrtf((0.291600019F * (h_a * h_a) -
(c_out - 0.0729000047F) * (c_out - 0.0729000047F)) +
0.291600019F * (i_a * i_a)) -
e_d_phi0_tmp) *
f_d_phi0_tmp / (j_a * j_a)) /
(k_a * k_a / (l_a * l_a) + 1.0F)) /
((0.27F *
arm_cos_f32(2.0F *
atanf((sqrtf((0.291600019F * (m_a * m_a) -
(d_out - 0.0729000047F) *
(d_out - 0.0729000047F)) +
0.291600019F * (n_a * n_a)) -
e_d_phi0_tmp) /
((((o_a * o_a + p_a * p_a) + 0.0729000047F) -
0.0729000047F) +
d_d_phi0_tmp))) -
0.075F) +
d_d_phi0_tmp_tmp) +
(0.27F * arm_sin_f32(2.0F *
atanf((sqrtf((0.291600019F * (q_a * q_a) -
(e_out - 0.0729000047F) *
(e_out - 0.0729000047F)) +
0.291600019F * (r_a * r_a)) -
e_d_phi0_tmp) /
((((s_a * s_a + t_a * t_a) + 0.0729000047F) -
0.0729000047F) +
d_d_phi0_tmp))) +
b_d_phi0_tmp_tmp) *
(b_d_phi0_tmp_tmp +
0.54F *
arm_sin_f32(2.0F *
atanf((sqrtf((0.291600019F * (u_a * u_a) -
(f_out - 0.0729000047F) *
(f_out - 0.0729000047F)) +
0.291600019F * (v_a * v_a)) -
e_d_phi0_tmp) /
((((w_a * w_a + x_a * x_a) + 0.0729000047F) -
0.0729000047F) +
d_d_phi0_tmp))) *
((((i_d_phi0_tmp -
h_d_phi0_tmp *
(((y_a * y_a + ab_a * ab_a) + 0.0729000047F) -
0.0729000047F)) +
j_d_phi0_tmp) /
(2.0F * sqrtf((0.291600019F * (bb_a * bb_a) -
(g_out - 0.0729000047F) *
(g_out - 0.0729000047F)) +
0.291600019F * (cb_a * cb_a))) -
d_phi0_tmp) /
((((db_a * db_a + eb_a * eb_a) + 0.0729000047F) -
0.0729000047F) +
d_d_phi0_tmp) -
(sqrtf((0.291600019F * (fb_a * fb_a) -
(h_out - 0.0729000047F) *
(h_out - 0.0729000047F)) +
0.291600019F * (gb_a * gb_a)) -
e_d_phi0_tmp) *
f_d_phi0_tmp / (hb_a * hb_a)) /
(ib_a * ib_a / (jb_a * jb_a) + 1.0F)) /
(kb_a * kb_a)) /
(lb_a * lb_a / (mb_a * mb_a) + 1.0F) -
d_phi4 *
(0.54F *
arm_cos_f32(2.0F *
atanf((sqrtf((0.291600019F * (nb_a * nb_a) -
(i_out - 0.0729000047F) *
(i_out - 0.0729000047F)) +
0.291600019F * (ob_a * ob_a)) -
e_d_phi0_tmp) /
((((pb_a * pb_a + qb_a * qb_a) + 0.0729000047F) -
0.0729000047F) +
d_d_phi0_tmp))) *
((((b_d_phi0_tmp -
k_d_phi0_tmp *
(((rb_a * rb_a + sb_a * sb_a) + 0.0729000047F) -
0.0729000047F)) +
c_d_phi0_tmp) /
(2.0F * sqrtf((0.291600019F * (tb_a * tb_a) -
(j_out - 0.0729000047F) *
(j_out - 0.0729000047F)) +
0.291600019F * (ub_a * ub_a))) -
l_d_phi0_tmp) /
((((vb_a * vb_a + wb_a * wb_a) + 0.0729000047F) -
0.0729000047F) +
d_d_phi0_tmp) -
(sqrtf((0.291600019F * (xb_a * xb_a) -
(k_out - 0.0729000047F) * (k_out - 0.0729000047F)) +
0.291600019F * (yb_a * yb_a)) -
e_d_phi0_tmp) *
g_d_phi0_tmp / (ac_a * ac_a)) /
((bc_a * bc_a / (cc_a * cc_a) + 1.0F) *
((0.27F * arm_cos_f32(2.0F *
atanf((sqrtf((0.291600019F * (dc_a * dc_a) -
(l_out - 0.0729000047F) *
(l_out - 0.0729000047F)) +
0.291600019F * (ec_a * ec_a)) -
e_d_phi0_tmp) /
((((fc_a * fc_a + gc_a * gc_a) +
0.0729000047F) -
0.0729000047F) +
d_d_phi0_tmp))) -
0.075F) +
d_d_phi0_tmp_tmp)) +
0.54F *
arm_sin_f32(2.0F *
atanf((sqrtf((0.291600019F * (hc_a * hc_a) -
(m_out - 0.0729000047F) *
(m_out - 0.0729000047F)) +
0.291600019F * (ic_a * ic_a)) -
e_d_phi0_tmp) /
((((jc_a * jc_a + kc_a * kc_a) + 0.0729000047F) -
0.0729000047F) +
d_d_phi0_tmp))) *
(0.27F *
arm_sin_f32(2.0F * atanf((sqrtf((0.291600019F * (lc_a * lc_a) -
(n_out - 0.0729000047F) *
(n_out - 0.0729000047F)) +
0.291600019F * (mc_a * mc_a)) -
e_d_phi0_tmp) /
((((nc_a * nc_a + oc_a * oc_a) +
0.0729000047F) -
0.0729000047F) +
d_d_phi0_tmp))) +
b_d_phi0_tmp_tmp) *
((((b_d_phi0_tmp -
k_d_phi0_tmp *
(((pc_a * pc_a + qc_a * qc_a) + 0.0729000047F) -
0.0729000047F)) +
c_d_phi0_tmp) /
(2.0F * sqrtf((0.291600019F * (rc_a * rc_a) -
(o_out - 0.0729000047F) *
(o_out - 0.0729000047F)) +
0.291600019F * (sc_a * sc_a))) -
l_d_phi0_tmp) /
((((tc_a * tc_a + uc_a * uc_a) + 0.0729000047F) -
0.0729000047F) +
d_d_phi0_tmp) -
(sqrtf((0.291600019F * (vc_a * vc_a) -
(p_out - 0.0729000047F) * (p_out - 0.0729000047F)) +
0.291600019F * (wc_a * wc_a)) -
e_d_phi0_tmp) *
g_d_phi0_tmp / (xc_a * xc_a)) /
((yc_a * yc_a / (ad_a * ad_a) + 1.0F) * (bd_a * bd_a))) /
(cd_a * cd_a / (dd_a * dd_a) + 1.0F);
}
/*
* File trailer for get_dphi0.c
*
* [EOF]
*/

10
application/chassis/cal_phi0_dot.h Normal file
View File

@ -0,0 +1,10 @@
//
// Created by SJQ on 2024/1/3.
//
#ifndef WHEEL_LEGGED_CAL_PHI0_DOT_H
#define WHEEL_LEGGED_CAL_PHI0_DOT_H
float get_dphi0(float phi_1, float phi_4, float d_phi1, float d_phi4);
#endif //WHEEL_LEGGED_CAL_PHI0_DOT_H

216
application/chassis/chassis.c
View File

@ -15,18 +15,21 @@
#include "robot_def.h"
#include "dji_motor.h"
#include "HT04.h"
#include "LK9025.h"
#include "super_cap.h"
#include "message_center.h"
#include "referee_task.h"
#include "power_meter.h"
#include "ins_task.h"
#include "leg.h"
#include "balance.h"
#include "general_def.h"
#include "bsp_dwt.h"
#include "referee_UI.h"
#include "arm_math.h"
#include "vofa.h"
#include "user_lib.h"
/* 根据robot_def.h中的macro自动计算的参数 */
#define HALF_WHEEL_BASE (WHEEL_BASE / 2.0f) // 半轴距
#define HALF_TRACK_WIDTH (TRACK_WIDTH / 2.0f) // 半轮距
@ -51,24 +54,26 @@ static Referee_Interactive_info_t ui_data; // UI数据，将底盘中的数据
static SuperCapInstance *cap; // 超级电容
static PowerMeterInstance *power_meter; //功率计
//static DJIMotorInstance *motor_lf, *motor_rf, *motor_lb, *motor_rb; // left right forward back
static HTMotorInstance *motor_lf, *motor_rf, *motor_lb, *motor_rb; // left right forward back
static LKMotorInstance *wheel_motor_r,*wheel_motor_l;
/* 用于自旋变速策略的时间变量 */
// static float t;
/* 私有函数计算的中介变量,设为静态避免参数传递的开销 */
static float chassis_vx, chassis_vy; // 将云台系的速度投影到底盘
static float vt_lf, vt_rf, vt_lb, vt_rb; // 底盘速度解算后的临时输出,待进行限幅
//static Leg_State_t legState;
//static Leg_Input_t legInput;
attitude_t *Chassis_IMU_data;
static LegInstance legInstance;
Balance_Control_t BalanceControl;
void ChassisInit()
{
// 四个轮子的参数一样,改tx_id和反转标志位即可
// 四个关节电机的参数,改tx_id和反转标志位即可
Motor_Init_Config_s chassis_motor_config = {
.can_init_config.can_handle = &hcan1,
.can_init_config.can_handle = &hcan2,
.controller_setting_init_config = {
.angle_feedback_source = MOTOR_FEED,
.speed_feedback_source = MOTOR_FEED,
@ -77,6 +82,17 @@ void ChassisInit()
},
.motor_type = HT04,
};
Motor_Init_Config_s wheel_motor_config = {
.can_init_config.can_handle = &hcan1,
.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 = LK9025,
};
// @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;
@ -96,6 +112,17 @@ void ChassisInit()
//chassis_motor_config.controller_setting_init_config.motor_reverse_flag = MOTOR_DIRECTION_NORMAL;
motor_lb = HTMotorInit(&chassis_motor_config);
//@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);
referee_data = UITaskInit(&huart6,&ui_data); // 裁判系统初始化,会同时初始化UI
SuperCap_Init_Config_s cap_conf = {
@ -111,24 +138,48 @@ void ChassisInit()
.rx_id = 0x211,
}
};
//功率計初始化
power_meter = PowerMeterInit(&power_conf);
PID_Init_Config_s leg_pid_config = {
.Kp = 50,
.Ki = 10,
.Kd = 0,
.MaxOut = 50,
Balance_Init_config_s balanceInitConfig = {
.legInitConfig = {
.length_PID_conf = {
.Kp = 150,//180 ,//50,
.Ki = 20,//5,
.Kd = 10,//10,//6,//6,
.MaxOut = 100,
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement | PID_DerivativeFilter,
.Derivative_LPF_RC = 1/(2*PI*10),
.IntegralLimit = 100,
},
.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),
.IntegralLimit = 100,
},
.init_target_L0 = 0.15f,
.F_feedforward = 0,
},
.leg_cor_PID_config = {
.Kp = 8,
.Ki = 0,
.Kd = 1,
.MaxOut = 3,
.Improve = PID_Derivative_On_Measurement| PID_DerivativeFilter,
.Derivative_LPF_RC = 1/(2*PI*10),
},
.LQR_state_num = 6,
.LQR_control_num = 2,
};
PID_Init_Config_s phi_pid_config = {
.Kp = 2,
.Ki = 0,
.Kd = 0,
.MaxOut = 5,
};
Leg_Init(&legInstance,leg_pid_config,phi_pid_config);
//Leg_Init(&legInstance,&leg_r_init_conf);
Balance_Control_Init(&BalanceControl,balanceInitConfig);
Chassis_IMU_data = INS_Init(); // 底盘IMU初始化
// 发布订阅初始化,如果为双板,则需要can comm来传递消息
#ifdef CHASSIS_BOARD
@ -222,21 +273,27 @@ void ChassisTask()
chassis_cmd_recv = *(Chassis_Ctrl_Cmd_s *)CANCommGet(chasiss_can_comm);
#endif // CHASSIS_BOARD
if (chassis_cmd_recv.chassis_mode == CHASSIS_ZERO_FORCE)
if (chassis_cmd_recv.chassis_mode == CHASSIS_NO_FOLLOW)
{ // 如果出现重要模块离线或遥控器设置为急停,让电机停止
// HTMotorStop(motor_lf);
// HTMotorStop(motor_rf);
// HTMotorStop(motor_lb);
// HTMotorStop(motor_rb);
HTMotorStop(motor_lf);
HTMotorStop(motor_rf);
HTMotorStop(motor_lb);
HTMotorStop(motor_rb);
LKMotorStop(wheel_motor_r);
LKMotorStop(wheel_motor_l);
}
else
{ // 正常工作
// HTMotorEnable(motor_lf);
// HTMotorEnable(motor_rf);
// HTMotorEnable(motor_lb);
// HTMotorEnable(motor_rb);
HTMotorEnable(motor_lf);
HTMotorEnable(motor_rf);
HTMotorEnable(motor_lb);
HTMotorEnable(motor_rb);
LKMotorEnable(wheel_motor_r);
LKMotorEnable(wheel_motor_l);
}
static int8_t init_flag = FALSE;
// 根据控制模式设定旋转速度
switch (chassis_cmd_recv.chassis_mode)
{
@ -263,15 +320,10 @@ void ChassisTask()
chassis_vy = chassis_cmd_recv.vx * sin_theta + chassis_cmd_recv.vy * cos_theta;
// 根据控制模式进行正运动学解算,计算底盘输出
//MecanumCalculate();
OmniCalculate();
// 根据裁判系统的反馈数据和电容数据对输出限幅并设定闭环参考值
LimitChassisOutput();
// float vofa_send_data[2];
// vofa_send_data[0]=motor_lb->motor_controller.speed_PID.Ref;
// vofa_send_data[1]=motor_lb->motor_controller.speed_PID.Measure;
// vofa_justfloat_output(vofa_send_data,8,&huart1);
// 根据电机的反馈速度和IMU(如果有)计算真实速度
EstimateSpeed();
@ -283,38 +335,86 @@ void ChassisTask()
// chassis_feedback_data.bullet_speed = referee_data->GameRobotState.shooter_id1_17mm_speed_limit;
// chassis_feedback_data.rest_heat = referee_data->PowerHeatData.shooter_heat0;
HTMotorEnable(motor_lf);
HTMotorEnable(motor_rf);
HTMotorEnable(motor_lb);
HTMotorEnable(motor_rb);
// HTMotorSetRef(motor_rf,0);
// HTMotorSetRef(motor_rb,0);
HTMotorSetRef(motor_lf,0);
HTMotorSetRef(motor_lb,0);
float vofa_send_data[7];
float vofa_send_data[4];
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);
// vofa_send_data[0] = PI-(motor_rf->measure.total_angle+0.8f);
////
//// // phi1 = PI/2-motor_rf->measure.total_angle;
// vofa_send_data[1] = -(motor_rb->measure.total_angle-0.8f);
// // phi4 = PI/2-motor_rb->measure.total_angle;
leg_phi_dot[0] = -motor_rf->measure.speed_rads;
leg_phi_dot[1] = -motor_rb->measure.speed_rads;
leg_phi_dot[2] = motor_lf->measure.speed_rads;
leg_phi_dot[3] = motor_lb->measure.speed_rads;
float body_phi = - Chassis_IMU_data->Pitch * DEGREE_2_RAD;
float body_phi_dot = - Chassis_IMU_data->Gyro[X];
float x = - (wheel_motor_r->measure.total_angle * DEGREE_2_RAD ) * 0.135f/2;
float x_dot = - wheel_motor_r->measure.speed_rads * 0.135f/2;
Leg_State_update(&legInstance.legState,PI-(motor_rf->measure.total_angle+0.8f),-(motor_rb->measure.total_angle-0.8f));
legInstance.target_L0 = 0.3f;
legInstance.target_phi0 = PI/2;
Leg_length_control_loop(&legInstance);
Balance_feedback_update(&BalanceControl,leg_phi,leg_phi_dot,body_phi,body_phi_dot,x,x_dot);
BalanceControl.leg_right.target_L0 += chassis_cmd_recv.vx/5280 * 0.0001f;
BalanceControl.leg_right.target_L0 = float_constrain(BalanceControl.leg_right.target_L0,0.15f,0.30f);
BalanceControl.leg_right.target_phi0 = PI/2;
BalanceControl.leg_left.target_L0 = BalanceControl.leg_right.target_L0;
BalanceControl.leg_left.target_L0 = float_constrain(BalanceControl.leg_left.target_L0,0.15f,0.30f);
BalanceControl.leg_left.target_phi0 = PI/2;
vofa_send_data[0] = BalanceControl.leg_right.legState.phi0;
vofa_send_data[1] = BalanceControl.leg_left.legState.phi0;
vofa_send_data[2] = BalanceControl.leg_coordination_PID.Output;
vofa_send_data[3] = BalanceControl.leg_coordination_PID.Measure;
vofa_send_data[4] = BalanceControl.leg_right.legInput.F;
vofa_justfloat_output(vofa_send_data,28,&huart1);
static float target_x = 0;
if(chassis_cmd_recv.chassis_mode == CHASSIS_ROTATE) // 右侧开关状态[下],底盘跟随云台
{
target_x += chassis_cmd_recv.vy/5280 * 0.001f;
target_x_set(&BalanceControl,target_x);
Balance_Control_Loop(&BalanceControl);
float T = float_constrain(BalanceControl.balance_LQR.control_vector[0],-4,4);
float wheel_current = T / 0.00512f;
float_constrain(wheel_current,-2000,2000);
HTMotorSetRef(motor_rf,-BalanceControl.leg_right.legOutput.T1 * 0.5142857f);
HTMotorSetRef(motor_rb,-BalanceControl.leg_right.legOutput.T2 * 0.5142857f);
HTMotorSetRef(motor_lf,BalanceControl.leg_left.legOutput.T1 * 0.5142857f);
HTMotorSetRef(motor_lb,BalanceControl.leg_left.legOutput.T2 * 0.5142857f);
LKMotorSetRef(wheel_motor_r,wheel_current);
LKMotorSetRef(wheel_motor_l,wheel_current);
}
else{
// HTMotorSetRef(motor_rf,0);
// HTMotorSetRef(motor_rb,0);
// HTMotorSetRef(motor_lf,0);
// HTMotorSetRef(motor_lb,0);
//LKMotorSetRef(wheel_motor_r,0);
// LKMotorSetRef(wheel_motor_l,0);
}
vofa_send_data[0] = legInstance.Length_PID.Output;
vofa_send_data[2] = legInstance.legState.L0;
vofa_send_data[3] = legInstance.legState.phi0;
vofa_justfloat_output(vofa_send_data,16,&huart1);
HTMotorSetRef(motor_rf,-legInstance.legOutput.T1);
HTMotorSetRef(motor_rb,-legInstance.legOutput.T2);
// 推送反馈消息
#ifdef ONE_BOARD

33
application/chassis/leg.c
View File

@ -7,6 +7,8 @@
#include "main.h"
#include "arm_math.h"
#include "cal_phi0_dot.h"
static const float l[5]={0.15f,0.27f,0.27f,0.15f,0.15f};
static void calculate_leg_pos(Leg_State_t* leg_state);
@ -14,10 +16,15 @@ static void VMC_jacobian(float L0, float phi0, float phi1, float phi2, float phi
float phi4, float F, float Tp, float T12[2]);
void VMC_getT(LegInstance* legInstance);
void Leg_Init(LegInstance* legInstance,PID_Init_Config_s lengthConfig,PID_Init_Config_s phiConfig)
void Leg_Init(LegInstance* legInstance, Leg_init_config_s* legInitConfig)
{
PIDInit(&legInstance->Length_PID,&lengthConfig);
PIDInit(&legInstance->Phi0_PID,&phiConfig);
PIDInit(&legInstance->Length_PID,&legInitConfig->length_PID_conf);
PIDInit(&legInstance->Phi0_PID,&legInitConfig->phi0_PID_conf);
legInstance->target_L0 = legInitConfig->init_target_L0;
legInstance->F_feedforward = legInitConfig->F_feedforward;
// set rest of memory to 0
DWT_GetDeltaT(&legInstance->legState.DWT_CNT);
}
void Leg_length_control_loop(LegInstance* legInstance)
@ -28,9 +35,9 @@ void Leg_length_control_loop(LegInstance* legInstance)
//Leg_calc_output(&legInstance->legState,&legInstance->legInput,&legInstance->legOutput);
VMC_getT(legInstance);
}
void Leg_State_update(Leg_State_t* leg_state, float phi1,float phi4)
void Leg_State_update(LegInstance* legInstance, float phi1,float phi4 ,float phi1_dot,float phi4_dot)
{
//@todo:输入要限制在限幅以内
Leg_State_t* leg_state = &legInstance->legState;
if(phi1<=PI/2)
phi1 = PI/2;
if(phi4>=PI/2)
@ -42,10 +49,16 @@ void Leg_State_update(Leg_State_t* leg_state, float phi1,float phi4)
//calculate_leg_pos(leg_state->phi1,leg_state->phi4,leg_pos);
calculate_leg_pos(leg_state);
// leg_state->L0 = leg_pos[0];
// leg_state->phi0 = leg_pos[1];
// leg_state->phi2 = leg_pos[2];
// leg_state->phi3 = leg_pos[3];
//对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);
}
void Leg_Input_update(Leg_Input_t* leg_Input, float F,float Tp)
@ -68,7 +81,7 @@ void Leg_calc_output(Leg_State_t* leg_state,Leg_Input_t* leg_input,Leg_Output_t*
void calculate_leg_pos(Leg_State_t* leg_state)
{
//以机体中点建立坐标系
float xb = -l[4]/2+l[0]* arm_sin_f32(leg_state->phi1);
float xb = -l[4]/2+l[0]* arm_cos_f32(leg_state->phi1);
float yb = l[0]* arm_sin_f32(leg_state->phi1);
float xd = l[4]/2+l[3]* arm_cos_f32(leg_state->phi4);

22
application/chassis/leg.h
View File

@ -16,6 +16,12 @@ typedef struct
float phi2;
float phi3;
uint32_t DWT_CNT;
float dt; //用于计算时间并求导
float last_phi0;
float phi0_dot;
}Leg_State_t;
typedef struct
@ -30,6 +36,14 @@ typedef struct
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;
@ -41,14 +55,18 @@ typedef struct
PIDInstance Length_PID;
PIDInstance Phi0_PID;
float F_feedforward; //支持力前馈补偿
}LegInstance;
void Leg_State_update(Leg_State_t* leg_state, float phi1,float phi4);
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_Init(LegInstance* legInstance,PID_Init_Config_s lengthConfig,PID_Init_Config_s phiConfig);
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
application/robot.c
View File

@ -32,12 +32,13 @@ void RobotInit()
#if defined(ONE_BOARD) || defined(GIMBAL_BOARD)
RobotCMDInit();
//暂时注释云台和发射任务 调试底盘
GimbalInit();
// ShootInit();
//GimbalInit();
//ShootInit();
#endif
#if defined(ONE_BOARD) || defined(CHASSIS_BOARD)
ChassisInit();
#endif
OSTaskInit(); // 创建基础任务
@ -52,7 +53,7 @@ void RobotTask()
RobotCMDTask();
//暂时注释云台和发射任务 调试底盘
//GimbalTask();
// ShootTask();
//ShootTask();
#endif
#if defined(ONE_BOARD) || defined(CHASSIS_BOARD)

2
application/robot_def.h
View File

@ -18,7 +18,7 @@
/* 开发板类型定义,烧录时注意不要弄错对应功能;修改定义后需要重新编译,只能存在一个定义! */
#define ONE_BOARD // 单板控制整车
// #define CHASSIS_BOARD //底盘板
//#define CHASSIS_BOARD //底盘板
// #define GIMBAL_BOARD //云台板
#define VISION_USE_VCP // 使用虚拟串口发送视觉数据

123
application/shoot/shoot.c
View File

@ -5,6 +5,7 @@
#include "message_center.h"
#include "bsp_dwt.h"
#include "general_def.h"
#include "vofa.h"
/* 对于双发射机构的机器人,将下面的数据封装成结构体即可,生成两份shoot应用实例 */
static DJIMotorInstance *friction_l, *friction_r, *loader; // 拨盘电机
@ -62,8 +63,8 @@ void ShootInit()
// 拨盘电机
Motor_Init_Config_s loader_config = {
.can_init_config = {
.can_handle = &hcan2,
.tx_id = 3,
.can_handle = &hcan1,
.tx_id = 1,
},
.controller_param_init_config = {
.angle_PID = {
@ -93,7 +94,7 @@ void ShootInit()
.controller_setting_init_config = {
.angle_feedback_source = MOTOR_FEED, .speed_feedback_source = MOTOR_FEED,
.outer_loop_type = SPEED_LOOP, // 初始化成SPEED_LOOP,让拨盘停在原地,防止拨盘上电时乱转
.close_loop_type = CURRENT_LOOP | SPEED_LOOP,
.close_loop_type = SPEED_LOOP,
.motor_reverse_flag = MOTOR_DIRECTION_NORMAL, // 注意方向设置为拨盘的拨出的击发方向
},
.motor_type = M2006 // 英雄使用m3508
@ -108,21 +109,21 @@ void ShootInit()
void ShootTask()
{
// 从cmd获取控制数据
SubGetMessage(shoot_sub, &shoot_cmd_recv);
//SubGetMessage(shoot_sub, &shoot_cmd_recv);
// 对shoot mode等于SHOOT_STOP的情况特殊处理,直接停止所有电机(紧急停止)
if (shoot_cmd_recv.shoot_mode == SHOOT_OFF)
{
DJIMotorStop(friction_l);
DJIMotorStop(friction_r);
DJIMotorStop(loader);
}
else // 恢复运行
{
DJIMotorEnable(friction_l);
DJIMotorEnable(friction_r);
DJIMotorEnable(loader);
}
// if (shoot_cmd_recv.shoot_mode == SHOOT_OFF)
// {
// DJIMotorStop(friction_l);
// DJIMotorStop(friction_r);
// DJIMotorStop(loader);
// }
// else // 恢复运行
// {
// DJIMotorEnable(friction_l);
// DJIMotorEnable(friction_r);
// DJIMotorEnable(loader);
// }
// 如果上一次触发单发或3发指令的时间加上不应期仍然大于当前时间(尚未休眠完毕),直接返回即可
// 单发模式主要提供给能量机关激活使用(以及英雄的射击大部分处于单发)
@ -130,43 +131,43 @@ void ShootTask()
// return;
// 若不在休眠状态,根据robotCMD传来的控制模式进行拨盘电机参考值设定和模式切换
switch (shoot_cmd_recv.load_mode)
{
// 停止拨盘
case LOAD_STOP:
DJIMotorOuterLoop(loader, SPEED_LOOP); // 切换到速度环
DJIMotorSetRef(loader, 0); // 同时设定参考值为0,这样停止的速度最快
break;
// 单发模式,根据鼠标按下的时间,触发一次之后需要进入不响应输入的状态(否则按下的时间内可能多次进入,导致多次发射)
case LOAD_1_BULLET: // 激活能量机关/干扰对方用,英雄用.
DJIMotorOuterLoop(loader, ANGLE_LOOP); // 切换到角度环
DJIMotorSetRef(loader, loader->measure.total_angle + ONE_BULLET_DELTA_ANGLE); // 控制量增加一发弹丸的角度
hibernate_time = DWT_GetTimeline_ms(); // 记录触发指令的时间
dead_time = 150; // 完成1发弹丸发射的时间
break;
// 三连发,如果不需要后续可能删除
case LOAD_3_BULLET:
DJIMotorOuterLoop(loader, ANGLE_LOOP); // 切换到速度环
DJIMotorSetRef(loader, loader->measure.total_angle + 3 * ONE_BULLET_DELTA_ANGLE); // 增加3发
hibernate_time = DWT_GetTimeline_ms(); // 记录触发指令的时间
dead_time = 300; // 完成3发弹丸发射的时间
break;
// 连发模式,对速度闭环,射频后续修改为可变,目前固定为1Hz
case LOAD_BURSTFIRE:
DJIMotorOuterLoop(loader, SPEED_LOOP);
DJIMotorSetRef(loader, shoot_cmd_recv.shoot_rate * 360 * REDUCTION_RATIO_LOADER / 8);
// x颗/秒换算成速度: 已知一圈的载弹量,由此计算出1s需要转的角度,注意换算角速度(DJIMotor的速度单位是angle per second)
break;
// 拨盘反转,对速度闭环,后续增加卡弹检测(通过裁判系统剩余热量反馈和电机电流)
// 也有可能需要从switch-case中独立出来
case LOAD_REVERSE:
DJIMotorOuterLoop(loader, SPEED_LOOP);
// ...
break;
default:
while (1)
; // 未知模式,停止运行,检查指针越界,内存溢出等问题
}
// switch (shoot_cmd_recv.load_mode)
// {
// // 停止拨盘
// case LOAD_STOP:
// DJIMotorOuterLoop(loader, SPEED_LOOP); // 切换到速度环
// DJIMotorSetRef(loader, 0); // 同时设定参考值为0,这样停止的速度最快
// break;
// // 单发模式,根据鼠标按下的时间,触发一次之后需要进入不响应输入的状态(否则按下的时间内可能多次进入,导致多次发射)
// case LOAD_1_BULLET: // 激活能量机关/干扰对方用,英雄用.
// DJIMotorOuterLoop(loader, ANGLE_LOOP); // 切换到角度环
// DJIMotorSetRef(loader, loader->measure.total_angle + ONE_BULLET_DELTA_ANGLE); // 控制量增加一发弹丸的角度
// hibernate_time = DWT_GetTimeline_ms(); // 记录触发指令的时间
// dead_time = 150; // 完成1发弹丸发射的时间
// break;
// // 三连发,如果不需要后续可能删除
// case LOAD_3_BULLET:
// DJIMotorOuterLoop(loader, ANGLE_LOOP); // 切换到速度环
// DJIMotorSetRef(loader, loader->measure.total_angle + 3 * ONE_BULLET_DELTA_ANGLE); // 增加3发
// hibernate_time = DWT_GetTimeline_ms(); // 记录触发指令的时间
// dead_time = 300; // 完成3发弹丸发射的时间
// break;
// // 连发模式,对速度闭环,射频后续修改为可变,目前固定为1Hz
// case LOAD_BURSTFIRE:
// DJIMotorOuterLoop(loader, SPEED_LOOP);
// DJIMotorSetRef(loader, shoot_cmd_recv.shoot_rate * 360 * REDUCTION_RATIO_LOADER / 8);
// // x颗/秒换算成速度: 已知一圈的载弹量,由此计算出1s需要转的角度,注意换算角速度(DJIMotor的速度单位是angle per second)
// break;
// // 拨盘反转,对速度闭环,后续增加卡弹检测(通过裁判系统剩余热量反馈和电机电流)
// // 也有可能需要从switch-case中独立出来
// case LOAD_REVERSE:
// DJIMotorOuterLoop(loader, SPEED_LOOP);
// // ...
// break;
// default:
// while (1)
// ; // 未知模式,停止运行,检查指针越界,内存溢出等问题
// }
// 确定是否开启摩擦轮,后续可能修改为键鼠模式下始终开启摩擦轮(上场时建议一直开启)
if (shoot_cmd_recv.friction_mode == FRICTION_ON)
@ -210,4 +211,20 @@ void ShootTask()
// 反馈数据,目前暂时没有要设定的反馈数据,后续可能增加应用离线监测以及卡弹反馈
PubPushMessage(shoot_pub, (void *)&shoot_feedback_data);
DJIMotorEnable(loader);
DJIMotorSetRef(loader,1);
// float vofa_send_data[7];
//
// vofa_send_data[0] = loader->motor_controller.pid_ref;
// vofa_send_data[1] = loader->measure.speed_aps;
// vofa_send_data[2] = loader->motor_controller.speed_PID.Output;
//// vofa_send_data[2] = BalanceControl.leg_right.target_L0;
//// vofa_send_data[3] = BalanceControl.leg_right.legState.L0;
//// vofa_send_data[4] = BalanceControl.leg_right.legInput.F;
//
//
// vofa_justfloat_output(vofa_send_data,28,&huart1);
}

51
modules/algorithm/LQR.c Normal file
View File

@ -0,0 +1,51 @@
//
// Created by SJQ on 2023/12/29.
//
#include "LQR.h"
void LQR_Init(LQRInstance* lqrInstance,int state_cnt,int control_cnt)
{
lqrInstance->state_num = state_cnt;
lqrInstance->control_num = control_cnt;
lqrInstance->state_vector = (float *)zmalloc(state_cnt *4);
lqrInstance->state_cmd = (float *)zmalloc(state_cnt *4);
lqrInstance->control_vector = (float *)zmalloc(control_cnt *4);
MatInit(&lqrInstance->K_matrix,control_cnt,state_cnt);
MatInit(&lqrInstance->state_err,state_cnt,1);
MatInit(&lqrInstance->state_matrix,state_cnt,1);
MatInit(&lqrInstance->control_matrix,control_cnt,1);
MatInit(&lqrInstance->state_cmd_matrix,state_cnt,1);
}
void LQR_update(LQRInstance* lqrInstance,float* new_state,float* target_state)
{
memcpy(lqrInstance->state_vector,new_state,lqrInstance->state_num * 4);
memcpy(lqrInstance->state_matrix.pData,new_state,lqrInstance->state_num * 4);
memcpy(lqrInstance->state_cmd,target_state,lqrInstance->state_num* 4);
memcpy(lqrInstance->state_cmd_matrix.pData,target_state,lqrInstance->state_num* 4);
arm_mat_sub_f32(&lqrInstance->state_cmd_matrix,&lqrInstance->state_matrix,&lqrInstance->state_err);
arm_mat_mult_f32(&lqrInstance->K_matrix,&lqrInstance->state_err,&lqrInstance->control_matrix);
memcpy(lqrInstance->control_vector,lqrInstance->control_matrix.pData,lqrInstance->control_num* 4);
// arm_mat_init_f32(&lqrInstance->state_matrix,lqrInstance->state_num,1,new_state);
// arm_mat_init_f32(&lqrInstance->state_cmd_matrix,lqrInstance->state_num,1,target_state);
//
// arm_mat_sub_f32(&lqrInstance->state_cmd_matrix,&lqrInstance->state_matrix,&lqrInstance->state_err);
// arm_mat_mult_f32(&lqrInstance->K_matrix,&lqrInstance->state_err,&lqrInstance->control_matrix);
}
void LQR_set_K(LQRInstance* lqrInstance, float *K_value)
{
int K_cnt = lqrInstance->K_matrix.numCols * lqrInstance->K_matrix.numRows;
memcpy(lqrInstance->K_matrix.pData,K_value,K_cnt *4);
//arm_mat_init_f32(&lqrInstance->K_matrix,lqrInstance->control_num,lqrInstance->state_num,K_value);
}

30
modules/algorithm/LQR.h Normal file
View File

@ -0,0 +1,30 @@
//
// Created by SJQ on 2023/12/29.
//
#ifndef WHEEL_LEGGED_LQR_H
#define WHEEL_LEGGED_LQR_H
#include "user_lib.h"
typedef struct
{
float* state_vector;
float* control_vector;
float* state_cmd;
int state_num; //状态量维数
int control_num; //控制量维数
mat control_matrix; //其实是向量 定义成矩阵方便计算 @todo：更新DSP库 新版本库支持矩阵直接乘数组
mat state_matrix;
mat state_cmd_matrix;
mat state_err;
mat K_matrix;
}LQRInstance;
void LQR_Init(LQRInstance* lqrInstance,int state_cnt,int control_cnt);
void LQR_update(LQRInstance* lqrInstance,float* new_state,float* target_state);
void LQR_set_K(LQRInstance* lqrInstance, float *K_value);
#endif //WHEEL_LEGGED_LQR_H

41
modules/algorithm/user_lib.c
View File

@ -212,3 +212,44 @@ void MatInit(mat *m, uint8_t row, uint8_t col)
m->numRows = row;
m->pData = (float *)zmalloc(row * col * sizeof(float));
}
/**
* @brief 斜波函数初始化
* @author RM
* @param[in] 斜波函数结构体
* @param[in] 间隔的时间，单位 s
* @param[in] 最大值
* @param[in] 最小值
* @retval 返回空
*/
void ramp_init(ramp_function_source_t *ramp_source_type, float frame_period, float max, float min)
{
ramp_source_type->frame_period = frame_period;
ramp_source_type->max_value = max;
ramp_source_type->min_value = min;
ramp_source_type->input = 0.0f;
ramp_source_type->out = 0.0f;
}
/**
* @brief 斜波函数计算，根据输入的值进行叠加， 输入单位为 /s 即一秒后增加输入的值
* @author RM
* @param[in] 斜波函数结构体
* @param[in] 输入值
* @param[in] 滤波参数
* @retval 返回空
*/
void ramp_calc(ramp_function_source_t *ramp_source_type, float input)
{
ramp_source_type->input = input;
ramp_source_type->out += ramp_source_type->input * ramp_source_type->frame_period;
if (ramp_source_type->out > ramp_source_type->max_value)
{
ramp_source_type->out = ramp_source_type->max_value;
}
else if (ramp_source_type->out < ramp_source_type->min_value)
{
ramp_source_type->out = ramp_source_type->min_value;
}
}

14
modules/algorithm/user_lib.h
View File

@ -49,6 +49,15 @@ void MatInit(mat *m, uint8_t row, uint8_t col);
#define FALSE 0 /**< boolean fails */
#endif
typedef struct
{
float input; //输入数据
float out; //输出数据
float min_value; //限幅最小值
float max_value; //限幅最大值
float frame_period; //时间间隔
} ramp_function_source_t;
/* circumference ratio */
#ifndef PI
#define PI 3.14159265354f
@ -121,6 +130,11 @@ float Dot3d(float *v1, float *v2);
float AverageFilter(float new_data, float *buf, uint8_t len);
#define rad_format(Ang) loop_float_constrain((Ang), -PI, PI)
#endif

69
modules/imu/ins_task.c
View File

@ -20,19 +20,13 @@
#include "general_def.h"
#include "master_process.h"
#include "vofa.h"
static INS_t INS;
static IMU_Param_t IMU_Param;
static PIDInstance TempCtrl = {0};
//const float xb[3] = {1, 0, 0};
//const float yb[3] = {0, 1, 0};
//const float zb[3] = {0, 0, 1};
const float xb[3] = {0, 0, 1};
const float xb[3] = {1, 0, 0};
const float yb[3] = {0, 1, 0};
const float zb[3] = {-1, 0, 0};
const float zb[3] = {0, 0, 1};
// 用于获取两次采样之间的时间间隔
static uint32_t INS_DWT_Count = 0;
@ -43,7 +37,7 @@ static void IMU_Param_Correction(IMU_Param_t *param, float gyro[3], float accel[
static void IMUPWMSet(uint16_t pwm)
{
__HAL_TIM_SetCompare(&htim10, TIM_CHANNEL_1, pwm);
__HAL_TIM_SetCompare(&htim10, TIM_CHANNEL_1, pwm);
}
/**
@ -66,9 +60,9 @@ static void InitQuaternion(float *init_q4)
for (uint8_t i = 0; i < 100; ++i)
{
BMI088_Read(&BMI088);
acc_init[X] += -BMI088.Accel[2];
acc_init[X] += BMI088.Accel[X];
acc_init[Y] += BMI088.Accel[Y];
acc_init[Z] += BMI088.Accel[0];
acc_init[Z] += BMI088.Accel[Z];
DWT_Delay(0.001);
}
for (uint8_t i = 0; i < 3; ++i)
@ -78,9 +72,6 @@ static void InitQuaternion(float *init_q4)
float angle = acosf(Dot3d(acc_init, gravity_norm));
Cross3d(acc_init, gravity_norm, axis_rot);
Norm3d(axis_rot);
//q = cos (a/2) + i(x * sin(a/2)) + j(y * sin(a/2)) + k(z * sin(a/2))
//其中a表示旋转角度，(x,y,z)表示旋转轴 计算由实际重力加速度方向(0,0,1)到当前加速度方向的旋转
init_q4[0] = cosf(angle / 2.0f);
for (uint8_t i = 0; i < 2; ++i)
init_q4[i + 1] = axis_rot[i] * sinf(angle / 2.0f); // 轴角公式,第三轴为0(没有z轴分量)
@ -106,16 +97,16 @@ attitude_t *INS_Init(void)
IMU_Param.flag = 1;
float init_quaternion[4] = {0};
InitQuaternion(init_quaternion);//计算由实际重力加速度方向(0,0,1)到当前加速度方向的旋转
InitQuaternion(init_quaternion);
IMU_QuaternionEKF_Init(init_quaternion, 10, 0.001, 1000000, 1, 0);
// imu heat init
PID_Init_Config_s config = {.MaxOut = 2000,
.IntegralLimit = 300,
.DeadBand = 0,
.Kp = 1000,
.Ki = 20,
.Kd = 0,
.Improve = 0x01}; // enable integratiaon limit
.IntegralLimit = 300,
.DeadBand = 0,
.Kp = 1000,
.Ki = 20,
.Kd = 0,
.Improve = 0x01}; // enable integratiaon limit
PIDInit(&TempCtrl, &config);
// noise of accel is relatively big and of high freq,thus lpf is used
@ -138,12 +129,12 @@ void INS_Task(void)
{
BMI088_Read(&BMI088);
INS.Accel[X] = -BMI088.Accel[2];
INS.Accel[Y] = BMI088.Accel[1];
INS.Accel[Z] = BMI088.Accel[0];
INS.Gyro[X] = -BMI088.Gyro[2];
INS.Gyro[Y] = BMI088.Gyro[1];
INS.Gyro[Z] = BMI088.Gyro[0];
INS.Accel[X] = BMI088.Accel[X];
INS.Accel[Y] = BMI088.Accel[Y];
INS.Accel[Z] = BMI088.Accel[Z];
INS.Gyro[X] = BMI088.Gyro[X];
INS.Gyro[Y] = BMI088.Gyro[Y];
INS.Gyro[Z] = BMI088.Gyro[Z];
// demo function,用于修正安装误差,可以不管,本demo暂时没用
IMU_Param_Correction(&IMU_Param, INS.Gyro, INS.Accel);
@ -171,35 +162,11 @@ void INS_Task(void)
}
BodyFrameToEarthFrame(INS.MotionAccel_b, INS.MotionAccel_n, INS.q); // 转换回导航系n
INS.Yaw = QEKF_INS.Yaw;
INS.Pitch = QEKF_INS.Pitch;
INS.Roll = QEKF_INS.Roll;
// // get Yaw total, yaw数据可能会超过360,处理一下方便其他功能使用(如小陀螺)
// if (INS.Yaw - INS.YawAngleLast > 180.0f)
// {
// INS.YawRoundCount--;
// }
// else if (INS.Yaw - INS.YawAngleLast < -180.0f)
// {
// INS.YawRoundCount++;
// }
// INS.YawTotalAngle = 360.0f * INS.YawRoundCount + INS.Yaw;
INS.YawTotalAngle = QEKF_INS.YawTotalAngle;
// float vofa_send_data[9];
// vofa_send_data[0]=INS.Yaw;
// vofa_send_data[1]=INS.Pitch;
// vofa_send_data[2]=INS.Roll;
// vofa_send_data[3]=INS.Gyro[X];
// vofa_send_data[4]=INS.Gyro[Y];
// vofa_send_data[5]=INS.Gyro[Z];
// vofa_send_data[6]=BMI088.Accel[X];
// vofa_send_data[7]=BMI088.Accel[Y];
// vofa_send_data[8]=BMI088.Accel[Z];
// vofa_justfloat_output(vofa_send_data,36,&huart1);
VisionSetAltitude(INS.Yaw, INS.Pitch, INS.Roll);
}

8
modules/imu/ins_task.h
View File

@ -23,10 +23,6 @@
#define Y 1
#define Z 2
//#define X 2
//#define Y 1
//#define Z 0
#define INS_TASK_PERIOD 1
typedef struct
@ -38,7 +34,6 @@ typedef struct
float Pitch;
float Yaw;
float YawTotalAngle;
} attitude_t; // 最终解算得到的角度,以及yaw转动的总角度(方便多圈控制)
typedef struct
@ -68,9 +63,6 @@ typedef struct
float Yaw;
float YawTotalAngle;
float YawAngleLast;
float YawRoundCount;
uint8_t init;
} INS_t;

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

@ -16,8 +16,8 @@
#define P_MAX 95.5f
#define V_MIN -45.0f // Rad/s
#define V_MAX 45.0f
#define T_MIN -5.0f // N·m
#define T_MAX 5.0f
#define T_MIN -18.0f // N·m 这里是用于发送扭矩命令时映射的，不能改 严格按照例程说明的
#define T_MAX 18.0f
#define KP_MIN 0.0f // N-m/rad
#define KP_MAX 500.0f
#define KD_MIN 0.0f // N-m/rad/s

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

@ -10,6 +10,16 @@ static LKMotorInstance *lkmotor_instance[LK_MOTOR_MX_CNT] = {NULL};
static CANInstance *sender_instance; // 多电机发送时使用的caninstance(当前保存的是注册的第一个电机的caninstance)
// 后续考虑兼容单电机和多电机指令.
#define MULTI_MOTOR //多电机指令
//#define SINGLE_MOTOR //单电机指令
//将-32~32 映射为-2000到2000
static int16_t float_to_int16(float x, float x_abs_max,float out_abs_max)
{
int16_t output = (int16_t)(x/x_abs_max * out_abs_max);
return output;
}
/**
* @brief 电机反馈报文解析
*
@ -24,8 +34,14 @@ static void LKMotorDecode(CANInstance *_instance)
DaemonReload(motor->daemon); // 喂狗
measure->feed_dt = DWT_GetDeltaT(&measure->feed_dwt_cnt);
if(measure->init_flag == 0)
{
measure->offset_ecd = (uint16_t)((rx_buff[7] << 8) | rx_buff[6]);
measure->init_flag = 1;
}
measure->last_ecd = measure->ecd;
measure->ecd = (uint16_t)((rx_buff[7] << 8) | rx_buff[6]);
measure->ecd = (uint16_t)((rx_buff[7] << 8) | rx_buff[6]) - measure->offset_ecd; //上电位置为ecd 0点
measure->angle_single_round = ECD_ANGLE_COEF_LK * measure->ecd;
@ -65,11 +81,20 @@ LKMotorInstance *LKMotorInit(Motor_Init_Config_s *config)
config->can_init_config.id = motor;
config->can_init_config.can_module_callback = LKMotorDecode;
#ifdef MULTI_MOTOR //多电机指令
config->can_init_config.rx_id = 0x140 + config->can_init_config.tx_id;
config->can_init_config.tx_id = config->can_init_config.tx_id + 0x280 - 1; // 这样在发送写入buffer的时候更方便,因为下标从0开始,LK多电机发送id为0x280
#endif
#ifdef SINGLE_MOTOR //单电机指令
config->can_init_config.rx_id = 0x140 + config->can_init_config.tx_id;
config->can_init_config.tx_id = 0x140 + config->can_init_config.tx_id;
#endif
motor->motor_can_ins = CANRegister(&config->can_init_config);
if (idx == 0) // 用第一个电机的can instance发送数据
if (idx == 0) // 多电机指令用第一个电机的can instance发送数据
{
sender_instance = motor->motor_can_ins;
sender_instance->tx_id = 0x280; // 修改tx_id为0x280,用于多电机发送,不用管其他LKMotorInstance的tx_id,它们仅作初始化用
@ -132,20 +157,31 @@ void LKMotorControl()
pid_ref = PIDCalculate(&motor->current_PID, measure->real_current, pid_ref);
}
set = pid_ref;
set = (int16_t)pid_ref;
if (setting->motor_reverse_flag == MOTOR_DIRECTION_REVERSE)
set *= -1;
//int16_t tmp = float_to_int16(set,I_MAX,COMMAND_MAX);
// 这里随便写的,为了兼容多电机命令.后续应该将tx_id以更好的方式表达电机id,单独使用一个CANInstance,而不是用第一个电机的CANInstance
memcpy(sender_instance->tx_buff + (motor->motor_can_ins->tx_id - 0x280) * 2, &set, sizeof(uint16_t));
memcpy(sender_instance->tx_buff + (motor->motor_can_ins->tx_id - 0x280) * 2, &set, sizeof(int16_t));
// motor->motor_can_ins->tx_buff[0] = 0xA2; //A1转矩闭环 A2速度闭环
// int32_t tmp = -1080*100;//(int32_t)set;
// memcpy(motor->motor_can_ins->tx_buff + 4 , &tmp, sizeof(int32_t));
if (motor->stop_flag == MOTOR_STOP)
{ // 若该电机处于停止状态,直接将发送buff置零
memset(sender_instance->tx_buff + (motor->motor_can_ins->tx_id - 0x280) * 2, 0, sizeof(uint16_t));
memset(sender_instance->tx_buff + (motor->motor_can_ins->tx_id - 0x280) * 2, 0, sizeof(int16_t));
//memset( motor->motor_can_ins->tx_buff + 4 , 0, 4);
}
#ifdef SINGLE_MOTOR
CANTransmit(motor->motor_can_ins, 0.2);
#endif
}
#ifdef MULTI_MOTOR
if (idx) // 如果有电机注册了
CANTransmit(sender_instance, 0.2);
#endif
}
void LKMotorStop(LKMotorInstance *motor)

12
modules/motor/LKmotor/LK9025.h
View File

@ -9,8 +9,13 @@
#define LK_MOTOR_MX_CNT 4 // 最多允许4个LK电机使用多电机指令,挂载在一条总线上
#define I_MIN -2000
#define I_MAX 2000
#define I_MIN -32.0f
#define I_MAX 32.0f
#define COMMAND_MAX 2000.0f
#define TORQUE_2_I (1.0f / 0.32f) //16T版本 扭矩常数 0.32Nm/A
#define CURRENT_SMOOTH_COEF 0.9f
#define SPEED_SMOOTH_COEF 0.85f
#define REDUCTION_RATIO_DRIVEN 1
@ -19,6 +24,9 @@
typedef struct // 9025
{
uint16_t offset_ecd; //上电时刻的编码器值
uint8_t init_flag; //是否首次上电的标志位
uint16_t last_ecd; // 上一次读取的编码器值
uint16_t ecd; // 当前编码器值
float angle_single_round; // 单圈角度

4
modules/motor/motor_task.c
View File

@ -15,14 +15,14 @@ void MotorControlTask()
/* 如果有对应的电机则取消注释,可以加入条件编译或者register对应的idx判断是否注册了电机 */
LKMotorControl();
ECMotorControl();
//ECMotorControl();
// legacy support
// 由于ht04电机的反馈方式为接收到一帧消息后立刻回传,以此方式连续发送可能导致总线拥塞
// 为了保证高频率控制,HTMotor中提供了以任务方式启动控制的接口,可通过宏定义切换
// HTMotorControl();
// 将所有的CAN设备集中在一处发送,最高反馈频率仅能达到500Hz,为了更好的控制效果,应使用新的HTMotorControlInit()接口
ServeoMotorControl();
//ServeoMotorControl();
// StepMotorControl();
}