From 1b7ad61fa278c43d5c005b29f449cdc4bdd9f94e Mon Sep 17 00:00:00 2001
From: Alexander Entinger <cto@lxrobotics.com>
Date: Tue, 23 Aug 2022 12:32:50 +0200
Subject: [PATCH] Provide public API to configure servo runtime.

---
 src/Braccio++.h               | 2 ++
 src/lib/motors/SmartServo.cpp | 6 ++++++
 src/lib/motors/SmartServo.h   | 5 +++--
 3 files changed, 11 insertions(+), 2 deletions(-)

diff --git a/src/Braccio++.h b/src/Braccio++.h
index ad0b014..6d3d1e6 100644
--- a/src/Braccio++.h
+++ b/src/Braccio++.h
@@ -196,6 +196,8 @@ class Servo
   inline float position()            { return _servos.getPosition(_id); }
   inline bool  connected()           { return Braccio.connected(_id); }
   inline void  info(Stream & stream) { _servos.getInfo(stream, _id); }
+  inline uint16_t runtime() { return _servos.getTime(_id); }
+  inline void     setRuntime(uint16_t const time) { _servos.setTime(_id, time); }
 
   operator bool() { return connected(); }
 
diff --git a/src/lib/motors/SmartServo.cpp b/src/lib/motors/SmartServo.cpp
index 4e29d75..0e6d311 100644
--- a/src/lib/motors/SmartServo.cpp
+++ b/src/lib/motors/SmartServo.cpp
@@ -274,6 +274,12 @@ void SmartServoClass::setTime(uint8_t const id, uint16_t const time)
   writeWordCmd(id, REG(SmartServoRegister::RUN_TIME_H), time);
 }
 
+uint16_t SmartServoClass::getTime(uint8_t const id)
+{
+  mbed::ScopedLock<rtos::Mutex> lock(_mtx);
+  return readWordCmd(id, REG(SmartServoRegister::RUN_TIME_H));
+}
+
 void SmartServoClass::setMaxTorque(uint8_t const id, uint16_t const max_torque)
 {
   mbed::ScopedLock<rtos::Mutex> lock(_mtx);
diff --git a/src/lib/motors/SmartServo.h b/src/lib/motors/SmartServo.h
index 43d38bb..d42d487 100644
--- a/src/lib/motors/SmartServo.h
+++ b/src/lib/motors/SmartServo.h
@@ -74,8 +74,9 @@ class SmartServoClass
   void setMaxAngle(uint16_t const max_angle);
   void setMaxAngle(uint8_t const id, uint16_t const max_angle);
 
-  void setTime(uint16_t const time);
-  void setTime(uint8_t const id, uint16_t const time);
+  void     setTime(uint16_t const time);
+  void     setTime(uint8_t const id, uint16_t const time);
+  uint16_t getTime(uint8_t const id);
 
   void center(uint8_t const id, uint16_t const position);