Program Listing for File motor.hpp

↰ Return to documentation for file (/tmp/ws/src/ros2_canopen/canopen_402_driver/include/canopen_402_driver/motor.hpp)

//    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)
  : MotorBase(), switching_state_(switching_state), monitor_mode_(true), state_switch_timeout_(5)
  {
    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();

  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);
    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_;

  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