From 6b8633be0b3af905b9dafd1f09b8656f08a01225 Mon Sep 17 00:00:00 2001 From: "coord.e" Date: Mon, 11 Feb 2019 13:04:20 +0000 Subject: [PATCH 01/10] Add: Add declaration of OffsetConfig --- src/include/servoarray/user_config.h | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/src/include/servoarray/user_config.h b/src/include/servoarray/user_config.h index c0cd499..96432a4 100644 --- a/src/include/servoarray/user_config.h +++ b/src/include/servoarray/user_config.h @@ -53,9 +53,22 @@ class MapConfig { MapConfig& merge(const MapConfig&); }; +class OffsetConfig { + std::vector offsets_; + + friend class UserConfig; + +public: + const std::vector& offsets() const&; + std::vector 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&); }; From 22c150d0396ca7fea5357ce2ccbafdd66ee43def Mon Sep 17 00:00:00 2001 From: "coord.e" Date: Mon, 11 Feb 2019 13:07:10 +0000 Subject: [PATCH 02/10] Fix: Use unorderd_map to hold offsets to perform merge properly --- src/include/servoarray/user_config.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/include/servoarray/user_config.h b/src/include/servoarray/user_config.h index 96432a4..8e72b89 100644 --- a/src/include/servoarray/user_config.h +++ b/src/include/servoarray/user_config.h @@ -54,13 +54,13 @@ class MapConfig { }; class OffsetConfig { - std::vector offsets_; + std::unordered_map offsets_; friend class UserConfig; public: - const std::vector& offsets() const&; - std::vector offsets() &&; + const std::unordered_map& offsets() const&; + std::unordered_map offsets() &&; OffsetConfig& merge(const OffsetConfig&); }; From 2aa56066718c5c04fc4b82937c0f24124becc671 Mon Sep 17 00:00:00 2001 From: "coord.e" Date: Mon, 11 Feb 2019 13:08:33 +0000 Subject: [PATCH 03/10] Add: Implement OffsetConfig --- src/lib/user_config.cpp | 21 +++++++++++++++++++++ 1 file changed, 21 insertions(+) diff --git a/src/lib/user_config.cpp b/src/lib/user_config.cpp index aaf5f7a..dc5d09e 100644 --- a/src/lib/user_config.cpp +++ b/src/lib/user_config.cpp @@ -53,10 +53,27 @@ MapConfig& MapConfig::merge(const MapConfig& other) { return *this; } +const std::unordered_map& OffsetConfig::offsets() const& { + return this->offsets_; +} + +std::unordered_map 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>(config, "mapping", {}); + this->offset_.offsets_ = toml::get_or>(config, "offsets", {}); // TODO: Seperate this into some function { @@ -108,9 +125,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; } From 13f36cf6d9a51496445ab1a19f59847b2d31ecc9 Mon Sep 17 00:00:00 2001 From: "coord.e" Date: Mon, 11 Feb 2019 13:24:42 +0000 Subject: [PATCH 04/10] Fix: Convert to index from string key of toml --- src/lib/user_config.cpp | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/src/lib/user_config.cpp b/src/lib/user_config.cpp index dc5d09e..874dcb1 100644 --- a/src/lib/user_config.cpp +++ b/src/lib/user_config.cpp @@ -14,6 +14,7 @@ // along with servoarray. If not, see . #include +#include #include "servoarray/user_config.h" @@ -73,7 +74,13 @@ UserConfig::UserConfig(const std::string& path) { auto const config = toml::parse(path); this->mapping_.names_ = toml::get_or>(config, "mapping", {}); - this->offset_.offsets_ = toml::get_or>(config, "offsets", {}); + { + auto const offsets = toml::get_or>(config, "offsets", {}); + for (auto const& p : offsets) { + auto const idx = boost::lexical_cast(p.first); + this->offset_.offsets_[idx] = p.second; + } + } // TODO: Seperate this into some function { From ba2605e1bfaed1e2f16512a9946ed858144b2691 Mon Sep 17 00:00:00 2001 From: "coord.e" Date: Mon, 11 Feb 2019 13:29:30 +0000 Subject: [PATCH 05/10] Add: Hold offsets in ServoArray --- src/include/servoarray/servoarray.h | 3 ++- src/lib/servoarray.cpp | 2 +- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/src/include/servoarray/servoarray.h b/src/include/servoarray/servoarray.h index 265bd0b..b247b08 100644 --- a/src/include/servoarray/servoarray.h +++ b/src/include/servoarray/servoarray.h @@ -34,10 +34,11 @@ class ServoArray { std::shared_ptr driver_; ReadMode read_mode_ = ReadMode::Direct; + std::vector offsets_; std::vector cache_; public: - ServoArray(std::shared_ptr); + ServoArray(std::shared_ptr, const std::vector& offsets = {}); ServoArray(const std::string& name = "", const DriverParams& = {}, DriverManager& = default_manager); void write(std::size_t index, double rad); diff --git a/src/lib/servoarray.cpp b/src/lib/servoarray.cpp index 0aba7de..f30d635 100644 --- a/src/lib/servoarray.cpp +++ b/src/lib/servoarray.cpp @@ -20,7 +20,7 @@ namespace ServoArray { -ServoArray::ServoArray(std::shared_ptr driver) : driver_(driver) { +ServoArray::ServoArray(std::shared_ptr driver, const std::vector& offsets) : driver_(driver), offsets_(offsets) { this->cache_.resize(this->size()); } From 70c527037e591f00d3869303873fb9999a061fa8 Mon Sep 17 00:00:00 2001 From: "coord.e" Date: Mon, 11 Feb 2019 13:33:31 +0000 Subject: [PATCH 06/10] Add: Use offsets in ServoArray --- src/lib/servoarray.cpp | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/src/lib/servoarray.cpp b/src/lib/servoarray.cpp index f30d635..5b61ea3 100644 --- a/src/lib/servoarray.cpp +++ b/src/lib/servoarray.cpp @@ -22,23 +22,26 @@ namespace ServoArray { ServoArray::ServoArray(std::shared_ptr driver, const std::vector& 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()); } 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 } From 0ce412e9b8dad50168ead2bbb3288c474d52dc1b Mon Sep 17 00:00:00 2001 From: "coord.e" Date: Mon, 11 Feb 2019 13:36:42 +0000 Subject: [PATCH 07/10] Add: Use offsets from config --- src/lib/servoarray.cpp | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/lib/servoarray.cpp b/src/lib/servoarray.cpp index 5b61ea3..5d3b646 100644 --- a/src/lib/servoarray.cpp +++ b/src/lib/servoarray.cpp @@ -28,6 +28,14 @@ ServoArray::ServoArray(std::shared_ptr driver, const std::vector 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()) { + throw std::runtime_error("Offset index out of range"); + } + + this->offsets_[p.first] = p.second; + } } void ServoArray::write(std::size_t index, double rad) { From a28d84b09b46840511c3c4ee378436ecf43ca986 Mon Sep 17 00:00:00 2001 From: "coord.e" Date: Mon, 11 Feb 2019 13:39:39 +0000 Subject: [PATCH 08/10] Add: Add TODO comment --- src/lib/servoarray.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/lib/servoarray.cpp b/src/lib/servoarray.cpp index 5d3b646..e701602 100644 --- a/src/lib/servoarray.cpp +++ b/src/lib/servoarray.cpp @@ -31,6 +31,7 @@ ServoArray::ServoArray(const std::string& name, const DriverParams& params, Driv 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"); } From 6721a7d57b720141181c3a8ecb7b4a729c4bc57f Mon Sep 17 00:00:00 2001 From: "coord.e" Date: Mon, 11 Feb 2019 13:43:40 +0000 Subject: [PATCH 09/10] Add: Add APIs to access to offsets --- src/include/servoarray/servoarray.h | 3 +++ src/lib/servoarray.cpp | 8 ++++++++ 2 files changed, 11 insertions(+) diff --git a/src/include/servoarray/servoarray.h b/src/include/servoarray/servoarray.h index b247b08..bb80df4 100644 --- a/src/include/servoarray/servoarray.h +++ b/src/include/servoarray/servoarray.h @@ -49,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; }; diff --git a/src/lib/servoarray.cpp b/src/lib/servoarray.cpp index e701602..0ebbf28 100644 --- a/src/lib/servoarray.cpp +++ b/src/lib/servoarray.cpp @@ -69,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(); } From 983a01fd0a4e792c723f786d47ea9b3ca303a1c3 Mon Sep 17 00:00:00 2001 From: "coord.e" Date: Mon, 11 Feb 2019 13:46:20 +0000 Subject: [PATCH 10/10] Add: Add python APIs --- src/python/binding.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/python/binding.cpp b/src/python/binding.cpp index d2d4148..b08d397 100644 --- a/src/python/binding.cpp +++ b/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)