// // 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] */