Fix include

git-subtree-dir: ControlCore
git-subtree-mainline: 2b5f5a58ac608aca3aad452a87f6cb27f428cbde
git-subtree-split: 0a57e6d99abadc3257c6b1fdf5880b993e0d0fcb

.gitignore

@@ -1,2 +1,4 @@
+doxygen/html
+doxygen/DoxyWarnLogfile.txt
 build
-.vscode
+.vscode

.gitlab-ci.yml

@@ -0,0 +1,61 @@
+# This file is a template, and might need editing before it works on your project.
+# This is a sample GitLab CI/CD configuration file that should run without any modifications.
+# It demonstrates a basic 3 stage CI/CD pipeline. Instead of real tests or scripts,
+# it uses echo commands to simulate the pipeline execution.
+#
+# A pipeline is composed of independent jobs that run scripts, grouped into stages.
+# Stages run in sequential order, but jobs within stages run in parallel.
+#
+# For more information, see: https://docs.gitlab.com/ee/ci/yaml/index.html#stages
+#
+# You can copy and paste this template into a new `.gitlab-ci.yml` file.
+# You should not add this template to an existing `.gitlab-ci.yml` file by using the `include:` keyword.
+#
+# To contribute improvements to CI/CD templates, please follow the Development guide at:
+# https://docs.gitlab.com/ee/development/cicd/templates.html
+# This specific template is located at:
+# https://gitlab.com/gitlab-org/gitlab/-/blob/master/lib/gitlab/ci/templates/Getting-Started.gitlab-ci.yml
+
+# This file is a template, and might need editing before it works on your project.
+# This is a sample GitLab CI/CD configuration file that should run without any modifications.
+# It demonstrates a basic 3 stage CI/CD pipeline. Instead of real tests or scripts,
+# it uses echo commands to simulate the pipeline execution.
+#
+# A pipeline is composed of independent jobs that run scripts, grouped into stages.
+# Stages run in sequential order, but jobs within stages run in parallel.
+#
+# For more information, see: https://docs.gitlab.com/ee/ci/yaml/index.html#stages
+#
+# You can copy and paste this template into a new `.gitlab-ci.yml` file.
+# You should not add this template to an existing `.gitlab-ci.yml` file by using the `include:` keyword.
+#
+# To contribute improvements to CI/CD templates, please follow the Development guide at:
+# https://docs.gitlab.com/ee/development/cicd/templates.html
+# This specific template is located at:
+# https://gitlab.com/gitlab-org/gitlab/-/blob/master/lib/gitlab/ci/templates/Getting-Started.gitlab-ci.yml
+
+variables:
+  GIT_SUBMODULE_STRATEGY: recursive
+default:
+  image: rikorose/gcc-cmake
+stages:
+- test
+unit-test-job:
+  stage: test
+  script:
+  - mkdir build
+  - cd build
+  - cmake ..
+  - cmake --build .
+  - export GTEST_OUTPUT="xml:report.xml"
+  - ls -la
+  - ls -ls test
+  - "./test/RoboidControlTest"
+  artifacts:
+    when: always
+    reports:
+      junit: build/report.xml
+sast:
+  stage: test
+include:
+- template: Security/SAST.gitlab-ci.yml

.gitmodules

@@ -0,0 +1,7 @@
+[submodule "LinearAlgebra"]
+	path = LinearAlgebra
+	url = ../linear-algebra.git
+	brnach = main
+[submodule "float16"]
+	path = float16
+	url = ../float16.git

AbsoluteEncoder.cpp

@@ -0,0 +1,2 @@
+#include "AbsoluteEncoder.h"
+

AbsoluteEncoder.h

@@ -0,0 +1,17 @@
+#pragma once
+
+#include "ServoMotor.h"
+
+namespace Passer {
+namespace RoboidContol {
+
+class AbsoluteEncoder {
+ public:
+  AbsoluteEncoder() {}
+
+  virtual Angle16 GetActualAngle() = 0;
+  virtual float GetActualVelocity() = 0;
+};
+
+}  // namespace RoboidContol
+}  // namespace Passer

Accelerometer.cpp

@@ -0,0 +1,5 @@
+#include "Accelerometer.h"
+
+Accelerometer::Accelerometer() {}
+
+Spherical Accelerometer::GetVector() { return Spherical(); }

Accelerometer.h

@@ -0,0 +1,18 @@
+#pragma once
+
+#include "LinearAlgebra/Spherical.h"
+#include "Sensor.h"
+
+namespace Passer {
+namespace RoboidControl {
+
+class Accelerometer : public Sensor {
+public:
+  Accelerometer();
+
+  virtual Spherical GetVector();
+};
+
+} // namespace RoboidControl
+} // namespace Passer
+using namespace Passer::RoboidControl;

Activation.cpp

@@ -0,0 +1,46 @@
+#include "Activation.h"
+
+float Activation::HeavisideStep(float inputValue, float bias) {
+  return (inputValue + bias > 0) ? 1.0F : 0.0F;
+}
+
+float Activation::Tanh(float inputValue) {
+    return (exp(inputValue) - exp(-inputValue)) / (exp(inputValue) + exp(-inputValue));
+}
+
+float Activation::Sigmoid(float inputValue) {
+    return 1 / (1 + expf(-inputValue));
+}
+
+float Activation::Linear(float inputValue, float minValue, float range) {
+  if (inputValue > minValue + range)
+    return 0;
+  if (inputValue < minValue)
+    return 1;
+
+  float f = (inputValue - minValue) * (1 / range);  // normalize to 1..0
+  float influence = 1 - f;             // invert
+  return influence;
+}
+
+
+float Activation::Quadratic(float inputValue, float minValue, float range) {
+  if (inputValue > minValue + range)
+    return 0;
+  if (inputValue < minValue)
+    return 1;
+
+  float f = (inputValue - minValue) * (1 / range);  // normalize to 1..0
+  float influence = 1 - (f * f);       // quadratic & invert
+  return influence;
+}
+
+float Activation::ParticleLife(float minValue, float maxValue, float attraction, float inputValue) {
+  if (inputValue < minValue)
+    return inputValue / minValue - 1;
+
+  if (inputValue < maxValue)
+    return attraction * (1 - fabs(2 * inputValue - minValue - maxValue) / (maxValue - minValue));
+
+  return 0;
+}

Activation.h

@@ -0,0 +1,32 @@
+#ifndef RC_ACTIVATION_H
+#define RC_ACTIVATION_H
+
+#include <math.h>
+
+namespace Passer {
+namespace RoboidControl {
+
+/// @brief Activation function for control
+/// @note This is mainly for future use :-)
+class Activation {
+public:
+  static float HeavisideStep(float inputValue, float bias = 0); // Range: {0,1}
+
+  static float Tanh(float inputValue); // Range: (-1, 1)
+
+  static float Sigmoid(float inputValue); // Range: (0, 1)
+
+  static float Linear(float inputValue, float bias = 0, float range = 0);
+
+  static float Quadratic(float inputValue, float bias = 0,
+                         float range = 0); // minValue = bias
+
+  static float ParticleLife(float minValue, float maxValue, float attraction,
+                            float inputValue); // minValue = bias
+};
+
+} // namespace RoboidControl
+} // namespace Passer
+using namespace Passer::RoboidControl;
+
+#endif

CMakeLists.txt

@@ -1,61 +1,65 @@
 cmake_minimum_required(VERSION 3.13)  # CMake version check
 if(ESP_PLATFORM)
