From 0779554313d15cc801686b8e54fb0fd01a54009d Mon Sep 17 00:00:00 2001
From: Pascal Serrarens <pse@pale.blue>
Date: Fri, 30 Aug 2024 16:28:15 +0200
Subject: [PATCH] Added actual angle

---
 NetworkSync.cpp | 2 +-
 ServoMotor.cpp  | 6 +++++-
 ServoMotor.h    | 3 ++-
 3 files changed, 8 insertions(+), 3 deletions(-)

diff --git a/NetworkSync.cpp b/NetworkSync.cpp
index c2defa4..ccaa630 100644
--- a/NetworkSync.cpp
+++ b/NetworkSync.cpp
@@ -1,6 +1,6 @@
 #include "NetworkSync.h"
 
-#define RC_DEBUG 1
+// #define RC_DEBUG 1
 
 #ifdef RC_DEBUG
 #include <Arduino.h>
diff --git a/ServoMotor.cpp b/ServoMotor.cpp
index d388ed1..eef8e15 100644
--- a/ServoMotor.cpp
+++ b/ServoMotor.cpp
@@ -97,4 +97,8 @@ void ServoMotor::Update(unsigned long currentTimeMs) {
 
     this->lastUpdateTimeMs = currentTimeMs;
   }
-}
\ No newline at end of file
+}
+
+void ServoMotor::SetAngle(Angle16 angle) {
+  this->actualAngle = angle;
+};
diff --git a/ServoMotor.h b/ServoMotor.h
index cfe62b3..8325503 100644
--- a/ServoMotor.h
+++ b/ServoMotor.h
@@ -32,6 +32,7 @@ class ServoMotor : public Thing {
  protected:
   bool hasTargetAngle = false;
   Angle16 targetAngle = 0.0F;
+  Angle16 actualAngle = 0.0F;
 
   float maxVelocity = 0.0F;
 
@@ -40,7 +41,7 @@ class ServoMotor : public Thing {
 
   float lastUpdateTimeMs = 0.0F;
 
-  virtual void SetAngle(Angle16 angle) {};
+  virtual void SetAngle(Angle16 angle);
 };
 
 }  // namespace RoboidContol