Merge pull request #279 from project-march/feature/PM-523-adding-DER2

Feature: add der2

Author: Olavhaasie

march_hardware/include/march_hardware/error/motion_error.h

@@ -9,15 +9,23 @@ namespace march
 {
 namespace error
 {
+enum class ErrorRegisters
+{
+  MOTION_ERROR,
+  DETAILED_ERROR,
+  SECOND_DETAILED_ERROR
+};
+
 const size_t MOTION_ERRORS_SIZE = 16;
 extern const char* MOTION_ERRORS[MOTION_ERRORS_SIZE];
 
 const size_t DETAILED_MOTION_ERRORS_SIZE = 9;
 extern const char* DETAILED_MOTION_ERRORS[DETAILED_MOTION_ERRORS_SIZE];
 
-std::string parseMotionError(uint16_t motion_error);
+const size_t SECOND_DETAILED_MOTION_ERROR_SIZE = 7;
+extern const char* SECOND_DETAILED_MOTION_ERRORS[SECOND_DETAILED_MOTION_ERROR_SIZE];
 
-std::string parseDetailedError(uint16_t detailed_error);
+std::string parseError(uint16_t error, ErrorRegisters);
 
 }  // namespace error
 }  // namespace march

march_hardware/include/march_hardware/ethercat/pdo_map.h

@@ -47,6 +47,7 @@ enum class IMCObjectName
   ActualVelocity,
   MotionErrorRegister,
   DetailedErrorRegister,
+  SecondDetailedErrorRegister,
   DCLinkVoltage,
   DriveTemperature,
   ActualTorque,

march_hardware/include/march_hardware/imotioncube/imotioncube.h

@@ -57,6 +57,7 @@ class IMotionCube : public Slave
   uint16_t getStatusWord();
   uint16_t getMotionError();
   uint16_t getDetailedError();
+  uint16_t getSecondDetailedError();
 
   ActuationMode getActuationMode() const;
 

march_hardware/include/march_hardware/imotioncube/imotioncube_state.h

@@ -125,11 +125,14 @@ struct IMotionCubeState
   IMotionCubeState() = default;
 
   std::string statusWord;
-  std::string detailedError;
   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;

march_hardware/src/error/motion_error.cpp

@@ -36,29 +36,39 @@ const char* DETAILED_MOTION_ERRORS[DETAILED_MOTION_ERRORS_SIZE] = {
   "Invalid S-curve profile. ",
 };
 
-std::string parseMotionError(uint16_t motion_error)
-{
-  std::string description;
-  const std::bitset<16> bitset(motion_error);
-  for (size_t i = 0; i < MOTION_ERRORS_SIZE; i++)
-  {
-    if (bitset.test(i))
-    {
-      description += MOTION_ERRORS[i];
-    }
-  }
-  return description;
-}
+const char* SECOND_DETAILED_MOTION_ERRORS[SECOND_DETAILED_MOTION_ERROR_SIZE] = {
+  "BiSS data CRC error. ",
+  "BiSS data warning bit is set. ",
+  "BiSS data error bit is set. ",
+  "BiSS sensor missing. ",
+  "Absolute Encoder Interface (AEI) interface error. ",
+  "Hall sensor missing. ",
+  "Position wraparound. The position 2^31 was exceeded. ",
+};
 
