Make (empty) implementation of the remaining template methods

Author: Roelemans

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/motor_controller/imotioncube/imotioncube.h

@@ -77,7 +77,7 @@ class IMotionCube : public MotorController, public Slave
   void goToTargetState(IMotionCubeTargetState target_state);
   virtual void prepareActuation() override;
 
-  uint16_t getSlaveIndex() const override;
+  int getSlaveIndex() const override;
   virtual void reset() override;
 
   /** @brief Override comparison operator */

march_hardware/include/march_hardware/motor_controller/motor_controller.h

@@ -23,7 +23,7 @@ class MotorController
    * Getter for the slave index.
    * @return slave index if the motor controller is an ethercat slave, else -1
    */
-  virtual uint16_t getSlaveIndex() const = 0;
+  virtual int getSlaveIndex() const = 0;
 
   virtual float getMotorCurrent() = 0;
   virtual float getMotorControllerVoltage() = 0;

march_hardware/include/march_hardware/motor_controller/odrive/odrive_enums.h

@@ -8,6 +8,8 @@
 #define O_PM_ENCODER_VELOCITY_UI ".encoder.vel_estimate"
 #define O_PM_ODRIVE_INPUT_VOLTAGE "vbus_voltage"
 #define O_PM_ACTUAL_MOTOR_CURRENT ".motor.current_control.Iq_measured"
+#define O_PM_ACTUAL_MOTOR_VOLTAGE_D ".motor.current_control.v_current_control_integral_d"
+#define O_PM_ACTUAL_MOTOR_VOLTAGE_Q ".motor.current_control.v_current_control_integral_q"
 
 // axis states
 enum States : int

march_hardware/include/march_hardware/motor_controller/odrive/odrive_motor.h

@@ -20,24 +20,45 @@ class OdriveMotor : public Odrive
 public:
   OdriveMotor(const std::string& axisNumber, std::shared_ptr<OdriveEndpoint> odriveEndpoint, ActuationMode mode);
 
-  int initialize();
+  bool initialize(int cycle_time) override;
+  void prepareActuation() override;
+  void reset() override;
 
-  int setState(uint8_t state);
-  int getState();
+  void actuateRad(double target_rad) override;
+  void actuateTorque(int16_t target_torque) override;
+
+  MotorControllerStates& getStates() override;
+
+  float getMotorControllerVoltage() override;
+  float getMotorCurrent() override;
+  float getMotorVoltage() override;
+  double getTorque() override;
 
-  float getMotorControllerVoltage();
-  float getMotorCurrent();
-  double getTorque();
+  double getAngleRadAbsolute() override;
+  double getVelocityRadAbsolute() override;
 
-  double getAngleRadAbsolute();
-  double getVelocityRadAbsolute();
+  double getAngleRadIncremental() override;
+  double getVelocityRadIncremental() override;
 
-  double getAngleRadIncremental();
-  double getVelocityRadIncremental();
+  bool getIncrementalMorePrecise() const override;
+  ActuationMode getActuationMode() const
+  {
+    return this->mode_;
+  }
+  int getSlaveIndex() const
+  {
+    return -1;
+  }
 
 private:
   std::string create_command(std::string command_name);
-  ActuationMode mode;
+  int setState(uint8_t state);
+  int getState();
+
+  int getAngleCountsAbsolute();
+  int getAngleCountsIncremental();
+
+  ActuationMode mode_;
 };
 
 }  // namespace march

march_hardware/include/march_hardware/motor_controller/odrive/odrive_states.h

@@ -0,0 +1,35 @@
+// Copyright 2019 Project March.
+#ifndef MARCH_HARDWARE_IMOTIONCUBE_STATES_H
+#define MARCH_HARDWARE_IMOTIONCUBE_STATES_H
+
+#include <string>
+#include "march_hardware/motor_controller/motor_controller_states.h"
+#include "odrive_enums.h"
+
+namespace march
+{
+struct OdriveStates : public MotorControllerStates
+{
+public:
+  OdriveStates() = default;
+
+  States state;
+
+  virtual bool checkState()
+  {
+    return true;
+  }
+
+  virtual std::string getErrorStatus()
+  {
+    std::ostringstream error_stream;
+//    std::string state = IMCStateOfOperation(this->statusWord).getString().c_str();
+
+    error_stream << "State: " << state;
+    return error_stream.str();
+  }
+};
+
+}  // namespace march
+
+#endif  // MARCH_HARDWARE_IMOTIONCUBE_STATES_H

march_hardware/src/imotioncube/imotioncube.cpp

