marlin改动支持dobot机型-正反解

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);
    d