Feature: add incremental encoder (#225)

* add absolute and differential encoder

* update IMotionCube

* update Joint

* update march_hardware tests

* update march_hardware_builder

* update hardware_builder tests

* update march_hardware_interface

* fix build errors

* fix build errors part two

* fix incorrect yaml files

* format code to clang format

* Move encoders to separate directory

* Rename encoders

* Sort library sources list

* Add tests for incremental and base class encoder

* Actually add the incremental value to the state lol

* Add transmission variable for incremental encoders to calculate radians

* Make Encoder pure abstract

Co-authored-by: bjornminderman <33688606+bjornminderman@users.noreply.github.com>

Author: RutgerVanBeek

.gitignore

@@ -1,2 +1,3 @@
 .idea/*
 */cmake-build-debug/**
+.vscode/settings.json

march_hardware/CMakeLists.txt

@@ -38,10 +38,12 @@ if(CATKIN_ENABLE_TESTING AND ENABLE_COVERAGE_TESTING)
 endif()
 
 add_library(${PROJECT_NAME}
+    include/${PROJECT_NAME}/encoder/AbsoluteEncoder.h
+    include/${PROJECT_NAME}/encoder/Encoder.h
+    include/${PROJECT_NAME}/encoder/IncrementalEncoder.h
     include/${PROJECT_NAME}/error/error_type.h
     include/${PROJECT_NAME}/error/hardware_exception.h
     include/${PROJECT_NAME}/error/motion_error.h
-    src/Encoder.cpp
     src/EtherCAT/EthercatIO.cpp
     src/EtherCAT/EthercatMaster.cpp
     src/EtherCAT/EthercatSDO.cpp
@@ -54,6 +56,9 @@ add_library(${PROJECT_NAME}
     src/PDOmap.cpp
     src/PowerDistributionBoard.cpp
     src/TemperatureGES.cpp
+    src/encoder/AbsoluteEncoder.cpp
+    src/encoder/Encoder.cpp
+    src/encoder/IncrementalEncoder.cpp
     src/error/error_type.cpp
     src/error/motion_error.cpp
 )
@@ -89,7 +94,6 @@ install(DIRECTORY launch
 if(CATKIN_ENABLE_TESTING)
     catkin_add_gmock(${PROJECT_NAME}_test
         test/TestBootShutdownOffsets.cpp
-        test/TestEncoder.cpp
         test/TestHighVoltage.cpp
         test/TestIMotionCube.cpp
         test/TestJoint.cpp
@@ -100,10 +104,15 @@ if(CATKIN_ENABLE_TESTING)
         test/TestPowerDistributionBoard.cpp
         test/TestRunner.cpp
         test/TestTemperatureGES.cpp
+        test/encoder/TestAbsoluteEncoder.cpp
+        test/encoder/TestIncrementalEncoder.cpp
+        test/encoder/TestEncoder.cpp
         test/error/test_hardware_exception.cpp
         test/error/test_motion_error.cpp
+        test/mocks/MockAbsoluteEncoder.cpp
         test/mocks/MockEncoder.cpp
         test/mocks/MockIMotionCube.cpp
+        test/mocks/MockIncrementalEncoder.cpp
         test/mocks/MockJoint.cpp
         test/mocks/MockTemperatureGES.cpp
         test/mocks/MockTemperatureSensor.cpp

march_hardware/include/march_hardware/Encoder.h

@@ -2,13 +2,14 @@
 
 #ifndef MARCH_HARDWARE_IMOTIONCUBE_H
 #define MARCH_HARDWARE_IMOTIONCUBE_H
-#include <march_hardware/ActuationMode.h>
-#include <march_hardware/Encoder.h>
-#include <march_hardware/EtherCAT/EthercatIO.h>
-#include <march_hardware/IMotionCubeState.h>
-#include <march_hardware/IMotionCubeTargetState.h>
-#include <march_hardware/PDOmap.h>
-#include <march_hardware/Slave.h>
+#include "march_hardware/ActuationMode.h"
+#include "march_hardware/encoder/AbsoluteEncoder.h"
+#include "march_hardware/encoder/IncrementalEncoder.h"
+#include "march_hardware/EtherCAT/EthercatIO.h"
+#include "march_hardware/IMotionCubeState.h"
+#include "march_hardware/IMotionCubeTargetState.h"
+#include "march_hardware/PDOmap.h"
+#include "march_hardware/Slave.h"
 
 #include <unordered_map>
 #include <string>
@@ -18,15 +19,18 @@ namespace march
 class IMotionCube : public Slave
 {
 public:
-  IMotionCube(int slave_index, Encoder encoder, ActuationMode actuation_mode);
+  IMotionCube(int slave_index, AbsoluteEncoder absolute_encoder, IncrementalEncoder incremental_encoder,
+              ActuationMode actuation_mode);
 
   ~IMotionCube() = default;
 
   void writeInitialSDOs(int cycle_time) override;
 
-  double getAngleRad();
+  double getAngleRadAbsolute();
+  double getAngleRadIncremental();
   int16_t getTorque();
-  int32_t getAngleIU();
+  int32_t getAngleIUAbsolute();
+  int32_t getAngleIUIncremental();
 
   uint16_t getStatusWord();
   uint16_t getMotionError();
@@ -49,13 +53,15 @@ class IMotionCube : public Slave
   /** @brief Override comparison operator */
   friend bool operator==(const IMotionCube& lhs, const IMotionCube& rhs)
   {
-    return lhs.slaveIndex == rhs.slaveIndex && lhs.encoder_ == rhs.encoder_;
+    return lhs.slaveIndex == rhs.slaveIndex && lhs.absolute_encoder_ == rhs.absolute_encoder_ &&
+           lhs.incremental_encoder_ == rhs.incremental_encoder_;
   }
   /** @brief Override stream operator for clean printing */
   friend std::ostream& operator<<(std::ostream& os, const IMotionCube& imc)
   {
     return os << "slaveIndex: " << imc.slaveIndex << ", "
-              << "encoder: " << imc.encoder_;
+              << "incrementalEncoder: " << imc.incremental_encoder_ << ", "
+              << "absoluteEncoder: " << imc.absolute_encoder_;
   }
 
   constexpr static double MAX_TARGET_DIFFERENCE = 0.393;
@@ -75,7 +81,8 @@ class IMotionCube : public Slave
   void mapMosiPDOs();
   void writeInitialSettings(uint8_t cycle_time);
 
-  Encoder encoder_;
+  AbsoluteEncoder absolute_encoder_;
+  IncrementalEncoder incremental_encoder_;
   ActuationMode actuation_mode_;
 
   std::unordered_map<IMCObjectName, uint8_t> miso_byte_offsets_;

march_hardware/include/march_hardware/IMotionCube.h

@@ -132,6 +132,7 @@ struct IMotionCubeState
   std::string motionErrorDescription;
   float motorCurrent;
   float motorVoltage;
+  int incrementalEncoderValue;
 };
 
 }  // namespace march

