Read parameters only once each cycle

Author: Roelemans

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

@@ -22,6 +22,8 @@
 #define ODRIVE_OK 0;
 #define ODRIVE_ERROR 1;
 
+static constexpr double PI_2 = 2 * M_PI;
+
 namespace march
 {
 typedef struct odrive_json_object
@@ -75,6 +77,23 @@ class Odrive : public MotorController
 
   std::string axis_number;
 
+protected:
+  float readMotorControllerVoltage();
+  float readMotorCurrent();
+  float readMotorVoltage();
+
+  uint16_t readAxisError();
+  uint16_t readAxisMotorError();
+  uint8_t readAxisEncoderError();
+  uint8_t readAxisControllerError();
+
+  int readAngleCountsAbsolute();
+  double readVelocityRadAbsolute();
+
+  int readAngleCountsIncremental();
+  double readVelocityRadIncremental();
+  std::string create_command(std::string command_name);
+
 private:
   int json_string_read(const Json::Value& json_parameter_object);
 

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

@@ -9,7 +9,6 @@
 #include "odrive_enums.h"
 #include <march_hardware/motor_controller/actuation_mode.h>
 
-static constexpr double PI_2 = 2 * M_PI;
 static constexpr double MOTOR_KV = 100;
 static constexpr double CURRENT_TO_TORQUE_CONVERSION = 8.27;
 
@@ -60,7 +59,6 @@ class OdriveMotor : public Odrive
   }
 
 private:
-  std::string create_command(std::string command_name);
   int setState(uint8_t state);
   uint8_t getState();
 
@@ -69,6 +67,22 @@ class OdriveMotor : public Odrive
 
   ActuationMode mode_;
   std::string json_config_file_path_;
+
+  void readValues();
+
+  uint16_t axis_error;
+  uint16_t axis_motor_error;
+  uint8_t axis_encoder_error;
+  uint8_t axis_controller_error;
+
+  float motor_controller_voltage;
+  float motor_current;
+  float motor_voltage;
+
+  double angle_counts_absolute;
+  double velocity_rad_absolute;
+  double angle_counts_incremental;
+  double velocity_rad_incremental;
 };
 
 }  // namespace march

march_hardware/src/odrive/odrive.cpp

@@ -391,6 +391,141 @@ int Odrive::setConfigurations(const std::string& configuration_json_path)
   return ODRIVE_OK
 }
 
+uint16_t Odrive::readAxisError()
+{
+  uint16_t axis_error;
+  std::string command_name_ = this->create_command(O_PM_AXIS_ERROR);
+  if (this->read(command_name_, axis_error) == 1)
+  {
+    ROS_ERROR("Could not retrieve axis error");
+    return ODRIVE_ERROR;
+  }
+
+  return axis_error;
+}
+
+uint16_t Odrive::readAxisMotorError()
+{
+  uint16_t axis_motor_error;
+  std::string command_name_ = this->create_command(O_PM_AXIS_MOTOR_ERROR);
+  if (this->read(command_name_, axis_motor_error) == 1)
+  {
+    ROS_ERROR("Could not retrieve axis motor error");
+    return ODRIVE_ERROR;
+  }
+
+  return axis_motor_error;
+}
+
+uint8_t Odrive::readAxisEncoderError()
+{
+  uint8_t axis_encoder_error;
+  std::string command_name_ = this->create_command(O_PM_AXIS_ENCODER_ERROR);
+  if (this->read(command_name_, axis_encoder_error) == 1)
+  {
+    ROS_ERROR("Could not retrieve axis encoder error");
+    return ODRIVE_ERROR;
+  }
+
+  return axis_encoder_error;
+}
+
+uint8_t Odrive::readAxisControllerError()
+{
+  uint8_t axis_controller_error;
+  std::string command_name_ = this->create_command(O_PM_AXIS_CONTROLLER_ERROR);
+  if (this->read(command_name_, axis_controller_error) == 1)
+  {
+    ROS_ERROR("Could not retrieve axis controller error");
+    return ODRIVE_ERROR;
+  }
+
+  return axis_controller_error;
+}
+
+float Odrive::readMotorControllerVoltage()
+{
+  float motor_controller_voltage;
+  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 motor controller voltage");
+    return ODRIVE_ERROR;
+  }
+  return motor_controller_voltage;
+}
+
+float Odrive::readMotorCurrent()
+{
+  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 motor current");
+    return ODRIVE_ERROR;
+  }
+
+  return motor_current;
+}
+
+float Odrive::readMotorVoltage()
+{
+  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;
+}
+
+int Odrive::readAngleCountsAbsolute()
+{
+  return 0;
+}
+
+double Odrive::readVelocityRadAbsolute()
+{
+  return 0;
+}
+
+int Odrive::readAngleCountsIncremental()
+{
+  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 Odrive::readVelocityRadIncremental()
+{
+  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 velocity of the encoder");
+    return ODRIVE_ERROR
+  }
+
+  double angle_rad = iu_velocity * PI_2 / (std::pow(2, 12) * 101);
+  return angle_rad;
+}
+
+std::string Odrive::create_command(std::string command_name)
+{
+  if (command_name.at(0) == '.')
+  {
+    return this->axis_number + command_name;
+  }
+  return command_name;
+}
+
 template int Odrive::validateType(const odrive_json_object& json_object, int8_t&);
 template int Odrive::validateType(const odrive_json_object& json_object, int16_t&);
 template int Odrive::validateType(const odrive_json_object& json_object, int32_t&);