Velocity refactoring

DifferentialDrive.cpp

@@ -73,7 +73,7 @@ void DifferentialDrive::SetVelocity(Polar velocity) {
   SetTwistSpeed(velocity.distance, velocity.angle.InDegrees());
 }
 
-Polar DifferentialDrive::GetVelocity() {
+Spherical16 DifferentialDrive::GetVelocity() {
   Motor *leftMotor = motors[0];
   Motor *rightMotor = motors[1];
   float leftSpeed = leftMotor->GetActualSpeed();   // in revolutions per second
@@ -85,8 +85,9 @@ Polar DifferentialDrive::GetVelocity() {
 
   float direction = speed >= 0 ? 0.0F : 180.0F;
   float magnitude = fabsf(speed);
-  Polar velocity = Polar(
-      magnitude, Angle::Degrees(direction)); // Polar(direction, magnitude);
+  Spherical16 velocity =
+      Spherical16(magnitude, Angle16::Degrees(direction),
+                  Angle16::Degrees(0)); // Polar(direction, magnitude);
   return velocity;
 }
 

DifferentialDrive.h

@@ -68,7 +68,7 @@ public:
   /// information
   /// @remark This will be more expanded/detailed in a future version of Roboid
   /// Control
-  virtual Polar GetVelocity() override;
+  virtual Spherical16 GetVelocity() override;
   /// @brief Calculate the angular velocity of the roboid based on the wheel
   /// velocities
   /// @return The angular speed of the roboid in local space

NetworkSync.cpp

@@ -1,6 +1,6 @@
 #include "NetworkSync.h"
 
-#define RC_DEBUG 1
+// #define RC_DEBUG 1
 
 #ifdef RC_DEBUG
 #include <Arduino.h>
@@ -39,6 +39,7 @@ void NetworkSync::ReceiveNetworkId() {
 #ifdef RC_DEBUG
   SERIALPORT.printf("Received network Id %d\n", this->networkId);
 #endif
+  SendThing(roboid);
   SendName(roboid);
   SendModel(roboid);
   for (unsigned char childIx = 0; childIx < roboid->childCount; childIx++) {
@@ -64,6 +65,21 @@ void NetworkSync::PublishState(Roboid *roboid) {
   PublishPerception(roboid);
 }
 
+void NetworkSync::SendThing(Thing *thing) {
+  if (thing == nullptr)
+    return;
+
+  unsigned char ix = 0;
+  buffer[ix++] = CreateMsg;
+  buffer[ix++] = thing->id;
+  buffer[ix++] = thing->type;
+  SendBuffer(ix);
+
+#ifdef RC_DEBUG
+  printf("Sent Thing [%d/%d] %d\n", networkId, buffer[1], buffer[2]);
+#endif
+}
+
 void NetworkSync::NewObject(InterestingThing *thing) {
   if (thing == nullptr || thing->networkId != 0x00)
     return;
@@ -314,11 +330,9 @@ void NetworkSync::PublishTrackedObject(Roboid *roboid,
 }
 
 void NetworkSync::SendPoseMsg(Buffer sendBuffer, Roboid *roboid) {
-  // SERIALPORT.printf("old send pose?\n");
-  Polar velocity = roboid->propulsion->GetVelocity();
-  Vector2 worldVelocity2 =
-      Vector2::Rotate(Vector2::forward * velocity.distance, velocity.angle);
-  Vector3 worldVelocity3 = Vector3(worldVelocity2.x, 0, worldVelocity2.y);
+  Spherical16 velocity = roboid->propulsion->GetVelocity();
+  Spherical16 worldVelocity = roboid->orientation * velocity;
+  Vector3 worldVelocity3 = worldVelocity.ToVector3();
 
   float angularVelocity = roboid->propulsion->GetAngularVelocity();
   Vector3 worldAngularVelocity = Vector3(0, angularVelocity, 0);

NetworkSync.h

@@ -23,6 +23,7 @@ class NetworkSync {
   /// @param obj
   virtual void SendDestroyThing(InterestingThing *obj);
   virtual void NewObject(InterestingThing *obj);
+  void SendThing(Thing *thing);
   void SendName(Roboid *roboid);
   virtual void SendModel(Roboid *obj);
   void SendModel(Thing *thing);
@@ -99,29 +100,23 @@ class NetworkSync {
   void SendFloat16(unsigned char *data, unsigned char *startIndex, float value);
 
   void SendInt32(unsigned char *data, unsigned int startIndex, Int32 value);
-  void SendAngle8(unsigned char* data,
-                  unsigned int startIndex,
+  void SendAngle8(unsigned char *data, unsigned int startIndex,
                   const float value);
   // void SendAngle16(unsigned char *data, unsigned int startIndex,
   //                  const float value);
   // void SendAngle32(unsigned char *data, unsigned int startIndex,
   //                  const float value);
-  void SendVector3(unsigned char* data,
-                   unsigned char* startIndex,
+  void SendVector3(unsigned char *data, unsigned char *startIndex,
                    const Vector3 v);
-  void SendQuaternion(unsigned char* data,
-                      const int startIndex,
+  void SendQuaternion(unsigned char *data, const int startIndex,
                       const Quaternion q);
 
   void SendPolar(unsigned char *data, unsigned char *startIndex, Polar p);
-  void SendSpherical16(unsigned char* data,
-                       unsigned char* startIndex,
+  void SendSpherical16(unsigned char *data, unsigned char *startIndex,
                        Spherical16 s);
-  void SendSwingTwist(unsigned char* data,
-                      unsigned char* startIndex,
+  void SendSwingTwist(unsigned char *data, unsigned char *startIndex,
                       const SwingTwist16 r);
-  void SendQuat32(unsigned char* data,
-                  unsigned char* startIndex,
+  void SendQuat32(unsigned char *data, unsigned char *startIndex,
                   const Quaternion q);
 
   unsigned char buffer[256];

Propulsion.cpp

@@ -35,8 +35,10 @@ void Propulsion::SetTwistSpeed(Vector2 linear, float yaw) {}
 void Propulsion::SetTwistSpeed(Vector3 linear, float yaw, float pitch,
                                float roll) {}
 
-void Propulsion::SetVelocity(Polar velocity) {}
+void Propulsion::SetVelocity(Spherical16 velocity) {
+  this->linearVelocity = velocity;
+}
 
-Polar Propulsion::GetVelocity() { return Polar::zero; }
+Spherical16 Propulsion::GetVelocity() { return this->linearVelocity; }
 
 float Propulsion::GetAngularVelocity() { return 0; }

Propulsion.h

@@ -59,12 +59,12 @@ public:
   virtual void SetTwistSpeed(Vector3 linear, float yaw = 0.0F,
                              float pitch = 0.0F, float roll = 0.0F);
 
-  virtual void SetVelocity(Polar velocity);
+  virtual void SetVelocity(Spherical16 velocity);
 
   /// @brief Retrieve the current velocity of the roboid
   /// @return The velocity in polar coordinates
   /// The actual units of the velocity depend on the implementation
-  virtual Polar GetVelocity();
+  virtual Spherical16 GetVelocity();
   /// @brief Retrieve the current angular velocity of the roboid
   /// @return The angular velocity
   /// The actual unit of the angular velocity depend on the implementation
@@ -79,6 +79,8 @@ protected:
   /// @brief The Placement of the motors used for Propulsion
   // Placement *placement = nullptr;
   Motor **motors = nullptr;
+
+  Spherical16 linearVelocity = Spherical16::zero;
 };
 
 } // namespace RoboidControl

Roboid.cpp

@@ -18,11 +18,10 @@ Roboid::Roboid() : Thing(0) {
   this->type = RoboidType;
   this->perception = new Perception();
   this->perception->roboid = this;
-  this->propulsion = nullptr;
+  this->propulsion = new Propulsion();
   this->networkSync = nullptr;
-
-  this->roboidPosition = Spherical16::zero;
-  this->roboidOrientation = SwingTwist16::identity;
+  this->position = Spherical16::zero;
+  this->orientation = SwingTwist16::identity;
 }
 
 Roboid::Roboid(Propulsion *propulsion) : Roboid() {
@@ -43,10 +42,11 @@ void Roboid::Update(unsigned long currentTimeMs) {
     float deltaTime = (float)(currentTimeMs - lastUpdateTimeMs) / 1000;
 
     // Conversion from old units
-    Polar polarVelocity = this->propulsion->GetVelocity();
-    this->linearVelocity = Spherical16(
-        polarVelocity.distance,
-        Angle16::Degrees(polarVelocity.angle.InDegrees()), Angle16());
+    // Polar polarVelocity = this->propulsion->GetVelocity();
+    // this->linearVelocity = Spherical16(
+    //     polarVelocity.distance,
+    //     Angle16::Degrees(polarVelocity.angle.InDegrees()), Angle16());
+    this->linearVelocity = this->propulsion->GetVelocity();
     float oldAngular = this->propulsion->GetAngularVelocity();
     this->angularVelocity =
         Spherical16(oldAngular, Angle16(), Angle16::Degrees(90));
@@ -54,7 +54,8 @@ void Roboid::Update(unsigned long currentTimeMs) {
     // SetPosition(this->position + this->orientation * Spherical16::forward *
     //                                  this->linearVelocity.distance *
     //                                  deltaTime);
-    // this->roboidPosition = this->position; // assuming the roboid is the root
+    // this->roboidPosition = this->position; // assuming the roboid is the
+    // root
     this->position += this->orientation * Spherical16::forward *
                       this->linearVelocity.distance * deltaTime;
 

Thing.cpp

@@ -21,6 +21,7 @@ const unsigned int Thing::ControlledMotorType =
 const unsigned int Thing::UncontrolledMotorType =
     MotorType | (unsigned int)Type::UncontrolledMotor;
 const unsigned int Thing::ServoType = (unsigned int)Type::Servo;
+const unsigned int Thing::HumanoidType = (unsigned int)Type::Humanoid;
 
 bool Thing::IsMotor() { return (type & Thing::MotorType) != 0; }
 

Thing.h

@@ -32,6 +32,7 @@ public:
   static const unsigned int UncontrolledMotorType;
   /// @brief The type of an object received from the network
   static const unsigned int ServoType;
+  static const unsigned int HumanoidType;
 
   /// @brief Check if the Thing is a Motor
   /// @return True when the Thing is a Motor and False otherwise
@@ -118,6 +119,7 @@ protected:
     UncontrolledMotor,
     Servo,
     // Other
+    Humanoid,
     ExternalSensor,
   };