YARP
Yet Another Robot Platform
ImplementCurrentControl.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
9
10#include <cstdio>
11using namespace yarp::dev;
12using namespace yarp::os;
13
14#define JOINTIDCHECK if (j >= castToMapper(helper)->axes()){yError("joint id out of bound"); return false;}
15
16ImplementCurrentControl::ImplementCurrentControl(ICurrentControlRaw *tq):
17 iCurrentRaw(tq),
18 helper(nullptr),
19 intBuffManager(nullptr),
20 doubleBuffManager(nullptr)
21{;}
22
24{
26}
27
28bool ImplementCurrentControl::initialize(int size, const int *amap, const double* ampsToSens)
29{
30 if (helper != nullptr) {
31 return false;
32 }
33
35 yAssert (intBuffManager != nullptr);
36
38 yAssert (doubleBuffManager != nullptr);
39
40 helper = (void *)(new ControlBoardHelper(size, amap, nullptr, nullptr, nullptr, ampsToSens, nullptr, nullptr));
41 yAssert (helper != nullptr);
42
43 return true;
44}
45
47{
48 if (helper!=nullptr)
49 {
50 delete castToMapper(helper);
51 helper=nullptr;
52 }
53
55 {
56 delete intBuffManager;
57 intBuffManager=nullptr;
58 }
59
61 {
62 delete doubleBuffManager;
63 doubleBuffManager=nullptr;
64 }
65
66 return true;
67}
68
70{
72}
73
75{
77 int k;
78 bool ret;
79 double current;
81 ret = iCurrentRaw->getRefCurrentRaw(k, &current);
82 *r = castToMapper(helper)->ampereS2A(current, k);
83 return ret;
84}
85
87{
89 bool ret = iCurrentRaw->getRefCurrentsRaw(buffValues.getData());
90 castToMapper(helper)->ampereS2A(buffValues.getData(),t);
92 return ret;
93}
94
96{
98 castToMapper(helper)->ampereA2S(t, buffValues.getData());
99 bool ret = iCurrentRaw->setRefCurrentsRaw(buffValues.getData());
100 doubleBuffManager->releaseBuffer(buffValues);
101 return ret;
102}
103
105{
107 int k;
108 double sens;
109 castToMapper(helper)->ampereA2S(t,j,sens,k);
110 return iCurrentRaw->setRefCurrentRaw(k, sens);
111}
112
114{
116 bool ret = iCurrentRaw->getCurrentsRaw(buffValues.getData());
117 castToMapper(helper)->ampereS2A(buffValues.getData(), t);
118 doubleBuffManager->releaseBuffer(buffValues);
119 return ret;
120}
121
122bool ImplementCurrentControl::setRefCurrents(const int n_joint, const int *joints, const double *t)
123{
124 if (!castToMapper(helper)->checkAxesIds(n_joint, joints)) {
125 return false;
126 }
127
130
131 for(int idx=0; idx<n_joint; idx++)
132 {
133 buffJoints[idx] = castToMapper(helper)->toHw(joints[idx]);
134 buffValues[idx] = castToMapper(helper)->ampereA2S(t[idx], joints[idx]);
135 }
136
137 bool ret = iCurrentRaw->setRefCurrentsRaw(n_joint, buffJoints.getData(), buffValues.getData());
138
139 doubleBuffManager->releaseBuffer(buffValues);
140 intBuffManager->releaseBuffer(buffJoints);
141 return ret;
142}
143
145{
147 int k;
148 bool ret;
149 double current;
150 k=castToMapper(helper)->toHw(j);
151 ret = iCurrentRaw->getCurrentRaw(k, &current);
152 *t = castToMapper(helper)->ampereS2A(current, k);
153 return ret;
154}
155
156bool ImplementCurrentControl::getCurrentRanges(double *min, double *max)
157{
160 bool ret = iCurrentRaw->getCurrentRangesRaw(b_min.getData(), b_max.getData());
161 castToMapper(helper)->ampereS2A(b_min.getData(), min);
162 castToMapper(helper)->ampereS2A(b_max.getData(), max);
165 return ret;
166}
167
168bool ImplementCurrentControl::getCurrentRange(int j, double *min, double *max)
169{
171 int k;
172 k=castToMapper(helper)->toHw(j);
173 double min_t, max_t;
174 bool ret = iCurrentRaw->getCurrentRangeRaw(k, &min_t, &max_t);
175 *min = castToMapper(helper)->ampereS2A(min_t, k);
176 *max = castToMapper(helper)->ampereS2A(max_t, k);
177 return ret;
178}
yarp::dev::ControlBoardHelper * castToMapper(void *p)
float t
bool ret
#define JOINTIDCHECK
#define yAssert(x)
Definition: Log.h:383
void ampereA2S(double ampere, int j, double &sens, int &k)
void ampereS2A(const double *sens, double *ampere)
Interface for control boards implementing current control.
virtual bool getNumberOfMotorsRaw(int *number)=0
Retrieves the number of controlled motors from the current physical interface.
virtual bool setRefCurrentsRaw(const double *currs)=0
Set the reference value of the currents for all motors.
virtual bool getCurrentRangesRaw(double *min, double *max)=0
Get the full scale of the current measurements for all motors motor (e.g.
virtual bool getCurrentsRaw(double *currs)=0
Get the instantaneous current measurement for all motors.
virtual bool getRefCurrentsRaw(double *currs)=0
Get the reference value of the currents for all motors.
virtual bool getCurrentRaw(int m, double *curr)=0
Get the instantaneous current measurement for a single motor.
virtual bool setRefCurrentRaw(int m, double curr)=0
Set the reference value of the current for a single motor.
virtual bool getCurrentRangeRaw(int m, double *min, double *max)=0
Get the full scale of the current measurement for a given motor (e.g.
virtual bool getRefCurrentRaw(int m, double *curr)=0
Get the reference value of the current for a single motor.
bool getCurrentRanges(double *min, double *max) override
Get the full scale of the current measurements for all motors motor (e.g.
yarp::dev::impl::FixedSizeBuffersManager< int > * intBuffManager
bool getCurrents(double *t) override
Get the instantaneous current measurement for all motors.
bool getCurrent(int j, double *t) override
Get the instantaneous current measurement for a single motor.
bool uninitialize()
Clean up internal data and memory.
bool getRefCurrents(double *t) override
Get the reference value of the currents for all motors.
bool getRefCurrent(int j, double *) override
Get the reference value of the current for a single motor.
yarp::dev::impl::FixedSizeBuffersManager< double > * doubleBuffManager
bool setRefCurrents(const double *t) override
Set the reference value of the currents for all motors.
bool getCurrentRange(int j, double *min, double *max) override
Get the full scale of the current measurement for a given motor (e.g.
bool getNumberOfMotors(int *ax) override
Retrieves the number of controlled axes from the current physical interface.
yarp::dev::ICurrentControlRaw * iCurrentRaw
bool initialize(int size, const int *amap, const double *ampsToSens)
Initialize the internal data and alloc memory.
bool setRefCurrent(int j, double t) override
Set the reference value of the current for a single motor.
Buffer contains info about a buffer of type T and it is used to exchange information with yarp::dev::...
T * getData()
Return the data pointer.
Buffer< T > getBuffer()
Get a buffer and fill its information in @buffer.
void releaseBuffer(Buffer< T > &buffer)
Release a buffer.
For streams capable of holding different kinds of content, check what they actually have.
An interface to the operating system, including Port based communication.