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/Data_Manager.cpp

@@ -0,0 +1,28 @@
+#include "Data_Manager.h"
+
+// Raw variable that must be send to the ssc32.
+int RAW_2_SERVO[18];
+
+// Angel to converter.
+int DEG_2_CON[18];
+
+// Coordinate to iverse kinematics.
+int COR_2_IK[18];
+
+// Orbit angles.
+int ORBIT_X, ORBIT_Y, ORBIT_Z;
+
+// Hight.
+int HIGHT;
+
+// Walk Direction relative from body.
+int DIR_R_BODY;
+
+// IK Legnth.
+int l1, l2;
+
+// Min and Max for SSC32.
+int smin[3], smax[3];
+
+// WalkTime
+int walktime;

2011-firefly-mega-ssc32/firmware/2011-nunchuk/hexapod_oud/Data_Manager.h

@@ -0,0 +1,20 @@
+// Data_Manager.h
+
+#ifndef DATA_MANAGER
+#define DATA_MANAGER
+
+extern int RAW_2_SERVO[18];
+extern int DEG_2_CON[18];// int
+extern int COR_2_IK[18];
+extern int ORBIT_X, ORBIT_Y, ORBIT_Z;
+extern int HIGHT;
+extern int DIRECTION;
+extern int l1, l2;
+extern int smin[3], smax[3];
+extern int walktime;
+
+
+#endif
+
+
+

2011-firefly-mega-ssc32/firmware/2011-nunchuk/hexapod_oud/Header.h

@@ -0,0 +1,50 @@
+// Include.
+#include <math.h>
+#include "InverseKinemetics.h"
+#include "SSC32.h"
+#include "sequnces.h"
+#include "Orbit.h"
+#include "Data_Manager.h"
+
+// Classes.
+InverseKinemetics ik;
+SSC32 body;
+Orbit orbit;
+
+// Nunchuck.
+#include "WiiChuck.h"
+
+#define MAXANGLE 90
+#define MINANGLE -90
+
+WiiChuck chuck = WiiChuck();
+int angleStart, currentAngle;
+int tillerStart = 100;
+double angle;
+
+// Functions.
+void FireFly()
+{
+  Serial.println("");
+  Serial.println(" ____                     ____    ___                ");
+  Serial.println("/\\  _`\\   __             /\\  _`\\ /\\_ \\               ");
+  Serial.println("\\ \\ \\L\\_\\/\\_\\  _ __    __\\ \\ \\L\\_\\//\\ \\    __  __    ");
+  Serial.println(" \\ \\  _\\/\\/\\ \\/\\`'__\\/'__`\\ \\  _\\/ \\ \\ \\  /\\ \\/\\ \\   ");
+  Serial.println("  \\ \\ \\/  \\ \\ \\ \\ \\//\\  __/\\ \\ \\/   \\_\\ \\_\\ \\ \\_\\ \\  ");
+  Serial.println("   \\ \\_\\   \\ \\_\\ \\_\\\\ \\____\\\\ \\_\\   /\\____\\\\/`____ \\ ");
+  Serial.println("    \\/_/    \\/_/\\/_/ \\/____/ \\/_/   \\/____/ `/___/> \\");
+  Serial.println("                                               /\\___/");
+  Serial.println("                                               \\/__/ ");
+  Serial.println("");
+  Serial.println("Version 3.0");
+  delay(800);
+}
+
+void warning ()
+{
+  digitalWrite(13, HIGH);
+  delay(100);
+  digitalWrite(13, LOW); 
+}
+
+

2011-firefly-mega-ssc32/firmware/2011-nunchuk/hexapod_oud/InverseKinemetics.cpp

