From 78c5c11d41545ab3561063a2707fbe84f100a604 Mon Sep 17 00:00:00 2001 From: MangaValk Date: Mon, 6 Oct 2014 14:30:00 +0200 Subject: [PATCH] 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