1.
/******************************************/
/* dobot正解,由关节角度到空间位置
/******************************************/
void calculate_DOBOT_forward_Transform(float f_dobot[3]) {
float l1_sin, l1_cos, l2_sin, l2_cos, Vxy, Vz;
l1_sin = sin(f_dobot[THETA_2] / DOBOT_RAD2DEG) * Linkage_1;l1_cos = cos(f_dobot[THETA_2] / DOBOT_RAD2DEG) * Linkage_1;
l2_sin = sin(f_dobot[THETA_3] / DOBOT_RAD2DEG) * Linkage_2;
l2_cos = cos(f_dobot[THETA_3] / DOBOT_RAD2DEG) * Linkage_2;
Vxy = l1_sin + l2_sin;
Vz= l1_cos + l2_cos;
delta[X_AXIS] = Vxy * cos(f_dobot(THETA_1) / DOBOT_RAD2DEG);
delta[Y_AXIS] = Vxy * sin(f_dobot(THETA_1) / D