Merge pull request #266 from project-march/feature/PM-422-add-motor-voltage

Feature: add motor voltage

Author: RutgerVanBeek

march_hardware/include/march_hardware/IMotionCube.h

@@ -62,6 +62,7 @@ class IMotionCube : public Slave
 
   virtual float getMotorCurrent();
   virtual float getIMCVoltage();
+  virtual float getMotorVoltage();
 
   void setControlWord(uint16_t control_word);
 

march_hardware/include/march_hardware/IMotionCubeState.h

@@ -132,6 +132,7 @@ struct IMotionCubeState
   std::string motionErrorDescription;
   float motorCurrent;
   float IMCVoltage;
+  float motorVoltage;
   int absoluteEncoderValue;
   int incrementalEncoderValue;
   double absoluteVelocity;

march_hardware/include/march_hardware/Joint.h

@@ -127,6 +127,7 @@ class Joint
   bool allow_actuation_ = false;
   float previous_imc_volt_ = 0.0;
   float previous_motor_current_ = 0.0;
+  float previous_motor_volt_ = 0.0;
   double previous_absolute_position_ = 0.0;
   double previous_incremental_position_ = 0.0;
   double previous_absolute_velocity_ = 0.0;

march_hardware/include/march_hardware/PDOmap.h

@@ -57,7 +57,8 @@ enum class IMCObjectName
   TargetPosition,
   TargetTorque,
   QuickStopDeceleration,
-  QuickStopOption
+  QuickStopOption,
+  MotorVoltage
 };
 
 class PDOmap

march_hardware/src/IMotionCube.cpp

@@ -62,6 +62,7 @@ void IMotionCube::mapMisoPDOs(SdoSlaveInterface& sdo)
   map_miso.addObject(IMCObjectName::MotionErrorRegister);
   map_miso.addObject(IMCObjectName::DetailedErrorRegister);
   map_miso.addObject(IMCObjectName::DCLinkVoltage);
+  map_miso.addObject(IMCObjectName::MotorVoltage);
   map_miso.addObject(IMCObjectName::MotorPosition);
   map_miso.addObject(IMCObjectName::MotorVelocity);
   map_miso.addObject(IMCObjectName::ActualVelocity);
@@ -128,7 +129,12 @@ bool IMotionCube::writeInitialSettings(SdoSlaveInterface& sdo, int cycle_time)
   int rate_ec_x = sdo.write<uint8_t>(0x60C2, 1, cycle_time);
   int rate_ec_y = sdo.write<int8_t>(0x60C2, 2, -3);
 
-  if (!(mode_of_op && max_pos_lim && min_pos_lim && stop_opt && stop_decl && abort_con && rate_ec_x && rate_ec_y))
+  // use filter object to read motor voltage
+  int volt_address = sdo.write<int16_t>(0x2108, 1, 0x0232);
+  int volt_filter = sdo.write<int16_t>(0x2108, 2, 32767);
+
+  if (!(mode_of_op && max_pos_lim && min_pos_lim && stop_opt && stop_decl && abort_con && rate_ec_x && rate_ec_y &&
+        volt_address && volt_filter))
   {
     throw error::HardwareException(error::ErrorType::WRITING_INITIAL_SETTINGS_FAILED,
                                    "Failed writing initial settings to IMC of slave %d", this->getSlaveIndex());
@@ -296,6 +302,11 @@ float IMotionCube::getIMCVoltage()
          static_cast<float>(imc_voltage_iu);  // Conversion to Volt, see Technosoft CoE programming manual
 }
 
