YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
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
11
12using namespace yarp::dev;
13
14namespace {
15YARP_LOG_COMPONENT(IMPLEMENT_LAYER_COMPONENT, "yarp.ImplementControlLimits")
16}
17
22
23
28
34{
35 if(m_helper != nullptr)
36 {
37 delete castToMapper(m_helper);
38 m_helper = nullptr;
39 }
40 return true;
41}
42
43bool ImplementControlLimits::initialize(int size, const int *amap, const double *enc, const double *zos)
44{
45 if (m_helper != nullptr) {
46 return false;
47 }
48
49 m_helper=(void *)(new ControlBoardHelper(size, amap, enc, zos));
50 yAssert(m_helper != nullptr);
51
52 m_buffer_ints.resize (size);
53 m_buffer_doubles.resize(size);
54
55 return true;
56}
57
58
59ReturnValue ImplementControlLimits::setPosLimits(int axis, double min, double max)
60{
61 std::lock_guard lock(m_imp_mutex);
62 JOINTIDCHECK(IMPLEMENT_LAYER_COMPONENT,axis);
63
64 double minEnc=0;
65 double maxEnc=0;
66
67 int k=0;
68 castToMapper(m_helper)->posA2E(min, axis, minEnc, k);
69 castToMapper(m_helper)->posA2E(max, axis, maxEnc, k);
70
71 if( (max > min) && (minEnc > maxEnc)) //angle to encoder conversion factor is negative
72 {
73 double temp; // exchange max and min limits
74 temp = minEnc;
75 minEnc = maxEnc;
76 maxEnc = temp;
77 }
78
79 return m_iraw->setPosLimitsRaw(k, minEnc, maxEnc);
80}
81
82
83ReturnValue ImplementControlLimits::getPosLimits(int axis, double *min, double *max)
84{
85 std::lock_guard lock(m_imp_mutex);
86 JOINTIDCHECK(IMPLEMENT_LAYER_COMPONENT,axis)
87 POINTERCHECK(IMPLEMENT_LAYER_COMPONENT,min)
88 POINTERCHECK(IMPLEMENT_LAYER_COMPONENT,max)
89
90 double minEnc=0;
91 double maxEnc=0;
92
93 int k=castToMapper(m_helper)->toHw(axis);
94 ReturnValue ret=m_iraw->getPosLimitsRaw(k, &minEnc, &maxEnc);
95
96 *min=castToMapper(m_helper)->posE2A(minEnc, k);
97 *max=castToMapper(m_helper)->posE2A(maxEnc, k);
98
99 if( (*max < *min) && (minEnc < maxEnc)) //angle to encoder conversion factor is negative
100 {
101 double temp; // exchange max and min limits
102 temp = *min;
103 *min = *max;
104 *max = temp;
105 }
106 return ret;
107}
108
109ReturnValue ImplementControlLimits::setVelLimits(int axis, double min, double max)
110{
111 std::lock_guard lock(m_imp_mutex);
112 JOINTIDCHECK(IMPLEMENT_LAYER_COMPONENT,axis);
113
114 double minEnc=0;
115 double maxEnc=0;
116
117 int k=0;
118 castToMapper(m_helper)->velA2E_abs(min, axis, minEnc, k);
119 castToMapper(m_helper)->velA2E_abs(max, axis, maxEnc, k);
120
121 return m_iraw->setVelLimitsRaw(k, minEnc, maxEnc);
122}
123
124ReturnValue ImplementControlLimits::getVelLimits(int axis, double *min, double *max)
125{
126 std::lock_guard lock(m_imp_mutex);
127 JOINTIDCHECK(IMPLEMENT_LAYER_COMPONENT,axis);
128 POINTERCHECK(IMPLEMENT_LAYER_COMPONENT,min)
129 POINTERCHECK(IMPLEMENT_LAYER_COMPONENT,max)
130
131 double minEnc=0;
132 double maxEnc=0;
133
134 int k=castToMapper(m_helper)->toHw(axis);
135 ReturnValue ret=m_iraw->getVelLimitsRaw(k, &minEnc, &maxEnc);
136
137 *min = castToMapper(m_helper)->velE2A_abs(minEnc, k);
138 *max = castToMapper(m_helper)->velE2A_abs(maxEnc, k);
139
140 return ret;
141}
yarp::dev::ControlBoardHelper * castToMapper(void *p)
#define yAssert(x)
Definition Log.h:388
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 yarp::dev::ReturnValue getVelLimitsRaw(int axis, double *min, double *max)=0
Get the software speed limits for a particular axis.
virtual yarp::dev::ReturnValue getPosLimitsRaw(int axis, double *min, double *max)=0
Get the software limits for a particular axis.
virtual yarp::dev::ReturnValue 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 yarp::dev::ReturnValue setPosLimitsRaw(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...
yarp::dev::ReturnValue getPosLimits(int axis, double *min, double *max) override
Get the software limits for a particular axis.
ImplementControlLimits(yarp::dev::IControlLimitsRaw *y)
Constructor.
bool uninitialize()
Clean up internal data and memory.
yarp::dev::ReturnValue setPosLimits(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...
yarp::dev::ReturnValue 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...
yarp::dev::ReturnValue getVelLimits(int axis, double *min, double *max) override
Get the software speed limits for a particular axis.
bool initialize(int size, const int *amap, const double *enc, const double *zos)
Initialize the internal data and alloc memory.
#define YARP_LOG_COMPONENT(name,...)
For streams capable of holding different kinds of content, check what they actually have.