-std::string parseDetailedError(uint16_t detailed_error)
+std::string parseError(uint16_t error, ErrorRegisters error_register)
 {
   std::string description;
-  const std::bitset<16> bitset(detailed_error);
-  for (size_t i = 0; i < DETAILED_MOTION_ERRORS_SIZE; i++)
+  const std::bitset<16> bitset(error);
+
+  for (size_t i = 0; i < 16; i++)
   {
     if (bitset.test(i))
     {
-      description += DETAILED_MOTION_ERRORS[i];
+      switch (error_register)
+      {
+        case ErrorRegisters::MOTION_ERROR:
+          description += MOTION_ERRORS[i];
+          break;
+        case ErrorRegisters::DETAILED_ERROR:
+          description += DETAILED_MOTION_ERRORS[i];
+          break;
+        case ErrorRegisters::SECOND_DETAILED_ERROR:
+          description += SECOND_DETAILED_MOTION_ERRORS[i];
+          break;
+        default:
+          break;
+      }
     }
   }
   return description;

march_hardware/src/ethercat/pdo_map.cpp

@@ -15,6 +15,7 @@ std::unordered_map<IMCObjectName, IMCObject> PDOmap::all_objects = {
   { IMCObjectName::ActualVelocity, IMCObject(0x6069, 0, 32) },
   { IMCObjectName::MotionErrorRegister, IMCObject(0x2000, 0, 16) },
   { IMCObjectName::DetailedErrorRegister, IMCObject(0x2002, 0, 16) },
+  { IMCObjectName::SecondDetailedErrorRegister, IMCObject(0x2009, 0, 16) },
   { IMCObjectName::DCLinkVoltage, IMCObject(0x2055, 0, 16) },
   { IMCObjectName::DriveTemperature, IMCObject(0x2058, 0, 16) },
   { IMCObjectName::ActualTorque, IMCObject(0x6077, 0, 16) },

march_hardware/src/imotioncube/imotioncube.cpp

@@ -61,6 +61,7 @@ void IMotionCube::mapMisoPDOs(SdoSlaveInterface& sdo)
   map_miso.addObject(IMCObjectName::ActualTorque);    // Compulsory!
   map_miso.addObject(IMCObjectName::MotionErrorRegister);
   map_miso.addObject(IMCObjectName::DetailedErrorRegister);
+  map_miso.addObject(IMCObjectName::SecondDetailedErrorRegister);
   map_miso.addObject(IMCObjectName::DCLinkVoltage);
   map_miso.addObject(IMCObjectName::MotorVoltage);
   map_miso.addObject(IMCObjectName::MotorPosition);
@@ -280,6 +281,11 @@ uint16_t IMotionCube::getDetailedError()
   return this->read16(this->miso_byte_offsets_.at(IMCObjectName::DetailedErrorRegister)).ui;
 }
 
+uint16_t IMotionCube::getSecondDetailedError()
+{
+  return this->read16(this->miso_byte_offsets_.at(IMCObjectName::SecondDetailedErrorRegister)).ui;
+}
+
 float IMotionCube::getMotorCurrent()
 {
   const float PEAK_CURRENT = 40.0;            // Peak current of iMC drive
@@ -326,8 +332,14 @@ void IMotionCube::goToTargetState(IMotionCubeTargetState target_state)
     {
       ROS_FATAL("IMotionCube went to fault state while attempting to go to '%s'. Shutting down.",
                 target_state.getDescription().c_str());
-      ROS_FATAL("Detailed Error: %s", error::parseDetailedError(this->getDetailedError()).c_str());
-      ROS_FATAL("Motion Error: %s", error::parseMotionError(this->getMotionError()).c_str());
+      ROS_FATAL("Motion Error (MER): %s",
+                error::parseError(this->getMotionError(), error::ErrorRegisters::MOTION_ERROR).c_str());
+      ROS_FATAL("Detailed Error (DER): %s",
+                error::parseError(this->getDetailedError(), error::ErrorRegisters::DETAILED_ERROR).c_str());
+      ROS_FATAL(
+          "Detailed Error 2 (DER2): %s",
+          error::parseError(this->getSecondDetailedError(), error::ErrorRegisters::SECOND_DETAILED_ERROR).c_str());
+
       throw std::domain_error("IMC to fault state");
     }
   }

march_hardware/src/joint.cpp

@@ -229,14 +229,20 @@ IMotionCubeState Joint::getIMotionCubeState()
 
   std::bitset<16> statusWordBits = this->imc_->getStatusWord();
   states.statusWord = statusWordBits.to_string();
-  std::bitset<16> detailedErrorBits = this->imc_->getDetailedError();
-  states.detailedError = detailedErrorBits.to_string();
   std::bitset<16> motionErrorBits = this->imc_->getMotionError();
   states.motionError = motionErrorBits.to_string();
+  std::bitset<16> detailedErrorBits = this->imc_->getDetailedError();
+  states.detailedError = detailedErrorBits.to_string();
+  std::bitset<16> secondDetailedErrorBits = this->imc_->getSecondDetailedError();
+  states.secondDetailedError = secondDetailedErrorBits.to_string();
 
   states.state = IMCState(this->imc_->getStatusWord());
-  states.detailedErrorDescription = error::parseDetailedError(this->imc_->getDetailedError());
-  states.motionErrorDescription = error::parseMotionError(this->imc_->getMotionError());
+
+  states.motionErrorDescription = error::parseError(this->imc_->getMotionError(), error::ErrorRegisters::MOTION_ERROR);
+  states.detailedErrorDescription =
+      error::parseError(this->imc_->getDetailedError(), error::ErrorRegisters::DETAILED_ERROR);
+  states.secondDetailedErrorDescription =
+      error::parseError(this->imc_->getSecondDetailedError(), error::ErrorRegisters::SECOND_DETAILED_ERROR);
 
   states.motorCurrent = this->imc_->getMotorCurrent();
   states.IMCVoltage = this->imc_->getIMCVoltage();

march_hardware/test/error/motion_error_test.cpp

@@ -5,13 +5,14 @@
 
 TEST(MotionErrorTest, ParseNoMotionError)
 {
-  ASSERT_EQ(march::error::parseMotionError(0), "");
+  ASSERT_EQ(march::error::parseError(0, march::error::ErrorRegisters::MOTION_ERROR), "");
 }
 
 TEST(MotionErrorTest, ParseCorrectMotionError)
 {
   const uint16_t error = 1;
-  ASSERT_EQ(march::error::parseMotionError(error), march::error::MOTION_ERRORS[0]);
+  ASSERT_EQ(march::error::parseError(error, march::error::ErrorRegisters::MOTION_ERROR),
+            march::error::MOTION_ERRORS[0]);
 }
 
 TEST(MotionErrorTest, ParseMultipleErrors)
@@ -21,18 +22,19 @@ TEST(MotionErrorTest, ParseMultipleErrors)
   expected += march::error::MOTION_ERRORS[2];
   expected += march::error::MOTION_ERRORS[3];
   expected += march::error::MOTION_ERRORS[15];
-  ASSERT_EQ(march::error::parseMotionError(error), expected);
+  ASSERT_EQ(march::error::parseError(error, march::error::ErrorRegisters::MOTION_ERROR), expected);
 }
 
 TEST(TestDetailedMotionError, ParseNoDetailedMotionError)
 {
-  ASSERT_EQ(march::error::parseDetailedError(0), "");
+  ASSERT_EQ(march::error::parseError(0, march::error::ErrorRegisters::DETAILED_ERROR), "");
 }
 
 TEST(TestDetailedMotionError, ParseCorrectDetailedMotionError)
 {
   const uint16_t error = 1;
-  ASSERT_EQ(march::error::parseDetailedError(error), march::error::DETAILED_MOTION_ERRORS[0]);
+  ASSERT_EQ(march::error::parseError(error, march::error::ErrorRegisters::DETAILED_ERROR),
+            march::error::DETAILED_MOTION_ERRORS[0]);
 }
 
 TEST(TestDetailedMotionError, ParseMultipleDetailedErrors)
