cia402_drive.h

Go to the documentation of this file.

#pragma once
 
#include <chrono>
#include <cstdint>
#include <functional>
#include <list>
#include <map>
#include <mutex>
#include <string>
#include <unordered_map>
#include <variant>
#include <vector>
 
#include "dictionary_symbols.h"
#include "virtual_parameter.h"
 
/*
 * CiA402 State defines
 */
 
#define CIA402_STATUS_WORD_MASQ_A 0x6fu
#define CIA402_STATUS_WORD_MASQ_B 0x4fu
#define CIA402_STATUS_WORD_MASQ_WARN 0x80u
 
#define CIA402_STATUS_NOT_READY 0x00u          /* masq B */
#define CIA402_STATUS_SWITCH_ON_DISABLED 0x40u /* masq B */
#define CIA402_STATUS_READY_SWITCH_ON 0x21u
#define CIA402_STATUS_SWITCHED_ON 0x23u
#define CIA402_STATUS_OP_ENABLED 0x27u
#define CIA402_STATUS_QUICK_STOP_ACTIVE 0x07u
#define CIA402_STATUS_FAULT_REACTION_ACTIVE 0x0fu /* masq B */
#define CIA402_STATUS_FAULT 0x08u                 /* masq B */
 
#define CIA402_STATUS_INTERNAL_LIMIT_REACHED 0x0800u
 
#define CIA402_CONTROL_BIT_ENABLE_OP 0x08u
#define CIA402_CONTROL_BIT_QUICK_STOP 0x04u
#define CIA402_CONTROL_BIT_ENABLE_VOLTAGE 0x02u
#define CIA402_CONTROL_BIT_SWITCH_ON 0x01u
#define CIA402_CONTROL_BIT_FAULT_RESET 0x80u
 
#define FAULT_RESET_TIMEOUT_ATTEMPTS 2000
 
class MotionMasterError;
class VirtualDevice;
class VirtualParameter;
 
class Cia402Drive {
 public:
  enum class OpMode {
    kTuning = -128,
    kOpenLoopFieldMode = -3,
    kDiagnostics = -2,
    kCoggingCompensationRecording = -1,
    kNo = 0,
    kPp,   // Profile position
    kVel,  // Velocity
    kPv,   // Profile velocity
    kPt,   // Profile torque
    kRes,  // Reserved
    kHom,  // Homing
    kIp,   // Interpolated position
    kCsp,  // Cyclic synchronous position
    kCsv,  // Cyclic synchronous velocity
    kCst   // Cyclic synchronous torque
  };
 
  enum class Cia402State {
    kNotReady = 0,
    kSwitchOnDisabled,
    kReadySwitchOn,
    kSwitchedOn,
    kOpEnabled,
    kQuickStopActive,
    kFaultReactionActive,
    kFault
  };
 
  enum class Cia402Command {
    kNone = 0,
    kShutdown,
    kSwitchOn,
    kDisableVoltage,
    kQuickStop,
    kDisableOperation,
    kEnableOperation,
    kFaultReset,
  };
 
  // map<encoder ordinal, encoder configuration index>
  static inline std::map<uint8_t, uint16_t> fw_v5_encoder_configuration_map_ = {
      {1, OD_INDEX_ENCODER_1_CONFIGURATION},
      {2, OD_INDEX_ENCODER_2_CONFIGURATION}};
 
  // map<encoder ordinal, encoder feedback index>
  static inline std::map<uint8_t, uint16_t> fw_v5_encoder_feedback_map_ = {
      {1, OD_INDEX_ENCODER_1_FEEDBACK}, {2, OD_INDEX_ENCODER_2_FEEDBACK}};
 
  enum class EncoderFunction {
    kDisabled,
    kCommutationAndMotionControlFeedback,
    kCommutationAndMonitoring,
    kMotionControlFeedbackOnly,
    kMonitoringOnly,
    kCommutationOnly,
    kPosition,
    kCommutationAndVelocity,
    kVelocity,               // only for FW >= v5
    kCommutationAndPosition  // only for FW >= v5
  };
 
  // Controller (position, velocity, torque) to encoder function
  static inline std::map<std::tuple<bool, bool, bool>, EncoderFunction>
      controller_to_encoder_function_map_ = {
          {{false, false, false}, EncoderFunction::kDisabled},
          {{false, false, true}, EncoderFunction::kCommutationOnly},
          {{false, true, false}, EncoderFunction::kVelocity},
          {{false, true, true}, EncoderFunction::kCommutationAndVelocity},
          {{true, false, false}, EncoderFunction::kPosition},
          {{true, false, true}, EncoderFunction::kCommutationAndPosition},
          {{true, true, false}, EncoderFunction::kMotionControlFeedbackOnly},
          {{true, true, true},
           EncoderFunction::kCommutationAndMotionControlFeedback}};
 
  enum class EncoderType {
    kNone = 0,
    kHallEncoder = 1,
    kQeiEncoder = 2,
    kBissEncoder = 4,
    kRem14Encoder = 5,
    kRem16MtEncoder = 6,
    kSsiEncoder = 7
  };
 
