// // 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 sin_phi1 = arm_sin_f32(phi_1); float sin_phi4 = arm_sin_f32(phi_4); float cos_phi1 = arm_cos_f32(phi_1); float cos_phi4 = arm_cos_f32(phi_4); 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 = cos_phi1; d_phi0_tmp_tmp = sin_phi1; b_d_phi0_tmp_tmp = 0.15F * d_phi0_tmp_tmp; c_d_phi0_tmp_tmp = sin_phi4; 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 = cos_phi4; 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 * sin_phi1 - 0.15F * sin_phi4; e_d_phi0_tmp_tmp = 0.15F * sin_phi1 - 0.15F * sin_phi4; c_d_phi0_tmp_tmp = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; 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 * cos_phi1) + 0.15F * cos_phi4; c_d_phi0_tmp_tmp = 0.15F * sin_phi1 - 0.15F * sin_phi4; a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; b_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; c_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; d_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; e_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; f_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; b_out = (e_a * e_a + f_a * f_a) + 0.0729000047F; e_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; f_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; g_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; h_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; i_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; j_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; c_out = (i_a * i_a + j_a * j_a) + 0.0729000047F; i_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; j_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; k_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; a_tmp = 0.54F * ((0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4); j_a = (((j_a * j_a + k_a * k_a) + 0.0729000047F) - 0.0729000047F) + a_tmp; k_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; l_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; m_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; d_out = (l_a * l_a + m_a * m_a) + 0.0729000047F; l_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; b_a_tmp = 0.54F * (0.15F * sin_phi1 - 0.15F * sin_phi4); 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 * sin_phi1 - 0.15F * sin_phi4; m_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; l_a = (((l_a * l_a + m_a * m_a) + 0.0729000047F) - 0.0729000047F) + a_tmp; m_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; n_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; o_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; d_out = (n_a * n_a + o_a * o_a) + 0.0729000047F; n_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; o_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; p_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; q_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; r_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; s_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; e_out = (r_a * r_a + s_a * s_a) + 0.0729000047F; r_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; s_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; t_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; u_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; v_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; w_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; f_out = (v_a * v_a + w_a * w_a) + 0.0729000047F; v_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; w_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; x_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; y_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; ab_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; bb_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; cb_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; db_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; g_out = (cb_a * cb_a + db_a * db_a) + 0.0729000047F; cb_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; db_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; eb_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; fb_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; gb_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; hb_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; h_out = (gb_a * gb_a + hb_a * hb_a) + 0.0729000047F; gb_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; hb_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; ib_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; hb_a = (((hb_a * hb_a + ib_a * ib_a) + 0.0729000047F) - 0.0729000047F) + a_tmp; ib_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; jb_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; kb_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; i_out = (jb_a * jb_a + kb_a * kb_a) + 0.0729000047F; jb_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; 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 * sin_phi1 - 0.15F * sin_phi4; kb_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; jb_a = (((jb_a * jb_a + kb_a * kb_a) + 0.0729000047F) - 0.0729000047F) + a_tmp; kb_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; lb_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; mb_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; i_out = (lb_a * lb_a + mb_a * mb_a) + 0.0729000047F; lb_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; mb_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; nb_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; 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 * cos_phi1; lb_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; mb_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; nb_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; i_out = (mb_a * mb_a + nb_a * nb_a) + 0.0729000047F; mb_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; nb_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; ob_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; 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 * sin_phi1; mb_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; nb_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; ob_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; i_out = (nb_a * nb_a + ob_a * ob_a) + 0.0729000047F; nb_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; ob_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; pb_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; 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 * cos_phi1; nb_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; ob_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; pb_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; i_out = (ob_a * ob_a + pb_a * pb_a) + 0.0729000047F; ob_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; pb_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; qb_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; rb_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; sb_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; tb_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; ub_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; vb_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; j_out = (ub_a * ub_a + vb_a * vb_a) + 0.0729000047F; ub_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; vb_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; wb_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; xb_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; yb_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; ac_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; k_out = (yb_a * yb_a + ac_a * ac_a) + 0.0729000047F; yb_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; ac_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; bc_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; ac_a = (((ac_a * ac_a + bc_a * bc_a) + 0.0729000047F) - 0.0729000047F) + a_tmp; bc_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; cc_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; dc_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; l_out = (cc_a * cc_a + dc_a * dc_a) + 0.0729000047F; cc_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; 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 * sin_phi1 - 0.15F * sin_phi4; dc_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; cc_a = (((cc_a * cc_a + dc_a * dc_a) + 0.0729000047F) - 0.0729000047F) + a_tmp; dc_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; ec_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; fc_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; l_out = (ec_a * ec_a + fc_a * fc_a) + 0.0729000047F; ec_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; fc_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; gc_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; hc_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; ic_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; jc_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; m_out = (ic_a * ic_a + jc_a * jc_a) + 0.0729000047F; ic_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; jc_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; kc_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; lc_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; mc_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; nc_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; n_out = (mc_a * mc_a + nc_a * nc_a) + 0.0729000047F; mc_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; nc_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; oc_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; pc_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; qc_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; rc_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; sc_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; tc_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; o_out = (sc_a * sc_a + tc_a * tc_a) + 0.0729000047F; sc_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; tc_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; uc_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; vc_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; wc_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; xc_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; p_out = (wc_a * wc_a + xc_a * xc_a) + 0.0729000047F; wc_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; xc_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; yc_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; xc_a = (((xc_a * xc_a + yc_a * yc_a) + 0.0729000047F) - 0.0729000047F) + a_tmp; yc_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; ad_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; bd_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; q_out = (ad_a * ad_a + bd_a * bd_a) + 0.0729000047F; ad_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; 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 * sin_phi1 - 0.15F * sin_phi4; bd_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; ad_a = (((ad_a * ad_a + bd_a * bd_a) + 0.0729000047F) - 0.0729000047F) + a_tmp; bd_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; cd_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; dd_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; q_out = (cd_a * cd_a + dd_a * dd_a) + 0.0729000047F; cd_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; dd_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; ed_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; 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 * cos_phi1; cd_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; dd_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; ed_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; q_out = (dd_a * dd_a + ed_a * ed_a) + 0.0729000047F; dd_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; ed_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; fd_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; 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 * sin_phi1; dd_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; ed_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; fd_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; q_out = (ed_a * ed_a + fd_a * fd_a) + 0.0729000047F; ed_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; fd_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; gd_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; 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 * cos_phi1; 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); } /* Function Definitions */ /* * Arguments : float phi_1 * float phi_4 * float d_phi1 * float d_phi4 * Return Type : float */ float get_dL0(float phi_1, float phi_4, float d_phi1, float d_phi4) { //空间换时间 提前算好三角函数 float sin_phi1 = arm_sin_f32(phi_1); float sin_phi4 = arm_sin_f32(phi_4); float cos_phi1 = arm_cos_f32(phi_1); float cos_phi4 = arm_cos_f32(phi_4); float a; float a_tmp; float ab_a; float ac_a; float ad_a; float b_a; float b_a_tmp; float b_d_L0_tmp; float b_d_L0_tmp_tmp; float b_d_L0_tmp_tmp_tmp; float b_out; float bb_a; float bc_a; float bd_a; float c_a; float c_d_L0_tmp; float c_d_L0_tmp_tmp; float c_out; float cb_a; float cc_a; float cd_a; float d_L0_tmp; float d_L0_tmp_tmp; float d_L0_tmp_tmp_tmp; float d_a; float d_d_L0_tmp; float d_out; float db_a; float dc_a; float dd_a; float e_a; float e_d_L0_tmp; float e_out; float eb_a; float ec_a; float ed_a; float f_a; float f_d_L0_tmp; float f_out; float fb_a; float fc_a; float g_a; float g_d_L0_tmp; float g_out; float gb_a; float gc_a; float h_a; float h_d_L0_tmp; float h_out; float hb_a; float hc_a; float i_a; float i_d_L0_tmp; float i_out; float ib_a; float ic_a; float j_a; float j_d_L0_tmp; float j_out; float jb_a; float jc_a; float k_a; float k_d_L0_tmp; float k_out; float kb_a; float kc_a; float l_a; float l_d_L0_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_L0_tmp = cos_phi4; d_L0_tmp_tmp = sin_phi4; d_L0_tmp_tmp_tmp = sin_phi1; b_d_L0_tmp_tmp = 0.15F * d_L0_tmp_tmp_tmp; b_d_L0_tmp = b_d_L0_tmp_tmp - 0.15F * d_L0_tmp_tmp; b_d_L0_tmp_tmp_tmp = cos_phi1; c_d_L0_tmp_tmp = 0.15F * b_d_L0_tmp_tmp_tmp; c_d_L0_tmp = (0.15F - c_d_L0_tmp_tmp) + 0.15F * d_L0_tmp; d_d_L0_tmp = 0.54F * c_d_L0_tmp; e_d_L0_tmp = 0.54F * b_d_L0_tmp; f_d_L0_tmp = 0.3F * d_L0_tmp * b_d_L0_tmp; g_d_L0_tmp = 0.3F * d_L0_tmp_tmp * c_d_L0_tmp; h_d_L0_tmp = 2.0F * (f_d_L0_tmp + g_d_L0_tmp); i_d_L0_tmp = 0.0874800086F * d_L0_tmp * b_d_L0_tmp; j_d_L0_tmp = 0.0874800086F * d_L0_tmp_tmp * c_d_L0_tmp; d_L0_tmp *= 0.0810000077F; f_d_L0_tmp = (0.0810000077F * d_L0_tmp_tmp + f_d_L0_tmp) + g_d_L0_tmp; g_d_L0_tmp = 0.3F * b_d_L0_tmp_tmp_tmp * b_d_L0_tmp; d_L0_tmp_tmp = 0.3F * d_L0_tmp_tmp_tmp * c_d_L0_tmp; k_d_L0_tmp = 2.0F * (g_d_L0_tmp + d_L0_tmp_tmp); b_d_L0_tmp *= 0.0874800086F * b_d_L0_tmp_tmp_tmp; c_d_L0_tmp *= 0.0874800086F * d_L0_tmp_tmp_tmp; l_d_L0_tmp = 0.0810000077F * b_d_L0_tmp_tmp_tmp; g_d_L0_tmp = (0.0810000077F * d_L0_tmp_tmp_tmp + g_d_L0_tmp) + d_L0_tmp_tmp; d_L0_tmp_tmp = 0.15F * sin_phi1 - 0.15F * sin_phi4; b_d_L0_tmp_tmp_tmp = 0.15F * sin_phi1 - 0.15F * sin_phi4; d_L0_tmp_tmp_tmp = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; out = (b_d_L0_tmp_tmp_tmp * b_d_L0_tmp_tmp_tmp + d_L0_tmp_tmp_tmp * d_L0_tmp_tmp_tmp) + 0.0729000047F; b_d_L0_tmp_tmp_tmp = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; d_L0_tmp_tmp_tmp = 0.15F * sin_phi1 - 0.15F * sin_phi4; a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; b_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; c_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; d_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; e_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; f_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; b_out = (e_a * e_a + f_a * f_a) + 0.0729000047F; e_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; f_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; g_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; h_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; i_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; j_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; c_out = (i_a * i_a + j_a * j_a) + 0.0729000047F; i_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; j_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; k_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; a_tmp = 0.54F * ((0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4); j_a = (((j_a * j_a + k_a * k_a) + 0.0729000047F) - 0.0729000047F) + a_tmp; k_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; l_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; m_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; d_out = (l_a * l_a + m_a * m_a) + 0.0729000047F; l_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; m_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; n_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; o_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; p_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; q_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; e_out = (p_a * p_a + q_a * q_a) + 0.0729000047F; p_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; b_a_tmp = 0.54F * (0.15F * sin_phi1 - 0.15F * sin_phi4); o_a = sqrtf((0.291600019F * (o_a * o_a) - (e_out - 0.0729000047F) * (e_out - 0.0729000047F)) + 0.291600019F * (p_a * p_a)) - b_a_tmp; p_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; q_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; p_a = (((p_a * p_a + q_a * q_a) + 0.0729000047F) - 0.0729000047F) + a_tmp; q_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; r_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; s_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; e_out = (r_a * r_a + s_a * s_a) + 0.0729000047F; r_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; s_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; t_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; u_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; v_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; w_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; f_out = (v_a * v_a + w_a * w_a) + 0.0729000047F; v_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; w_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; x_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; y_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; ab_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; bb_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; cb_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; db_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; g_out = (cb_a * cb_a + db_a * db_a) + 0.0729000047F; cb_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; db_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; eb_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; fb_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; gb_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; hb_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; h_out = (gb_a * gb_a + hb_a * hb_a) + 0.0729000047F; gb_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; hb_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; ib_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; hb_a = (((hb_a * hb_a + ib_a * ib_a) + 0.0729000047F) - 0.0729000047F) + a_tmp; ib_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; jb_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; kb_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; i_out = (jb_a * jb_a + kb_a * kb_a) + 0.0729000047F; jb_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; 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 * sin_phi1 - 0.15F * sin_phi4; kb_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; jb_a = (((jb_a * jb_a + kb_a * kb_a) + 0.0729000047F) - 0.0729000047F) + a_tmp; kb_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; lb_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; mb_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; i_out = (lb_a * lb_a + mb_a * mb_a) + 0.0729000047F; lb_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; mb_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; nb_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; 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 * cos_phi1; lb_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; mb_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; nb_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; i_out = (mb_a * mb_a + nb_a * nb_a) + 0.0729000047F; mb_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; nb_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; ob_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; 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 * sin_phi1; mb_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; nb_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; ob_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; i_out = (nb_a * nb_a + ob_a * ob_a) + 0.0729000047F; nb_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; ob_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; pb_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; qb_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; rb_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; sb_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; tb_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; ub_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; j_out = (tb_a * tb_a + ub_a * ub_a) + 0.0729000047F; tb_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; ub_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; vb_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; wb_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; xb_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; yb_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; k_out = (xb_a * xb_a + yb_a * yb_a) + 0.0729000047F; xb_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; yb_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; ac_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; yb_a = (((yb_a * yb_a + ac_a * ac_a) + 0.0729000047F) - 0.0729000047F) + a_tmp; ac_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; bc_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; cc_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; l_out = (bc_a * bc_a + cc_a * cc_a) + 0.0729000047F; bc_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; ac_a = sqrtf((0.291600019F * (ac_a * ac_a) - (l_out - 0.0729000047F) * (l_out - 0.0729000047F)) + 0.291600019F * (bc_a * bc_a)) - b_a_tmp; bc_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; cc_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; bc_a = (((bc_a * bc_a + cc_a * cc_a) + 0.0729000047F) - 0.0729000047F) + a_tmp; cc_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; dc_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; ec_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; l_out = (dc_a * dc_a + ec_a * ec_a) + 0.0729000047F; dc_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; ec_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; fc_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; gc_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; hc_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; ic_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; m_out = (hc_a * hc_a + ic_a * ic_a) + 0.0729000047F; hc_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; ic_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; jc_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; kc_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; lc_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; mc_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; n_out = (lc_a * lc_a + mc_a * mc_a) + 0.0729000047F; lc_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; mc_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; nc_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; oc_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; pc_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; qc_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; rc_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; sc_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; o_out = (rc_a * rc_a + sc_a * sc_a) + 0.0729000047F; rc_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; sc_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; tc_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; uc_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; vc_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; wc_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; p_out = (vc_a * vc_a + wc_a * wc_a) + 0.0729000047F; vc_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; wc_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; xc_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; wc_a = (((wc_a * wc_a + xc_a * xc_a) + 0.0729000047F) - 0.0729000047F) + a_tmp; xc_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; yc_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; ad_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; q_out = (yc_a * yc_a + ad_a * ad_a) + 0.0729000047F; yc_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; xc_a = sqrtf((0.291600019F * (xc_a * xc_a) - (q_out - 0.0729000047F) * (q_out - 0.0729000047F)) + 0.291600019F * (yc_a * yc_a)) - b_a_tmp; yc_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; ad_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; yc_a = (((yc_a * yc_a + ad_a * ad_a) + 0.0729000047F) - 0.0729000047F) + a_tmp; ad_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; bd_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; cd_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; q_out = (bd_a * bd_a + cd_a * cd_a) + 0.0729000047F; bd_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; cd_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; dd_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; ad_a = (0.27F * arm_cos_f32(2.0F * atanf((sqrtf((0.291600019F * (ad_a * ad_a) - (q_out - 0.0729000047F) * (q_out - 0.0729000047F)) + 0.291600019F * (bd_a * bd_a)) - b_a_tmp) / ((((cd_a * cd_a + dd_a * dd_a) + 0.0729000047F) - 0.0729000047F) + a_tmp))) - 0.075F) + 0.15F * cos_phi1; bd_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; cd_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; dd_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; q_out = (cd_a * cd_a + dd_a * dd_a) + 0.0729000047F; cd_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; dd_a = 0.15F * sin_phi1 - 0.15F * sin_phi4; ed_a = (0.15F - 0.15F * cos_phi1) + 0.15F * cos_phi4; bd_a = 0.27F * arm_sin_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.15F * sin_phi1; return d_phi4 * (1.08F * arm_sin_f32(2.0F * atanf((sqrtf((0.291600019F * (d_L0_tmp_tmp * d_L0_tmp_tmp) - (out - 0.0729000047F) * (out - 0.0729000047F)) + 0.291600019F * (b_d_L0_tmp_tmp_tmp * b_d_L0_tmp_tmp_tmp)) - e_d_L0_tmp) / ((((d_L0_tmp_tmp_tmp * d_L0_tmp_tmp_tmp + a * a) + 0.0729000047F) - 0.0729000047F) + d_d_L0_tmp))) * ((((i_d_L0_tmp - h_d_L0_tmp * (((b_a * b_a + c_a * c_a) + 0.0729000047F) - 0.0729000047F)) + j_d_L0_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_L0_tmp) / ((((f_a * f_a + g_a * g_a) + 0.0729000047F) - 0.0729000047F) + d_d_L0_tmp) - (sqrtf((0.291600019F * (h_a * h_a) - (c_out - 0.0729000047F) * (c_out - 0.0729000047F)) + 0.291600019F * (i_a * i_a)) - e_d_L0_tmp) * f_d_L0_tmp / (j_a * j_a)) * ((0.27F * arm_cos_f32(2.0F * atanf((sqrtf((0.291600019F * (k_a * k_a) - (d_out - 0.0729000047F) * (d_out - 0.0729000047F)) + 0.291600019F * (l_a * l_a)) - e_d_L0_tmp) / ((((m_a * m_a + n_a * n_a) + 0.0729000047F) - 0.0729000047F) + d_d_L0_tmp))) - 0.075F) + c_d_L0_tmp_tmp) / (o_a * o_a / (p_a * p_a) + 1.0F) - 1.08F * arm_cos_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_L0_tmp) / ((((s_a * s_a + t_a * t_a) + 0.0729000047F) - 0.0729000047F) + d_d_L0_tmp))) * (0.27F * 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_L0_tmp) / ((((w_a * w_a + x_a * x_a) + 0.0729000047F) - 0.0729000047F) + d_d_L0_tmp))) + b_d_L0_tmp_tmp) * ((((i_d_L0_tmp - h_d_L0_tmp * (((y_a * y_a + ab_a * ab_a) + 0.0729000047F) - 0.0729000047F)) + j_d_L0_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_L0_tmp) / ((((db_a * db_a + eb_a * eb_a) + 0.0729000047F) - 0.0729000047F) + d_d_L0_tmp) - (sqrtf((0.291600019F * (fb_a * fb_a) - (h_out - 0.0729000047F) * (h_out - 0.0729000047F)) + 0.291600019F * (gb_a * gb_a)) - e_d_L0_tmp) * f_d_L0_tmp / (hb_a * hb_a)) / (ib_a * ib_a / (jb_a * jb_a) + 1.0F)) / (2.0F * sqrtf(kb_a * kb_a + lb_a * lb_a)) - d_phi1 * (2.0F * (b_d_L0_tmp_tmp + 0.54F * arm_sin_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)) - e_d_L0_tmp) / ((((ob_a * ob_a + pb_a * pb_a) + 0.0729000047F) - 0.0729000047F) + d_d_L0_tmp))) * ((((b_d_L0_tmp - k_d_L0_tmp * (((qb_a * qb_a + rb_a * rb_a) + 0.0729000047F) - 0.0729000047F)) + c_d_L0_tmp) / (2.0F * sqrtf((0.291600019F * (sb_a * sb_a) - (j_out - 0.0729000047F) * (j_out - 0.0729000047F)) + 0.291600019F * (tb_a * tb_a))) - l_d_L0_tmp) / ((((ub_a * ub_a + vb_a * vb_a) + 0.0729000047F) - 0.0729000047F) + d_d_L0_tmp) - (sqrtf((0.291600019F * (wb_a * wb_a) - (k_out - 0.0729000047F) * (k_out - 0.0729000047F)) + 0.291600019F * (xb_a * xb_a)) - e_d_L0_tmp) * g_d_L0_tmp / (yb_a * yb_a)) / (ac_a * ac_a / (bc_a * bc_a) + 1.0F)) * ((0.27F * arm_cos_f32(2.0F * atanf((sqrtf((0.291600019F * (cc_a * cc_a) - (l_out - 0.0729000047F) * (l_out - 0.0729000047F)) + 0.291600019F * (dc_a * dc_a)) - e_d_L0_tmp) / ((((ec_a * ec_a + fc_a * fc_a) + 0.0729000047F) - 0.0729000047F) + d_d_L0_tmp))) - 0.075F) + c_d_L0_tmp_tmp) - 2.0F * (0.27F * arm_sin_f32(2.0F * atanf((sqrtf((0.291600019F * (gc_a * gc_a) - (m_out - 0.0729000047F) * (m_out - 0.0729000047F)) + 0.291600019F * (hc_a * hc_a)) - e_d_L0_tmp) / ((((ic_a * ic_a + jc_a * jc_a) + 0.0729000047F) - 0.0729000047F) + d_d_L0_tmp))) + b_d_L0_tmp_tmp) * (c_d_L0_tmp_tmp + 0.54F * arm_cos_f32(2.0F * atanf((sqrtf((0.291600019F * (kc_a * kc_a) - (n_out - 0.0729000047F) * (n_out - 0.0729000047F)) + 0.291600019F * (lc_a * lc_a)) - e_d_L0_tmp) / ((((mc_a * mc_a + nc_a * nc_a) + 0.0729000047F) - 0.0729000047F) + d_d_L0_tmp))) * ((((b_d_L0_tmp - k_d_L0_tmp * (((oc_a * oc_a + pc_a * pc_a) + 0.0729000047F) - 0.0729000047F)) + c_d_L0_tmp) / (2.0F * sqrtf((0.291600019F * (qc_a * qc_a) - (o_out - 0.0729000047F) * (o_out - 0.0729000047F)) + 0.291600019F * (rc_a * rc_a))) - l_d_L0_tmp) / ((((sc_a * sc_a + tc_a * tc_a) + 0.0729000047F) - 0.0729000047F) + d_d_L0_tmp) - (sqrtf((0.291600019F * (uc_a * uc_a) - (p_out - 0.0729000047F) * (p_out - 0.0729000047F)) + 0.291600019F * (vc_a * vc_a)) - e_d_L0_tmp) * g_d_L0_tmp / (wc_a * wc_a)) / (xc_a * xc_a / (yc_a * yc_a) + 1.0F))) / (2.0F * sqrtf(ad_a * ad_a + bd_a * bd_a)); } /* * File trailer for get_dphi0.c * * [EOF] */