@@ -0,0 +1,153 @@
+//#include "WProgram.h"
+#include "InverseKinemetics.h"
+
+void InverseKinemetics::RL3 (int i)
+{
+  l3 = sqrt(pow(abs(COR_2_IK[0+(i*3)]), 2) + pow(abs(COR_2_IK[2+(i*3)]), 2));
+}
+
+float InverseKinemetics::ZX (int i)
+{
+  RL3(i);
+  float temp = degrees(acos((float)COR_2_IK[2+(i*3)] / l3));
+  return temp;
+}
+
+float InverseKinemetics::TT (int i)
+{
+  int x = pow(abs(COR_2_IK[0+(i*3)]), 2);
+  int y = pow(abs(COR_2_IK[1+(i*3)]), 2);
+  int z = pow(abs(COR_2_IK[2+(i*3)]), 2);  
+  s = sqrt(x + y + z);
+  
+  if (s > (l1 + l2) )
+    R[i] = 3;
+  
+  float t1 = degrees(acos((pow(s, 2) + pow(COR_2_IK[1+(i*3)], 2) - pow(l3, 2))
+                                  / (2 * s * COR_2_IK[1+(i*3)])));
+                                  
+  float t2 = degrees(acos((pow(s, 2) + pow(l1, 2) - pow(l2, 2))
+                                  / (2 * s * l1)));
+  
+  float temp = (int)t1 + t2;
+  //Serial.print("DEBUG: "); Serial.print(t1); Serial.print(", "); Serial.print(t2); Serial.print(", "); Serial.println(temp);
+  
+  return temp;
+}
+
+int InverseKinemetics::LL ()
+{
+  int temp = degrees(acos((float)(pow(l1, 2) + pow(l2, 2) - pow(s, 2)) / (2 * l1 * l2)));
+  return temp;
+}
+
+void InverseKinemetics::setleg (int i)
+{
+  Serial.print("Set Leg "); 
+  Serial.println(i);
+  if (i >= 3)
+  {
+    //Error_Coordinate(i);
+    DEG_2_CON[0+(i*3)] = 180 - ZX(i);
+    DEG_2_CON[1+(i*3)] = 180 - TT(i);
+    DEG_2_CON[2+(i*3)] = LL();
+    Error_Angle(i);  
+  }
+  else
+  {
+    //Error_Coordinate(i);
+    DEG_2_CON[0+(i*3)] = ZX(i);
+    DEG_2_CON[1+(i*3)] = TT(i);
+    DEG_2_CON[2+(i*3)] = 180 - LL();
+    Error_Angle(i);
+  }
+}
+
+void InverseKinemetics::setlegs ()
+{
+  for (int i=0; i<6; i++)
+  {
+    setleg(i);
+  }
+  for (int i=0; i<6; i++)
+  {
+    if (i != 0)
+      Serial.print(", ");
+    else
+      Serial.print("Cor: ");
+    Serial.print(COR_2_IK[0+(i*3)]);
+    Serial.print(", ");
+    Serial.print(COR_2_IK[1+(i*3)]);
+    Serial.print(", ");
+    Serial.print(COR_2_IK[2+(i*3)]);
+  }
+  Serial.println(";");
+}
+
+void InverseKinemetics::Error_Angle (int i)
+{  
+  for (int i2=0; i2<3; i2++)
+    R[i2+(i*3)] = 0;
+  if (DEG_2_CON[0+(i*3)] < 0) 
+    R[0+(i*3)] = 1;
+  if (DEG_2_CON[0+(i*3)] > 180) 
+    R[0+(i*3)] = 2;
+  if (DEG_2_CON[1+(i*3)] < 0) 
+    R[1+(i*3)] = 1; 
+  if (DEG_2_CON[1+(i*3)] > 180)
+    R[1+(i*3)] = 2;
+  if (DEG_2_CON[2+(i*3)] < 0)
+    R[2+(i*3)] = 1;
+  if (DEG_2_CON[2+(i*3)] > 180)
+    R[2+(i*3)] = 2;
+}
+
+void InverseKinemetics::Error_Coordinate (int i)
+{  
+  Ra[i] = 0;
+  float temp = sqrt(pow(COR_2_IK[0+(i*3)], 2) + pow(COR_2_IK[1+(i*3)], 2) + pow(COR_2_IK[2+(i*3)], 2));
+  if (temp <= 5) {
+    Ra[i] = 1;
+  }    
+  if (temp > l1 + l2) {
+    Ra[i] = 2;
+  }
+}
+
+int InverseKinemetics::CheckForErrorCor()
+{
+  for (int i=1;  i<19; i++)
+    if (R[i-1]!=0)
+      return i;
+  return 0;
+}
+
+String InverseKinemetics::CheckForErrorCor_print()
+{
+  String temp = "Error Coordinate: ";
+  for (int i=0; i<18; i++)
+  {
+    temp = temp + (int)DEG_2_CON[i] + ", ";
+  }
+  temp = temp + "\nCheckForErrorCor returnd: " + CheckForErrorCor();
+  return temp;
+}
+
+int InverseKinemetics::CheckForErrorAng()
+{
+  for (int i=0;  i<6; i++)
+    if (Ra[i]!=0)
+      return i;
+  return 0;
+}
+
+
+
+
+
+
+
+
+
+
+