@@ -206,7 +206,7 @@ void IMotionCube::actuateTorque(int16_t target_torque)
   this->write16(target_torque_location, target_torque_struct);
 }
 
-uint16_t IMotionCube::getSlaveIndex() const
+int IMotionCube::getSlaveIndex() const
 {
   return this->Slave::getSlaveIndex();
 }

march_hardware/src/odrive/odrive_motor.cpp

@@ -1,47 +1,65 @@
 #include "march_hardware/motor_controller/odrive/odrive_motor.h"
+#include "march_hardware/motor_controller/odrive/odrive_states.h"
 
 #include <utility>
 
 namespace march
 {
 OdriveMotor::OdriveMotor(const std::string& axisNumber, std::shared_ptr<OdriveEndpoint> odriveEndpoint,
                          ActuationMode mode)
-  : Odrive(axisNumber, std::move(odriveEndpoint), false), mode(mode)
+  : Odrive(axisNumber, std::move(odriveEndpoint), false), mode_(mode)
 {
 }
 
-int OdriveMotor::initialize()
+bool OdriveMotor::initialize(int cycle_time)
+{
+  return cycle_time;
+}
+
+void OdriveMotor::prepareActuation()
 {
   if (this->setState(States::AXIS_STATE_FULL_CALIBRATION_SEQUENCE) == 1)
   {
-    ROS_ERROR("Calibration sequence was not finished successfully");
-    return ODRIVE_ERROR;
+    ROS_FATAL("Calibration sequence was not finished successfully");
   }
+}
 
-  return ODRIVE_OK;
+// to be implemented
+    void OdriveMotor::reset()
+    {
+        return;
+    }
+
+void OdriveMotor::actuateRad(double target_rad)
+{
+  ROS_INFO("Actuating rad %f", target_rad);
+  return;
 }
 
-int OdriveMotor::setState(uint8_t state)
+void OdriveMotor::actuateTorque(int16_t target_torque)
 {
-  std::string command_name_ = this->create_command(O_PM_REQUEST_STATE);
-  if (this->write(command_name_, state) == 1)
-  {
-    ROS_ERROR("Could net set state; %i to the axis", state);
-    return ODRIVE_ERROR;
-  }
-  return ODRIVE_OK
+    ROS_INFO("Actuating torque %d", target_torque);
+  return;
 }
-int OdriveMotor::getState()
+
+MotorControllerStates& OdriveMotor::getStates()
 {
-  int axis_state;
-  std::string command_name_ = this->create_command(O_PM_CURRENT_STATE);
-  if (this->read(command_name_, axis_state) == 1)
-  {
-    ROS_ERROR("Could not retrieve incremental position of the encoder");
-    return ODRIVE_ERROR;
-  }
+    static OdriveStates states;
+
+    // Common states
+    states.motorCurrent = this->getMotorCurrent();
+    states.controllerVoltage = this->getMotorControllerVoltage();
+    states.motorVoltage = this->getMotorVoltage();
+
+    states.absoluteEncoderValue = this->getAngleCountsAbsolute();
+    states.incrementalEncoderValue = this->getAngleCountsIncremental();
+    states.absoluteVelocity = this->getVelocityRadAbsolute();
+    states.incrementalVelocity = this->getVelocityRadIncremental();
+
+    states.state = States(this->getState());
+
+    return states;
 
-  return axis_state;
 }
 
 float OdriveMotor::getMotorControllerVoltage()
@@ -50,23 +68,38 @@ float OdriveMotor::getMotorControllerVoltage()
   std::string command_name_ = this->create_command(O_PM_ODRIVE_INPUT_VOLTAGE);
   if (this->read(command_name_, motor_controller_voltage) == 1)
   {
-    ROS_ERROR("Could not retrieve incremental position of the encoder");
+    ROS_ERROR("Could not retrieve motor controller voltage");
     return ODRIVE_ERROR;
   }
   return motor_controller_voltage;
 }
+
 float OdriveMotor::getMotorCurrent()
 {
   float motor_current;
   std::string command_name_ = this->create_command(O_PM_ACTUAL_MOTOR_CURRENT);
   if (this->read(command_name_, motor_current) == 1)
   {
-    ROS_ERROR("Could not retrieve incremental position of the encoder");
+    ROS_ERROR("Could not retrieve motor courrent");
     return ODRIVE_ERROR;
   }
 
   return motor_current;
 }