-    idf_component_register(
-        SRC_DIRS "."
-        INCLUDE_DIRS "."
-    )
-else() 
-    project(ControlCore)
-    add_subdirectory(LinearAlgebra)
-
-    set(CMAKE_CXX_STANDARD 11)            # Enable c++11 standard
-    set(CMAKE_POSITION_INDEPENDENT_CODE ON)
-
-    add_compile_definitions(GTEST)
-    include(FetchContent)
-    FetchContent_Declare(
-        googletest
-        DOWNLOAD_EXTRACT_TIMESTAMP ON
-        URL https://github.com/google/googletest/archive/refs/heads/main.zip
-    )
-
-    # For Windows: Prevent overriding the parent project's compiler/linker settings
-    set(gtest_force_shared_crt ON CACHE BOOL "" FORCE)
-    FetchContent_MakeAvailable(googletest)
-
-    include_directories(
+    set(sourcedirs
         .
         LinearAlgebra
-    )
-    add_library(ControlCore STATIC 
-        "Thing.cpp"
-        "LowLevelMessages.cpp"
-        "Messages.cpp" 
-        "Participant.cpp" 
-        "float16.cpp"
-    )
-
-    enable_testing()
-
-    file(GLOB_RECURSE test_srcs test/*_test.cc)
-    add_executable(
-        ControlCoreTest
-        ${test_srcs}
-    )
-    target_link_libraries(
-        ControlCoreTest
-        gtest_main
         ControlCore
     )
 
-    if(MSVC)
-    target_compile_options(ControlCoreTest PRIVATE /W4 /WX)
-  else()
-    target_compile_options(ControlCoreTest PRIVATE -Wall -Wextra -Wpedantic -Werror)
-  endif()
-  
+    set(includedirs
+        .
+        LinearAlgebra
+        ControlCore
+    )
 
-    include(GoogleTest)
-    gtest_discover_tests(ControlCoreTest)
+    idf_component_register(
+        SRC_DIRS ${sourcedirs}
+        INCLUDE_DIRS ${includedirs}
+        REQUIRES arduino
+    )
 endif()
 
+project(RoboidControl)
+add_subdirectory(ControlCore)
+#add_subdirectory(LinearAlgebra)
+add_subdirectory(test)
+
+set(CMAKE_CXX_STANDARD 11)
+set(CMAKE_POSITION_INDEPENDENT_CODE ON)
+
+add_compile_definitions(GTEST)
+include(FetchContent)
+FetchContent_Declare(
+    googletest
+    DOWNLOAD_EXTRACT_TIMESTAMP ON
+    URL https://github.com/google/googletest/archive/refs/heads/main.zip
+)
+
+# For Windows: Prevent overriding the parent project's compiler/linker settings
+set(gtest_force_shared_crt ON CACHE BOOL "" FORCE)
+FetchContent_MakeAvailable(googletest)
+
+include_directories(
+    .
+    ControlCore
+    LinearAlgebra
+)
+
+add_library(RoboidControl STATIC 
+    "Roboid.cpp"
+    "Perception.cpp"
+    "TrackedObject.cpp"
+    "Propulsion.cpp"
+    "Motor.cpp"
+    "DifferentialDrive.cpp"
+    "DistanceSensor.cpp"
+    "Sensor.cpp"
+    "Switch.cpp"
+   
+    "Quadcopter.cpp"
+    #"Messages.cpp"
+)
+
+enable_testing()
+
+include(GoogleTest)

ControlCore/.gitignore

@@ -0,0 +1,2 @@
+build
+.vscode

ControlCore/CMakeLists.txt

@@ -0,0 +1,61 @@
+cmake_minimum_required(VERSION 3.13)  # CMake version check
+if(ESP_PLATFORM)
+    idf_component_register(
+        SRC_DIRS "."
+        INCLUDE_DIRS "."
+    )
+else() 
+    project(ControlCore)
+    add_subdirectory(LinearAlgebra)
+
+    set(CMAKE_CXX_STANDARD 11)            # Enable c++11 standard
+    set(CMAKE_POSITION_INDEPENDENT_CODE ON)
+
+    add_compile_definitions(GTEST)
+    include(FetchContent)
+    FetchContent_Declare(
+        googletest
+        DOWNLOAD_EXTRACT_TIMESTAMP ON
+        URL https://github.com/google/googletest/archive/refs/heads/main.zip
+    )
+
+    # For Windows: Prevent overriding the parent project's compiler/linker settings
+    set(gtest_force_shared_crt ON CACHE BOOL "" FORCE)
+    FetchContent_MakeAvailable(googletest)
+
+    include_directories(
+        .
+        LinearAlgebra
+    )
+    add_library(ControlCore STATIC 
+        "Thing.cpp"
+        "LowLevelMessages.cpp"
+        "Messages.cpp" 
+        "Participant.cpp" 
+        "float16.cpp"
+    )
+
+    enable_testing()
+
+    file(GLOB_RECURSE test_srcs test/*_test.cc)
+    add_executable(
+        ControlCoreTest
+        ${test_srcs}
+    )
+    target_link_libraries(
+        ControlCoreTest
+        gtest_main
+        ControlCore
+    )
+
+    if(MSVC)
+    target_compile_options(ControlCoreTest PRIVATE /W4 /WX)
+  else()
+    target_compile_options(ControlCoreTest PRIVATE -Wall -Wextra -Wpedantic -Werror)
+  endif()
+  
+
+    include(GoogleTest)
+    gtest_discover_tests(ControlCoreTest)
+endif()
+

ControlCore/test/CMakeLists.txt

@@ -0,0 +1,36 @@
+cmake_minimum_required(VERSION 3.13)  # CMake version check
+Project(ControlCoreTest)
+
+set(CMAKE_CXX_STANDARD 11)            # Enable c++11 standard
+set(CMAKE_POSITION_INDEPENDENT_CODE ON)
+
+add_compile_definitions(GTEST)
+include(FetchContent)
+FetchContent_Declare(
+    googletest
+    DOWNLOAD_EXTRACT_TIMESTAMP ON
+    URL https://github.com/google/googletest/archive/refs/heads/main.zip
+)
+
+# For Windows: Prevent overriding the parent project's compiler/linker settings
+set(gtest_force_shared_crt ON CACHE BOOL "" FORCE)
+FetchContent_MakeAvailable(googletest)
+
+include_directories(
+    . 
+    ..
+)
+enable_testing()
+
+add_executable(
+    ControlCoreTest
+    "dummy_test.cc"
+)
+
+target_link_libraries(
+    ControlCoreTest
+    gtest_main
+)
+
+include(GoogleTest)
+gtest_discover_tests(ControlCoreTest)

ControlledMotor.cpp

@@ -0,0 +1,65 @@
+#include "ControlledMotor.h"
+
+ControlledMotor::ControlledMotor() {
+  this->type = (unsigned char)Type::ControlledMotor;
+}
+
+ControlledMotor::ControlledMotor(Motor *motor, Encoder *encoder)
+    : ControlledMotor() {
+  this->motor = motor;
+  this->encoder = encoder;
+  // this->rotationDirection = Direction::Forward;
+}
+
+#include <Arduino.h>
+
+void ControlledMotor::SetTargetSpeed(float speed) {
+  this->currentTargetSpeed = speed;
+  // this->rotationDirection =
+  //     (targetSpeed < 0) ? Direction::Reverse : Direction::Forward;
+  // this->direction = (targetSpeed < 0) ? Motor::Direction::CounterClockwise
+  //                                     : Motor::Direction::Clockwise;
+}
+
+void ControlledMotor::Update(unsigned long currentTimeMs) {
+  this->actualSpeed = encoder->GetRevolutionsPerSecond(currentTimeMs);
+  if (this->currentTargetSpeed < 0)
+    this->actualSpeed = -this->actualSpeed;
+
+  float deltaTime = currentTimeMs - this->lastUpdateTime;
+
+  float error = this->currentTargetSpeed - this->actualSpeed;
+  float errorChange = (error - lastError) * deltaTime;
+
+  float delta = error * pidP + errorChange * pidD;
+
+  Serial.print("  actual Speed ");
+  Serial.print(actualSpeed);
+  Serial.print("  target Speed ");
+  Serial.print(this->currentTargetSpeed);
+  Serial.print("    motor target speed ");
+  Serial.print(motor->currentTargetSpeed);
+  Serial.print(" + ");
+  Serial.print(error * pidP);
+  Serial.print(" + ");
+  Serial.print(errorChange * pidD);
+  Serial.print(" = ");
+  Serial.println(motor->currentTargetSpeed + delta);
+
+  motor->SetTargetSpeed(motor->currentTargetSpeed +
+                        delta); // or something like that
+  this->lastUpdateTime = currentTimeMs;
+}
+
+float ControlledMotor::GetActualSpeed() { return actualSpeed; }
+
+bool ControlledMotor::Drive(float distance) {
+  if (!driving) {
+    targetDistance = distance;
+    startDistance = encoder->GetDistance();
+    driving = true;
+  }
+  float totalDistance = encoder->GetDistance() - startDistance;
+  bool completed = totalDistance > targetDistance;
+  return completed;
+}

ControlledMotor.h

@@ -0,0 +1,60 @@
+#pragma once
+
+#include "Encoder.h"
+#include "Motor.h"
+
+namespace Passer {
+namespace RoboidControl {
+
+/// @brief A motor with speed control
+/// It uses a feedback loop from an encoder to regulate the speed
+/// The speed is measured in revolutions per second.
+class ControlledMotor : public Motor {
+public:
+  ControlledMotor();
+  ControlledMotor(Motor *motor, Encoder *encoder);
+
+  inline static bool CheckType(Thing *thing) {
+    return (thing->type & (int)Thing::Type::ControlledMotor) != 0;
+  }
+  float velocity;
+
+  float pidP = 0.1F;
+  float pidD = 0.0F;
+  float pidI = 0.0F;
+
+  void Update(unsigned long currentTimeMs) override;
+
+  /// @brief Set the target speed for the motor controller
+  /// @param speed the target in revolutions per second.
+  virtual void SetTargetSpeed(float speed) override;
+
+  /// @brief Get the actual speed from the encoder
+  /// @return The speed in revolutions per second
+  virtual float GetActualSpeed() override;
+
+  bool Drive(float distance);
+
+  Motor *motor;
+  Encoder *encoder;
+
+protected:
+  float lastUpdateTime = 0;
+  float lastError = 0;
+  // float targetSpeed;
+  float actualSpeed;
+  float netDistance = 0;
+  float startDistance = 0;
+
+  // enum Direction { Forward = 1, Reverse = -1 };
+
+  // Direction rotationDirection;
+
+  bool driving = false;
+  float targetDistance = 0;
+  float lastEncoderPosition = 0;
+};
+
+} // namespace RoboidControl
+} // namespace Passer
+using namespace Passer::RoboidControl;

DifferentialDrive.cpp

@@ -0,0 +1,119 @@
+#include "DifferentialDrive.h"
+#include "LinearAlgebra/Angle.h"
+#include "LinearAlgebra/FloatSingle.h"
+
+#include <math.h>
+
+DifferentialDrive::DifferentialDrive() {};
+
+DifferentialDrive::DifferentialDrive(Motor *leftMotor, Motor *rightMotor) {
+  this->motorCount = 2;
+  this->motors = new Motor *[2];
+  this->motors[0] = leftMotor;
+  this->motors[1] = rightMotor;
+
+  float distance = this->wheelSeparation / 2;
+  leftMotor->direction = Motor::Direction::CounterClockwise;
+  leftMotor->SetPosition(Spherical16(distance, Direction16::left));
+  // leftMotor->position.direction.horizontal = Angle16::Degrees(-90);
+  // leftMotor->position.distance = distance;
+  rightMotor->direction = Motor::Direction::Clockwise;
+  rightMotor->SetPosition(Spherical16(distance, Direction16::right));
+  // rightMotor->position.direction.horizontal = Angle16::Degrees(90);
+  // rightMotor->position.distance = distance;
+}
+
+void DifferentialDrive::SetDimensions(float wheelDiameter,
+                                      float wheelSeparation) {
+  this->wheelDiameter = wheelDiameter;
+  this->wheelSeparation = wheelSeparation;
+  this->rpsToMs = wheelDiameter * Passer::LinearAlgebra::pi;
+
+  float distance = this->wheelSeparation / 2;
+  Spherical16 motor0Position = this->motors[0]->GetPosition();
+  motor0Position.distance = distance;
+  this->motors[0]->SetPosition(motor0Position);
+  Spherical16 motor1Position = this->motors[0]->GetPosition();
+  motor1Position.distance = distance;
+  this->motors[1]->SetPosition(motor1Position);
+}
+
+void DifferentialDrive::SetMotorTargetSpeeds(float leftSpeed,
+                                             float rightSpeed) {
+  for (unsigned int motorIx = 0; motorIx < this->motorCount; motorIx++) {
+    Motor *motor = motors[motorIx];
+    if (motor == nullptr)
+      continue;
+
+    float xPosition =
+        motors[motorIx]->GetPosition().direction.horizontal.InDegrees();
+    if (xPosition < 0)
+      motor->SetTargetSpeed(leftSpeed);
+    else if (xPosition > 0)
+      motor->SetTargetSpeed(rightSpeed);
+  };
+}
+
+void DifferentialDrive::SetTwistSpeed(float forward, float yaw) {
+  float leftSpeed =
+      Float::Clamp(forward + yaw, -1, 1); // revolutions per second
+  float rightSpeed =
+      Float::Clamp(forward - yaw, -1, 1); // revolutions per second
+
+  float leftMotorSpeed = leftSpeed / rpsToMs;   // meters per second
+  float rightMotorSpeed = rightSpeed / rpsToMs; // meters per second
+
+  SetMotorTargetSpeeds(leftMotorSpeed, rightMotorSpeed);
+}
+
+void DifferentialDrive::SetTwistSpeed(Vector2 linear, float yaw) {
+  SetTwistSpeed(linear.y, yaw);
+}
+
+void DifferentialDrive::SetTwistSpeed(Vector3 linear, float yaw, float pitch,
+                                      float roll) {
+  SetTwistSpeed(linear.Forward(), yaw);
+}
+
+// void DifferentialDrive::SetVelocity(Polar velocity) {
+//   SetTwistSpeed(velocity.distance, velocity.angle.InDegrees());
+// }
+
+// Spherical16 DifferentialDrive::GetVelocity() {
+//   Motor *leftMotor = motors[0];
+//   Motor *rightMotor = motors[1];
+//   float leftSpeed = leftMotor->GetActualSpeed();   // in revolutions per
+//   second float rightSpeed = rightMotor->GetActualSpeed(); // in revolutions
+//   per second
+
+//   leftSpeed = leftSpeed * rpsToMs;   // in meters per second
+//   rightSpeed = rightSpeed * rpsToMs; // in meters per second
+//   float speed = (leftSpeed + rightSpeed) / 2;
+
+//   float direction = speed >= 0 ? 0.0F : 180.0F;
+//   float magnitude = fabsf(speed);
+//   Spherical16 velocity =
+//       Spherical16(magnitude, Angle16::Degrees(direction),
+//                   Angle16::Degrees(0)); // Polar(direction, magnitude);
+//   return velocity;
+// }
+
+// float DifferentialDrive::GetAngularVelocity() {
+//   Motor *leftMotor = motors[0];
+//   Motor *rightMotor = motors[1];
+//   float leftSpeed = leftMotor->GetActualSpeed();   // in revolutions per
+//   second float rightSpeed = rightMotor->GetActualSpeed(); // in revolutions
+//   per second
+
+//   leftSpeed = leftSpeed * rpsToMs;   // in meters per second
+//   rightSpeed = rightSpeed * rpsToMs; // in meters per second
+
+//   float angularSpeed = (leftSpeed - rightSpeed) / 2;
+//   float angularDistance = wheelSeparation / 2 * Passer::LinearAlgebra::pi;
+//   float rotationsPerSecond = angularSpeed / angularDistance;
+//   float degreesPerSecond =
+//       Angle::Normalize(Angle::Degrees(360 * rotationsPerSecond)).InDegrees();
+//   float angularVelocity = degreesPerSecond;
+
+//   return angularVelocity;
+// }

DifferentialDrive.h

@@ -0,0 +1,90 @@
+#pragma once
+
+#include "Propulsion.h"
+
+namespace Passer {
+namespace RoboidControl {
+
+/// @brief A two-wheeled Propulsion method
+///
+/// The wheels are put at either side of the roboid with the following behaviour
+/// * When both wheels spin forward, the Roboid moves forward
+/// * When both wheels spin backward, the Roboid moves backward
+/// * When both wheels are spinning in opposite directions, the Roboid rotates
+/// wihout moving forward or backward
+/// * When just one wheel is spinning, the Roboid turnes while moving forward or
+/// backward.
+class DifferentialDrive : public Propulsion {
+public:
+  /// @brief Default constructor
+  DifferentialDrive();
+  /// @brief Setup of the DifferentialDrive with the Placement of the motors
+  /// @param leftMotorPlacement Placement of the left Motor
+  /// @param rightMotorPlacement Placement of the right Motor
+  /// In this setup, the left motor Direction will be CounterClockWise when
+  /// 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,
+  //                   Placement rightMotorPlacement);
+
+  DifferentialDrive(Motor *leftMotor, Motor *rightMotor);
+
+  void SetDimensions(float wheelDiameter, float wheelSeparation);
+
+  /// @brief Set the target speeds of the motors directly
+  /// @param leftSpeed The target speed of the left Motor
+  /// @param rightSpeed The target speed of the right Motor
+  void SetMotorTargetSpeeds(float leftSpeed, float rightSpeed);
+
+  /// @brief Controls the motors through forward and rotation speeds
+  /// @param forward The target forward speed of the Roboid
+  /// @param yaw The target rotation speed of the Roboid
+  virtual void SetTwistSpeed(float forward, float yaw) override;
+  /// @brief Controls the motors through forward and rotation speeds
+  /// @param linear The target linear speed of the Roboid
+  /// @param yaw The target rotation speed of the Roboid
+  /// @note As a DifferentialDrive cannot move sideward, this function has the
+  /// same effect as using the void SetTwistSpeed(float forward, float yaw)
+  /// function.
+  virtual void SetTwistSpeed(Vector2 linear, float yaw = 0.0F);
+  /// @brief Controls the motors through forward and rotation speeds
+  /// @param linear The target linear speed
+  /// @param yaw The target rotation speed around the vertical axis
+  /// @param pitch Pitch is not supported and is ignored
+  /// @param roll Roll is not supported and is ignores
+  /// @note As a DifferentialDrive cannot move sideward or vertical, this
+  /// function has the same effect as using the void SetTwistSpeed(float
+  /// forward, float yaw) function.
+  virtual void SetTwistSpeed(Vector3 linear, float yaw = 0.0F,
+                             float pitch = 0.0F, float roll = 0.0F);
+
+  // virtual void SetVelocity(Polar velocity);
+
+  /// @brief Calculate the linear velocity of the roboid based on the wheel
+  /// velocities
+  /// @return The velocity of the roboid in local space
+  /// @details The actual values may not be accurate, depending on the available
+  /// information
+  /// @remark This will be more expanded/detailed in a future version of Roboid
+  /// Control
+  // 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
+  /// @details The actual value may not be accurate, depending on the available
+  /// information
+  /// @remark This will be more expanded/detailed in a future version of Roboid
+  /// Control
+  // virtual float GetAngularVelocity() override;
+
+protected:
+  float wheelDiameter = 1.0F;   // in meters
+  float wheelSeparation = 1.0F; // in meters;
+
+  float rpsToMs = 1.0F; // convert revolutions per second to meters per second
+};
+
+} // namespace RoboidControl
+} // namespace Passer
+using namespace Passer::RoboidControl;

DirectionalSensor.cpp

@@ -0,0 +1,15 @@
+#include "DirectionalSensor.h"
+
+#include "ControlCore/LowLevelMessages.h"
+
+DirectionalSensor::DirectionalSensor() : Sensor() {
+  this->type = (unsigned char)Type::DirectionalSensor;
+  this->vector = Spherical16::zero;
+}
+
+Spherical16 DirectionalSensor::GetVector() { return Spherical16::zero; }
+
+void DirectionalSensor::ProcessBytes(unsigned char *data) {
+  unsigned char ix = 0;
+  this->vector = LowLevelMessages::ReceiveSpherical16(data, &ix);
+}

DirectionalSensor.h

@@ -0,0 +1,30 @@
+#pragma once
+
+#include "Sensor.h"
+
+namespace Passer {
+namespace RoboidControl {
+
+/// @brief A Sensor which can measure the distance to the nearest object
+class DirectionalSensor : public Sensor {
+public:
+  /// @brief Default constructor
+  DirectionalSensor();
+
+  // const unsigned int Type = SensorType | (unsigned
+  // int)Type::DirectionalSensor;
+
+  /// @brief Determine the distance to the nearest object
+  /// @return the measured distance in meters to the nearest object
+  virtual Spherical16 GetVector();
+
+  virtual void ProcessBytes(unsigned char *bytes) override;
+
+protected:
+  /// @brief Distance to the closest object
+  Spherical16 vector = Spherical16::zero;
+};
+
+} // namespace RoboidControl
+} // namespace Passer
+using namespace Passer::RoboidControl;

DistanceSensor.cpp

@@ -0,0 +1,37 @@
+#include "DistanceSensor.h"
+
+#include "ControlCore/LowLevelMessages.h"
+#include <math.h>
+
+DistanceSensor::DistanceSensor() : Sensor() {
+  this->type = (unsigned char)Type::DistanceSensor;
+  this->distance = INFINITY;
+  this->triggerDistance = 1.0F;
+}
+
+DistanceSensor::DistanceSensor(float triggerDistance) : DistanceSensor() {
+  this->triggerDistance = triggerDistance;
+}
+
+float DistanceSensor::GetDistance() {
+  if (this->distance < minRange || this->distance > maxRange)
+    return INFINITY; // invalid distance
+
+  float d = this->distance;
+  this->distance = INFINITY;
+  return d;
+};
+
+bool DistanceSensor::ObjectNearby() {
+  if (distance < minRange || distance > maxRange)
+    return false;
+
+  bool isOn = distance <= triggerDistance;
+  return isOn;
+}
+
+void DistanceSensor::ProcessBytes(unsigned char *bytes) {
+  unsigned char ix = 0;
+  this->distance = LowLevelMessages::ReceiveFloat16(bytes, &ix);
+  // std::cout << "received distance " << this->distance << "\n";
+}

DistanceSensor.h

@@ -0,0 +1,43 @@
+#pragma once
+
+#include "Sensor.h"
+
+namespace Passer {
+namespace RoboidControl {
+
+/// @brief A Sensor which can measure the distance to the nearest object
+class DistanceSensor : public Sensor {
+public:
+  /// @brief Default constructor
+  DistanceSensor();
+  /// @brief Creates a DistanceSensor with the given trigger distance
+  /// @param triggerDistance The distance at which the sensors indicates that a
+  /// object is close by
+  DistanceSensor(float triggerDistance);
+
+  /// @brief Determine the distance to the nearest object
+  /// @return the measured distance in meters to the nearest object
+  virtual float GetDistance();
+
+  /// @brief The minimum range at which the sensor gives reliable measurements
+  float minRange = 0.01F;
+  /// @brief The maximum range at which the sensor gives reliable measurements
+  float maxRange = 10.0F;
+
+  /// @brief The distance at which ObjectNearby triggers
+  float triggerDistance = 1;
+
+  /// @brief Indicate that an object is nearby
+  /// @return True when an object is nearby
+  bool ObjectNearby();
+
+  virtual void ProcessBytes(unsigned char *bytes) override;
+
+protected:
+  /// @brief Distance to the closest object
+  float distance = 0;
+};
+
+} // namespace RoboidControl
+} // namespace Passer
+using namespace Passer::RoboidControl;

Encoder.cpp

@@ -0,0 +1,21 @@
+/*
+#include "Encoder.h"
+
+IncrementalEncoder::IncrementalEncoder(unsigned char pulsesPerRevolution,
+                                       unsigned char distancePerRotation) {
+  this->pulsesPerRevolution = pulsesPerRevolution;
+  this->distancePerRevolution = distancePerRotation;
+}
+
+int IncrementalEncoder::GetPulseCount() { return 0; }
+
+float IncrementalEncoder::GetDistance() { return 0; }
+
+float IncrementalEncoder::GetPulsesPerSecond(float currentTimeMs) { return 0; }
+
+float IncrementalEncoder::GetRevolutionsPerSecond(float currentTimeMs) {
+  return 0;
+}
+
+float IncrementalEncoder::GetSpeed(float currentTimeMs) { return 0; }
+*/

Encoder.h

@@ -0,0 +1,57 @@
+#pragma once
+
+#include "IncrementalEncoder.h"
+
+namespace Passer {
+namespace RoboidControl {
+
+// Deprecated, use explicit IncrementalEncoder in the future
+using Encoder = IncrementalEncoder;
+
+/*
+/// @brief An Encoder measures the rotations of an axle using a rotary
+/// sensor Some encoders are able to detect direction, while others can not.
+class IncrementalEncoder {
+public:
+  /// @brief Creates a sensor which measures distance from pulses
+  /// @param pulsesPerRevolution The number of pulse edges which make a
+  /// full rotation
+  /// @param distancePerRevolution The distance a wheel travels per full
+  /// rotation
+  IncrementalEncoder(unsigned char pulsesPerRevolution = 1,
+                     unsigned char distancePerRevolution = 1);
+
+  /// @brief Get the total number of pulses since the previous call
+  /// @return The number of pulses, is zero or greater
+  virtual int GetPulseCount();
+  /// @brief Get the pulse speed since the previous call
+  /// @param currentTimeMs The clock time in milliseconds
+  /// @return The average pulses per second in the last period.
+  virtual float GetPulsesPerSecond(float currentTimeMs);
+
+  /// @brief Get the distance traveled since the previous call
+  /// @return The distance in meters.
+  virtual float GetDistance();
+
+  /// @brief Get the rotation speed since the previous call
+  /// @param currentTimeMs The clock time in milliseconds
+  /// @return The speed in rotations per second
+  virtual float GetRevolutionsPerSecond(float currentTimeMs);
+
+  /// @brief Get the speed since the previous call
+  /// @param currentTimeMs The clock time in milliseconds
+  /// @return The speed in meters per second.
+  /// @note this value is dependent on the accurate setting of the
+  /// pulsesPerRevolution and distancePerRevolution parameters;
+  virtual float GetSpeed(float currentTimeMs);
+
+  /// @brief The numer of pulses corresponding to a full rotation of the axle
+  unsigned char pulsesPerRevolution = 1;
+  /// @brief The number of revolutions which makes the wheel move forward 1
+  /// meter
+  unsigned char distancePerRevolution = 1;
+};
+*/
+} // namespace RoboidControl
+} // namespace Passer
+using namespace Passer::RoboidControl;

IncrementalEncoder.cpp

@@ -0,0 +1,19 @@
+#include "Encoder.h"
+
+IncrementalEncoder::IncrementalEncoder(unsigned char pulsesPerRevolution,
+                                       unsigned char distancePerRotation) {
+  this->pulsesPerRevolution = pulsesPerRevolution;
+  this->distancePerRevolution = distancePerRotation;
+}
+
+int IncrementalEncoder::GetPulseCount() { return 0; }
+
+float IncrementalEncoder::GetDistance() { return 0; }
+
+float IncrementalEncoder::GetPulsesPerSecond(float currentTimeMs) { return 0; }
+
+float IncrementalEncoder::GetRevolutionsPerSecond(float currentTimeMs) {
+  return 0;
+}
+
+float IncrementalEncoder::GetSpeed(float currentTimeMs) { return 0; }

IncrementalEncoder.h

@@ -0,0 +1,51 @@
+#pragma once
+
+namespace Passer {
+namespace RoboidControl {
+
+/// @brief An Encoder measures the rotations of an axle using a rotary
+/// sensor Some encoders are able to detect direction, while others can not.
+class IncrementalEncoder {
+public:
+  /// @brief Creates a sensor which measures distance from pulses
+  /// @param pulsesPerRevolution The number of pulse edges which make a
+  /// full rotation
+  /// @param distancePerRevolution The distance a wheel travels per full
+  /// rotation
+  IncrementalEncoder(unsigned char pulsesPerRevolution = 1,
+                     unsigned char distancePerRevolution = 1);
+
+  /// @brief Get the total number of pulses since the previous call
+  /// @return The number of pulses, is zero or greater
+  virtual int GetPulseCount();
+  /// @brief Get the pulse speed since the previous call
+  /// @param currentTimeMs The clock time in milliseconds
+  /// @return The average pulses per second in the last period.
+  virtual float GetPulsesPerSecond(float currentTimeMs);
+
+  /// @brief Get the distance traveled since the previous call
+  /// @return The distance in meters.
+  virtual float GetDistance();
+
+  /// @brief Get the rotation speed since the previous call
+  /// @param currentTimeMs The clock time in milliseconds
+  /// @return The speed in rotations per second
+  virtual float GetRevolutionsPerSecond(float currentTimeMs);
+
+  /// @brief Get the speed since the previous call
+  /// @param currentTimeMs The clock time in milliseconds
+  /// @return The speed in meters per second.
+  /// @note this value is dependent on the accurate setting of the
+  /// pulsesPerRevolution and distancePerRevolution parameters;
+  virtual float GetSpeed(float currentTimeMs);
+
+  /// @brief The numer of pulses corresponding to a full rotation of the axle
+  unsigned char pulsesPerRevolution = 1;
+  /// @brief The number of revolutions which makes the wheel move forward 1
+  /// meter
+  unsigned char distancePerRevolution = 1;
+};
+
+} // namespace RoboidControl
+} // namespace Passer
+using namespace Passer::RoboidControl;

LICENSE

@@ -0,0 +1,373 @@
+Mozilla Public License Version 2.0
+==================================
+
+1. Definitions
+--------------
+
+1.1. "Contributor"
+    means each individual or legal entity that creates, contributes to
+    the creation of, or owns Covered Software.
+
+1.2. "Contributor Version"
+    means the combination of the Contributions of others (if any) used
+    by a Contributor and that particular Contributor's Contribution.
+
+1.3. "Contribution"
+    means Covered Software of a particular Contributor.
+
+1.4. "Covered Software"
+    means Source Code Form to which the initial Contributor has attached
+    the notice in Exhibit A, the Executable Form of such Source Code
+    Form, and Modifications of such Source Code Form, in each case
+    including portions thereof.
+
+1.5. "Incompatible With Secondary Licenses"
+    means
+
+    (a) that the initial Contributor has attached the notice described
+        in Exhibit B to the Covered Software; or
+
+    (b) that the Covered Software was made available under the terms of
+        version 1.1 or earlier of the License, but not also under the
+        terms of a Secondary License.
+
+1.6. "Executable Form"
+    means any form of the work other than Source Code Form.
+
+1.7. "Larger Work"
+    means a work that combines Covered Software with other material, in
+    a separate file or files, that is not Covered Software.
+
+1.8. "License"
+    means this document.
+
+1.9. "Licensable"
+    means having the right to grant, to the maximum extent possible,
+    whether at the time of the initial grant or subsequently, any and
+    all of the rights conveyed by this License.
+
+1.10. "Modifications"
+    means any of the following:
+
+    (a) any file in Source Code Form that results from an addition to,
+        deletion from, or modification of the contents of Covered
+        Software; or
+
+    (b) any new file in Source Code Form that contains any Covered
+        Software.
+
+1.11. "Patent Claims" of a Contributor
+    means any patent claim(s), including without limitation, method,
+    process, and apparatus claims, in any patent Licensable by such
+    Contributor that would be infringed, but for the grant of the
+    License, by the making, using, selling, offering for sale, having
+    made, import, or transfer of either its Contributions or its
+    Contributor Version.
+
+1.12. "Secondary License"
+    means either the GNU General Public License, Version 2.0, the GNU
+    Lesser General Public License, Version 2.1, the GNU Affero General
+    Public License, Version 3.0, or any later versions of those
+    licenses.
+
+1.13. "Source Code Form"
+    means the form of the work preferred for making modifications.
+
+1.14. "You" (or "Your")
+    means an individual or a legal entity exercising rights under this
+    License. For legal entities, "You" includes any entity that
+    controls, is controlled by, or is under common control with You. For
+    purposes of this definition, "control" means (a) the power, direct
+    or indirect, to cause the direction or management of such entity,
+    whether by contract or otherwise, or (b) ownership of more than
+    fifty percent (50%) of the outstanding shares or beneficial
+    ownership of such entity.
+
+2. License Grants and Conditions
+--------------------------------
+
+2.1. Grants
+
+Each Contributor hereby grants You a world-wide, royalty-free,
+non-exclusive license:
+
+(a) under intellectual property rights (other than patent or trademark)
+    Licensable by such Contributor to use, reproduce, make available,
+    modify, display, perform, distribute, and otherwise exploit its
+    Contributions, either on an unmodified basis, with Modifications, or
+    as part of a Larger Work; and
+
+(b) under Patent Claims of such Contributor to make, use, sell, offer
+    for sale, have made, import, and otherwise transfer either its
+    Contributions or its Contributor Version.
+
+2.2. Effective Date
+
+The licenses granted in Section 2.1 with respect to any Contribution
+become effective for each Contribution on the date the Contributor first
+distributes such Contribution.
+
+2.3. Limitations on Grant Scope
+
+The licenses granted in this Section 2 are the only rights granted under
+this License. No additional rights or licenses will be implied from the
+distribution or licensing of Covered Software under this License.
+Notwithstanding Section 2.1(b) above, no patent license is granted by a
+Contributor:
+
+(a) for any code that a Contributor has removed from Covered Software;
+    or
+
+(b) for infringements caused by: (i) Your and any other third party's
+    modifications of Covered Software, or (ii) the combination of its
+    Contributions with other software (except as part of its Contributor
+    Version); or
+
+(c) under Patent Claims infringed by Covered Software in the absence of
+    its Contributions.
+
+This License does not grant any rights in the trademarks, service marks,
+or logos of any Contributor (except as may be necessary to comply with
+the notice requirements in Section 3.4).
+
+2.4. Subsequent Licenses
+
+No Contributor makes additional grants as a result of Your choice to
+distribute the Covered Software under a subsequent version of this
+License (see Section 10.2) or under the terms of a Secondary License (if
+permitted under the terms of Section 3.3).
+
+2.5. Representation
+
+Each Contributor represents that the Contributor believes its
+Contributions are its original creation(s) or it has sufficient rights
+to grant the rights to its Contributions conveyed by this License.
+
+2.6. Fair Use
+
+This License is not intended to limit any rights You have under
+applicable copyright doctrines of fair use, fair dealing, or other
+equivalents.
+
+2.7. Conditions
+
+Sections 3.1, 3.2, 3.3, and 3.4 are conditions of the licenses granted
+in Section 2.1.
+
+3. Responsibilities
+-------------------
+
+3.1. Distribution of Source Form
+
+All distribution of Covered Software in Source Code Form, including any
+Modifications that You create or to which You contribute, must be under
+the terms of this License. You must inform recipients that the Source
+Code Form of the Covered Software is governed by the terms of this
+License, and how they can obtain a copy of this License. You may not
+attempt to alter or restrict the recipients' rights in the Source Code
+Form.
+
+3.2. Distribution of Executable Form
+
+If You distribute Covered Software in Executable Form then:
+
+(a) such Covered Software must also be made available in Source Code
+    Form, as described in Section 3.1, and You must inform recipients of
+    the Executable Form how they can obtain a copy of such Source Code
+    Form by reasonable means in a timely manner, at a charge no more
+    than the cost of distribution to the recipient; and
+
+(b) You may distribute such Executable Form under the terms of this
+    License, or sublicense it under different terms, provided that the
+    license for the Executable Form does not attempt to limit or alter
+    the recipients' rights in the Source Code Form under this License.
+
+3.3. Distribution of a Larger Work
+
+You may create and distribute a Larger Work under terms of Your choice,
+provided that You also comply with the requirements of this License for
+the Covered Software. If the Larger Work is a combination of Covered
+Software with a work governed by one or more Secondary Licenses, and the
+Covered Software is not Incompatible With Secondary Licenses, this
+License permits You to additionally distribute such Covered Software
+under the terms of such Secondary License(s), so that the recipient of
+the Larger Work may, at their option, further distribute the Covered
+Software under the terms of either this License or such Secondary
+License(s).
+
+3.4. Notices
+
+You may not remove or alter the substance of any license notices
+(including copyright notices, patent notices, disclaimers of warranty,
+or limitations of liability) contained within the Source Code Form of
+the Covered Software, except that You may alter any license notices to
+the extent required to remedy known factual inaccuracies.
+
+3.5. Application of Additional Terms
+
+You may choose to offer, and to charge a fee for, warranty, support,
+indemnity or liability obligations to one or more recipients of Covered
+Software. However, You may do so only on Your own behalf, and not on
+behalf of any Contributor. You must make it absolutely clear that any
+such warranty, support, indemnity, or liability obligation is offered by
+You alone, and You hereby agree to indemnify every Contributor for any
+liability incurred by such Contributor as a result of warranty, support,
+indemnity or liability terms You offer. You may include additional
+disclaimers of warranty and limitations of liability specific to any
+jurisdiction.
+
+4. Inability to Comply Due to Statute or Regulation
+---------------------------------------------------
+
+If it is impossible for You to comply with any of the terms of this
+License with respect to some or all of the Covered Software due to
+statute, judicial order, or regulation then You must: (a) comply with
+the terms of this License to the maximum extent possible; and (b)
+describe the limitations and the code they affect. Such description must
+be placed in a text file included with all distributions of the Covered
+Software under this License. Except to the extent prohibited by statute
+or regulation, such description must be sufficiently detailed for a
+recipient of ordinary skill to be able to understand it.
+
+5. Termination
+--------------
+
+5.1. The rights granted under this License will terminate automatically
+if You fail to comply with any of its terms. However, if You become
+compliant, then the rights granted under this License from a particular
+Contributor are reinstated (a) provisionally, unless and until such
+Contributor explicitly and finally terminates Your grants, and (b) on an
+ongoing basis, if such Contributor fails to notify You of the
+non-compliance by some reasonable means prior to 60 days after You have
+come back into compliance. Moreover, Your grants from a particular
+Contributor are reinstated on an ongoing basis if such Contributor
+notifies You of the non-compliance by some reasonable means, this is the
+first time You have received notice of non-compliance with this License
+from such Contributor, and You become compliant prior to 30 days after
+Your receipt of the notice.
+
+5.2. If You initiate litigation against any entity by asserting a patent
+infringement claim (excluding declaratory judgment actions,
+counter-claims, and cross-claims) alleging that a Contributor Version
+directly or indirectly infringes any patent, then the rights granted to
+You by any and all Contributors for the Covered Software under Section
+2.1 of this License shall terminate.
+
+5.3. In the event of termination under Sections 5.1 or 5.2 above, all
+end user license agreements (excluding distributors and resellers) which
+have been validly granted by You or Your distributors under this License
+prior to termination shall survive termination.
+
+************************************************************************
+*                                                                      *
+*  6. Disclaimer of Warranty                                           *
+*  -------------------------                                           *
+*                                                                      *
+*  Covered Software is provided under this License on an "as is"       *
+*  basis, without warranty of any kind, either expressed, implied, or  *
+*  statutory, including, without limitation, warranties that the       *
+*  Covered Software is free of defects, merchantable, fit for a        *
+*  particular purpose or non-infringing. The entire risk as to the     *
+*  quality and performance of the Covered Software is with You.        *
+*  Should any Covered Software prove defective in any respect, You     *
+*  (not any Contributor) assume the cost of any necessary servicing,   *
+*  repair, or correction. This disclaimer of warranty constitutes an   *
+*  essential part of this License. No use of any Covered Software is   *
+*  authorized under this License except under this disclaimer.         *
+*                                                                      *
+************************************************************************
+
+************************************************************************
+*                                                                      *
+*  7. Limitation of Liability                                          *
+*  --------------------------                                          *
+*                                                                      *
+*  Under no circumstances and under no legal theory, whether tort      *
+*  (including negligence), contract, or otherwise, shall any           *
+*  Contributor, or anyone who distributes Covered Software as          *
+*  permitted above, be liable to You for any direct, indirect,         *
+*  special, incidental, or consequential damages of any character      *
+*  including, without limitation, damages for lost profits, loss of    *
+*  goodwill, work stoppage, computer failure or malfunction, or any    *
+*  and all other commercial damages or losses, even if such party      *
+*  shall have been informed of the possibility of such damages. This   *
+*  limitation of liability shall not apply to liability for death or   *
+*  personal injury resulting from such party's negligence to the       *
+*  extent applicable law prohibits such limitation. Some               *
+*  jurisdictions do not allow the exclusion or limitation of           *
+*  incidental or consequential damages, so this exclusion and          *
+*  limitation may not apply to You.                                    *
+*                                                                      *
+************************************************************************
+
+8. Litigation
+-------------
+
+Any litigation relating to this License may be brought only in the
+courts of a jurisdiction where the defendant maintains its principal
+place of business and such litigation shall be governed by laws of that
+jurisdiction, without reference to its conflict-of-law provisions.
+Nothing in this Section shall prevent a party's ability to bring
+cross-claims or counter-claims.
+
+9. Miscellaneous
+----------------
+
+This License represents the complete agreement concerning the subject
+matter hereof. If any provision of this License is held to be
+unenforceable, such provision shall be reformed only to the extent
+necessary to make it enforceable. Any law or regulation which provides
+that the language of a contract shall be construed against the drafter
+shall not be used to construe this License against a Contributor.
+
+10. Versions of the License
+---------------------------
+
+10.1. New Versions
+
+Mozilla Foundation is the license steward. Except as provided in Section
+10.3, no one other than the license steward has the right to modify or
+publish new versions of this License. Each version will be given a
+distinguishing version number.
+
+10.2. Effect of New Versions
+
+You may distribute the Covered Software under the terms of the version
+of the License under which You originally received the Covered Software,
+or under the terms of any subsequent version published by the license
+steward.
+
+10.3. Modified Versions
+
+If you create software not governed by this License, and you want to
+create a new license for such software, you may create and use a
+modified version of this License if you rename the license and remove
+any references to the name of the license steward (except to note that
+such modified license differs from this License).
+
+10.4. Distributing Source Code Form that is Incompatible With Secondary
+Licenses
+
+If You choose to distribute Source Code Form that is Incompatible With
+Secondary Licenses under the terms of this version of the License, the
+notice described in Exhibit B of this License must be attached.
+
+Exhibit A - Source Code Form License Notice
+-------------------------------------------
+
+  This Source Code Form is subject to the terms of the Mozilla Public
+  License, v. 2.0. If a copy of the MPL was not distributed with this
+  file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+If it is not possible or desirable to put the notice in a particular
+file, then You may include the notice in a location (such as a LICENSE
+file in a relevant directory) where a recipient would be likely to look
+for such a notice.
+
+You may add additional accurate notices of copyright ownership.
+
+Exhibit B - "Incompatible With Secondary Licenses" Notice
+---------------------------------------------------------
+
+  This Source Code Form is "Incompatible With Secondary Licenses", as
+  defined by the Mozilla Public License, v. 2.0.

MessageFormat.md

@@ -0,0 +1,78 @@
+
+Message Format
+==============
+
+## Data type
+
+The datatype consists of 4-bit (nibble) 2 parts: the most significant nibble is the syntax, while the least significant nibble is the sematics. 
+
+| 7 . 6 | 5 . 4| 3. 2. 1. 0 |
+|-|-|-|
+| angular | linear | semantics |
+
+When the Data type is 0x00, this indicates that a second byte will follow which defines further syntax options. Currently, this has not been defined yet, but can be used to define high-resolution angles using 2 bytes per dimension.
+
+
+### Syntax
+
+The syntax defines the format of the data sent and makes it possible to parse or skip parts of the message. Every value can consist of two parts: the angular and linear part, each represented by 2 bits.
+
+
+| Angular part | |
+|-|-|
+| 00 (0x0) | No angle | 
+| 01 (0x4) | 1-byte, 1D angle |
+| 10 (0x8) | 2-byte, 2D angle |
+| 11 (0xC) | 3-byte, 3D angle |
+
+| Linear part | |
+|-|-|
+| 00 (0x0) | No magnitude |
+| 01 (0x1) | 1 byte magnitude |
+| 10 (0x2) | 2 byte magnitude |
+| 11 (0x3) | 4 byte magnitude |
+
+Note that these values do not actually say how the angular and linear values are encoded. A 4-byte magnitude may be a 32-bit unsigned int or a single precision float for example. This depends on the semantics.
+
+When the angular and linear parts are combined, the size of the message can be determined. A few examples:
+
+| Data type | Size | Description |
+|-|-|-|
+| 0001 (0x1) | 1 byte | A single byte of linear data, like an intensity value |
+| 0100 (0x4) | 1 byte | An angle, like an angle between 0 and 360 degrees |
+| 0101 (0x5) | 2 bytes | An angle with a distance, like a polar coordinate |
+| 1101 (0xD) | 4 bytes | For example a quaternion, where the x,y,z = angular and w = linear part |
+| 0002 (0x2) | 2 bytes | A 16-bit value, like a single short int |
+| 1011 (0xB) | 6 bytes | 2D angles and a 32-bit distance, for example a spherical coordinate |
+
+### Semantics
+
+The semantics defines how the data should be interpreted. The following are defined:
+
+| Semantics | |
+|-|-|
+| 0b0001 (0x1) | Position |
+| 0b0010 (0x2) | Linear velocity |
+| 0b0011 (0x3) | Linear acceleration |
+| 0b0101 (0x5) | Orientation or rotation |
+| ob0110 (0x6) | Angular velocity |
+| 0b0111 (0x7) | Angular acceleration |
+| 0b1000 (0x8) | Direction |
+
+## Currently implemented data types
+
+| Data type | Message |
+|-|-|
+| 0x62 | Polar velocity with 2-byte distance |
+
+## Typical data types
+
+| Data type | Interpretation |
+| 0x48 | A direction in 2D space, represented by an angle. |
+| 0x88 | A direction in 3D space, represented by 2 angles. |
+| 0x83 | A position in 3D space, using Spherical coordinates |
+| 0xD4 | An orientation represented by a quaternion |
+| 0xE5 | Angular verlocity, represented in Angle-axis format |
+| 0xF5 | Angular verlocity, represented in Angle-axis format using a float for the magnitude |
+
+Note that a 2D or 3D vector is not directly supported. In alignment to the coordinate sytem of Roboid controle, these need to be converted to Polar or Spherical coordinates before they can be sent.

Motor.cpp

@@ -0,0 +1,17 @@
+#include "Motor.h"
+
+#include <time.h>
+
+Motor::Motor() : Thing() {
+  this->type = (unsigned char)Type::UncontrolledMotor;
+}
+
+float Motor::GetActualSpeed() { return this->currentTargetSpeed; }
+
+void Motor::SetTargetSpeed(float targetSpeed) {
+  this->currentTargetSpeed = targetSpeed;
+}
+
+float Motor::GetTargetSpeed() { return (this->currentTargetSpeed); }
+
+void Motor::Update(unsigned long currentTimeMs) {}

Motor.h

@@ -0,0 +1,46 @@
+#pragma once
+
+#include "ControlCore/Thing.h"
+
+#include <time.h>
+
+namespace Passer {
+using namespace Control;
+namespace RoboidControl {
+
+/// @brief A Motor is a Thing which can move parts of the Roboid
+/// @note Currently only rotational motors are supported
+class Motor : public Thing {
+public:
+  /// @brief Default constructor for the Motor
+  Motor();
+
+  /// @brief Motor turning direction
+  enum class Direction { Clockwise = 1, CounterClockwise = -1 };
+  /// @brief The forward turning direction of the motor
+  Direction direction = Direction::Clockwise;
+
+  /// @brief Set the target motor speed
+  /// @param speed The speed between -1 (full backward), 0 (stop) and 1 (full
+  /// forward)
+  virtual void SetTargetSpeed(float speed);
+  /// @brief Get the current target speed of the motor
+  /// @return The speed between -1 (full backward), 0 (stop) and 1 (full
+  /// forward)
+  virtual float GetTargetSpeed();
+
+  /// @brief Get the current actual speed of the motor
+  /// @return The speed between -1 (full backward), 0 (stop) and 1 (full
+  /// forward)
+  virtual float GetActualSpeed();
+
+  virtual void Update(unsigned long currentTimeMs);
+
+  float currentTargetSpeed = 0;
+
+protected:
+};
+
+} // namespace RoboidControl
+} // namespace Passer
+using namespace Passer::RoboidControl;

NetworkSync.cpp

@@ -0,0 +1,330 @@
+#include "NetworkSync.h"
+
+// #define RC_DEBUG 1
+
+#ifdef RC_DEBUG
+#include <Arduino.h>
+#if ESP32
+#define SERIALPORT Serial
+#else
+#define SERIALPORT Serial
+#endif
+#endif
+
+#include "ControlCore/Messages.h"
+#include "LinearAlgebra/Angle.h"
+#include "LinearAlgebra/AngleUsing.h"
+#include "LinearAlgebra/Spherical.h"
+
+#include <string.h>
+
+NetworkSync::NetworkSync(Roboid *roboid) {
+  this->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);
+  ReceiveData(bytecount);
+}
+
+void NetworkSync::PublishClient() {
+  ClientMsg msg = ClientMsg(this->networkId);
+  msg.Publish(this);
+
+#ifdef RC_DEBUG
+  SERIALPORT.printf("Publish Device\n");
+#endif
+}
+
+void NetworkSync::ProcessNetworkIdMsg(Passer::Control::NetworkIdMsg msg) {
+#ifdef RC_DEBUG
+  SERIALPORT.printf("Received network Id %d\n", msg.networkId);
+#endif
+  this->networkId = msg.networkId;
+  SendThingInfo(roboid, true);
+  printf("completed\n");
+}
+
+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::ProcessInvestigateMsg(InvestigateMsg msg) {
+#if RC_DEBUG
+  printf("Received InvestigateMsg [%d/%d]\n", msg.networkId, msg.thingId);
+#endif
+
+  if (msg.networkId != this->networkId)
+    // We only respond to investigation requests for our own objects
+    return;
+
+  Thing *thing = (Thing *)Thing::Get(0, msg.thingId);
+  if (thing != nullptr)
+    this->SendThingInfo(thing, true);
+}
+
+void NetworkSync::ProcessThingMsg(ThingMsg msg) {
+  if (msg.networkId == roboid->networkSync->networkId)
+    // Do not process messages for you own things
+    return;
+
+  printf("Received Thing message [%d/%d]\n", msg.networkId, msg.thingId);
+  InterestingThing *thing =
+      roboid->perception->FindTrackedObject(msg.networkId, msg.thingId);
+  if (thing != nullptr) {
+    thing->type = msg.thingType;
+  }
+  printf("complete\n");
+}
+
+void NetworkSync::ProcessPoseMsg(PoseMsg msg) {}
+
+void NetworkSync::PublishState(Roboid *roboid) {
+  SendPose(roboid);
+  PublishPerception(roboid);
+}
+
+void NetworkSync::SendThingInfo(Thing *thing, bool recurse) {
+  if (thing == nullptr)
+    return;
+
+  SendThing(thing);
+  SendName(thing);
+  SendModel(thing);
+  SendCustom(thing);
+  SendPose(thing, true, false);
+
+  if (recurse) {
+    for (unsigned char childIx = 0; childIx < thing->childCount; childIx++) {
+      Thing *child = (Thing *)thing->GetChildByIndex(childIx);
+      if (child != nullptr)
+        SendThingInfo(child, true);
+    }
+  }
+}
+
+void NetworkSync::SendThing(Thing *thing) {
+  if (thing == nullptr)
+    return;
+
+  Thing *parent = (Thing *)thing->GetParent();
+  ThingMsg msg = ThingMsg(this->networkId, thing->id, thing->type,
+                          parent == nullptr ? 0 : parent->id);
+  msg.SendTo(this);
+
+#ifdef RC_DEBUG
+  printf("Sent Thing [%d/%d] %d\n", networkId, thing->id, (byte)thing->type);
+#endif
+}
+
+void NetworkSync::SendName(Thing *thing) {
+  if (thing->name == nullptr)
+    return;
+
+  unsigned char len = strlen(thing->name);
+  if (len > 255)
+    return;
+
+  NameMsg msg = NameMsg(this->networkId, thing->id, thing->name, len);
+  msg.SendTo(this);
+
+#ifdef RC_DEBUG
+  SERIALPORT.printf("Sent Name [%d/%d] %s\n", networkId, buffer[1],
+                    thing->name);
+#endif
+}
+
+void NetworkSync::SendModel(Thing *thing) {
+  if (thing->modelUrl == nullptr)
+    return;
+
+  unsigned char len = strlen(thing->modelUrl);
+  if (len > 255)
+    return;
+
+  ModelUrlMsg msg = ModelUrlMsg(this->networkId, thing->id, len,
+                                thing->modelUrl, thing->modelScale);
+  msg.SendTo(this);
+}
+
+void NetworkSync::SendCustom(Thing *thing) {
+  CustomMsg msg = CustomMsg(this->networkId, thing);
+  msg.SendTo(this);
+}
+
+void NetworkSync::SendDestroy(Thing *thing) {
+  DestroyMsg msg = DestroyMsg(this->networkId, thing);
+  msg.SendTo(this);
+#if RC_DEBUG
+  printf("Sent DestroyMsg [%d/%d]\n", thing->networkId, thing->id);
+#endif
+}
+
+#include <iostream>
+void NetworkSync::ProcessCustomMsg(CustomMsg msg) {
+  // we assume networkId == 0 as custom messages are intended for my things
+  Thing *thing = (Thing *)roboid->GetChild(msg.thingId, true);
+  if (thing != nullptr)
+    thing->ProcessBytes(msg.data);
+}
+
+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 (force || thing->positionUpdated || thing->orientationUpdated) {
+    unsigned char poseType = 0;
+    if (force || thing->positionUpdated)
+      poseType |= PoseMsg::Pose_Position;
+    if (force || thing->orientationUpdated)
+      poseType |= PoseMsg::Pose_Orientation;
+    if (thing->linearVelocity.distance > 0)
+      poseType |= PoseMsg::Pose_LinearVelocity;
+    if (thing->angularVelocity.distance > 0)
+      poseType |= PoseMsg::Pose_AngularVelocity;
+    PoseMsg msg = PoseMsg(this->networkId, thing->id, poseType,
+                          thing->GetPosition(), thing->GetOrientation(),
+                          thing->linearVelocity, thing->angularVelocity);
+    msg.SendTo(this);
+
+    thing->positionUpdated = false;
+    thing->orientationUpdated = false;
+
+#if RC_DEBUG
+    if (thing->id != 0) {
+      Vector3 v = thing->position.ToVector3();
+      printf("Sent PoseMsg Thing [%d/%d] %f(%f)-(%f %f %f) %f\n",
+             this->networkId, thing->id, thing->position.distance,
+             thing->position.direction.horizontal.InDegrees(), v.Right(),
+             v.Up(), v.Forward(),
+             thing->orientation.swing.horizontal.InDegrees());
+    }
+#endif
+  }
+
+  if (recurse) {
+    for (unsigned char childIx = 0; childIx < thing->childCount; childIx++) {
+      Thing *child = (Thing *)thing->GetChildByIndex(childIx);
+      if (child != nullptr)
+        SendPose(child, force, recurse);
+    }
+  }
+}
+
+void NetworkSync::PublishPerception(Roboid *roboid) {
+  // Perception *perception = roboid->perception;
+  // if (perception == nullptr)
+  //   return;
+
+  // for (unsigned int sensorIx = 0; sensorIx < perception->sensorCount;
+  //      sensorIx++) {
+  //   Sensor *sensor = perception->sensors[sensorIx];
+  //   if (sensor == nullptr)
+  //     continue;
+  // }
+  PublishTrackedObjects(roboid, roboid->perception->GetTrackedObjects());
+}
+
+void NetworkSync::PublishTrackedObjects(Roboid *roboid,
+                                        InterestingThing **objects) {
+  if (networkId == 0) // We're not connected to a site yet
+    return;
+
+  int n = 0;
+  for (unsigned char objIx = 0; objIx < Perception::maxObjectCount; objIx++) {
+    InterestingThing *obj = objects[objIx];
+    if (obj == nullptr)
+      continue;
+
+    if (obj->confidence > 0)
+      PublishTrackedObject(roboid, obj);
+
+    n++;
+  }
+}
+
+void NetworkSync::PublishTrackedObject(Roboid *roboid,
+                                       InterestingThing *thing) {
+  if (thing == nullptr || thing->updated == false || thing->networkId != 0x00) {
+    return;
+  }
+  SwingTwist16 inv_originOrientation;
+  Spherical16 originPosition;
+  if (roboid->worldOrigin == nullptr) {
+    inv_originOrientation = SwingTwist16::identity;
+    originPosition = Spherical16::zero;
+  } else {
+    inv_originOrientation =
+        SwingTwist16::Inverse(roboid->worldOrigin->GetOrientation());
+    originPosition = roboid->worldOrigin->GetPosition();
+  }
+
+  // SwingTwist16 inv_originOrientation =
+  //     SwingTwist16::Inverse(roboid->worldOrigin->orientation);
+  // Spherical16 originPosition = roboid->worldOrigin->position;
+
+  SwingTwist16 worldOrientation =
+      inv_originOrientation * thing->GetOrientation();
+  Spherical16 worldPosition =
+      inv_originOrientation * (thing->GetPosition() - originPosition);
+
+  PoseMsg msg = PoseMsg(this->networkId, thing->id,
+                        PoseMsg::Pose_Position | PoseMsg::Pose_Orientation,
+                        worldPosition, worldOrientation);
+  SendBuffer(msg.Serialize(this->buffer));
+
+#if RC_DEBUG
+  printf("Sent Thing PoseMsg [%d/%d] %d\n", networkId, buffer[1], thing->type);
+#endif
+
+  thing->updated = false;
+}
+
+void NetworkSync::SendText(const char *s) {
+  unsigned char length;
+  for (length = 0; length < 253; length++) {
+    if (s[length] == '\0')
+      break;
+  }
+  if (length >= 253)
+    return;
+
+  unsigned char ix = 0;
+  buffer[ix++] = 0xB0;
+  buffer[ix++] = length;
+  for (int urlIx = 0; urlIx < length; urlIx++)
+    buffer[ix++] = s[urlIx];
+
+  SendBuffer(ix);
+#ifdef RC_DEBUG
+  printf("Sent Text %s\n", s);
+#endif
+}
+
+#if ESP32
+#include <Arduino.h>
+#endif
+void NetworkSync::SendInt(const int x) {
+#if ESP32
+  String s = String(x);
+  char length = s.length();
+
+  unsigned char ix = 0;
+  buffer[ix++] = 0xB0;
+  buffer[ix++] = length;
+  for (int urlIx = 0; urlIx < length; urlIx++)
+    buffer[ix++] = s[urlIx];
+
+  SendBuffer(ix);
+#endif
+}