2011-firefly-mega-ssc32/firmware/2011-nunchuk/hexapod_oud/InverseKinemetics.h

@@ -0,0 +1,61 @@
+#ifndef InverseKinemetics_H
+#define InverseKinemetics_H
+
+#include "WProgram.h"
+#include <math.h>
+#include "Data_Manager.h"
+
+class InverseKinemetics
+{
+public:
+  //
+  void RL3(int i);
+  float ZX (int i);
+  float TT (int i);
+  int LL ();
+  void setlegs ();
+  void setleg (int i);
+
+  /* // Input
+   void setLeftFrontLeg	(int ix, int iy, int iz);	  //	clockwise,		t3=clockwise,		t3=counterclockwise
+   void setLeftMiddleLeg	(int ix, int iy, int iz);	  //	clockwise,		t3=clockwise,		t3=counterclockwise
+   void setLeftBackLeg	(int ix, int iy, int iz);         //	clockwise,		t3=clockwise,		t3=counterclockwise
+   void setRightFrontLeg	(int ix, int iy, int iz);	  //	counterclockwise,	t3=counterclockwise,	t3=clockwise
+   void setRightMiddleLeg(int ix, int iy, int iz);         //	counterclockwise,	t3=counterclockwise,	t3=clockwise
+   void setRightBackLeg	(int ix, int iy, int iz);	  //	counterclockwise,	t3=counterclockwise,	t3=clockwise*/
+
+  // 0 | 3
+  // 1 | 4
+  // 2 | 5
+
+  // 
+  void Error_Angle  (int i);
+  void Error_Coordinate (int i);
+  
+  int CheckForErrorCor();
+  String CheckForErrorCor_print();
+  int CheckForErrorAng();
+
+  // Variable's
+  int s;
+  float l3;
+  byte R[18];
+  byte Ra[6];
+};
+
+
+#endif
+
+
+
+
+
+
+
+
+
+
+
+
+
+

2011-firefly-mega-ssc32/firmware/2011-nunchuk/hexapod_oud/Orbit.cpp

@@ -0,0 +1,91 @@
+// Orbit.cpp
+
+#include "Orbit.h"
+
+void Orbit::getData(int iy[6], int ix[6], int iz[6])
+{
+  for (int i=0; i<6; i++)
+  {
+    x[i] = ix[i];
+    y[i] = iy[i];
+    z[i] = iz[i];
+  }
+  lx[0] = 5; 
+  lx[1] = 7; 
+  lx[2] = 5; 
+  lx[3] = 5; 
+  lx[4] = 7; 
+  lx[5] = 5; 
+  ly[0] = 10; 
+  ly[1] = 0; 
+  ly[2] = 10; 
+  ly[3] = 10; 
+  ly[4] = 0; 
+  ly[5] = 10;  
+}
+
+void Orbit::getAngle (int ix, int iy)
+{
+  oP = ix;
+  o2P = iy;
+  oN = 180 - ix;
+  o2N = 180 - iy;  
+}
+
+void Orbit::calculate()
+{
+  // X Size.
+  // Right side.
+  for (int i=3; i<6; i++)
+  {
+    float l1 = sqrt(pow(x[i], 2) + pow(y[i], 2));
+    float o1 = degrees(atan(y[i] / x[i]));
+    float o2 = oP - o1;  
+    Ny[i] = sin(radians(o2)) * l1;
+    Nx[i] = (cos(radians(o2)) * l1) - lx[i];
+    Nz[i] = z[i];
+  }
+
+  // Left size;
+  for (int i=0; i<4; i++)
+  {
+    float l1 = sqrt(pow(x[i], 2) + pow(y[i], 2));
+    float o1 = degrees(atan(y[i] / x[i]));
+    float o2 = oN - o1;  
+    Ny[i] = sin(radians(o2)) * l1;
+    Nx[i] = (cos(radians(o2)) * l1) - lx[i];
+    Nz[i] = z[i];
+  }
+
+  // Y Size.
+  // Right side.
+  /*for (int i=3; i<6; i++) WRong
+   {
+   float l1 = sqrt(pow(x[i], 2) + pow(y[i], 2));
+   float o1 = degrees(atan(y[i] / x[i]));
+   float o2 = o2P - o1;  
+   Ny[i] = sin(radians(o2)) * l1;
+   Nx[i] = (cos(radians(o2)) * l1) - lx[i];
+   Nz[i] = z[i];
+   }
+   
+   // Left size;
+   for (int i=0; i<4; i++) WRong
+   {
+   float l1 = sqrt(pow(x[i], 2) + pow(y[i], 2));
+   float o1 = degrees(atan(y[i] / x[i]));
+   float o2 = o2N - o1;  
+   Ny[i] = sin(radians(o2)) * l1;
+   Nx[i] = (cos(radians(o2)) * l1) - lx[i];
+   Nz[i] = z[i];
+   }*/
+}
+
+
+
+
+
+
+
+
+

