ros2_canopen  master
C++ ROS CANopen Library
motor.hpp
Go to the documentation of this file.
1 // Copyright 2023 Christoph Hellmann Santos
2 // Copyright 2014-2022 Authors of ros_canopen
3 //
4 // This program is free software: you can redistribute it and/or modify
5 // it under the terms of the GNU General Public License as published by
6 // the Free Software Foundation, either version 3 of the License, or
7 // (at your option) any later version.
8 //
9 // This program is distributed in the hope that it will be useful,
10 // but WITHOUT ANY WARRANTY; without even the implied warranty of
11 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 // GNU General Public License for more details.
13 //
14 // You should have received a copy of the GNU General Public License
15 // along with this program. If not, see <https://www.gnu.org/licenses/>.
16 //
17 
18 #ifndef MOTOR_HPP
19 #define MOTOR_HPP
20 
21 #include <algorithm>
22 #include <atomic>
23 #include <bitset>
24 #include <condition_variable>
25 #include <functional>
26 #include <iostream>
27 #include <limits>
28 #include <mutex>
29 #include <thread>
30 #include "rclcpp/rclcpp.hpp"
31 
37 
38 namespace ros2_canopen
39 {
40 
49 typedef ModeForwardHelper<
50  MotorBase::Velocity, int16_t, 0x6042, 0,
54 typedef ModeForwardHelper<
55  MotorBase::Interpolated_Position, int32_t, 0x60C1, 0x01,
58 
59 class Motor402 : public MotorBase
60 {
61 public:
63  std::shared_ptr<LelyDriverBridge> driver, ros2_canopen::State402::InternalState switching_state)
64  : MotorBase(), switching_state_(switching_state), monitor_mode_(true), state_switch_timeout_(5)
65  {
66  this->driver = driver;
67  }
68 
69  virtual bool setTarget(double val);
70  virtual bool enterModeAndWait(uint16_t mode);
71  virtual bool isModeSupported(uint16_t mode);
72  virtual uint16_t getMode();
73  bool readState();
74 
82  void handleDiag();
91  bool handleInit();
99  void handleRead();
107  void handleWrite();
122  bool handleHalt();
123 
132 
147  template <typename T, typename... Args>
148  bool registerMode(uint16_t mode, Args &&... args)
149  {
150  return mode_allocators_
151  .insert(std::make_pair(
152  mode,
153  [args..., mode, this]()
154  {
155  if (isModeSupportedByDevice(mode)) registerMode(mode, ModeSharedPtr(new T(args...)));
156  }))
157  .second;
158  }
159 
164  virtual void registerDefaultModes()
165  {
166  registerMode<ProfiledPositionMode>(MotorBase::Profiled_Position, driver);
167  registerMode<VelocityMode>(MotorBase::Velocity, driver);
168  registerMode<ProfiledVelocityMode>(MotorBase::Profiled_Velocity, driver);
169  registerMode<ProfiledTorqueMode>(MotorBase::Profiled_Torque, driver);
170  registerMode<DefaultHomingMode>(MotorBase::Homing, driver);
171  registerMode<InterpolatedPositionMode>(MotorBase::Interpolated_Position, driver);
172  registerMode<CyclicSynchronousPositionMode>(MotorBase::Cyclic_Synchronous_Position, driver);
173  registerMode<CyclicSynchronousVelocityMode>(MotorBase::Cyclic_Synchronous_Velocity, driver);
174  registerMode<CyclicSynchronousTorqueMode>(MotorBase::Cyclic_Synchronous_Torque, driver);
175  }
176 
177  double get_speed() const { return (double)this->driver->universal_get_value<int32_t>(0x606C, 0); }
178  double get_position() const
179  {
180  return (double)this->driver->universal_get_value<int32_t>(0x6064, 0);
181  }
182 
183  void set_diagnostic_status_msgs(std::shared_ptr<DiagnosticsCollector> status, bool enable)
184  {
185  this->enable_diagnostics_.store(enable);
186  this->diag_collector_ = status;
187  }
188 
189 private:
190  virtual bool isModeSupportedByDevice(uint16_t mode);
191  void registerMode(uint16_t id, const ModeSharedPtr & m);
192 
193  ModeSharedPtr allocMode(uint16_t mode);
194 
195  bool switchMode(uint16_t mode);
196  bool switchState(const State402::InternalState & target);
197 
198  std::atomic<uint16_t> status_word_;
199  uint16_t control_word_;
200  std::mutex cw_mutex_;
201  std::atomic<bool> start_fault_reset_;
202  std::atomic<State402::InternalState> target_state_;
203 
204  State402 state_handler_;
205 
206  std::mutex map_mutex_;
207  std::unordered_map<uint16_t, ModeSharedPtr> modes_;
208  typedef std::function<void()> AllocFuncType;
209  std::unordered_map<uint16_t, AllocFuncType> mode_allocators_;
210 
211  ModeSharedPtr selected_mode_;
212  uint16_t mode_id_;
213  std::condition_variable mode_cond_;
214  std::mutex mode_mutex_;
215  const State402::InternalState switching_state_;
216  const bool monitor_mode_;
217  const std::chrono::seconds state_switch_timeout_;
218 
219  std::shared_ptr<LelyDriverBridge> driver;
220  const uint16_t status_word_entry_index = 0x6041;
221  const uint16_t control_word_entry_index = 0x6040;
222  const uint16_t op_mode_display_index = 0x6061;
223  const uint16_t op_mode_index = 0x6060;
224  const uint16_t supported_drive_modes_index = 0x6502;
225 
226  // Diagnostic components
227  std::atomic<bool> enable_diagnostics_;
228  std::shared_ptr<DiagnosticsCollector> diag_collector_;
229 };
230 
231 } // namespace ros2_canopen
232 
233 #endif
@ CW_Operation_mode_specific1
Definition: command.hpp:65
@ CW_Operation_mode_specific0
Definition: command.hpp:64
@ CW_Operation_mode_specific2
Definition: command.hpp:66
Definition: mode_forward_helper.hpp:31
Definition: motor.hpp:60
void handleDiag()
Updates the device diagnostic information.
Motor402(std::shared_ptr< LelyDriverBridge > driver, ros2_canopen::State402::InternalState switching_state)
Definition: motor.hpp:62
bool handleShutdown()
Shutdowns the drive.
bool handleHalt()
Executes a quickstop.
double get_position() const
Definition: motor.hpp:178
virtual bool isModeSupported(uint16_t mode)
Check if Operation Mode is supported.
bool handleInit()
Initialise the drive.
double get_speed() const
Definition: motor.hpp:177
bool handleRecover()
Recovers the device from fault.
virtual bool enterModeAndWait(uint16_t mode)
Enter Operation Mode.
void handleRead()
Read objects of the drive.
virtual bool setTarget(double val)
Set target.
virtual uint16_t getMode()
Get current Mode.
void handleWrite()
Writes objects to the drive.
virtual void registerDefaultModes()
Tries to register the standard operation modes defined in cia402.
Definition: motor.hpp:164
bool registerMode(uint16_t mode, Args &&... args)
Register a new operation mode for the drive.
Definition: motor.hpp:148
void set_diagnostic_status_msgs(std::shared_ptr< DiagnosticsCollector > status, bool enable)
Definition: motor.hpp:183
Motor Base Class.
Definition: base.hpp:32
@ Profiled_Velocity
Definition: base.hpp:42
@ Cyclic_Synchronous_Velocity
Definition: base.hpp:48
@ Cyclic_Synchronous_Torque
Definition: base.hpp:49
@ Cyclic_Synchronous_Position
Definition: base.hpp:47
@ Profiled_Position
Definition: base.hpp:40
@ Homing
Definition: base.hpp:45
@ Profiled_Torque
Definition: base.hpp:43
@ Interpolated_Position
Definition: base.hpp:46
@ Velocity
Definition: base.hpp:41
Definition: state.hpp:28
InternalState
Definition: state.hpp:50
Definition: configuration_manager.hpp:28
ModeForwardHelper< MotorBase::Cyclic_Synchronous_Position, int32_t, 0x607A, 0, 0 > CyclicSynchronousPositionMode
Definition: motor.hpp:44
std::shared_ptr< Mode > ModeSharedPtr
Definition: mode.hpp:45
ModeForwardHelper< MotorBase::Cyclic_Synchronous_Velocity, int32_t, 0x60FF, 0, 0 > CyclicSynchronousVelocityMode
Definition: motor.hpp:46
ModeForwardHelper< MotorBase::Profiled_Velocity, int32_t, 0x60FF, 0, 0 > ProfiledVelocityMode
Definition: motor.hpp:41
ModeForwardHelper< MotorBase::Profiled_Torque, int16_t, 0x6071, 0, 0 > ProfiledTorqueMode
Definition: motor.hpp:42
ModeForwardHelper< MotorBase::Cyclic_Synchronous_Torque, int16_t, 0x6071, 0, 0 > CyclicSynchronousTorqueMode
Definition: motor.hpp:48