dual_angle_ik.cpp

// note PI10 is a constant with pi to ten places
void ikDualAngle(double targetX, double targetY, double targetZ) {
    double legLength, base, lift, reach;

    // leglength is our first hurdle
    legLength = sqrt(sq(targetX) + sq(targetZ));

    // base rotation -- not working on this yet - set it to zero
    base = 0;

    // lift servo position
    double a1 = atan(targetX/targetZ) * 180/PI10;
    double a2 = acos(
      (sq(RADIUS) - sq(HUMEROUS) - sq(legLength)) 
      /
      (-2 * legLength * HUMEROUS) 
    ) * 180/PI10;
    lift = 180 - (a1 + a2); // my joints are zero on the vertical

    // reach servo position
    reach = acos(
      (sq(legLength) - sq(RADIUS) - sq(HUMEROUS)) 
      /
      (-2 * RADIUS * HUMEROUS)
    ) * 180/PI10;

    dxLegState.angleBase = base;
    dxLegState.angleReach = reach;
    dxLegState.angleLift = lift;
}