diff --git a/cob_base_drive_chain/common/include/cob_base_drive_chain/CanCtrlPltfCOb3.h b/cob_base_drive_chain/common/include/cob_base_drive_chain/CanCtrlPltfCOb3.h index e1030ba61..060950ad5 100644 --- a/cob_base_drive_chain/common/include/cob_base_drive_chain/CanCtrlPltfCOb3.h +++ b/cob_base_drive_chain/common/include/cob_base_drive_chain/CanCtrlPltfCOb3.h @@ -287,6 +287,8 @@ class CanCtrlPltfCOb3 // : public CanCtrlPltfItf double dWheel3SteerDriveCoupling; double dWheel4SteerDriveCoupling; + double dHomeVeloRadS; + int iRadiusWheelMM; int iDistSteerAxisToDriveWheelMM; diff --git a/cob_base_drive_chain/common/src/CanCtrlPltfCOb3.cpp b/cob_base_drive_chain/common/src/CanCtrlPltfCOb3.cpp index 10332be35..af04a2a38 100644 --- a/cob_base_drive_chain/common/src/CanCtrlPltfCOb3.cpp +++ b/cob_base_drive_chain/common/src/CanCtrlPltfCOb3.cpp @@ -269,6 +269,8 @@ void CanCtrlPltfCOb3::readConfiguration() m_IniFile.GetKeyInt("Geom", "RadiusWheel", &m_Param.iRadiusWheelMM, true); m_IniFile.GetKeyInt("Geom", "DistSteerAxisToDriveWheelCenter", &m_Param.iDistSteerAxisToDriveWheelMM, true); + m_IniFile.GetKeyDouble("DrivePrms","HomingVelocityRadS", &m_Param.dHomeVeloRadS, true); + if(m_iNumDrives >= 1) m_IniFile.GetKeyDouble("DrivePrms", "Wheel1SteerDriveCoupling", &m_Param.dWheel1SteerDriveCoupling, true); if(m_iNumDrives >= 2) @@ -960,7 +962,6 @@ bool CanCtrlPltfCOb3::initPltf() std::vector vdFactorVel; // vdFactorVel.assign(4,0); vdFactorVel.assign(m_iNumDrives,0); - double dhomeVeloRadS = -1.0; // Start can open network @@ -1063,8 +1064,8 @@ bool CanCtrlPltfCOb3::initPltf() // make motors move for (int i = 0; isetGearVelRadS(dhomeVeloRadS); - vpDriveMotor[i]->setGearVelRadS(dhomeVeloRadS * vdFactorVel[i]); + vpSteerMotor[i]->setGearVelRadS(m_Param.dHomeVeloRadS); + vpDriveMotor[i]->setGearVelRadS(m_Param.dHomeVeloRadS * vdFactorVel[i]); } // wait at least 0.5 sec.