march_hardware/include/march_hardware/IMotionCubeState.h

@@ -34,9 +34,11 @@ class Joint
   void actuateRad(double targetPositionRad);
   void actuateTorque(int16_t targetTorque);
 
-  double getAngleRad();
+  double getAngleRadAbsolute();
+  double getAngleRadIncremental();
   int16_t getTorque();
-  int32_t getAngleIU();
+  int32_t getAngleIUAbsolute();
+  int32_t getAngleIUIncremental();
   float getTemperature();
   IMotionCubeState getIMotionCubeState();
 
@@ -78,7 +80,6 @@ class Joint
 
   void setName(const std::string& name);
   void setAllowActuation(bool allowActuation);
-  void setIMotionCube(const IMotionCube& iMotionCube);
   void setTemperatureGes(const TemperatureGES& temperatureGes);
   void setNetNumber(int netNumber);
 };

march_hardware/include/march_hardware/Joint.h

@@ -1,8 +1,6 @@
 // Copyright 2019 Project March.
 #ifndef MARCH_HARDWARE_SLAVE_H
 #define MARCH_HARDWARE_SLAVE_H
-
-#include <stdexcept>
 #include <ros/ros.h>
 
 namespace march

march_hardware/include/march_hardware/Slave.h

@@ -0,0 +1,64 @@
+// Copyright 2019 Project March.
+
+#ifndef MARCH_HARDWARE_ABSOLUTE_ENCODER_H
+#define MARCH_HARDWARE_ABSOLUTE_ENCODER_H
+#include "march_hardware/encoder/Encoder.h"
+
+#include <ostream>
+
+namespace march
+{
+class AbsoluteEncoder : public Encoder
+{
+public:
+  AbsoluteEncoder(size_t number_of_bits, int32_t lower_limit_iu, int32_t upper_limit_iu, double lower_limit_rad,
+                  double upper_limit_rad, double lower_soft_limit_rad, double upper_soft_limit_rad);
+
+  double toRad(int32_t iu) const override;
+
+  /*
+   * Converts radians to encoder Internal Units (IU).
+   */
+  int32_t fromRad(double rad) const;
+
+  bool isWithinHardLimitsIU(int32_t iu) const;
+  bool isWithinSoftLimitsIU(int32_t iu) const;
+  bool isValidTargetIU(int32_t current_iu, int32_t target_iu) const;
+
+  int32_t getUpperSoftLimitIU() const;
+  int32_t getLowerSoftLimitIU() const;
+  int32_t getUpperHardLimitIU() const;
+  int32_t getLowerHardLimitIU() const;
+
+  /** @brief Override comparison operator */
+  friend bool operator==(const AbsoluteEncoder& lhs, const AbsoluteEncoder& rhs)
+  {
+    return lhs.getSlaveIndex() == rhs.getSlaveIndex() && lhs.getTotalPositions() == rhs.getTotalPositions() &&
+           lhs.upper_soft_limit_iu_ == rhs.upper_soft_limit_iu_ &&
+           lhs.lower_soft_limit_iu_ == rhs.lower_soft_limit_iu_ && lhs.upper_limit_iu_ == rhs.upper_limit_iu_ &&
+           lhs.lower_limit_iu_ == rhs.lower_limit_iu_ && lhs.zero_position_iu_ == rhs.zero_position_iu_;
+  }
+  /** @brief Override stream operator for clean printing */
+  friend std::ostream& operator<<(std::ostream& os, const AbsoluteEncoder& encoder)
+  {
+    return os << "slaveIndex: " << encoder.getSlaveIndex() << ", "
+              << "totalPositions: " << encoder.getTotalPositions() << ", "
+              << "upperHardLimit: " << encoder.upper_limit_iu_ << ", "
+              << "lowerHardLimit: " << encoder.lower_limit_iu_ << ", "
+              << "upperSoftLimit: " << encoder.upper_soft_limit_iu_ << ", "
+              << "lowerSoftLimit: " << encoder.lower_soft_limit_iu_ << ", "
+              << "zeroPositionIU: " << encoder.zero_position_iu_;
+  }
+
+  static constexpr double MAX_RANGE_DIFFERENCE = 0.05;
+
+private:
+  int32_t zero_position_iu_ = 0;
+  int32_t lower_limit_iu_ = 0;
+  int32_t upper_limit_iu_ = 0;
+  int32_t lower_soft_limit_iu_ = 0;
+  int32_t upper_soft_limit_iu_ = 0;
+};
+}  // namespace march
+
+#endif  // MARCH_HARDWARE_ABSOLUTE_ENCODER_H

