FPGA version (2009-2011): photos and videos

Everything created on or before 2 Nov 2011 20:44:07, incl. the 'hexapod film 18-11-09'. From Electronics\Hexapod\media and Verwerkte media.

2011-firefly-mega-ssc32/docs/math/z axis rotation.url

@@ -0,0 +1,5 @@
+[{000214A0-0000-0000-C000-000000000046}]
+Prop3=19,11
+[InternetShortcut]
+URL=https://www.siggraph.org/education/materials/HyperGraph/modeling/mod_tran/3drota.htm
+IDList=

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,157 @@
+//#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);
+  if (COR_2_IK[2+(i*3)] != 0)
+    float temp = degrees(acos((float)COR_2_IK[2+(i*3)] / l3));
+  else
+    float temp = 90; // Math Error Protection.
+  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)] = 10;// z side body
+        COR_2_IK[0+(4*3)] = 10;
+      }
+      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,19 @@
+// sequnces
+
+// Hexapod seqencer.
+int TriPoD_Size = 8;
+
+int TriPoD[8][18] = { 
+ {10,10, 0,10,15, 0,10,10, 0,10,15, 0,10,10, 0,10,15, 0},
+ {10,10,-2,10,15, 2,10,10,-2,10,15, 3,10,10,-2,10,15, 2},
+ {10,15,-2,10,15, 2,10,15,-2,10,15, 3,10,15,-2,10,15, 2},
+ {10,15,-2,10,10, 2,10,15,-2,10,10, 3,10,15,-2,10,10, 2},
+ {10,15, 0,10,10, 0,10,15, 0,10,10, 0,10,15, 0,10,10, 0},
+ {10,15, 2,10,10,-2,10,15, 2,10,10,-2,10,15, 2,10,10,-2},
+ {10,15, 2,10,15,-2,10,15, 2,10,15,-2,10,15, 2,10,15,-2},
+ {10,10, 2,10,15,-2,10,10, 2,10,15,-2,10,10, 2,10,15,-2}
+ };
+
+
+
+

2011-firefly-mega-ssc32/firmware/2012-behaviours-wip/FireFly/FireFly.ino

@@ -0,0 +1,31 @@
+// Hexapod FireFly
+
+#include "hexapod.h"
+
+#include "Gedrags_Classen.cpp"
+
+//Watchdog WD;
+//Ultrasone US;
+hexapod *FireFly = new hexapod;
+
+void setup()
+{
+  Serial.begin(38400);
+  Serial1.begin(38400);
+  pinMode(13, OUTPUT);
+
+  FireFly->FireFly();
+}
+
+// Main loop.
+void loop()
+{
+  DataObject *cache = new DataObject;
+  
+  cache->input(5, 3, 8, 8, 3, 0, 5, 3, -8, 5, 3, 8, 8, 3, 0, 5, 3, -8, 500);
+
+  //WD.CheckTime(DataLine);
+
+  FireFly->CoreSend(cache);
+}
+

2011-firefly-mega-ssc32/firmware/2012-behaviours-wip/FireFly/Gedrags_Classen.cpp

@@ -0,0 +1,55 @@
+//Gedrags classen,
+
+#ifndef GCC
+#define GCC
+
+#include "hexapod.h"
+
+//    LAYOUT
+//      _U_
+//  0==/   \==3
+//     >***<
+// 1===|---|===4
+//     >***<
+//  2==\___/==5
+
+class WalkForward	//Direction
+{
+
+private:
+  DataObject LoopPatroon[8];
+  DataObject HuidigePositie;
+  hexapod *FireFly;
+
+  //Startup VARIABLES
+  void init(hexapod *pntr)
+  {
+    LoopPatroon[0](0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 800);
+    LoopPatroon[1](0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 800);
+    LoopPatroon[2](0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 800);
+    LoopPatroon[3](0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 800);
+    LoopPatroon[4](0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 800);
+    LoopPatroon[5](0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 800);
+    LoopPatroon[6](0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 800);
+    LoopPatroon[7](0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 800);
+
+    FireFly = pntr;
+  }
+  
+  void send(DataObject pntr)
+  {
+    FireFly->CoreSend(&pntr);
+  }
+
+
+public:
+  void Run()
+  {
+    //Start runnning code until exit or expection raises.
+    send(LoopPatroon[0]);//Test
+  }
+
+};
+
+#endif
+

2011-firefly-mega-ssc32/firmware/2012-behaviours-wip/FireFly/Ultrasone/Ultrasone.cpp

