YARP
Yet Another Robot Platform
ControlBoardWrapperEncodersTimed.cpp
Go to the documentation of this file.
1 /*
2  * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT)
3  * SPDX-License-Identifier: BSD-3-Clause
4  */
5 
7 
9 
11 {
12  size_t off;
13  try {
14  off = device.lut.at(j).offset;
15  } catch (...) {
16  yCError(CONTROLBOARD, "Joint number %d out of bound [0-%zu] for part %s", j, controlledJoints, partName.c_str());
17  return false;
18  }
19  size_t subIndex = device.lut[j].deviceEntry;
20 
21  SubDevice* p = device.getSubdevice(subIndex);
22  if (!p) {
23  return false;
24  }
25 
26  if (p->iJntEnc) {
27  return p->iJntEnc->resetEncoder(static_cast<int>(off + p->base));
28  }
29  return false;
30 }
31 
32 
34 {
35  bool ret = true;
36 
37  for (size_t l = 0; l < controlledJoints; l++) {
38  int off = device.lut[l].offset;
39  size_t subIndex = device.lut[l].deviceEntry;
40 
41  SubDevice* p = device.getSubdevice(subIndex);
42  if (!p) {
43  return false;
44  }
45 
46  if (p->iJntEnc) {
47  ret = ret && p->iJntEnc->resetEncoder(static_cast<int>(off + p->base));
48  } else {
49  ret = false;
50  }
51  }
52  return ret;
53 }
54 
55 
57 {
58  size_t off;
59  try {
60  off = device.lut.at(j).offset;
61  } catch (...) {
62  yCError(CONTROLBOARD, "Joint number %d out of bound [0-%zu] for part %s", j, controlledJoints, partName.c_str());
63  return false;
64  }
65  size_t subIndex = device.lut[j].deviceEntry;
66 
67  SubDevice* p = device.getSubdevice(subIndex);
68  if (!p) {
69  return false;
70  }
71 
72  if (p->iJntEnc) {
73  return p->iJntEnc->setEncoder(static_cast<int>(off + p->base), val);
74  }
75  return false;
76 }
77 
78 
80 {
81  bool ret = true;
82 
83  for (size_t l = 0; l < controlledJoints; l++) {
84  int off = device.lut[l].offset;
85  size_t subIndex = device.lut[l].deviceEntry;
86 
87  SubDevice* p = device.getSubdevice(subIndex);
88  if (!p) {
89  return false;
90  }
91 
92  if (p->iJntEnc) {
93  ret = ret && p->iJntEnc->setEncoder(static_cast<int>(off + p->base), vals[l]);
94  } else {
95  ret = false;
96  }
97  }
98  return ret;
99 }
100 
101 
103 {
104  size_t off;
105  try {
106  off = device.lut.at(j).offset;
107  } catch (...) {
108  yCError(CONTROLBOARD, "Joint number %d out of bound [0-%zu] for part %s", j, controlledJoints, partName.c_str());
109  return false;
110  }
111  size_t subIndex = device.lut[j].deviceEntry;
112 
113  SubDevice* p = device.getSubdevice(subIndex);
114  if (!p) {
115  return false;
116  }
117 
118  if (p->iJntEnc) {
119  return p->iJntEnc->getEncoder(static_cast<int>(off + p->base), v);
120  }
121  *v = 0.0;
122  return false;
123 }
124 
125 
127 {
128  auto* encValues = new double[device.maxNumOfJointsInDevices];
129  bool ret = true;
130  for (size_t d = 0; d < device.subdevices.size(); d++) {
131  SubDevice* p = device.getSubdevice(d);
132  if (!p) {
133  ret = false;
134  break;
135  }
136 
137  if ((p->iJntEnc) && (ret = p->iJntEnc->getEncoders(encValues))) {
138  for (size_t juser = p->wbase, jdevice = p->base; juser <= p->wtop; juser++, jdevice++) {
139  encs[juser] = encValues[jdevice];
140  }
141  } else {
142  printError("getEncoders", p->id, ret);
143  ret = false;
144  break;
145  }
146  }
147 
148  delete[] encValues;
149  return ret;
150 }
151 
152 
154 {
155  auto* encValues = new double[device.maxNumOfJointsInDevices];
156  auto* tValues = new double[device.maxNumOfJointsInDevices];
157  bool ret = true;
158  for (size_t d = 0; d < device.subdevices.size(); d++) {
159  SubDevice* p = device.getSubdevice(d);
160  if (!p) {
161  ret = false;
162  break;
163  }
164 
165  if ((p->iJntEnc) && (ret = p->iJntEnc->getEncodersTimed(encValues, tValues))) {
166  for (size_t juser = p->wbase, jdevice = p->base; juser <= p->wtop; juser++, jdevice++) {
167  encs[juser] = encValues[jdevice];
168  t[juser] = tValues[jdevice];
169  }
170  } else {
171  printError("getEncodersTimed", p->id, ret);
172  ret = false;
173  break;
174  }
175  }
176 
177  delete[] encValues;
178  delete[] tValues;
179  return ret;
180 }
181 
182 
184 {
185  size_t off;
186  try {
187  off = device.lut.at(j).offset;
188  } catch (...) {
189  yCError(CONTROLBOARD, "Joint number %d out of bound [0-%zu] for part %s", j, controlledJoints, partName.c_str());
190  return false;
191  }
192  size_t subIndex = device.lut[j].deviceEntry;
193 
194  SubDevice* p = device.getSubdevice(subIndex);
195  if (!p) {
196  return false;
197  }
198 
199  if (p->iJntEnc) {
200  return p->iJntEnc->getEncoderTimed(static_cast<int>(off + p->base), v, t);
201  }
202  *v = 0.0;
203  return false;
204 }
205 
206 
208 {
209  size_t off;
210  try {
211  off = device.lut.at(j).offset;
212  } catch (...) {
213  yCError(CONTROLBOARD, "Joint number %d out of bound [0-%zu] for part %s", j, controlledJoints, partName.c_str());
214  return false;
215  }
216  size_t subIndex = device.lut[j].deviceEntry;
217 
218  SubDevice* p = device.getSubdevice(subIndex);
219  if (!p) {
220  return false;
221  }
222 
223  if (p->iJntEnc) {
224  return p->iJntEnc->getEncoderSpeed(static_cast<int>(off + p->base), sp);
225  }
226  *sp = 0.0;
227  return false;
228 }
229 
230 
232 {
233  auto* sValues = new double[device.maxNumOfJointsInDevices];
234  bool ret = true;
235  for (size_t d = 0; d < device.subdevices.size(); d++) {
236  SubDevice* p = device.getSubdevice(d);
237  if (!p) {
238  ret = false;
239  break;
240  }
241 
242  if ((p->iJntEnc) && (ret = p->iJntEnc->getEncoderSpeeds(sValues))) {
243  for (size_t juser = p->wbase, jdevice = p->base; juser <= p->wtop; juser++, jdevice++) {
244  spds[juser] = sValues[jdevice];
245  }
246  } else {
247  printError("getEncoderSpeeds", p->id, ret);
248  ret = false;
249  break;
250  }
251  }
252 
253  delete[] sValues;
254  return ret;
255 }
256 
257 
259 {
260  size_t off;
261  try {
262  off = device.lut.at(j).offset;
263  } catch (...) {
264  yCError(CONTROLBOARD, "Joint number %d out of bound [0-%zu] for part %s", j, controlledJoints, partName.c_str());
265  return false;
266  }
267  size_t subIndex = device.lut[j].deviceEntry;
268 
269  SubDevice* p = device.getSubdevice(subIndex);
270  if (!p) {
271  return false;
272  }
273 
274  if (p->iJntEnc) {
275  return p->iJntEnc->getEncoderAcceleration(static_cast<int>(off + p->base), acc);
276  }
277  *acc = 0.0;
278  return false;
279 }
280 
281 
283 {
284  auto* aValues = new double[device.maxNumOfJointsInDevices];
285  bool ret = true;
286  for (size_t d = 0; d < device.subdevices.size(); d++) {
287  SubDevice* p = device.getSubdevice(d);
288  if (!p) {
289  ret = false;
290  break;
291  }
292 
293  if ((p->iJntEnc) && (ret = p->iJntEnc->getEncoderAccelerations(aValues))) {
294  for (size_t juser = p->wbase, jdevice = p->base; juser <= p->wtop; juser++, jdevice++) {
295  accs[juser] = aValues[jdevice];
296  }
297  } else {
298  printError("getEncoderAccelerations", p->id, ret);
299  ret = false;
300  break;
301  }
302  }
303 
304  delete[] aValues;
305  return ret;
306 }
const yarp::os::LogComponent & CONTROLBOARD()
float t
bool ret
void printError(const std::string &func_name, const std::string &info, bool result)
bool getEncoderAccelerations(double *accs) override
Read the instantaneous acceleration of all axes.
bool setEncoders(const double *vals) override
Set the value of all encoders.
bool getEncoderAcceleration(int j, double *acc) override
Read the instantaneous acceleration of an axis.
bool setEncoder(int j, double val) override
Set the value of the encoder for a given joint.
bool getEncoderSpeed(int j, double *sp) override
Read the istantaneous speed of an axis.
bool getEncoderSpeeds(double *spds) override
Read the instantaneous speed of all axes.
bool getEncoder(int j, double *v) override
Read the value of an encoder.
bool getEncoders(double *encs) override
Read the position of all axes.
bool getEncoderTimed(int j, double *v, double *t) override
Read the instantaneous acceleration of all axes.
bool resetEncoder(int j) override
Reset encoder, single joint.
bool getEncodersTimed(double *encs, double *t) override
Read the instantaneous acceleration of all axes.
size_t base
Definition: SubDevice.h:55
std::string id
Definition: SubDevice.h:54
yarp::dev::IEncodersTimed * iJntEnc
Definition: SubDevice.h:70
size_t wbase
Definition: SubDevice.h:57
std::vector< DevicesLutEntry > lut
Definition: SubDevice.h:122
SubDevice * getSubdevice(size_t i)
Definition: SubDevice.h:125
size_t maxNumOfJointsInDevices
Definition: SubDevice.h:123
SubDeviceVector subdevices
Definition: SubDevice.h:121
virtual bool getEncodersTimed(double *encs, double *time)=0
Read the instantaneous acceleration of all axes.
virtual bool getEncoderTimed(int j, double *encs, double *time)=0
Read the instantaneous acceleration of all axes.
virtual bool getEncoderSpeed(int j, double *sp)=0
Read the istantaneous speed of an axis.
virtual bool getEncoderAccelerations(double *accs)=0
Read the instantaneous acceleration of all axes.
virtual bool setEncoder(int j, double val)=0
Set the value of the encoder for a given joint.
virtual bool getEncoder(int j, double *v)=0
Read the value of an encoder.
virtual bool resetEncoder(int j)=0
Reset encoder, single joint.
virtual bool getEncoders(double *encs)=0
Read the position of all axes.
virtual bool getEncoderSpeeds(double *spds)=0
Read the instantaneous speed of all axes.
virtual bool getEncoderAcceleration(int j, double *spds)=0
Read the instantaneous acceleration of an axis.
#define yCError(component,...)
Definition: LogComponent.h:213