YARP
Yet Another Robot Platform
ControlBoardWrapperAmplifierControl.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 
8 #include <yarp/os/LogStream.h>
9 
11 
12 
14 {
15  size_t off;
16  try {
17  off = device.lut.at(j).offset;
18  } catch (...) {
19  yCError(CONTROLBOARD, "Joint number %d out of bound [0-%zu] for part %s", j, controlledJoints, partName.c_str());
20  return false;
21  }
22  size_t subIndex = device.lut[j].deviceEntry;
23 
24  SubDevice* p = device.getSubdevice(subIndex);
25  if (!p) {
26  return false;
27  }
28 
29  if (p->amp) {
30  return p->amp->enableAmp(static_cast<int>(off + p->base));
31  }
32  return false;
33 }
34 
35 
37 {
38  size_t off;
39  try {
40  off = device.lut.at(j).offset;
41  } catch (...) {
42  yCError(CONTROLBOARD, "Joint number %d out of bound [0-%zu] for part %s", j, controlledJoints, partName.c_str());
43  return false;
44  }
45  size_t subIndex = device.lut[j].deviceEntry;
46 
47  bool ret = true;
48  SubDevice* p = device.getSubdevice(subIndex);
49  if (!p) {
50  return false;
51  }
52 
53  // Use the newer interface if available, otherwise fallback on the old one.
54  if (p->iMode) {
55  ret = p->iMode->setControlMode(static_cast<int>(off + p->base), VOCAB_CM_IDLE);
56  } else {
57  if (p->pos) {
58  ret = p->amp->disableAmp(static_cast<int>(off + p->base));
59  } else {
60  ret = false;
61  }
62  }
63  return ret;
64 }
65 
66 
68 {
69  int* status = new int[device.maxNumOfJointsInDevices];
70  bool ret = true;
71  for (size_t d = 0; d < device.subdevices.size(); d++) {
73  if (!p) {
74  ret = false;
75  break;
76  }
77 
78  if ((p->amp) && (ret = p->amp->getAmpStatus(status))) {
79  for (size_t juser = p->wbase, jdevice = p->base; juser <= p->wtop; juser++, jdevice++) {
80  st[juser] = status[jdevice];
81  }
82  } else {
83  printError("getAmpStatus", p->id, ret);
84  ret = false;
85  break;
86  }
87  }
88 
89  delete[] status;
90  return ret;
91 }
92 
93 
95 {
96  size_t off;
97  try {
98  off = device.lut.at(j).offset;
99  } catch (...) {
100  yCError(CONTROLBOARD, "Joint number %d out of bound [0-%zu] for part %s", j, controlledJoints, partName.c_str());
101  return false;
102  }
103  size_t subIndex = device.lut[j].deviceEntry;
104 
105  SubDevice* p = device.getSubdevice(subIndex);
106  if (p && p->amp) {
107  return p->amp->getAmpStatus(static_cast<int>(off + p->base), v);
108  }
109  *v = 0;
110  return false;
111 }
112 
113 
115 {
116  size_t off;
117  try {
118  off = device.lut.at(j).offset;
119  } catch (...) {
120  yCError(CONTROLBOARD, "Joint number %d out of bound [0-%zu] for part %s", j, controlledJoints, partName.c_str());
121  return false;
122  }
123  size_t subIndex = device.lut[j].deviceEntry;
124 
125  SubDevice* p = device.getSubdevice(subIndex);
126  if (!p) {
127  return false;
128  }
129 
130  if (p->amp) {
131  return p->amp->setMaxCurrent(static_cast<int>(off + p->base), v);
132  }
133  return false;
134 }
135 
136 
138 {
139  size_t off;
140  try {
141  off = device.lut.at(j).offset;
142  } catch (...) {
143  yCError(CONTROLBOARD, "Joint number %d out of bound [0-%zu] for part %s", j, controlledJoints, partName.c_str());
144  return false;
145  }
146  size_t subIndex = device.lut[j].deviceEntry;
147 
148  SubDevice* p = device.getSubdevice(subIndex);
149  if (!p) {
150  *v = 0.0;
151  return false;
152  }
153 
154  if (p->amp) {
155  return p->amp->getMaxCurrent(static_cast<int>(off + p->base), v);
156  }
157  *v = 0.0;
158  return false;
159 }
160 
161 
163 {
164  int off = device.lut[m].offset;
165  size_t subIndex = device.lut[m].deviceEntry;
166 
167  SubDevice* p = device.getSubdevice(subIndex);
168  if (!p) {
169  *val = 0.0;
170  return false;
171  }
172 
173  if (!p->amp) {
174  *val = 0.0;
175  return false;
176  }
177  return p->amp->getNominalCurrent(static_cast<int>(off + p->base), val);
178 }
179 
180 
182 {
183  int off = device.lut[m].offset;
184  size_t subIndex = device.lut[m].deviceEntry;
185 
186  SubDevice* p = device.getSubdevice(subIndex);
187  if (!p) {
188  *val = 0.0;
189  return false;
190  }
191 
192  if (!p->amp) {
193  *val = 0.0;
194  return false;
195  }
196  return p->amp->getPeakCurrent(static_cast<int>(off + p->base), val);
197 }
198 
199 
201 {
202  int off = device.lut[m].offset;
203  size_t subIndex = device.lut[m].deviceEntry;
204 
205  SubDevice* p = device.getSubdevice(subIndex);
206  if (!p) {
207  return false;
208  }
209 
210  if (!p->amp) {
211  return false;
212  }
213  return p->amp->setPeakCurrent(static_cast<int>(off + p->base), val);
214 }
215 
216 
218 {
219  int off = device.lut[m].offset;
220  size_t subIndex = device.lut[m].deviceEntry;
221 
222  SubDevice* p = device.getSubdevice(subIndex);
223  if (!p) {
224  return false;
225  }
226 
227  if (!p->amp) {
228  return false;
229  }
230  return p->amp->setNominalCurrent(static_cast<int>(off + p->base), val);
231 }
232 
233 
235 {
236  int off = device.lut[m].offset;
237  size_t subIndex = device.lut[m].deviceEntry;
238  SubDevice* p = device.getSubdevice(subIndex);
239 
240  yCTrace(CONTROLBOARD) << "CBW2::getPWMlimit j" << static_cast<int>(off + p->base) << " p " << (p ? "1" : "0") << " amp " << (p->amp ? "1" : "0");
241  if (!p) {
242  *val = 0.0;
243  return false;
244  }
245 
246  if (!p->amp) {
247  *val = 0.0;
248  return false;
249  }
250  return p->amp->getPWM(static_cast<int>(off + p->base), val);
251 }
252 
253 
255 {
256  int off = device.lut[m].offset;
257  size_t subIndex = device.lut[m].deviceEntry;
258 
259  SubDevice* p = device.getSubdevice(subIndex);
260  yCTrace(CONTROLBOARD) << "CBW2::getPWMlimit j" << static_cast<int>(off + p->base) << " p " << (p ? "1" : "0") << " amp " << (p->amp ? "1" : "0");
261 
262  if (!p) {
263  *val = 0.0;
264  return false;
265  }
266 
267  if (!p->amp) {
268  *val = 0.0;
269  return false;
270  }
271  return p->amp->getPWMLimit(static_cast<int>(off + p->base), val);
272 }
273 
274 
276 {
277  int off = device.lut[m].offset;
278  size_t subIndex = device.lut[m].deviceEntry;
279 
280  SubDevice* p = device.getSubdevice(subIndex);
281  if (!p) {
282  return false;
283  }
284 
285  if (!p->amp) {
286  return false;
287  }
288  return p->amp->setPWMLimit(static_cast<int>(off + p->base), val);
289 }
290 
291 
293 {
294  int off = device.lut[m].offset;
295  size_t subIndex = device.lut[m].deviceEntry;
296 
297  SubDevice* p = device.getSubdevice(subIndex);
298  if (!p) {
299  *val = 0.0;
300  return false;
301  }
302 
303  if (!p->amp) {
304  *val = 0.0;
305  return false;
306  }
307  return p->amp->getPowerSupplyVoltage(static_cast<int>(off + p->base), val);
308 }
const yarp::os::LogComponent & CONTROLBOARD()
constexpr yarp::conf::vocab32_t VOCAB_CM_IDLE
Definition: IControlMode.h:121
bool ret
bool setPeakCurrent(int m, const double val) override
bool setNominalCurrent(int m, const double val) override
bool disableAmp(int j) override
Disable the amplifier on a specific joint.
bool enableAmp(int j) override
Enable the amplifier on a specific joint.
bool getMaxCurrent(int j, double *v) override
Returns the maximum electric current allowed for a given motor.
bool setPWMLimit(int m, const double val) override
void printError(const std::string &func_name, const std::string &info, bool result)
size_t base
Definition: SubDevice.h:55
yarp::dev::IControlMode * iMode
Definition: SubDevice.h:78
std::string id
Definition: SubDevice.h:54
yarp::dev::IAmplifierControl * amp
Definition: SubDevice.h:72
yarp::dev::IPositionControl * pos
Definition: SubDevice.h:68
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 getPWMLimit(int j, double *val)
virtual bool enableAmp(int j)=0
Enable the amplifier on a specific joint.
virtual bool setNominalCurrent(int m, const double val)
virtual bool disableAmp(int j)=0
Disable the amplifier on a specific joint.
virtual bool getPWM(int j, double *val)
virtual bool getAmpStatus(int *st)=0
virtual bool setPWMLimit(int j, const double val)
virtual bool getMaxCurrent(int j, double *v)=0
Returns the maximum electric current allowed for a given motor.
virtual bool getNominalCurrent(int m, double *val)
virtual bool getPowerSupplyVoltage(int j, double *val)
virtual bool setMaxCurrent(int j, double v)=0
virtual bool setPeakCurrent(int m, const double val)
virtual bool getPeakCurrent(int m, double *val)
virtual bool setControlMode(const int j, const int mode)=0
Set the current control mode.
#define yCError(component,...)
Definition: LogComponent.h:213
#define yCTrace(component,...)
Definition: LogComponent.h:84