OK, I think I got all the formulas correct now. The answers make sense.

```
void setup()
{
Serial.begin(115200);
delay(200);
IK();
}
void loop() {}
void IK()
{
// It is traditional to label the sides of a triangle
// 'a', 'b', and 'c' with angle 'A' opposite side 'a',
// angle 'B' opposite side 'b', and angle 'C' opposite
// side 'c'.
const float a = 204.0; // upper leg length (mm)
const float b = 176.0; // lower leg length (mm)
// 'c' will be the length of the leg from the hip joint to the toe.
// 'C' will be the knee angle
// 'B' will be the upper leg angle
const float h = 290.0; // Height of hip joint
const float asq = a * a;
const float bsq = b * b;
const float ThighAngleVertical = 86.8; // Degrees
// P1
const float InitialX = 0.0;
const float InitialY = 0.0;
// P2
const float TargetX = 60.0;
const float TargetY = 0.0;
const float RangeX = TargetX - InitialX;
const float RangeY = TargetY - InitialY;
const float RadiusOfMotion = (TargetX - InitialX) / 2.0;
// Serial.print("asq=");
// Serial.print(asq);
// Serial.print(", bsq=");
// Serial.println(bsq);
for (float X = InitialX; X <= TargetX; X += 6.0)
{
float Y = sqrt(RadiusOfMotion * RadiusOfMotion - pow(X - RadiusOfMotion, 2)) + // Y for semicircle
InitialY + RangeY * (X - InitialX) / RangeX; // linear interpolation of Y motion
float LegAngle = degrees(atan2(X, (h - Y))); // leg angle relative to vertical
float c = sqrt(X * X + pow((h - Y), 2)); // Length of leg (hip to toe)
Serial.print("X=");
Serial.print(X);
Serial.print(", Y=");
Serial.print(Y);
Serial.print(", LegAngle=");
Serial.print(LegAngle); // Display in degrees
Serial.print(", c=");
Serial.print(c);
float csq = c * c;
// Law of Cosine
// Given the lengths of three sides of a triangle, a, b, and c,
// calculate the cosine of the knee angle 'C' opposite side 'c' (leg length).
float cosC = (asq + bsq - csq) / (2.0 * a * b);
Serial.print(", cosC=");
Serial.print(cosC);
// Given the lengths of three sides of a triangle, a, b, and c,
// calculate the cosine of the thigh angle 'B' opposite side 'b' (lower leg).
float cosB = (asq + csq - bsq) / (2.0 * a * c);
Serial.print(", cosB=");
Serial.print(cosB);
float KneeAngle = degrees(acos(cosC));
float ThighAngleFromLeg = degrees(acos(cosB));
float ThighAngle = ThighAngleVertical + LegAngle - ThighAngleFromLeg;
Serial.print(", KneeAngle=");
Serial.print(KneeAngle);
Serial.print(", ThighAngleFromLeg=");
Serial.print(ThighAngleFromLeg);
Serial.print(", ThighAngle=");
Serial.println(ThighAngle);
//setServo(0, ThighAngle);
//setServo(1, KneeAngle);
delay(500);
}
}
```

Results:

```
X=0.00, Y=0.00, LegAngle=0.00, c=290.00, cosC=-0.16, cosB=0.80, KneeAngle=99.22, ThighAngleFromLeg=36.80, ThighAngle=50.00
X=6.00, Y=18.00, LegAngle=1.26, c=272.07, cosC=-0.02, cosB=0.76, KneeAngle=91.14, ThighAngleFromLeg=40.30, ThighAngle=47.76
X=12.00, Y=24.00, LegAngle=2.58, c=266.27, cosC=0.02, cosB=0.75, KneeAngle=88.65, ThighAngleFromLeg=41.36, ThighAngle=48.02
X=18.00, Y=27.50, LegAngle=3.92, c=263.12, cosC=0.05, cosB=0.74, KneeAngle=87.32, ThighAngleFromLeg=41.93, ThighAngle=48.80
X=24.00, Y=29.39, LegAngle=5.26, c=261.71, cosC=0.06, cosB=0.74, KneeAngle=86.73, ThighAngleFromLeg=42.18, ThighAngle=49.89
X=30.00, Y=30.00, LegAngle=6.58, c=261.73, cosC=0.06, cosB=0.74, KneeAngle=86.73, ThighAngleFromLeg=42.17, ThighAngle=51.21
X=36.00, Y=29.39, LegAngle=7.87, c=263.08, cosC=0.05, cosB=0.74, KneeAngle=87.30, ThighAngleFromLeg=41.93, ThighAngle=52.73
X=42.00, Y=27.50, LegAngle=9.09, c=265.84, cosC=0.03, cosB=0.75, KneeAngle=88.47, ThighAngleFromLeg=41.44, ThighAngle=54.45
X=48.00, Y=24.00, LegAngle=10.23, c=270.30, cosC=-0.01, cosB=0.76, KneeAngle=90.37, ThighAngleFromLeg=40.63, ThighAngle=56.40
X=54.00, Y=18.00, LegAngle=11.23, c=277.31, cosC=-0.06, cosB=0.77, KneeAngle=93.44, ThighAngleFromLeg=39.31, ThighAngle=58.72
X=60.00, Y=0.00, LegAngle=11.69, c=296.14, cosC=-0.21, cosB=0.81, KneeAngle=102.15, ThighAngleFromLeg=35.52, ThighAngle=62.97
```