From 814c10e4615a71d6d01d57f1e08768b48985ea1d Mon Sep 17 00:00:00 2001
From: Xoaquin Castrelo <xoaquin.cb@gmail.com>
Date: Sat, 13 Nov 2021 23:47:53 +0000
Subject: [PATCH] Added function to get servo angle.

---
 emb/servos.cpp | 18 +++++++++++++++++-
 emb/servos.hpp |  7 +++++++
 2 files changed, 24 insertions(+), 1 deletion(-)

diff --git a/emb/servos.cpp b/emb/servos.cpp
index 0ec79e2..c51fe1e 100644
--- a/emb/servos.cpp
+++ b/emb/servos.cpp
@@ -4,7 +4,6 @@
 #include <avr/interrupt.h>
 #include <stdint.h>
 
-#define NUM_SERVOS 3
 #define MIN_PULSE 999
 #define MAX_PULSE 4999
 
@@ -18,6 +17,7 @@ struct Servo
     const uint8_t pinMask;          // the pin mask for the servo
     uint16_t tempValue;             // stores a temporary value before it's written to the servo
     uint16_t value;                 // current pulse width value
+    int16_t angle;                  // the angle in arc-minutes
 };
 
 static Servo servos[NUM_SERVOS] = {
@@ -43,7 +43,23 @@ void initServos()
 void setServoAngle(uint8_t servo, int16_t arcMin)
 {
     if (0 <= servo && servo < NUM_SERVOS)
+    {
+        if (arcMin < -180 * 60)
+            arcMin = -180 * 60;
+        else if (arcMin > 180 * 60)
+            arcMin = 180 * 60;
+        
         servos[servo].tempValue = MIN_PULSE + (int32_t) (arcMin + 180 * 60)  * (MAX_PULSE - MIN_PULSE) / (360 * 60);
+        servos[servo].angle = arcMin;
+    }
+}
+
+int16_t getServoAngle(uint8_t servo)
+{
+    if (0 <= servo && servo < NUM_SERVOS)
+        return servos[servo].angle;
+    else
+        return 0;
 }
 
 void updateServos()
diff --git a/emb/servos.hpp b/emb/servos.hpp
index c2aeff4..910ab3f 100644
--- a/emb/servos.hpp
+++ b/emb/servos.hpp
@@ -3,6 +3,8 @@
 
 #include <stdint.h>
 
+#define NUM_SERVOS 3
+
 /**
  * Initialises the servos.
  */
@@ -16,6 +18,11 @@ void initServos();
  */
 void setServoAngle(uint8_t servo, int16_t arcMin);
 
+/**
+ * Gets the angle of a servo.
+ */
+int16_t getServoAngle(uint8_t servo);
+
 /**
  * Writes the servo values out to the servos.
  */
-- 
GitLab