Added controlledMotor, ptr to ref for hierarchy

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) {
+void DRV8833Motor::SetTargetVelocity(float motorSpeed) {
-  Motor::SetTargetSpeed(motorSpeed);
+  Motor::SetTargetVelocity(motorSpeed);
 
   uint8_t motorSignal =
       (uint8_t)(motorSpeed > 0 ? motorSpeed * 255 : -motorSpeed * 255);

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:

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)
+DigitalInput::TouchSensor::TouchSensor(unsigned char pin, Thing& parent)
-    : RoboidControl::TouchSensor(parent), digitalInput(parent, pin) {}
+    : 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
+   std::cout << "pulses: " << pulseCount << " per second " << pulseFrequency
-  //  << " timestep " << timeStep << std::endl;
+   << " timestep " << timeStep << std::endl;
 
   this->lastUpdateTime = currentTimeMs;
 }

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;
 

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::UltrasonicSensor(Configuration config, Thing& parent)
-    : UltrasonicSensor(config, parent->owner) {
+    : Thing(parent) {
-  this->SetParent(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;
 }

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;
 

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

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

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

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(
+    this->rightWheel->SetTargetVelocity(velocityRight);
-        Spherical(speedRight, Direction::right));
 }
 
 void DifferentialDrive::Update(bool recursive) {

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) {
+void RoboidControl::Motor::SetTargetVelocity(float targetSpeed) {
-  this->targetSpeed = targetSpeed;
+    this->targetVelocity = targetSpeed;
 }
 
 }  // namespace RoboidControl

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

Things/RelativeEncoder.cpp

@@ -4,8 +4,8 @@ namespace RoboidControl {
 
 RelativeEncoder::RelativeEncoder(Participant* owner)
     : Thing(owner, Type::IncrementalEncoder) {}
-// RelativeEncoder::RelativeEncoder(Thing* parent)
+RelativeEncoder::RelativeEncoder(Thing& parent)
-//     : Thing(parent, Type::IncrementalEncoder) {}
+    : Thing(Type::IncrementalEncoder, parent) {}
 
 float RelativeEncoder::GetRotationSpeed() {
   return rotationSpeed;

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