25 fbk_units = PidFeedbackUnitsEnum::RAW_MACHINE_UNITS;
26 out_units = PidOutputUnitsEnum::RAW_MACHINE_UNITS;
228ControlBoardHelper::ControlBoardHelper(
int n,
const int *aMap,
const double *angToEncs,
const double *zs,
const double *newtons,
const double *amps,
const double *volts,
const double *dutycycles,
const double *kbemf,
const double *ktau)
234 memcpy(mPriv->
axisMap, aMap,
sizeof(
int)*n);
239 if (angToEncs !=
nullptr) {
242 if (newtons !=
nullptr) {
245 if (amps !=
nullptr) {
248 if (volts !=
nullptr) {
251 if (dutycycles !=
nullptr) {
254 if (kbemf !=
nullptr) {
255 memcpy(mPriv->
bemfToRaws, kbemf,
sizeof(
double) * n);
257 if (ktau !=
nullptr) {
258 memcpy(mPriv->
ktauToRaws, ktau,
sizeof(
double) * n);
262 memset (mPriv->
invAxisMap, 0,
sizeof(
int) * n);
264 for (i = 0; i < n; i++)
267 for (j = 0; j < n; j++)
280 if (mPriv) {
delete mPriv; mPriv =
nullptr; }
296 if( (
id >= mPriv->
nj) || (
id< 0))
310 if(n_axes > mPriv->
nj)
313 yCError(CB_HELPER,
"checkAxesIds: num of axes is too big");
317 if(axesList==
nullptr)
320 yCError(CB_HELPER,
"checkAxesIds: axesList is nullptr");
324 for(
int idx = 0; idx<n_axes; idx++)
326 if( (axesList[idx]<0) || (axesList[idx]>= mPriv->
nj) )
329 yCError(CB_HELPER,
"checkAxesIds: joint id out of bound");
339{
return mPriv->
axisMap[axis]; }
347 for (
int k = 0; k < mPriv->
nj; k++) {
348 user[
toUser(k)] = hwData[k];
353void ControlBoardHelper::ControlBoardHelper::toUser(
const int *hwData,
int *user)
355 for (
int k = 0; k < mPriv->nj; k++) {
356 user[toUser(k)] = hwData[k];
363 for (
int k = 0; k < mPriv->
nj; k++) {
364 hwData[
toHw(k)] = usr[k];
371 for (
int k = 0; k < mPriv->
nj; k++) {
372 hwData[
toHw(k)] = usr[k];
416 for(
int j=0;j<mPriv->
nj;j++)
418 impN2S(newtons[j], j, tmp, index);
439 for(
int j=0;j<mPriv->
nj;j++)
441 trqN2S(newtons[j], j, tmp, index);
451 for(
int j=0;j<mPriv->
nj;j++)
453 trqS2N(sens[j], j, tmp, index);
474 for(
int j=0;j<mPriv->
nj;j++)
476 impS2N(sens[j], j, tmp, index);
571 for(
int j=0;j<mPriv->
nj;j++)
573 posA2E(ang[j], j, tmp, index);
583 for(
int j=0;j<mPriv->
nj;j++)
585 posE2A(enc[j], j, tmp, index);
594 for(
int j=0;j<mPriv->
nj;j++)
596 velA2E(ang[j], j, tmp, index);
605 for(
int j=0;j<mPriv->
nj;j++)
616 for(
int j=0;j<mPriv->
nj;j++)
618 velE2A(enc[j], j, tmp, index);
627 for(
int j=0;j<mPriv->
nj;j++)
638 for(
int j=0;j<mPriv->
nj;j++)
640 accA2E(ang[j], j, tmp, index);
649 for(
int j=0;j<mPriv->
nj;j++)
660 for(
int j=0;j<mPriv->
nj;j++)
662 accE2A(enc[j], j, tmp, index);
671 for(
int j=0;j<mPriv->
nj;j++)
695 for(
int j=0;j<mPriv->
nj;j++)
707 for(
int j=0;j<mPriv->
nj;j++)
744 for(
int j=0;j<mPriv->
nj;j++)
756 for(
int j=0;j<mPriv->
nj;j++)
792 for (
int j = 0; j<mPriv->
nj; j++)
803 for (
int j = 0; j<mPriv->
nj; j++)
806 dutycycle[index] = tmp;
868 bemf_user = bemf_raw / mPriv->
bemfToRaws[j_user];
874 ktau_user = ktau_raw / mPriv->
ktauToRaws[j_user];
952 double output_conversion_factor;
953 switch ((mPriv->
pid_units[pidtype])[j].out_units)
961 default: output_conversion_factor = mPriv->
helper_ones[j];
break;
963 return output_conversion_factor;
968 double feedback_conversion_factor = 0.0;
969 switch (( mPriv->
pid_units[pidtype])[j].fbk_units)
1004 default: feedback_conversion_factor = mPriv->
helper_ones[j];
break;
1006 return (1.0 / feedback_conversion_factor);
1011 k_usr = cb_helper->
toUser(j_raw);
1016 out_usr.
kp = out_usr.
kp / feedback_conversion_units_user2raw;
1017 out_usr.
ki = out_usr.
ki / feedback_conversion_units_user2raw;
1018 out_usr.
kd = out_usr.
kd / feedback_conversion_units_user2raw;
1022 out_usr.
kp = out_usr.
kp / output_conversion_units_user2raw;
1023 out_usr.
ki = out_usr.
ki / output_conversion_units_user2raw;
1024 out_usr.
kd = out_usr.
kd / output_conversion_units_user2raw;
1026 out_usr.
max_int = out_usr.
max_int / output_conversion_units_user2raw;
1029 out_usr.
offset = out_usr.
offset / output_conversion_units_user2raw;
1043 k_raw = cb_helper->
toHw(j_usr);
1048 out_raw.
kp = out_raw.
kp * feedback_conversion_units_user2raw;
1049 out_raw.
ki = out_raw.
ki * feedback_conversion_units_user2raw;
1050 out_raw.
kd = out_raw.
kd * feedback_conversion_units_user2raw;
1054 out_raw.
kp = out_raw.
kp * output_conversion_units_user2raw;
1055 out_raw.
ki = out_raw.
ki * output_conversion_units_user2raw;
1056 out_raw.
kd = out_raw.
kd * output_conversion_units_user2raw;
1058 out_raw.
max_int = out_raw.
max_int * output_conversion_units_user2raw;
1061 out_raw.
offset = out_raw.
offset * output_conversion_units_user2raw;
1074 cb_helper->
posA2E(userval, j, machineval, k);
1079 cb_helper->
velA2E(userval, j, machineval, k);
1084 cb_helper->
trqN2S(userval, j, machineval, k);
1089 cb_helper->
ampereA2S(userval, j, machineval, k);
1092 yError() <<
"convert_units_to_machine: invalid pidtype";
1107 cb_helper->
posA2E(userval, machineval);
1112 cb_helper->
velA2E(userval, machineval);
1117 cb_helper->
trqN2S(userval, machineval);
1122 cb_helper->
ampereA2S(userval, machineval);
1125 yError() <<
"convert_units_to_machine: invalid pidtype";
1140 *userval = cb_helper->
posE2A(machineval, k);
1145 *userval = cb_helper->
velE2A(machineval, k);
1150 *userval = cb_helper->
trqS2N(machineval, k);
1155 *userval = cb_helper->
ampereS2A(machineval, k);
1158 yError() <<
"convert_units_to_machine: invalid pidtype";
1173 cb_helper->
posE2A(machineval, userval);
1178 cb_helper->
velE2A(machineval, userval);
1183 cb_helper->
trqS2N(machineval, userval);
1188 cb_helper->
ampereS2A(machineval, userval);
1191 yError() <<
"convert_units_to_machine: invalid pidtype";
1199 int nj = cb_helper->
axes();
1200 for (
int i = 0; i < nj; i++)
1202 mPriv->
pid_units[pidtype][i].fbk_units = fbk_conv_units;
1203 mPriv->
pid_units[pidtype][i].out_units = out_conv_units;
void checkAndDestroy(T *&p)
double * viscousPosToRaws
double * newtonsToSensors
double * coulombNegToRaws
PidUnits * PosDirPid_units
double * velocityThresToRaws
PrivateUnitsHandler(int size)
PidUnits * VelDirPid_units
PrivateUnitsHandler(const PrivateUnitsHandler &other)
double * viscousNegToRaws
std::map< PidControlTypeEnum, PidUnits * > pid_units
double * coulombPosToRaws
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)
double velocityThres_user2raw(double velocityThres_user, int j)
void velocityThres_raw2user(double threshold_raw, int k_raw, double &velocityThres_user, int &j_user)
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 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)
double ki
integrative gain
double kp
proportional gain
double max_output
max output
double stiction_down_val
down stiction offset added to the pid output
double stiction_up_val
up stiction offset added to the pid output
double offset
pwm offset added to the pid output
double max_int
saturation threshold for the integrator
Contains the parameters for a PID.
#define yCError(component,...)
#define YARP_LOG_COMPONENT(name,...)
For streams capable of holding different kinds of content, check what they actually have.
@ VOCAB_PIDTYPE_VELOCITY_DIRECT_2
@ VOCAB_PIDTYPE_VELOCITY_3
@ VOCAB_PIDTYPE_POSITION_DIRECT_1
@ VOCAB_PIDTYPE_CURRENT_3
@ VOCAB_PIDTYPE_CURRENT_1
@ VOCAB_PIDTYPE_VELOCITY_2
@ VOCAB_PIDTYPE_CURRENT_2
@ VOCAB_PIDTYPE_VELOCITY_DIRECT_1
@ VOCAB_PIDTYPE_POSITION_DIRECT_3
@ VOCAB_PIDTYPE_VELOCITY_1
@ VOCAB_PIDTYPE_VELOCITY_DIRECT_3
@ VOCAB_PIDTYPE_POSITION_1
@ VOCAB_PIDTYPE_POSITION_DIRECT_2
@ VOCAB_PIDTYPE_POSITION_2
@ VOCAB_PIDTYPE_POSITION_3
PidOutputUnitsEnum out_units
PidFeedbackUnitsEnum fbk_units