NetworkSync.h

@@ -0,0 +1,68 @@
+#pragma once
+
+#include "ControlCore/Participant.h"
+#include "Perception.h"
+#include "Roboid.h"
+#include "Types.h"
+
+namespace Passer {
+namespace RoboidControl {
+
+/// @brief Interface for synchronizaing state between clients across a network
+class NetworkSync : public Participant {
+public:
+  NetworkSync() {};
+  NetworkSync(Roboid *roboid);
+
+  unsigned char networkId = 0;
+
+  /// @brief Retreive and send the roboid state
+  /// @param roboid The roboid for which the state is updated
+  virtual void NetworkUpdate(Roboid *roboid) = 0;
+  void SendThingInfo(Thing *thing, bool recurse = false);
+  void SendThing(Thing *thing);
+  void SendName(Thing *roboid);
+  void SendModel(Thing *thing);
+  void SendCustom(Thing *thing);
+  /// @brief Inform that the given object is no longer being tracked
+  /// @param obj
+  void SendDestroy(Thing *thing);
+
+  typedef void (*Buffer)(UInt8 *buffer, UInt16 bufferSize);
+
+  void ReceiveMessage(Roboid *roboid, unsigned char bytecount);
+
+  void PublishState(Roboid *roboid);
+
+  void PublishPerception(Roboid *roboid);
+  void PublishTrackedObjects(Roboid *roboid, InterestingThing **objects);
+
+  virtual void SendPosition(Spherical16 worldPosition) {};
+  virtual void SendPose(Spherical16 worldPosition,
+                        SwingTwist16 worldOrientation) {};
+  void SendPose(Thing *thing, bool force = 0, bool recurse = true);
+
+  virtual void SendText(const char *s);
+  void SendInt(const int x);
+
+  virtual void Download(const char *url) {};
+
+protected:
+  Roboid *roboid;
+
+  virtual void ProcessNetworkIdMsg(Passer::Control::NetworkIdMsg msg) override;
+  virtual void ProcessInvestigateMsg(InvestigateMsg msg) override;
+  virtual void ProcessThingMsg(ThingMsg msg) override;
+  virtual void ProcessPoseMsg(PoseMsg msg) override;
+  virtual void ProcessCustomMsg(CustomMsg msg) override;
+
+  void ReceiveNetworkId();
+  void ReceiveCustom(unsigned char packetSize);
+
+  void PublishTrackedObject(Roboid *roboid, InterestingThing *object);
+
+  void PublishClient();
+};
+
+} // namespace RoboidControl
+} // namespace Passer

