diff --git a/DirectionalSensor.cpp b/DirectionalSensor.cpp
index fa1a974..cc988c8 100644
--- a/DirectionalSensor.cpp
+++ b/DirectionalSensor.cpp
@@ -2,7 +2,7 @@
 
 #include "Messages.h"
 
-DirectionalSensor::DirectionalSensor() {
+DirectionalSensor::DirectionalSensor() : Sensor() {
   this->type = DirectionalSensor::Type;
   this->vector = Spherical16::zero;
 }
diff --git a/DistanceSensor.cpp b/DistanceSensor.cpp
index 2a7aa60..2aa06f9 100644
--- a/DistanceSensor.cpp
+++ b/DistanceSensor.cpp
@@ -15,10 +15,12 @@ DistanceSensor::DistanceSensor(float triggerDistance) : DistanceSensor() {
 }
 
 float DistanceSensor::GetDistance() {
-  if (distance < minRange || distance > maxRange)
-    return -1; // invalid distance
+  if (this->distance < minRange || this->distance > maxRange)
+    return INFINITY; // invalid distance
 
-  return distance;
+  float d = this->distance;
+  this->distance = INFINITY;
+  return d;
 };
 
 bool DistanceSensor::ObjectNearby() {
diff --git a/Messages.cpp b/Messages.cpp
index 93f3f23..1c6f44d 100644
--- a/Messages.cpp
+++ b/Messages.cpp
@@ -14,7 +14,7 @@ Angle8 Messages::ReceiveAngle8(unsigned char *buffer,
 float Messages::ReceiveFloat16(unsigned char *buffer,
                                unsigned char *startIndex) {
   unsigned char ix = *startIndex;
-  unsigned short value = buffer[ix++] << 8 | buffer[ix];
+  unsigned short value = buffer[ix++] << 8 | buffer[ix++];
   float16 f = float16();
   f.setBinary(value);
 
@@ -27,10 +27,10 @@ Spherical16 Messages::ReceiveSpherical16(unsigned char *buffer,
   float distance = ReceiveFloat16(buffer, startIndex);
 
   Angle8 horizontal8 = ReceiveAngle8(buffer, startIndex);
-  Angle16 horizontal = Angle16::Binary(horizontal8.GetBinary() * 255);
+  Angle16 horizontal = Angle16::Binary(horizontal8.GetBinary() * 256);
 
   Angle8 vertical8 = ReceiveAngle8(buffer, startIndex);
-  Angle16 vertical = Angle16::Binary(vertical8.GetBinary() * 255);
+  Angle16 vertical = Angle16::Binary(vertical8.GetBinary() * 256);
 
   Spherical16 s = Spherical16(distance, horizontal, vertical);
   return s;
@@ -63,6 +63,7 @@ void Messages::SendPolar(unsigned char *buffer, unsigned char *startIndex,
 
 void Messages::SendSpherical16(unsigned char *buffer, unsigned char *startIndex,
                                Spherical16 s) {
+  // Receive first does distance...
   SendAngle8(buffer, (*startIndex)++, s.direction.horizontal.InDegrees());
   SendAngle8(buffer, (*startIndex)++, s.direction.vertical.InDegrees());
   SendFloat16(buffer, startIndex, s.distance);
diff --git a/NetworkPerception.cpp b/NetworkPerception.cpp
index bbe3fe1..439f663 100644
--- a/NetworkPerception.cpp
+++ b/NetworkPerception.cpp
@@ -16,8 +16,8 @@ void NetworkPerception::ProcessPacket(Roboid *roboid, unsigned char *buffer,
 #endif
 
   switch (buffer[0]) {
-  case NetworkSync::CreateMsg:
-    ReceiveCreateMsg(buffer, roboid);
+  case NetworkSync::ThingMsg:
+    ReceiveThingMsg(buffer, roboid);
     break;
   case NetworkSync::InvestigateMsg:
     ReceiveInvestigateMsg(buffer, roboid);
@@ -31,7 +31,7 @@ void NetworkPerception::ProcessPacket(Roboid *roboid, unsigned char *buffer,
   }
 }
 
-void NetworkPerception::ReceiveCreateMsg(unsigned char *data, Roboid *roboid) {
+void NetworkPerception::ReceiveThingMsg(unsigned char *data, Roboid *roboid) {
   unsigned char networkId = data[1];
   unsigned char objectId = data[2];
   unsigned char objectType = data[3];
@@ -50,30 +50,33 @@ void NetworkPerception::ReceiveCreateMsg(unsigned char *data, Roboid *roboid) {
 void NetworkPerception::ReceiveInvestigateMsg(unsigned char *data,
                                               Roboid *roboid) {
   unsigned char networkId = data[1];
-  unsigned char objectId = data[2];
+  unsigned char thingId = data[2];
 
-#if RC_DEBUG
-  printf("Received InvestigateMsg [%d/%d]\n", networkId, objectId);
-#endif
+  // #if RC_DEBUG
+  printf("Received InvestigateMsg [%d/%d]\n", networkId, thingId);
+  // #endif
 
   if (networkId != roboid->networkSync->networkId)
     // We only response to investigation requests for our own objects
     return;
 
-  if (objectId == 0x00) {
+  if (thingId == 0x00) {
     // They are investigating the roboid itself!
     if (roboid->modelUrl != nullptr)
-      roboid->networkSync->SendModel(roboid);
+      // roboid->networkSync->SendModel(roboid);
+      roboid->networkSync->SendThingInfo(roboid);
   } else {
-    InterestingThing *thing =
-        roboid->perception->FindTrackedObject(0x00, objectId);
-    if (thing == nullptr)
-      return;
+    Thing *thing = roboid->FindChild(thingId, true);
+    if (thing != nullptr)
+      roboid->networkSync->SendThingInfo(thing);
+    else {
+      InterestingThing *interestingThing =
+          roboid->perception->FindTrackedObject(0x00, thingId);
+      if (interestingThing == nullptr)
+        return;
 
-    // roboid->networkSync->NewObject(thing);
-    // roboid->networkSync->SendThing(thing);
-    // roboid->networkSync->SendModel(thing);
-    roboid->networkSync->SendThingInfo(thing, false);
+      roboid->networkSync->SendThingInfo(interestingThing, false);
+    }
   }
 }
 
@@ -117,7 +120,8 @@ void NetworkPerception::ReceivePlane(unsigned char *data, Roboid *roboid) {
     originPosition = roboid->worldOrigin->position;
   }
   Spherical16 position = originPosition + originOrientation * worldPosition;
-  // printf("Received plane (%f %f %f) (%f %f %f) %f %f %f\n", worldPosition.x,
+  // printf("Received plane (%f %f %f) (%f %f %f) %f %f %f\n",
+  // worldPosition.x,
   //        worldPosition.y, worldPosition.z, roboidPosition.x,
   //        roboidPosition.y, roboidPosition.z, position.distance,
   //        (float)position.horizontalAngle, (float)position.verticalAngle);
diff --git a/NetworkPerception.h b/NetworkPerception.h
index b84b254..68f975f 100644
--- a/NetworkPerception.h
+++ b/NetworkPerception.h
@@ -12,7 +12,7 @@ public:
   void ProcessPacket(Roboid *roboid, unsigned char *buffer, int packetsize);
 
 protected:
-  void ReceiveCreateMsg(unsigned char *data, Roboid *roboid);
+  void ReceiveThingMsg(unsigned char *data, Roboid *roboid);
   void ReceiveInvestigateMsg(unsigned char *data, Roboid *roboid);
   void ReceivePoseMsg(unsigned char *data, Roboid *roboid);
   void ReceiveTypedObject(unsigned char *data, Roboid *roboid);
diff --git a/NetworkSync.cpp b/NetworkSync.cpp
index 111d36e..8997353 100644
--- a/NetworkSync.cpp
+++ b/NetworkSync.cpp
@@ -24,50 +24,26 @@ NetworkSync::NetworkSync(Roboid *roboid) {
   this->networkId = 0;
 }
 
+#include <Arduino.h>
+
 void NetworkSync::ReceiveMessage(Roboid *roboid, unsigned char bytecount) {
+  // printf("Received msgId %d, length %d\n", buffer[0], bytecount);
+
   networkPerception->ProcessPacket(roboid, buffer, bytecount);
 
   switch (buffer[0]) {
   case NetworkIdMsg:
     ReceiveNetworkId();
     break;
-  case BytesMsg:
-    ReceiveBytes();
+  case CustomMsg:
+    ReceiveCustom(bytecount);
     break;
   }
 }
 
-// Actually, this is a kind of investigate...
-void NetworkSync::ReceiveNetworkId() {
-  this->networkId = buffer[1];
-#ifdef RC_DEBUG
-  SERIALPORT.printf("Received network Id %d\n", this->networkId);
-#endif
-  SendThingInfo(roboid, true);
-}
-
-void NetworkSync::ReceiveBytes() {
-  unsigned char ix = 1; // first byte is the msgId
-  unsigned char networkId = buffer[ix++];
-  unsigned char thingId = buffer[ix++];
-  unsigned char len = buffer[ix++];
-  unsigned char *bytes = new unsigned char[len];
-  // printf("Received %d bytes for [%d/%d]\n", len, networkId, thingId);
-  for (unsigned char bytesIx = 0; bytesIx < len; bytesIx++)
-    bytes[bytesIx] = buffer[ix++];
-
-  // networkId is not used right now, we assume networkId == 0
-  Thing *thing = roboid->FindChild(thingId);
-  // printf("Found thing %d\n", thing);
-  if (thing != nullptr)
-    thing->ProcessBytes(bytes);
-
-  delete bytes;
-}
-
 void NetworkSync::PublishClient() {
   unsigned char ix = 0;
-  buffer[ix++] = DeviceMsg;
+  buffer[ix++] = ClientMsg;
   buffer[ix++] = this->networkId;
   PublishBuffer(ix);
 
@@ -76,6 +52,49 @@ void NetworkSync::PublishClient() {
 #endif
 }
 
+void NetworkSync::ReceiveNetworkId() {
+  this->networkId = buffer[1];
+#ifdef RC_DEBUG
+  SERIALPORT.printf("Received network Id %d\n", this->networkId);
+#endif
+  SendThingInfo(roboid, true);
+  printf("completed\n");
+}
+
+void NetworkSync::SendInvestigate(InterestingThing *thing) {
+  unsigned char ix = 0;
+  buffer[ix++] = InvestigateMsg;
+  buffer[ix++] = thing->networkId;
+  buffer[ix++] = thing->id;
+  SendBuffer(ix);
+#ifdef RC_DEBUG
+  printf("Sent Investigate [%d/%d]\n", thing->networkId, thing->id);
+#endif
+}
+
+void NetworkSync::ReceiveCustom(unsigned char packetSize) {
+  unsigned char ix = 1; // first byte is the msgId
+  unsigned char networkId = buffer[ix++];
+  unsigned char thingId = buffer[ix++];
+  // unsigned char len = buffer[ix++];
+  unsigned char len = packetSize - 3;
+  printf("Custom size = %d\n", len);
+  if (len > 0) {
+    unsigned char *bytes = new unsigned char[len];
+    // printf("Received %d bytes for [%d/%d]\n", len, networkId, thingId);
+    for (unsigned char bytesIx = 0; bytesIx < len; bytesIx++)
+      bytes[bytesIx] = buffer[ix++];
+
+    // networkId is not used right now, we assume networkId == 0
+    Thing *thing = roboid->FindChild(thingId);
+    printf("Found thing %s\n", thing->name);
+    if (thing != nullptr)
+      thing->ProcessBytes(bytes);
+
+    delete bytes;
+  }
+}
+
 void NetworkSync::PublishState(Roboid *roboid) {
   SendPose(roboid);
   PublishPerception(roboid);
@@ -85,6 +104,8 @@ void NetworkSync::SendThingInfo(Thing *thing, bool recurse) {
   SendThing(thing);
   SendName(thing);
   SendModel(thing);
+  SendCustom(thing);
+  SendPose(thing, true, false);
 
   if (recurse) {
     for (unsigned char childIx = 0; childIx < thing->childCount; childIx++) {
@@ -100,9 +121,10 @@ void NetworkSync::SendThing(Thing *thing) {
     return;
 
   unsigned char ix = 0;
-  buffer[ix++] = CreateMsg;
+  buffer[ix++] = ThingMsg;
+  buffer[ix++] = this->networkId;
   buffer[ix++] = thing->id;
-  buffer[ix++] = thing->type;
+  buffer[ix++] = (unsigned char)thing->type;
   Thing *parentThing = thing->GetParent();
   if (parentThing != nullptr)
     buffer[ix++] = parentThing->id;
@@ -111,7 +133,7 @@ void NetworkSync::SendThing(Thing *thing) {
   SendBuffer(ix);
 
 #ifdef RC_DEBUG
-  printf("Sent Thing [%d/%d] %d\n", networkId, thing->id, thing->type);
+  printf("Sent Thing [%d/%d] %d\n", networkId, thing->id, (byte)thing->type);
 #endif
 }
 
@@ -125,6 +147,7 @@ void NetworkSync::SendName(Thing *thing) {
 
   unsigned char ix = 0;
   buffer[ix++] = NameMsg;
+  buffer[ix++] = this->networkId;
   buffer[ix++] = thing->id;
 
   buffer[ix++] = len;
@@ -149,9 +172,10 @@ void NetworkSync::SendModel(Thing *thing) {
 
   unsigned char ix = 0;
   buffer[ix++] = ModelMsg;
-  buffer[ix++] = thing->id;          // objectId
-  Spherical16 s = Spherical16::zero; // Spherical(thing->modelPosition);
-  Messages::SendSpherical16(buffer, &ix, s);
+  buffer[ix++] = this->networkId;
+  buffer[ix++] = thing->id; // objectId
+  // Spherical16 s = Spherical16::zero; // Spherical(thing->modelPosition);
+  // Messages::SendSpherical16(buffer, &ix, s);
   Messages::SendFloat16(buffer, &ix, thing->modelScale);
 
   buffer[ix++] = len;
@@ -161,12 +185,24 @@ void NetworkSync::SendModel(Thing *thing) {
   SendBuffer(ix);
 }
 
+void NetworkSync::SendCustom(Thing *thing) {
+  unsigned char ix = 0;
+  buffer[ix++] = NetworkSync::CustomMsg;
+  buffer[ix++] = this->networkId;
+  buffer[ix++] = thing->id;
+  thing->SendBytes(buffer, &ix);
+
+  if (ix > 3) // When ix <= 3 then there is no custom data
+    SendBuffer(ix);
+}
+
 void NetworkSync::SendDestroyThing(InterestingThing *thing) {
   if (networkId == 0) // We're not connected to a site yet
     return;
 
   unsigned char ix = 0;
   buffer[ix++] = DestroyMsg;
+  buffer[ix++] = this->networkId;
   buffer[ix++] = thing->id;
   SendBuffer(ix);
 
@@ -175,20 +211,21 @@ void NetworkSync::SendDestroyThing(InterestingThing *thing) {
 #endif
 }
 
-void NetworkSync::SendPose(Thing *thing, bool recurse) {
+void NetworkSync::SendPose(Thing *thing, bool force, bool recurse) {
   if (this->networkId == 0) // We're not connected to a site yet
     return;
 
   thing->positionUpdated |= thing->GetLinearVelocity().distance > 0;
   thing->orientationUpdated |= thing->GetAngularVelocity().distance > 0;
-  if (thing->positionUpdated || thing->orientationUpdated) {
+  if (force || thing->positionUpdated || thing->orientationUpdated) {
     unsigned char ix = 0;
     buffer[ix++] = PoseMsg;
+    buffer[ix++] = this->networkId;
     buffer[ix++] = thing->id;
     unsigned char pattern = 0;
-    if (thing->positionUpdated)
+    if (force || thing->positionUpdated)
       pattern |= Pose_Position;
-    if (thing->orientationUpdated)
+    if (force || thing->orientationUpdated)
       pattern |= Pose_Orientation;
     buffer[ix++] = pattern;
     Messages::SendSpherical16(buffer, &ix, thing->position);
@@ -214,7 +251,7 @@ void NetworkSync::SendPose(Thing *thing, bool recurse) {
     for (unsigned char childIx = 0; childIx < thing->childCount; childIx++) {
       Thing *child = thing->GetChild(childIx);
       if (child != nullptr)
-        SendPose(thing->GetChild(childIx), true);
+        SendPose(thing->GetChild(childIx), force, recurse);
     }
   }
 }
@@ -299,7 +336,8 @@ void NetworkSync::PublishTrackedObject(Roboid *roboid,
       inv_originOrientation * (thing->position - originPosition);
 
   unsigned char ix = 0;
-  buffer[ix++] = PoseMsg;   // Position2DMsg;
+  buffer[ix++] = PoseMsg; // Position2DMsg;
+  buffer[ix++] = this->networkId;
   buffer[ix++] = thing->id; // objectId;
   buffer[ix++] = Pose_Position | Pose_Orientation;
   Messages::SendSpherical16(buffer, &ix, worldPosition);
@@ -313,17 +351,6 @@ void NetworkSync::PublishTrackedObject(Roboid *roboid,
   thing->updated = false;
 }
 
-void NetworkSync::SendInvestigate(InterestingThing *thing) {
-  unsigned char ix = 0;
-  buffer[ix++] = InvestigateMsg;
-  buffer[ix++] = thing->networkId;
-  buffer[ix++] = thing->id;
-  SendBuffer(ix);
-#ifdef RC_DEBUG
-  printf("Sent Investigate [%d/%d]\n", thing->networkId, thing->id);
-#endif
-}
-
 void NetworkSync::SendText(const char *s) {
   unsigned char length;
   for (length = 0; length < 253; length++) {
diff --git a/NetworkSync.h b/NetworkSync.h
index 573e590..04e6fad 100644
--- a/NetworkSync.h
+++ b/NetworkSync.h
@@ -27,6 +27,7 @@ public:
   void SendThing(Thing *thing);
   void SendName(Thing *roboid);
   void SendModel(Thing *thing);
+  void SendCustom(Thing *thing);
 
   /// @brief The id of a Pose message
   static const unsigned char PoseMsg = 0x10;
@@ -51,13 +52,13 @@ public:
   static const unsigned char AngVelocity2DMsg = 0x46;
   static const unsigned char Position2DMsg = 0x61;
   static const unsigned char Velocity2DMsg = 0x62;
-  static const unsigned char CreateMsg = 0x80;
+  static const unsigned char ThingMsg = 0x80;
   static const unsigned char InvestigateMsg = 0x81;
   static const unsigned char ModelMsg = 0x90;
   static const unsigned char NameMsg = 0x91;
-  static const unsigned char DeviceMsg = 0xA0;
+  static const unsigned char ClientMsg = 0xA0;
   static const unsigned char NetworkIdMsg = 0xA1;
-  static const unsigned char BytesMsg = 0xB1;
+  static const unsigned char CustomMsg = 0xB1;
 
   typedef void (*Buffer)(UInt8 *buffer, UInt16 bufferSize);
 
@@ -74,7 +75,7 @@ public:
   virtual void SendPose(Spherical16 worldPosition,
                         SwingTwist16 worldOrientation) {};
   // void SendPose(Roboid* roboid, bool recurse = true);
-  void SendPose(Thing *thing, bool recurse = true);
+  void SendPose(Thing *thing, bool force = 0, bool recurse = true);
 
   virtual void SendText(const char *s);
   void SendInt(const int x);
@@ -86,7 +87,7 @@ protected:
   NetworkPerception *networkPerception;
 
   void ReceiveNetworkId();
-  void ReceiveBytes();
+  void ReceiveCustom(unsigned char packetSize);
 
   void PublishState(Sensor *sensor);
 
diff --git a/Roboid.cpp b/Roboid.cpp
index 493fc7c..4212e22 100644
--- a/Roboid.cpp
+++ b/Roboid.cpp
@@ -22,6 +22,7 @@ Roboid::Roboid() : Thing(0) {
   this->networkSync = nullptr;
   this->position = Spherical16::zero;
   this->orientation = SwingTwist16::identity;
+  this->lastUpdateTimeMs = 0;
 }
 
 Roboid::Roboid(Propulsion *propulsion) : Roboid() {
@@ -31,8 +32,11 @@ Roboid::Roboid(Propulsion *propulsion) : Roboid() {
 }
 
 void Roboid::Update(unsigned long currentTimeMs) {
-  if (perception != nullptr)
-    perception->Update(currentTimeMs);
+  if (this->lastUpdateTimeMs == 0)
+    this->lastUpdateTimeMs = currentTimeMs;
+
+  // if (perception != nullptr)
+  //   perception->Update(currentTimeMs);
 
   if (propulsion != nullptr) {
     propulsion->Update(currentTimeMs);
diff --git a/Sensor.cpp b/Sensor.cpp
index ff61c27..f44e411 100644
--- a/Sensor.cpp
+++ b/Sensor.cpp
@@ -2,7 +2,7 @@
 
 #include "Roboid.h"
 
-Sensor::Sensor() : Thing(0) { // for now, id should be set properly later
+Sensor::Sensor() : Thing() { // for now, id should be set properly later
   this->type = Thing::SensorType;
 }
 
diff --git a/Thing.cpp b/Thing.cpp
index 37a2766..c5b646c 100644
--- a/Thing.cpp
+++ b/Thing.cpp
@@ -4,6 +4,16 @@
 
 using namespace Passer::RoboidControl;
 
+int Thing::lastThingId = 1;
+
+Thing::Thing() {
+  this->id = lastThingId++;
+  this->type = (unsigned int)Type::Undetermined;
+  this->childCount = 0;
+  this->parent = nullptr;
+  this->children = nullptr;
+}
+
 Thing::Thing(unsigned char id) : id(id) {
   // this->position = SphericalOf<signed short>::zero;
   this->type = (unsigned int)Type::Undetermined;
diff --git a/Thing.h b/Thing.h
index 39641c6..2c8ee21 100644
--- a/Thing.h
+++ b/Thing.h
@@ -11,6 +11,7 @@ namespace RoboidControl {
 /// @brief A thing is a functional component on a robot
 class Thing {
 public:
+  Thing();
   /// @brief Default constructor for a Thing
   Thing(unsigned char id);
 
@@ -108,9 +109,11 @@ public:
   const char *modelUrl = nullptr;
   float modelScale = 1;
 
-  virtual void ProcessBytes(unsigned char *bytes) {}
+  virtual void SendBytes(unsigned char *buffer, unsigned char *ix) {};
+  virtual void ProcessBytes(unsigned char *bytes) {};
 
 protected:
+  static int lastThingId;
   /// @brief Bitmask for Motor type
   static const unsigned int MotorType = 0x8000;
   /// @brief Bitmap for Sensor type