72 m_vdCtrlVal.assign( 4, std::vector<double> (2,0.0) );
153 for(
int i = 0; i<4; i++)
176 for(
int i=0; i<4; i++)
186 for(
int i = 0; i<4; i++)
198 double dCurrentPosWheelRAD;
199 double dtempDeltaPhi1RAD, dtempDeltaPhi2RAD;
200 double dtempDeltaPhiCmd1RAD, dtempDeltaPhiCmd2RAD;
201 double dtempWeightedDelta1RAD, dtempWeightedDelta2RAD;
212 for (
int i = 0; i<4; i++)
235 dtempWeightedDelta1RAD = 0.6*fabs(dtempDeltaPhi1RAD) + 0.4*fabs(dtempDeltaPhiCmd1RAD);
236 dtempWeightedDelta2RAD = 0.6*fabs(dtempDeltaPhi2RAD) + 0.4*fabs(dtempDeltaPhiCmd2RAD);
239 if (dtempWeightedDelta1RAD <= dtempWeightedDelta2RAD)
329 double & dVelLongMMS,
double & dVelLatMMS,
double & dRotRobRadS,
double & dRotVelRadS)
359 double & dVelLongMMS,
double & dVelLatMMS,
double & dRotRobRadS,
double & dRotVelRadS)
379 double dtempAxVelXRobMMS, dtempAxVelYRobMMS;
384 for(
int i = 0; i<4; i++)
395 for (
int i = 0; i<4; i++)
424 double dtempVelXRobMMS;
425 double dtempVelYRobMMS;
426 double dtempRotRobRADPS;
429 double dtempRelPhiWheelsRAD;
430 double dtempRelDistWheelsMM;
431 double dtempRelPhiWheel1RAD;
432 double dtempRelPhiWheel2RAD;
433 std::vector<double> vdtempVelWheelMMS(4);
438 dtempRotRobRADPS = 0;
450 for(
int i = 0; i< (m_iNumberOfDrives-1) ; i++)
455 dtempRelDistWheelsMM = sqrt( dtempDiffXMM*dtempDiffXMM + dtempDiffYMM*dtempDiffYMM );
462 dtempRotRobRADPS += (vdtempVelWheelMMS[i+1] * sin(dtempRelPhiWheel2RAD) - vdtempVelWheelMMS[i] * sin(dtempRelPhiWheel1RAD))/dtempRelDistWheelsMM;
469 dtempRelDistWheelsMM = sqrt( dtempDiffXMM*dtempDiffXMM + dtempDiffYMM*dtempDiffYMM );
477 dtempRotRobRADPS += (vdtempVelWheelMMS[0]*sin(dtempRelPhiWheel2RAD) - vdtempVelWheelMMS[m_iNumberOfDrives-1]*sin(dtempRelPhiWheel1RAD))/dtempRelDistWheelsMM;
502 for(
int i = 0; i<4; i++)
526 for(
int i=0; i<4; i++)
535 double dCurrentPosWheelRAD;
537 double dForceDamp, dForceProp, dAccCmd, dVelCmdInt;
539 for (
int i=0; i<4; i++)
558 for (
int i = 0; i<4; i++)
571 dAccCmd = (dForceDamp + dForceProp) /
m_dVirtM;
590 m_vdCtrlVal[i][0] = dDeltaPhi;
591 m_vdCtrlVal[i][1] = dVelCmdInt;
606 for (
int i = 0; i<4; i++)
682 for(
int i=0; i<4; i++)
684 for(
int j=0; j< 2; j++)
690 for(
int i=0; i<4; i++)
std::vector< double > m_vdWheelXPosMM
std::vector< std::vector< double > > m_vdCtrlVal
void setEMStopActive(bool bEMStopActive)
~UndercarriageCtrlGeom(void)
std::vector< double > m_vdExWheelDistMM
static double atan4quad(double y, double x)
std::vector< double > m_vdAngGearSteerCmdRad
std::vector< double > m_vdAngGearSteerTargetRad
std::vector< double > m_vdVelGearDriveCmdRadS
void operator=(const UndercarriageCtrlGeom &GeomCtrl)
void CalcControlStep(void)
std::vector< double > m_vdDltAngGearDriveRad
std::vector< double > m_vdAngGearSteerTarget2Rad
std::vector< double > m_vdAngGearSteerTarget1Rad
static void normalizePi(double &angle)
std::vector< double > vdSteerDriveCoupling
void GetNewCtrlStateSteerDriveSetValues(std::vector< double > &vdVelGearDriveRadS, std::vector< double > &vdVelGearSteerRadS, std::vector< double > &vdAngGearSteerRad, double &dVelLongMMS, double &dVelLatMMS, double &dRotRobRadS, double &dRotVelRadS)
std::string m_sIniDirectory
std::vector< double > m_vdVelGearDriveRadS
void InitUndercarriageCtrl(void)
int GetKeyInt(const char *pSect, const char *pKey, int *pValue, bool bWarnIfNotfound=true)
double dMaxDriveRateRadpS
std::vector< double > m_vdExWheelYPosMM
std::vector< double > m_vdWheelDistMM
int GetKeyDouble(const char *pSect, const char *pKey, double *pValue, bool bWarnIfNotfound=true)
void SetDesiredPltfVelocity(double dCmdVelLongMMS, double dCmdVelLatMMS, double dCmdRotRobRadS, double dCmdRotVelRadS)
std::vector< double > vdFactorVel
std::vector< double > m_vdVelGearDriveTarget2RadS
int iDistSteerAxisToDriveWheelMM
std::vector< double > m_vdVelGearSteerRadS
std::vector< double > m_vdVelGearDriveTargetRadS
int SetFileName(std::string fileName, std::string strIniFileUsedBy="", bool bCreate=false)
std::vector< double > m_vdAngGearSteerRad
std::vector< double > m_vdExWheelAngRad
double dMaxSteerRateRadpS
std::vector< double > WheelNeutralPos
std::vector< double > m_vdExWheelXPosMM
static double convDegToRad(const double &dAngDeg)
void CalcExWheelPos(void)
void SetActualWheelValues(std::vector< double > vdVelGearDriveRadS, std::vector< double > vdVelGearSteerRadS, std::vector< double > vdDltAngGearDriveRad, std::vector< double > vdAngGearSteerRad)
void GetSteerDriveSetValues(std::vector< double > &vdVelGearDriveRadS, std::vector< double > &vdAngGearSteerRad)
std::vector< double > m_vdWheelYPosMM
std::vector< double > m_vdVelGearSteerCmdRadS
ParamType m_UnderCarriagePrms
void GetActualPltfVelocity(double &dDeltaLongMM, double &dDeltaLatMM, double &dDeltaRotRobRad, double &dDeltaRotVelRad, double &dVelLongMMS, double &dVelLatMMS, double &dRotRobRadS, double &dRotVelRadS)
std::vector< double > m_vdVelGearDriveTarget1RadS
std::vector< double > m_vdWheelAngRad
UndercarriageCtrlGeom(std::string sIniDirectory)