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/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;
@@ -64,16 +64,16 @@ class Joint
   double getVelocityIUAbsolute();
   double getVelocityIUIncremental();
   float getTemperature();
-  IMotionCubeState getIMotionCubeState();
+  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 +82,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 +102,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 +119,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 +132,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,23 +43,23 @@ class MarchRobot
   MarchRobot(MarchRobot&&) = delete;
   MarchRobot& operator=(MarchRobot&&) = delete;
 
-  void resetIMotionCubes();
+  void resetMotorControllers();
 
-  void startEtherCAT(bool reset_imc);
+  void startCommunication(bool reset_motor_controllers);
 
-  void stopEtherCAT();
+  void stopCommunication();
 
   int getMaxSlaveIndex();
 
   bool hasValidSlaves();
 
-  bool isEthercatOperational();
+  bool isCommunicationOperational();
 
-  std::exception_ptr getLastEthercatException() const noexcept;
+  std::exception_ptr getLastCommunicationException() const noexcept;
 
-  void waitForPdo();
+  void waitForUpdate();
 
-  int getEthercatCycleTime() const;
+  int getCycleTime() const;
 
   Joint& getJoint(::std::string jointName);
 

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;
+  int16_t 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);
+  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(int16_t target_torque) override;
 
+  bool initialize(int cycle_time) override;
   void goToTargetState(IMotionCubeTargetState target_state);
-  virtual void goToOperationEnabled();
+  virtual void prepareActuation() override;
+
+  uint16_t getSlaveIndex() const override;
+  virtual void reset() override;
 
   /** @brief Override comparison operator */
   friend bool operator==(const IMotionCube& lhs, const IMotionCube& rhs)

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

@@ -1,12 +1,12 @@
 // Copyright 2019 Project March.
-#ifndef MARCH_HARDWARE_IMOTIONCUBE_STATE_H
-#define MARCH_HARDWARE_IMOTIONCUBE_STATE_H
+#ifndef MARCH_HARDWARE_IMOTIONCUBE_STATE_OF_OPERATION_H
+#define MARCH_HARDWARE_IMOTIONCUBE_STATE_OF_OPERATION_H
 
 #include <string>
 
 namespace march
 {
-class IMCState
+class IMCStateOfOperation
 {
 public:
   enum Value
@@ -22,11 +22,11 @@ class IMCState
     UNKNOWN,
   };
 
-  IMCState() : value_(UNKNOWN)
+  IMCStateOfOperation() : value_(UNKNOWN)
   {
   }
 
-  explicit IMCState(uint16_t status) : value_(UNKNOWN)
+  explicit IMCStateOfOperation(uint16_t status) : value_(UNKNOWN)
   {
     const uint16_t five_bit_mask = 0b0000000001001111;
     const uint16_t six_bit_mask = 0b0000000001101111;
@@ -45,35 +45,35 @@ class IMCState
 
     if (five_bit_masked == not_ready_switch_on)
     {
-      this->value_ = IMCState::NOT_READY_TO_SWITCH_ON;
+      this->value_ = IMCStateOfOperation::NOT_READY_TO_SWITCH_ON;
     }
     else if (five_bit_masked == switch_on_disabled)
     {
-      this->value_ = IMCState::SWITCH_ON_DISABLED;
+      this->value_ = IMCStateOfOperation::SWITCH_ON_DISABLED;
     }
     else if (six_bit_masked == ready_to_switch_on)
     {
-      this->value_ = IMCState::READY_TO_SWITCH_ON;
+      this->value_ = IMCStateOfOperation::READY_TO_SWITCH_ON;
     }
     else if (six_bit_masked == switched_on)
     {
-      this->value_ = IMCState::SWITCHED_ON;
+      this->value_ = IMCStateOfOperation::SWITCHED_ON;
     }
     else if (six_bit_masked == operation_enabled)
     {
-      this->value_ = IMCState::OPERATION_ENABLED;
+      this->value_ = IMCStateOfOperation::OPERATION_ENABLED;
     }
     else if (six_bit_masked == quick_stop_active)
     {
-      this->value_ = IMCState::QUICK_STOP_ACTIVE;
+      this->value_ = IMCStateOfOperation::QUICK_STOP_ACTIVE;
     }
     else if (five_bit_masked == fault_reaction_active)
     {
-      this->value_ = IMCState::FAULT_REACTION_ACTIVE;
+      this->value_ = IMCStateOfOperation::FAULT_REACTION_ACTIVE;
     }
     else if (five_bit_masked == fault)
     {
-      this->value_ = IMCState::FAULT;
+      this->value_ = IMCStateOfOperation::FAULT;
     }
   }
 
@@ -99,49 +99,27 @@ class IMCState
         return "Fault";
       case UNKNOWN:
         return "Not in a recognized IMC state";
+      default:
+        return "Not in a recognized IMC state";
     }
   }
 
   bool operator==(Value v) const
   {
     return this->value_ == v;
   }
-  bool operator==(IMCState a) const
+  bool operator==(IMCStateOfOperation a) const
   {
     return this->value_ == a.value_;
   }
-  bool operator!=(IMCState a) const
+  bool operator!=(IMCStateOfOperation a) const
   {
     return this->value_ != a.value_;
   }
 
 private:
   Value value_;
 };
-
-struct IMotionCubeState
-{
-public:
-  IMotionCubeState() = default;
-
-  std::string statusWord;
-  std::string motionError;
-  std::string detailedError;
-  std::string secondDetailedError;
-  IMCState state;
-  std::string detailedErrorDescription;
-  std::string motionErrorDescription;
-  std::string secondDetailedErrorDescription;
-
-  float motorCurrent;
-  float IMCVoltage;
-  float motorVoltage;
-  int absoluteEncoderValue;
-  int incrementalEncoderValue;
-  double absoluteVelocity;
-  double incrementalVelocity;
-};
-
 }  // namespace march
 
-#endif  // MARCH_HARDWARE_IMOTIONCUBE_STATE_H
+#endif  // MARCH_HARDWARE_IMOTIONCUBE_STATE_OF_OPERATION_H