From da8831a9685c7b0c14968f21beb3c0ac058796a9 Mon Sep 17 00:00:00 2001
From: Pascal Serrarens <pse@pale.blue>
Date: Mon, 19 May 2025 08:55:00 +0200
Subject: [PATCH] Added controlledMotor, ptr to ref for hierarchy

---
 Arduino/Things/DRV8833.cpp          | 10 ++++--
 Arduino/Things/DRV8833.h            |  4 +--
 Arduino/Things/DigitalInput.cpp     | 21 +++++++-----
 Arduino/Things/DigitalInput.h       |  7 ++--
 Arduino/Things/UltrasonicSensor.cpp | 22 +++++++-----
 Arduino/Things/UltrasonicSensor.h   |  4 +--
 Posix/PosixParticipant.h            |  2 ++
 Things/ControlledMotor.cpp          | 50 +++++++++++++++++++++++++++
 Things/ControlledMotor.h            | 52 +++++++++++++++++++++++++++++
 Things/DifferentialDrive.cpp        | 12 ++++---
 Things/Motor.cpp                    |  4 +--
 Things/Motor.h                      |  4 +--
 Things/RelativeEncoder.cpp          |  4 +--
 Things/RelativeEncoder.h            |  2 +-
 14 files changed, 160 insertions(+), 38 deletions(-)
 create mode 100644 Things/ControlledMotor.cpp
 create mode 100644 Things/ControlledMotor.h

