introduced param to set homing velocity

ipa-nhg · Sep 6, 2016 · 7edde94 · 7edde94
1 parent 2c21a40
commit 7edde94
Show file tree

Hide file tree

Showing 2 changed files with 6 additions and 3 deletions.
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
@@ -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
@@ -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<double> 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; i<m_iNumDrives; i++)
 			{
-				vpSteerMotor[i]->setGearVelRadS(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.