FireFly 2011: Arduino Mega + SSC-32, Wii Nunchuk, preset walking sequences

From Electronics\Hexapod\Arduino sketches 2011-2012\hexapod_oud (banner 'Version 3.0').

2011-firefly-mega-ssc32/firmware/2011-nunchuk/hexapod_oud/hexapod_oud.pde

@@ -0,0 +1,196 @@
+// Hexapod FireFly
+
+#include "Header.h"
+#include "Wire.h" // Just dont work's, if you put it in the Header file.
+
+// Setup.
+void setup()
+{
+  chuck.begin();
+  chuck.update();
+  smin[0] = 600;
+  smin[1] = 960;
+  smin[2] = 600;
+  smax[0] = 2400;
+  smax[1] = 2100;
+  smax[2] = 2400;
+  l1 = 10;
+  l2 = 15;
+  pinMode(13, OUTPUT); // Led L
+  Serial.begin(38400);
+  body.setup();
+  FireFly();
+}
+
+// Position.
+int Position=1;
+int PosMax=15;
+int BeginPos=0;
+int Directory=0;
+
+void UpdatePosition(int i=0)
+{
+  if (i == 0)
+  {
+    if ((Position < PosMax) && (Directory == 0))
+      Position++;
+    else if (Directory == 0)
+      Directory=1;
+    else if ((Position > 1) && (Directory == 1))
+      Position--;
+    else
+      Directory=0;
+  }
+  else
+  {
+    if (Position < PosMax)
+      Position++;
+    else
+      Position = BeginPos;
+
+    if (Directory == 1)
+    {
+      Directory = 2;
+      Position = BeginPos; 
+    }
+  }
+}
+
+/*==============================================/
+ 
+ Go Use Class::operator=, To give Data From IK Class To Rotation or SSC32 Class.
+ 
+ /==============================================*/
+
+// Variables.
+double past = millis();
+int Watchdog=0;
+
+// Main loop.
+void loop()
+{
+  if (millis() < past) // to handle te exception of 50 day overflow of millis()
+    past = millis();
+
+  while ( ((millis() - past) <  walktime) || (Watchdog == 0))
+  {
+    /* chuck.update();
+     ojx = chuck.readJoyX();
+     ojy = chuck.readJoyY();
+     oax = chuck.readAccelX();
+     oay = chuck.readAccelY();
+     
+     // Mode Head Servo.
+     body.MoveHead(chuck.readRoll());*/
+
+    if (Watchdog == 0)
+    {
+      walktime = 300;
+
+      if (Directory==0)
+      {
+        UpdatePosition();
+
+        for (int i=0; i<6; i++)
+        {
+          COR_2_IK[0+(i*3)] = 10; //x
+          COR_2_IK[1+(i*3)] = Position;  //y
+          COR_2_IK[2+(i*3)] = 0;  //z
+        }
+        COR_2_IK[2+(0*3)] = -10;// x side body
+        COR_2_IK[2+(2*3)] = 10;///
+        COR_2_IK[2+(3*3)] = -10;//
+        COR_2_IK[2+(5*3)] = 10;///
+        COR_2_IK[0+(1*3)] = 15;// z side body
+        COR_2_IK[0+(4*3)] = 15;
+      }
+      else
+      {
+        PosMax = 7;
+        UpdatePosition(1);
+        for (int i2=0; i2<18; i2++)
+        {
+          COR_2_IK[i2] = TriPoD[Position][i2];
+          COR_2_IK[i2] = TriPoD[Position][i2];
+        }
+        for(int i=0; i<6; i++)
+        {
+          // zij, laag, achter
+          // 10, 15, -10,   15, 15, 0,   10, 15, 10,   10, 15, -10,   15, 15, 0,   10, 15, 10;
+          int cor[6] = {  -5, 0, 5, -5, 0, 5    };
+          COR_2_IK[2+(i*3)] += cor[i]; 
+        }
+      }
+
+      /* orbit.getAngle(90 + (chuck.readRoll()/3), 90);// 90 + (chuck.readPitch()/3));
+       orbit.getData(x, y, z);
+       orbit.calculate();*/
+
+      //                  x, y, z
+      // x, z is above the leg, z is depth
+      // y, l3 is side leg, y is height
+      ik.setlegs();
+      body.Render();  
+
+      Serial.println("Done Rendering");
+      /*if (ik.CheckForErrorCor() != 0)
+      {
+        warning();
+        Serial.println("Coordinate Error. ");
+        Serial.println(ik.CheckForErrorCor_print());
+      }
+      else*/
+        Watchdog = 1;
+    }
+  }
+
+  past = millis(); // gifs an overflow after 50 day run.
+  Serial.println("Out of loop");
+
+  if (Watchdog == 1)
+  {
+    if (ik.CheckForErrorAng() == 0) // check if there is no error.
+    {
+      Serial.println("Sending");
+      body.toServos();
+    }
+    else
+    {
+      warning();
+      Serial.println("Axis Error.");
+    }
+    Watchdog = 0;
+  }
+}
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+