diff --git a/2011-firefly-mega-ssc32/firmware/2012-ir-radar-gps/hexapodV3/Converter.cpp b/2011-firefly-mega-ssc32/firmware/2012-ir-radar-gps/hexapodV3/Converter.cpp
new file mode 100644
index 0000000..5a638d4
--- /dev/null
+++ b/2011-firefly-mega-ssc32/firmware/2012-ir-radar-gps/hexapodV3/Converter.cpp
@@ -0,0 +1,73 @@
+// Converter.cpp
+
+#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;
+}
+
+
+
+
+
+
diff --git a/2011-firefly-mega-ssc32/firmware/2012-ir-radar-gps/hexapodV3/Converter.h b/2011-firefly-mega-ssc32/firmware/2012-ir-radar-gps/hexapodV3/Converter.h
new file mode 100644
index 0000000..e6b4494
--- /dev/null
+++ b/2011-firefly-mega-ssc32/firmware/2012-ir-radar-gps/hexapodV3/Converter.h
@@ -0,0 +1,26 @@
+// Converter.h
+
+#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
+
+
+
+
+
+
+
diff --git a/2011-firefly-mega-ssc32/firmware/2012-ir-radar-gps/hexapodV3/Data.h b/2011-firefly-mega-ssc32/firmware/2012-ir-radar-gps/hexapodV3/Data.h
new file mode 100644
index 0000000..389d1de
--- /dev/null
+++ b/2011-firefly-mega-ssc32/firmware/2012-ir-radar-gps/hexapodV3/Data.h
@@ -0,0 +1,70 @@
+//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
+
+
+
+
+
+
+
+
+
+
diff --git a/2011-firefly-mega-ssc32/firmware/2012-ir-radar-gps/hexapodV3/Def.h b/2011-firefly-mega-ssc32/firmware/2012-ir-radar-gps/hexapodV3/Def.h
new file mode 100644
index 0000000..c0f844b
--- /dev/null
+++ b/2011-firefly-mega-ssc32/firmware/2012-ir-radar-gps/hexapodV3/Def.h
@@ -0,0 +1,24 @@
+//Def.h
+//Define some Hard Coded Variables
+
+//DEBUG CODES
+//#define DEBUGC
+//#define DEBUGIK
+//#define DEBUGS
+//#define DEBUGHS // ServosHead
+//#define DEBUGW
+//#define Freeze
+
+//InverseKinemetics VARIABLES
+#define l2 15
+#define l1 10
+
+//Converter VARIABLES
+#define smin0 600
+#define smin1 960
+#define smin2 600
+#define smin3 600
+#define smax0 2400
+#define smax1 2100
+#define smax2 2400
+#define smax3 2400
diff --git a/2011-firefly-mega-ssc32/firmware/2012-ir-radar-gps/hexapodV3/InverseKinemetics.cpp b/2011-firefly-mega-ssc32/firmware/2012-ir-radar-gps/hexapodV3/InverseKinemetics.cpp
new file mode 100644
index 0000000..090c217
--- /dev/null
+++ b/2011-firefly-mega-ssc32/firmware/2012-ir-radar-gps/hexapodV3/InverseKinemetics.cpp
@@ -0,0 +1,158 @@
+#include "InverseKinemetics.h"
+
+//Variables
+boolean error;
+
+void RL3 (int i, Data *DataLine)
+{
+  float x = DataLine->ServoData[i].x;
+  float z = DataLine->ServoData[i].z;
+
+  if ((x == 0) && (z == 0)) 
+    DataLine->l3 = 0;
+  else DataLine->l3 = hypot(z, x);
+
+  if (x < 0)
+    DataLine->l3 = DataLine->l3 * -1;
+}
+
+float O1 (int i, Data *DataLine)
+{
+  RL3(i, DataLine);
+
+  float x = DataLine->ServoData[i].x;
+  float z = DataLine->ServoData[i].z;
+  float r;
+
+  if ((x != 0) & (z != 0)){
+    double a = (z / x);
+    return atan(a);
+  }
+  else
+    return atan(0.0); // Math Error Protection.
+}
+
+float T1 (int i, Data *DataLine)
+{
+  float x = DataLine->ServoData[i].x;
+  float y = DataLine->ServoData[i].y;
+  float z = DataLine->ServoData[i].z;
+  float x2 = x * x;
+  float y2 = y * y;
+  float z2 = z * z;
+  float l32 = DataLine->l3 * DataLine->l3;
+
+  DataLine->s = sqrt(x2 + y2 + z2);
+
+  float t1 = 0;
+
+  float s2 = DataLine->s * DataLine->s;
+
+  if ((y == 0) && (DataLine->l3 == 0))
+    t1 = 0; // Math Error Protection.
+  else
+    t1 = (atan2(float(DataLine->l3), float(y)));
+
+  float a = (l1 * l1) - (l2*l2) + s2;
+  float b = 2 * l1 * DataLine->s;
+  float t2 = acos(a / b);
+
+  float ret = 0;
+
+  ret = t1 + abs(t2);
+
+  return ret;
+}
+
+float T3 (Data *DataLine)
+{
+  return (acos(((l1*l1) + (l2*l2) - (DataLine->s*DataLine->s)) / (2 * l1 * l2)));
+}
+
+void setleg (int i, Data *DataLine)
+{
+  Segment Seg;
+
+  if (i < 3)
+  {
+    float a = ((float)90 - degrees(O1(i, DataLine)));
+    float b = ((float)180 - degrees(T1(i, DataLine)));
+    float c = degrees(T3(DataLine));
+
+    if ((a >= 0) & (a <= 180)
+      & (b >= 0) & (b <= 180)
+      & (c >= 0) & (c <= 180))
+    {
+      DataLine->ServoData[i].o1 = a;
+      DataLine->ServoData[i].o2 = b;
+      DataLine->ServoData[i].o3 = c;
+    }
+    else
+    {
+      error = true;
+      Seg.sevenSegWrite('e');
+    }
+  }
+  else
+  {
+    float a = ((float)90 + degrees(O1(i, DataLine)));
+    float b = degrees(T1(i, DataLine));
+    float c = ((float)180 - degrees(T3(DataLine)));
+
+    if ((a >= 0) & (a <= 180)
+      & (b >= 0) & (b <= 180)
+      & (c >= 0) & (c <= 180))
+    {
+      DataLine->ServoData[i].o1 = a;
+      DataLine->ServoData[i].o2 = b;
+      DataLine->ServoData[i].o3 = c;
+    }
+    else
+    {
+      error = true;
+      Seg.sevenSegWrite('e');
+    }
+  }
+}
+
+void InverseKinemetics::IK(Data *DataLine)  
+{
+  Segment Seg;
+  Seg.sevenSegWrite('o');
+
+  error = false;
+
+  for (int i=0; i<6; i++)
+    setleg(i, DataLine);
+
+#ifdef DEBUGIK
+  for (int i=0; i<6; i++)
+  {
+    if (i != 0)
+      Serial.print(", ");
+    else
+      Serial.print("### InverseKinemetics->ServosPosition's are: ");
+
+    Serial.print(DataLine->ServoData[i].x);
+    Serial.print(", ");
+    Serial.print(DataLine->ServoData[i].y);
+    Serial.print(", ");
+    Serial.print(DataLine->ServoData[i].z);
+  }
+  if (error == true)
+    Serial.println("; ERROR");
+  else
+    Serial.println(";");
+#endif
+}
+
+
+
+
+
+
+
+
+
+
+
diff --git a/2011-firefly-mega-ssc32/firmware/2012-ir-radar-gps/hexapodV3/InverseKinemetics.h b/2011-firefly-mega-ssc32/firmware/2012-ir-radar-gps/hexapodV3/InverseKinemetics.h
new file mode 100644
index 0000000..8d4bd16
--- /dev/null
+++ b/2011-firefly-mega-ssc32/firmware/2012-ir-radar-gps/hexapodV3/InverseKinemetics.h
@@ -0,0 +1,27 @@
+// InverseKinemetics.h
+
+#ifndef inverseKinemetics_H
+#define inverseKinemetics_H
+
+#include "Data.h"
+#include "Arduino.h"
+#include "hexapod.h"
+
+class InverseKinemetics
+{
+public:
+  void IK(Data *DataLine);
+
+};
+
+
+#endif
+
+
+
+
+
+
+
+
+
diff --git a/2011-firefly-mega-ssc32/firmware/2012-ir-radar-gps/hexapodV3/Oud/Data_Manager.cpp b/2011-firefly-mega-ssc32/firmware/2012-ir-radar-gps/hexapodV3/Oud/Data_Manager.cpp
new file mode 100644
index 0000000..13cd069
--- /dev/null
+++ b/2011-firefly-mega-ssc32/firmware/2012-ir-radar-gps/hexapodV3/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;
diff --git a/2011-firefly-mega-ssc32/firmware/2012-ir-radar-gps/hexapodV3/Oud/Data_Manager.h b/2011-firefly-mega-ssc32/firmware/2012-ir-radar-gps/hexapodV3/Oud/Data_Manager.h
new file mode 100644
index 0000000..b8ca939
--- /dev/null
+++ b/2011-firefly-mega-ssc32/firmware/2012-ir-radar-gps/hexapodV3/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
+
+
+
diff --git a/2011-firefly-mega-ssc32/firmware/2012-ir-radar-gps/hexapodV3/Oud/Header.h b/2011-firefly-mega-ssc32/firmware/2012-ir-radar-gps/hexapodV3/Oud/Header.h
new file mode 100644
index 0000000..1b735e0
--- /dev/null
+++ b/2011-firefly-mega-ssc32/firmware/2012-ir-radar-gps/hexapodV3/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); 
+}
+
+
diff --git a/2011-firefly-mega-ssc32/firmware/2012-ir-radar-gps/hexapodV3/Oud/InverseKinemetics.cpp b/2011-firefly-mega-ssc32/firmware/2012-ir-radar-gps/hexapodV3/Oud/InverseKinemetics.cpp
new file mode 100644
index 0000000..c558a18
--- /dev/null
+++ b/2011-firefly-mega-ssc32/firmware/2012-ir-radar-gps/hexapodV3/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;
+}
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/2011-firefly-mega-ssc32/firmware/2012-ir-radar-gps/hexapodV3/Oud/InverseKinemetics.h b/2011-firefly-mega-ssc32/firmware/2012-ir-radar-gps/hexapodV3/Oud/InverseKinemetics.h
new file mode 100644
index 0000000..92216e6
--- /dev/null
+++ b/2011-firefly-mega-ssc32/firmware/2012-ir-radar-gps/hexapodV3/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
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/2011-firefly-mega-ssc32/firmware/2012-ir-radar-gps/hexapodV3/Oud/Orbit.cpp b/2011-firefly-mega-ssc32/firmware/2012-ir-radar-gps/hexapodV3/Oud/Orbit.cpp
new file mode 100644
index 0000000..9c8f4aa
--- /dev/null
+++ b/2011-firefly-mega-ssc32/firmware/2012-ir-radar-gps/hexapodV3/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];
+   }*/
+}
+
+
+
+
+
+
+
+
+
diff --git a/2011-firefly-mega-ssc32/firmware/2012-ir-radar-gps/hexapodV3/Oud/Orbit.h b/2011-firefly-mega-ssc32/firmware/2012-ir-radar-gps/hexapodV3/Oud/Orbit.h
new file mode 100644
index 0000000..55796c1
--- /dev/null
+++ b/2011-firefly-mega-ssc32/firmware/2012-ir-radar-gps/hexapodV3/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
+
+
+
+
+
+
+
diff --git a/2011-firefly-mega-ssc32/firmware/2012-ir-radar-gps/hexapodV3/Oud/Rotation.cpp b/2011-firefly-mega-ssc32/firmware/2012-ir-radar-gps/hexapodV3/Oud/Rotation.cpp
new file mode 100644
index 0000000..b6ff0d4
--- /dev/null
+++ b/2011-firefly-mega-ssc32/firmware/2012-ir-radar-gps/hexapodV3/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));
+}
+
+
+
diff --git a/2011-firefly-mega-ssc32/firmware/2012-ir-radar-gps/hexapodV3/Oud/Rotation.h b/2011-firefly-mega-ssc32/firmware/2012-ir-radar-gps/hexapodV3/Oud/Rotation.h
new file mode 100644
index 0000000..af8fffd
--- /dev/null
+++ b/2011-firefly-mega-ssc32/firmware/2012-ir-radar-gps/hexapodV3/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
+
+
+
+
+
+
+
diff --git a/2011-firefly-mega-ssc32/firmware/2012-ir-radar-gps/hexapodV3/Oud/SSC32.cpp b/2011-firefly-mega-ssc32/firmware/2012-ir-radar-gps/hexapodV3/Oud/SSC32.cpp
new file mode 100644
index 0000000..3a9a1cf
--- /dev/null
+++ b/2011-firefly-mega-ssc32/firmware/2012-ir-radar-gps/hexapodV3/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);
+}
+
+
+
diff --git a/2011-firefly-mega-ssc32/firmware/2012-ir-radar-gps/hexapodV3/Oud/SSC32.h b/2011-firefly-mega-ssc32/firmware/2012-ir-radar-gps/hexapodV3/Oud/SSC32.h
new file mode 100644
index 0000000..2351e3e
--- /dev/null
+++ b/2011-firefly-mega-ssc32/firmware/2012-ir-radar-gps/hexapodV3/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
+
+
+
+
+
+
diff --git a/2011-firefly-mega-ssc32/firmware/2012-ir-radar-gps/hexapodV3/Oud/WiiChuck.h b/2011-firefly-mega-ssc32/firmware/2012-ir-radar-gps/hexapodV3/Oud/WiiChuck.h
new file mode 100644
index 0000000..86b5cc5
--- /dev/null
+++ b/2011-firefly-mega-ssc32/firmware/2012-ir-radar-gps/hexapodV3/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
+
diff --git a/2011-firefly-mega-ssc32/firmware/2012-ir-radar-gps/hexapodV3/Oud/atan2.cpp b/2011-firefly-mega-ssc32/firmware/2012-ir-radar-gps/hexapodV3/Oud/atan2.cpp
new file mode 100644
index 0000000..2e3573c
--- /dev/null
+++ b/2011-firefly-mega-ssc32/firmware/2012-ir-radar-gps/hexapodV3/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)));
+}
+
diff --git a/2011-firefly-mega-ssc32/firmware/2012-ir-radar-gps/hexapodV3/Oud/atan2.h b/2011-firefly-mega-ssc32/firmware/2012-ir-radar-gps/hexapodV3/Oud/atan2.h
new file mode 100644
index 0000000..c69e52a
--- /dev/null
+++ b/2011-firefly-mega-ssc32/firmware/2012-ir-radar-gps/hexapodV3/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
+
+
+
+
+
+
+
+
diff --git a/2011-firefly-mega-ssc32/firmware/2012-ir-radar-gps/hexapodV3/Oud/hexapod.cpp b/2011-firefly-mega-ssc32/firmware/2012-ir-radar-gps/hexapodV3/Oud/hexapod.cpp
new file mode 100644
index 0000000..0866ea2
--- /dev/null
+++ b/2011-firefly-mega-ssc32/firmware/2012-ir-radar-gps/hexapodV3/Oud/hexapod.cpp
@@ -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;
+  }
+}
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/2011-firefly-mega-ssc32/firmware/2012-ir-radar-gps/hexapodV3/Oud/sequnces.h b/2011-firefly-mega-ssc32/firmware/2012-ir-radar-gps/hexapodV3/Oud/sequnces.h
new file mode 100644
index 0000000..0f0fbd6
--- /dev/null
+++ b/2011-firefly-mega-ssc32/firmware/2012-ir-radar-gps/hexapodV3/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}
+ };
+
+
+
+
diff --git a/2011-firefly-mega-ssc32/firmware/2012-ir-radar-gps/hexapodV3/SerD.h b/2011-firefly-mega-ssc32/firmware/2012-ir-radar-gps/hexapodV3/SerD.h
new file mode 100644
index 0000000..dbe9348
--- /dev/null
+++ b/2011-firefly-mega-ssc32/firmware/2012-ir-radar-gps/hexapodV3/SerD.h
@@ -0,0 +1,39 @@
+//SerD.h
+#ifndef SERD
+#define SERD
+
+class SerD
+{
+public:
+
+  //Coordinate system
+  int x;
+  int y;
+  int z;
+
+  //Angles
+  int o1;
+  int o2;
+  int o3;
+
+  //Timings
+  int t1;
+  int t2;
+  int t3;
+
+  //Input
+  SerD & operator() (int X, int Y, int Z)
+  {
+    x = X;
+    y = Y;
+    z = Z;
+
+    return *this;
+  }
+
+};
+
+#endif
+
+
+
diff --git a/2011-firefly-mega-ssc32/firmware/2012-ir-radar-gps/hexapodV3/Servos.cpp b/2011-firefly-mega-ssc32/firmware/2012-ir-radar-gps/hexapodV3/Servos.cpp
new file mode 100644
index 0000000..3f85f2d
--- /dev/null
+++ b/2011-firefly-mega-ssc32/firmware/2012-ir-radar-gps/hexapodV3/Servos.cpp
@@ -0,0 +1,112 @@
+// Servos.cpp
+
+#include "Servos.h"
+
+void Servos::MoveHead (Data *DataLine)
+{  
+#ifdef DEBUGHS
+  Serial.print("### Servos->Head Time/Position: ");
+  Serial.print(DataLine->HeadTime);
+  Serial.print(", ");
+  Serial.println(DataLine->HeadPosition);
+#endif
+
+  Serial1.print(" #"); 
+  Serial1.print(9); 
+  Serial1.print(" P");
+  Serial1.print(DataLine->HeadPosition);
+  Serial1.print(" T");
+  Serial1.println(DataLine->HeadTime);
+}
+
+void mSend(Data *DataLine)
+{
+  int s=3;
+
+  for (int i=0; i<9; i += 3){
+    Serial1.print(" #"); 
+    Serial1.print(i);
+    Serial1.print(" P");
+    Serial1.print(DataLine->ServoData[s].t1);
+    Serial1.print(" #"); 
+    Serial1.print(i+1);
+    Serial1.print(" P");
+    Serial1.print(DataLine->ServoData[s].t2);
+    Serial1.print(" #"); 
+    Serial1.print(i+2);
+    Serial1.print(" P");
+    Serial1.print(DataLine->ServoData[s].t3);
+    s++;
+  }
+
+  s = 0;
+
+  for (int i=16; i<25; i += 3){
+    Serial1.print(" #"); 
+    Serial1.print(i);
+    Serial1.print(" P");
+    Serial1.print(DataLine->ServoData[s].t1);
+    Serial1.print(" #"); 
+    Serial1.print(i+1);
+    Serial1.print(" P");
+    Serial1.print(DataLine->ServoData[s].t2);
+    Serial1.print(" #"); 
+    Serial1.print(i+2);
+    Serial1.print(" P");
+    Serial1.print(DataLine->ServoData[s].t3);
+    s++;
+  }
+
+  Serial1.print(" T");
+  Serial1.println(DataLine->WalkTime);
+
+#ifdef DEBUGC
+  for (int i=0; i<6; i++)
+  {
+    if (i != 0)
+      Serial.print(", ");
+    else
+    {
+      Serial.print("### Converter->ServosAnglesTime Are: ");
+    }
+
+    Serial.print(DataLine->ServoData[i].t1);
+    Serial.print(", ");
+    Serial.print(DataLine->ServoData[i].t2);
+    Serial.print(", ");
+    Serial.print(DataLine->ServoData[i].t3);
+  }
+  Serial.println(";");
+#endif
+}
+
+void Servos::Send(Data *DataLine)
+{
+  if (DataLine->Send)
+  {
+#ifdef DEBUGS
+    Serial.println("### Servos->Servos ###");
+    Serial.println("### Servos Send ###");
+    digitalWrite(13, HIGH);
+    delay(10);
+    digitalWrite(13, LOW);
+#endif
+
+#ifndef Freeze
+    mSend(DataLine);
+#endif
+
+    DataLine->Send = false;	
+    DataLine->ReadyToSend = false;
+  }
+}
+
+
+
+
+
+
+
+
+
+
diff --git a/2011-firefly-mega-ssc32/firmware/2012-ir-radar-gps/hexapodV3/Servos.h b/2011-firefly-mega-ssc32/firmware/2012-ir-radar-gps/hexapodV3/Servos.h
new file mode 100644
index 0000000..6410de4
--- /dev/null
+++ b/2011-firefly-mega-ssc32/firmware/2012-ir-radar-gps/hexapodV3/Servos.h
@@ -0,0 +1,26 @@
+// Servos.h
+
+#ifndef SERVOS_H
+#define SERVOS_H
+
+#include "Arduino.h"
+#include "Data.h"
+
+class Servos 
+{
+public:
+  void Send(Data *DataLine);
+  void MoveHead (Data *DataLine);
+
+};
+
+
+#endif
+
+
+
+
+
+
+
+
diff --git a/2011-firefly-mega-ssc32/firmware/2012-ir-radar-gps/hexapodV3/Ultrasone.cpp b/2011-firefly-mega-ssc32/firmware/2012-ir-radar-gps/hexapodV3/Ultrasone.cpp
new file mode 100644
index 0000000..4315666
--- /dev/null
+++ b/2011-firefly-mega-ssc32/firmware/2012-ir-radar-gps/hexapodV3/Ultrasone.cpp
@@ -0,0 +1,55 @@
+//Ultrasone.cpp
+
+#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;
+  }
+}
+
+
+
+
+
+
+
diff --git a/2011-firefly-mega-ssc32/firmware/2012-ir-radar-gps/hexapodV3/Ultrasone.h b/2011-firefly-mega-ssc32/firmware/2012-ir-radar-gps/hexapodV3/Ultrasone.h
new file mode 100644
index 0000000..0a921ae
--- /dev/null
+++ b/2011-firefly-mega-ssc32/firmware/2012-ir-radar-gps/hexapodV3/Ultrasone.h
@@ -0,0 +1,42 @@
+//Ultrasone.h
+
+#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
+
+
+
+
+
+
+
+
diff --git a/2011-firefly-mega-ssc32/firmware/2012-ir-radar-gps/hexapodV3/Watchdog.cpp b/2011-firefly-mega-ssc32/firmware/2012-ir-radar-gps/hexapodV3/Watchdog.cpp
new file mode 100644
index 0000000..42e53e7
--- /dev/null
+++ b/2011-firefly-mega-ssc32/firmware/2012-ir-radar-gps/hexapodV3/Watchdog.cpp
@@ -0,0 +1,39 @@
+// 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
+  }
+}
+
+
+
+
diff --git a/2011-firefly-mega-ssc32/firmware/2012-ir-radar-gps/hexapodV3/Watchdog.h b/2011-firefly-mega-ssc32/firmware/2012-ir-radar-gps/hexapodV3/Watchdog.h
new file mode 100644
index 0000000..f509b18
--- /dev/null
+++ b/2011-firefly-mega-ssc32/firmware/2012-ir-radar-gps/hexapodV3/Watchdog.h
@@ -0,0 +1,24 @@
+// Watchdog.h
+
+#ifndef WATCHDOG_H
+#define WATCHDOG_H
+
+#include "Arduino.h"
+#include "Data.h"
+
+class Watchdog 
+{
+public:
+  void CheckTime(Data *DataLine);
+
+};
+
+
+#endif
+
+
+
+
+
+
+
diff --git a/2011-firefly-mega-ssc32/firmware/2012-ir-radar-gps/hexapodV3/hexapodV3.ino b/2011-firefly-mega-ssc32/firmware/2012-ir-radar-gps/hexapodV3/hexapodV3.ino
new file mode 100644
index 0000000..cba286d
--- /dev/null
+++ b/2011-firefly-mega-ssc32/firmware/2012-ir-radar-gps/hexapodV3/hexapodV3.ino
@@ -0,0 +1,212 @@
+// Hexapod FireFly
+
+#include "hexapod.h"
+#include "Data.h"
+#include "Converter.h"
+#include "InverseKinemetics.h"
+#include "Servos.h"
+#include "Watchdog.h"
+#include "Ultrasone.h"
+
+Data *DataLine = new Data;
+Converter Con;
+InverseKinemetics IK;
+Servos SSC32;
+Watchdog WD;
+Ultrasone US;
+
+void setup()
+{
+  Serial.begin(38400);
+  Serial1.begin(38400);
+  pinMode(13, OUTPUT);  
+  DataLine->WalkTime = 100;
+  DataLine->FireFly.FireFly();
+}
+
+int a=0;
+int b=6;
+int tt;
+byte Sit;
+byte SitC;
+
+// Main loop.
+void loop()
+{
+  int d = DataLine->FireFly.GetNewCmd();
+
+  US.GetRadar(DataLine);
+
+  /*if(Sit == 1)
+   if (SitC <= millis())
+   {
+   Sit = 0;
+   DataLine->WalkTime = tt;
+   } */
+
+  if (d != 0)
+  {
+    switch(d)
+    {
+    case power:
+      DataLine->ServoData[0](5, 3, 8);
+      DataLine->ServoData[1](8, 3, 0);
+      DataLine->ServoData[2](5, 3, -8);
+      DataLine->ServoData[3](5, 3, 8);
+      DataLine->ServoData[4](8, 3, 0);
+      DataLine->ServoData[5](5, 3, -8); 
+      break;
+    case up:  
+      Serial.println("UP");
+      for (int i=a; i<=b; i++)
+        DataLine->ServoData[i].y -= 1;
+      break;
+    case down:  
+      Serial.println("DOWN");
+      for (int i=a; i<=b; i++)
+        DataLine->ServoData[i].y += 1;
+      break;
+    case left:  
+      Serial.println("LEFT");
+      for (int i=a; i<=b; i++)
+      {
+        if (i<3)
+          DataLine->ServoData[i].x += 1;
+        else
+          DataLine->ServoData[i].x -= 1;
+      }
+      break;
+    case right:  
+      Serial.println("RIGHT");
+      for (int i=a; i<=b; i++)
+      {
+        if (i<3)
+          DataLine->ServoData[i].x -= 1;
+        else
+          DataLine->ServoData[i].x += 1;
+      }
+      break;
+    case forward:  
+      Serial.println("forward");
+      for (int i=a; i<=b; i++)
+        DataLine->ServoData[i].z -= 1;
+      break;
+    case back:  
+      Serial.println("back");
+      for (int i=a; i<=b; i++)
+        DataLine->ServoData[i].z += 1;
+      break;
+    case ok:  
+      Serial.println("OK");
+
+      tt = DataLine->WalkTime;
+      DataLine->WalkTime = 3000;
+
+      DataLine->ServoData[0](5, 3, 8);
+      DataLine->ServoData[1](8, 3, 0);
+      DataLine->ServoData[2](5, 3, -8);
+      DataLine->ServoData[3](5, 3, 8);
+      DataLine->ServoData[4](8, 3, 0);
+      DataLine->ServoData[5](5, 3, -8);
+
+      IK.IK(DataLine);
+      Con.Convert(DataLine);
+      DataLine->Send = 1;
+      SSC32.Send(DataLine);
+
+      Sit = 1;
+      break;
+    case IR_0:  
+      Serial.println("0");
+      a = 0;
+      b = 5;
+      break;
+    case IR_1:  
+      Serial.println("1");
+      a = 0;
+      b = 0;
+      break;
+      /*case IR_2:  
+       Serial.println("OK");
+       a = ;
+       b = ;
+       break;*/
+    case IR_3:  
+      Serial.println("3");
+      a = 3;
+      b = 3;
+      break;
+    case IR_4:  
+      Serial.println("4");
+      a = 1;
+      b = 1;
+      break;
+      /*case IR_5:  
+       Serial.println("OK");
+       a = ;
+       b = ;
+       break;*/
+    case IR_6:  
+      Serial.println("6");
+      a = 4;
+      b = 4;
+      break;
+    case IR_7:  
+      Serial.println("7");
+      a = 2;
+      b = 2;
+      break;
+      /*case IR_8:  
+       Serial.println("OK");
+       a = ;
+       b = ;
+       break;*/
+    case IR_9:  
+      Serial.println("9");
+      a = 5;
+      b = 5;
+      break;
+    default:  
+      Serial.println(d);
+      break;
+    }
+  }
+  IK.IK(DataLine);
+  Con.Convert(DataLine);
+  WD.CheckTime(DataLine);
+
+  if (Sit){
+    if ((DataLine->Send) & (SitC))
+    {
+      DataLine->WalkTime = tt;
+      SitC = 0;
+      Sit = 0;
+    }
+    else if (DataLine->Send)
+      SitC = 1;
+  }
+
+  SSC32.Send(DataLine);
+}
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+