Perception.cpp

@@ -0,0 +1,576 @@
+#include "Perception.h"
+#include "ControlCore/LinearAlgebra/Angle.h"
+#include "ControlCore/Thing.h"
+#include "DistanceSensor.h"
+#include "NetworkSync.h"
+#include "Switch.h"
+
+#include <math.h>
+
+// #define RC_DEBUG
+#ifdef RC_DEBUG
+#include <Arduino.h>
+#endif
+
+unsigned char Perception::maxObjectCount = 7; // 7 is typically the maximum
+                                              // number of object which can
+                                              // be tracked by a human
+
+Perception::Perception() {
+  this->sensorCount = 0;
+  this->sensors = new Sensor *[0];
+
+  this->trackedObjects = new InterestingThing *[maxObjectCount];
+  for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++)
+    this->trackedObjects[objIx] = nullptr;
+}
+
+Perception::Perception(Sensor **sensors, unsigned int sensorCount)
+    : Perception() {
+  this->sensorCount = sensorCount;
+  this->sensors = new Sensor *[this->sensorCount];
+  for (unsigned char sensorIx = 0; sensorIx < this->sensorCount; sensorIx++)
+    this->sensors[sensorIx] = sensors[sensorIx];
+
+  this->trackedObjects = new InterestingThing *[maxObjectCount];
+  for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++)
+    this->trackedObjects[objIx] = nullptr;
+}
+
+unsigned int Perception::GetSensorCount() { return this->sensorCount; }
+
+Sensor *Perception::GetSensor(unsigned int sensorId) {
+  if (sensorId >= this->sensorCount)
+    return nullptr;
+
+  Sensor *sensor = this->sensors[sensorId];
+  return sensor;
+}
+
+unsigned int Perception::AddSensor(Sensor *newSensor) {
+  unsigned int newSensorCount = this->sensorCount + 1;
+  Sensor **newSensors = new Sensor *[newSensorCount];
+  for (unsigned char sensorIx = 0; sensorIx < this->sensorCount; sensorIx++)
+    newSensors[sensorIx] = sensors[sensorIx];
+
+  unsigned int sensorId = this->sensorCount;
+  newSensors[sensorId] = newSensor;
+
+  Sensor **oldSensors = this->sensors;
+  this->sensors = newSensors;
+  this->sensorCount = newSensorCount;
+  delete[] oldSensors;
+  return sensorId;
+}
+
+Sensor *Perception::FindSensorOfType(unsigned int sensorType) {
+  for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) {
+    Sensor *sensor = this->sensors[sensorIx];
+    if (sensor->type == sensorType)
+      return sensor;
+  }
+
+  return nullptr;
+}
+
+float GetPlaneDistance(InterestingThing *plane, float horizontalAngle,
+                       float range) {
+  float distance = plane->GetPosition().distance;
+  float deltaAngle =
+      AngleSingle::Normalize(
+          AngleSingle::Degrees(
+              plane->GetPosition().direction.horizontal.InDegrees() -
+              horizontalAngle))
+          .InDegrees();
+  if (fabsf(deltaAngle) < fabsf(range)) {
+    // distance = distance
+    // printf("   plane distance = %f (%f-%f)+%f=%f", distance,
+    //        (float)plane->position.horizontalAngle, horizontalAngle, range,
+    //        deltaAngle);
+
+  } else if (deltaAngle < -range) {
+    float angle = deltaAngle + range;
+    // printf("   plane distance < %f (%f-%f)+%f=%f %f", distance,
+    //        (float)plane->position.horizontalAngle, horizontalAngle, range,
+    //        angle, cosf(angle * Angle::Deg2Rad));
+    if (angle > -90)
+      distance = distance / cosf(angle * Passer::LinearAlgebra::Deg2Rad);
+    else
+      distance = 9999; // infinity?
+
+  } else if (deltaAngle > range) {
+    float angle = deltaAngle - range;
+    // printf("   plane distance > %f (%f-%f)-%f=%f %f", distance,
+    //        (float)plane->position.horizontalAngle, horizontalAngle, range,
+    //        angle, cosf(angle * Angle::Deg2Rad));
+    if (angle < 90)
+      distance = distance / cosf(angle * Passer::LinearAlgebra::Deg2Rad);
+    else
+      distance = 9999; // infinity?
+  }
+  // printf("    distance = %f\n", distance);
+  return distance;
+}
+
+float Perception::GetDistance(float horizontalDirection, float range) {
+  float minDistance = INFINITY;
+  if (range < 0)
+    range = -range;
+
+  for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
+    InterestingThing *obj = trackedObjects[objIx];
+    if (obj == nullptr)
+      continue;
+
+    if (obj->type == 0x080) { // plane
+      float planeDistance = GetPlaneDistance(obj, horizontalDirection, range);
+      minDistance = fminf(minDistance, planeDistance);
+    } else if (obj->GetPosition().direction.horizontal.InDegrees() >
+                   horizontalDirection - range &&
+               obj->GetPosition().direction.horizontal.InDegrees() <
+                   horizontalDirection + range) {
+      minDistance = fminf(minDistance, obj->GetPosition().distance);
+    }
+  }
+  return minDistance;
+}
+
+float Perception::GetDistanceOfType(unsigned char thingType,
+                                    float horizontalAngle, float range) {
+  float minDistance = INFINITY;
+  if (range < 0)
+    range = -range;
+
+  for (unsigned char thingIx = 0; thingIx < maxObjectCount; thingIx++) {
+    InterestingThing *thing = trackedObjects[thingIx];
+    if (thing == nullptr)
+      continue;
+    if (thing->type != thingType)
+      continue;
+
+    if (thing->type == 0x080) { // plane
+      float planeDistance = GetPlaneDistance(thing, horizontalAngle, range);
+      minDistance = fminf(minDistance, planeDistance);
+    } else if (thing->GetPosition().direction.horizontal.InDegrees() >
+                   horizontalAngle - range &&
+               thing->GetPosition().direction.horizontal.InDegrees() <
+                   horizontalAngle + range) {
+      minDistance = fminf(minDistance, thing->GetPosition().distance);
+    }
+  }
+  return minDistance;
+}
+
+float Perception::GetDistance(float horizontalDirection,
+                              float verticalDirection, float range) {
+  float minDistance = INFINITY;
+  if (range < 0)
+    range = -range;
+
+  for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
+    InterestingThing *obj = trackedObjects[objIx];
+    if (obj == nullptr)
+      continue;
+    if (obj->GetPosition().direction.horizontal.InDegrees() >
+            horizontalDirection - range &&
+        obj->GetPosition().direction.horizontal.InDegrees() <
+            horizontalDirection + range) {
+      minDistance = fminf(minDistance, obj->GetPosition().distance);
+    }
+  }
+
+  return minDistance;
+}
+bool Perception::ObjectNearby(float direction, float range) {
+  if (range < 0)
+    range = -range;
+
+  for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
+    InterestingThing *obj = trackedObjects[objIx];
+    if (obj == nullptr)
+      continue;
+
+    if (obj->GetPosition().direction.horizontal.InDegrees() >
+            direction - range &&
+        obj->GetPosition().direction.horizontal.InDegrees() <
+            direction + range) {
+      if (obj->GetPosition().distance <= nearbyDistance)
+        return true;
+    }
+  }
+  return false;
+}
+
+InterestingThing *
+Perception::AddTrackedObject(Sensor *sensor, Spherical16 position,
+                             SwingTwist16 orientation, unsigned char thingType,
+                             unsigned char thingId, unsigned char networkId) {
+  InterestingThing *thing = new InterestingThing(sensor, position, orientation);
+  if (thingId != 0x00)
+    thing->id = thingId;
+  thing->type = thingType;
+
+  unsigned char farthestObjIx = 0;
+  unsigned char availableSlotIx = 0;
+  for (unsigned char thingIx = 0; thingIx < maxObjectCount; thingIx++) {
+    if (this->trackedObjects[thingIx] == nullptr) {
+      availableSlotIx = thingIx;
+    }
+    // Do we see the same object?
+    else {
+      if (thing->IsTheSameAs(this->trackedObjects[thingIx])) {
+        this->trackedObjects[thingIx]->Refresh(
+            thing->GetPosition(), thing->GetOrientation()); //.ToQuaternion());
+        delete thing;
+
+        return this->trackedObjects[thingIx];
+      }
+      // Is this the fartest object we see?
+      else if (this->trackedObjects[farthestObjIx] == nullptr ||
+               (this->trackedObjects[thingIx]->GetPosition().distance >
+                this->trackedObjects[farthestObjIx]->GetPosition().distance)) {
+        farthestObjIx = thingIx;
+      }
+    }
+  }
+
+  // Check if an perception slot is available (we currently see less than the
+  // max number of objects)
+  if (availableSlotIx < maxObjectCount) {
+    // a slot is available
+    this->trackedObjects[availableSlotIx] = thing;
+    thing->networkId = networkId;
+    if (thingId == 0x00)
+      thing->id = lastObjectId++; // availableSlotIx + 1;
+    return thing;
+  }
+  // If this object is closer than the farthest object, then replace it
+  else if (thing->GetPosition().distance <
+           this->trackedObjects[farthestObjIx]->GetPosition().distance) {
+    delete this->trackedObjects[farthestObjIx];
+    this->trackedObjects[farthestObjIx] = thing;
+    thing->networkId = networkId;
+    if (thingId == 0x00)
+      thing->id = lastObjectId++; // availableSlotIx + 1;
+    return thing;
+  } else {
+    // No available slot, delete trackedobject
+    delete thing;
+    return nullptr;
+  }
+}
+
+InterestingThing *Perception::AddTrackedObject(Sensor *sensor,
+                                               unsigned char networkId,
+                                               unsigned char thingId,
+                                               Spherical16 position,
+                                               SwingTwist16 orientation) {
+  InterestingThing *thing = FindTrackedObject(networkId, thingId);
+  if (thing == nullptr) {
+    thing = AddTrackedObject(sensor, position, orientation, 0xFF, thingId,
+                             networkId);
+    // Don't we set this above already?
+    thing->networkId = networkId;
+    thing->id = thingId;
+    thing->type = 0xFF; // unknown
+  }
+  return thing;
+}
+
+InterestingThing *Perception::AddTrackedObject(Sensor *sensor,
+                                               Thing *orgThing) {
+  InterestingThing *thing = new InterestingThing(
+      sensor, orgThing->GetPosition(), orgThing->GetOrientation());
+  thing->id = orgThing->id;
+  thing->networkId = 0;
+  thing->type = orgThing->type;
+  thing->name = orgThing->name;
+  thing->modelUrl = orgThing->modelUrl;
+  thing->modelScale = orgThing->modelScale;
+
+  unsigned char farthestObjIx = 0;
+  unsigned char availableSlotIx = 0;
+  for (unsigned char thingIx = 0; thingIx < maxObjectCount; thingIx++) {
+    if (this->trackedObjects[thingIx] == nullptr) {
+      availableSlotIx = thingIx;
+    }
+    // Do we see the same object?
+    else {
+      if (thing->IsTheSameAs(this->trackedObjects[thingIx])) {
+        this->trackedObjects[thingIx]->Refresh(
+            thing->GetPosition(), thing->GetOrientation()); //.ToQuaternion());
+        delete thing;
+
+        return this->trackedObjects[thingIx];
+      }
+      // Is this the fartest object we see?
+      else if (this->trackedObjects[farthestObjIx] == nullptr ||
+               (this->trackedObjects[thingIx]->GetPosition().distance >
+                this->trackedObjects[farthestObjIx]->GetPosition().distance)) {
+        farthestObjIx = thingIx;
+      }
+    }
+  }
+
+  // Check if an perception slot is available (we currently see less than the
+  // max number of objects)
+  if (availableSlotIx < maxObjectCount) {
+    // a slot is available
+    this->trackedObjects[availableSlotIx] = thing;
+    if (thing->id == 0x00)
+      thing->id = lastObjectId++; // availableSlotIx + 1;
+    return thing;
+  }
+  // If this object is closer than the farthest object, then replace it
+  else if (thing->GetPosition().distance <
+           this->trackedObjects[farthestObjIx]->GetPosition().distance) {
+    delete this->trackedObjects[farthestObjIx];
+    this->trackedObjects[farthestObjIx] = thing;
+    if (thing->id == 0x00)
+      thing->id = lastObjectId++; // availableSlotIx + 1;
+    return thing;
+  } else {
+    return nullptr;
+  }
+}
+
+bool Perception::IsInteresting(float distance) {
+  for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
+    InterestingThing *thing = this->trackedObjects[objIx];
+    if (thing == nullptr)
+      return true;
+    if (thing->GetPosition().distance > distance)
+      return true;
+  }
+  return false;
+}
+
+InterestingThing *Perception::FindTrackedObject(char objectId) {
+  for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
+    InterestingThing *thing = this->trackedObjects[objIx];
+    if (thing == nullptr)
+      continue;
+    if (thing->id == objectId) {
+      return thing;
+    }
+  }
+  return nullptr;
+}
+
+InterestingThing *Perception::FindTrackedObject(unsigned char networkId,
+                                                unsigned char objectId) {
+  for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
+    InterestingThing *thing = this->trackedObjects[objIx];
+    if (thing == nullptr)
+      continue;
+
+    if (thing->networkId == networkId && thing->id == objectId) {
+      return thing;
+    }
+  }
+  return nullptr;
+}
+
+unsigned char Perception::TrackedObjectCount() {
+  unsigned char objectCount = 0;
+  for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
+    if (this->trackedObjects[objIx] != nullptr)
+      objectCount++;
+  }
+  return objectCount;
+}
+
+InterestingThing **Perception::GetTrackedObjects() {
+  return this->trackedObjects;
+}
+
+unsigned char Perception::ThingsOfType(unsigned char thingType,
+                                       InterestingThing *buffer[],
+                                       unsigned char bufferSize) {
+  unsigned char thingCount = 0;
+  for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
+    InterestingThing *thing = this->trackedObjects[objIx];
+    if (thing == nullptr)
+      continue;
+
+    // printf("[%d/%d]%d  ", thing->networkId, thing->id, thing->type);
+    if (thing->type == thingType) {
+      buffer[thingCount] = thing;
+      thingCount++;
+      if (thingCount >= bufferSize) {
+        // printf("\n");
+        return bufferSize;
+      }
+    }
+  }
+  // printf("\n");
+  return thingCount;
+}
+
+InterestingThing *Perception::ThingOfType(unsigned char thingType) {
+  for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
+    InterestingThing *thing = this->trackedObjects[objIx];
+    if (thing == nullptr)
+      continue;
+
+    if (thing->type == thingType)
+      return thing;
+  }
+  return nullptr;
+}
+
+InterestingThing *Perception::GetMostInterestingThing() {
+  if (this->trackedObjects == nullptr)
+    return nullptr;
+
+  InterestingThing *closestObject = nullptr;
+  float closestDistance = INFINITY;
+  for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
+    InterestingThing *obj = this->trackedObjects[objIx];
+    if (obj != nullptr) {
+      if (obj->GetPosition().distance < closestDistance) {
+        closestObject = obj;
+        closestDistance = obj->GetPosition().distance;
+      }
+    }
+  }
+  return closestObject;
+}
+
+void Perception::Update(unsigned long currentTimeMs) {
+  float deltaTime = (float)(currentTimeMs - lastUpdateTimeMs) / 1000.0f;
+  if (deltaTime <= 0)
+    return;
+
+  lastUpdateTimeMs = currentTimeMs;
+
+  // Update sensing
+  for (unsigned int sensorIx = 0; sensorIx < this->sensorCount; sensorIx++) {
+    Sensor *sensor = sensors[sensorIx];
+    if (sensor == nullptr)
+      continue;
+
+    if (sensor->type == (unsigned char)Thing::Type::DistanceSensor) {
+      DistanceSensor *distanceSensor = (DistanceSensor *)sensor;
+
+      float distance = distanceSensor->GetDistance();
+      if (distance >= 0) {
+        // Angle16 angle = sensor->position.horizontal;
+        //  Polar position = Polar(angle, distance);
+        //  Polar position = Polar(distance, angle.ToFloat());
+        //  AddTrackedObject(distanceSensor, position);
+        Spherical16 position =
+            Spherical16(distance, sensor->GetPosition().direction);
+        AddTrackedObject(distanceSensor, position);
+      }
+
+    } else if (sensor->type == (unsigned char)Thing::Type::Switch) {
+      Switch *switchSensor = (Switch *)sensor;
+      if (switchSensor->IsOn()) {
+        // Polar position = Polar(sensor->position.angle, nearbyDistance);
+        AngleSingle horizontal = AngleSingle::Degrees(horizontal.InDegrees());
+        PolarSingle position = PolarSingle(nearbyDistance, horizontal);
+        // AddTrackedObject(switchSensor, position);
+      }
+    } else {
+      sensor->Update(currentTimeMs);
+    }
+  }
+
+  for (unsigned char objIx = 0; objIx < maxObjectCount; objIx++) {
+    InterestingThing *thing = trackedObjects[objIx];
+    if (thing == nullptr)
+      continue;
+
+    if (thing->DegradeConfidence(deltaTime) == false) {
+      // delete obj
+      if (roboid != nullptr && roboid->networkSync != nullptr) {
+        // roboid->networkSync->SendDestroy((Passer::Control::Thing *)thing);
+      }
+      this->trackedObjects[objIx] = nullptr;
+      delete thing;
+    }
+  }
+}
+
+void Perception::UpdatePose(Polar16 translation) {
+  for (unsigned char thingIx = 0; thingIx < maxObjectCount; thingIx++) {
+    InterestingThing *thing = trackedObjects[thingIx];
+    if (thing == nullptr)
+      continue;
+
+    // We only support translations in the horizontal plane at this moment...
+    // This needs Spherical operator- to be implemented to work in 3d space
+    if (thing->type == 0x80) { // plane
+      // printf("[1/%d] %f (%f %f) ", thing->id, thing->position.distance,
+      //        (float)thing->position.horizontalAngle,
+      //        (float)thing->position.verticalAngle);
+      // Update the closest point to the plane
+      float angle =
+          (float)thing->GetPosition().direction.horizontal.InDegrees() +
+          translation.angle.InDegrees();
+      angle = fabsf(angle);
+
+      float deltaDistance =
+          translation.distance * cosf(angle * Passer::LinearAlgebra::Deg2Rad);
+      // printf(" | translate %f %f %f | ", (float)translation.distance,
+      //        (float)angle, deltaDistance);
+      Spherical16 position = thing->GetPosition();
+      position.distance -= deltaDistance;
+      thing->SetPosition(position); //.distance -= deltaDistance;
+      // printf("-> %f (%f %f)\n", thing->position.distance,
+      //        (float)thing->position.horizontalAngle,
+      //        (float)thing->position.verticalAngle);
+    } else {
+      // Polar horizontalPosition = Polar(thing->position);
+      // // obj->position.ProjectOnHorizontalPlane();
+      // Spherical16 newPosition = Spherical16(horizontalPosition -
+      // translation);
+
+      Spherical16 translationS = Spherical16(
+          translation.distance, Angle16::Degrees(translation.angle.InDegrees()),
+          Angle16::Degrees(0));
+      Spherical16 newPosition = thing->GetPosition() + translationS;
+
+      Vector3 oldPos = thing->GetPosition().ToVector3();
+      Vector3 newPos = newPosition.ToVector3();
+      // printf(" update percepted position (%f 0 %f) -> (%f 0 %f)\n",
+      //        oldPos.Right(), oldPos.Forward(), newPos.Right(),
+      //        newPos.Forward());
+
+      thing->SetPosition(newPosition);
+    }
+  }
+}
+
+void Perception::UpdatePose(SwingTwist16 rotation) {
+  // only rotation around vertical axis is supported for now
+  // float rotationAngle;
+  // Vector3 rotationAxis;
+  // rotation.ToAngleAxis(&rotationAngle, &rotationAxis);
+  // Make sure rotation axis is positive
+  // if (rotationAxis.Up() < 0)
+  //   rotationAngle = -rotationAngle;
+  Angle16 rotationAngle = rotation.swing.horizontal;
+
+  for (unsigned char thingIx = 0; thingIx < maxObjectCount; thingIx++) {
+    InterestingThing *thing = trackedObjects[thingIx];
+    if (thing == nullptr)
+      continue;
+
+    // printf("[1/%d] %f (%f %f) ", thing->id, thing->position.distance,
+    //        (float)thing->position.horizontalAngle,
+    //        (float)thing->position.verticalAngle);
+    // printf("| rotate %f  | ", rotationAngle);
+
+    Spherical16 newPosition = thing->GetPosition();
+    newPosition.direction.horizontal - rotationAngle;
+    thing->SetPosition(newPosition);
+    // thing->position.direction.horizontal =
+    //     thing->position.direction.horizontal - rotationAngle;
+
+    // printf("-> %f (%f %f) \n", thing->position.distance,
+    //        (float)thing->position.horizontalAngle,
+    //        (float)thing->position.verticalAngle);
+  }
+}

