From 09f74247145852c01240df6956729895cee1597b Mon Sep 17 00:00:00 2001 From: MangaValk Date: Sun, 25 Oct 2026 01:17:46 +0200 Subject: [PATCH] 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 --- 2026-current/CLAUDE.md | 17 +- 2026-current/LEGACY/HexapodRobot/Hexapod.h | 317 +++++++++++++++ .../LEGACY/HexapodRobot/HexapodRobot.ino | 17 + 2026-current/LEGACY/HexapodRobot/SerD.h | 47 +++ 2026-current/LEGACY/Hexapod_NEW/CustomTimer.h | 33 ++ 2026-current/LEGACY/Hexapod_NEW/FootClass.h | 259 ++++++++++++ 2026-current/LEGACY/Hexapod_NEW/Global.h | 30 ++ .../LEGACY/Hexapod_NEW/HexapodClass.h | 376 ++++++++++++++++++ .../LEGACY/Hexapod_NEW/Hexapod_NEW.ino | 53 +++ 2026-current/LEGACY/Hexapod_NEW/Servo.h | 104 +++++ 2026-current/LEGACY/Hexapod_NEW/Vector.h | 66 +++ 11 files changed, 1318 insertions(+), 1 deletion(-) create mode 100644 2026-current/LEGACY/HexapodRobot/Hexapod.h create mode 100644 2026-current/LEGACY/HexapodRobot/HexapodRobot.ino create mode 100644 2026-current/LEGACY/HexapodRobot/SerD.h create mode 100644 2026-current/LEGACY/Hexapod_NEW/CustomTimer.h create mode 100644 2026-current/LEGACY/Hexapod_NEW/FootClass.h create mode 100644 2026-current/LEGACY/Hexapod_NEW/Global.h create mode 100644 2026-current/LEGACY/Hexapod_NEW/HexapodClass.h create mode 100644 2026-current/LEGACY/Hexapod_NEW/Hexapod_NEW.ino create mode 100644 2026-current/LEGACY/Hexapod_NEW/Servo.h create mode 100644 2026-current/LEGACY/Hexapod_NEW/Vector.h diff --git a/2026-current/CLAUDE.md b/2026-current/CLAUDE.md index 40aacf8..7319973 100644 --- a/2026-current/CLAUDE.md +++ b/2026-current/CLAUDE.md @@ -16,7 +16,9 @@ sim/ robot firmware -> hexapod_sim.dll (CMake + g++), hea Hexapod_Simulation_Unity/ Unity 6000.6 project that loads the DLL (Assets/HexapodSim/) LEGACY/ owner's older material for reference (remote/remote.HMI = old Nextion UI, 6 pages; remote/teensy/ = old remote sketch from https://mangavalk.com/Hexapod/remote.git @ 492dece; - Hexapod_Teensy/ = old robot sketch: servo passthrough only, no IK - see "Old robot sketch" below) + Hexapod_Teensy/ = old robot sketch: servo passthrough only, no IK - see "Old robot sketch" below; + Hexapod_NEW/, HexapodRobot/ = older generations with their own IK, driving an external + SSC-32-style servo controller - all three from https://mangavalk.com/Hexapod/Hexapod @ 7945005) scripts/ PlatformIO helper ``` @@ -320,6 +322,19 @@ Robot (`robot/README.md` has detail): - tibia servo = −tibia (right), 180 + tibia (left), so 90 = knee bent 90°. - All of this is now in `Config.h` `SERVOS` (see "Servos" above). +- **Older sketches (`LEGACY/Hexapod_NEW/`, `LEGACY/HexapodRobot/`, 2014–2016 + era)**: an earlier electronics generation. They did IK on the board and + sent `#n Pxxxx … T0` text commands over `Serial1` to an external + SSC-32-style servo controller (channels 0–9 and 16–24). + - **Femur range:** it was already narrower there (600–2400 µs coxa/tibia, + 960–2100 µs femur over 0–180°), which corroborates the ≈6.7 µs/° femur + in `SERVOS`. + - **Links:** femur 10, tibia 15 (cm), with no coxa length in that IK. + - **Hip layout comment in `Hexapod_NEW/HexapodClass.h`:** width 9.5 cm + (front/rear) and 14 cm (middle), 10 cm front-to-middle-to-rear. The + Unity model has 10 / 13 / 10. Whichever chassis is current, measure + before trusting `LEG_MOUNT_*`. + - Nothing else from them applies to the current Teensy 3.6 hardware. - `RobotIMU.cpp` reads `(Wire.read() << 8) | Wire.read()`, and C++ leaves the order of those two calls unspecified (fixed in the remote's copy). - Heading for mode B is open-loop (integrated commanded ω). Fusing IMU yaw diff --git a/2026-current/LEGACY/HexapodRobot/Hexapod.h b/2026-current/LEGACY/HexapodRobot/Hexapod.h new file mode 100644 index 0000000..0b32d07 --- /dev/null +++ b/2026-current/LEGACY/HexapodRobot/Hexapod.h @@ -0,0 +1,317 @@ +// Version 1 + +#ifndef HEX +#define HEX + +#include "Arduino.h" +#include "SerD.h" +#include + +// Angle to Pulse Length converter. +#define smin0 600 +#define smin1 960 +#define smin2 600 +#define smin3 600 +#define smax0 2400 +#define smax1 2100 +#define smax2 2400 +#define smax3 2400 + +// Task Pipeline +#define H_TASK 001 +#define H_IK 002 +#define H_IKER 003 +#define H_ATPC 004 + +// ProgramControl +#define PC_EMPTY 101 +#define PC_NEEDDATA 102 + +// LAYOUT +// _U_ +// 0==/ \==3 +// >***< +// 1===|---|===4 +// >***< +// 2==\___/==5 + +class Hexapod +{ +private: + // Task Pipeline. + QueueList TaskQueue; + QueueList Parameter; + + // Program Control + int PcCommand; + + // Timing. + int OldTime; + int TimeBetweenNewPosition; + + /* Servo Data + # Including coordinate, Angle and Pulse Length. + */ + boolean DataReady; + SerD ServoData[6]; + + // Inverse Kinemetics. + float s; + float l3; +#define l2 15 +#define l1 10 + + void RL3 (int i) + { + float x = ServoData[i].x; + float z = ServoData[i].z; + + if ((x == 0) && (z == 0)) + l3 = 0; + else l3 = hypot(z, x); + + if (x < 0) + l3 *= -1; + } + + float O1 (int i) + { + RL3(i); + + float x = ServoData[i].x; + float z = ServoData[i].z; + float r; + + if ((x != 0) & (z != 0)){ + double a = (z / x); + return atan(a); + } + else + return atan(0.0); // Math Error Protection. + } + + float T1 (int i) + { + float x = ServoData[i].x; + float y = ServoData[i].y; + float z = ServoData[i].z; + float x2 = x * x; + float y2 = y * y; + float z2 = z * z; + float l32 = l3 * l3; + + s = sqrt(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(((l1*l1) + (l2*l2) - (s*s)) / (2 * l1 * l2))); + } + + void setleg (int i) + { + if (i < 3) + { + float a = ((float)90 - degrees(O1(i))); + float b = ((float)180 - degrees(T1(i))); + float c = degrees(T3()); + + if ((a >= 0) & (a <= 180) + & (b >= 0) & (b <= 180) + & (c >= 0) & (c <= 180)) + { + ServoData[i].o1 = a; + ServoData[i].o2 = b; + ServoData[i].o3 = c; + } + else + { + TaskQueue.push(H_IKER); + Parameter.push(i); + } + } + else + { + float a = ((float)90 + degrees(O1(i))); + float b = degrees(T1(i)); + float c = ((float)180 - degrees(T3())); + + if ((a >= 0) & (a <= 180) + & (b >= 0) & (b <= 180) + & (c >= 0) & (c <= 180)) + { + ServoData[i].o1 = a; + ServoData[i].o2 = b; + ServoData[i].o3 = c; + } + else + { + TaskQueue.push(H_IKER); + Parameter.push(i); + } + } + } + + void IK() + { + for (int i=0; i<6; i++) + setleg(i); + } + + // Angle to Pulse Length converter. + + int ConverttoMS(int Position, int low, int high) + { + int tmp = ( Position * ( ( high - low ) / 180 ) ); + return low + tmp; + } + + void ConvertAngleToPulseLength() + { + for (int i=0; i<6; i++) + { + ServoData[i].t1 = ConverttoMS(ServoData[i].o1, smin0, smax0); + ServoData[i].t2 = ConverttoMS(ServoData[i].o2, smin1, smax1); + ServoData[i].t3 = ConverttoMS(ServoData[i].o3, smin2, smax2); + } + } + + void ProgramControl( int Command ) + { + // Drop task from queue + TaskQueue.pop(); + + // Check Command. + switch (Command) + { + case PC_EMPTY: + { + + break; + } + + case PC_NEEDDATA: + { + /* + Queue = { + Get Move + Do IK + Convert To MS + Send } + */ + break; + } + + default: + break; + } + } + +public: + // Task Pipeline. + void loop() + { + PcCommand = 0; + do{ + /* + Runmonitoing methods and + Timingdependent methods. + */ + if (millis() >= (OldTime + TimeBetweenNewPosition)) + { + OldTime = millis(); + } + + //Add Task toqueue + TaskQueue.push(H_TASK); + + //task queue. + + switch ( TaskQueue.peek() ) + { + + case H_TASK: + { + + break; + } + + case H_IK: // Do InverseKinemetics. + { + IK(); + break; + } + + case H_IKER: // Inverse Kinemetics Error. + { + while ( !Parameter.isEmpty() ) + { + // Go troughtparameters. + /* + # W + # I + # P + */ + } + + break; + } + + case H_ATPC: // Angel to pulse length converter. + { + ConvertAngleToPulseLength(); + DataReady = true; + break; + } + + case H_SEND: + { + if (DataReady) + { + // Send + DataReady = false; + // Get New Move + } + else + { + PcCommand = PC_NEEDDATA; + } + break; + } + + default: // TaskList was empty. + { + // Do something when there is no task. + PcCommand = PC_EMPTY; + break; + } + } + + + //Program Control. + ProgramControl( PcCommand ); + } while (1); + } +}; + +#endif + diff --git a/2026-current/LEGACY/HexapodRobot/HexapodRobot.ino b/2026-current/LEGACY/HexapodRobot/HexapodRobot.ino new file mode 100644 index 0000000..0713ffc --- /dev/null +++ b/2026-current/LEGACY/HexapodRobot/HexapodRobot.ino @@ -0,0 +1,17 @@ +// Version 1 +#include "Hexapod.h" +#include + +void setup() +{ + +} + +Hexapod hexapod; + +void loop() +{ + hexapod.loop(); +} + + diff --git a/2026-current/LEGACY/HexapodRobot/SerD.h b/2026-current/LEGACY/HexapodRobot/SerD.h new file mode 100644 index 0000000..b8c088a --- /dev/null +++ b/2026-current/LEGACY/HexapodRobot/SerD.h @@ -0,0 +1,47 @@ +// Version 1 + +#ifndef SerD_H +#define SerD_H + +class SerD +{ +public: + + // Coordinate + int x; + int y; + int z; + + // Angles + int o1; + int o2; + int o3; + + // Pulse Length. + int t1; + int t2; + int t3; + + //Input + SerD& operator() (int X, int Y, int Z) + { + x = X; + y = Y; + z = Z; + + return *this; + } + + SerD& operator= (SerD& R) + { + x = R.x; + y = R.y; + z = R.z; + + return *this; + } + +}; + +#endif + diff --git a/2026-current/LEGACY/Hexapod_NEW/CustomTimer.h b/2026-current/LEGACY/Hexapod_NEW/CustomTimer.h new file mode 100644 index 0000000..cf40859 --- /dev/null +++ b/2026-current/LEGACY/Hexapod_NEW/CustomTimer.h @@ -0,0 +1,33 @@ +// Version 1 + +#ifndef CustomTimer_H +#define CustomTimer_H + +class CustomTimer +{ + int time; + boolean start; + public: CustomTimer() + { + start=false; + } + public: boolean Delay(int delay) + { + if (!start) + { + time = millis(); + start = true; + } + if (millis() - time >= delay) + { + start = false; + return true; + } + else + return false; + } +}; + +#endif + + diff --git a/2026-current/LEGACY/Hexapod_NEW/FootClass.h b/2026-current/LEGACY/Hexapod_NEW/FootClass.h new file mode 100644 index 0000000..f581e71 --- /dev/null +++ b/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 + diff --git a/2026-current/LEGACY/Hexapod_NEW/Global.h b/2026-current/LEGACY/Hexapod_NEW/Global.h new file mode 100644 index 0000000..a511889 --- /dev/null +++ b/2026-current/LEGACY/Hexapod_NEW/Global.h @@ -0,0 +1,30 @@ +// Version 1 + +#ifndef Global_H +#define Global_H + +float rotationX = 0; +float rotationY = 0; +float rotationZ = 0; + +CustomTimer timerAnimation = CustomTimer(); +CustomTimer TimerFPS_SERVO = CustomTimer(); + +const int FPS = 30; +int FPS_SERVO = 30; + +float counterRotation = 0; +int counterXRotation = 0; +int counterYRotation = 0; +int counterZRotation = 0; + +Vector *walk1 = new Vector[8]; + +float robotHeigth = 12; + +int rotationRange = 8; + +#endif + + + diff --git a/2026-current/LEGACY/Hexapod_NEW/HexapodClass.h b/2026-current/LEGACY/Hexapod_NEW/HexapodClass.h new file mode 100644 index 0000000..6ee0076 --- /dev/null +++ b/2026-current/LEGACY/Hexapod_NEW/HexapodClass.h @@ -0,0 +1,376 @@ +// Version 1 + +#ifndef Hexapod_H +#define Hexapod_H + +#include "FootClass.h"; +#include "Vector.h"; +#include "CustomTimer.h"; +#include "Global.h" +#include + +Foot foot1 = Foot(); +Foot foot2 = Foot(); +Foot foot3 = Foot(); +Foot foot4 = Foot(); +Foot foot5 = Foot(); +Foot foot6 = Foot(); + +// Spaces from body to feet +// 9.5 +// 14 +// 9.5 + +// 10 +// 10 + +// LAYOUT HARDWARE +// _U_ +// 1==/ \==4 +// >***< +// 2===|---|===5 +// >***< +// 3==\___/==6 + +// LAYOUT SOFTWARE +// _U_ +// 1==/ \==6 +// >***< +// 2===|---|===5 +// >***< +// 3==\___/==4 + +Vector point1 = Vector(4.75, 0, 10); +Vector point2 = Vector(7, 0, 0); +Vector point3 = Vector(4.75, 0, -10); +Vector point4 = Vector(-4.75, 0, -10); +Vector point5 = Vector(-7, 0, 0); +Vector point6 = Vector(-4.75, 0, 10); + +Vector fpoint1 = Vector(10, 5, 0); +Vector fpoint2 = Vector(10, 5, 0); // y negative is down from body. +Vector fpoint3 = Vector(10, 5, 0); +Vector fpoint4 = Vector(10, 5, 0); +Vector fpoint5 = Vector(10, 5, 0); +Vector fpoint6 = Vector(10, 5, 0); + +Vector* standup = new Vector[2]; + +// Animation new +int animationSpeed = 20; + +int ConverttoMS(double Position, int low, int high, int _min = 15, int _max = 165, int correction = 0) +{ + //Position += 90; + + if (Position < _min | Position > _max) + { + Position = 90; + Serial.println("ERROR"); + } + + int tmp = ( Position * ( ( high - low ) / 180 ) ); + + //Serial1.println(Position); + + return low + tmp + correction; +} + +void mSend(Foot f1, Foot f2, Foot f3, Foot f4, Foot f5, Foot f6) +{ + int smin0 = 600; + int smin1 = 960; + int smin2 = 600; + int smin3 = 600; + int smax0 = 2400; + int smax1 = 2100; + int smax2 = 2400; + int smax3 = 2400; + + Serial1.print(" #"); + Serial1.print(0); + Serial1.print(" P"); + Serial1.print(ConverttoMS(f4.O1, smin0, smax0)); + Serial1.print(" #"); + Serial1.print(1); + Serial1.print(" P"); + Serial1.print(ConverttoMS(f4.O2, smin1, smax1)); + Serial1.print(" #"); + Serial1.print(2); + Serial1.print(" P"); + Serial1.print(ConverttoMS(f4.O3, smin2, smax2)); + Serial1.print(" #"); + Serial1.print(3); + Serial1.print(" P"); + Serial1.print(ConverttoMS(f5.O1, smin0, smax0)); + Serial1.print(" #"); + Serial1.print(4); + Serial1.print(" P"); + Serial1.print(ConverttoMS(f5.O2, smin1, smax1)); + Serial1.print(" #"); + Serial1.print(5); + Serial1.print(" P"); + Serial1.print(ConverttoMS(f5.O3, smin2, smax2)); + Serial1.print(" #"); + Serial1.print(6); + Serial1.print(" P"); + Serial1.print(ConverttoMS(f6.O1, smin0, smax0)); + Serial1.print(" #"); + Serial1.print(7); + Serial1.print(" P"); + Serial1.print(ConverttoMS(f6.O2, smin1, smax1)); + Serial1.print(" #"); + Serial1.print(8); + Serial1.print(" P"); + Serial1.print(ConverttoMS(f6.O3, smin2, smax2)); + + Serial1.print(" #"); + Serial1.print(9); + Serial1.print(" P"); + Serial1.print(ConverttoMS(90, smin3, smax3)); + + Serial1.print(" #"); + Serial1.print(16); + Serial1.print(" P"); + Serial1.print(ConverttoMS(f1.O1, smin0, smax0)); + Serial1.print(" #"); + Serial1.print(17); + Serial1.print(" P"); + Serial1.print(ConverttoMS(f1.O2, smin1, smax1)); + Serial1.print(" #"); + Serial1.print(18); + Serial1.print(" P"); + Serial1.print(ConverttoMS(f1.O3, smin2, smax2)); + Serial1.print(" #"); + Serial1.print(19); + Serial1.print(" P"); + Serial1.print(ConverttoMS(f2.O1, smin0, smax0)); + Serial1.print(" #"); + Serial1.print(20); + Serial1.print(" P"); + Serial1.print(ConverttoMS(f2.O2, smin1, smax1)); + Serial1.print(" #"); + Serial1.print(21); + Serial1.print(" P"); + Serial1.print(ConverttoMS(f2.O3, smin2, smax2)); + Serial1.print(" #"); + Serial1.print(22); + Serial1.print(" P"); + Serial1.print(ConverttoMS(f3.O1, smin0, smax0)); + Serial1.print(" #"); + Serial1.print(23); + Serial1.print(" P"); + Serial1.print(ConverttoMS(f3.O2, smin1, smax1)); + Serial1.print(" #"); + Serial1.print(24); + Serial1.print(" P"); + Serial1.print(ConverttoMS(f3.O3, smin2, smax2)); + + Serial1.print(" T"); + Serial1.println(0); + + + + /*Serial << " #"; + Serial << 0; + Serial << " P"; + Serial << f4.O1; + Serial << " #"; + Serial << 1; + Serial << " P"; + Serial << f4.O2; + Serial << " #"; + Serial << 2; + Serial << " P"; + Serial << f4.O3; + Serial << " #"; + Serial << 3; + Serial << " P"; + Serial << f5.O1; + Serial << " #"; + Serial << 4; + Serial << " P"; + Serial << f5.O2; + Serial << " #"; + Serial << 5; + Serial << " P"; + Serial << f5.O3; + Serial << " #"; + Serial << 6; + Serial << " P"; + Serial << f6.O1; + Serial << " #"; + Serial << 7; + Serial << " P"; + Serial << f6.O2; + Serial << " #"; + Serial << 8; + Serial << " P"; + Serial << f6.O3; + + Serial << " #"; + Serial << 16; + Serial << " P"; + Serial << f1.O1; + Serial << " #"; + Serial << 17; + Serial << " P"; + Serial << f1.O2; + Serial << " #"; + Serial << 18; + Serial << " P"; + Serial << f1.O3; + Serial << " #"; + Serial << 19; + Serial << " P"; + Serial << f2.O1; + Serial << " #"; + Serial << 20; + Serial << " P"; + Serial << f2.O2; + Serial << " #"; + Serial << 21; + Serial << " P"; + Serial << f2.O3; + Serial << " #"; + Serial << 22; + Serial << " P"; + Serial << f3.O1; + Serial << " #"; + Serial << 23; + Serial << " P"; + Serial << f3.O2; + Serial << " #"; + Serial << 24; + Serial << " P"; + Serial << f3.O3; + + Serial << " T"; + Serial << 0 << "\n\r";*/ +} +////////////////////////////////////////////////// + +void DrawFeet() +{ + if (TimerFPS_SERVO.Delay(1000 / FPS_SERVO)) + { + foot1.Translate(point1); + foot1.SetFootCoordinates(fpoint1); + foot1.Invert = true; + foot1.setleg(); + + foot1.Translate(point2); + foot2.SetFootCoordinates(fpoint2); + foot2.Invert = true; + foot2.setleg(); + + foot1.Translate(point3); + foot3.SetFootCoordinates(fpoint3); + foot3.Invert = true; + foot3.setleg(); + + foot1.Translate(point4); + foot4.SetFootCoordinates(fpoint4); + foot4.Invert = false; + foot4.setleg(); + + foot1.Translate(point5); + foot5.SetFootCoordinates(fpoint5); + foot5.Invert = false; + foot5.setleg(); + + foot1.Translate(point6); + foot6.SetFootCoordinates(fpoint6); + foot6.Invert = false; + foot6.setleg(); + + mSend(foot1, foot2, foot3, foot4, foot5, foot6); + } +} + +/*void RobotWalk() + { + if (robotStanding) + { + //animationRepeat = true; + animationSpeed = 20; + + if (!animationRepeatComplete) + if (timerAnimation.Delay(animationSpeed)) + animationSubStep++; + + Serial1.println("robot walk animation"); + + foot1.SetFootCoordinates(Animation(walk1, 8, 0)); // BULLSHIT BUG + foot1.SetFootCoordinates(Animation(walk1, 8, 0)); + foot2.SetFootCoordinates(Animation(walk1, 8, 4)); + foot3.SetFootCoordinates(Animation(walk1, 8, 0));// + foot4.SetFootCoordinates(Animation(walk1, 8, 4));// + foot5.SetFootCoordinates(Animation(walk1, 8, 0)); + foot6.SetFootCoordinates(Animation(walk1, 8, 4)); + } + }*/ + +float _animationProgress = 0; +bool rotation = false; + +void RobotStandup() +{ + standup[0] = Vector(10, 5, 0); + standup[1] = Vector(10, 15, 0); + + if (timerAnimation.Delay(1000 / FPS)) + { + Serial.println("robot standup animation"); + + fpoint1.Lerp(standup[0], standup[1], _animationProgress); + fpoint2.Lerp(standup[0], standup[1], _animationProgress); + fpoint3.Lerp(standup[0], standup[1], _animationProgress); + fpoint4.Lerp(standup[0], standup[1], _animationProgress); + fpoint5.Lerp(standup[0], standup[1], _animationProgress); + fpoint6.Lerp(standup[0], standup[1], _animationProgress); + + Serial.println(point1.Print()); + + int _speed = 2; + + _animationProgress = _animationProgress < 1.0f ? (_animationProgress + (1.0f / (FPS * _speed))) : 1.0f; + + /*if (_animationProgress == 1) + { + int uhh = (millis() / 100) % 100; + if (rotation == true) + { + Serial.println("rotate up"); + rotationZ = map((millis() / 100) % 100, 0, 99, -25, 25); + if (uhh >= 99) + { + rotation = true; + uhh++; + } + } + else + { + Serial.println("rotate upside down"); + rotationZ = map((millis() / 100) % 100, 0, 99, 25, -25); + if (uhh >= 99) + { + rotation = false; + uhh++; + } + } + }*/ + } +} + +void DrawBody() +{ + RobotStandup(); + + //RobotWalk(); + + DrawFeet(); +} + +#endif + diff --git a/2026-current/LEGACY/Hexapod_NEW/Hexapod_NEW.ino b/2026-current/LEGACY/Hexapod_NEW/Hexapod_NEW.ino new file mode 100644 index 0000000..08881ba --- /dev/null +++ b/2026-current/LEGACY/Hexapod_NEW/Hexapod_NEW.ino @@ -0,0 +1,53 @@ +// Version 1 + +#include "Arduino.h" +#include "CustomTimer.h" +#include "HexapodClass.h" +#include "Vector.h" +#include "Global.h" + +//#include + +void setup() +{ + //wdt_enable(WDTO_1S); + + walk1[0] = Vector(12, robotHeigth, 2); + walk1[1] = Vector(12, robotHeigth, 0); + walk1[2] = Vector(12, robotHeigth, -2); + walk1[3] = Vector(12, robotHeigth, -2); + walk1[4] = Vector(12, robotHeigth - 10, -2); + walk1[5] = Vector(12, robotHeigth - 10, 0); + walk1[6] = Vector(12, robotHeigth - 10, 2); + walk1[7] = Vector(12, robotHeigth, 2); + + Serial.begin(115200); + Serial1.begin(115200); + + Serial.println(""); + Serial.println(" ____ ____ ___ "); + Serial.println("/\\ _`\\ __ /\\ _`\\ /\\_ \\ "); + Serial.println("\\ \\ \\L\\_\\/\\_\\ _ __ __\\ \\ \\L\\_\\//\\ \\ __ __ "); + Serial.println(" \\ \\ _\\/\\/\\ \\/\\`'__\\/'__`\\ \\ _\\/ \\ \\ \\ /\\ \\/\\ \\ "); + Serial.println(" \\ \\ \\/ \\ \\ \\ \\ \\//\\ __/\\ \\ \\/ \\_\\ \\_\\ \\ \\_\\ \\ "); + Serial.println(" \\ \\_\\ \\ \\_\\ \\_\\\\ \\____\\\\ \\_\\ /\\____\\\\/`____ \\ "); + Serial.println(" \\/_/ \\/_/\\/_/ \\/____/ \\/_/ \\/____/ `/___/> \\"); + Serial.println(" /\\___/"); + Serial.println(" \\/__/ "); + Serial.println(""); + Serial.println("Version 3.0"); + delay(800); +} + +void loop() +{ + // the program is alive...for now. + //wdt_reset(); + + // Rotation calculation, please use transformation matrixes instead. + //rotationX = ( counterXRotation % (rotationRange * 4) < (rotationRange * 2) ? -rotationRange + counterXRotation % (rotationRange * 2) : rotationRange + (-1 * (counterXRotation % (rotationRange * 2))) ); + //rotationY = ( counterYRotation % (rotationRange * 4) < (rotationRange * 2) ? -rotationRange + counterYRotation % (rotationRange * 2) : rotationRange + (-1 * (counterYRotation % (rotationRange * 2))) ); + //rotationZ = ( counterZRotation % (rotationRange * 4) < (rotationRange * 2) ? -rotationRange + counterZRotation % (rotationRange * 2) : rotationRange + (-1 * (counterZRotation % (rotationRange * 2))) ); + + DrawBody(); +} diff --git a/2026-current/LEGACY/Hexapod_NEW/Servo.h b/2026-current/LEGACY/Hexapod_NEW/Servo.h new file mode 100644 index 0000000..2d23692 --- /dev/null +++ b/2026-current/LEGACY/Hexapod_NEW/Servo.h @@ -0,0 +1,104 @@ +// Version 1 + +//#ifndef Servo_H +//#define Servo_H + +/*#include "FootClass.h"; + +int ConverttoMS(int Position, int low, int high) +{ + int tmp = ( Position * ( ( high - low ) / 180 ) ); + return low + tmp; +} + +void mSend(Foot f1, Foot f2, Foot f3, Foot f4, Foot f5, Foot f6) +{ + int smin0 = 600; + int smin1 = 960; + int smin2 = 600; + int smin3 = 600; + int smax0 = 2400; + int smax1 = 2100; + int smax2 = 2400; + int smax3 = 2400; + + Serial1.print(" #"); + Serial1.print(0); + Serial1.print(" P"); + Serial1.print(ConverttoMS(f1.O1, smin0, smax0)); + Serial1.print(" #"); + Serial1.print(1); + Serial1.print(" P"); + Serial1.print(ConverttoMS(f1.O2, smin1, smax1)); + Serial1.print(" #"); + Serial1.print(2); + Serial1.print(" P"); + Serial1.print(ConverttoMS(f1.O3, smin2, smax2)); + Serial1.print(" #"); + Serial1.print(4); + Serial1.print(" P"); + Serial1.print(ConverttoMS(f1.O1, smin0, smax0)); + Serial1.print(" #"); + Serial1.print(5); + Serial1.print(" P"); + Serial1.print(ConverttoMS(f1.O2, smin1, smax1)); + Serial1.print(" #"); + Serial1.print(6); + Serial1.print(" P"); + Serial1.print(ConverttoMS(f1.O3, smin2, smax2)); + Serial1.print(" #"); + Serial1.print(7); + Serial1.print(" P"); + Serial1.print(ConverttoMS(f1.O1, smin0, smax0)); + Serial1.print(" #"); + Serial1.print(8); + Serial1.print(" P"); + Serial1.print(ConverttoMS(f1.O2, smin1, smax1)); + Serial1.print(" #"); + Serial1.print(9); + Serial1.print(" P"); + Serial1.print(ConverttoMS(f1.O3, smin2, smax2)); + + Serial1.print(" #"); + Serial1.print(16); + Serial1.print(" P"); + Serial1.print(ConverttoMS(f1.O1, smin0, smax0)); + Serial1.print(" #"); + Serial1.print(17); + Serial1.print(" P"); + Serial1.print(ConverttoMS(f1.O2, smin1, smax1)); + Serial1.print(" #"); + Serial1.print(18); + Serial1.print(" P"); + Serial1.print(ConverttoMS(f1.O3, smin2, smax2)); + Serial1.print(" #"); + Serial1.print(19); + Serial1.print(" P"); + Serial1.print(ConverttoMS(f1.O1, smin0, smax0)); + Serial1.print(" #"); + Serial1.print(20); + Serial1.print(" P"); + Serial1.print(ConverttoMS(f1.O2, smin1, smax1)); + Serial1.print(" #"); + Serial1.print(21); + Serial1.print(" P"); + Serial1.print(ConverttoMS(f1.O3, smin2, smax2)); + Serial1.print(" #"); + Serial1.print(22); + Serial1.print(" P"); + Serial1.print(ConverttoMS(f1.O1, smin0, smax0)); + Serial1.print(" #"); + Serial1.print(23); + Serial1.print(" P"); + Serial1.print(ConverttoMS(f1.O2, smin1, smax1)); + Serial1.print(" #"); + Serial1.print(24); + Serial1.print(" P"); + Serial1.print(ConverttoMS(f1.O3, smin2, smax2)); + + Serial1.print(" T"); + Serial1.println(0); +}*/ + +//#endif + diff --git a/2026-current/LEGACY/Hexapod_NEW/Vector.h b/2026-current/LEGACY/Hexapod_NEW/Vector.h new file mode 100644 index 0000000..adca6c8 --- /dev/null +++ b/2026-current/LEGACY/Hexapod_NEW/Vector.h @@ -0,0 +1,66 @@ +// Version 1 + +#ifndef Servo_H +#define Servo_H + +class Vector +{ + public: + float x, y, z; + Vector(float X, float Y, float Z) + { + x = X; + y = Y; + z = Z; + } + void Init(float X, float Y, float Z) + { + x = X; + y = Y; + z = Z; + } + Vector() + { + + } + Vector Lerp(Vector start, Vector end, float percent) + { + Vector _new = (start + ((end - start) * percent)); + x = _new.x; + y = _new.y; + z = _new.z; + return _new; + } + Vector operator + (const Vector other) + { + Vector _new = Vector(); + _new.x = x + other.x; + _new.y = y + other.y; + _new.z = z + other.z; + + return _new; + } + Vector operator - (const Vector other) + { + Vector _new = Vector(); + _new.x = x - other.x; + _new.y = y - other.y; + _new.z = z - other.z; + + return _new; + } + Vector operator * (const float other) + { + Vector _new = Vector(x * other, y * other, z * other); + + return _new; + } + String Print() + { + return String(x) + " " + String(y) + " " + String(z); + } +}; + +#endif + +