diff --git a/2012-simulation/processing-3d/.gitattributes b/2012-simulation/processing-3d/.gitattributes new file mode 100644 index 0000000..a348939 --- /dev/null +++ b/2012-simulation/processing-3d/.gitattributes @@ -0,0 +1,2 @@ +* text=auto +*.pde text diff --git a/2012-simulation/processing-3d/README.md b/2012-simulation/processing-3d/README.md new file mode 100644 index 0000000..6ed2555 --- /dev/null +++ b/2012-simulation/processing-3d/README.md @@ -0,0 +1,26 @@ +# Hexapod – Processing simulator + +A 3D simulator for the hexapod robot's leg kinematics, written in [Processing](https://processing.org/) (Java mode, 2.x). +It draws the body and six legs, solves the inverse kinematics per leg (`FootClass`) and animates body rotation +and translation (`HexapodClass`), cycling through a few camera views. + +The same `FootClass` / `HexapodClass` / `CustomTimer` design was later ported to the Arduino firmware in the +[Hexapod/hexapod](https://mangavalk.com/Hexapod/hexapod) repository (`Hexapod_NEW`). + +## Running + +Open `hexapod/hexapod.pde` in the Processing IDE and press Run. The sketch folder must stay named `hexapod`. + +## History + +This repository was rebuilt in 2026 from old archives. Each archive became one commit, dated by its newest file: + +| Tag | Source archive | Date | +|---------|-------------------|------------------| +| `rar-1` | `Hexapod 1.rar` | 2014-10-06 14:30 | +| `rar-2` | `hexapod 2.rar` | 2014-10-06 22:39 | +| `rar-3` | `hexapod 3.rar` | 2014-10-12 16:17 | +| `rar-4` | `hexapod 4.rar` | 2014-10-13 00:49 | + +The commit after `rar-4` is the last unarchived sketch folder; its real date is unknown (file dates were lost in a +2016-11-14 copy), so it carries that date. diff --git a/2012-simulation/processing-3d/hexapod/CustomTimer.pde b/2012-simulation/processing-3d/hexapod/CustomTimer.pde new file mode 100644 index 0000000..b911496 --- /dev/null +++ b/2012-simulation/processing-3d/hexapod/CustomTimer.pde @@ -0,0 +1,19 @@ +class CustomTimer +{ + int time; + boolean start=true; + public boolean delay(int delay) + { + if (start) + { + time = millis(); + start = false; + } + if (millis() - time >= delay) + { + start = true; + return true; + } else + return false; + } +} diff --git a/2012-simulation/processing-3d/hexapod/FootClass.pde b/2012-simulation/processing-3d/hexapod/FootClass.pde new file mode 100644 index 0000000..8c5e0fe --- /dev/null +++ b/2012-simulation/processing-3d/hexapod/FootClass.pde @@ -0,0 +1,272 @@ +class Foot +{ + float TranslateX, TranslateY, TranslateZ; + boolean leftLegorRight=false; + + void SetFootCoordinates(PVector P) + { + Xi = P.x; + Yi = P.y; + Zi = P.z; + } + + void Translate(int x, int y, int z) + { + TranslateX = x; + TranslateY = y; + TranslateZ = z; + } + + void Translate(PVector i) + { + TranslateX = i.x; + TranslateY = i.y; + TranslateZ = i.z; + } + + void DrawFoot() + { + setleg(leftLegorRight); + + pushMatrix(); + + translate(TranslateX, TranslateY, TranslateZ); + + stroke(!error?255:0, !error?0:255, 0); + noFill(); + beginShape(); + + Vertex(0, 0, 0); + Vertex(0, 0, 0); + if (leftLegorRight) + Vertex(-1*X2, -1*Y2, Z2); + else + Vertex(X2, -1*Y2, Z2); + + endShape(); + + beginShape(); + + if (leftLegorRight) + { + Vertex(-1*X2, -1*Y2, Z2); + Vertex(-1*X2, -1*Y2, Z2); + Vertex(-1*X, Y, -1*Z); + } else + { + Vertex(X2, -1*Y2, Z2); + Vertex(X2, -1*Y2, Z2); + Vertex(X, Y, -1*Z); + } + + endShape(); + + popMatrix(); + } + + // Inverse Kinemetics Region + double l3, s, O1, O2, O3, X2, Y2, Z2; + double l1=10, l2=15; + double Xi, Yi, Zi; + double X, Y, Z; + boolean error=false; + + 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 O1 () + { + 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. + } + + double T1 () + { + double x = X; + double y = Y; + double z = Z; + double x2 = x * x; + double y2 = y * y; + double z2 = z * z; + double l32 = l3 * l3; + + s = sqrt((float)(x2 + y2 + z2)); + + double t1 = 0; + + double s2 = s * s; + + if ((y == 0) && (l3 == 0)) + t1 = 0; // Math Error Protection. + else + t1 = (atan2((float)(l3), (float)(y))); + + double a = (l1 * l1) - (l2*l2) + s2; + double b = 2 * l1 * s; + double t2 = acos((float)(a / b)); + + double ret = 0; + + ret = t1 + abs((float)(t2)); + + return ret; + } + + double T3 () + { + return (acos((float)(((l1*l1) + (l2*l2) - (s*s)) / (2 * l1 * l2)))); + } + + double PI = 3.14159265; + + void setleg (boolean Side) + { + X = Xi; + Y = Yi; + Z = Zi; + + // Gaits + if (leftLegorRight) + { // Right + X = (-1*X) + TranslateX; + Z = (-1*Z) + TranslateZ; + } else + { + X = Xi + TranslateX; + Z = (-1*Z) + TranslateZ; + } + + RotateY(); + RotateX(); + RotateZ(); + + // Remove gaits + if (leftLegorRight) + { // Right + X = -1 * (X - TranslateX); + Z = -1 * (Z - TranslateZ); + } else + { + X -= TranslateX; + Z = -1 * (Z - TranslateZ); + } + + if (leftLegorRight) + { // right + double a = (90 - degrees((float)(O1()))); + double b = (180 - degrees((float)(T1()))); + double c = degrees((float)(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)(O1()))); + 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)(O1()))); + 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 = 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; + } + } + } +} + diff --git a/2012-simulation/processing-3d/hexapod/HexapodClass.pde b/2012-simulation/processing-3d/hexapod/HexapodClass.pde new file mode 100644 index 0000000..36ead3f --- /dev/null +++ b/2012-simulation/processing-3d/hexapod/HexapodClass.pde @@ -0,0 +1,362 @@ +Foot foot1 = new Foot(); +Foot foot2 = new Foot(); +Foot foot3 = new Foot(); +Foot foot4 = new Foot(); +Foot foot5 = new Foot(); +Foot foot6 = new Foot(); + +// 9.5 +// 14 +// 9.5 + +// 10 +// 10 + +// LAYOUT HARDWARE +// _U_ +// 0==/ \==3 +// >***< +// 1===|---|===4 +// >***< +// 2==\___/==5 + +// LAYOUT SOFTWARE? +// _U_ +// 1==/ \==6 +// >***< +// 2===|---|===5 +// >***< +// 3==\___/==4 + +PVector point1 = new PVector(4.75, 0, 10); +PVector point2 = new PVector(7, 0, 0); +PVector point3 = new PVector(4.75, 0, -10); +PVector point4 = new PVector(-4.75, 0, -10); +PVector point5 = new PVector(-7, 0, 0); +PVector point6 = new PVector(-4.75, 0, 10); + +float rotationX=0; +float rotationY=0; +float rotationZ=0; + +boolean robotStanding = false; + +float robotHeigth = 12; + +void drawTestSpot(PVector p, boolean leftLegorRight) +{ + beginShape(); + stroke(0, 0, 255); + if (leftLegorRight) + { + Vertex((-1*p.x)-1, p.y, (-1*p.z)-1); + Vertex((-1*p.x)-1, p.y, (-1*p.z)+1); + Vertex((-1*p.x)+1, p.y, (-1*p.z)+1); + Vertex((-1*p.x)+1, p.y, (-1*p.z)-1); + Vertex((-1*p.x)-1, p.y, (-1*p.z)-1); + } else + { + Vertex(p.x-1, p.y, (-1*p.z)-1); + Vertex(p.x-1, p.y, (-1*p.z)+1); + Vertex(p.x+1, p.y, (-1*p.z)+1); + Vertex(p.x+1, p.y, (-1*p.z)-1); + Vertex(p.x-1, p.y, (-1*p.z)-1); + } + endShape(); +} + +/* // Animation old. + PVector Animation(int addition) + { + return animationSubSteps(walk1[((animationStep+addition)%6)], walk1[((animationStep+addition+1)%6)]); + } + + PVector animationSubSteps(PVector from, PVector to) + { + PVector newStep = new PVector(); + int steps=5; + + if (animationSubStep < steps) + { + float differenceX = (to.x - from.x) / steps; + float differenceY = (to.y - from.y) / steps; + float differenceZ = (to.z - from.z) / steps; + + newStep.x = from.x + ( differenceX * animationSubStep ); + newStep.y = from.y + ( differenceY * animationSubStep ); + newStep.z = from.z + ( differenceZ * animationSubStep ); + } else + { + animationSubStep=0; + animationStep++; + newStep = to; + } + + return newStep; + } + */ + + +PVector []walk1 = new PVector[6]; + +// Animation new +int animationStep=0; +int animationSubStep=0; +boolean animationRepeat = false; +boolean animationRepeatComplete = false; +int animationSpeed = 20; +PVector animationOld; + +PVector Animation(PVector from, PVector to) +{ + PVector[] newAnimation = new PVector[2]; + newAnimation[0] = from; + newAnimation[1] = to; + return Animation(newAnimation, 2, 0); +} + +PVector Animation(PVector[] animation, int animationLength, int addition) +{ + PVector newStep = new PVector(); + + PVector from = animation[((animationStep+addition) % animationLength)]; + PVector to = animation[((animationStep+addition+1) % animationLength)]; + + /*if (!((animationStep >= animationLength - 1) && !animationRepeat)) + { + if (timerAnimation.delay(animationSpeed)) + { + animationSubStep++; + } + } else + { + return animationOld; + }*/ + if ( (animationRepeatComplete) || ((animationStep >= animationLength - 1) && !animationRepeat) ) + { + return animationOld; + } else { + /*if (timerAnimation.delay(animationSpeed)) + { + animationSubStep++; + }*/ + + int steps=5; + + if (animationSubStep < steps) + { + float differenceX = (to.x - from.x) / steps; + float differenceY = (to.y - from.y) / steps; + float differenceZ = (to.z - from.z) / steps; + + newStep.x = from.x + ( differenceX * animationSubStep ); + newStep.y = from.y + ( differenceY * animationSubStep ); + newStep.z = from.z + ( differenceZ * animationSubStep ); + } else + { + animationSubStep = 0; + + if ((animationStep >= animationLength - 2) && !animationRepeat) + { + animationRepeatComplete = true; + animationStep = 0; + animationOld = to; + return to; + } else if (!animationRepeatComplete) + { + animationStep++; + } + } + + return newStep; + } +} + +void Vertex(double x, double y, double z) +{ + vertex((float)x, (float)y, (float)z); +} + +void DrawFeet() +{ + foot1.Translate(point1); + foot1.DrawFoot(); + + foot2.Translate(point2); + foot2.DrawFoot(); + + foot3.Translate(point3); + foot3.DrawFoot(); + + foot4.Translate(point4); + foot4.leftLegorRight=true; + foot4.DrawFoot(); + + foot5.Translate(point5); + foot5.leftLegorRight=true; + foot5.DrawFoot(); + + foot6.Translate(point6); + foot6.leftLegorRight=true; + foot6.DrawFoot(); +} + +void RobotWalk() +{ + if (robotStanding) + { + if (!animationRepeatComplete) + if (timerAnimation.delay(animationSpeed)) + animationSubStep++; + + animationRepeat = true; + animationSpeed = 20; + /* + foot1.SetFootCoordinates(Animation(walk1, 6, 2)); // BULLSHIT BUG + foot1.SetFootCoordinates(Animation(walk1, 6, 2)); + foot2.SetFootCoordinates(Animation(walk1, 6, 3)); + foot3.SetFootCoordinates(Animation(walk1, 6, 4));// + foot4.SetFootCoordinates(Animation(walk1, 6, 1));// + foot5.SetFootCoordinates(Animation(walk1, 6, 0)); + foot6.SetFootCoordinates(Animation(walk1, 6, 5));*/ + foot1.SetFootCoordinates(Animation(walk1, 6, 1)); // BULLSHIT BUG + foot1.SetFootCoordinates(Animation(walk1, 6, 1)); + foot2.SetFootCoordinates(Animation(walk1, 6, 3)); + foot3.SetFootCoordinates(Animation(walk1, 6, 5));// + + foot4.SetFootCoordinates(Animation(walk1, 6, 0));// + foot5.SetFootCoordinates(Animation(walk1, 6, 2)); + foot6.SetFootCoordinates(Animation(walk1, 6, 4)); + } +} + +void DrawGround() +{ + noStroke(); // Ground + fill(128); + beginShape(); + Vertex(-60, robotHeigth, -60); + Vertex(60, robotHeigth, -60); + Vertex(60, robotHeigth, 60); + Vertex(-60, robotHeigth, 60); + Vertex(-60, robotHeigth, -60); + endShape(); +} + +void DrawTestSpots() +{ + /*drawTestSpot(new PVector((Animation(walk1, 6, 2).x + point1.x), + robotHeigth, + (Animation(walk1, 6, 2).z - point1.z)), false); + drawTestSpot(new PVector((Animation(walk1, 6, 3).x + point2.x), + robotHeigth, + (Animation(walk1, 6, 3).z - point2.z)), false); + drawTestSpot(new PVector((Animation(walk1, 6, 4).x + point3.x), + robotHeigth, + (Animation(walk1, 6, 4).z - point3.z)), false); + drawTestSpot(new PVector((Animation(walk1, 6, 1).x - point4.x), + robotHeigth, + (Animation(walk1, 6, 1).z - point4.z)), true); + drawTestSpot(new PVector((Animation(walk1, 6, 0).x - point5.x), + robotHeigth, + (Animation(walk1, 6, 0).z - point5.z)), true); + drawTestSpot(new PVector((Animation(walk1, 6, 5).x - point6.x), + robotHeigth, + (Animation(walk1, 6, 5).z - point6.z)), true);*/ +} + +void DrawBody() +{ + if (!robotStanding) + { + rotationX = 0; + rotationY = 0; + rotationZ = 0; + } + // Rotate body + rotateX(radians(-1*rotationX)); + rotateY(radians(-1*rotationY)); + rotateZ(radians(-1*rotationZ)); + + stroke(100, 100, 100); // Body + fill(0, 0, 255); + beginShape(); + Vertex(point1.x, point1.y, point1.z); + Vertex(point2.x, point2.y, point2.z); + Vertex(point3.x, point3.y, point3.z); + Vertex(point4.x, point4.y, point4.z); + Vertex(point5.x, point5.y, point5.z); + Vertex(point6.x, point6.y, point6.z); + Vertex(point1.x, point1.y, point1.z); + endShape(); + stroke(100, 100, 100); // Body top + fill(0, 255, 0); + beginShape(); + Vertex(point1.x, point1.y, point1.z); + Vertex(point2.x, point2.y, point2.z); + Vertex(point5.x, point5.y, point5.z); + Vertex(point6.x, point6.y, point6.z); + Vertex(point1.x, point1.y, point1.z); + endShape(); + + RobotStandup(); + + RobotWalk(); + + DrawFeet(); +} + +PVector []standup = new PVector[2]; +int standupStep = 0; + +void RobotStandup() +{ + standup[0] = new PVector(5, 6, 0); + standup[1] = new PVector(10, 6, 0); + + if (!robotStanding) + { + if (!animationRepeatComplete) + if (timerAnimation.delay(animationSpeed)) + animationSubStep++; + + animationRepeat = false; + animationSpeed = 250; + + switch (standupStep) + { + case 0: + foot1.SetFootCoordinates(Animation(standup, 2, 0)); + foot2.SetFootCoordinates(Animation(standup, 2, 0)); + foot3.SetFootCoordinates(Animation(standup, 2, 0));// + foot4.SetFootCoordinates(Animation(standup, 2, 0));// + foot5.SetFootCoordinates(Animation(standup, 2, 0)); + foot6.SetFootCoordinates(Animation(standup, 2, 0)); + + if (animationRepeatComplete) + { + animationRepeatComplete = false; + standupStep = 1; + animationStep = 0; + } + break; + + case 1: + foot1.SetFootCoordinates(Animation(standup[1], walk1[2])); + foot2.SetFootCoordinates(Animation(standup[1], walk1[3])); + foot3.SetFootCoordinates(Animation(standup[1], walk1[4]));// + foot4.SetFootCoordinates(Animation(standup[1], walk1[1]));// + foot5.SetFootCoordinates(Animation(standup[1], walk1[0])); + foot6.SetFootCoordinates(Animation(standup[1], walk1[5])); + + if (animationRepeatComplete) + { + robotStanding = true; + animationRepeatComplete = false; + standupStep = 2; + } + break; + } + } +} + diff --git a/2012-simulation/processing-3d/hexapod/hexapod.pde b/2012-simulation/processing-3d/hexapod/hexapod.pde new file mode 100644 index 0000000..9f446c6 --- /dev/null +++ b/2012-simulation/processing-3d/hexapod/hexapod.pde @@ -0,0 +1,81 @@ +void setup() { + size(600, 600, P3D); + background(0); + + walk1[0] = new PVector(12, robotHeigth, -5); + walk1[1] = new PVector(12, robotHeigth, 0); + walk1[2] = new PVector(12, robotHeigth, 5); + walk1[3] = new PVector(12, robotHeigth-2, 5); + walk1[4] = new PVector(12, robotHeigth-2, 0); + walk1[5] = new PVector(12, robotHeigth-2, -5); +} + +CustomTimer timerAnimation = new CustomTimer(); +CustomTimer timerXRotation = new CustomTimer(); +CustomTimer timerYRotation = new CustomTimer(); +CustomTimer timerZRotation = new CustomTimer(); +CustomTimer timerScene = new CustomTimer(); + +float counterRotation=0; +int counterXRotation=0; +int counterYRotation=0; +int counterZRotation=0; +int counterScene=0; + +float rotationRange = 8; + +void draw() +{ + background(0); + + translate(width/2, height/2, 0); + + switch(counterScene) + { + case 0: + rotateX(-1*(PI/10)); + rotateY(radians(-210)); + break; + case 1: + rotateY(counterRotation); + break; + case 2: + rotateX(radians(-90)); + rotateY(radians(180)); + break; + default: + counterScene=0; + } + + 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))) ); + + // Animation + if (timerZRotation.delay(25)) + { + counterRotation-=0.012f; + counterZRotation++; + } + if (timerXRotation.delay(50)) + { + counterXRotation++; + } + if (timerYRotation.delay(75)) + { + counterYRotation++; + } + if (timerScene.delay(5000)) + { + counterScene++; + } + + scale(6); + + DrawGround(); + + DrawTestSpots(); + + DrawBody(); +} + diff --git a/2012-simulation/processing-3d/hexapod/sketch.properties b/2012-simulation/processing-3d/hexapod/sketch.properties new file mode 100644 index 0000000..09d6f35 --- /dev/null +++ b/2012-simulation/processing-3d/hexapod/sketch.properties @@ -0,0 +1,2 @@ +mode.id=processing.mode.java.JavaMode +mode=Java