ros2_canopen master
C++ ROS CANopen Library
Loading...
Searching...
No Matches
profiled_position_mode.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 PROFILED_POSITION_MODE_HPP
19#define PROFILED_POSITION_MODE_HPP
20
21#include <cstdint>
22#include <memory>
25
26namespace ros2_canopen
27{
29{
30 const uint16_t index = 0x607A;
31 std::shared_ptr<LelyDriverBridge> driver;
32
33 double last_target_;
34 uint16_t sw_;
35
36public:
49 ProfiledPositionMode(std::shared_ptr<LelyDriverBridge> driver)
50 : ModeTargetHelper(MotorBase::Profiled_Position)
51 {
52 this->driver = driver;
53 }
54
55 virtual bool start()
56 {
57 sw_ = 0;
58 last_target_ = std::numeric_limits<double>::quiet_NaN();
60 }
61 virtual bool read(const uint16_t & sw)
62 {
63 sw_ = sw;
64 return (sw & MASK_Error) == 0;
65 }
66 virtual bool write(OpModeAccesser & cw)
67 {
68 cw.set(CW_Immediate);
69 if (hasTarget())
70 {
71 int32_t target = getTarget();
72 if ((sw_ & MASK_Acknowledged) == 0 && target != last_target_)
73 {
74 if (cw.get(CW_NewPoint))
75 {
76 cw.reset(CW_NewPoint); // reset if needed
77 }
78 else
79 {
80 driver->universal_set_value(index, 0x0, target);
81 cw.set(CW_NewPoint);
82 last_target_ = target;
83 }
84 }
85 else if (sw_ & MASK_Acknowledged)
86 {
88 }
89 return true;
90 }
91 return false;
92 }
93};
94} // namespace ros2_canopen
95
96#endif // PROFILED_POSITION_MODE_HPP
@ CW_Operation_mode_specific3
Definition command.hpp:69
@ CW_Operation_mode_specific1
Definition command.hpp:65
@ CW_Operation_mode_specific0
Definition command.hpp:64
Definition mode_target_helper.hpp:37
virtual bool start()
Definition mode_target_helper.hpp:83
bool hasTarget()
Definition mode_target_helper.hpp:43
int32_t getTarget()
Definition mode_target_helper.hpp:44
Motor Base Class.
Definition base.hpp:32
Definition profiled_position_mode.hpp:29
virtual bool write(OpModeAccesser &cw)
Definition profiled_position_mode.hpp:66
CW_bits
Definition profiled_position_mode.hpp:44
@ CW_NewPoint
Definition profiled_position_mode.hpp:45
@ CW_Immediate
Definition profiled_position_mode.hpp:46
@ CW_Blending
Definition profiled_position_mode.hpp:47
ProfiledPositionMode(std::shared_ptr< LelyDriverBridge > driver)
Definition profiled_position_mode.hpp:49
virtual bool start()
Definition profiled_position_mode.hpp:55
virtual bool read(const uint16_t &sw)
Definition profiled_position_mode.hpp:61
SW_masks
Definition profiled_position_mode.hpp:38
@ MASK_Acknowledged
Definition profiled_position_mode.hpp:40
@ MASK_Error
Definition profiled_position_mode.hpp:41
@ MASK_Reached
Definition profiled_position_mode.hpp:39
@ SW_Target_reached
Definition state.hpp:42
@ SW_Operation_mode_specific0
Definition state.hpp:44
@ SW_Operation_mode_specific1
Definition state.hpp:45
Definition word_accessor.hpp:27
bool reset(uint8_t bit)
Definition word_accessor.hpp:38
bool set(uint8_t bit)
Definition word_accessor.hpp:32
bool get(uint8_t bit) const
Definition word_accessor.hpp:44
Definition configuration_manager.hpp:28