YARP
Yet Another Robot Platform
ImplementControlLimits.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 
6 #include <cstdio>
7 
10 
11 using namespace yarp::dev;
12 
13 
15  iLimits2(y),
16  helper(nullptr)
17 {;}
18 
19 
21 {
22  uninitialize();
23 }
24 
30 {
31  if(helper != nullptr)
32  {
33  delete castToMapper(helper);
34  helper = nullptr;
35  }
36  return true;
37 }
38 
39 bool ImplementControlLimits::initialize(int size, const int *amap, const double *enc, const double *zos)
40 {
41  if (helper != nullptr) {
42  return false;
43  }
44 
45  helper=(void *)(new ControlBoardHelper(size, amap, enc, zos));
46  yAssert(helper != nullptr);
47  return true;
48 }
49 
50 
51 bool ImplementControlLimits::setLimits(int axis, double min, double max)
52 {
53  double minEnc=0;
54  double maxEnc=0;
55 
56  int k=0;
57  castToMapper(helper)->posA2E(min, axis, minEnc, k);
58  castToMapper(helper)->posA2E(max, axis, maxEnc, k);
59 
60  if( (max > min) && (minEnc > maxEnc)) //angle to encoder conversion factor is negative
61  {
62  double temp; // exchange max and min limits
63  temp = minEnc;
64  minEnc = maxEnc;
65  maxEnc = temp;
66  }
67 
68  return iLimits2->setLimitsRaw(k, minEnc, maxEnc);
69 }
70 
71 
72 bool ImplementControlLimits::getLimits(int axis, double *min, double *max)
73 {
74  double minEnc=0;
75  double maxEnc=0;
76 
77  int k=castToMapper(helper)->toHw(axis);
78  bool ret=iLimits2->getLimitsRaw(k, &minEnc, &maxEnc);
79 
80  *min=castToMapper(helper)->posE2A(minEnc, k);
81  *max=castToMapper(helper)->posE2A(maxEnc, k);
82 
83  if( (*max < *min) && (minEnc < maxEnc)) //angle to encoder conversion factor is negative
84  {
85  double temp; // exchange max and min limits
86  temp = *min;
87  *min = *max;
88  *max = temp;
89  }
90  return ret;
91 }
92 
93 bool ImplementControlLimits::setVelLimits(int axis, double min, double max)
94 {
95  double minEnc=0;
96  double maxEnc=0;
97 
98  int k=0;
99  castToMapper(helper)->velA2E_abs(min, axis, minEnc, k);
100  castToMapper(helper)->velA2E_abs(max, axis, maxEnc, k);
101 
102  return iLimits2->setVelLimitsRaw(k, minEnc, maxEnc);
103 }
104 
105 bool ImplementControlLimits::getVelLimits(int axis, double *min, double *max)
106 {
107  double minEnc=0;
108  double maxEnc=0;
109 
110  int k=castToMapper(helper)->toHw(axis);
111  bool ret=iLimits2->getVelLimitsRaw(k, &minEnc, &maxEnc);
112 
113  *min = castToMapper(helper)->velE2A_abs(minEnc, k);
114  *max = castToMapper(helper)->velE2A_abs(maxEnc, k);
115 
116  return ret;
117 }
yarp::dev::ControlBoardHelper * castToMapper(void *p)
bool ret
#define yAssert(x)
Definition: Log.h:383
void velE2A_abs(double enc, int j, double &ang, int &k)
void posE2A(double enc, int j, double &ang, int &k)
void velA2E_abs(double ang, int j, double &enc, int &k)
void posA2E(double ang, int j, double &enc, int &k)
Interface for control devices.
virtual bool setVelLimitsRaw(int axis, double min, double max)=0
Set the software speed limits for a particular axis, the behavior of the control card when these limi...
virtual bool getLimitsRaw(int axis, double *min, double *max)=0
Get the software limits for a particular axis.
virtual bool setLimitsRaw(int axis, double min, double max)=0
Set the software limits for a particular axis, the behavior of the control card when these limits are...
virtual bool getVelLimitsRaw(int axis, double *min, double *max)=0
Get the software speed limits for a particular axis.
ImplementControlLimits(yarp::dev::IControlLimitsRaw *y)
Constructor.
bool getVelLimits(int axis, double *min, double *max) override
Get the software speed limits for a particular axis.
bool setVelLimits(int axis, double min, double max) override
Set the software speed limits for a particular axis, the behavior of the control card when these limi...
bool getLimits(int axis, double *min, double *max) override
Get the software limits for a particular axis.
bool uninitialize()
Clean up internal data and memory.
bool setLimits(int axis, double min, double max) override
Set the software limits for a particular axis, the behavior of the control card when these limits are...
bool initialize(int size, const int *amap, const double *enc, const double *zos)
Initialize the internal data and alloc memory.
For streams capable of holding different kinds of content, check what they actually have.