Perception.h

@@ -0,0 +1,177 @@
+#pragma once
+
+#include "ControlCore/LinearAlgebra/Polar.h"
+#include "ControlCore/LinearAlgebra/Quaternion.h"
+#include "ControlCore/LinearAlgebra/Spherical.h"
+#include "Sensor.h"
+#include "TrackedObject.h"
+
+// #include <vector.h>
+
+namespace Passer {
+namespace RoboidControl {
+
+class Roboid;
+
+/// @brief Module to which keeps track of objects around the roboid
+class Perception {
+public:
+  /// @brief Default Constructor
+  Perception();
+
+  /// @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(Sensor **sensors, unsigned int sensorCount);
+
+  /// @brief The roboid of this perception system
+  Roboid *roboid = nullptr;
+
+  unsigned int AddSensor(Sensor *sensor);
+
+  /// @brief Get the number of Sensors
+  /// @return The number of sensors, zero when no sensors are present
+  unsigned int GetSensorCount();
+  Sensor *GetSensor(unsigned int sensorId);
+  /// @brief Find the first sensor of the given type
+  /// @param sensorType The type of sensor as is defined in the Thing class (for
+  /// example Thing::SensorType)
+  /// @return The first sensor found or a nullptr which no sensor has been found
+  /// of the given type
+  Sensor *FindSensorOfType(unsigned int sensorType);
+
+  /// @brief Gets the distance to the closest object
+  /// @param direction The direction to look for objects
+  /// @param range The range in which objects should be looked for
+  /// @return The distance to the closest object in meters
+  float GetDistance(float direction, float range = 10.0F);
+  float GetDistanceOfType(unsigned char thingType, float horizontalAngle,
+                          float range = 10.0F);
+  /// @brief Gets the distance to the closest object
+  /// @param horizontalDirection The direction in the horizontal plane to look
+  /// for objects
+  /// @param verticalDirection The direction in the vertical plane to look for
+  /// objects
+  /// @param range The range in which objects should be looked for
+  /// @return The distance to the closest object in meters
+  /// @details The directions can be thought of as the polar angle (vertical)
+  /// and azimuthal angle (horizontal) in the spherical coordinate system.
+  float GetDistance(float horizontalDirection, float verticalDirection,
+                    float range = 10.0F);
+
+  /// @brief Checks if an object is nearby
+  /// @param direction The direction to look for objects
+  /// @param range The range in which objects should be looked for
+  /// @return True when an object is close, False otherwise
+  /// @details Wether an object is closeby depends on the sensor. This can be a
+  /// sensor like a Switch or a DistanceSensor. The latter uses the
+  /// DistanceSensor::triggerDistance to check if an object is nearby.
+  bool ObjectNearby(float direction, float range = 10.0F);
+  /// @brief Checks if an object is nearby
+  /// @param horizontalDirection The direction in the horizontal plane to look
+  /// for objects
+  /// @param verticalDirection The direction in the vertical plane to look for
+  /// objects
+  /// @param range The range in which objects should be looked for
+  /// @return True when an object is close, False otherwise
+  /// @details Wether an object is closeby depends on the sensor. This can be a
+  /// sensor like a Switch or a DistanceSensor. The latter uses the
+  /// DistanceSensor::triggerDistance to check if an object is nearby.
+  ///
+  /// The directions can be thought of as the polar angle (vertical) and
+  /// azimuthal angle (horizontal) in the spherical coordinate system.
+  // bool ObjectNearby(float horizontalDirection, float verticalDirection,
+  //                   float range = 10.0F);
+
+  /// @brief Add or update an object detected by the given sensor
+  /// @param sensor The sensor which has detected the object
+  /// @param position The position of the sensor in polar coordinates local to
+  /// the roboid
+  //   void AddTrackedObject(Sensor* sensor,
+  //                         Spherical16 position,
+  //                         unsigned char objectType = 0x00,
+  //                         unsigned char networkId = 0x00);
+  InterestingThing *
+  AddTrackedObject(Sensor *sensor, Spherical16 position,
+                   SwingTwist16 orientation = SwingTwist16::identity,
+                   unsigned char objectType = 0xFF,
+                   unsigned char objectId = 0x00,
+                   unsigned char networkId = 0x00);
+
+  InterestingThing *
+  AddTrackedObject(Sensor *sensor, unsigned char networkId,
+                   unsigned char objectId, Spherical16 position,
+                   SwingTwist16 orientation = SwingTwist16::identity);
+  InterestingThing *AddTrackedObject(Sensor *sensor, Thing *thing);
+
+  bool IsInteresting(float distance);
+
+  InterestingThing *FindTrackedObject(char objectId);
+  InterestingThing *FindTrackedObject(unsigned char networkId,
+                                      unsigned char objectId);
+
+  /// @brief Retrieve the number of objects currently being tracked by the
+  /// roboid
+  /// @return The object of objects, which is always lower than
+  /// maxObjectCount
+  unsigned char TrackedObjectCount();
+  /// @brief Retreive the objects currently tracked by the roboid
+  /// @return An array of current objects
+  /// @details The returned array this should never be a nullptr, but
+  /// each array entry may be a nullptr when less than maxObjectCount objects is
+  /// currently being tracked.
+  InterestingThing **GetTrackedObjects();
+
+  unsigned char ThingsOfType(unsigned char objectType,
+                             InterestingThing *buffer[],
+                             unsigned char bufferSize);
+  InterestingThing *ThingOfType(unsigned char objectType);
+
+  InterestingThing *GetMostInterestingThing();
+
+  // mainly used for confidence update
+
+  /// @brief Update the state of the perception.
+  /// @param currentTimeMs The current time in milliseconds
+  /// @details This will update the perceptoin of object. It will retrieve the
+  /// latest state for each sensor and update the confidence of the tracked
+  /// objects.
+  void Update(unsigned long currentTimeMs);
+
+  /// @brief Update the position/orientation of the preceived objects from the
+  /// given roboid translation
+  /// @param translation The translation of the roboid in world space in polar
+  /// coordinates
+  /// @details This function will be called through Roboid::SetPosition. It
+  /// is advised to use that function to update the roboid position instead of
+  /// this function.
+  void UpdatePose(Polar16 translation);
+  /// @brief Update the orientation of the perceived objecst from the given
+  /// roboid rotation
+  /// @param rotation The rotation of the roboid in world space
+  void UpdatePose(SwingTwist16 rotation);
+
+  /// @brief Objects with a distance closed that this value will be considered
+  /// nearby.
+  /// @details This value is used by the ObjectNearby function to select the
+  /// objects
+  float nearbyDistance = 0.02F;
+
+public:
+  /// @brief The Sensors used for Perception
+  Sensor **sensors = nullptr;
+  /// @brief The number of Sensors used for Perception
+  unsigned int sensorCount = 0;
+
+  unsigned long lastUpdateTimeMs = 0;
+  unsigned char lastObjectId = 1;
+
+  static unsigned char maxObjectCount; // = 7; // 7 is typically the maximum
+                                       // number of object which can
+                                       // be tracked by a human
+  InterestingThing **trackedObjects;
+};
+
+} // namespace RoboidControl
+} // namespace Passer
+using namespace Passer::RoboidControl;

