LEGACY: older hexapod sketches (Hexapod_NEW, HexapodRobot)

From https://mangavalk.com/Hexapod/Hexapod @ 7945005 (its Hexapod_Teensy
is identical to the copy already in LEGACY). Earlier electronics
generation: on-board IK driving an external SSC-32-style servo
controller. Notes on what still applies are in CLAUDE.md.

Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>

2026-current/LEGACY/Hexapod_NEW/FootClass.h

@@ -0,0 +1,259 @@
+// Version 1
+
+#ifndef Foot_H
+#define Foot_H
+
+#include "Vector.h";
+#include "Global.h"
+
+const double pi = 3.14159265359;
+
+class Foot
+{
+  public:
+    float TranslateX, TranslateY, TranslateZ;
+    boolean Invert;
+
+    void SetFootCoordinates(Vector P)
+    {
+      Xi = P.x;
+      Yi = P.y;
+      Zi = P.z;
+    }
+
+    // ?used to put in position from body?
+    void Translate(int x, int y, int z)
+    {
+      TranslateX = x;
+      TranslateY = y;
+      TranslateZ = z;
+    }
+
+    void Translate(Vector i)
+    {
+      TranslateX = i.x;
+      TranslateY = i.y;
+      TranslateZ = i.z;
+    }
+
+    //  Inverse Kinemetics Region
+    double l3, s, O1, O2, O3, X2, Y2, Z2;
+    double l1, l2;
+    double Xi, Yi, Zi;
+    double X, Y, Z;
+    boolean error;
+
+    void RotateX()
+    {
+      Y = Y * cos(radians(rotationX)) - Z * sin(radians(rotationX));
+      Z = Y * sin(radians(rotationX)) + Z * cos(radians(rotationX));
+    }
+
+    void RotateY()
+    {
+      Z = Z * cos(radians(rotationY)) - X * sin(radians(rotationY));
+      X = Z * sin(radians(rotationY)) + X * cos(radians(rotationY));
+    }
+
+    void RotateZ()
+    {
+      X = X * cos(radians(rotationZ)) - Y * sin(radians(rotationZ));
+      Y = X * sin(radians(rotationZ)) + Y * cos(radians(rotationZ));
+    }
+
+    void RL3 ()
+    {
+      double x = X;
+      double z = Z;
+
+      if ((x == 0) && (z == 0))
+        l3 = 0;
+      else l3 = sqrt((float)(x * x + z * z));
+
+      if (x < 0)
+        l3 = l3 * -1;
+    }
+
+    float FO1 ()
+    {
+      RL3();
+
+      double x = X;
+      double z = Z;
+      double r;
+
+      if ((x != 0) & (z != 0)) {
+        double a = (z / x);
+        return atan((float)(a));
+      }
+      else
+        return atan(0.0); // Math Error Protection.
+    }
+
+    float T1 ()
+    {
+      float x = X;
+      float y = Y;
+      float z = Z;
+      float x2 = x * x;
+      float y2 = y * y;
+      float z2 = z * z;
+      float l32 = l3 * l3;
+
+      s = sqrt((float)(x2 + y2 + z2));
+
+      float t1 = 0;
+
+      float s2 = s * s;
+
+      if ((y == 0) && (l3 == 0))
+        t1 = 0; // Math Error Protection.
+      else
+        t1 = (atan2((float)(l3), (float)(y)));
+
+      float a = (l1 * l1) - (l2 * l2) + s2;
+      float b = 2 * l1 * s;
+      float t2 = acos(a / b);
+
+      float ret = 0;
+
+      ret = t1 + abs(t2);
+
+      return ret;
+    }
+
+    float T3 ()
+    {
+      return (acos((float)(((l1 * l1) + (l2 * l2) - (s * s)) / (2 * l1 * l2))));
+    }
+
+    void setleg()
+    {
+      setleg(Invert);
+    }
+
+    void setleg(boolean Side)
+    {
+      l1 = 10;
+      l2 = 15;
+
+      X = Xi;
+      Y = Yi;
+      Z = Zi;
+
+      //  Rotation from body point
+      if (Invert)
+      { //  Right
+        X = (-1 * X) + TranslateX;
+        Z = (-1 * Z) + TranslateZ;
+      }
+      else
+      {
+        X = Xi + TranslateX;
+        Z = (-1 * Z) + TranslateZ;
+      }
+
+      RotateY();
+      RotateX();
+      RotateZ();
+
+      //  Remove gaits
+      if (Invert)
+      { //  Right
+        X = -1 * (X - TranslateX);
+        Z = -1 * (Z - TranslateZ);
+      }
+      else
+      {
+        X -= TranslateX;
+        Z = -1 * (Z - TranslateZ);
+      }
+
+      if (Invert)
+      { //  right
+        double a = (90 - degrees(FO1()));
+        double b = (180 - degrees(T1()));
+        double c = degrees(T3());
+
+        /*if ((a >= 15) & (a <= 165)
+            & (b >= 15) & (b <= 165)
+            & (c >= 15) & (c <= 165))
+          {*/
+        error = false;
+
+        O1 = a;
+        O2 = b;
+        O3 = c;
+
+        double d = sin((float)(b * (pi / 180)));
+        if ( d != 0 )
+          X2 = d * l1;
+        else
+          X2 = 0;
+
+        d = cos((float)(b * (pi / 180)));
+        if ( d != 0 )
+          Y2 = d * l1;
+        else
+          Y2 = 0;
+
+        d = sin((float)((a - 90) * (pi / 180)));
+        if ( d != 0 )
+          Z2 = d * abs((float)(X2));
+        else
+          Z2 = 0;
+        /*}
+          else
+          {
+          error = true;
+          }*/
+      }
+      else
+      {
+        double a = (90 - degrees((float)(FO1())));
+        double b = degrees((float)(T1()));
+        double c = 180 - degrees((float)(T3()));
+
+        /*if ((a >= 15) & (a <= 165)
+            & (b >= 15) & (b <= 165)
+            & (c >= 15) & (c <= 165))
+          {*/
+        error = false;
+
+        O1 = a;
+        O2 = b;
+        O3 = c;
+
+        a = (90 - degrees((float)(FO1())));
+        b = (180 - degrees((float)(T1())));
+        c = degrees((float)(T3()));
+
+        double d = sin((float)(b * (pi / 180)));
+        if ( d != 0 )
+          X2 = d * l1;
+        else
+          X2 = 0;
+
+        d = sin((float)(b * (pi / 180)));
+        if ( d != 0 )
+          Y2 = d * l1;
+        else
+          Y2 = 0;
+
+        d = sin((float)((a - 90) * (pi / 180)));
+        if ( d != 0 )
+          Z2 = d * abs((float)(X2));
+        else
+          Z2 = 0;
+        /*}
+          else
+          {
+          error = true;
+          Serial.println("ERROR");
+          }*/
+      }
+    }
+};
+
+#endif
+