@@ -0,0 +1,60 @@
+//Ultrasone.cpp
+//  V3.0
+
+#include "Ultrasone.h"
+
+void Ultrasone::GetRadar(Data *DataLine) 
+{
+  DataLine->HeadTime = 0;
+
+#ifdef DEBUGUS
+  Serial.print("### Ultrasone Direction/Position: ");
+  Serial.print(CurrentDirection);
+  Serial.print(", ");
+  Serial.println(CurrentPosition);
+#endif
+
+  if (CurrentDirection == 0) // Left
+  {
+    if (CurrentPosition > MaxLeft)
+    {
+      CurrentPosition--;
+      DataLine->HeadAngle = CurrentPosition;
+
+      Con.ConvertHead(DataLine);
+      SSC32.MoveHead(DataLine);
+
+      Serial.print("### Ultrasone Distance: ");
+      Serial.println(DataLine->FireFly.GetUltraSoneDistance());
+    }
+    else
+      CurrentDirection = 1;
+  }
+  else if(CurrentDirection == 1) // Right
+  {
+    if (CurrentPosition < MaxRight)
+    {
+      CurrentPosition++;
+      DataLine->HeadAngle = CurrentPosition;
+
+      Con.ConvertHead(DataLine);
+      SSC32.MoveHead(DataLine);
+
+      Serial.print("### Ultrasone Distance: ");
+      Serial.println(DataLine->FireFly.GetUltraSoneDistance());
+    }
+    else
+      CurrentDirection = 0;
+  }
+}
+
+
+/*
+    [Vector]
+    X = Distance / Cos( CurrentPosition * pi2rad );
+    Y = Distance / sin( CurrentPosition / pi2rad );    
+*/
+
+
+
+

2011-firefly-mega-ssc32/firmware/2012-behaviours-wip/FireFly/Ultrasone/Ultrasone.h

@@ -0,0 +1,39 @@
+//Ultrasone.h
+//  V3.0
+
+#ifndef ULTRASONE_H
+#define ULTRASONE_H
+
+#include "Arduino.h"
+#include "Data.h"
+#include "Servos.h"
+#include "Converter.h"
+
+#define MaxLeft -45
+#define MaxRight 45
+
+class  Ultrasone
+{
+private:
+  Servos SSC32;
+  Converter Con;
+
+public:
+  void GetRadar(Data *DataLine);
+
+  int CurrentPosition;
+  byte CurrentDirection;
+
+  Ultrasone()
+  {
+    CurrentPosition = 0; 
+    CurrentDirection = 0; // Left
+  }
+};
+
+
+#endif
+
+
+
+

2011-firefly-mega-ssc32/firmware/2012-behaviours-wip/FireFly/WatchDog/Watchdog.cpp

@@ -0,0 +1,37 @@
+// Watchdog.cpp
+
+#include "Watchdog.h"
+
+void Watchdog::CheckTime(Data *DataLine)
+{
+  if ((millis() >= (DataLine->LastSendTime + DataLine->WalkTime)) & (DataLine->ReadyToSend = true ))
+  {
+    DataLine->Send = 1;
+
+    DataLine->LastSendTime = millis();
+#ifdef DEBUGW
+    Serial.println("### WatchDog->Watchdog ###");
+    Serial.print("### ");
+    Serial.print( millis());
+    Serial.println(" WatchDog->DataLine.Send = True");
+    Serial.print("### WatchDog->Watchdog Walktime: ");
+    Serial.println(DataLine->WalkTime);
+#endif
+  }
+  else if ((DataLine->LastSendTime == 0 ) & (DataLine->ReadyToSend = true))
+  {
+    DataLine->Send = 1;
+
+    DataLine->LastSendTime = millis();
+#ifdef DEBUGW
+    Serial.println("### WatchDog->Watchdog Startup ###");
+    Serial.print("### ");
+    Serial.print( millis());
+    Serial.println(" WatchDog->DataLine.Send = True");
+    Serial.print("### WatchDog->Watchdog Walktime: ");
+    Serial.println(DataLine->WalkTime);
+#endif
+  }
+}
+
+

2011-firefly-mega-ssc32/firmware/2012-behaviours-wip/FireFly/WatchDog/Watchdog.h