2011-firefly-mega-ssc32/firmware/2011-nunchuk/hexapod_oud/Orbit.h

@@ -0,0 +1,30 @@
+// Orbit.h
+
+#ifndef Orbit_H
+#define Orbit_H
+
+#include "WProgram.h"
+
+class Orbit 
+{
+public:
+  void getData(int ix[6], int iy[6], int iz[6]);
+  void getAngle (int ix, int iy);
+  
+  void calculate();
+
+  int x[6], y[6], z[6];
+  int Nx[6], Ny[6], Nz[6];
+  int oP, o2P, oN, o2N;
+  int lx[6], ly[6];
+};
+
+
+#endif
+
+
+
+
+
+
+

2011-firefly-mega-ssc32/firmware/2011-nunchuk/hexapod_oud/Rotation.cpp

@@ -0,0 +1,23 @@
+// Rotation.cpp
+
+#include "Rotation.h"
+
+void Rotation::getData(int ix[6], int iy[6], int iz[6])
+{
+  for (int i=0; i<6; i++)
+  {
+    x[i] = ix[i];
+    y[i] = iy[i];
+    z[i] = iz[i];
+  } 
+}
+
+int NewX, NewY;
+void rotate (int x, int y, int angel)
+{
+  NewY = x * sin(radians(angel)) + y * cos(radians(angel));
+  NewX = x * cos(radians(angel)) - y * sin(radians(angel));
+}
+
+
+

2011-firefly-mega-ssc32/firmware/2011-nunchuk/hexapod_oud/Rotation.h

@@ -0,0 +1,24 @@
+// Rotation.h
+
+#ifndef Rotation_H
+#define Rotation_H
+
+#include "WProgram.h"
+
+class Rotation 
+{
+public:
+  void getData(int ix[6], int iy[6], int iz[6]);
+
+  int x[6], y[6], z[6];
+};
+
+
+#endif
+
+
+
+
+
+
+

2011-firefly-mega-ssc32/firmware/2011-nunchuk/hexapod_oud/SSC32.cpp