+
+float OdriveMotor::getMotorVoltage()
+{
+  float motor_voltage;
+  std::string command_name_ = this->create_command(O_PM_ACTUAL_MOTOR_VOLTAGE_D);
+  if (this->read(command_name_, motor_voltage) == 1)
+  {
+    ROS_ERROR("Could not retrieve motor voltage");
+    return ODRIVE_ERROR;
+  }
+
+  return motor_voltage;
+}
+
 double OdriveMotor::getTorque()
 {
   double motor_current = this->getMotorCurrent();
@@ -75,35 +108,52 @@ double OdriveMotor::getTorque()
   return motor_torque;
 }
 
+int OdriveMotor::getAngleCountsAbsolute()
+{
+    return 0;
+}
+
 double OdriveMotor::getAngleRadAbsolute()
 {
-  return 0;
+    double angle_rad = this->getAngleCountsAbsolute() * PI_2 / std::pow(2, 17);
+    return angle_rad;
 }
+
 double OdriveMotor::getVelocityRadAbsolute()
 {
   return 0;
 }
 
-double OdriveMotor::getAngleRadIncremental()
+bool OdriveMotor::getIncrementalMorePrecise() const
 {
-  float iu_position;
-  std::string command_name_ = this->create_command(O_PM_ENCODER_POSITION_UI);
-  if (this->read(command_name_, iu_position) == 1)
-  {
-    ROS_ERROR("Could not retrieve incremental position of the encoder");
-    return ODRIVE_ERROR;
-  }
+  return true;
+}
 
-  double angle_rad = iu_position * PI_2 / std::pow(2, 12);
+int OdriveMotor::getAngleCountsIncremental()
+{
+    float iu_position;
+    std::string command_name_ = this->create_command(O_PM_ENCODER_POSITION_UI);
+    if (this->read(command_name_, iu_position) == 1)
+    {
+        ROS_ERROR("Could not retrieve incremental position of the encoder");
+        return ODRIVE_ERROR;
+    }
+    return iu_position;
+}
+
+double OdriveMotor::getAngleRadIncremental()
+{
+  double angle_rad = this->getAngleCountsIncremental() * PI_2 / std::pow(2, 12);
   return angle_rad;
 }
+
 double OdriveMotor::getVelocityRadIncremental()
 {
   float iu_velocity;
   std::string command_name_ = this->create_command(O_PM_ENCODER_VELOCITY_UI);
   if (this->read(command_name_, iu_velocity) == 1)
   {
-    ROS_ERROR("Could not retrieve incremental position of the encoder");
+    ROS_ERROR("Could not retrieve incremental velocity of the encoder");
     return ODRIVE_ERROR;
   }
 
@@ -120,4 +170,28 @@ std::string OdriveMotor::create_command(std::string command_name)
   return command_name;
 }
 
+int OdriveMotor::setState(uint8_t state)
+{
+  std::string command_name_ = this->create_command(O_PM_REQUEST_STATE);
+  if (this->write(command_name_, state) == 1)
+  {
+    ROS_ERROR("Could net set state; %i to the axis", state);
+    return ODRIVE_ERROR;
+  }
+  return ODRIVE_OK
+}
+
+int OdriveMotor::getState()
+{
+  int axis_state;
+  std::string command_name_ = this->create_command(O_PM_CURRENT_STATE);
+  if (this->read(command_name_, axis_state) == 1)
+  {
+    ROS_ERROR("Could not retrieve state of the motor");
+    return ODRIVE_ERROR;
+  }
+
+  return axis_state;
+}
+
 }  // namespace march

march_hardware/test/mocks/mock_motor_controller.h

@@ -18,7 +18,7 @@ class MockMotorController : public march::MotorController
   MOCK_METHOD0(prepareActuation, void());
   MOCK_CONST_METHOD0(getActuationMode, march::ActuationMode());
 
-  MOCK_CONST_METHOD0(getSlaveIndex, uint16_t());
+  MOCK_CONST_METHOD0(getSlaveIndex, int());
   MOCK_CONST_METHOD0(getIncrementalMorePrecise, bool());
   MOCK_METHOD0(getStates, march::MotorControllerStates&());
 

march_hardware_builder/src/hardware_builder.cpp

@@ -134,7 +134,7 @@ march::Joint HardwareBuilder::createJoint(const YAML::Node& joint_config, const
   }
   if (joint_config["odrive"])
   {
-    HardwareBuilder::createOdrive(joint_config["odrive"], mode, urdf_joint, usb_master);
+      controller = HardwareBuilder::createOdrive(joint_config["odrive"], mode, urdf_joint, usb_master);
   }
   if (!controller)
   {