@@ -0,0 +1,20 @@
+// Watchdog.h
+//  V3.0
+
+#ifndef WATCHDOG_H
+#define WATCHDOG_H
+
+#include "Arduino.h"
+#include "Data.h"
+
+class Watchdog 
+{
+public:
+  void CheckTime(Data *DataLine);
+
+};
+
+
+#endif
+
+

2011-firefly-mega-ssc32/firmware/2012-behaviours-wip/FireFly/concept_informatie.txt

@@ -0,0 +1,62 @@
+class gedrag{
+
+	private:
+
+	data_line PositieInformatie;
+	body_struct main_positie;
+	int HuidigePositieinProgramma;
+	int matrix[..] MatrixmetdeGedragPatroonen;
+
+	public:
+
+	run()
+	{
+		/*
+			run normaal gedrag patroon.
+			
+			[loop patroon dat niet word beinvloed door userinput]
+			vraag aankomende gedrag op uit matrix,
+			-bereken de nieuwe posities
+			--sla deze op in cache
+			---wacht tot de watchdog een timeout geeft
+			----geef de cache door aan de servo controller.
+			
+			[loop patroon dat WORD beinvloed door userinput]
+			vraag aankomende gedrag op uit matrix met userinput,
+			-bereken de nieuwe posities
+			--sla deze op in cache
+			---loop tot de watchdog een timeout geeft
+			----geef de cache door aan de servo controller.
+		*/
+	};
+
+	quite()
+	{
+		while()
+		{
+			/*
+				maak routine af om terug te gaan naar de main positie.
+				daarna return 0;
+				
+				verplaats de body in een neutrale positie
+				+als niet mogeleijk verplaats de poten die problemen geven
+				-mogelijk ga verder met de poten een voor een in de goode positie te zetten
+			*/
+		}
+	};
+
+	exit()
+	{
+		/*
+			NOOD, stop nu en return positie;
+		*/
+	};
+
+	userinput()
+	{
+		/*
+			Mogelijkheid tot user input.
+		*/
+	};
+	
+};

2011-firefly-mega-ssc32/firmware/2012-glcd-remote/hexapod_remote_glcd/hexapod_remote_glcd.ino