Propulsion.cpp

@@ -0,0 +1,48 @@
+#include "Propulsion.h"
+#include "Roboid.h"
+
+#include "LinearAlgebra/FloatSingle.h"
+
+Propulsion::Propulsion() {
+  this->motors = nullptr;
+  this->motorCount = 0;
+}
+
+unsigned int Propulsion::GetMotorCount() { return this->motorCount; }
+
+Motor *Propulsion::GetMotor(unsigned int motorId) {
+  if (motorId >= this->motorCount)
+    return nullptr;
+
+  Motor *motor = this->motors[motorId];
+  return motor;
+}
+
+void Propulsion::Update(unsigned long currentTimeMs) {
+  for (unsigned char motorIx = 0; motorIx < this->motorCount; motorIx++) {
+    Motor *motor = this->motors[motorIx];
+    if (motor == nullptr)
+      continue;
+
+    motor->Update(currentTimeMs);
+  }
+}
+
+void Propulsion::SetTwistSpeed(float forward, float yaw) {}
+
+void Propulsion::SetTwistSpeed(Vector2 linear, float yaw) {}
+
+void Propulsion::SetTwistSpeed(Vector3 linear, float yaw, float pitch,
+                               float roll) {}
+
+void Propulsion::SetVelocity(Spherical16 velocity) {
+  this->linearVelocity = velocity;
+}
+
+void Propulsion::SetAngularVelocity(Spherical16 velocity) {
+  this->angularVelocity = velocity;
+}
+
+Spherical16 Propulsion::GetVelocity() { return this->linearVelocity; }
+
+Spherical16 Propulsion::GetAngularVelocity() { return this->angularVelocity; }