@@ -42,5 +44,26 @@ TEST(TestDetailedMotionError, ParseMultipleDetailedErrors)
   expected += march::error::DETAILED_MOTION_ERRORS[2];
   expected += march::error::DETAILED_MOTION_ERRORS[3];
   expected += march::error::DETAILED_MOTION_ERRORS[8];
-  ASSERT_EQ(march::error::parseDetailedError(error), expected);
+  ASSERT_EQ(march::error::parseError(error, march::error::ErrorRegisters::DETAILED_ERROR), expected);
 }
+
+TEST(TestSecondDetailedMotionError, ParseNoSecondDetailedMotionError)
+{
+  ASSERT_EQ(march::error::parseError(0, march::error::ErrorRegisters::SECOND_DETAILED_ERROR), "");
+}
+
+TEST(TestSecondDetailedMotionError, ParseCorrectSecondDetailedMotionError)
+{
+  const uint16_t error = 1;
+  ASSERT_EQ(march::error::parseError(error, march::error::ErrorRegisters::SECOND_DETAILED_ERROR),
+            march::error::SECOND_DETAILED_MOTION_ERRORS[0]);
+}
+
+TEST(TestSecondDetailedMotionError, ParseMultipleSecondDetailedErrors)
+{
+  const uint16_t error = 0b0001100;
+  std::string expected;
+  expected += march::error::SECOND_DETAILED_MOTION_ERRORS[2];
+  expected += march::error::SECOND_DETAILED_MOTION_ERRORS[3];
+  ASSERT_EQ(march::error::parseError(error, march::error::ErrorRegisters::SECOND_DETAILED_ERROR), expected);
+}

march_hardware_interface/src/march_hardware_interface.cpp

@@ -459,9 +459,14 @@ bool MarchHardwareInterface::iMotionCubeStateCheck(size_t joint_index)
   march::IMotionCubeState imc_state = joint.getIMotionCubeState();
   if (imc_state.state == march::IMCState::FAULT)
   {
-    ROS_ERROR("IMotionCube of joint %s is in fault state %s\nDetailed Error: %s (%s)\nMotion Error: %s (%s)",
-              joint.getName().c_str(), imc_state.state.getString().c_str(), imc_state.detailedErrorDescription.c_str(),
-              imc_state.detailedError.c_str(), imc_state.motionErrorDescription.c_str(), imc_state.motionError.c_str());
+    ROS_ERROR("IMotionCube of joint %s is in fault state %s"
+              "\nMotion Error: %s (%s)"
+              "\nDetailed Error: %s (%s)"
+              "\nSecond Detailed Error: %s (%s)",
+              joint.getName().c_str(), imc_state.state.getString().c_str(), imc_state.motionErrorDescription.c_str(),
+              imc_state.motionError.c_str(), imc_state.detailedErrorDescription.c_str(),
+              imc_state.detailedError.c_str(), imc_state.secondDetailedErrorDescription.c_str(),
+              imc_state.secondDetailedError.c_str());
     return false;
   }
   return true;