From b86484d59deabcc20cc32e9191027946cb84a0fc Mon Sep 17 00:00:00 2001
From: Pascal Serrarens <pascal@passervr.com>
Date: Fri, 19 Jan 2024 14:14:28 +0100
Subject: [PATCH] Clean & pid tuning

---
 ControlledMotor.cpp | 26 +++++++++++++++-----------
 ControlledMotor.h   |  6 +++---
 2 files changed, 18 insertions(+), 14 deletions(-)

diff --git a/ControlledMotor.cpp b/ControlledMotor.cpp
index 2e4769b..467d24e 100644
--- a/ControlledMotor.cpp
+++ b/ControlledMotor.cpp
@@ -20,25 +20,29 @@ void ControlledMotor::SetTargetSpeed(float speed) {
 }
 
 void ControlledMotor::Update(float currentTimeMs) {
-  actualSpeed = encoder->GetRevolutionsPerSecond(currentTimeMs);
+  this->actualSpeed = encoder->GetRevolutionsPerSecond(currentTimeMs);
   if (this->currentTargetSpeed < 0)
-    actualSpeed = -actualSpeed;
+    this->actualSpeed = -this->actualSpeed;
 
   float deltaTime = currentTimeMs - this->lastUpdateTime;
 
-  float error = currentTargetSpeed - actualSpeed;
+  float error = this->currentTargetSpeed - this->actualSpeed;
   float errorChange = (error - lastError) * deltaTime;
 
   float delta = error * pidP + errorChange * pidD;
 
-  // Serial.print("  actualSpeed ");
-  // Serial.print(actualSpeed);
-  // Serial.print("    motor target speed ");
-  // Serial.print(motor->currentTargetSpeed);
-  // Serial.print(" + ");
-  // Serial.print(error * pidP);
-  // Serial.print(" + ");
-  // Serial.println(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
diff --git a/ControlledMotor.h b/ControlledMotor.h
index 6ddd74f..a4a0cf6 100644
--- a/ControlledMotor.h
+++ b/ControlledMotor.h
@@ -19,9 +19,9 @@ public:
   }
   float velocity;
 
-  float pidP = 0.02F;
-  float pidD = 0.001F;
-  float pidI = 0;
+  float pidP = 0.1F;
+  float pidD = 0.0F;
+  float pidI = 0.0F;
 
   void Update(float currentTimeMs) override;