YARP
Yet Another Robot Platform
 
Loading...
Searching...
No Matches
ImplementControlCalibration.cpp
Go to the documentation of this file.
1/*
2 * SPDX-FileCopyrightText: 2006-2021 Istituto Italiano di Tecnologia (IIT)
3 * SPDX-FileCopyrightText: 2006-2010 RobotCub Consortium
4 * SPDX-License-Identifier: BSD-3-Clause
5 */
6
11#include <yarp/os/Log.h>
12
13#include <cstdio>
14
15using namespace yarp::dev;
16
17namespace {
18YARP_LOG_COMPONENT(IMPLEMENT_LAYER_COMPONENT, "yarp.IControlCalibration")
19}
20
22{
23 calibrator=nullptr;
24}
25
36
38{
40 if (calibrator!=nullptr)
41 {
42 yDebug("Going to call calibrator\n");
43 ret=calibrator->calibrate(dynamic_cast<DeviceDriver *>(this));
44 } else {
45 yWarning("Warning called calibrate but no calibrator was set\n");
46 }
47
48 return ret;
49}
50
52{
54 if (calibrator != nullptr) {
55 ret = calibrator->quitCalibrate();
56 }
57 return ret;
58}
59
61{
63 if (calibrator != nullptr) {
64 ret = calibrator->quitPark();
65 }
66 return ret;
67}
68
70{
72 if (calibrator!=nullptr)
73 {
74 yDebug("Going to call calibrator\n");
75 ret=calibrator->park(dynamic_cast<DeviceDriver *>(this), wait);
76 } else {
77 yWarning("Warning called park but no calibrator was set\n");
78 }
79
80 return ret;
81}
82
84// ControlCalibration Interface Implementation
90
95
96bool ImplementControlCalibration::initialize(int size, const int *amap, const double *enc, const double *zos)
97{
98 if (m_helper != nullptr) {
99 return false;
100 }
101
102 m_helper = (void *)(new ControlBoardHelper(size, amap, enc, zos));
103 yAssert(m_helper != nullptr);
104
105 m_buffer_ints.resize (size);
106 m_buffer_doubles.resize(size);
107
108 return true;
109}
110
116{
117 if (m_helper != nullptr)
118 {
119 delete castToMapper(m_helper);
120 m_helper = nullptr;
121 }
122
123 return true;
124}
125
127{
128 std::lock_guard lock(m_imp_mutex);
129 JOINTIDCHECK(IMPLEMENT_LAYER_COMPONENT,j);
130
131 int k = castToMapper(m_helper)->toHw(j);
132
133 return m_iraw->calibrationDoneRaw(k);
134}
135
136ReturnValue ImplementControlCalibration::calibrateAxisWithParams(int axis, unsigned int type, double p1, double p2, double p3)
137{
138 std::lock_guard lock(m_imp_mutex);
139 JOINTIDCHECK(IMPLEMENT_LAYER_COMPONENT,axis);
140
141 int k = castToMapper(m_helper)->toHw(axis);
142
143 return m_iraw->calibrateAxisWithParamsRaw(k, type, p1, p2, p3);
144}
145
147{
148 std::lock_guard lock(m_imp_mutex);
149 JOINTIDCHECK(IMPLEMENT_LAYER_COMPONENT,axis);
150
151 int k = castToMapper(m_helper)->toHw(axis);
152
153 return m_iraw->setCalibrationParametersRaw(k, params);
154}
yarp::dev::ControlBoardHelper * castToMapper(void *p)
define control board standard interfaces
#define yDebug(...)
Definition Log.h:275
#define yAssert(x)
Definition Log.h:388
#define yWarning(...)
Definition Log.h:340
#define ReturnValue_ok
Definition ReturnValue.h:82
Interface implemented by all device drivers.
Interface for a calibrator device.
Definition ICalibrator.h:22
virtual yarp::dev::ReturnValue quitPark()=0
virtual yarp::dev::ReturnValue quitCalibrate()=0
virtual yarp::dev::ReturnValue park(DeviceDriver *dd, bool wait=true)=0
virtual yarp::dev::ReturnValue calibrate(DeviceDriver *dd)=0
Interface for control devices, calibration commands.
virtual yarp::dev::ReturnValue calibrationDoneRaw(int j)=0
Check if the calibration is terminated, on a particular joint.
virtual yarp::dev::ReturnValue calibrateAxisWithParamsRaw(int axis, unsigned int type, double p1, double p2, double p3)=0
Start calibration, this method is very often platform specific.
virtual yarp::dev::ReturnValue setCalibrationParametersRaw(int axis, const CalibrationParameters &params)=0
Start calibration, this method is very often platform specific.
virtual yarp::dev::ReturnValue abortPark()
virtual yarp::dev::ReturnValue calibrateRobot()
Calibrate robot by using an external calibrator.
virtual yarp::dev::ReturnValue abortCalibration()
virtual yarp::dev::ReturnValue park(bool wait=true)
virtual yarp::dev::ReturnValue setCalibrator(ICalibrator *c)
Set the calibrator object to be used to calibrate the robot.
ReturnValue calibrateAxisWithParams(int axis, unsigned int type, double p1, double p2, double p3) override
Start calibration, this method is very often platform specific.
ReturnValue calibrationDone(int j) override
Check if the calibration is terminated, on a particular joint.
ImplementControlCalibration(yarp::dev::IControlCalibrationRaw *y)
bool initialize(int size, const int *amap, const double *enc, const double *zos)
Initialize the internal data and alloc memory.
bool uninitialize()
Clean up internal data and memory.
ReturnValue setCalibrationParameters(int axis, const CalibrationParameters &params) override
Start calibration, this method is very often platform specific.
@ return_value_error_input_out_of_bounds
Method failed because some initialization is missing.
@ return_value_error_not_ready
Method failed due to invalid internal status/invalid request.
#define YARP_LOG_COMPONENT(name,...)
For streams capable of holding different kinds of content, check what they actually have.