Split leg and body logic into FootClass and HexapodClass

Imported from 'hexapod 3.rar'.

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)
+  {
+  }
+}
+