YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
ImplementEncoders.cpp
Go to the documentation of this file.
1/*
2 * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT)
3 * SPDX-FileCopyrightText: 2006-2010 RobotCub Consortium
4 * SPDX-License-Identifier: BSD-3-Clause
5 */
6
10
11#include <cmath>
12
13// Be careful: this file contains template implementations and is included by translation
14// units that use the template (e.g. .cpp files). Avoid putting here non-template functions to
15// avoid multiple definitions.
16
17using namespace yarp::dev;
18
19namespace {
20YARP_LOG_COMPONENT(IMPLEMENT_LAYER_COMPONENT, "yarp.ImplementEncoders")
21}
22
24// Encoder Interface Implementation
30
35
36bool ImplementEncoders:: initialize (int size, const int *amap, const double *enc, const double *zos)
37{
38 if (m_helper != nullptr) {
39 return false;
40 }
41
42 m_helper=(void *)(new ControlBoardHelper(size, amap, enc, zos));
43 yAssert (m_helper != nullptr);
44
45 m_buffer_doubles.resize(size);
46 m_buffer_ints.resize(size);
47
48 return true;
49}
50
56{
57 if (m_helper!=nullptr)
58 {
59 delete castToMapper(m_helper);
60 m_helper=nullptr;
61 }
62
63 return true;
64}
65
67{
68 std::lock_guard lock(m_imp_mutex);
69
70 (ax)=castToMapper(m_helper)->axes();
71 return ReturnValue_ok;
72}
73
75{
76 std::lock_guard lock(m_imp_mutex);
77 JOINTIDCHECK(IMPLEMENT_LAYER_COMPONENT,j)
78
79 int k;
81
82 return m_iraw->resetEncoderRaw(k);
83}
84
86{
87 std::lock_guard lock(m_imp_mutex);
88
89 return m_iraw->resetEncodersRaw();
90}
91
93{
94 std::lock_guard lock(m_imp_mutex);
95 JOINTIDCHECK(IMPLEMENT_LAYER_COMPONENT,j)
96
97 int k;
98 double enc;
99
100 castToMapper(m_helper)->posA2E(val, j, enc, k);
101
102 return m_iraw->setEncoderRaw(k, enc);
103}
104
106{
107 std::lock_guard lock(m_imp_mutex);
108 POINTERCHECK(IMPLEMENT_LAYER_COMPONENT,val)
109
111
112 return m_iraw->setEncodersRaw(m_buffer_doubles.data());
113}
114
116{
117 std::lock_guard lock(m_imp_mutex);
118 JOINTIDCHECK(IMPLEMENT_LAYER_COMPONENT,j)
119 POINTERCHECK(IMPLEMENT_LAYER_COMPONENT,v)
120
121 int k;
122 double enc;
123 ReturnValue ret;
124
126
127 ret=m_iraw->getEncoderRaw(k, &enc);
128
129 *v=castToMapper(m_helper)->posE2A(enc, k);
130
131 return ret;
132}
133
135{
136 std::lock_guard lock(m_imp_mutex);
137 POINTERCHECK(IMPLEMENT_LAYER_COMPONENT,v)
138
139 ReturnValue ret;
141
143
145
146 return ret;
147}
148
150{
151 std::lock_guard lock(m_imp_mutex);
152 JOINTIDCHECK(IMPLEMENT_LAYER_COMPONENT,j)
153 POINTERCHECK(IMPLEMENT_LAYER_COMPONENT,v)
154
155 int k;
156 double enc;
157 ReturnValue ret;
158
160
161 ret=m_iraw->getEncoderSpeedRaw(k, &enc);
162
163 *v=castToMapper(m_helper)->velE2A(enc, k);
164
165 return ret;
166}
167
169{
170 std::lock_guard lock(m_imp_mutex);
171 POINTERCHECK(IMPLEMENT_LAYER_COMPONENT,v)
172
173 ReturnValue ret;
175
177
178 return ret;
179}
180
182{
183 std::lock_guard lock(m_imp_mutex);
184 JOINTIDCHECK(IMPLEMENT_LAYER_COMPONENT,j)
185 POINTERCHECK(IMPLEMENT_LAYER_COMPONENT,v)
186
187 int k;
188 double enc;
189 ReturnValue ret;
190
192
193 ret=m_iraw->getEncoderAccelerationRaw(k, &enc);
194
195 *v=castToMapper(m_helper)->accE2A(enc, k);
196
197 return ret;
198}
199
201{
202 std::lock_guard lock(m_imp_mutex);
203 POINTERCHECK(IMPLEMENT_LAYER_COMPONENT,v)
204
205 ReturnValue ret;
207
209
210 return ret;
211}
yarp::dev::ControlBoardHelper * castToMapper(void *p)
#define yAssert(x)
Definition Log.h:388
#define ReturnValue_ok
Definition ReturnValue.h:82
void accE2A(double enc, int j, double &ang, int &k)
void velE2A(double enc, int j, double &ang, int &k)
void posE2A(double enc, int j, double &ang, int &k)
void posA2E(double ang, int j, double &enc, int &k)
Control board, encoder interface.
Definition IEncoders.h:24
virtual yarp::dev::ReturnValue getEncoderSpeedsRaw(double *spds)=0
Read the instantaneous acceleration of an axis.
virtual yarp::dev::ReturnValue getEncodersRaw(double *encs)=0
Read the position of all axes.
virtual yarp::dev::ReturnValue getEncoderRaw(int j, double *v)=0
Read the value of an encoder.
virtual yarp::dev::ReturnValue setEncoderRaw(int j, double val)=0
Set the value of the encoder for a given joint.
virtual yarp::dev::ReturnValue getEncoderAccelerationsRaw(double *accs)=0
Read the instantaneous acceleration of all axes.
virtual yarp::dev::ReturnValue setEncodersRaw(const double *vals)=0
Set the value of all encoders.
virtual yarp::dev::ReturnValue getEncoderSpeedRaw(int j, double *sp)=0
Read the instantaneous speed of an axis.
virtual yarp::dev::ReturnValue getEncoderAccelerationRaw(int j, double *spds)=0
Read the instantaneous acceleration of an axis.
virtual yarp::dev::ReturnValue resetEncoderRaw(int j)=0
Reset encoder, single joint.
virtual yarp::dev::ReturnValue resetEncodersRaw()=0
Reset encoders.
yarp::dev::ReturnValue resetEncoders() override
Reset encoders.
yarp::dev::ReturnValue getAxes(size_t &ax) override
Get the number of controlled axes.
yarp::dev::ReturnValue getEncoders(double *encs) override
Read the position of all axes.
yarp::dev::ReturnValue getEncoderAccelerations(double *accs) override
Read the instantaneous acceleration of all axes.
bool initialize(int size, const int *amap, const double *enc, const double *zos)
Initialize the internal data and alloc memory.
yarp::dev::ReturnValue resetEncoder(int j) override
Reset encoder, single joint.
yarp::dev::ReturnValue getEncoder(int j, double *v) override
Read the value of an encoder.
yarp::dev::ReturnValue getEncoderAcceleration(int j, double *spds) override
Read the instantaneous acceleration of an axis.
yarp::dev::ReturnValue getEncoderSpeed(int j, double *spds) override
Read the instantaneous speed of an axis.
ImplementEncoders(yarp::dev::IEncodersRaw *y)
yarp::dev::ReturnValue setEncoder(int j, double val) override
Set the value of the encoder for a given joint.
virtual ~ImplementEncoders()
Destructor.
yarp::dev::ReturnValue setEncoders(const double *vals) override
Set the value of all encoders.
std::vector< double > m_buffer_doubles
yarp::dev::ReturnValue getEncoderSpeeds(double *spds) override
Read the instantaneous speed of all axes.
bool uninitialize()
Clean up internal data and memory.
#define YARP_LOG_COMPONENT(name,...)
For streams capable of holding different kinds of content, check what they actually have.