From 78c5c11d41545ab3561063a2707fbe84f100a604 Mon Sep 17 00:00:00 2001 From: MangaValk Date: Mon, 6 Oct 2014 14:30:00 +0200 Subject: [PATCH 1/6] Hexapod simulator: first version Single-sketch version, imported from 'Hexapod 1.rar'. --- .gitattributes | 37 ++ .../processing-3d/hexapod/hexapod.pde | 547 ++++++++++++++++++ .../processing-3d/hexapod/sketch.properties | 2 + 3 files changed, 586 insertions(+) create mode 100644 .gitattributes create mode 100644 2012-simulation/processing-3d/hexapod/hexapod.pde create mode 100644 2012-simulation/processing-3d/hexapod/sketch.properties diff --git a/.gitattributes b/.gitattributes new file mode 100644 index 0000000..533fe76 --- /dev/null +++ b/.gitattributes @@ -0,0 +1,37 @@ +*.wmv filter=lfs diff=lfs merge=lfs -text +*.WMV filter=lfs diff=lfs merge=lfs -text +*.mp4 filter=lfs diff=lfs merge=lfs -text +*.MP4 filter=lfs diff=lfs merge=lfs -text +*.MPG filter=lfs diff=lfs merge=lfs -text +*.mpg filter=lfs diff=lfs merge=lfs -text +*.jpg filter=lfs diff=lfs merge=lfs -text +*.JPG filter=lfs diff=lfs merge=lfs -text +*.jpeg filter=lfs diff=lfs merge=lfs -text +*.png filter=lfs diff=lfs merge=lfs -text +*.PNG filter=lfs diff=lfs merge=lfs -text +*.bmp filter=lfs diff=lfs merge=lfs -text +*.exr filter=lfs diff=lfs merge=lfs -text +*.pdf filter=lfs diff=lfs merge=lfs -text +*.docx filter=lfs diff=lfs merge=lfs -text +*.vsdx filter=lfs diff=lfs merge=lfs -text +*.dwg filter=lfs diff=lfs merge=lfs -text +*.f3d filter=lfs diff=lfs merge=lfs -text +*.zip filter=lfs diff=lfs merge=lfs -text +*.zi filter=lfs diff=lfs merge=lfs -text +*.stl filter=lfs diff=lfs merge=lfs -text +*.STL filter=lfs diff=lfs merge=lfs -text +*.HMI filter=lfs diff=lfs merge=lfs -text +*.exe filter=lfs diff=lfs merge=lfs -text +*.dll filter=lfs diff=lfs merge=lfs -text +*.pdb filter=lfs diff=lfs merge=lfs -text +*.idb filter=lfs diff=lfs merge=lfs -text +*.obj filter=lfs diff=lfs merge=lfs -text +*.ipch filter=lfs diff=lfs merge=lfs -text +*.sdf filter=lfs diff=lfs merge=lfs -text +*.suo filter=lfs diff=lfs merge=lfs -text +*.db filter=lfs diff=lfs merge=lfs -text +*.lnk filter=lfs diff=lfs merge=lfs -text +*.ide filter=lfs diff=lfs merge=lfs -text +*.ide-wal filter=lfs diff=lfs merge=lfs -text +*.ide-shm filter=lfs diff=lfs merge=lfs -text +2018-unity-sim/**/*.unity filter=lfs diff=lfs merge=lfs -text diff --git a/2012-simulation/processing-3d/hexapod/hexapod.pde b/2012-simulation/processing-3d/hexapod/hexapod.pde new file mode 100644 index 0000000..13db192 --- /dev/null +++ b/2012-simulation/processing-3d/hexapod/hexapod.pde @@ -0,0 +1,547 @@ +void setup() { + size(600, 600, P3D); + background(0); +} + +Custom c1 = new Custom(); +Custom c2 = new Custom(); +Custom c3 = new Custom(); +Custom c4 = new Custom(); +Custom c5 = new Custom(); + +Foot foot1 = new Foot(); +Foot foot2 = new Foot(); +Foot foot3 = new Foot(); +Foot foot4 = new Foot(); +Foot foot5 = new Foot(); +Foot foot6 = new Foot(); + +PVector point1 = new PVector(8, 0, 20); +PVector point2 = new PVector(12, 0, 0); +PVector point3 = new PVector(8, 0, -20); +PVector point4 = new PVector(-8, 0, -20); +PVector point5 = new PVector(-12, 0, 0); +PVector point6 = new PVector(-8, 0, 20); + +float rotationX=0; +float rotationY=0; +float rotationZ=0; + +int animationStep=0; +int animationSubStep=0; + +float i=0; +int i2=0; +int i3=0; +int i4=0; +int i5=0; + +void draw() +{ + background(0); + + translate(width/2, height/2, 0); + + switch(/*i5*/2) + { + case 0: + rotateX(-1*(PI/10)); + rotateY(radians(-210)); + break; + case 1: + rotateY(i); + break; + case 2: + rotateX(radians(-90)); + rotateY(radians(180)); + break; + default: + i5=0; + } + + float rotationRange = 8; + + //rotationX = ( i3%(rotationRange*4) < (rotationRange*2) ? -rotationRange+i3%(rotationRange*2) : rotationRange+(-1*(i3%(rotationRange*2))) ); + rotationY = ( i4%(rotationRange*4) < (rotationRange*2) ? -rotationRange+i4%(rotationRange*2) : rotationRange+(-1*(i4%(rotationRange*2))) ); + //rotationZ = ( i2%(rotationRange*4) < (rotationRange*2) ? -rotationRange+i2%(rotationRange*2) : rotationRange+(-1*(i2%(rotationRange*2))) ); + + if (c2.delay(25)) + { + i-=0.012f; + i2++; + } + if (c3.delay(50)) + { + i3++; + } + if (c4.delay(75)) + { + i4++; + } + if (c5.delay(5000)) + { + i5++; + } + + scale(6); + + noStroke(); // Ground + fill(128); + beginShape(); + Vertex(-60, 15, -60); + Vertex(60, 15, -60); + Vertex(60, 15, 60); + Vertex(-60, 15, 60); + Vertex(-60, 15, -60); + endShape(); + + drawTestSpot(new PVector((Animation(2).x + point1.x), + 15, + (Animation(2).z - point1.z)), false); + drawTestSpot(new PVector((Animation(3).x + point2.x), + 15, + (Animation(3).z - point2.z)), false); + drawTestSpot(new PVector((Animation(4).x + point3.x), + 15, + (Animation(4).z - point3.z)), false); + drawTestSpot(new PVector((Animation(1).x - point4.x), + 15, + (Animation(1).z - point4.z)), true); + drawTestSpot(new PVector((Animation(0).x - point5.x), + 15, + (Animation(0).z - point5.z)), true); + drawTestSpot(new PVector((Animation(5).x - point6.x), + 15, + (Animation(5).z - point6.z)), true); + + // 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(); + + // Animation + if (c1.delay(1)) + { + animationSubStep++; + } + + animationSubStep = 0; // REMOVE + + foot1.SetFootCoordinates(Animation(2)); + foot2.SetFootCoordinates(Animation(3)); + foot3.SetFootCoordinates(Animation(4));// + foot4.SetFootCoordinates(Animation(1));// + foot5.SetFootCoordinates(Animation(0)); + foot6.SetFootCoordinates(Animation(5)); + + 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 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(); +} + +PVector []walk1 = new PVector[6]; + +PVector Animation(int addition) +{ + walk1[0] = new PVector(12, 15, -5); + walk1[1] = new PVector(12, 15, 0); + walk1[2] = new PVector(12, 15, 5); + walk1[3] = new PVector(12, 10, 5); + walk1[4] = new PVector(12, 10, 0); + walk1[5] = new PVector(12, 10, -5); + + PVector P = new PVector(); + + P = animationSubSteps(walk1[((animationStep+addition)%6)], walk1[((animationStep+addition+1)%6)]); + + if (false) + { + P.x=10; + P.y=15; + P.z=0; + } + + return P; +} + +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; +} + +void Vertex(double x, double y, double z) +{ + vertex((float)x, (float)y, (float)z); +} + +class Custom +{ + 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; + } +} + +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() + { + if (leftLegorRight) + { + double yt = Y + TranslateY; + double zt = Z + TranslateZ; + + double NY = yt * cos(radians(rotationX)) - zt * sin(radians(rotationX)); + double NZ = yt * sin(radians(rotationX)) + zt * cos(radians(rotationX)); + + Y = NY - TranslateY; + Z = NZ - TranslateZ; + } else + { + double yt = Yi + TranslateY; + double zt = Zi + TranslateZ; + + double NY = yt * cos(radians(rotationX)) - zt * sin(radians(rotationX)); + double NZ = yt * sin(radians(rotationX)) + zt * cos(radians(rotationX)); + + Y = NY - TranslateY; + Z = NZ - TranslateZ; + } + } + + void RotateY() + { + if (leftLegorRight) + { // Right + /*double xt = (-1*X) + TranslateX; + double zt = -1*(Z + TranslateZ); + + double NZ = zt * cos(radians(rotationY)) - xt * sin(radians(rotationY)); + double NX = zt * sin(radians(rotationY)) + xt * cos(radians(rotationY)); + + X = -1 * (NX - TranslateX); + Z = -1*(NZ + TranslateZ);*/ + double xt = (-1*X) + TranslateX; + double zt = (-1*Z) + TranslateZ; + + double NZ = zt * cos(radians(rotationY)) - xt * sin(radians(rotationY)); + double NX = zt * sin(radians(rotationY)) + xt * cos(radians(rotationY)); + + X = -1 * (NX - TranslateX); + Z = -1 * (NZ - TranslateZ); + } else + { + double xt = X + TranslateX; + double zt = Z + TranslateZ; + + double NZ = zt * cos(radians(rotationY)) - xt * sin(radians(rotationY)); + double NX = zt * sin(radians(rotationY)) + xt * cos(radians(rotationY)); + + X = NX - TranslateX; + Z = NZ - TranslateZ; + } + } + + void RotateZ() + { + if (leftLegorRight) + { + double xt = (-1*X) + TranslateX; + double yt = Y + TranslateY; + + double NX = xt * cos(radians(rotationZ)) - yt * sin(radians(rotationZ)); + double NY = xt * sin(radians(rotationZ)) + yt * cos(radians(rotationZ)); + + X = -1 * (NX - TranslateX); + Y = NY - TranslateY; + } else + { + double xt = X + TranslateX; + double yt = Y + TranslateY; + + double NX = xt * cos(radians(rotationZ)) - yt * sin(radians(rotationZ)); + double NY = xt * sin(radians(rotationZ)) + yt * cos(radians(rotationZ)); + + X = NX - TranslateX; + Y = NY - TranslateY; + Z = Zi; + } + } + + 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; + + RotateY(); + //RotateX(); + //RotateZ(); + + 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; + } + } +} + 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 From c977b3acab77cdc55e2cc4aaa0ae094105ff2925 Mon Sep 17 00:00:00 2001 From: MangaValk Date: Mon, 6 Oct 2014 22:39:00 +0200 Subject: [PATCH 2/6] Clean up main sketch Imported from 'hexapod 2.rar'. --- .../processing-3d/hexapod/hexapod.pde | 112 +++++------------- 1 file changed, 32 insertions(+), 80 deletions(-) diff --git a/2012-simulation/processing-3d/hexapod/hexapod.pde b/2012-simulation/processing-3d/hexapod/hexapod.pde index 13db192..c993b00 100644 --- a/2012-simulation/processing-3d/hexapod/hexapod.pde +++ b/2012-simulation/processing-3d/hexapod/hexapod.pde @@ -42,7 +42,7 @@ void draw() translate(width/2, height/2, 0); - switch(/*i5*/2) + switch(i5) { case 0: rotateX(-1*(PI/10)); @@ -61,9 +61,9 @@ void draw() float rotationRange = 8; - //rotationX = ( i3%(rotationRange*4) < (rotationRange*2) ? -rotationRange+i3%(rotationRange*2) : rotationRange+(-1*(i3%(rotationRange*2))) ); + rotationX = ( i3%(rotationRange*4) < (rotationRange*2) ? -rotationRange+i3%(rotationRange*2) : rotationRange+(-1*(i3%(rotationRange*2))) ); rotationY = ( i4%(rotationRange*4) < (rotationRange*2) ? -rotationRange+i4%(rotationRange*2) : rotationRange+(-1*(i4%(rotationRange*2))) ); - //rotationZ = ( i2%(rotationRange*4) < (rotationRange*2) ? -rotationRange+i2%(rotationRange*2) : rotationRange+(-1*(i2%(rotationRange*2))) ); + rotationZ = ( i2%(rotationRange*4) < (rotationRange*2) ? -rotationRange+i2%(rotationRange*2) : rotationRange+(-1*(i2%(rotationRange*2))) ); if (c2.delay(25)) { @@ -145,9 +145,7 @@ void draw() { animationSubStep++; } - - animationSubStep = 0; // REMOVE - + foot1.SetFootCoordinates(Animation(2)); foot2.SetFootCoordinates(Animation(3)); foot3.SetFootCoordinates(Animation(4));// @@ -348,86 +346,20 @@ class Foot void RotateX() { - if (leftLegorRight) - { - double yt = Y + TranslateY; - double zt = Z + TranslateZ; - - double NY = yt * cos(radians(rotationX)) - zt * sin(radians(rotationX)); - double NZ = yt * sin(radians(rotationX)) + zt * cos(radians(rotationX)); - - Y = NY - TranslateY; - Z = NZ - TranslateZ; - } else - { - double yt = Yi + TranslateY; - double zt = Zi + TranslateZ; - - double NY = yt * cos(radians(rotationX)) - zt * sin(radians(rotationX)); - double NZ = yt * sin(radians(rotationX)) + zt * cos(radians(rotationX)); - - Y = NY - TranslateY; - Z = NZ - TranslateZ; - } + Y = Y * cos(radians(rotationX)) - Z * sin(radians(rotationX)); + Z = Y * sin(radians(rotationX)) + Z * cos(radians(rotationX)); } void RotateY() { - if (leftLegorRight) - { // Right - /*double xt = (-1*X) + TranslateX; - double zt = -1*(Z + TranslateZ); - - double NZ = zt * cos(radians(rotationY)) - xt * sin(radians(rotationY)); - double NX = zt * sin(radians(rotationY)) + xt * cos(radians(rotationY)); - - X = -1 * (NX - TranslateX); - Z = -1*(NZ + TranslateZ);*/ - double xt = (-1*X) + TranslateX; - double zt = (-1*Z) + TranslateZ; - - double NZ = zt * cos(radians(rotationY)) - xt * sin(radians(rotationY)); - double NX = zt * sin(radians(rotationY)) + xt * cos(radians(rotationY)); - - X = -1 * (NX - TranslateX); - Z = -1 * (NZ - TranslateZ); - } else - { - double xt = X + TranslateX; - double zt = Z + TranslateZ; - - double NZ = zt * cos(radians(rotationY)) - xt * sin(radians(rotationY)); - double NX = zt * sin(radians(rotationY)) + xt * cos(radians(rotationY)); - - X = NX - TranslateX; - Z = NZ - TranslateZ; - } + Z = Z * cos(radians(rotationY)) - X * sin(radians(rotationY)); + X = Z * sin(radians(rotationY)) + X * cos(radians(rotationY)); } void RotateZ() { - if (leftLegorRight) - { - double xt = (-1*X) + TranslateX; - double yt = Y + TranslateY; - - double NX = xt * cos(radians(rotationZ)) - yt * sin(radians(rotationZ)); - double NY = xt * sin(radians(rotationZ)) + yt * cos(radians(rotationZ)); - - X = -1 * (NX - TranslateX); - Y = NY - TranslateY; - } else - { - double xt = X + TranslateX; - double yt = Y + TranslateY; - - double NX = xt * cos(radians(rotationZ)) - yt * sin(radians(rotationZ)); - double NY = xt * sin(radians(rotationZ)) + yt * cos(radians(rotationZ)); - - X = NX - TranslateX; - Y = NY - TranslateY; - Z = Zi; - } + X = X * cos(radians(rotationZ)) - Y * sin(radians(rotationZ)); + Y = X * sin(radians(rotationZ)) + Y * cos(radians(rotationZ)); } void RL3 () @@ -503,9 +435,29 @@ class Foot Y = Yi; Z = Zi; + if (leftLegorRight) + { // Right + X = (-1*X) + TranslateX; + Z = (-1*Z) + TranslateZ; + } else + { + X = Xi + TranslateX; + Z = (-1*Z) + TranslateZ; + } + RotateY(); - //RotateX(); - //RotateZ(); + RotateX(); + RotateZ(); + + if (leftLegorRight) + { // Right + X = -1 * (X - TranslateX); + Z = -1 * (Z - TranslateZ); + } else + { + X -= TranslateX; + Z = -1 * (Z - TranslateZ); + } double a = (90 - degrees((float)(O1()))); double b = (180 - degrees((float)(T1()))); From 00bf3187feabdf61e67d5f834a000f233a3cfc36 Mon Sep 17 00:00:00 2001 From: MangaValk Date: Sun, 12 Oct 2014 16:17:00 +0200 Subject: [PATCH 3/6] Split leg and body logic into FootClass and HexapodClass Imported from 'hexapod 3.rar'. --- .../processing-3d/hexapod/FootClass.pde | 228 ++++++++ .../processing-3d/hexapod/HexapodClass.pde | 230 ++++++++ .../processing-3d/hexapod/hexapod.pde | 496 ++---------------- 3 files changed, 499 insertions(+), 455 deletions(-) create mode 100644 2012-simulation/processing-3d/hexapod/FootClass.pde create mode 100644 2012-simulation/processing-3d/hexapod/HexapodClass.pde diff --git a/2012-simulation/processing-3d/hexapod/FootClass.pde b/2012-simulation/processing-3d/hexapod/FootClass.pde new file mode 100644 index 0000000..5aa9321 --- /dev/null +++ b/2012-simulation/processing-3d/hexapod/FootClass.pde @@ -0,0 +1,228 @@ +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); + } + + 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; + } + } +} + diff --git a/2012-simulation/processing-3d/hexapod/HexapodClass.pde b/2012-simulation/processing-3d/hexapod/HexapodClass.pde new file mode 100644 index 0000000..00a90cd --- /dev/null +++ b/2012-simulation/processing-3d/hexapod/HexapodClass.pde @@ -0,0 +1,230 @@ +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 + +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; + +int animationStep=0; +int animationSubStep=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(); +} + +PVector []walk1 = new PVector[6]; + +PVector Animation(int addition) +{ + + PVector P = new PVector(); + + P = animationSubSteps(walk1[((animationStep+addition)%6)], walk1[((animationStep+addition+1)%6)]); + + return P; +} + +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; +} + +void Vertex(double x, double y, double z) +{ + vertex((float)x, (float)y, (float)z); +} + +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; + } +} + +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 SetFeetCoordinatesUsingAnimation() +{ + foot1.SetFootCoordinates(Animation(2)); + foot2.SetFootCoordinates(Animation(3)); + foot3.SetFootCoordinates(Animation(4));// + foot4.SetFootCoordinates(Animation(1));// + foot5.SetFootCoordinates(Animation(0)); + foot6.SetFootCoordinates(Animation(5)); +} + +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(2).x + point1.x), + robotHeigth, + (Animation(2).z - point1.z)), false); + drawTestSpot(new PVector((Animation(3).x + point2.x), + robotHeigth, + (Animation(3).z - point2.z)), false); + drawTestSpot(new PVector((Animation(4).x + point3.x), + robotHeigth, + (Animation(4).z - point3.z)), false); + drawTestSpot(new PVector((Animation(1).x - point4.x), + robotHeigth, + (Animation(1).z - point4.z)), true); + drawTestSpot(new PVector((Animation(0).x - point5.x), + robotHeigth, + (Animation(0).z - point5.z)), true); + drawTestSpot(new PVector((Animation(5).x - point6.x), + robotHeigth, + (Animation(5).z - point6.z)), true); +} + +void DrawBody() +{ + // 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(); + + SetFeetCoordinatesUsingAnimation(); + + DrawFeet(); +} + +PVector []standup = new PVector[6]; + +void RobotStandup() +{ + standup[0] = new PVector(3, 5, 0); + standup[1] = new PVector(12, robotHeigth, -5); + standup[2] = new PVector(12, robotHeigth, -5); + + if (!robotStanding) + { + } +} + diff --git a/2012-simulation/processing-3d/hexapod/hexapod.pde b/2012-simulation/processing-3d/hexapod/hexapod.pde index c993b00..c42db6c 100644 --- a/2012-simulation/processing-3d/hexapod/hexapod.pde +++ b/2012-simulation/processing-3d/hexapod/hexapod.pde @@ -1,40 +1,28 @@ 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); } -Custom c1 = new Custom(); -Custom c2 = new Custom(); -Custom c3 = new Custom(); -Custom c4 = new Custom(); -Custom c5 = new Custom(); +CustomTimer timerAnimation = new CustomTimer(); +CustomTimer timerXRotation = new CustomTimer(); +CustomTimer timerYRotation = new CustomTimer(); +CustomTimer timerZRotation = new CustomTimer(); +CustomTimer timerScene = new CustomTimer(); -Foot foot1 = new Foot(); -Foot foot2 = new Foot(); -Foot foot3 = new Foot(); -Foot foot4 = new Foot(); -Foot foot5 = new Foot(); -Foot foot6 = new Foot(); +float counterRotation=0; +int counterXRotation=0; +int counterYRotation=0; +int counterZRotation=0; +int counterScene=0; -PVector point1 = new PVector(8, 0, 20); -PVector point2 = new PVector(12, 0, 0); -PVector point3 = new PVector(8, 0, -20); -PVector point4 = new PVector(-8, 0, -20); -PVector point5 = new PVector(-12, 0, 0); -PVector point6 = new PVector(-8, 0, 20); - -float rotationX=0; -float rotationY=0; -float rotationZ=0; - -int animationStep=0; -int animationSubStep=0; - -float i=0; -int i2=0; -int i3=0; -int i4=0; -int i5=0; +float rotationRange = 8; void draw() { @@ -42,458 +30,56 @@ void draw() translate(width/2, height/2, 0); - switch(i5) + switch(counterScene) { case 0: rotateX(-1*(PI/10)); rotateY(radians(-210)); break; case 1: - rotateY(i); + rotateY(counterRotation); break; case 2: rotateX(radians(-90)); rotateY(radians(180)); break; default: - i5=0; + counterScene=0; } - float rotationRange = 8; + 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))) ); - rotationX = ( i3%(rotationRange*4) < (rotationRange*2) ? -rotationRange+i3%(rotationRange*2) : rotationRange+(-1*(i3%(rotationRange*2))) ); - rotationY = ( i4%(rotationRange*4) < (rotationRange*2) ? -rotationRange+i4%(rotationRange*2) : rotationRange+(-1*(i4%(rotationRange*2))) ); - rotationZ = ( i2%(rotationRange*4) < (rotationRange*2) ? -rotationRange+i2%(rotationRange*2) : rotationRange+(-1*(i2%(rotationRange*2))) ); - - if (c2.delay(25)) + // Animation + if (timerAnimation.delay(1)) { - i-=0.012f; - i2++; + animationSubStep++; } - if (c3.delay(50)) + if (timerZRotation.delay(25)) { - i3++; + counterRotation-=0.012f; + counterZRotation++; } - if (c4.delay(75)) + if (timerXRotation.delay(50)) { - i4++; + counterXRotation++; } - if (c5.delay(5000)) + if (timerYRotation.delay(75)) { - i5++; + counterYRotation++; + } + if (timerScene.delay(5000)) + { + counterScene++; } scale(6); - noStroke(); // Ground - fill(128); - beginShape(); - Vertex(-60, 15, -60); - Vertex(60, 15, -60); - Vertex(60, 15, 60); - Vertex(-60, 15, 60); - Vertex(-60, 15, -60); - endShape(); + DrawGround(); - drawTestSpot(new PVector((Animation(2).x + point1.x), - 15, - (Animation(2).z - point1.z)), false); - drawTestSpot(new PVector((Animation(3).x + point2.x), - 15, - (Animation(3).z - point2.z)), false); - drawTestSpot(new PVector((Animation(4).x + point3.x), - 15, - (Animation(4).z - point3.z)), false); - drawTestSpot(new PVector((Animation(1).x - point4.x), - 15, - (Animation(1).z - point4.z)), true); - drawTestSpot(new PVector((Animation(0).x - point5.x), - 15, - (Animation(0).z - point5.z)), true); - drawTestSpot(new PVector((Animation(5).x - point6.x), - 15, - (Animation(5).z - point6.z)), true); + DrawTestSpots(); - // 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(); - - // Animation - if (c1.delay(1)) - { - animationSubStep++; - } - - foot1.SetFootCoordinates(Animation(2)); - foot2.SetFootCoordinates(Animation(3)); - foot3.SetFootCoordinates(Animation(4));// - foot4.SetFootCoordinates(Animation(1));// - foot5.SetFootCoordinates(Animation(0)); - foot6.SetFootCoordinates(Animation(5)); - - 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 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(); -} - -PVector []walk1 = new PVector[6]; - -PVector Animation(int addition) -{ - walk1[0] = new PVector(12, 15, -5); - walk1[1] = new PVector(12, 15, 0); - walk1[2] = new PVector(12, 15, 5); - walk1[3] = new PVector(12, 10, 5); - walk1[4] = new PVector(12, 10, 0); - walk1[5] = new PVector(12, 10, -5); - - PVector P = new PVector(); - - P = animationSubSteps(walk1[((animationStep+addition)%6)], walk1[((animationStep+addition+1)%6)]); - - if (false) - { - P.x=10; - P.y=15; - P.z=0; - } - - return P; -} - -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; -} - -void Vertex(double x, double y, double z) -{ - vertex((float)x, (float)y, (float)z); -} - -class Custom -{ - 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; - } -} - -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; - - if (leftLegorRight) - { // Right - X = (-1*X) + TranslateX; - Z = (-1*Z) + TranslateZ; - } else - { - X = Xi + TranslateX; - Z = (-1*Z) + TranslateZ; - } - - RotateY(); - RotateX(); - RotateZ(); - - if (leftLegorRight) - { // Right - X = -1 * (X - TranslateX); - Z = -1 * (Z - TranslateZ); - } else - { - X -= TranslateX; - Z = -1 * (Z - TranslateZ); - } - - 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; - } - } + DrawBody(); } From 0447cb8c7a81a9930b29bc1873290010e893163f Mon Sep 17 00:00:00 2001 From: MangaValk Date: Mon, 13 Oct 2014 00:49:00 +0200 Subject: [PATCH 4/6] Rework HexapodClass Imported from 'hexapod 4.rar'. --- .../processing-3d/hexapod/HexapodClass.pde | 239 ++++++++++++++---- .../processing-3d/hexapod/hexapod.pde | 4 - 2 files changed, 183 insertions(+), 60 deletions(-) diff --git a/2012-simulation/processing-3d/hexapod/HexapodClass.pde b/2012-simulation/processing-3d/hexapod/HexapodClass.pde index 00a90cd..f22a215 100644 --- a/2012-simulation/processing-3d/hexapod/HexapodClass.pde +++ b/2012-simulation/processing-3d/hexapod/HexapodClass.pde @@ -23,9 +23,6 @@ float rotationX=0; float rotationY=0; float rotationZ=0; -int animationStep=0; -int animationSubStep=0; - boolean robotStanding = false; float robotHeigth = 12; @@ -52,40 +49,111 @@ void drawTestSpot(PVector p, boolean leftLegorRight) 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]; -PVector Animation(int addition) +// 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 P = new PVector(); - - P = animationSubSteps(walk1[((animationStep+addition)%6)], walk1[((animationStep+addition+1)%6)]); - - return P; + PVector[] newAnimation = new PVector[2]; + newAnimation[0] = from; + newAnimation[1] = to; + return Animation(newAnimation, 2, 0); } -PVector animationSubSteps(PVector from, PVector to) +PVector Animation(PVector[] animation, int animationLength, int addition) { 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; + PVector from = animation[((animationStep+addition) % animationLength)]; + PVector to = animation[((animationStep+addition+1) % animationLength)]; - newStep.x = from.x + ( differenceX * animationSubStep ); - newStep.y = from.y + ( differenceY * animationSubStep ); - newStep.z = from.z + ( differenceZ * animationSubStep ); - } else + /*if (!((animationStep >= animationLength - 1) && !animationRepeat)) + { + if (timerAnimation.delay(animationSpeed)) + { + animationSubStep++; + } + } else + { + return animationOld; + }*/ + if ( (animationRepeatComplete) || ((animationStep >= animationLength - 1) && !animationRepeat) ) { - animationSubStep=0; - animationStep++; - newStep = to; + 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; } - - return newStep; } void Vertex(double x, double y, double z) @@ -137,14 +205,24 @@ void DrawFeet() foot6.DrawFoot(); } -void SetFeetCoordinatesUsingAnimation() +void RobotWalk() { - foot1.SetFootCoordinates(Animation(2)); - foot2.SetFootCoordinates(Animation(3)); - foot3.SetFootCoordinates(Animation(4));// - foot4.SetFootCoordinates(Animation(1));// - foot5.SetFootCoordinates(Animation(0)); - foot6.SetFootCoordinates(Animation(5)); + 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)); + } } void DrawGround() @@ -162,28 +240,34 @@ void DrawGround() void DrawTestSpots() { - drawTestSpot(new PVector((Animation(2).x + point1.x), - robotHeigth, - (Animation(2).z - point1.z)), false); - drawTestSpot(new PVector((Animation(3).x + point2.x), - robotHeigth, - (Animation(3).z - point2.z)), false); - drawTestSpot(new PVector((Animation(4).x + point3.x), - robotHeigth, - (Animation(4).z - point3.z)), false); - drawTestSpot(new PVector((Animation(1).x - point4.x), - robotHeigth, - (Animation(1).z - point4.z)), true); - drawTestSpot(new PVector((Animation(0).x - point5.x), - robotHeigth, - (Animation(0).z - point5.z)), true); - drawTestSpot(new PVector((Animation(5).x - point6.x), - robotHeigth, - (Animation(5).z - point6.z)), true); + /*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)); @@ -210,21 +294,64 @@ void DrawBody() Vertex(point1.x, point1.y, point1.z); endShape(); - SetFeetCoordinatesUsingAnimation(); + RobotStandup(); + + RobotWalk(); DrawFeet(); } -PVector []standup = new PVector[6]; +PVector []standup = new PVector[2]; +int standupStep = 0; void RobotStandup() { - standup[0] = new PVector(3, 5, 0); - standup[1] = new PVector(12, robotHeigth, -5); - standup[2] = new PVector(12, robotHeigth, -5); + 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 index c42db6c..9f446c6 100644 --- a/2012-simulation/processing-3d/hexapod/hexapod.pde +++ b/2012-simulation/processing-3d/hexapod/hexapod.pde @@ -52,10 +52,6 @@ void draw() rotationZ = ( counterZRotation%(rotationRange*4) < (rotationRange*2) ? -rotationRange+counterZRotation%(rotationRange*2) : rotationRange+(-1*(counterZRotation%(rotationRange*2))) ); // Animation - if (timerAnimation.delay(1)) - { - animationSubStep++; - } if (timerZRotation.delay(25)) { counterRotation-=0.012f; From 972ba2cb73d52e269a36de1542d15f9cc9dada4f Mon Sep 17 00:00:00 2001 From: MangaValk Date: Mon, 14 Nov 2016 13:07:00 +0100 Subject: [PATCH 5/6] Add CustomTimer and separate left/right leg handling Imported from the unarchived 'hexapod' sketch folder. The real date of this version is unknown; file dates were lost in a 2016-11-14 copy. --- .../processing-3d/hexapod/CustomTimer.pde | 19 ++++ .../processing-3d/hexapod/FootClass.pde | 98 ++++++++++++++----- .../processing-3d/hexapod/HexapodClass.pde | 47 +++++---- 3 files changed, 116 insertions(+), 48 deletions(-) create mode 100644 2012-simulation/processing-3d/hexapod/CustomTimer.pde 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 index 5aa9321..8c5e0fe 100644 --- a/2012-simulation/processing-3d/hexapod/FootClass.pde +++ b/2012-simulation/processing-3d/hexapod/FootClass.pde @@ -188,40 +188,84 @@ class Foot Z = -1 * (Z - TranslateZ); } - double a = (90 - degrees((float)(O1()))); - double b = (180 - degrees((float)(T1()))); - double c = degrees((float)(T3())); + 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; + if ((a >= 15) & (a <= 165) + & (b >= 15) & (b <= 165) + & (c >= 15) & (c <= 165)) + { + error = false; - O1 = a; - O2 = b; - O3 = c; + O1 = a; + O2 = b; + O3 = c; - double d = sin((float)(b*(PI/180))); - if ( d != 0 ) - X2 = d * l1; - else - X2 = 0; + 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 = 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; + d = sin((float)((a-90)*(PI/180))); + if ( d != 0 ) + Z2 = d * abs((float)(X2)); + else + Z2 = 0; + } else + { + error = true; + } } else { - error = true; + 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 index f22a215..36ead3f 100644 --- a/2012-simulation/processing-3d/hexapod/HexapodClass.pde +++ b/2012-simulation/processing-3d/hexapod/HexapodClass.pde @@ -12,6 +12,22 @@ Foot foot6 = new Foot(); // 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); @@ -161,26 +177,6 @@ void Vertex(double x, double y, double z) vertex((float)x, (float)y, (float)z); } -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; - } -} - void DrawFeet() { foot1.Translate(point1); @@ -215,13 +211,22 @@ void RobotWalk() 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)); + 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)); } } From 902b4501aaf679d3e4377face07dc6948b68439a Mon Sep 17 00:00:00 2001 From: MangaValk Date: Sat, 24 Oct 2026 23:50:19 +0200 Subject: [PATCH 6/6] Add README and .gitattributes --- 2012-simulation/processing-3d/.gitattributes | 2 ++ 2012-simulation/processing-3d/README.md | 26 ++++++++++++++++++++ 2 files changed, 28 insertions(+) create mode 100644 2012-simulation/processing-3d/.gitattributes create mode 100644 2012-simulation/processing-3d/README.md 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.