made pose sending generic

NetworkSync.cpp

@@ -4,13 +4,25 @@
 #include <Arduino.h>
 #endif
 
+UInt8 NetworkSync::CreatePoseMsg(Roboid* roboid, UInt8* buffer) {
+  Vector3 position = roboid->GetPosition();
+
+  UInt8 bufferSize = 3 + 12;
+  buffer[0] = PoseMsg;
+  buffer[1] = 0;  // ObjectId;
+  buffer[2] = Pose_Position;
+  SendVector3(buffer, 3, position);
+  return bufferSize;
+}
+
 void NetworkSync::SendVector3(unsigned char* data, int startIndex, Vector3 v) {
   SendSingle100(data, startIndex, v.x);
   SendSingle100(data, startIndex + 4, v.y);
   SendSingle100(data, startIndex + 8, v.z);
 }
 
-void NetworkSync::SendSingle100(unsigned char *data, int startIndex,
+void NetworkSync::SendSingle100(unsigned char* data,
+                                int startIndex,
                                 float value) {
   // Sends a float with truncated 2 decimal precision
   Int32 intValue = value * 100;

NetworkSync.h

@@ -46,6 +46,7 @@ protected:
   NetworkPerception* networkPerception;
   void PublishTrackedObject(SendBuffer sendBuffer, TrackedObject* object);
 
+  UInt8 CreatePoseMsg(Roboid* roboid, UInt8* buffer);
   void SendVector3(unsigned char* data, int startIndex, Vector3 v);
   void SendSingle100(unsigned char* data, int startIndex, float value);
   void SendInt32(unsigned char* data, int startIndex, Int32 value);