From ead61a98809a08d67102b2daf828155d91b6041c Mon Sep 17 00:00:00 2001 From: HshineO <1491134420@qq.com> Date: Wed, 10 Jan 2024 18:27:25 +0800 Subject: [PATCH] =?UTF-8?q?=E5=9F=BA=E6=9C=AC=E5=B9=B3=E8=A1=A1=E6=8E=A7?= =?UTF-8?q?=E5=88=B6=E6=A1=86=E6=9E=B6=E5=AE=8C=E6=88=90=EF=BC=8C=E8=83=BD?= =?UTF-8?q?=E8=B7=91=E4=BA=86?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- application/chassis/balance.c | 136 +++++++ application/chassis/balance.h | 39 ++ application/chassis/cal_phi0_dot.c | 626 +++++++++++++++++++++++++++++ application/chassis/cal_phi0_dot.h | 10 + application/chassis/chassis.c | 216 +++++++--- application/chassis/leg.c | 33 +- application/chassis/leg.h | 22 +- application/robot.c | 7 +- application/robot_def.h | 2 +- application/shoot/shoot.c | 123 +++--- modules/algorithm/LQR.c | 51 +++ modules/algorithm/LQR.h | 30 ++ modules/algorithm/user_lib.c | 41 ++ modules/algorithm/user_lib.h | 14 + modules/imu/ins_task.c | 69 +--- modules/imu/ins_task.h | 8 - modules/motor/HTmotor/HT04.h | 4 +- modules/motor/LKmotor/LK9025.c | 48 ++- modules/motor/LKmotor/LK9025.h | 12 +- modules/motor/motor_task.c | 4 +- 20 files changed, 1297 insertions(+), 198 deletions(-) create mode 100644 application/chassis/balance.c create mode 100644 application/chassis/balance.h create mode 100644 application/chassis/cal_phi0_dot.c create mode 100644 application/chassis/cal_phi0_dot.h create mode 100644 modules/algorithm/LQR.c create mode 100644 modules/algorithm/LQR.h diff --git a/application/chassis/balance.c b/application/chassis/balance.c new file mode 100644 index 0000000..c1dd908 --- /dev/null +++ b/application/chassis/balance.c @@ -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] + */ diff --git a/application/chassis/balance.h b/application/chassis/balance.h new file mode 100644 index 0000000..f0ca8b4 --- /dev/null +++ b/application/chassis/balance.h @@ -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 diff --git a/application/chassis/cal_phi0_dot.c b/application/chassis/cal_phi0_dot.c new file mode 100644 index 0000000..722c125 --- /dev/null +++ b/application/chassis/cal_phi0_dot.c @@ -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] + */ diff --git a/application/chassis/cal_phi0_dot.h b/application/chassis/cal_phi0_dot.h new file mode 100644 index 0000000..a5dec13 --- /dev/null +++ b/application/chassis/cal_phi0_dot.h @@ -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 diff --git a/application/chassis/chassis.c b/application/chassis/chassis.c index 88d4abb..5a1b4ad 100644 --- a/application/chassis/chassis.c +++ b/application/chassis/chassis.c @@ -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 diff --git a/application/chassis/leg.c b/application/chassis/leg.c index 7c200d3..f83a0d4 100644 --- a/application/chassis/leg.c +++ b/application/chassis/leg.c @@ -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); diff --git a/application/chassis/leg.h b/application/chassis/leg.h index cf0d903..d416570 100644 --- a/application/chassis/leg.h +++ b/application/chassis/leg.h @@ -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 diff --git a/application/robot.c b/application/robot.c index e06dee4..23fd7bc 100644 --- a/application/robot.c +++ b/application/robot.c @@ -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) diff --git a/application/robot_def.h b/application/robot_def.h index e66ac8f..235f480 100644 --- a/application/robot_def.h +++ b/application/robot_def.h @@ -18,7 +18,7 @@ /* 开发板类型定义,烧录时注意不要弄错对应功能;修改定义后需要重新编译,只能存在一个定义! */ #define ONE_BOARD // 单板控制整车 -// #define CHASSIS_BOARD //底盘板 +//#define CHASSIS_BOARD //底盘板 // #define GIMBAL_BOARD //云台板 #define VISION_USE_VCP // 使用虚拟串口发送视觉数据 diff --git a/application/shoot/shoot.c b/application/shoot/shoot.c index e19af5c..cde05da 100644 --- a/application/shoot/shoot.c +++ b/application/shoot/shoot.c @@ -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); + } \ No newline at end of file diff --git a/modules/algorithm/LQR.c b/modules/algorithm/LQR.c new file mode 100644 index 0000000..cdda5f2 --- /dev/null +++ b/modules/algorithm/LQR.c @@ -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); +} \ No newline at end of file diff --git a/modules/algorithm/LQR.h b/modules/algorithm/LQR.h new file mode 100644 index 0000000..5dacaab --- /dev/null +++ b/modules/algorithm/LQR.h @@ -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 diff --git a/modules/algorithm/user_lib.c b/modules/algorithm/user_lib.c index 5029109..f22a4bb 100644 --- a/modules/algorithm/user_lib.c +++ b/modules/algorithm/user_lib.c @@ -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; + } +} diff --git a/modules/algorithm/user_lib.h b/modules/algorithm/user_lib.h index 9a15097..e93a5d8 100644 --- a/modules/algorithm/user_lib.h +++ b/modules/algorithm/user_lib.h @@ -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 diff --git a/modules/imu/ins_task.c b/modules/imu/ins_task.c index efdcced..e53ede4 100644 --- a/modules/imu/ins_task.c +++ b/modules/imu/ins_task.c @@ -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); } diff --git a/modules/imu/ins_task.h b/modules/imu/ins_task.h index f42decf..4acac19 100644 --- a/modules/imu/ins_task.h +++ b/modules/imu/ins_task.h @@ -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; diff --git a/modules/motor/HTmotor/HT04.h b/modules/motor/HTmotor/HT04.h index b33be20..1a0162c 100644 --- a/modules/motor/HTmotor/HT04.h +++ b/modules/motor/HTmotor/HT04.h @@ -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 diff --git a/modules/motor/LKmotor/LK9025.c b/modules/motor/LKmotor/LK9025.c index c9fadbc..75f9047 100644 --- a/modules/motor/LKmotor/LK9025.c +++ b/modules/motor/LKmotor/LK9025.c @@ -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) diff --git a/modules/motor/LKmotor/LK9025.h b/modules/motor/LKmotor/LK9025.h index a5cef5a..9a62b1f 100644 --- a/modules/motor/LKmotor/LK9025.h +++ b/modules/motor/LKmotor/LK9025.h @@ -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; // 单圈角度 diff --git a/modules/motor/motor_task.c b/modules/motor/motor_task.c index 9a8f874..e8beb09 100644 --- a/modules/motor/motor_task.c +++ b/modules/motor/motor_task.c @@ -15,14 +15,14 @@ void MotorControlTask() /* 如果有对应的电机则取消注释,可以加入条件编译或者register对应的idx判断是否注册了电机 */ LKMotorControl(); - ECMotorControl(); + //ECMotorControl(); // legacy support // 由于ht04电机的反馈方式为接收到一帧消息后立刻回传,以此方式连续发送可能导致总线拥塞 // 为了保证高频率控制,HTMotor中提供了以任务方式启动控制的接口,可通过宏定义切换 // HTMotorControl(); // 将所有的CAN设备集中在一处发送,最高反馈频率仅能达到500Hz,为了更好的控制效果,应使用新的HTMotorControlInit()接口 - ServeoMotorControl(); + //ServeoMotorControl(); // StepMotorControl(); }