From 1b36f97e18b92e74eabc7de9422af60bbfe1a43a Mon Sep 17 00:00:00 2001
From: Pascal Serrarens <pse@pale.blue>
Date: Tue, 5 Dec 2023 11:18:38 +0100
Subject: [PATCH] Moved ControlledMotor to branch

---
 ControlledMotor.cpp | 44 ---------------------------------
 ControlledMotor.h   | 59 ---------------------------------------------
 Motor.cpp           | 10 +++++---
 3 files changed, 7 insertions(+), 106 deletions(-)
 delete mode 100644 ControlledMotor.cpp
 delete mode 100644 ControlledMotor.h

diff --git a/ControlledMotor.cpp b/ControlledMotor.cpp
deleted file mode 100644
index 3462bbc..0000000
--- a/ControlledMotor.cpp
+++ /dev/null
@@ -1,44 +0,0 @@
-#include "ControlledMotor.h"
-#include <Arduino.h>
-ControlledMotor::ControlledMotor() {
-  this->type = Thing::ControlledMotorType;
-}
-
-ControlledMotor::ControlledMotor(Motor* motor, Encoder* encoder)
-    : ControlledMotor() {
-  this->motor = motor;
-  this->encoder = encoder;
-}
-
-void ControlledMotor::SetTargetSpeed(float velocity) {
-  this->targetVelocity = velocity;
-  this->rotationDirection =
-      (targetVelocity < 0) ? Direction::Reverse : Direction::Forward;
-}
-
-void ControlledMotor::Update(float currentTimeMs) {
-  actualVelocity =
-      (int)rotationDirection * encoder->GetRevolutionsPerSecond(currentTimeMs);
-  float error = targetVelocity - velocity;
-
-  float timeStep = currentTimeMs - lastUpdateTime;
-  float acceleration =
-      error * timeStep * pidP;  // Just P is used at this moment
-  motor->SetSpeed(targetVelocity + acceleration);  // or something like that
-  this->lastUpdateTime = currentTimeMs;
-}
-
-float ControlledMotor::GetActualSpeed() {
-  return actualVelocity;
-}
-
-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;
-}
\ No newline at end of file
diff --git a/ControlledMotor.h b/ControlledMotor.h
deleted file mode 100644
index 66d958f..0000000
--- a/ControlledMotor.h
+++ /dev/null
@@ -1,59 +0,0 @@
-#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 Thing {
- 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 = 1;
-  float pidD = 0;
-  float pidI = 0;
-
-  void Update(float currentTimeMs);
-
-  /// @brief Set the target speed for the motor controller
-  /// @param speed the target in revolutions per second.
-  void SetTargetSpeed(float speed);
-
-  /// @brief Get the actual speed from the encoder
-  /// @return The speed in revolutions per second
-  float GetActualSpeed();
-
-  bool Drive(float distance);
-
-  Motor* motor;
-  Encoder* encoder;
-
- protected:
-  float lastUpdateTime;
-  float targetVelocity;
-  float actualVelocity;
-  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;
\ No newline at end of file
diff --git a/Motor.cpp b/Motor.cpp
index f18a7c9..3c23653 100644
--- a/Motor.cpp
+++ b/Motor.cpp
@@ -1,9 +1,13 @@
 #include "Motor.h"
 #include <time.h>
 
-Motor::Motor() { type = (int)Thing::UncontrolledMotorType; }
+Motor::Motor() {
+  type = (int)Thing::UncontrolledMotorType;
+}
 
-float Motor::GetSpeed() { return this->currentTargetSpeed; }
+float Motor::GetSpeed() {
+  return this->currentTargetSpeed;
+}
 
 void Motor::SetSpeed(float targetSpeed) {
   this->currentTargetSpeed = targetSpeed;
@@ -21,6 +25,6 @@ bool Motor::Drive(float distance) {
     return true;
   }
 
-  SetSpeed(distance < 0 ? -1 : 1); // max speed
+  SetSpeed(distance < 0 ? -1 : 1);  // max speed
   return false;
 }
\ No newline at end of file