@@ -0,0 +1,79 @@
+#include "SSC32.h"
+
+void SSC32::setup()
+{
+  Serial1.begin(38400);
+  Serial.begin(38400);
+  time = 0;
+  walktime = 0;
+}
+
+void SSC32::MoveHead (int angel_from_mid)
+{
+  Serial1.print(" #"); 
+  Serial1.print(9); 
+  Serial1.print(" P");
+  Serial1.print(ConverttoMS((90 + angel_from_mid), smin[0], smax[0]));
+  Serial1.print(" T");
+  Serial1.println(100);
+}
+
+void SSC32::Render()
+{
+  for (int i=0; i<6; i++)
+  {
+    RAW_2_SERVO[0+(i*3)] = ConverttoMS(DEG_2_CON[0+(i*3)], smin[0], smax[0]);
+    RAW_2_SERVO[1+(i*3)] = ConverttoMS(DEG_2_CON[1+(i*3)], smin[1], smax[1]);
+    RAW_2_SERVO[2+(i*3)] = ConverttoMS(DEG_2_CON[2+(i*3)], smin[2], smax[2]);
+	
+    if (i != 0)
+      Serial.print(", ");
+    else
+      Serial.print("Ang: ");
+	  
+    Serial.print(DEG_2_CON[0+(i*3)]);
+    Serial.print(", ");
+    Serial.print(DEG_2_CON[1+(i*3)]);
+    Serial.print(", ");
+    Serial.print(DEG_2_CON[2+(i*3)]);
+  }
+  Serial.println(";");
+}
+
+int SSC32::ConverttoMS(int i, int low, int high)
+{
+  int tmp = ( i * ( ( high - low ) / 180 ) );
+  return low + tmp;
+}
+
+void SSC32::toServos ()
+{
+  // puss to servos
+
+    // var.    SSC32.
+  // 0 | 3  // 16 | 0
+  // 1 | 4  //  | | |
+  // 2 | 5  // 24 | 8
+
+  for (int i=0; i<9; i++)
+  {
+    Serial1.print(" #"); 
+    Serial1.print(16+i);// 18 22
+    Serial1.print(" P");
+    Serial1.print(RAW_2_SERVO[9+i]);
+  }
+
+  for (int i=0; i<9; i++)
+  {
+    Serial1.print(" #"); 
+    Serial1.print(i); 
+    Serial1.print(" P");
+    Serial1.print(RAW_2_SERVO[i]);
+  }
+
+  Serial1.print(" T");
+  Serial1.println(walktime);
+}
+
+
+

2011-firefly-mega-ssc32/firmware/2011-nunchuk/hexapod_oud/SSC32.h

@@ -0,0 +1,34 @@
+// SSC32.h
+
+#ifndef SSC32_H
+#define SSC32_H
+
+#include "WProgram.h"
+#include "Data_Manager.h"
+
+class SSC32 
+{
+public:
+  void setup();
+
+  void Render();
+  void toServos ();
+  void MoveHead (int angel_from_mid);
+
+  int ConverttoMS(int i, int low, int high);
+
+  // 0 | 3
+  // 1 | 4
+  // 2 | 5
+
+  int time;
+};
+
+
+#endif
+
+
+
+
+
+

2011-firefly-mega-ssc32/firmware/2011-nunchuk/hexapod_oud/WiiChuck.h

