YARP
Yet Another Robot Platform
ControlBoardHelper.h
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#ifndef YARP_DEV_CONTROLBOARDHELPER_H
7#define YARP_DEV_CONTROLBOARDHELPER_H
8
10#include <yarp/dev/PidEnums.h>
11#include <yarp/os/Log.h>
12
13#include <cstring> // for memset
14#include <type_traits>
15
16 /*
17 * simple helper template to alloc memory.
18 */
19template <class T,
20 std::enable_if_t<std::is_trivial<T>::value, int> = 0>
21T* allocAndCheck(int size)
22{
23 T* t = new T[size];
24 yAssert(t != 0);
25 memset(t, 0, sizeof(T) * size);
26 return t;
27}
28
29template <class T,
30 std::enable_if_t<!std::is_trivial<T>::value, int> = 0>
31T* allocAndCheck(int size)
32{
33 T* t = new T[size];
34 yAssert(t != 0);
35 return t;
36}
37
38/*
39*
40*/
41template <class T>
42void checkAndDestroy(T* &p) {
43 if (p != 0) {
44 delete[] p;
45 p = 0;
46 }
47}
48
49namespace yarp::dev {
50class ControlBoardHelper;
51}
52
55{
56public:
57 ControlBoardHelper(int n, const int *aMap, const double *angToEncs = nullptr, const double *zs = nullptr, const double *newtons = nullptr, const double *amps = nullptr, const double *volts = nullptr, const double *dutycycles = nullptr, const double *kbemf = nullptr, const double *ktau = nullptr);
61
62public:
63 bool checkAxisId(int id);
64 bool checkAxesIds(const int n_axes, const int* axesList);
65 int toHw(int axis);
66 int toUser(int axis);
67 void toUser(const double *hwData, double *user);
68 void toUser(const int *hwData, int *user);
69 void toHw(const double *usr, double *hwData);
70 void toHw(const int *usr, int *hwData);
71
72 //***************** position ******************//
73 void posA2E(double ang, int j, double &enc, int &k);
74 double posA2E(double ang, int j);
75 void posE2A(double enc, int j, double &ang, int &k);
76 double posE2A(double enc, int j);
77 void posA2E(const double *ang, double *enc);
78 void posE2A(const double *enc, double *ang);
79
80 //***************** velocity ******************//
81 void velA2E(double ang, int j, double &enc, int &k);
82 double velA2E(double ang, int j);
83 void velA2E_abs(double ang, int j, double &enc, int &k);
84 void velE2A(double enc, int j, double &ang, int &k);
85 void velE2A_abs(double enc, int j, double &ang, int &k);
86 double velE2A(double enc, int j);
87 double velE2A_abs(double enc, int j);
88 void velA2E(const double *ang, double *enc);
89 void velA2E_abs(const double *ang, double *enc);
90 void velE2A(const double *enc, double *ang);
91 void velE2A_abs(const double *enc, double *ang);
92
93 //***************** acceleration ******************//
94 void accA2E(double ang, int j, double &enc, int &k);
95 void accA2E_abs(double ang, int j, double &enc, int &k);
96 void accE2A(double enc, int j, double &ang, int &k);
97 void accE2A_abs(double enc, int j, double &ang, int &k);
98 double accE2A(double enc, int j);
99 double accE2A_abs(double enc, int j);
100 void accA2E(const double *ang, double *enc);
101 void accA2E_abs(const double *ang, double *enc);
102 void accE2A(const double *enc, double *ang);
103 void accE2A_abs(const double *enc, double *ang);
104
105 //***************** torque ******************//
106 void trqN2S(double newtons, int j, double &sens, int &k);
107 double trqN2S(double newtons, int j);
108 void trqN2S(const double *newtons, double *sens);
109 void trqS2N(const double *sens, double *newtons);
110 void trqS2N(double sens, int j, double &newton, int &k);
111 double trqS2N(double sens, int j);
112
113 //***************** impedance ******************//
114 void impN2S(double newtons, int j, double &sens, int &k);
115 double impN2S(double newtons, int j);
116 void impN2S(const double *newtons, double *sens);
117 void impS2N(const double *sens, double *newtons);
118 void impS2N(double sens, int j, double &newton, int &k);
119 double impS2N(double sens, int j);
120
121 //***************** current ******************//
122 void ampereA2S(double ampere, int j, double &sens, int &k);
123 double ampereA2S(double ampere, int j);
124 void ampereA2S(const double *ampere, double *sens);
125 void ampereS2A(const double *sens, double *ampere);
126 void ampereS2A(double sens, int j, double &ampere, int &k);
127 double ampereS2A(double sens, int j);
128
129 //***************** voltage ******************//
130 void voltageV2S(double voltage, int j, double &sens, int &k);
131 double voltageV2S(double voltage, int j);
132 void voltageV2S(const double *voltage, double *sens);
133 void voltageS2V(const double *sens, double *voltage);
134 void voltageS2V(double sens, int j, double &voltage, int &k);
135 double voltageS2V(double sens, int j);
136
137 //***************** dutycycle ******************//
138 void dutycycle2PWM(double dutycycle, int j, double &pwm, int &k);
139 double dutycycle2PWM(double dutycycle, int j);
140 void dutycycle2PWM(const double *dutycycle, double *sens);
141 void PWM2dutycycle(const double *pwm, double *dutycycle);
142 void PWM2dutycycle(double pwm_raw, int k_raw, double &dutycycle, int &j);
143 double PWM2dutycycle(double pwm_raw, int k_raw);
144
145 // *******************************************//
146 double bemf_user2raw(double bemf_user, int j);
147 double ktau_user2raw(double ktau_user, int j);
148 double viscousPos_user2raw(double viscousPos_user, int j);
149 double viscousNeg_user2raw(double viscousNeg_user, int j);
150 double coulombPos_user2raw(double coulombPos_user, int j);
151 double coulombNeg_user2raw(double coulombNeg_user, int j);
152
153 void bemf_user2raw(double bemf_user, int j, double &bemf_raw, int &k);
154 void ktau_user2raw(double ktau_user, int j, double &ktau_raw, int &k);
155 void viscousPos_user2raw(double viscousPos_user, int j, double &viscousPos_raw, int &k);
156 void viscousNeg_user2raw(double viscousNeg_user, int j, double &viscousNeg_raw, int &k);
157 void coulombPos_user2raw(double coulombPos_user, int j, double &coulombPos_raw, int &k);
158 void coulombNeg_user2raw(double coulombNeg_user, int j, double &coulombNeg_raw, int &k);
159 void bemf_raw2user(double bemf_raw, int k_raw, double &bemf_user, int &j_user);
160 void ktau_raw2user(double ktau_raw, int k_raw, double &ktau_user, int &j_user);
161 void viscousPos_raw2user(double viscousPos_raw, int k_raw, double &viscousPos_user, int &j_user);
162 void viscousNeg_raw2user(double viscousNeg_raw, int k_raw, double &viscousNeg_user, int &j_user);
163 void coulombPos_raw2user(double coulombPos_raw, int k_raw, double &coulombPos_user, int &j_user);
164 void coulombNeg_raw2user(double coulombNeg_raw, int k_raw, double &coulombNeg_user, int &j_user);
165
166 int axes();
167
168 // *******************************************//
169public:
170 void convert_pidunits_to_machine(const yarp::dev::PidControlTypeEnum& pidtype, double userval, int j, double &machineval, int &k);
171 void convert_pidunits_to_machine(const yarp::dev::PidControlTypeEnum& pidtype, const double* userval, double* machineval);
172 void convert_pidunits_to_user(const yarp::dev::PidControlTypeEnum& pidtype, const double machineval, double* userval, int k);
173 void convert_pidunits_to_user(const yarp::dev::PidControlTypeEnum& pidtype, const double* machineval, double* userval);
174
175 void convert_pid_to_user(const yarp::dev::PidControlTypeEnum& pidtype, const Pid &in_raw, int j_raw, Pid &out_usr, int &k_usr);
176 void convert_pid_to_machine(const yarp::dev::PidControlTypeEnum& pidtype, const Pid &in_usr, int j_usr, Pid &out_raw, int &k_raw);
177 Pid convert_pid_to_machine(const yarp::dev::PidControlTypeEnum& pidtype, const Pid &in_usr, int j_usr);
178
179 void set_pid_conversion_units(const PidControlTypeEnum& pidtype, const PidFeedbackUnitsEnum fbk_conv_units, const PidOutputUnitsEnum out_conv_units);
182
183private:
185 PrivateUnitsHandler* mPriv;
186};
187
189{ return static_cast<yarp::dev::ControlBoardHelper *>(p); }
190
191#endif // YARP_DEV_CONTROLBOARDHELPER_H
void checkAndDestroy(T *&p)
T * allocAndCheck(int size)
yarp::dev::ControlBoardHelper * castToMapper(void *p)
define control board standard interfaces
float t
#define yAssert(x)
Definition: Log.h:383
double get_pidfeedback_conversion_factor_user2raw(const yarp::dev::PidControlTypeEnum &pidtype, int j)
void accE2A(double enc, int j, double &ang, int &k)
void convert_pid_to_user(const yarp::dev::PidControlTypeEnum &pidtype, const Pid &in_raw, int j_raw, Pid &out_usr, int &k_usr)
void viscousNeg_raw2user(double viscousNeg_raw, int k_raw, double &viscousNeg_user, int &j_user)
double viscousNeg_user2raw(double viscousNeg_user, int j)
void ktau_raw2user(double ktau_raw, int k_raw, double &ktau_user, int &j_user)
void trqN2S(double newtons, int j, double &sens, int &k)
void velE2A_abs(double enc, int j, double &ang, int &k)
void convert_pidunits_to_machine(const yarp::dev::PidControlTypeEnum &pidtype, double userval, int j, double &machineval, int &k)
void bemf_raw2user(double bemf_raw, int k_raw, double &bemf_user, int &j_user)
void coulombPos_raw2user(double coulombPos_raw, int k_raw, double &coulombPos_user, int &j_user)
void voltageS2V(const double *sens, double *voltage)
void convert_pid_to_machine(const yarp::dev::PidControlTypeEnum &pidtype, const Pid &in_usr, int j_usr, Pid &out_raw, int &k_raw)
void ampereA2S(double ampere, int j, double &sens, int &k)
void impN2S(double newtons, int j, double &sens, int &k)
double ktau_user2raw(double ktau_user, int j)
double coulombPos_user2raw(double coulombPos_user, int j)
void velE2A(double enc, int j, double &ang, int &k)
double coulombNeg_user2raw(double coulombNeg_user, int j)
void velA2E(double ang, int j, double &enc, int &k)
void dutycycle2PWM(double dutycycle, int j, double &pwm, int &k)
void ampereS2A(const double *sens, double *ampere)
double viscousPos_user2raw(double viscousPos_user, int j)
void posE2A(double enc, int j, double &ang, int &k)
void coulombNeg_raw2user(double coulombNeg_raw, int k_raw, double &coulombNeg_user, int &j_user)
void trqS2N(const double *sens, double *newtons)
double get_pidoutput_conversion_factor_user2raw(const yarp::dev::PidControlTypeEnum &pidtype, int j)
void set_pid_conversion_units(const PidControlTypeEnum &pidtype, const PidFeedbackUnitsEnum fbk_conv_units, const PidOutputUnitsEnum out_conv_units)
void toUser(const int *hwData, int *user)
void voltageV2S(double voltage, int j, double &sens, int &k)
ControlBoardHelper & operator=(const ControlBoardHelper &other)
double bemf_user2raw(double bemf_user, int j)
void accA2E_abs(double ang, int j, double &enc, int &k)
void accA2E(double ang, int j, double &enc, int &k)
void accE2A_abs(double enc, int j, double &ang, int &k)
void velA2E_abs(double ang, int j, double &enc, int &k)
ControlBoardHelper(int n, const int *aMap, const double *angToEncs=nullptr, const double *zs=nullptr, const double *newtons=nullptr, const double *amps=nullptr, const double *volts=nullptr, const double *dutycycles=nullptr, const double *kbemf=nullptr, const double *ktau=nullptr)
void viscousPos_raw2user(double viscousPos_raw, int k_raw, double &viscousPos_user, int &j_user)
void convert_pidunits_to_user(const yarp::dev::PidControlTypeEnum &pidtype, const double machineval, double *userval, int k)
void impS2N(const double *sens, double *newtons)
void PWM2dutycycle(const double *pwm, double *dutycycle)
bool checkAxesIds(const int n_axes, const int *axesList)
void posA2E(double ang, int j, double &enc, int &k)
Contains the parameters for a PID.
For streams capable of holding different kinds of content, check what they actually have.
PidOutputUnitsEnum
Definition: PidEnums.h:31
PidControlTypeEnum
Definition: PidEnums.h:15
PidFeedbackUnitsEnum
Definition: PidEnums.h:25
The main, catch-all namespace for YARP.
Definition: dirs.h:16
#define YARP_dev_API
Definition: api.h:18