diff --git a/2011-firefly-mega-ssc32/firmware/2012-behaviours-wip/FireFly/Data.h b/2011-firefly-mega-ssc32/firmware/2012-behaviours-wip/FireFly/Data.h
deleted file mode 100644
index 9490e45..0000000
--- a/2011-firefly-mega-ssc32/firmware/2012-behaviours-wip/FireFly/Data.h
+++ /dev/null
@@ -1,71 +0,0 @@
-//Data.h Data Structure
-
-#ifndef DATA
-#define DATA
-
-#include "Arduino.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;
-
-  //Init
-  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);
-  }
-
-  Data& operator() (DataObject *R)
-	{
-		ServoData[0] = R.s0;
-		ServoData[1] = R.s1;
-		ServoData[2] = R.s2;
-		ServoData[3] = R.s3;
-		ServoData[4] = R.s4;
-		ServoData[5] = R.s5;
-	}
-
-};
-
-#endif
-
-
-
diff --git a/2011-firefly-mega-ssc32/firmware/2012-behaviours-wip/FireFly/Def.h b/2011-firefly-mega-ssc32/firmware/2012-behaviours-wip/FireFly/Def.h
deleted file mode 100644
index a58ff4f..0000000
--- a/2011-firefly-mega-ssc32/firmware/2012-behaviours-wip/FireFly/Def.h
+++ /dev/null
@@ -1,25 +0,0 @@
-// Def.h
-// Define some Hard Coded Variables
-//  V3.0
-
-//DEBUG CODES
-//#define DEBUGC  // Converter
-//#define DEBUGIK // InverseKinemetics
-//#define DEBUGS  // Servos
-//#define DEBUGHS // ServosHead
-//#define DEBUGW  // Watchdog
-//#define Freeze  // Protect all the servos
-
-//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-behaviours-wip/FireFly/FireFly.ino b/2011-firefly-mega-ssc32/firmware/2012-behaviours-wip/FireFly/FireFly.ino
index bd6dd26..3de5584 100644
--- a/2011-firefly-mega-ssc32/firmware/2012-behaviours-wip/FireFly/FireFly.ino
+++ b/2011-firefly-mega-ssc32/firmware/2012-behaviours-wip/FireFly/FireFly.ino
@@ -1,196 +1,31 @@
-// Hexapod FireFly
-//  V3.0
-
-#include "hexapod.h"
-#include "Data.h"
-#include "hexapod.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;
-hexapod FireFly;
-
-void setup()
-{
-  Serial.begin(38400);
-  Serial1.begin(38400);
-  pinMode(13, OUTPUT);  
-  //DataLine->WalkTime = 100;
-  FireFly.FireFly();
-}
-
-int a=0;
-int b=6;
-int tt;
-byte Sit;
-byte SitC;
-
-// Main loop.
-void loop()
-{
-  int d = 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);
-}
-
-
+// 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);
+}
 
diff --git a/2011-firefly-mega-ssc32/firmware/2012-behaviours-wip/FireFly/Gedrags_Classen.cpp b/2011-firefly-mega-ssc32/firmware/2012-behaviours-wip/FireFly/Gedrags_Classen.cpp
index 0656b03..b5abafb 100644
--- a/2011-firefly-mega-ssc32/firmware/2012-behaviours-wip/FireFly/Gedrags_Classen.cpp
+++ b/2011-firefly-mega-ssc32/firmware/2012-behaviours-wip/FireFly/Gedrags_Classen.cpp
@@ -1,88 +1,55 @@
-//Gedrags classen,
-
-#ifndef GCC
-#define GCC
-
-#include "SerD.h"
-
-//    LAYOUT
-//      _U_
-//  0==/   \==3
-//     >***<
-// 1===|---|===4
-//     >***<
-//  2==\___/==5
-
-class DataObject
-{
-public:
-
-	SerD S0;
-	SerD S1;
-	SerD S2;
-	SerD S3;
-	SerD S4;
-	SerD S5;
-
-	DataObject& operator() (SerD s0, SerD s1, SerD s2, SerD s3, SerD s4, SerD s5)
-	{
-		S0 = s0;
-		S1 = s1;
-		S2 = s2;
-		S3 = s3;
-		S4 = s4;
-		S5 = s5;
-	}
-
-	DataObject& operator() (	int a1, int a2, int a3,
-		int b1, int b2, int b3,
-		int c1, int c2, int c3,
-		int d1, int d2, int d3,
-		int e1, int e2, int e3,
-		int f1, int f2, int f3)
-	{
-		S0(a1, a2, a3);
-		S1(b1, b2, b3);
-		S2(c1, c2, c3);
-		S3(d1, d2, d3);
-		S4(e1, e2, e3);
-		S5(f1, f2, f3);
-	}
-};
-
-class WalkForward	//Direction
-{
-
-private:
-
-	DataObject LoopPatroon[8];
-	DataObject HuidigePositie;
-
-	//Startup VARIABLES
-	void init()
-	{
-		/*LoopPatroon.S0[0][0](, , );
-		LoopPatroon.S1[0][1](, , );
-		LoopPatroon.S3[0][2](, , );
-		LoopPatroon.S4[0][3](, , );
-		LoopPatroon.S5[0][4](, , );
-		LoopPatroon.S6[0][5](, , );*/
-
-		LoopPatroon[0](0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0);
-
-	}
-
-
-public:
-
-	void Run()
-	{
-		//Start runnning code until exit or expection raises.
-
-	}
-
-
-};
-
-#endif
+//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
 