@@ -0,0 +1,210 @@
+/*
+ * Nunchuck -- Use a Wii Nunchuck
+ * Tim Hirzel http://www.growdown.com
+ * 
+ notes on Wii Nunchuck Behavior.
+ This library provides an improved derivation of rotation angles from the nunchuck accelerometer data.
+ The biggest different over existing libraries (that I know of ) is the full 360 degrees of Roll data
+ from teh combination of the x and z axis accelerometer data using the math library atan2. 
+ 
+ It is accurate with 360 degrees of roll (rotation around axis coming out of the c button, the front of the wii),
+ and about 180 degrees of pitch (rotation about the axis coming out of the side of the wii).  (read more below)
+ 
+ In terms of mapping the wii position to angles, its important to note that while the Nunchuck
+ sense Pitch, and Roll, it does not sense Yaw, or the compass direction.  This creates an important
+ disparity where the nunchuck only works within one hemisphere.  At a result, when the pitch values are 
+ less than about 10, and greater than about 170, the Roll data gets very unstable.  essentially, the roll
+ data flips over 180 degrees very quickly.   To understand this property better, rotate the wii around the
+ axis of the joystick.  You see the sensor data stays constant (with noise).  Because of this, it cant know
+ the difference between arriving upside via 180 degree Roll, or 180 degree pitch.  It just assumes its always
+ 180 roll.
+ 
+ 
+ * 
+ * This file is an adaptation of the code by these authors:
+ * Tod E. Kurt, http://todbot.com/blog/
+ *
+ * The Wii Nunchuck reading code is taken from Windmeadow Labs
+ * http://www.windmeadow.com/node/42
+ */
+
+#ifndef WiiChuck_h
+#define WiiChuck_h
+
+#include "WProgram.h" 
+#include <Wire.h>
+#include <math.h>
+
+
+// these may need to be adjusted for each nunchuck for calibration
+#define ZEROX 510  
+#define ZEROY 490
+#define ZEROZ 460
+#define RADIUS 180  // probably pretty universal
+
+#define DEFAULT_ZERO_JOY_X 124
+#define DEFAULT_ZERO_JOY_Y 132
+
+
+
+class WiiChuck {
+private:
+  byte cnt;
+  uint8_t status[6];		// array to store wiichuck output
+  byte averageCounter;
+  //int accelArray[3][AVERAGE_N];  // X,Y,Z
+  int i;
+  int total;
+  uint8_t zeroJoyX;   // these are about where mine are
+  uint8_t zeroJoyY; // use calibrateJoy when the stick is at zero to correct
+  int lastJoyX;
+  int lastJoyY;
+  int angles[3];
+
+  boolean lastZ, lastC;
+
+
+public:
+
+  byte joyX;
+  byte joyY;
+  boolean buttonZ;
+  boolean buttonC;
+  void begin() 
+  {
+    Wire.begin();
+    cnt = 0;
+    averageCounter = 0;
+    Wire.beginTransmission (0x52);	// transmit to device 0x52
+    Wire.send (0x40);		// sends memory address
+    Wire.send (0x00);		// sends memory address
+    Wire.endTransmission ();	// stop transmitting
+    update();            
+    for (i = 0; i<3;i++) {
+      angles[i] = 0;
+    }
+    zeroJoyX = DEFAULT_ZERO_JOY_X;
+    zeroJoyY = DEFAULT_ZERO_JOY_Y;
+  }
+
+
+  void calibrateJoy() {
+    zeroJoyX = joyX;
+    zeroJoyY = joyY;
+  }
+
+  void update() {
+
+    Wire.requestFrom (0x52, 6);	// request data from nunchuck
+    while (Wire.available ()) {
+      // receive byte as an integer
+      status[cnt] = _nunchuk_decode_byte (Wire.receive()); //
+      cnt++;
+    }
+    if (cnt > 5) {
+      lastZ = buttonZ;
+      lastC = buttonC;
+      lastJoyX = readJoyX();
+      lastJoyY = readJoyY();
+      //averageCounter ++;
+      //if (averageCounter >= AVERAGE_N)
+      //    averageCounter = 0;
+
+      cnt = 0;
+      joyX = (status[0]);
+      joyY = (status[1]);
+      for (i = 0; i < 3; i++) 
+        //accelArray[i][averageCounter] = ((int)status[i+2] << 2) + ((status[5] & (B00000011 << ((i+1)*2) ) >> ((i+1)*2))); 
+        angles[i] = (status[i+2] << 2) + ((status[5] & (B00000011 << ((i+1)*2) ) >> ((i+1)*2))); 
+
+      //accelYArray[averageCounter] = ((int)status[3] << 2) + ((status[5] & B00110000) >> 4); 
+      //accelZArray[averageCounter] = ((int)status[4] << 2) + ((status[5] & B11000000) >> 6); 
+
+      buttonZ = !( status[5] & B00000001);
+      buttonC = !((status[5] & B00000010) >> 1);
+      _send_zero(); // send the request for next bytes
+
+    }
+  }
+
+
+  // UNCOMMENT FOR DEBUGGING
+  //byte * getStatus() {
+  //    return status;
+  //}
+
+  float readAccelX() {
+    // total = 0; // accelArray[xyz][averageCounter] * FAST_WEIGHT;
+    return (float)angles[0] - ZEROX;
+  }
+  float readAccelY() {
+    // total = 0; // accelArray[xyz][averageCounter] * FAST_WEIGHT;
+    return (float)angles[1] - ZEROY;
+  }
+  float readAccelZ() {
+    // total = 0; // accelArray[xyz][averageCounter] * FAST_WEIGHT;
+    return (float)angles[2] - ZEROZ;
+  }
+
+  boolean zPressed() {
+    return (buttonZ && ! lastZ);
+  }
+  boolean cPressed() {
+    return (buttonC && ! lastC);
+  }
+
+  // for using the joystick like a directional button
+  boolean rightJoy(int thresh=60) {
+    return (readJoyX() > thresh and lastJoyX <= thresh);
+  }
+
+  // for using the joystick like a directional button
+  boolean leftJoy(int thresh=60) {
+    return (readJoyX() < -thresh and lastJoyX >= -thresh);
+  }
+
+
+  int readJoyX() {
+    return (int) joyX - zeroJoyX;
+  }
+
+  int readJoyY() {
+    return (int)joyY - zeroJoyY;
+  }
+
+
+  // R, the radius, generally hovers around 210 (at least it does with mine)
+  // int R() {
+  //     return sqrt(readAccelX() * readAccelX() +readAccelY() * readAccelY() + readAccelZ() * readAccelZ());  
+  // }
+
+
+  // returns roll degrees
+  int readRoll() {
+    return (int)(atan2(readAccelX(),readAccelZ())/ M_PI * 180.0);
+  }
+
+  // returns pitch in degrees
+  int readPitch() {        
+    return (int) (acos(readAccelY()/RADIUS)/ M_PI * 180.0);  // optionally swap 'RADIUS' for 'R()'
+  }
+
+private:
+  byte _nunchuk_decode_byte (byte x)
+  {
+    x = (x ^ 0x17) + 0x17;
+    return x;
+  }
+
+  void _send_zero()
+  {
+    Wire.beginTransmission (0x52);	// transmit to device 0x52
+    Wire.send (0x00);		// sends one byte
+    Wire.endTransmission ();	// stop transmitting
+  }
+
+};
+
+
+#endif
+