diff --git a/Arduino/Things/DRV8833.cpp b/Arduino/Things/DRV8833.cpp
index 80fc402..373bce1 100644
--- a/Arduino/Things/DRV8833.cpp
+++ b/Arduino/Things/DRV8833.cpp
@@ -5,6 +5,8 @@
 namespace RoboidControl {
 namespace Arduino {
 
+#pragma region DRV8833
+
 DRV8833::DRV8833(Configuration config, Participant* participant)
     : Thing(participant) {
   this->pinStandby = pinStandby;
@@ -22,6 +24,8 @@ DRV8833::DRV8833(Configuration config, Thing* parent)
   this->SetParent(parent);
 }
 
+#pragma endregion DRV8833
+
 #pragma region Differential drive
 
 DRV8833::DifferentialDrive::DifferentialDrive(DRV8833::Configuration config,
@@ -32,7 +36,7 @@ DRV8833::DifferentialDrive::DifferentialDrive(DRV8833::Configuration config,
 }
 
 DRV8833::DifferentialDrive::DifferentialDrive(DRV8833::Configuration config,
-                                              Thing* parent)
+                                              Thing& parent)
     : RoboidControl::DifferentialDrive(parent), drv8833(config) {}
 
 void DRV8833::DifferentialDrive::Update(bool recurse) {
@@ -79,8 +83,8 @@ DRV8833Motor::DRV8833Motor(DRV8833* driver,
 //   this->maxRpm = rpm;
 // }
 
-void DRV8833Motor::SetTargetSpeed(float motorSpeed) {
-  Motor::SetTargetSpeed(motorSpeed);
+void DRV8833Motor::SetTargetVelocity(float motorSpeed) {
+  Motor::SetTargetVelocity(motorSpeed);
 
   uint8_t motorSignal =
       (uint8_t)(motorSpeed > 0 ? motorSpeed * 255 : -motorSpeed * 255);
diff --git a/Arduino/Things/DRV8833.h b/Arduino/Things/DRV8833.h
index 2da2664..c13a333 100644
--- a/Arduino/Things/DRV8833.h
+++ b/Arduino/Things/DRV8833.h
@@ -40,7 +40,7 @@ class DRV8833::DifferentialDrive : public RoboidControl::DifferentialDrive {
  public:
   DifferentialDrive(DRV8833::Configuration config,
                     Participant* participant = nullptr);
-  DifferentialDrive(DRV8833::Configuration config, Thing* parent);
+  DifferentialDrive(DRV8833::Configuration config, Thing& parent);
 
   virtual void Update(bool recurse = false) override;
 
@@ -66,7 +66,7 @@ class DRV8833Motor : public Motor {
   // void SetMaxRPM(unsigned int rpm);
 
   // virtual void SetAngularVelocity(Spherical velocity) override;
-  virtual void SetTargetSpeed(float targetSpeed) override;
+  virtual void SetTargetVelocity(float targetSpeed) override;
   // bool reverse = false;
 
  protected:
diff --git a/Arduino/Things/DigitalInput.cpp b/Arduino/Things/DigitalInput.cpp
index 5c3a2c2..64486cc 100644
--- a/Arduino/Things/DigitalInput.cpp
+++ b/Arduino/Things/DigitalInput.cpp
@@ -14,12 +14,17 @@ DigitalInput::DigitalInput(Participant* participant, unsigned char pin)
   pinMode(this->pin, INPUT);
 }
 
-DigitalInput::DigitalInput(Thing* parent, unsigned char pin) : Thing(parent) {
+DigitalInput::DigitalInput(unsigned char pin, Thing& parent) : Thing(parent) {
   this->pin = pin;
-
   pinMode(this->pin, INPUT);
 }
 
+// DigitalInput::DigitalInput(Thing* parent, unsigned char pin) : Thing(parent) {
+//   this->pin = pin;
+
+//   pinMode(this->pin, INPUT);
+// }
+
 void DigitalInput::Update(bool recursive) {
   this->isHigh = digitalRead(this->pin);
   this->isLow = !this->isHigh;
@@ -31,8 +36,8 @@ void DigitalInput::Update(bool recursive) {
 
 #pragma region Touch sensor
 
-DigitalInput::TouchSensor::TouchSensor(unsigned char pin, Thing* parent)
-    : RoboidControl::TouchSensor(parent), digitalInput(parent, pin) {}
+DigitalInput::TouchSensor::TouchSensor(unsigned char pin, Thing& parent)
+    : RoboidControl::TouchSensor(parent), digitalInput(pin, parent) {}
 
 void DigitalInput::TouchSensor::Update(bool recursive) {
   this->touchedSomething = digitalInput.isLow;
@@ -47,9 +52,9 @@ volatile int DigitalInput::RelativeEncoder::pulseCount0 = 0;
 volatile int DigitalInput::RelativeEncoder::pulseCount1 = 0;
 
 DigitalInput::RelativeEncoder::RelativeEncoder(Configuration config,
-                                               Thing* parent)
+                                               Thing& parent)
     : RoboidControl::RelativeEncoder(parent),
-      digitalInput(parent, config.pin),
+      digitalInput(config.pin, parent),
       pulsesPerRevolution(config.pulsesPerRevolution) {
 
   // We support up to 2 pulse counters
@@ -94,8 +99,8 @@ void DigitalInput::RelativeEncoder::Update(bool recursive) {
   this->pulseFrequency = pulseCount / timeStep;
   this->rotationSpeed = pulseFrequency / pulsesPerRevolution;
 
-  //  std::cout << "pulses: " << pulseCount << " per second " << pulseFrequency
-  //  << " timestep " << timeStep << std::endl;
+   std::cout << "pulses: " << pulseCount << " per second " << pulseFrequency
+   << " timestep " << timeStep << std::endl;
 
   this->lastUpdateTime = currentTimeMs;
 }
diff --git a/Arduino/Things/DigitalInput.h b/Arduino/Things/DigitalInput.h
index 966cfa8..ef3d25c 100644
--- a/Arduino/Things/DigitalInput.h
+++ b/Arduino/Things/DigitalInput.h
@@ -13,7 +13,8 @@ class DigitalInput : public Thing {
   /// @param participant The participant to use
   /// @param pin The digital pin
   DigitalInput(Participant* participant, unsigned char pin);
-  DigitalInput(Thing* parent, unsigned char pin);
+  //DigitalInput(Thing* parent, unsigned char pin);
+  DigitalInput(unsigned char pin, Thing& parent);
 
   bool isHigh = false;
   bool isLow = false;
@@ -34,7 +35,7 @@ class DigitalInput : public Thing {
 
 class DigitalInput::TouchSensor : public RoboidControl::TouchSensor {
  public:
-  TouchSensor(unsigned char pin, Thing* parent);
+  TouchSensor(unsigned char pin, Thing& parent);
 
   /// @copydoc RoboidControl::Thing::Update(unsigned long currentTimeMs)
   virtual void Update(bool recurse = false) override;
@@ -54,7 +55,7 @@ class DigitalInput::RelativeEncoder : public RoboidControl::RelativeEncoder {
     unsigned char pulsesPerRevolution;
   };
 
-  RelativeEncoder(Configuration config, Thing* parent);
+  RelativeEncoder(Configuration config, Thing& parent);
 
   unsigned char pulsesPerRevolution;
 
diff --git a/Arduino/Things/UltrasonicSensor.cpp b/Arduino/Things/UltrasonicSensor.cpp
index 95d0bb6..7abd9a8 100644
--- a/Arduino/Things/UltrasonicSensor.cpp
+++ b/Arduino/Things/UltrasonicSensor.cpp
@@ -16,9 +16,13 @@ UltrasonicSensor::UltrasonicSensor(Configuration config,
   pinMode(pinEcho, INPUT);      // configure the echo pin to input mode
 }
 
-UltrasonicSensor::UltrasonicSensor(Configuration config, Thing* parent)
-    : UltrasonicSensor(config, parent->owner) {
-  this->SetParent(parent);
+UltrasonicSensor::UltrasonicSensor(Configuration config, Thing& parent)
+    : Thing(parent) {
+  this->pinTrigger = config.triggerPin;
+  this->pinEcho = config.echoPin;
+
+  pinMode(pinTrigger, OUTPUT);  // configure the trigger pin to output mode
+  pinMode(pinEcho, INPUT);      // configure the echo pin to input mode
 }
 
 float UltrasonicSensor::GetDistance() {
@@ -63,15 +67,15 @@ void UltrasonicSensor::Update(bool recursive) {
 
 #pragma region Touch sensor
 
+// UltrasonicSensor::TouchSensor::TouchSensor(
+//     UltrasonicSensor::Configuration config,
+//     Thing& parent)
+//     : RoboidControl::TouchSensor(parent), ultrasonic(config, parent) {
+//   this->touchedSomething = false;
+// }
 UltrasonicSensor::TouchSensor::TouchSensor(
     UltrasonicSensor::Configuration config,
     Thing& parent)
-    : RoboidControl::TouchSensor(&parent), ultrasonic(config, &parent) {
-  this->touchedSomething = false;
-}
-UltrasonicSensor::TouchSensor::TouchSensor(
-    UltrasonicSensor::Configuration config,
-    Thing* parent)
     : RoboidControl::TouchSensor(parent), ultrasonic(config, parent) {
   this->touchedSomething = false;
 }
diff --git a/Arduino/Things/UltrasonicSensor.h b/Arduino/Things/UltrasonicSensor.h
index 563594e..0ae252e 100644
--- a/Arduino/Things/UltrasonicSensor.h
+++ b/Arduino/Things/UltrasonicSensor.h
@@ -18,7 +18,7 @@ class UltrasonicSensor : Thing {
   /// @param pinTrigger The pin number of the trigger signal
   /// @param pinEcho The pin number of the echo signal
   UltrasonicSensor(Configuration config, Participant* participant);
-  UltrasonicSensor(Configuration config, Thing* parent);
+  UltrasonicSensor(Configuration config, Thing& parent);
 
   // parameters
 
@@ -52,7 +52,7 @@ class UltrasonicSensor : Thing {
 class UltrasonicSensor::TouchSensor : public RoboidControl::TouchSensor {
  public:
   TouchSensor(UltrasonicSensor::Configuration config, Thing& parent);
-  TouchSensor(UltrasonicSensor::Configuration config, Thing* parent);
+  //TouchSensor(UltrasonicSensor::Configuration config, Thing* parent);
 
   float touchDistance = 0.2f;
 
diff --git a/Posix/PosixParticipant.h b/Posix/PosixParticipant.h
index a83569f..98302f3 100644
--- a/Posix/PosixParticipant.h
+++ b/Posix/PosixParticipant.h
@@ -13,9 +13,11 @@ class ParticipantUDP : public RoboidControl::ParticipantUDP {
   bool Publish(IMessage* msg);
 
  protected:
+#if defined(__unix__) || defined(__APPLE__)
   sockaddr_in remote_addr;
   sockaddr_in server_addr;
   sockaddr_in broadcast_addr;
+#endif
 };
 
 }  // namespace Posix
diff --git a/Things/ControlledMotor.cpp b/Things/ControlledMotor.cpp
new file mode 100644
index 0000000..bff3b80
--- /dev/null
+++ b/Things/ControlledMotor.cpp
@@ -0,0 +1,50 @@
+#include "ControlledMotor.h"
+
+namespace RoboidControl {
+
+ControlledMotor::ControlledMotor(Motor* motor, RelativeEncoder* encoder)
+    : Motor(), motor(motor), encoder(encoder) {
+  this->type = Type::ControlledMotor;
+  Thing* parent = motor->GetParent();
+  this->SetParent(parent);
+}
+
+void ControlledMotor::SetTargetVelocity(float velocity) {
+  this->targetVelocity = velocity;
+  this->rotationDirection =
+      (targetVelocity < 0) ? Direction::Reverse : Direction::Forward;
+}
+
+void ControlledMotor::Update(bool recurse) {
+  encoder->Update(recurse);
+
+  this->actualVelocity = (int)rotationDirection * encoder->rotationSpeed;
+
+  unsigned long currentTimeMs = GetTimeMs();
+  float timeStep = (currentTimeMs - this->lastUpdateTime) / 1000.0f;
+  this->lastUpdateTime = currentTimeMs;
+
+  float error = this->targetVelocity - this->actualVelocity;
+  float acceleration =
+      error * timeStep * pidP;  // Just P is used at this moment
+  motor->SetTargetVelocity(targetVelocity +
+                           acceleration);  // or something like that
+
+  motor->Update(recurse);
+}
+
+// float ControlledMotor::GetActualVelocity() {
+//   return (int)rotationDirection * encoder->rotationSpeed;
+// }
+
+// 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;
+// }
+}  // namespace RoboidControl
\ No newline at end of file
diff --git a/Things/ControlledMotor.h b/Things/ControlledMotor.h
new file mode 100644
index 0000000..1742289
--- /dev/null
+++ b/Things/ControlledMotor.h
@@ -0,0 +1,52 @@
+#pragma once
+
+#include "Motor.h"
+#include "RelativeEncoder.h"
+
+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(Motor* motor, RelativeEncoder* encoder);
+
+  float pidP = 1;
+  float pidD = 0;
+  float pidI = 0;
+
+  float actualVelocity;
+
+  enum Direction { Forward = 1, Reverse = -1 };
+  Direction rotationDirection;
+
+  virtual void Update(bool recurse = false) override;
+
+  /// @brief Set the target verlocity for the motor controller
+  /// @param speed the target velocity in revolutions per second.
+  virtual void SetTargetVelocity(float velocity) override;
+
+  /// @brief Get the actual velocity from the encoder
+  /// @return The velocity in revolutions per second
+  //   float GetActualVelocity();
+
+  //   bool Drive(float distance);
+
+  Motor* motor;
+  RelativeEncoder* encoder;
+
+ protected:
+  float lastUpdateTime;
+
+
+  //float targetVelocity;
+
+//   float netDistance = 0;
+//   float startDistance = 0;
+//   bool driving = false;
+//   float targetDistance = 0;
+//   float lastEncoderPosition = 0;
+};
+
+}  // namespace RoboidControl
diff --git a/Things/DifferentialDrive.cpp b/Things/DifferentialDrive.cpp
index 29c9e66..0b472a7 100644
--- a/Things/DifferentialDrive.cpp
+++ b/Things/DifferentialDrive.cpp
@@ -52,12 +52,16 @@ void DifferentialDrive::SetDriveDimensions(float wheelDiameter,
 //     this->rightWheel->SetPosition(Spherical(distance, Direction::right));
 // }
 
-void DifferentialDrive::SetWheelVelocity(float speedLeft, float speedRight) {
+void DifferentialDrive::SetWheelVelocity(float velocityLeft, float velocityRight) {
+  // if (this->leftWheel != nullptr)
+  //   this->leftWheel->SetAngularVelocity(Spherical(velocityLeft, Direction::left));
+  // if (this->rightWheel != nullptr)
+  //   this->rightWheel->SetAngularVelocity(
+  //       Spherical(velocityRight, Direction::right));
   if (this->leftWheel != nullptr)
-    this->leftWheel->SetAngularVelocity(Spherical(speedLeft, Direction::left));
+    this->leftWheel->SetTargetVelocity(velocityLeft);
   if (this->rightWheel != nullptr)
-    this->rightWheel->SetAngularVelocity(
-        Spherical(speedRight, Direction::right));
+    this->rightWheel->SetTargetVelocity(velocityRight);
 }
 
 void DifferentialDrive::Update(bool recursive) {
diff --git a/Things/Motor.cpp b/Things/Motor.cpp
index f9796b1..bbcd532 100644
--- a/Things/Motor.cpp
+++ b/Things/Motor.cpp
@@ -10,8 +10,8 @@ RoboidControl::Motor::Motor(Participant* owner)
 
 Motor::Motor(Thing& parent) : Thing(Type::UncontrolledMotor, parent) {}
 
-void RoboidControl::Motor::SetTargetSpeed(float targetSpeed) {
-  this->targetSpeed = targetSpeed;
+void RoboidControl::Motor::SetTargetVelocity(float targetSpeed) {
+    this->targetVelocity = targetSpeed;
 }
 
 }  // namespace RoboidControl
\ No newline at end of file
diff --git a/Things/Motor.h b/Things/Motor.h
index d10d15b..530941b 100644
--- a/Things/Motor.h
+++ b/Things/Motor.h
@@ -15,10 +15,10 @@ class Motor : public Thing {
   /// @brief The forward turning direction of the motor
   Direction direction;
 
-  virtual void SetTargetSpeed(float targetSpeed); // -1..0..1
+  virtual void SetTargetVelocity(float velocity); // -1..0..1
 
  protected:
-  float targetSpeed = 0;
+  float targetVelocity = 0;
 };
 
 }  // namespace RoboidControl
\ No newline at end of file
diff --git a/Things/RelativeEncoder.cpp b/Things/RelativeEncoder.cpp
index e13891f..b7f8a8c 100644
--- a/Things/RelativeEncoder.cpp
+++ b/Things/RelativeEncoder.cpp
@@ -4,8 +4,8 @@ namespace RoboidControl {
 
 RelativeEncoder::RelativeEncoder(Participant* owner)
     : Thing(owner, Type::IncrementalEncoder) {}
-// RelativeEncoder::RelativeEncoder(Thing* parent)
-//     : Thing(parent, Type::IncrementalEncoder) {}
+RelativeEncoder::RelativeEncoder(Thing& parent)
+    : Thing(Type::IncrementalEncoder, parent) {}
 
 float RelativeEncoder::GetRotationSpeed() {
   return rotationSpeed;
diff --git a/Things/RelativeEncoder.h b/Things/RelativeEncoder.h
index 573e7c5..ff951b8 100644
--- a/Things/RelativeEncoder.h
+++ b/Things/RelativeEncoder.h
@@ -16,7 +16,7 @@ class RelativeEncoder : public Thing {
   /// rotation
   RelativeEncoder(Participant* owner);
   // RelativeEncoder(Thing* parent);
-  RelativeEncoder(Thing& parent);
+  RelativeEncoder(Thing& parent = Thing::Root);
 
   /// @brief Get the rotation speed 
   /// @return The speed in revolutions per second