@@ -0,0 +1,259 @@
+/*
+  GLCD Library - Hello World
+ 
+ This sketch prints "Hello World!" to the LCD
+ and shows the time in seconds since reset.
+ 
+ The circuit:
+ See the inlcuded documentation in glcd/doc directory for how to wire
+ up the glcd module. glcd/doc/GLCDref.htm can be viewed in your browser
+ by clicking on the file.
+ 
+ */
+
+// include the library header
+#include "Arduino.h"
+#include "hexapod.h"
+#include "glcd.h"
+//#include "math.h"
+//#include "Wire.h"
+//#include "WiiChuck.h"
+#include "bitmaps/allBitmaps.h"
+//#include "IRremote.h"
+
+#define MAXANGLE 90
+#define MINANGLE -90
+
+// include the Fonts
+#include "fonts/allFonts.h"
+
+WiiChuck chuck = WiiChuck();
+int angleStart, currentAngle;
+int tillerStart = 0;
+double angle;
+int RECV_PIN = 11;
+int m, mt; //mode
+long int oud, oud2, oudt, t; //ouden ontvangen code
+String protocol;
+
+IRrecv irrecv(RECV_PIN);
+
+decode_results results;
+
+void setup() {
+  // Initialize the GLCD 
+  GLCD.Init();
+
+  pinMode(13, OUTPUT);
+  digitalWrite(13, LOW);
+
+  // Select the font for the default text area
+  GLCD.SelectFont(SystemFont5x7);
+
+  Serial.begin(9600);
+  irrecv.enableIRIn(); // Start the IR receiver
+
+  chuck.begin();
+  while (!chuck.plugd)
+    chuck.CheckifPlugd();
+
+  //GLCD.DrawBitmap(Earth, 0, 0, WHITE);
+
+  GLCD.CursorTo(12, 1); 
+  GLCD.print("Nederland");
+  delay(3500);
+}
+
+long int getIR(decode_results *results) {
+  return results->value; 
+}
+
+void dump(decode_results *results) {
+  int count = results->rawlen;
+  GLCD.CursorTo(0, 1);
+  if (results->decode_type == UNKNOWN) {
+    protocol = "Unknown encoding: ";
+  } 
+  else if (results->decode_type == NEC) {
+    protocol = "Decoded NEC: ";
+  } 
+  else if (results->decode_type == SONY) {
+    protocol = "Decoded SONY: ";
+  } 
+  else if (results->decode_type == RC5) {
+    protocol = "Decoded RC5: ";
+  } 
+  else if (results->decode_type == RC6) {
+    protocol = "Decoded RC6: ";
+  }
+  GLCD.print(protocol);
+  GLCD.CursorTo(0, 2); 
+  GLCD.print(oud2, HEX); 
+  GLCD.CursorTo(0, 3);
+  GLCD.print(oud, HEX);
+  GLCD.CursorTo(0, 4);
+  long int t = results->value;
+  oudt = t;
+}
+
+#define s1 50
+#define s2 50
+int stop = 0;
+int stopstate = 0;
+
+void loop() 
+{
+  if (irrecv.decode(&results)){
+    long int t2 = getIR(&results);
+    t = t2;
+  }
+
+  if (stop)
+  {
+    if ((t == 0x1045B) & (stopstate != 2)) {
+      mt = 0;
+      if (m == 0)
+        m = 1;
+      else
+        m = 0;
+      stopstate = 2;
+    }
+    else if (( t == 0x45B) & (stopstate != 3)) {
+      mt = 0;
+      if (m == 0)
+        m = 1;
+      else
+        m = 0;
+      stopstate = 3;
+    }
+    else if ((t == 0x1042C ) || ( t == 0x42C)) {
+      stop = false;
+    }
+  }
+  else if ((t == 0x431 ) || ( t == 0x10431)) { 
+    digitalWrite(13, HIGH);
+    stop = true;
+  }
+  else if ((t == 0x40F ) || ( t == 0x1040F)) {
+    digitalWrite(13, HIGH);
+    irrecv.resume(); // Receive the next value
+    m = 60;
+  }
+  else if ((t == 0x1040C ) || ( t == 0x40C)) {
+    //power
+    GLCD.ClearScreen();
+    //GLCD.DrawBitmap(Power, 64, 0, BLACK);
+
+    while (1){
+    }
+  }
+  else
+  {
+    mt++;
+    digitalWrite(13, LOW);
+  }
+
+  switch (mt){
+  case 1:   
+    m = 0;  
+    break;
+  case s1:  
+    m = 1;  
+    break;
+  case s1+s2:
+    mt = 0; 
+    break;
+  }
+
+  if (m == 0)
+  {
+    GLCD.ClearScreen();
+    GLCD.CursorTo(0, 0); 
+    GLCD.print("IR Receiver");
+    //GLCD.DrawBitmap(Ir, 116, 0, BLACK);
+    if (irrecv.decode(&results)) {
+      dump(&results);
+      delay(100);
+      irrecv.resume(); // Receive the next value
+      oud2 = oud;
+    }
+    else
+    {
+      oud = oudt;
+      GLCD.CursorTo(0, 1);
+      GLCD.print(protocol);
+      GLCD.CursorTo(0, 2); 
+      GLCD.print(oud2, HEX); 
+      GLCD.CursorTo(0, 3); 
+      GLCD.print(oud, HEX); 
+      GLCD.CursorTo(0, 4); 
+      GLCD.print("No DATA Input"); 
+    }
+
+  }
+  else
+  {
+    GLCD.ClearScreen();
+
+    chuck.update();
+    if (!chuck.plugd)
+      chuck.CheckifPlugd();
+    else {
+
+      GLCD.CursorTo(0, 0);
+
+      GLCD.print("MangaValk");
+      //GLCD.DrawBitmap(Nun, 106, 0, BLACK);
+
+      GLCD.CursorTo(0, 2);
+
+      GLCD.print("C: "); 
+      GLCD.print(chuck.buttonC); 
+      GLCD.print(" Z: "); 
+      GLCD.print(chuck.buttonZ);
+
+      GLCD.CursorTo(0, 3);
+      GLCD.print("Roll: ");
+      GLCD.print(chuck.readRoll());
+      GLCD.CursorTo(0, 4);
+      GLCD.print("Pitch: ");
+      GLCD.print(chuck.readPitch()-75);
+      GLCD.CursorTo(0, 5);
+      GLCD.print("Jy: ");
+      GLCD.print(chuck.readJoyY());
+      GLCD.CursorTo(0, 6);
+      GLCD.print("Jx: ");
+      GLCD.print(chuck.readJoyX()); 
+      GLCD.CursorTo(0, 7);
+      GLCD.print("Atan2: ");
+      GLCD.print(atan2(chuck.readJoyX(), chuck.readJoyY()));
+    }
+
+    irrecv.resume(); // Receive the next value
+  }
+  delay(100);
+}
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+