2011-firefly-mega-ssc32/firmware/2011-nunchuk/hexapod_oud/atan2.cpp

@@ -0,0 +1,15 @@
+// atan2.cpp
+
+#include "atan2.h"
+
+void joy::JoyToDegAndDis (int x, int y)
+{
+  deg = !(90 + degrees(atan2(y, x)));
+  distance = sqrt(y^2 + x^2);
+}
+
+int  joy::JoyToDeg (int x, int y)
+{ // watchout 90 + does not work good
+  return !(degrees(atan2(y, x)));
+}
+

2011-firefly-mega-ssc32/firmware/2011-nunchuk/hexapod_oud/atan2.h

@@ -0,0 +1,26 @@
+// atan2.h
+
+#ifndef joy_H
+#define joy_H
+
+#include "WProgram.h"
+
+class joy 
+{
+public:
+  void JoyToDegAndDis (int x, int y);
+  int  JoyToDeg (int x, int y);
+
+  int deg, distance;
+};
+
+
+#endif
+
+
+
+
+
+
+
+

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;
+  }
+}
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

2011-firefly-mega-ssc32/firmware/2011-nunchuk/hexapod_oud/sequnces.h

@@ -0,0 +1,39 @@
+// sequnces
+
+// Hexapod seqencer.
+int TriPoD_Size = 8;
+// zij, laag, achter
+// 15, 15, -15,   15, 15, 0,   15, 15, 15,   15, 15, -15,   15, 15, 0,   15, 15, 15;
+int TriPoD[8][18] = { 
+  {// -15 is voor
+    15, 2, 0,  15, 15, 0,    15, 2, 0,  15, 15, 0,15, 2, 0,  15, 15, 0                     }//miden*hoog, miden*laag
+  ,
+  {
+    15, 2, -2,  15, 15, 2,   15, 2, -2,  15, 15, 2,15, 2, -2,  15, 15, 2                  }// voor*hoog, achter*laag
+  ,
+  {
+    15, 15, -2,  15, 15, 2,    15, 15, -2,  15, 15, 2,15, 15, -2,  15, 15, 2                }// laag, niks
+  ,
+  {
+    15, 15, -2,  15, 2, 2,    15, 15, -2,  15, 2, 2,15, 15, -2,  15, 2, 2                 }// niks, hoog
+  ,
+  {
+    15, 15, 0,  15, 2, 0,    15, 15, 0,  15, 2, 0,15, 15, 0,  15, 2, 0                 }//midden*laag, miden*hoog
+  ,
+  {
+    15, 15, 2,  15, 2, -2,    15, 15, 2,  15, 2, -2,15, 15, 2,  15, 2, -2                   }//voor-laag, achter/hoog
+  ,
+  {
+    15, 15, 2,  15, 15, -2,    15, 15, 2,  15, 15, -2,15, 15, 2,  15, 15, -2                  }// niks, laag
+  ,
+  {
+    15, 2, 2,  15, 15, -2,    15, 2, 2,  15, 15, -2,15, 2, 2,  15, 15, -2                 }// hoog, niks
+};
+
+
+
+
+
+
+
+