From af98e92fe8557a04e17f85e66ead6fbd823a80bc Mon Sep 17 00:00:00 2001 From: Prateek Machiraju Date: Wed, 26 Jun 2019 15:53:41 -0400 Subject: [PATCH] Fix bug with acceleration --- src/include/fl/mathematics/control/TrajectoryTracker.h | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/include/fl/mathematics/control/TrajectoryTracker.h b/src/include/fl/mathematics/control/TrajectoryTracker.h index 2146586..06ccfb1 100644 --- a/src/include/fl/mathematics/control/TrajectoryTracker.h +++ b/src/include/fl/mathematics/control/TrajectoryTracker.h @@ -45,13 +45,14 @@ class TrajectoryTracker { return {linear_velocity, 0.0, angular_velocity, 0.0}; } - const auto previous_velocity = *previous_velocity_; + TrajectoryTrackerOutput output{ + linear_velocity, (linear_velocity - previous_velocity_->linear_velocity) / dt, angular_velocity, + (angular_velocity - previous_velocity_->angular_velocity) / dt}; previous_velocity_->linear_velocity = linear_velocity; previous_velocity_->angular_velocity = angular_velocity; - return {linear_velocity, (linear_velocity - previous_velocity.linear_velocity) / dt, angular_velocity, - (angular_velocity - previous_velocity.angular_velocity) / dt}; + return output; } virtual TrajectoryTrackerVelocityOutput CalculateState(const TimedIterator& iterator,