I have been working on the following code:
...
}
const float a = 204.0;
const float b = 176.0;
const float h = 306.0;
const float a_quadrat = a * a;
const float b_quadrat = b * b;
const float h_quadrat = h * h;
void setServo(int servo, int winkel) {
...
}
void loop() {
//Startposition
const float ThighStart = 140;
float KneeangleStart = degrees(acos((a_quadrat + b_quadrat - h_quadrat) / (2.0 * a * b)));
int Kneeangle = KneeangleStart;
setServo(0, ThighStart);
setServo(1, Kneeangle);
if ( ThighStart == 140) {
IK();
}
}
void IK() {
// P1: Startposition
const float InitialX = 0.0;
// P2: Endposition
const float TargetX = 200.0;
const float Radius = (TargetX - InitialX) / 2.0;
float ThighStart;
for (int X = 0 ; X <= 200; X += 10.0)
{
ThighStart = 140;
float Y = sqrt(Radius * Radius - pow(X - Radius, 2));
float c = sqrt(X * X + pow((h - Y), 2));
float angle = degrees(atan2(X, (h - Y)));
float c_quadrat = c * c;
float cosB = (a_quadrat + b_quadrat - c_quadrat) / (2.0 * a * b);
float Kneeangle = degrees(acos(cosB));
ThighStart = ThighStart - angle;
Serial.print("X=");
Serial.print(X);
Serial.print(", Y=");
Serial.print(Y);
Serial.print(", c = ");
Serial.print(c);
Serial.print(", cosB = ");
Serial.print(cosB);
Serial.print(", ThighStart = ");
Serial.print(ThighStart);
Serial.print(", Kneeangle = ");
Serial.println(Kneeangle);
setServo(0, ThighStart);
setServo(1, Kneeangle);
delay(1000);
}
}
While observing the KneeAngle values:
for X = 190, it prints KneeAngle = 62.49 with c = 260.96 which is wrong. Because for X = 190 it should be instead: c = 280.81 and KneeAngle = 86.41.
How does this error occur and how can it be corrected?
All the other values seem correct, except this one.