motor.hpp

Go to the documentation of this file.

//    Copyright 2023 Christoph Hellmann Santos
//    Copyright 2014-2022 Authors of ros_canopen
//
// This program is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program.  If not, see <https://www.gnu.org/licenses/>.
//
 
#ifndef MOTOR_HPP
#define MOTOR_HPP
 
#include <algorithm>
#include <atomic>
#include <bitset>
#include <condition_variable>
#include <functional>
#include <iostream>
#include <limits>
#include <mutex>
#include <thread>
#include "rclcpp/rclcpp.hpp"
 
#include "canopen_402_driver/default_homing_mode.hpp"
#include "canopen_402_driver/mode_forward_helper.hpp"
#include "canopen_402_driver/profiled_position_mode.hpp"
#include "canopen_base_driver/diagnostic_collector.hpp"
#include "canopen_base_driver/lely_driver_bridge.hpp"
 
namespace ros2_canopen
{
 
typedef ModeForwardHelper<MotorBase::Profiled_Velocity, int32_t, 0x60FF, 0, 0> ProfiledVelocityMode;
typedef ModeForwardHelper<MotorBase::Profiled_Torque, int16_t, 0x6071, 0, 0> ProfiledTorqueMode;
typedef ModeForwardHelper<MotorBase::Cyclic_Synchronous_Position, int32_t, 0x607A, 0, 0>
  CyclicSynchronousPositionMode;
typedef ModeForwardHelper<MotorBase::Cyclic_Synchronous_Velocity, int32_t, 0x60FF, 0, 0>
  CyclicSynchronousVelocityMode;
typedef ModeForwardHelper<MotorBase::Cyclic_Synchronous_Torque, int16_t, 0x6071, 0, 0>
  CyclicSynchronousTorqueMode;
typedef ModeForwardHelper<
  MotorBase::Velocity, int16_t, 0x6042, 0,
  (1 << Command402::CW_Operation_mode_specific0) | (1 << Command402::CW_Operation_mode_specific1) |
    (1 << Command402::CW_Operation_mode_specific2)>
  VelocityMode;
typedef ModeForwardHelper<
  MotorBase::Interpolated_Position, int32_t, 0x60C1, 0x01,
  (1 << Command402::CW_Operation_mode_specific0)>
  InterpolatedPositionMode;
 
class Motor402 : public MotorBase
{
public:
  Motor402(
    std::shared_ptr<LelyDriverBridge> driver, ros2_canopen::State402::InternalState switching_state,
    int homing_timeout_seconds)
  : MotorBase(),
    switching_state_(switching_state),
    monitor_mode_(true),
    state_switch_timeout_(5),
    homing_timeout_seconds_(homing_timeout_seconds)
  {
    this->driver = driver;
  }
 
  virtual bool setTarget(double val);
  virtual bool enterModeAndWait(uint16_t mode);
  virtual bool isModeSupported(uint16_t mode);
  virtual uint16_t getMode();
  bool readState();
 
  void handleDiag();
  bool handleInit();
  void handleRead();
  void handleWrite();
  bool handleShutdown();
  bool handleHalt();
 
  bool handleRecover();
 
  bool handleEnable();
  bool handleDisable();
 
  template <typename T, typename... Args>
  bool registerMode(uint16_t mode, Args &&... args)
  {
    return mode_allocators_
      .insert(std::make_pair(
        mode,
        [args..., mode, this]()
        {
          if (isModeSupportedByDevice(mode)) registerMode(mode, ModeSharedPtr(new T(args...)));
        }))
      .second;
  }
 
  virtual void registerDefaultModes()
  {
    registerMode<ProfiledPositionMode>(MotorBase::Profiled_Position, driver);
    registerMode<VelocityMode>(MotorBase::Velocity, driver);
    registerMode<ProfiledVelocityMode>(MotorBase::Profiled_Velocity, driver);
    registerMode<ProfiledTorqueMode>(MotorBase::Profiled_Torque, driver);
    registerMode<DefaultHomingMode>(MotorBase::Homing, driver, homing_timeout_seconds_);
    registerMode<InterpolatedPositionMode>(MotorBase::Interpolated_Position, driver);
    registerMode<CyclicSynchronousPositionMode>(MotorBase::Cyclic_Synchronous_Position, driver);
    registerMode<CyclicSynchronousVelocityMode>(MotorBase::Cyclic_Synchronous_Velocity, driver);
    registerMode<CyclicSynchronousTorqueMode>(MotorBase::Cyclic_Synchronous_Torque, driver);
  }
 
  double get_speed() const { return (double)this->driver->universal_get_value<int32_t>(0x606C, 0); }
  double get_position() const
  {
    return (double)this->driver->universal_get_value<int32_t>(0x6064, 0);
  }
 
  void set_diagnostic_status_msgs(std::shared_ptr<DiagnosticsCollector> status, bool enable)
  {
    this->enable_diagnostics_.store(enable);
    this->diag_collector_ = status;
  }
 
private:
  virtual bool isModeSupportedByDevice(uint16_t mode);
  void registerMode(uint16_t id, const ModeSharedPtr & m);
 
  ModeSharedPtr allocMode(uint16_t mode);
 
  bool switchMode(uint16_t mode);
  bool switchState(const State402::InternalState & target);
 
  std::atomic<uint16_t> status_word_;
  uint16_t control_word_;
  std::mutex cw_mutex_;
  std::atomic<bool> start_fault_reset_;
  std::atomic<State402::InternalState> target_state_;
 
  State402 state_handler_;
 
  std::mutex map_mutex_;
  std::unordered_map<uint16_t, ModeSharedPtr> modes_;
  typedef std::function<void()> AllocFuncType;
  std::unordered_map<uint16_t, AllocFuncType> mode_allocators_;
 
  ModeSharedPtr selected_mode_;
  uint16_t mode_id_;
  std::condition_variable mode_cond_;
  std::mutex mode_mutex_;
  const State402::InternalState switching_state_;
  const bool monitor_mode_;
  const std::chrono::seconds state_switch_timeout_;
  const int homing_timeout_seconds_;
 
  std::shared_ptr<LelyDriverBridge> driver;
  const uint16_t status_word_entry_index = 0x6041;
  const uint16_t control_word_entry_index = 0x6040;
  const uint16_t op_mode_display_index = 0x6061;
  const uint16_t op_mode_index = 0x6060;
  const uint16_t supported_drive_modes_index = 0x6502;
 
  // Diagnostic components
  std::atomic<bool> enable_diagnostics_;
  std::shared_ptr<DiagnosticsCollector> diag_collector_;
};
 
}  // namespace ros2_canopen
 
#endif