2011-firefly-mega-ssc32/firmware/2012-ir-radar-gps/hexapodV3/Converter.cpp

@@ -0,0 +1,70 @@
+// Converter.cpp
+//  V3.0
+
+#include "Converter.h"
+
+void Converter::setup()
+{
+  //Serial.begin(9600);
+}
+
+int ConverttoMS(int Position, int low, int high)
+{
+  int tmp = ( Position * ( ( high - low ) / 180 ) );
+  return low + tmp;
+}
+
+void ConvertServos(struct Data *DataLine)
+{
+  for (int i=0; i<6; i++)
+  {
+    DataLine->ServoData[i].t1 = ConverttoMS(DataLine->ServoData[i].o1, smin0, smax0);
+    DataLine->ServoData[i].t2 = ConverttoMS(DataLine->ServoData[i].o2, smin1, smax1);
+    DataLine->ServoData[i].t3 = ConverttoMS(DataLine->ServoData[i].o3, smin2, smax2);
+  }
+
+#ifdef DEBUGS
+  for (int i=0; i<6; i++)
+  {
+    if (i != 0)
+      Serial.print(", ");
+    else
+      Serial.print("### Servos->mSend ServosAngles Are: ");
+
+    Serial.print(DataLine->ServoData[i].o1);
+    Serial.print(", ");
+    Serial.print(DataLine->ServoData[i].o2);
+    Serial.print(", ");
+    Serial.print(DataLine->ServoData[i].o3);
+  }
+  Serial.println(";");
+#endif
+}
+
+void Converter::ConvertHead(struct Data *DataLine)
+{
+  DataLine->HeadPosition = ConverttoMS((90 + DataLine->HeadAngle), smin3, smax3);
+
+#ifdef DEBUGC
+  Serial.print("### Converter->HeadPosition is: ");
+  Serial.println(DataLine->HeadPosition);
+#endif
+}
+
+void Converter::Convert(Data *DataLine)
+{
+#ifdef DEBUGC
+  Serial.println("### Converter->Converter ###");
+#endif
+
+  ConvertServos(DataLine);
+  //ConvertHead(DataLine);
+
+#ifdef DEBUGC
+  Serial.println("### Converter->ReadyToSend = true");
+#endif
+
+  DataLine->ReadyToSend = true;
+}
+
+

2011-firefly-mega-ssc32/firmware/2012-ir-radar-gps/hexapodV3/Converter.h

@@ -0,0 +1,27 @@
+// Converter.h
+//  V3.0
+
+#ifndef CONVERTER_H
+#define CONVERTER_H
+
+#include "Arduino.h"
+#include "Data.h"
+
+class Converter 
+{
+public:
+  void setup();
+
+  void Convert(Data *DataLine);
+  void ConvertHead(Data *DataLine);
+};
+
+
+#endif
+
+
+
+
+
+
+

2011-firefly-mega-ssc32/firmware/2012-ir-radar-gps/hexapodV3/Data.h

@@ -0,0 +1,63 @@
+//Data.h Data Structure
+
+#ifndef DATA
+#define DATA
+
+#include "Arduino.h"
+#include "hexapod.h"
+#include "Def.h"
+#include "SerD.h"
+
+//    LAYOUT
+//      _U_
+//  0==/   \==3
+//     >***<
+// 1===|---|===4
+//     >***<
+//  2==\___/==5
+
+struct Data
+{
+  //MAIN
+  // Pointers to classes
+  hexapod FireFly;
+
+  //Watchdog
+  boolean Send;
+  double LastSendTime;
+  int WalkTime;
+  int HeadTime;  //100
+
+  //Converter
+  SerD ServoData[6];
+  float HeadAngle;
+  int ReadyToSend;
+
+  //IK
+  int HeadPosition;
+  boolean ReadyToConvert;
+  float l3;
+  float s;
+
+  //Startup VARIABLES
+  Data()
+  {
+    LastSendTime = 0;
+    HeadAngle = 0;
+    HeadTime = 100;
+
+    ServoData[0](5, 3, 8);
+    ServoData[1](8, 3, 0);
+    ServoData[2](5, 3, -8);
+    ServoData[3](5, 3, 8);
+    ServoData[4](8, 3, 0);
+    ServoData[5](5, 3, -8);
+
+  }
+
+};
+
+#endif
+
+
+