From 68a81fa412a9c3bd3c473c3870d993834379f2ae Mon Sep 17 00:00:00 2001 From: Saurabh Kamat Date: Thu, 15 Feb 2024 14:21:16 +0800 Subject: [PATCH] Added PID control to steer joint in velocity mode Signed-off-by: Saurabh Kamat --- .../ackermann_steering/AckermannSteering.cc | 19 +++++++++++-------- 1 file changed, 11 insertions(+), 8 deletions(-) diff --git a/src/systems/ackermann_steering/AckermannSteering.cc b/src/systems/ackermann_steering/AckermannSteering.cc index 39e5d4ac8a..22ce49eb78 100644 --- a/src/systems/ackermann_steering/AckermannSteering.cc +++ b/src/systems/ackermann_steering/AckermannSteering.cc @@ -952,14 +952,17 @@ void AckermannSteeringPrivate::UpdateVelocity( return; } - double leftDelta = leftSteeringJointAngle - leftSteeringPos->Data()[0]; - double rightDelta = rightSteeringJointAngle - rightSteeringPos->Data()[0]; - - // Simple proportional control with a gain of 1 - // Adding programmable PID values might be a future feature. - // Works as is for tested cases - this->leftSteeringJointSpeed = leftDelta; - this->rightSteeringJointSpeed = rightDelta; + double leftDelta = leftSteeringPos->Data()[0] - leftSteeringJointAngle; + double rightDelta = rightSteeringPos->Data()[0] - rightSteeringJointAngle; + + // Simple PID control with settable gains. + this->leftSteeringJointSpeed = this->steerPosPid.Update( + leftDelta, _info.dt); + this->steerPosPid.Reset(); + + this->rightSteeringJointSpeed = this->steerPosPid.Update( + rightDelta, _info.dt); + this->steerPosPid.Reset(); } //////////////////////////////////////////////////