diff --git a/2011-firefly-mega-ssc32/firmware/2012-behaviours-wip/FireFly/SerD.h b/2011-firefly-mega-ssc32/firmware/2012-behaviours-wip/FireFly/SerD.h
deleted file mode 100644
index a4a340a..0000000
--- a/2011-firefly-mega-ssc32/firmware/2012-behaviours-wip/FireFly/SerD.h
+++ /dev/null
@@ -1,87 +0,0 @@
-// 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;
-  }
-
-  SerD& operator= (SerD& R)
-  {
-    x = R.x;
-    y = R.y;
-    z = R.z;
-
-    return *this;
-  }
-
-};
-
-class DataObject
-{
-public:
-
-	SerD S0;
-	SerD S1;
-	SerD S2;
-	SerD S3;
-	SerD S4;
-	SerD S5;
-
-	DataObject& operator() (SerD s0, SerD s1, SerD s2, SerD s3, SerD s4, SerD s5)
-	{
-		S0 = s0;
-		S1 = s1;
-		S2 = s2;
-		S3 = s3;
-		S4 = s4;
-		S5 = s5;
-	}
-
-	DataObject& operator() (	int a1, int a2, int a3,
-		int b1, int b2, int b3,
-		int c1, int c2, int c3,
-		int d1, int d2, int d3,
-		int e1, int e2, int e3,
-		int f1, int f2, int f3)
-	{
-		S0(a1, a2, a3);
-		S1(b1, b2, b3);
-		S2(c1, c2, c3);
-		S3(d1, d2, d3);
-		S4(e1, e2, e3);
-		S5(f1, f2, f3);
-	}
-};
-
-
-#endif
-
-
-
diff --git a/2011-firefly-mega-ssc32/firmware/2012-behaviours-wip/FireFly/Ultrasone.cpp b/2011-firefly-mega-ssc32/firmware/2012-behaviours-wip/FireFly/Ultrasone/Ultrasone.cpp
similarity index 100%
rename from 2011-firefly-mega-ssc32/firmware/2012-behaviours-wip/FireFly/Ultrasone.cpp
rename to 2011-firefly-mega-ssc32/firmware/2012-behaviours-wip/FireFly/Ultrasone/Ultrasone.cpp
diff --git a/2011-firefly-mega-ssc32/firmware/2012-behaviours-wip/FireFly/Ultrasone.h b/2011-firefly-mega-ssc32/firmware/2012-behaviours-wip/FireFly/Ultrasone/Ultrasone.h
similarity index 100%
rename from 2011-firefly-mega-ssc32/firmware/2012-behaviours-wip/FireFly/Ultrasone.h
rename to 2011-firefly-mega-ssc32/firmware/2012-behaviours-wip/FireFly/Ultrasone/Ultrasone.h
diff --git a/2011-firefly-mega-ssc32/firmware/2012-behaviours-wip/FireFly/Watchdog.cpp b/2011-firefly-mega-ssc32/firmware/2012-behaviours-wip/FireFly/WatchDog/Watchdog.cpp
similarity index 100%
rename from 2011-firefly-mega-ssc32/firmware/2012-behaviours-wip/FireFly/Watchdog.cpp
rename to 2011-firefly-mega-ssc32/firmware/2012-behaviours-wip/FireFly/WatchDog/Watchdog.cpp
diff --git a/2011-firefly-mega-ssc32/firmware/2012-behaviours-wip/FireFly/Watchdog.h b/2011-firefly-mega-ssc32/firmware/2012-behaviours-wip/FireFly/WatchDog/Watchdog.h
similarity index 100%
rename from 2011-firefly-mega-ssc32/firmware/2012-behaviours-wip/FireFly/Watchdog.h
rename to 2011-firefly-mega-ssc32/firmware/2012-behaviours-wip/FireFly/WatchDog/Watchdog.h