+float IMotionCube::getMotorVoltage()
+{
+  return this->read16(this->miso_byte_offsets_.at(IMCObjectName::MotorVoltage)).ui;
+}
+
 void IMotionCube::setControlWord(uint16_t control_word)
 {
   bit16 control_word_ui = { .ui = control_word };

march_hardware/src/Joint.cpp

@@ -228,6 +228,7 @@ IMotionCubeState Joint::getIMotionCubeState()
 
   states.motorCurrent = this->imc_->getMotorCurrent();
   states.IMCVoltage = this->imc_->getIMCVoltage();
+  states.motorVoltage = this->imc_->getMotorVoltage();
 
   states.absoluteEncoderValue = this->imc_->getAngleIUAbsolute();
   states.incrementalEncoderValue = this->imc_->getAngleIUIncremental();
@@ -294,19 +295,21 @@ bool Joint::receivedDataUpdate()
   // If imc voltage, motor current, and both encoders positions and velocities did not change,
   // we probably did not receive an update for this joint.
   float new_imc_volt = this->imc_->getIMCVoltage();
+  float new_motor_volt = this->imc_->getMotorVoltage();
   float new_motor_current = this->imc_->getMotorCurrent();
   double new_absolute_position = this->imc_->getAngleRadAbsolute();
   double new_incremental_position = this->imc_->getAngleRadIncremental();
   double new_absolute_velocity = this->imc_->getVelocityRadAbsolute();
   double new_incremental_velocity = this->imc_->getVelocityRadIncremental();
-
-  bool data_updated = (new_imc_volt != this->previous_imc_volt_ || new_motor_current != this->previous_motor_current_ ||
+  bool data_updated = (new_imc_volt != this->previous_imc_volt_ || new_motor_volt != this->previous_motor_volt_ ||
+                       new_motor_current != this->previous_motor_current_ ||
                        new_absolute_position != this->previous_absolute_position_ ||
                        new_incremental_position != this->previous_incremental_position_ ||
                        new_absolute_velocity != this->previous_absolute_velocity_ ||
                        new_incremental_velocity != this->previous_incremental_velocity_);
 
   this->previous_imc_volt_ = new_imc_volt;
+  this->previous_motor_volt_ = new_motor_volt;
   this->previous_motor_current_ = new_motor_current;
   this->previous_absolute_position_ = new_absolute_position;
   this->previous_incremental_position_ = new_incremental_position;

march_hardware/src/PDOmap.cpp

@@ -25,7 +25,8 @@ std::unordered_map<IMCObjectName, IMCObject> PDOmap::all_objects = {
   { IMCObjectName::TargetPosition, IMCObject(0x607A, 0, 32) },
   { IMCObjectName::TargetTorque, IMCObject(0x6071, 0, 16) },
   { IMCObjectName::QuickStopDeceleration, IMCObject(0x6085, 0, 32) },
-  { IMCObjectName::QuickStopOption, IMCObject(0x605A, 0, 16) }
+  { IMCObjectName::QuickStopOption, IMCObject(0x605A, 0, 16) },
+  { IMCObjectName::MotorVoltage, IMCObject(0x2108, 3, 16) }
 };
 
 void PDOmap::addObject(IMCObjectName object_name)

march_hardware/test/mocks/MockIMotionCube.h

@@ -28,6 +28,7 @@ class MockIMotionCube : public march::IMotionCube
   MOCK_METHOD0(getVelocityRadAbsolute, double());
 
   MOCK_METHOD0(getIMCVoltage, float());
+  MOCK_METHOD0(getMotorVoltage, float());
   MOCK_METHOD0(getMotorCurrent, float());
 
   MOCK_METHOD1(actuateRad, void(double));

march_hardware_interface/src/march_hardware_interface.cpp

@@ -312,6 +312,7 @@ void MarchHardwareInterface::reserveMemory()
   imc_state_pub_->msg_.motion_error_description.resize(num_joints_);
   imc_state_pub_->msg_.motor_current.resize(num_joints_);
   imc_state_pub_->msg_.imc_voltage.resize(num_joints_);
+  imc_state_pub_->msg_.motor_voltage.resize(num_joints_);
   imc_state_pub_->msg_.absolute_encoder_value.resize(num_joints_);
   imc_state_pub_->msg_.incremental_encoder_value.resize(num_joints_);
   imc_state_pub_->msg_.absolute_velocity.resize(num_joints_);
@@ -432,6 +433,7 @@ void MarchHardwareInterface::updateIMotionCubeState()
     imc_state_pub_->msg_.motion_error_description[i] = imc_state.motionErrorDescription;
     imc_state_pub_->msg_.motor_current[i] = imc_state.motorCurrent;
     imc_state_pub_->msg_.imc_voltage[i] = imc_state.IMCVoltage;
+    imc_state_pub_->msg_.motor_voltage[i] = imc_state.motorVoltage;
     imc_state_pub_->msg_.absolute_encoder_value[i] = imc_state.absoluteEncoderValue;
     imc_state_pub_->msg_.incremental_encoder_value[i] = imc_state.incrementalEncoderValue;
     imc_state_pub_->msg_.absolute_velocity[i] = imc_state.absoluteVelocity;