yarp-devices
Loading...
Searching...
No Matches
EmulatedControlBoard.hpp
1// -*- mode:C++; tab-width:4; c-basic-offset:4; indent-tabs-mode:nil -*-
2
3#ifndef __EMULATED_CONTROL_BOARD_HPP__
4#define __EMULATED_CONTROL_BOARD_HPP__
5
6#include <mutex>
7#include <vector>
8
9#include <yarp/conf/version.h>
10
11#include <yarp/os/PeriodicThread.h>
12
13#include <yarp/dev/DeviceDriver.h>
14#include <yarp/dev/IControlLimits.h>
15#include <yarp/dev/IControlMode.h>
16#include <yarp/dev/IEncodersTimed.h>
17#include <yarp/dev/IPositionControl.h>
18#include <yarp/dev/IPositionDirect.h>
19#include <yarp/dev/IVelocityControl.h>
20#include <yarp/dev/PolyDriver.h>
21
22#include "EmulatedControlBoard_ParamsParser.h"
23
24#if YARP_VERSION_COMPARE(>=, 4, 0, 0)
25#define CHECK_JOINT(j) do { if ((j) < 0 || (j) >= m_axes) return yarp::dev::ReturnValue_error_input_out_of_bounds; } while (0)
26#else
27#define CHECK_JOINT(j) do { if ((j) < 0 || (j) >= m_axes) return false; } while (0)
28#endif
29
40class EmulatedControlBoard : public yarp::dev::DeviceDriver,
41 public yarp::dev::IControlLimits,
42 public yarp::dev::IControlMode,
43 public yarp::dev::IEncodersTimed,
44 public yarp::dev::IPositionControl,
45 public yarp::dev::IPositionDirect,
46 public yarp::dev::IVelocityControl,
47 public yarp::os::PeriodicThread,
48 public EmulatedControlBoard_ParamsParser
49{
50public:
51#if YARP_VERSION_COMPARE(>=, 4, 0, 0)
52 using return_t = yarp::dev::ReturnValue;
53#else
54 using return_t = bool;
55#endif
56
57 // Set the thread period in the class constructor
58 EmulatedControlBoard() : yarp::os::PeriodicThread(1.0) {} // In seconds
59
60 // ------- IPositionControl declarations. Implementation in IPositionControlImpl.cpp -------
61#if YARP_VERSION_COMPARE(>=, 4, 0, 0)
62 return_t getAxes(std::size_t & ax) override;
63#else
64 return_t getAxes(int * ax) override;
65#endif
66 return_t positionMove(int j, double ref) override;
67 return_t positionMove(const double * refs) override;
68 return_t positionMove(int n_joint, const int * joints, const double * refs) override;
69 return_t relativeMove(int j, double delta) override;
70 return_t relativeMove(const double * deltas) override;
71 return_t relativeMove(int n_joint, const int * joints, const double * deltas) override;
72#if YARP_VERSION_COMPARE(>=, 4, 0, 0)
73 return_t checkMotionDone(int j, bool & flag) override;
74 return_t checkMotionDone(bool & flag) override;
75 return_t checkMotionDone(const std::vector<int> & joints, bool & flag) override;
76 return_t setTrajSpeed(int j, double sp) override;
77 return_t setTrajSpeeds(const double * spds) override;
78 return_t setTrajSpeeds(int n_joint, const int * joints, const double * spds) override;
79 return_t getTrajSpeed(int j, double * ref) override;
80 return_t getTrajSpeeds(double * spds) override;
81 return_t getTrajSpeeds(int n_joint, const int * joints, double * spds) override;
82 return_t setTrajAcceleration(int j, double acc) override;
83 return_t setTrajAccelerations(const double * accs) override;
84 return_t setTrajAccelerations(int n_joint, const int * joints, const double * accs) override;
85 return_t getTrajAcceleration(int j, double * acc) override;
86 return_t getTrajAccelerations(double * accs) override;
87 return_t getTrajAccelerations(int n_joint, const int * joints, double * accs) override;
88#else
89 return_t checkMotionDone(int j, bool * flag) override;
90 return_t checkMotionDone(bool * flag) override;
91 return_t checkMotionDone(int n_joint, const int * joints, bool * flags) override;
92 return_t setRefSpeed(int j, double sp) override;
93 return_t setRefSpeeds(const double * spds) override;
94 return_t setRefSpeeds(int n_joint, const int * joints, const double * spds) override;
95 return_t getRefSpeed(int j, double * ref) override;
96 return_t getRefSpeeds(double * spds) override;
97 return_t getRefSpeeds(int n_joint, const int * joints, double * spds) override;
98 return_t setRefAcceleration(int j, double acc) override;
99 return_t setRefAccelerations(const double * accs) override;
100 return_t setRefAccelerations(int n_joint, const int * joints, const double * accs) override;
101 return_t getRefAcceleration(int j, double * acc) override;
102 return_t getRefAccelerations(double * accs) override;
103 return_t getRefAccelerations(int n_joint, const int * joints, double * accs) override;
104#endif
105 return_t stop(int j) override;
106 return_t stop() override;
107 return_t stop(int n_joint, const int * joints) override;
108 return_t getTargetPosition(int joint, double * ref) override;
109 return_t getTargetPositions(double * refs) override;
110 return_t getTargetPositions(int n_joint, const int * joints, double * refs) override;
111
112 // ------- IPositionDirect declarations. Implementation in IPositionDirectImpl.cpp -------
113 return_t setPosition(int j, double ref) override;
114 return_t setPositions(int n_joint, const int * joints, const double * refs) override;
115 return_t setPositions(const double * refs) override;
116 return_t getRefPosition(int joint, double * ref) override;
117 return_t getRefPositions(double * refs) override;
118 return_t getRefPositions(int n_joint, const int * joints, double * refs) override;
119
120 // ---------- IEncodersTimed Declarations. Implementation in IEncoderImpl.cpp ----------
121 return_t resetEncoder(int j) override;
122 return_t resetEncoders() override;
123 return_t setEncoder(int j, double val) override;
124 return_t setEncoders(const double * vals) override;
125 return_t getEncoder(int j, double * v) override;
126 return_t getEncoders(double * encs) override;
127 return_t getEncodersTimed(double * encs, double * time) override;
128 return_t getEncoderTimed(int j, double * encs, double * time) override;
129 return_t getEncoderSpeed(int j, double * sp) override;
130 return_t getEncoderSpeeds(double * spds) override;
131 return_t getEncoderAcceleration(int j, double * spds) override;
132 return_t getEncoderAccelerations(double * accs) override;
133
134 // --------- IVelocityControl Declarations. Implementation in IVelocityControlImpl.cpp ---------
135 return_t velocityMove(int j, double sp) override;
136 return_t velocityMove(const double * sp) override;
137 return_t velocityMove(int n_joint, const int * joints, const double * spds) override;
138#if YARP_VERSION_COMPARE(>=, 4, 0, 0)
139 return_t getTargetVelocity(int joint, double * vel) override;
140 return_t getTargetVelocities(double * vels) override;
141 return_t getTargetVelocities(int n_joint, const int * joints, double * vels) override;
142#else
143 return_t getRefVelocity(int joint, double * vel) override;
144 return_t getRefVelocities(double * vels) override;
145 return_t getRefVelocities(int n_joint, const int * joints, double * vels) override;
146#endif
147
148 // --------- IControlLimits Declarations. Implementation in IControlLimitsImpl.cpp ---------
149#if YARP_VERSION_COMPARE(>=, 4, 0, 0)
150 return_t setPosLimits(int axis, double min, double max) override;
151 return_t getPosLimits(int axis, double * min, double * max) override;
152#else
153 return_t setLimits(int axis, double min, double max) override;
154 return_t getLimits(int axis, double * min, double * max) override;
155#endif
156 return_t setVelLimits(int axis, double min, double max) override;
157 return_t getVelLimits(int axis, double * min, double * max) override;
158
159 // --------- IControlMode Declarations. Implementation in IControlModeImpl.cpp ---------
160#if YARP_VERSION_COMPARE(>=, 4, 0, 0)
161 return_t getAvailableControlModes(int j, std::vector<yarp::dev::SelectableControlModeEnum> & avail) override;
162 return_t getControlMode(int j, yarp::dev::ControlModeEnum & mode) override;
163 return_t getControlModes(std::vector<yarp::dev::ControlModeEnum> & modes) override;
164 return_t getControlModes(const std::vector<int> & joints, std::vector<yarp::dev::ControlModeEnum> & modes) override;
165 return_t setControlMode(int j, yarp::dev::SelectableControlModeEnum mode) override;
166 return_t setControlModes(const std::vector<int> & joints, const std::vector<yarp::dev::SelectableControlModeEnum> & modes) override;
167 return_t setControlModes(const std::vector<yarp::dev::SelectableControlModeEnum> & modes) override;
168#else
169 return_t getControlMode(int j, int * mode) override;
170 return_t getControlModes(int * modes) override;
171 return_t getControlModes(int n_joint, const int * joints, int * modes) override;
172 return_t setControlMode(int j, const int mode) override;
173 return_t setControlModes(int n_joint, const int * joints, int * modes) override;
174 return_t setControlModes(int * modes) override;
175#endif
176
177 // -------- DeviceDriver declarations. Implementation in DeviceDriverImpl.cpp --------
178 bool open(yarp::os::Searchable & config) override;
179 bool close() override;
180
181 // -------- PeriodicThread declarations. Implementation in PeriodicThreadImpl.cpp --------
182 bool threadInit() override;
183 void run() override;
184
185protected:
186 // ----- Shared Area Funcion declarations. Implementation in SharedArea.cpp -----
187 void setEncRaw(const int index, const double position);
188 void setEncsRaw(const std::vector<double> & positions);
189
190 double getEncRaw(const int index);
191 std::vector<double> getEncsRaw();
192
193 double getEncExposed(const int index);
194 std::vector<double> getEncsExposed();
195
196private:
197 enum jmc_state { NOT_CONTROLLING, POSITION_MOVE, RELATIVE_MOVE, VELOCITY_MOVE };
198
199 yarp::conf::vocab32_t controlMode {0};
200 double lastTime {0.0};
201
202 std::mutex encRawMutex; // SharedArea
203
204 std::vector<jmc_state> jointStatus;
205
206 std::vector<double> encRaw;
207 std::vector<double> refAcc; // Exposed.
208 std::vector<double> targetExposed; // Exposed.
209 std::vector<double> velRaw;
210};
211
212#endif // __EMULATED_CONTROL_BOARD_HPP__
EmulatedControlBoard_ParamsParser
Definition EmulatedControlBoard_ParamsParser.h:58
EmulatedControlBoard
Implements several motor interfaces.
Definition EmulatedControlBoard.hpp:49
Generated by doxygen 1.9.8