Feature/pm 524 Generalize motor controller

march_hardware/CMakeLists.txt

@@ -53,10 +53,13 @@ add_library(${PROJECT_NAME}
     include/${PROJECT_NAME}/ethercat/pdo_types.h
     include/${PROJECT_NAME}/ethercat/sdo_interface.h
     include/${PROJECT_NAME}/ethercat/slave.h
-    include/${PROJECT_NAME}/imotioncube/actuation_mode.h
-    include/${PROJECT_NAME}/imotioncube/imotioncube.h
-    include/${PROJECT_NAME}/imotioncube/imotioncube_state.h
-    include/${PROJECT_NAME}/imotioncube/imotioncube_target_state.h
+    include/${PROJECT_NAME}/motor_controller/actuation_mode.h
+    include/${PROJECT_NAME}/motor_controller/imotioncube/imotioncube.h
+    include/${PROJECT_NAME}/motor_controller/imotioncube/imotioncube_states.h
+    include/${PROJECT_NAME}/motor_controller/imotioncube/imotioncube_state_of_operation.h
+    include/${PROJECT_NAME}/motor_controller/imotioncube/imotioncube_target_state.h
+    include/${PROJECT_NAME}/motor_controller/motor_controller.h
+    include/${PROJECT_NAME}/motor_controller/motor_controller_states.h
     include/${PROJECT_NAME}/joint.h
     include/${PROJECT_NAME}/march_robot.h
     include/${PROJECT_NAME}/power/boot_shutdown_offsets.h
@@ -123,7 +126,7 @@ if(CATKIN_ENABLE_TESTING)
         test/joint_test.cpp
         test/mocks/mock_absolute_encoder.h
         test/mocks/mock_encoder.h
-        test/mocks/mock_imotioncube.h
+        test/mocks/mock_motor_controller.h
         test/mocks/mock_incremental_encoder.h
         test/mocks/mock_joint.h
         test/mocks/mock_pdo_interface.h

march_hardware/include/march_hardware/ethercat/slave.h

@@ -31,7 +31,7 @@ class Slave : public PdoSlaveInterface
 
   ~Slave() noexcept override = default;
 
-  uint16_t getSlaveIndex() const
+  int getSlaveIndex() const
   {
     return this->slave_index_;
   }

march_hardware/include/march_hardware/joint.h

@@ -7,10 +7,10 @@
 #include <utility>
 #include <vector>
 
-#include <march_hardware/imotioncube/imotioncube.h>
+#include <march_hardware/motor_controller/imotioncube/imotioncube.h>
 #include <march_hardware/power/power_distribution_board.h>
 #include <march_hardware/temperature/temperature_ges.h>
-#include <march_hardware/imotioncube/imotioncube_state.h>
+#include <march_hardware/motor_controller/motor_controller_states.h>
 
 namespace march
 {
@@ -26,12 +26,12 @@ class Joint
   /**
    * Initializes a Joint with motor controller and without temperature slave.
    */
-  Joint(std::string name, int net_number, bool allow_actuation, std::unique_ptr<IMotionCube> imc);
+  Joint(std::string name, int net_number, bool allow_actuation, std::unique_ptr<MotorController> controller);
 
   /**
    * Initializes a Joint with motor controller and temperature slave.
    */
-  Joint(std::string name, int net_number, bool allow_actuation, std::unique_ptr<IMotionCube> imc,
+  Joint(std::string name, int net_number, bool allow_actuation, std::unique_ptr<MotorController> controller,
         std::unique_ptr<TemperatureGES> temperature_ges);
 
   virtual ~Joint() noexcept = default;
@@ -40,7 +40,7 @@ class Joint
   Joint(const Joint&) = delete;
   Joint& operator=(const Joint&) = delete;
 
-  void resetIMotionCube();
+  void resetMotorController();
 
   /* Delete move assignment since string cannot be move assigned */
   Joint(Joint&&) = default;
@@ -50,30 +50,26 @@ class Joint
   void prepareActuation();
 
   void actuateRad(double target_position);
-  void actuateTorque(int16_t target_torque);
+  void actuateTorque(double target_torque);
   void readEncoders(const ros::Duration& elapsed_time);
 
   double getPosition() const;
   double getVelocity() const;
   double getVoltageVelocity() const;
   double getIncrementalPosition() const;
   double getAbsolutePosition() const;
-  int16_t getTorque();
-  int32_t getAngleIUAbsolute();
-  int32_t getAngleIUIncremental();
-  double getVelocityIUAbsolute();
-  double getVelocityIUIncremental();
+  double getTorque();
   float getTemperature();
-  IMotionCubeState getIMotionCubeState();
+  std::unique_ptr<MotorControllerStates> getMotorControllerStates();
 
   std::string getName() const;
   int getTemperatureGESSlaveIndex() const;
-  int getIMotionCubeSlaveIndex() const;
+  int getMotorControllerSlaveIndex() const;
   int getNetNumber() const;
 
   ActuationMode getActuationMode() const;
 
-  bool hasIMotionCube() const;
+  bool hasMotorController() const;
   bool hasTemperatureGES() const;
   bool canActuate() const;
   bool receivedDataUpdate();
@@ -82,7 +78,10 @@ class Joint
   /** @brief Override comparison operator */
   friend bool operator==(const Joint& lhs, const Joint& rhs)
   {
-    return lhs.name_ == rhs.name_ && ((lhs.imc_ && rhs.imc_ && *lhs.imc_ == *rhs.imc_) || (!lhs.imc_ && !rhs.imc_)) &&
+    return lhs.name_ == rhs.name_ &&
+           ((lhs.controller_ && rhs.controller_ &&
+             lhs.controller_->getSlaveIndex() == rhs.controller_->getSlaveIndex()) ||
+            (!lhs.controller_ && !rhs.controller_)) &&
            ((lhs.temperature_ges_ && rhs.temperature_ges_ && *lhs.temperature_ges_ == *rhs.temperature_ges_) ||
             (!lhs.temperature_ges_ && !rhs.temperature_ges_)) &&
            lhs.allow_actuation_ == rhs.allow_actuation_ &&
@@ -99,17 +98,7 @@ class Joint
     os << "name: " << joint.name_ << ", "
        << "ActuationMode: " << joint.getActuationMode().toString() << ", "
        << "allowActuation: " << joint.allow_actuation_ << ", "
-       << "imotioncube: ";
-    if (joint.imc_)
-    {
-      os << *joint.imc_;
-    }
-    else
-    {
-      os << "none";
-    }
-
-    os << ", temperatureges: ";
+       << "imotioncube slave index: " << joint.getMotorControllerSlaveIndex() << ", temperatureges: ";
     if (joint.temperature_ges_)
     {
       os << *joint.temperature_ges_;
@@ -126,7 +115,7 @@ class Joint
   const std::string name_;
   const int net_number_;
   bool allow_actuation_ = false;
-  float previous_imc_volt_ = 0.0;
+  float previous_controller_volt_ = 0.0;
   float previous_motor_current_ = 0.0;
   float previous_motor_volt_ = 0.0;
   double previous_absolute_position_ = 0.0;
@@ -139,7 +128,7 @@ class Joint
   double absolute_position_ = 0.0;
   double velocity_ = 0.0;
 
-  std::unique_ptr<IMotionCube> imc_ = nullptr;
+  std::unique_ptr<MotorController> controller_ = nullptr;
   std::unique_ptr<TemperatureGES> temperature_ges_ = nullptr;
 };
 

march_hardware/include/march_hardware/march_robot.h

@@ -43,9 +43,9 @@ class MarchRobot
   MarchRobot(MarchRobot&&) = delete;
   MarchRobot& operator=(MarchRobot&&) = delete;
 
-  void resetIMotionCubes();
+  void resetMotorControllers();
 
-  void startEtherCAT(bool reset_imc);
+  void startEtherCAT(bool reset_motor_controllers);
 
   void stopEtherCAT();
 

march_hardware/include/march_hardware/imotioncube/imotioncube.h → march_hardware/include/march_hardware/motor_controller/imotioncube/imotioncube.h

@@ -2,12 +2,13 @@
 
 #ifndef MARCH_HARDWARE_IMOTIONCUBE_H
 #define MARCH_HARDWARE_IMOTIONCUBE_H
-#include "actuation_mode.h"
+#include "march_hardware/motor_controller/actuation_mode.h"
 #include "march_hardware/ethercat/pdo_map.h"
 #include "march_hardware/ethercat/pdo_types.h"
 #include "march_hardware/ethercat/sdo_interface.h"
 #include "march_hardware/ethercat/slave.h"
-#include "imotioncube_state.h"
+#include "march_hardware/motor_controller/motor_controller.h"
+#include "march_hardware/motor_controller/motor_controller_states.h"
 #include "imotioncube_target_state.h"
 #include "march_hardware/encoder/absolute_encoder.h"
 #include "march_hardware/encoder/incremental_encoder.h"
@@ -18,7 +19,7 @@
 
 namespace march
 {
-class IMotionCube : public Slave
+class IMotionCube : public MotorController, public Slave
 {
 public:
   /**
@@ -36,42 +37,48 @@ class IMotionCube : public Slave
               std::unique_ptr<IncrementalEncoder> incremental_encoder, std::string& sw_stream,
               ActuationMode actuation_mode);
 
-  ~IMotionCube() noexcept override = default;
+  virtual ~IMotionCube() noexcept override = default;
 
   /* Delete copy constructor/assignment since the unique_ptrs cannot be copied */
   IMotionCube(const IMotionCube&) = delete;
   IMotionCube& operator=(const IMotionCube&) = delete;
 
-  virtual double getAngleRadAbsolute();
-  virtual double getAngleRadIncremental();
+  virtual double getAngleRadAbsolute() override;
+  virtual double getAngleRadIncremental() override;
   double getAbsoluteRadPerBit() const;
   double getIncrementalRadPerBit() const;
-  int16_t getTorque();
+  bool getIncrementalMorePrecise() const override;
+  double getTorque() override;
   int32_t getAngleIUAbsolute();
   int32_t getAngleIUIncremental();
   double getVelocityIUAbsolute();
   double getVelocityIUIncremental();
-  virtual double getVelocityRadAbsolute();
-  virtual double getVelocityRadIncremental();
+  virtual double getVelocityRadAbsolute() override;
+  virtual double getVelocityRadIncremental() override;
 
   uint16_t getStatusWord();
   uint16_t getMotionError();
   uint16_t getDetailedError();
   uint16_t getSecondDetailedError();
 
-  ActuationMode getActuationMode() const;
+  ActuationMode getActuationMode() const override;
 
-  virtual float getMotorCurrent();
-  virtual float getIMCVoltage();
-  virtual float getMotorVoltage();
+  virtual float getMotorCurrent() override;
+  virtual float getMotorControllerVoltage() override;
+  virtual float getMotorVoltage() override;
 
-  void setControlWord(uint16_t control_word);
+  std::unique_ptr<MotorControllerStates> getStates() override;
 
-  virtual void actuateRad(double target_rad);
-  virtual void actuateTorque(int16_t target_torque);
+  void setControlWord(uint16_t control_word);
+  virtual void actuateRad(double target_rad) override;
+  virtual void actuateTorque(double target_torque_ampere) override;
 
+  bool initialize(int cycle_time) override;
   void goToTargetState(IMotionCubeTargetState target_state);
-  virtual void goToOperationEnabled();
+  virtual void prepareActuation() override;
+
+  int getSlaveIndex() const override;
+  virtual void reset() override;
 
   /** @brief Override comparison operator */
   friend bool operator==(const IMotionCube& lhs, const IMotionCube& rhs)
@@ -90,7 +97,9 @@ class IMotionCube : public Slave
   constexpr static double MAX_TARGET_DIFFERENCE = 0.393;
   // This value is slightly larger than the current limit of the
   // linear joints defined in the URDF.
-  const static int16_t MAX_TARGET_TORQUE = 23500;
+  constexpr static double IPEAK = 40;
+  // See CoE manual page 222
+  constexpr static double AMPERE_TO_IU_FACTOR = 65520;
 
   // Watchdog base time = 1 / 25 MHz * (2498 + 2) = 0.0001 seconds=100 µs
   static const uint16_t WATCHDOG_DIVIDER = 2498;
@@ -104,6 +113,7 @@ class IMotionCube : public Slave
 
 private:
   void actuateIU(int32_t target_iu);
+  int16_t ampereToTorqueIU(double ampere);
 
   void mapMisoPDOs(SdoSlaveInterface& sdo);
   void mapMosiPDOs(SdoSlaveInterface& sdo);