Merge branch 'main' into ControlledMotor

DifferentialDrive.cpp

@@ -5,27 +5,30 @@
 
 DifferentialDrive::DifferentialDrive(){};
 
-DifferentialDrive::DifferentialDrive(Placement leftMotorPlacement,
+DifferentialDrive::DifferentialDrive(Motor *leftMotor, Motor *rightMotor,
-                                     Placement rightMotorPlacement) {
+                                     float separation) {
   this->motorCount = 2;
-  this->placement = new Placement[2];
+  this->motors = new Motor *[2];
-  this->placement[0] = leftMotorPlacement;
+  this->motors[0] = leftMotor;
-  this->placement[1] = rightMotorPlacement;
+  this->motors[1] = rightMotor;
-  Motor *leftMotor = (Motor *)leftMotorPlacement.thing;
+
+  float distance = separation / 2;
   leftMotor->direction = Motor::Direction::CounterClockwise;
-  Motor *rightMotor = (Motor *)rightMotorPlacement.thing;
+  leftMotor->position.angle = -90;
+  leftMotor->position.distance = distance;
   rightMotor->direction = Motor::Direction::Clockwise;
+  rightMotor->position.angle = 90;
+  rightMotor->position.distance = distance;
 }
 
 void DifferentialDrive::SetTargetSpeeds(float leftSpeed, float rightSpeed) {
   for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) {
-    Thing *thing = placement[motorIx].thing;
+    Motor *motor = motors[motorIx];
-    if (thing->type == Thing::UncontrolledMotorType) {
+    if (motor == nullptr)
-      Motor *motor = (Motor *)thing;
+      continue;
-      if (motor == nullptr)
-        continue;
 
-      float xPosition = placement[motorIx].position.x;
+    if (motor->type == Thing::UncontrolledMotorType) {
+      float xPosition = motors[motorIx]->position.angle;
       if (xPosition < 0)
         motor->SetSpeed(leftSpeed);
       else if (xPosition > 0)
@@ -50,8 +53,8 @@ void DifferentialDrive::SetTwistSpeed(Vector3 linear, float yaw, float pitch,
 }
 
 Polar DifferentialDrive::GetVelocity() {
-  Motor *leftMotor = (Motor *)placement[0].thing;
+  Motor *leftMotor = motors[0];
-  Motor *rightMotor = (Motor *)placement[1].thing;
+  Motor *rightMotor = motors[1];
   float leftSpeed = leftMotor->GetSpeed();
   float rightSpeed = rightMotor->GetSpeed();
   float speed = (leftSpeed + rightSpeed) / 2;
@@ -62,8 +65,8 @@ Polar DifferentialDrive::GetVelocity() {
 }
 
 float DifferentialDrive::GetAngularVelocity() {
-  Motor *leftMotor = (Motor *)placement[0].thing;
+  Motor *leftMotor = motors[0];
-  Motor *rightMotor = (Motor *)placement[1].thing;
+  Motor *rightMotor = motors[1];
   float leftSpeed = leftMotor->GetSpeed();
   float rightSpeed = rightMotor->GetSpeed();
   float angularVelocity = leftSpeed - rightSpeed;

DifferentialDrive.h

@@ -25,8 +25,10 @@ public:
   /// driving forward, while the right motor will turn Clockwise.
   /// @note When not using controlled motors, the placement of the motors is
   /// irrelevant.
-  DifferentialDrive(Placement leftMotorPlacement,
+  // DifferentialDrive(Placement leftMotorPlacement,
-                    Placement rightMotorPlacement);
+  //                   Placement rightMotorPlacement);
+
+  DifferentialDrive(Motor *leftMotor, Motor *rightMotor, float separation = 1);
 
   /// @brief Set the target speeds of the motors directly
   /// @param leftSpeed The target speed of the left Motor

Perception.cpp

@@ -7,14 +7,17 @@
 #include <math.h>
 
 Perception::Perception() {
+  this->trackedObjects = new TrackedObject *[maxObjectCount];
   for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++)
     this->trackedObjects[objIx] = nullptr;
 }
 
-Perception::Perception(Placement *sensors, unsigned int sensorCount)
+Perception::Perception(Sensor **sensors, unsigned int sensorCount)
     : Perception() {
   this->sensorCount = sensorCount;
-  this->sensorPlacements = (Placement *)sensors;
+  this->sensors = new Sensor *[this->sensorCount];
+  for (unsigned char sensorIx = 0; sensorIx < this->sensorCount; sensorIx++)
+    this->sensors[sensorIx] = sensors[sensorIx];
 }
 
 unsigned int Perception::GetSensorCount() { return this->sensorCount; }
@@ -23,16 +26,13 @@ Sensor *Perception::GetSensor(unsigned int sensorId) {
   if (sensorId >= this->sensorCount)
     return nullptr;
 
-  Thing *thing = this->sensorPlacements[sensorId].thing;
+  Sensor *sensor = this->sensors[sensorId];
-  if (thing->IsSensor())
+  return sensor;
-    return (Sensor *)thing;
-
-  return nullptr;
 }
 
 Sensor *Perception::FindSensorOfType(unsigned int sensorType) {
   for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) {
-    Sensor *sensor = (Sensor *)this->sensorPlacements[sensorIx].thing;
+    Sensor *sensor = this->sensors[sensorIx];
     if (sensor->type == sensorType)
       return sensor;
   }
@@ -108,6 +108,7 @@ void Perception::AddTrackedObject(Sensor *sensor, Polar position) {
     else {
       if (obj->IsTheSameAs(this->trackedObjects[objIx])) {
         this->trackedObjects[objIx]->Refresh(obj->position);
+        delete obj;
         return;
       }
       // Is this the fartest object we see?
@@ -128,9 +129,11 @@ void Perception::AddTrackedObject(Sensor *sensor, Polar position) {
   // If this object is closer than the farthest object, then replace it
   else if (obj->position.distance <
            this->trackedObjects[farthestObjIx]->position.distance) {
+    delete this->trackedObjects[farthestObjIx];
     this->trackedObjects[farthestObjIx] = obj;
-    // we may want to destroy the fartest object, but if it is created
+  } else {
-    // externally, other links may still exist...
+    // No available slot, delete trackedobject
+    delete obj;
   }
 }
 
@@ -154,23 +157,22 @@ void Perception::Update(float currentTimeMs) {
 
   // Update sensing
   for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) {
-    Placement thingPlacement = sensorPlacements[sensorIx];
+    Sensor *sensor = sensors[sensorIx];
-    Thing *thing = thingPlacement.thing;
+    if (sensor == nullptr)
-    if (thing == nullptr)
       continue;
 
-    if (thing->type == Thing::DistanceSensorType) {
+    if (sensor->type == Thing::DistanceSensorType) {
-      DistanceSensor *distanceSensor = (DistanceSensor *)thing;
+      DistanceSensor *distanceSensor = (DistanceSensor *)sensor;
 
       float distance = distanceSensor->GetDistance();
-      float angle = thingPlacement.horizontalDirection;
+      float angle = sensor->position.angle;
       Polar position = Polar(angle, distance);
       AddTrackedObject(distanceSensor, position);
-    } else if (thing->type == Thing::SwitchType) {
+
-      Switch *switchSensor = (Switch *)thing;
+    } else if (sensor->type == Thing::SwitchType) {
-      if (switchSensor != nullptr && switchSensor->IsOn()) {
+      Switch *switchSensor = (Switch *)sensor;
-        Polar position =
+      if (switchSensor->IsOn()) {
-            Polar(thingPlacement.horizontalDirection, nearbyDistance);
+        Polar position = Polar(sensor->position.angle, nearbyDistance);
         AddTrackedObject(switchSensor, position);
       }
     }
@@ -186,10 +188,9 @@ void Perception::Update(float currentTimeMs) {
       if (roboid != nullptr && roboid->networkSync != nullptr)
         roboid->networkSync->DestroyObject(obj);
       this->trackedObjects[objIx] = nullptr;
+      delete obj;
     }
   }
-  if (this->trackedObjects[0] != nullptr) {
-  }
 }
 
 void Perception::UpdatePose(Polar translation) {

Perception.h

@@ -1,11 +1,12 @@
 #pragma once
 
-#include "Placement.h"
 #include "Sensor.h"
 #include "TrackedObject.h"
 #include "VectorAlgebra/Polar.h"
 #include "VectorAlgebra/Quaternion.h"
 
+// #include <vector.h>
+
 namespace Passer {
 namespace RoboidControl {
 
@@ -20,7 +21,7 @@ public:
   /// @brief Create a perception setup with the given Sensors
   /// @param sensors The Placement of Sensors on the Roboid
   /// @param sensorCount The number of sensors in the placement array
-  Perception(Placement *sensors, unsigned int sensorCount);
+  Perception(Sensor **sensors, unsigned int sensorCount);
 
   /// @brief The roboid of this perception system
   Roboid *roboid = nullptr;
@@ -122,17 +123,17 @@ public:
   float nearbyDistance = 0.3F;
 
 public:
-  /// @brief The Placement of the Sensors used for Perception
+  /// @brief The Sensors used for Perception
-  Placement *sensorPlacements = nullptr;
+  Sensor **sensors = nullptr;
   /// @brief The number of Sensors used for Perception
   unsigned int sensorCount = 0;
 
   float lastUpdateTimeMs = 0;
 
-  static const unsigned char maxObjectCount = 7;
+  unsigned char maxObjectCount = 7; // 7 is typically the maximum
-  TrackedObject *trackedObjects[maxObjectCount]; // 7 is typically the maximum
+                                    // number of object which can
-                                                 // number of object which can
+                                    // be tracked by a human
-                                                 // be tracked by a human
+  TrackedObject **trackedObjects;
 };
 
 } // namespace RoboidControl

Placement.cpp

@@ -1,18 +0,0 @@
-#include "Placement.h"
-
-Placement::Placement() {
-  this->position = Vector3::zero;
-  this->thing = nullptr;
-}
-
-Placement::Placement(Thing *thing, Vector3 position, float horizontalDirection,
-                     float verticalDirection)
-    : horizontalDirection(horizontalDirection),
-      verticalDirection(verticalDirection) {
-  this->thing = thing;
-  this->position = position;
-}
-
-Placement::Placement(Thing *thing, float horizontalDirection,
-                     float verticalDirection)
-    : Placement(thing, Vector3::zero, horizontalDirection, verticalDirection) {}

Placement.h

@@ -1,82 +0,0 @@
-#pragma once
-
-#include "Thing.h"
-#include "VectorAlgebra/Vector2.h"
-#include "VectorAlgebra/Vector3.h"
-
-namespace Passer {
-namespace RoboidControl {
-
-/// @brief A plament is used to specify where a Thing is placed on the Roboid.
-/// @details
-/// It is not always necessary to exactly specify the position and orientation
-/// of a Thing. You can use a simple constructor in that case:
-///
-/// \code
-/// Thing* thing = new Thing();
-/// Placement p = Placement(thing);
-/// \endcode
-///
-/// The thing can be placed using carthesian coordinates in meters, while the
-/// orientation is specified with the horizontal and vertical direction. Whenö
-/// both horizontal and vertical direction are zero, the Thing is aligned with
-/// the parent thing in the hierarchy. When there is no parent, the thing is
-/// directed such that it is looking forward.
-///
-/// \code
-/// Thing* thing = new Thing();
-/// Placement p = Placement(thing, Vector3(-0.04F, 0.0F, 0.06F), 0.0F, 0.0F);
-/// \endcode
-/// In the example above, the thing is placed 4 cm to the left and 6 cm to the
-/// front relative to the parent. The orientation is the same as the parent. The
-/// second line can be simplified, as the default angles are zero and a Vector2
-/// position can be used which is a placement in the horizontal plane:
-///
-/// \code
-/// Placement p = Placement(thing, Vector2(-0.04F, 0.06F));
-/// \endcode
-class Placement {
-public:
-  /// @brief Placement of a Thing on a Roboid
-  /// @param thing The Thing which is placed
-  /// @param position The position of the Thing in carthesian coordinates
-  /// @param horizontalDirection The horizontal direction angle of the Thing.
-  /// Negative angles are to the left.
-  /// @param verticalAngle The vertical direction angle of the Thing. Negative
-  /// angles are downward.
-  Placement(Thing *thing, Vector3 position = Vector3::zero,
-            float horizontalDirection = 0.0F, float verticalAngle = 0.0F);
-  /// @brief Placement of a Thing on a Roboid without position
-  /// @param thing The Thing which is place
-  /// @param horizontalDirection The horizontal direction angle of the Thing.
-  /// Negative angles are to the left.
-  /// @param verticalDirection The vertical direction angle of the Thing.
-  /// Negative angles are downward.
-  Placement(Thing *thing, float horizontalDirection,
-            float verticalDirection = 0.0F);
-  /// @brief Default constructor with a zero placement
-  Placement();
-
-  /// @brief The parent placement in the Roboid hierarchy
-  /// @remark Reserved for future use
-  Placement *parent = nullptr;
-  /// @brief An array of children of this placement in the Roboid hierarchy
-  /// @remark Reserved for future use
-  Placement **children = nullptr;
-  /// @brief The number of children of this placemet in the Roboid hierarchy
-  /// @remark Reserved for future use
-  unsigned int childCount = 0;
-
-  /// @brief The Thing which is placed
-  Thing *thing;
-  /// @brief The position of the Thing in carthesian coordinates
-  Vector3 position;
-  /// @brief The angle or direction of the Thing in the horizontal plane
-  float horizontalDirection;
-  /// @brief The angle or direction of the Thing in the vertical plane
-  float verticalDirection;
-};
-
-} // namespace RoboidControl
-} // namespace Passer
-using namespace Passer::RoboidControl;

Propulsion.cpp

@@ -4,7 +4,7 @@
 #include "VectorAlgebra/FloatSingle.h"
 
 Propulsion::Propulsion() {
-  this->placement = nullptr;
+  this->motors == nullptr;
   this->motorCount = 0;
 }
 
@@ -14,22 +14,8 @@ Motor *Propulsion::GetMotor(unsigned int motorId) {
   if (motorId >= this->motorCount)
     return nullptr;
 
-  Thing *thing = this->placement[motorId].thing;
+  Motor *motor = this->motors[motorId];
-  if (thing->IsMotor())
+  return motor;
-    return (Motor *)thing;
-
-  return nullptr;
-}
-
-Placement *Propulsion::GetMotorPlacement(unsigned int motorId) {
-  if (motorId >= this->motorCount)
-    return nullptr;
-
-  Placement *placement = &this->placement[motorId];
-  if (placement->thing->IsMotor())
-    return placement;
-
-  return nullptr;
 }
 
 void Propulsion::Update(float currentTimeMs) {}

Propulsion.h

@@ -1,7 +1,6 @@
 #pragma once
 
 #include "Motor.h"
-#include "Placement.h"
 #include "VectorAlgebra/Polar.h"
 #include "VectorAlgebra/Quaternion.h"
 #include "VectorAlgebra/Vector2.h"
@@ -37,7 +36,7 @@ public:
   /// @param motorIx The index of the Motor
   /// @return Returns the Placement or a nullptr when no Placement with the give
   /// index could be found
-  Placement *GetMotorPlacement(unsigned int motorIx);
+  // Placement *GetMotorPlacement(unsigned int motorIx);
 
   /// @brief Sets the forward and rotation speed of a (grounded) Roboid
   /// @param forward The target forward speed
@@ -76,7 +75,8 @@ protected:
   /// @brief The number of motors used for Propulsion
   unsigned int motorCount = 0;
   /// @brief The Placement of the motors used for Propulsion
-  Placement *placement = nullptr;
+  // Placement *placement = nullptr;
+  Motor **motors = nullptr;
 };
 
 } // namespace RoboidControl

Switch.cpp

@@ -1,5 +1,5 @@
 #include "Switch.h"
 
-Switch::Switch() {}
+Switch::Switch() { this->type = Thing::SwitchType; }
 
 bool Switch::IsOn() { return false; }

Thing.cpp

@@ -2,7 +2,9 @@
 
 using namespace Passer::RoboidControl;
 
-Thing::Thing() { this->type = (unsigned int)Type::Undetermined; }
+Thing::Thing() : position(Polar::zero) {
+  this->type = (unsigned int)Type::Undetermined;
+}
 
 const unsigned int Thing::SwitchType = SensorType | (unsigned int)Type::Switch;
 const unsigned int Thing::DistanceSensorType =

Thing.h

@@ -1,5 +1,7 @@
 #pragma once
 
+#include "VectorAlgebra/Polar.h"
+
 namespace Passer {
 namespace RoboidControl {
 
@@ -30,6 +32,8 @@ public:
   /// @returns True when the Thing is a Sensor and False otherwise
   bool IsSensor();
 
+  Polar position;
+
 protected:
   /// @brief Bitmask for Motor type
   static const unsigned int MotorType = 0x8000;

TrackedObject.h

@@ -62,7 +62,7 @@ public:
 
 protected:
   static constexpr unsigned char maxConfidence = 255;
-  static constexpr unsigned char confidenceDropSpeed = 2;
+  static constexpr unsigned char confidenceDropSpeed = 1; // 2;
   unsigned char confidence;
 };