march_hardware/include/march_hardware/encoder/AbsoluteEncoder.h

@@ -0,0 +1,64 @@
+// Copyright 2019 Project March.
+
+#ifndef MARCH_HARDWARE_ENCODER_H
+#define MARCH_HARDWARE_ENCODER_H
+#include <cmath>
+#include <cstddef>
+#include <cstdint>
+
+namespace march
+{
+class Encoder
+{
+public:
+  explicit Encoder(size_t number_of_bits);
+
+  /*
+   * Reads out the encoder from the slave and returns the value in Internal Units (IU).
+   * @param byte_offset the byte offset in the slave register for the IU position
+   * @returns The current position of the encoder in Internal Units (IU)
+   */
+  int32_t getAngleIU(uint8_t byte_offset) const;
+
+  /*
+   * Reads out the encoder from the slave and transforms the result to radians.
+   * @param byte_offset the byte offset in the slave register for the IU position
+   * @returns The current position of the encoder in radians
+   */
+  double getAngleRad(uint8_t byte_offset) const;
+
+  /*
+   * Converts encoder Internal Units (IU) to radians.
+   * This is a pure virtual function and must be implemented by subclasses,
+   * since each type of encoder has a different way of calculating radians.
+   */
+  virtual double toRad(int32_t iu) const = 0;
+
+  size_t getTotalPositions() const;
+
+  int getSlaveIndex() const;
+  void setSlaveIndex(int slave_index);
+
+  static const size_t MIN_RESOLUTION = 1;
+  static const size_t MAX_RESOLUTION = 32;
+
+  static constexpr double PI_2 = 2 * M_PI;
+
+private:
+  /*
+   * Returns the total number of positions possible on an encoder
+   * with the given amount of bits.
+   * @param number_of_bits The resolution of the encoder
+   * @returns The total amount of different positions
+   * @throws HardwareException When the given resolution is outside the allowed range
+   *                           Which is determined by Encoder::MIN_RESOLUTION
+   *                           and Encoder::MAX_RESOLUTION.
+   */
+  static size_t calculateTotalPositions(size_t number_of_bits);
+
+  int slave_index_ = -1;
+  size_t total_positions_ = 0;
+};
+}  // namespace march
+
+#endif  // MARCH_HARDWARE_ENCODER_H