Propulsion.h

@@ -0,0 +1,90 @@
+#pragma once
+
+#include "ControlCore/LinearAlgebra/Polar.h"
+#include "ControlCore/LinearAlgebra/Quaternion.h"
+#include "ControlCore/LinearAlgebra/Vector2.h"
+#include "Motor.h"
+
+namespace Passer {
+namespace RoboidControl {
+
+class Roboid;
+
+/// @brief The Propulsion module for a Roboid is used to move the Roboid in
+/// space
+/// @details Usually, a specific implementation of the propulsion module is used
+/// for a robot. This base class does not implement the functions to move the
+/// Roboid around.
+class Propulsion {
+public:
+  /// @brief Default Constructor for Propulsion
+  Propulsion();
+
+  /// @brief Update the propulsion state of the Roboid
+  /// @param currentTimeMs  The time in milliseconds when calling this
+  void Update(unsigned long currentTimeMs);
+
+  /// @brief Get the number of motors in this roboid
+  /// @return The number of motors. Zero when no motors are present
+  unsigned int GetMotorCount();
+  /// @brief Get a specific motor
+  /// @param motorIx The index of the motor
+  /// @return Returns the motor or a nullptr when no motor with the given index
+  /// could be found
+  Motor *GetMotor(unsigned int motorIx);
+  /// @brief Get the Placement of a specific Motor
+  /// @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);
+
+  /// @brief Sets the forward and rotation speed of a (grounded) Roboid
+  /// @param forward The target forward speed
+  /// @param yaw The target rotation speed around the vertical axis
+  /// This function is typically used for Roboid which are driving on the
+  /// ground.
+  virtual void SetTwistSpeed(float forward, float yaw = 0.0F);
+  /// @brief Sets the forward, sideward and rotation speed of a (grounded)
+  /// Roboid
+  /// @param linear The target linear (forward, sideward) speed
+  /// @param yaw The target rotation speed around the vertical axis
+  /// This function is typically used for Roboid which are driving on the ground
+  /// which have to ability to move sideward
+  virtual void SetTwistSpeed(Vector2 linear, float yaw = 0.0F);
+  /// @brief Set the target 3D linear and 3D rotation speed of a (flying) Roboid
+  /// @param linear The target linear speed
+  /// @param yaw The target rotation speed around the vertical axis
+  /// @param pitch The target rotation speed around the sideward axis
+  /// @param roll The target rotation speed around hte forward axis
+  virtual void SetTwistSpeed(Vector3 linear, float yaw = 0.0F,
+                             float pitch = 0.0F, float roll = 0.0F);
+
+  virtual void SetVelocity(Spherical16 velocity);
+  virtual void SetAngularVelocity(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 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
+  virtual Spherical16 GetAngularVelocity();
+
+  /// @brief The roboid of this propulsion system
+  Roboid *roboid = nullptr;
+
+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;
+  Motor **motors = nullptr;
+
+  Spherical16 linearVelocity = Spherical16::zero;
+  Spherical16 angularVelocity = Spherical16::zero;
+};
+
+} // namespace RoboidControl
+} // namespace Passer
+using namespace Passer::RoboidControl;

Quadcopter.cpp

@@ -0,0 +1,38 @@
+#include "Quadcopter.h"
+
+Quadcopter::Quadcopter() {}
+
+void Quadcopter::SetTwistSpeed(float forward, float yaw) {
+  this->velocity = Vector3::forward * forward;
+  this->yawSpeed = yaw;
+}
+
+void Quadcopter::SetTwistSpeed(Vector2 linear, float yaw) {
+  this->velocity = Vector3(linear.x, 0.0F, linear.y);
+  this->yawSpeed = yaw;
+}
+
+void Quadcopter::SetTwistSpeed(Vector3 velocity,
+                               float yaw,
+                               float pitch,
+                               float roll) {
+  this->velocity = velocity;
+  this->yawSpeed = yaw;
+  this->rollSpeed = roll;
+  this->pitchSpeed = pitch;
+}
+
+Vector3 Quadcopter::GetTargetVelocity() {
+  return this->velocity;
+}
+
+float Quadcopter::GetPitchSpeed() {
+  return this->pitchSpeed;
+}
+
+float Quadcopter::GetYawSpeed() {
+  return this->yawSpeed;
+}
+float Quadcopter::GetRollSpeed() {
+  return this->rollSpeed;
+}

Quadcopter.h

@@ -0,0 +1,36 @@
+#pragma once
+
+#include "ControlCore/Thing.h"
+#include "LinearAlgebra/Vector3.h"
+#include "Propulsion.h"
+
+namespace Passer {
+namespace RoboidControl {
+
+/// @brief Support for Quadcopter as a propulsion method
+/// @note This is work in progress
+class Quadcopter : public Propulsion {
+public:
+  /// @brief Default constuctor
+  Quadcopter();
+
+  virtual void SetTwistSpeed(float forward, float yaw = 0.0F) override;
+  virtual void SetTwistSpeed(Vector2 linear, float yaw = 0.0F) override;
+  virtual void SetTwistSpeed(Vector3 linear, float yaw = 0.0F,
+                             float pitch = 0.0F, float roll = 0.0F) override;
+
+  Vector3 GetTargetVelocity();
+  float GetYawSpeed();
+  float GetPitchSpeed();
+  float GetRollSpeed();
+
+protected:
+  Vector3 velocity = Vector3::zero;
+  float pitchSpeed = 0.0F;
+  float yawSpeed = 0.0F;
+  float rollSpeed = 0.0F;
+};
+
+} // namespace RoboidControl
+} // namespace Passer
+using namespace Passer::RoboidControl;

README.md

@@ -0,0 +1,19 @@
+RoboidControl is a cross platform framework to control autonomous robots. This library contains the generic functionality. Most projects will use one of the platform-specific implementations:
+* [RoboidControl for Arduino](https://gitlab.passervr.com/passer/arduino/roboidcontrol)
+
+Network Sync Protocol
+=====================
+
+The client connects to a Roboid Site using a Serial or WiFi connection.
+It will then send a 'Client (0xA0)' message to the Site
+The Site will respond with a 'NetworkId (0xA1)' message containing the NetworkId of this new client.
+Then the client can start sending normal messages.
+
+When the client sends a 'Pose (0x10)' message for a thing which is not known by the Site
+(the networkId/thingId combination is not known), the Site will send an 'Investigate (0x81)'
+message to the client.
+The client shall respond with a 'NewThing (0x80)' message containing the type of the thing.
+Optionally, the client can send a 'ModelURL (0x90)' message containing a link to the OBJ model of the thing.
+
+The same mechanism work the other way round, when the client receives a pose for an unknown object.
+The deviation is that the Site will (currently) not send 'ModelURL (0x90)' messages.

Roboid.cpp

@@ -0,0 +1,93 @@
+#include "Roboid.h"
+
+#include "LinearAlgebra/FloatSingle.h"
+#include "NetworkSync.h"
+
+#include <string.h>
+
+// #define RC_DEBUG
+
+#ifdef RC_DEBUG
+#include <Arduino.h>
+#endif
+
+Roboid::Roboid() : Thing() {
+#ifdef RC_DEBUG
+  Serial.begin(115200);
+#endif
+  this->type = (unsigned char)Type::Roboid;
+  this->perception = new Perception();
+  this->perception->roboid = this;
+  this->propulsion = new Propulsion();
+  this->networkSync = nullptr;
+  this->position = Spherical16::zero;
+  this->orientation = SwingTwist16::identity;
+  this->lastUpdateTimeMs = 0;
+}
+
+Roboid::Roboid(Propulsion *propulsion) : Roboid() {
+  this->propulsion = propulsion;
+  if (propulsion != nullptr)
+    propulsion->roboid = this;
+}
+
+void Roboid::Update(unsigned long currentTimeMs) {
+  if (this->lastUpdateTimeMs == 0)
+    this->lastUpdateTimeMs = currentTimeMs;
+
+  // if (perception != nullptr)
+  //   perception->Update(currentTimeMs);
+
+  if (propulsion != nullptr) {
+    propulsion->Update(currentTimeMs);
+
+    float deltaTime = (float)(currentTimeMs - lastUpdateTimeMs) / 1000;
+
+    this->angularVelocity = this->propulsion->GetAngularVelocity();
+    SwingTwist16 rotation =
+        SwingTwist16::AngleAxis(this->angularVelocity.distance * deltaTime,
+                                this->angularVelocity.direction);
+    SetOrientation(this->orientation * rotation);
+
+    this->linearVelocity = this->propulsion->GetVelocity();
+    Spherical16 translation =
+        this->orientation * this->linearVelocity * deltaTime;
+    SetPosition(this->position + translation);
+  }
+
+  if (children != nullptr) {
+    for (unsigned char childIx = 0; childIx < this->childCount; childIx++)
+      children[childIx]->Update(currentTimeMs);
+  }
+
+  if (networkSync != nullptr)
+    networkSync->NetworkUpdate(this);
+
+  lastUpdateTimeMs = currentTimeMs;
+}
+
+// void Roboid::AddChild(Thing *child) {
+//   std::cout << "Roboid add child";
+//   Thing::AddChild(child);
+//   if (child->IsSensor()) {
+//     Sensor *childSensor = (Sensor *)child;
+//     this->perception->AddSensor(childSensor);
+//   }
+// }
+
+void Passer::RoboidControl::Roboid::Release(Thing *child) {
+  if (RemoveChild(child) != nullptr) {
+    child->SetPosition(this->position);
+    child->SetOrientation(this->orientation);
+    // this creates an new thing, I wish I could avoid this.
+    this->perception->AddTrackedObject(nullptr, child);
+  }
+}
+
+void Roboid::LoadModel(const char *url) {
+  this->modelUrl = url;
+  if (this->networkSync == nullptr)
+    return;
+
+  this->networkSync->Download(url);
+}

Roboid.h

@@ -0,0 +1,85 @@
+#pragma once
+
+#include "Perception.h"
+#include "Propulsion.h"
+#include "ServoMotor.h"
+
+namespace Passer {
+namespace RoboidControl {
+
+class NetworkSync;
+
+/// @brief A Roboid is used to control autonomous robots
+class Roboid : public Thing {
+public:
+  /// @brief Default constructor for a Roboid
+  Roboid();
+  /// @brief Creates a Roboid with Perception and Propulsion abilities
+  /// @param perception The Perception implementation to use for this Roboid
+  /// @param propulsion The Propulsion implementation to use for this Roboid
+  Roboid(Propulsion *propulsion);
+
+  /// @brief Update the state of the Roboid
+  /// @param currentTimeMs The time in milliseconds when calling this
+  /// function
+  void Update(unsigned long currentTimeMs);
+
+  /// @brief The Perception module of this Roboid
+  Perception *perception = nullptr;
+  /// @brief The Propulsion module of this Roboid
+  Propulsion *propulsion = nullptr;
+  // ServoMotor *actuation = nullptr;
+
+  /// @brief The reference to the module to synchronize states across a network
+  NetworkSync *networkSync = nullptr;
+
+  /// @brief Retrieve the current position of the roboid
+  /// @return The position in carthesian coordinates in world space
+  /// @details The origin and units of the position depends on the position
+  /// tracking system used. This value will be Vector3::zero unless a position
+  /// is received through network synchronisation
+  // virtual Spherical16 GetPosition();
+  // Vector2 GetPosition2D();
+  /// @brief Retrieve the current orientation of the roboid
+  /// @return The orientation quaternion in world space
+  /// @details The origin orientation depends on the position tracking system
+  /// used. This value will be Quaternion::identity unless an orientation is
+  /// received though network synchronization
+  // virtual SwingTwist16 GetOrientation();
+
+  /// @brief Update the current position of the roboid
+  /// @param worldPosition The position of the roboid in carthesian coordinates
+  /// in world space
+  /// @details The use of this function will also update the positions and
+  /// orientations of the perceived objects by the roboid
+  /// (roboid->perception->perceivedObjects), as these are local to the
+  /// roboid's position.
+  // virtual void SetPosition(Spherical16 worldPosition);
+  /// @brief Update the current orientation of the roboid
+  /// @param worldOrientation The orientation of the roboid in world space
+  /// @details The use of this function will also update the orientations of the
+  /// perceived objects by the roboid (roboid->perception->perceivedObjets),
+  /// as these are local to the roboid' orientation.
+  // virtual void SetOrientation(SwingTwist16 worldOrientation);
+
+  // virtual void AddChild(Thing *child) override;
+  void Release(Thing *child);
+
+  Thing *worldOrigin =
+      nullptr; // thing to track the world origin to be able to transform
+               // world coordinates into roboid or local coordinates
+               // Perhaps this will move to perception at some point
+
+  /// @brief Loads the model and adds the skeleton (if any) to the roboid
+  /// @param url The url of the model
+  /// @remark The only supported model format is .gltf
+  void LoadModel(const char *url);
+
+private:
+  unsigned long lastUpdateTimeMs = 0;
+};
+
+} // namespace RoboidControl
+} // namespace Passer
+
+using namespace Passer::RoboidControl;

RoboidControl.code-workspace

@@ -0,0 +1,8 @@
+{
+	"folders": [
+		{
+			"path": "."
+		}
+	],
+	"settings": {}
+}

Sensor.cpp

@@ -0,0 +1,42 @@
+#include "Sensor.h"
+
+#include "Roboid.h"
+
+Sensor::Sensor() : Thing() {
+  // this->type = (unsigned char)Type::Sensor;
+}
+
+// void Sensor::SetParent(Thing *parent) {
+//   this->parent = parent;
+//   if (this->parent != nullptr &&
+//       this->parent->GetParent() == nullptr) { // Is the parent a root object?
+//                                               // Then it is a roboid
+//     Roboid *roboidParent = (Roboid *)this->parent;
+//     roboidParent->perception->AddSensor(this);
+//   }
+// }
+
+void Sensor::ConnectTo(Thing *oldThing) {
+  this->name = oldThing->name;
+  this->id = oldThing->id;
+
+  Thing *oldParent = oldThing->GetParent();
+
+  oldParent->RemoveChild(oldThing);
+  oldParent->AddChild(this);
+
+  for (int childIx = 0; childIx < oldThing->childCount; childIx++) {
+    Thing *child = oldThing->GetChildByIndex(childIx);
+    this->AddChild(child);
+  }
+
+  this->position = oldThing->GetPosition();
+  this->orientation = oldThing->GetOrientation();
+  // delete (thing); // can we do this?
+}
+
+void Sensor::ConnectTo(Thing *rootThing, const char *thingName) {
+  Thing *thing = rootThing->FindThing(thingName);
+  if (thing != nullptr)
+    this->ConnectTo(thing);
+}