  enum class BrakeStatus { kNotConfigured = 0, kEngaged = 1, kDisengaged = 2 };
 
  enum class BrakeReleaseStrategy {
    kManualOutputVoltage = 0,
    kClutchStyleBrakeControl = 1,
    kPinBrakeControl = 2
  };
 
  explicit Cia402Drive(VirtualDevice* virtual_device);
 
  [[nodiscard]] int32_t get_id() const;
 
  [[nodiscard]] uint16_t get_hardware_id() const;
 
  MotionMasterError init_position_controller();
 
  MotionMasterError init_velocity_controller();
 
  MotionMasterError init_torque_controller();
 
  MotionMasterError init_slave_profile_position();
 
  MotionMasterError init_slave_profile_velocity();
 
  MotionMasterError init_slave_profile_torque();
 
  MotionMasterError deinit_current_controller();
 
  bool execute_profile_position_ramp();
 
  void set_halt_bit(bool value);
 
  void set_target_position(int32_t position);
 
  void set_target_velocity(int32_t velocity);
 
  void set_target_torque(int32_t torque);
 
  uint16_t get_statusword();
 
  int32_t get_target_position();
 
  int32_t get_target_velocity();
 
  int32_t get_target_torque();
 
  Cia402State get_cia402_state();
 
  OpMode get_op_mode();
 
  int32_t get_current_position();
 
  int32_t get_current_velocity();
 
  int32_t get_current_torque();
 
  uint32_t get_current_timestamp();
 
  MotionMasterError set_parameter(uint16_t index, uint8_t subindex,
                                  int32_t value);
 
  MotionMasterError set_parameter(uint16_t index, uint8_t subindex,
                                  int32_t value, bool reload_configuration);
 
  MotionMasterError set_parameter(uint16_t index, uint8_t subindex,
                                  float value);
 
  MotionMasterError set_parameter(uint16_t index, uint8_t subindex, float value,
                                  bool reload_configuration);
 
  MotionMasterError set_parameter(uint16_t index, uint8_t subindex,
                                  double value);
 
  MotionMasterError set_parameter(uint16_t index, uint8_t subindex,
                                  double value, bool reload_configuration);
 
  MotionMasterError set_parameter(uint16_t index, uint8_t subindex,
                                  const std::string& value);
 
  VirtualParameter* get_parameter(uint16_t index, uint8_t subindex);
 
  MotionMasterError reload_drive(bool ignore_fault_reset_timeout = false);
 
  [[nodiscard]] std::variant<MotionMasterError, std::list<std::string>>
  get_file_list() const;
 
  [[nodiscard]] std::variant<MotionMasterError, std::string> read_file(
      const std::string& file_name) const;
 
  MotionMasterError write_file(const std::string& name,
                               const std::string& content);
 
  MotionMasterError remove_file(const std::string& file_name);
 
  std::string get_log();
 
  std::map<uint16_t, EncoderFunction> get_all_encoder_functions();
 
  bool set_encoder_function(uint16_t index, EncoderFunction encoder_function);
 
  EncoderType get_commutation_encoder_type();
 
  uint32_t get_commutation_encoder_resolution();
 
  uint32_t get_position_control_encoder_resolution();
 
  bool is_gearbox_potentially_present();
 
  double get_gear_ratio();
 
  double get_feed_constant();
 
  bool is_brake_engaged();
 
  MotionMasterError engage_brake(bool value);
 
  void reset_torque_target_and_offset();
 
  void quick_stop();
 
  MotionMasterError quick_stop_blocking();
 
  int32_t save_current_configuration_to_flash();
 
  int32_t load_configuration_from_flash();
 
  bool is_running();
 
  bool get_fault_state();
 
  MotionMasterError reset_fault(bool ignore_timeout_error = false);
 
  bool get_warning_status();
 
  bool get_internal_limit_reached();
 
  bool is_operational();
 
  MotionMasterError state_machine_control(OpMode target_op_mode,
                                          Cia402State target_state);
 
  MotionMasterError state_machine_control(Cia402State target_state);
 
  std::vector<int16_t> get_cogging_torque_compensation_data();
 
  MotionMasterError read_encoder_register(uint8_t encoder_ordinal,
                                          uint8_t address, uint8_t& value);
 
  MotionMasterError write_encoder_register(uint8_t encoder_ordinal,
                                           uint8_t address, uint8_t value);
 
  std::tuple<uint8_t, uint8_t, uint8_t> get_controller_encoder_sources();
 
 private:
  VirtualDevice* virtual_device_;
 
  void refresh_current_position();
 
  void set_controlword(uint16_t controlword);
 
  void set_op_mode(OpMode op_mode);
 
  [[nodiscard]] Cia402State cia402_read_state(uint16_t statusword) const;
 
  static uint16_t cia402_command(Cia402Command command, uint16_t controlword);
 
  static uint16_t cia402_go_to_state(Cia402State target_state,
                                     Cia402State current_state,
                                     uint16_t controlword,
                                     bool skip_states = true);
 
  std::vector<VirtualParameter*> get_sensor_port_virtual_parameters();
};