24 #include <condition_variable>
30 #include "rclcpp/rclcpp.hpp"
64 :
MotorBase(), switching_state_(switching_state), monitor_mode_(true), state_switch_timeout_(5)
66 this->driver = driver;
147 template <
typename T,
typename... Args>
150 return mode_allocators_
151 .insert(std::make_pair(
153 [args..., mode,
this]()
155 if (isModeSupportedByDevice(mode)) registerMode(mode, ModeSharedPtr(new T(args...)));
177 double get_speed()
const {
return (
double)this->driver->universal_get_value<int32_t>(0x606C, 0); }
180 return (
double)this->driver->universal_get_value<int32_t>(0x6064, 0);
185 this->enable_diagnostics_.store(enable);
186 this->diag_collector_ = status;
190 virtual bool isModeSupportedByDevice(uint16_t mode);
195 bool switchMode(uint16_t mode);
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_;
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_;
213 std::condition_variable mode_cond_;
214 std::mutex mode_mutex_;
216 const bool monitor_mode_;
217 const std::chrono::seconds state_switch_timeout_;
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;
227 std::atomic<bool> enable_diagnostics_;
228 std::shared_ptr<DiagnosticsCollector> diag_collector_;
@ 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
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
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