Initial merge from haptic arm

Arduino/ArduinoParticipant.h

@@ -1,6 +1,6 @@
 #pragma once
 
-#include "../LocalParticipant.h"
+#include "Participants/ParticipantUDP.h"
 
 #if defined(HAS_WIFI)
 #include <WiFiUdp.h>

EspIdf/EspIdfParticipant.h

@@ -1,13 +1,13 @@
 #pragma once
 
-#include "../LocalParticipant.h"
+#include "Participants/ParticipantUDP.h"
 
 #include "lwip/sockets.h"
 
 namespace RoboidControl {
 namespace EspIdf {
 
-class LocalParticipant : public RoboidControl::LocalParticipant {
+class ParticipantUDP : public RoboidControl::LocalParticipant {
  public:
   void Setup(int localPort, const char* remoteIpAddress, int remotePort);
   void Receive();

Things/AbsoluteEncoder.cpp

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

Things/AbsoluteEncoder.h

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

Things/ControlledMotor.cpp

@@ -0,0 +1,63 @@
+#include "ControlledMotor.h"
+
+ControlledMotor::ControlledMotor() { this->type = Thing::ControlledMotorType; }
+
+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;
+}

Things/ControlledMotor.h

@@ -0,0 +1,57 @@
+#pragma once
+
+#include "IncrementalEncoder.h"
+#include "Motor.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();
+  ControlledMotor(Motor* motor, IncrementalEncoder* 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;
+  IncrementalEncoder* 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

Things/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; }

Things/IncrementalEncoder.h

@@ -0,0 +1,48 @@
+#pragma once
+
+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

Things/Motor.cpp

@@ -0,0 +1,21 @@
+#include "Motor.h"
+
+#include <time.h>
+
+namespace RoboidControl {
+
+Motor::Motor() : Thing(0) { // for now, id should be set properly later
+  this->type = (int)Thing::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) {}
+
+}

Things/Motor.h

@@ -0,0 +1,42 @@
+#pragma once
+
+#include "Thing.h"
+
+#include <time.h>
+
+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

Things/ServoMotor.cpp

@@ -0,0 +1,105 @@
+#include "ServoMotor.h"
+
+#include "LinearAlgebra/FloatSingle.h"
+
+ServoMotor::ServoMotor()
+    : Thing(0) {  // for now, id should be set properly later
+  this->type = Thing::ServoType;
+  this->controlMode = ControlMode::Position;
+  this->targetAngle = Angle16();
+  this->hasTargetAngle = false;
+}
+
+void ServoMotor::SetTargetAngle(Angle16 angle) {
+  angle = Angle16::Clamp(angle, minAngle, maxAngle);
+
+  if (maxSpeed == 0.0F) {
+    SetAngle(angle);
+    this->limitedTargetAngle = angle;
+  }
+
+  this->controlMode = ControlMode::Position;
+  this->targetAngle = angle;
+  this->hasTargetAngle = true;
+}
+
+Angle16 ServoMotor::GetTargetAngle() {
+  return this->targetAngle;
+}
+
+void ServoMotor::SetMaximumVelocity(float maxVelocity) {
+  this->maxSpeed = maxVelocity;
+}
+
+void ServoMotor::SetTargetVelocity(float targetVelocity) {
+  targetVelocity = Float::Clamp(targetVelocity, -this->maxSpeed, maxSpeed);
+
+  this->controlMode = ControlMode::Velocity;
+  this->targetVelocity = targetVelocity;
+  this->hasTargetAngle = false;  // can't we use the controlMode for this?
+}
+
+float ServoMotor::GetTargetVelocity() {
+  return this->targetVelocity;
+}
+
+void ServoMotor::Update(unsigned long currentTimeMs) {
+  for (unsigned char childIx = 0; childIx < this->childCount; childIx++) {
+    Thing* child = this->GetChild(childIx);
+    if (child != nullptr && child->type == Thing::ServoType) {
+      ServoMotor* servo = (ServoMotor*)child;
+      servo->Update(currentTimeMs);
+    }
+  }
+
+  if (this->lastUpdateTimeMs == 0 || currentTimeMs < this->lastUpdateTimeMs) {
+    this->lastUpdateTimeMs = currentTimeMs;
+    return;
+  }
+
+  float deltaTime = (currentTimeMs - this->lastUpdateTimeMs) / 1000.0F;
+
+  // Position control
+  if (controlMode == ControlMode::Position) {
+    if (maxSpeed == 0.0F || hasTargetAngle == false) {
+      this->lastUpdateTimeMs = currentTimeMs;
+      return;
+
+    } else {
+      float angleStep = maxSpeed * deltaTime;
+      float deltaAngle =
+          this->targetAngle.InDegrees() - this->limitedTargetAngle.InDegrees();
+      float absDeltaAngle = (deltaAngle < 0) ? -deltaAngle : deltaAngle;
+
+      if (absDeltaAngle < angleStep) {
+        this->limitedTargetAngle = this->targetAngle;
+        SetAngle(targetAngle);
+      } else {
+        if (deltaAngle < 0)
+          limitedTargetAngle = limitedTargetAngle - Angle16::Degrees(angleStep);
+        else
+          limitedTargetAngle = limitedTargetAngle + Angle16::Degrees(angleStep);
+      }
+      SetAngle(limitedTargetAngle);
+
+      this->lastUpdateTimeMs = currentTimeMs;
+      return;
+    }
+
+    // Velocity control
+  } else {
+    float newAngle = this->targetAngle.InDegrees() + targetVelocity * deltaTime;
+    newAngle =
+        Float::Clamp(newAngle, minAngle.InDegrees(), maxAngle.InDegrees());
+
+    Angle16 targetAngle = Angle16::Degrees(newAngle);
+    ServoMotor::SetTargetAngle(targetAngle);
+    SetAngle(targetAngle);
+
+    this->lastUpdateTimeMs = currentTimeMs;
+  }
+}
+
+void ServoMotor::SetAngle(Angle16 angle) {
+  this->actualAngle = angle;
+};

Things/ServoMotor.h

@@ -0,0 +1,47 @@
+#pragma once
+
+#include "ControlledMotor.h"
+#include "LinearAlgebra/Angle.h"
+
+namespace RoboidControl {
+
+class ServoMotor : public Thing {
+ public:
+  ServoMotor();
+
+  Direction16 rotationAxis = Direction16::up;
+  Angle16 minAngle = Angle16::Degrees(-90);
+  Angle16 maxAngle = Angle16::Degrees(90);
+
+  enum ControlMode { Position, Velocity };
+  ControlMode controlMode = ControlMode::Position;
+
+  Thing* target = nullptr;
+
+  virtual void SetTargetAngle(Angle16 angle);
+  virtual Angle16 GetTargetAngle();
+
+  virtual void SetMaximumVelocity(float maxVelocity);
+
+  virtual void SetTargetVelocity(float velocity);
+  virtual float GetTargetVelocity();
+
+  virtual void Update(unsigned long currentTimeMs, bool recurse) override;
+
+  Angle16 limitedTargetAngle = Angle16();
+
+ protected:
+  bool hasTargetAngle = false;
+  Angle16 targetAngle = Angle16();
+  Angle16 actualAngle = Angle16();
+
+  float maxSpeed = 0.0F;
+
+  float targetVelocity = 0.0F;
+
+  float lastUpdateTimeMs = 0.0F;
+
+  virtual void SetAngle(Angle16 angle);
+};
+
+}  // namespace RoboidContol