Feature/offset

src/include/servoarray/servoarray.h

@@ -34,10 +34,11 @@ class ServoArray {
   std::shared_ptr<Driver> driver_;
   ReadMode read_mode_ = ReadMode::Direct;
 
+  std::vector<double> offsets_;
   std::vector<double> cache_;
 
 public:
-  ServoArray(std::shared_ptr<Driver>);
+  ServoArray(std::shared_ptr<Driver>, const std::vector<double>& offsets = {});
   ServoArray(const std::string& name = "", const DriverParams& = {}, DriverManager& = default_manager);
 
   void write(std::size_t index, double rad);
@@ -48,6 +49,9 @@ class ServoArray {
   void set_read_mode(ReadMode);
   ReadMode read_mode() const;
 
+  double offset(std::size_t) const;
+  void set_offset(std::size_t, double);
+
   std::size_t size() const;
 };
 

src/include/servoarray/user_config.h

@@ -53,9 +53,22 @@ class MapConfig {
   MapConfig& merge(const MapConfig&);
 };
 
+class OffsetConfig {
+  std::unordered_map<std::size_t, double> offsets_;
+
+  friend class UserConfig;
+
+public:
+  const std::unordered_map<std::size_t, double>& offsets() const&;
+  std::unordered_map<std::size_t, double> offsets() &&;
+
+  OffsetConfig& merge(const OffsetConfig&);
+};
+
 class UserConfig {
   DriverConfig driver_;
   MapConfig mapping_;
+  OffsetConfig offset_;
 
 public:
   UserConfig() = default;
@@ -68,6 +81,9 @@ class UserConfig {
   const MapConfig& mapping() const&;
   MapConfig mapping() &&;
 
+  const OffsetConfig& offset() const&;
+  OffsetConfig offset() &&;
+
   UserConfig& merge(const UserConfig&);
 };
 

src/lib/servoarray.cpp

@@ -20,25 +20,37 @@
 
 namespace ServoArray {
 
-ServoArray::ServoArray(std::shared_ptr<Driver> driver) : driver_(driver) {
+ServoArray::ServoArray(std::shared_ptr<Driver> driver, const std::vector<double>& offsets) : driver_(driver), offsets_(offsets) {
   this->cache_.resize(this->size());
+  this->offsets_.resize(this->size());
 }
 
 ServoArray::ServoArray(const std::string& name, const DriverParams& params, DriverManager& manager) : driver_(manager.load(name, params)) {
   this->cache_.resize(this->size());
+  this->offsets_.resize(this->size());
+
+  for (const auto& p : manager.config().offset().offsets()) {
+    if (p.first >= this->size()) {
+      // TODO: ignore this with warning
+      throw std::runtime_error("Offset index out of range");
+    }
+
+    this->offsets_[p.first] = p.second;
+  }
 }
 
 void ServoArray::write(std::size_t index, double rad) {
-  this->driver_->write(index, rad);
-  this->cache_[index] = rad;
+  auto const value = this->offsets_[index] + rad;
+  this->driver_->write(index, value);
+  this->cache_[index] = value;
 }
 
 double ServoArray::read(std::size_t index) {
   switch(this->read_mode_) {
     case ReadMode::Cached:
-      return this->cache_[index];
+      return this->cache_[index] - this->offsets_[index];
     case ReadMode::Direct:
-      return this->driver_->read(index);
+      return this->driver_->read(index) - this->offsets_[index];
     default:
       assert(false); // unreachable
   }
@@ -57,6 +69,14 @@ ReadMode ServoArray::read_mode() const {
   return this->read_mode_;
 }
 
+double ServoArray::offset(std::size_t idx) const {
+  return this->offsets_[idx];
+}
+
+void ServoArray::set_offset(std::size_t idx, double value) {
+  this->offsets_[idx] = value;
+}
+
 std::size_t ServoArray::size() const {
   return this->driver_->size();
 }

src/lib/user_config.cpp

@@ -14,6 +14,7 @@
 // along with servoarray.  If not, see <http://www.gnu.org/licenses/>.
 
 #include <toml11/toml.hpp>
+#include <boost/lexical_cast.hpp>
 
 #include "servoarray/user_config.h"
 
@@ -53,10 +54,33 @@ MapConfig& MapConfig::merge(const MapConfig& other) {
   return *this;
 }
 
+const std::unordered_map<std::size_t, double>& OffsetConfig::offsets() const& {
+  return this->offsets_;
+}
+
+std::unordered_map<std::size_t, double> OffsetConfig::offsets() && {
+  return std::move(this->offsets_);
+}
+
+OffsetConfig& OffsetConfig::merge(const OffsetConfig& other) {
+  for (const auto& p : other.offsets_) {
+    this->offsets_[p.first] = p.second;
+  }
+
+  return *this;
+}
+
 UserConfig::UserConfig(const std::string& path) {
   auto const config = toml::parse(path);
 
   this->mapping_.names_ = toml::get_or<std::unordered_map<std::string, std::size_t>>(config, "mapping", {});
+  {
+    auto const offsets = toml::get_or<std::unordered_map<std::string, double>>(config, "offsets", {});
+    for (auto const& p : offsets) {
+      auto const idx = boost::lexical_cast<std::size_t>(p.first);
+      this->offset_.offsets_[idx] = p.second;
+    }
+  }
 
   // TODO: Seperate this into some function
   {
@@ -108,9 +132,13 @@ DriverConfig UserConfig::driver() && { return std::move(this->driver_); }
 const MapConfig& UserConfig::mapping() const& { return this->mapping_; }
 MapConfig UserConfig::mapping() && { return std::move(this->mapping_); }
 
+const OffsetConfig& UserConfig::offset() const& { return this->offset_; }
+OffsetConfig UserConfig::offset() && { return std::move(this->offset_); }
+
 UserConfig& UserConfig::merge(const UserConfig& other) {
   this->driver_.merge(other.driver());
   this->mapping_.merge(other.mapping());
+  this->offset_.merge(other.offset());
 
   return *this;
 }

src/python/binding.cpp

@@ -106,6 +106,9 @@ class ServoArray {
 
   void set_read_mode(::ServoArray::ReadMode mode) { this->sa.set_read_mode(mode); }
   ::ServoArray::ReadMode read_mode() const { return this->sa.read_mode(); }
+
+  void set_offset(std::size_t index, double value) { this->sa.set_offset(index, value); }
+  double offset(std::size_t index) const { return this->sa.offset(index); }
 };
 
 }
@@ -134,6 +137,8 @@ PYBIND11_MODULE(servoarray, m) {
     .def("write", &Adaptor::ServoArray::write)
     .def("read", &Adaptor::ServoArray::read)
     .def_property("read_mode", &Adaptor::ServoArray::read_mode, &Adaptor::ServoArray::set_read_mode)
+    .def("offset", &Adaptor::ServoArray::offset)
+    .def("set_offset", &Adaptor::ServoArray::set_offset)
     .def("__len__", &Adaptor::ServoArray::size)
     .def("__setitem__", &Adaptor::ServoArray::write_slice)
     .def("__setitem__", &Adaptor::ServoArray::write)