Added function to get servo angle.

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()

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.
  */