YARP
Yet Another Robot Platform
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 
10 #include <yarp/os/Log.h>
11 
12 #include <cstdio>
13 
14 using namespace yarp::dev;
15 
17 {
18  calibrator=nullptr;
19 }
20 
22 {
23  if (c!=nullptr)
24  {
25  calibrator=c;
26  return true;
27  }
28 
29  return false;
30 }
31 
33 {
34  bool ret = false;
35  if (calibrator!=nullptr)
36  {
37  yDebug("Going to call calibrator\n");
38  ret=calibrator->calibrate(dynamic_cast<DeviceDriver *>(this));
39  } else {
40  yWarning("Warning called calibrate but no calibrator was set\n");
41  }
42 
43  return ret;
44 }
45 
47 {
48  bool ret=false;
49  if (calibrator != nullptr) {
50  ret = calibrator->quitCalibrate();
51  }
52  return ret;
53 }
54 
56 {
57  bool ret=false;
58  if (calibrator != nullptr) {
59  ret = calibrator->quitPark();
60  }
61  return ret;
62 }
63 
65 {
66  bool ret=false;
67  if (calibrator!=nullptr)
68  {
69  yDebug("Going to call calibrator\n");
70  ret=calibrator->park(dynamic_cast<DeviceDriver *>(this), wait);
71  } else {
72  yWarning("Warning called park but no calibrator was set\n");
73  }
74 
75  return ret;
76 }
77 
79 // ControlCalibration Interface Implementation
81 {
82  iCalibrate = dynamic_cast<IControlCalibrationRaw *> (y);
83  helper = nullptr;
84  temp = nullptr;
85 }
86 
88 {
89  uninitialize();
90 }
91 
92 bool ImplementControlCalibration::initialize(int size, const int *amap, const double *enc, const double *zos)
93 {
94  if (helper != nullptr) {
95  return false;
96  }
97 
98  helper = (void *)(new ControlBoardHelper(size, amap, enc, zos));
99  yAssert(helper != nullptr);
100  temp = new double[size];
101  yAssert(temp != nullptr);
102  return true;
103 }
104 
110 {
111  if (helper != nullptr)
112  {
113  delete castToMapper(helper);
114  helper = nullptr;
115  }
116 
118 
119  return true;
120 }
121 
123 {
124  int k = castToMapper(helper)->toHw(j);
125 
126  return iCalibrate->calibrationDoneRaw(k);
127 }
128 
129 bool ImplementControlCalibration::calibrateAxisWithParams(int axis, unsigned int type, double p1, double p2, double p3)
130 {
131  int k = castToMapper(helper)->toHw(axis);
132 
133  return iCalibrate->calibrateAxisWithParamsRaw(k, type, p1, p2, p3);
134 }
135 
137 {
138  int k = castToMapper(helper)->toHw(axis);
139 
140  return iCalibrate->setCalibrationParametersRaw(k, params);
141 }
void checkAndDestroy(T *&p)
yarp::dev::ControlBoardHelper * castToMapper(void *p)
define control board standard interfaces
bool ret
#define yDebug(...)
Definition: Log.h:270
#define yAssert(x)
Definition: Log.h:383
#define yWarning(...)
Definition: Log.h:335
Interface implemented by all device drivers.
Definition: DeviceDriver.h:30
virtual bool park(DeviceDriver *dd, bool wait=true)=0
virtual bool calibrate(DeviceDriver *dd)=0
virtual bool quitPark()=0
virtual bool quitCalibrate()=0
Interface for control devices, calibration commands.
virtual bool setCalibrationParametersRaw(int axis, const CalibrationParameters &params)
Start calibration, this method is very often platform specific.
virtual bool calibrationDoneRaw(int j)=0
Check if the calibration is terminated, on a particular joint.
virtual bool calibrateAxisWithParamsRaw(int axis, unsigned int type, double p1, double p2, double p3)=0
Start calibration, this method is very often platform specific.
virtual bool setCalibrator(ICalibrator *c)
Set the calibrator object to be used to calibrate the robot.
virtual bool calibrateRobot()
Calibrate robot by using an external calibrator.
bool calibrationDone(int j) override
Check if the calibration is terminated, on a particular joint.
bool setCalibrationParameters(int axis, const CalibrationParameters &params) override
Start calibration, this method is very often platform specific.
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 calibrateAxisWithParams(int axis, unsigned int type, double p1, double p2, double p3) override
Start calibration, this method is very often platform specific.
bool uninitialize()
Clean up internal data and memory.
For streams capable of holding different kinds of content, check what they actually have.