Add: Use offsets in ServoArray

Author: coord-e

src/lib/servoarray.cpp

@@ -22,23 +22,26 @@ namespace ServoArray {
 
 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());
 }
 
 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
   }