diff --git a/FalconLibraryCPP.sln.DotSettings.user b/FalconLibraryCPP.sln.DotSettings.user
index 736d9ef..e13cfe1 100644
--- a/FalconLibraryCPP.sln.DotSettings.user
+++ b/FalconLibraryCPP.sln.DotSettings.user
@@ -1,5 +1,7 @@
True
True
+ True
+ True
True
True
\ No newline at end of file
diff --git a/FalconLibraryCPP/FalconLibraryCPP.vcxproj b/FalconLibraryCPP/FalconLibraryCPP.vcxproj
index 7f7c2ab..00a8117 100644
--- a/FalconLibraryCPP/FalconLibraryCPP.vcxproj
+++ b/FalconLibraryCPP/FalconLibraryCPP.vcxproj
@@ -124,6 +124,8 @@
+
+
diff --git a/FalconLibraryCPP/FalconLibraryCPP.vcxproj.filters b/FalconLibraryCPP/FalconLibraryCPP.vcxproj.filters
index b1db845..3e8b72a 100644
--- a/FalconLibraryCPP/FalconLibraryCPP.vcxproj.filters
+++ b/FalconLibraryCPP/FalconLibraryCPP.vcxproj.filters
@@ -81,5 +81,11 @@
Header Files
+
+ Header Files
+
+
+ Header Files
+
\ No newline at end of file
diff --git a/FalconLibraryCPP/src/FalconLibrary.h b/FalconLibraryCPP/src/FalconLibrary.h
index aacf517..3fef48c 100644
--- a/FalconLibraryCPP/src/FalconLibrary.h
+++ b/FalconLibraryCPP/src/FalconLibrary.h
@@ -15,6 +15,8 @@
#include "mathematics/trajectory/TrajectoryGenerator.h"
+#include "mathematics/control/RamseteTracker.h"
+
#include "Utilities.h"
namespace frc5190 {}
\ No newline at end of file
diff --git a/FalconLibraryCPP/src/mathematics/control/RamseteTracker.h b/FalconLibraryCPP/src/mathematics/control/RamseteTracker.h
new file mode 100644
index 0000000..ee42938
--- /dev/null
+++ b/FalconLibraryCPP/src/mathematics/control/RamseteTracker.h
@@ -0,0 +1,39 @@
+#pragma once
+
+#include "TrajectoryTracker.h"
+
+namespace frc5190 {
+class RamseteTracker : public TrajectoryTracker {
+ public:
+ RamseteTracker(const double beta, const double zeta) : beta_(beta), zeta_(zeta) {}
+
+ TrajectoryTrackerVelocityOutput CalculateState(const TimedIterator& iterator,
+ const Pose2d& robot_pose) const override {
+
+ const auto reference_state = iterator.CurrentState().state;
+ const auto error = reference_state.State().Pose().InFrameOfReferenceOf(robot_pose);
+
+ const auto vd = reference_state.Velocity();
+ const auto wd = vd * reference_state.State().Curvature();
+
+ const auto k1 = 2 * zeta_ * std::sqrt(wd * wd + beta_ * vd * vd);
+ const auto angle_error = error.Rotation().Radians();
+
+ const auto linear = vd * error.Rotation().Cos() + k1 * error.Translation().X();
+ const auto angular = wd + beta_ * vd * Sinc(angle_error) * error.Translation().Y() + k1 * angle_error;
+
+ return {linear, angular};
+ }
+
+ private:
+ static double Sinc(const double theta) {
+ if (EpsilonEquals(theta, 0.0)) {
+ return 1.0 - 1.0 / 6.0 * theta * theta;
+ }
+ return std::sin(theta) / theta;
+ }
+
+ double beta_;
+ double zeta_;
+};
+} // namespace frc5190
diff --git a/FalconLibraryCPP/src/mathematics/control/TrajectoryTracker.h b/FalconLibraryCPP/src/mathematics/control/TrajectoryTracker.h
new file mode 100644
index 0000000..ad06120
--- /dev/null
+++ b/FalconLibraryCPP/src/mathematics/control/TrajectoryTracker.h
@@ -0,0 +1,66 @@
+#pragma once
+#include "../geometry/Pose2dWithCurvature.h"
+#include "../trajectory/TimedTrajectory.h"
+
+#include
+
+namespace frc5190 {
+
+struct TrajectoryTrackerVelocityOutput {
+ double linear_velocity = 0.0;
+ double angular_velocity = 0.0;
+};
+
+struct TrajectoryTrackerOutput {
+ double linear_velocity = 0.0;
+ double linear_acceleration = 0.0;
+ double angular_velocity = 0.0;
+ double angular_acceleration = 0.0;
+};
+
+class TrajectoryTracker {
+ public:
+ void Reset(const TimedTrajectory& trajectory) {
+ iterator_ = dynamic_cast*>(trajectory.Iterator().get());
+ previous_velocity_ = nullptr;
+ previous_time_ = -1.;
+ }
+
+ TrajectoryTrackerOutput NextState(const Pose2d& current_pose, const double current_time) {
+ if (iterator_ == nullptr) throw std::exception("Iterator was nullptr.");
+ const auto& iterator = *iterator_;
+
+ const auto dt = (previous_time_ < 0.0) ? 0.0 : current_time - previous_time_;
+ previous_time_ = current_time;
+
+ const auto velocity = CalculateState(iterator, current_pose);
+ const auto previous_velocity = *previous_velocity_;
+
+ const auto linear_velocity = velocity.linear_velocity;
+ const auto angular_velocity = velocity.angular_velocity;
+
+ previous_velocity_->linear_velocity = linear_velocity;
+ previous_velocity_->angular_velocity = angular_velocity;
+
+ if (previous_velocity_ == nullptr || dt <= 0.) {
+ return {linear_velocity, 0.0, angular_velocity, 0.0};
+ }
+ return {linear_velocity, (linear_velocity - previous_velocity.linear_velocity) / dt, angular_velocity,
+ (angular_velocity - previous_velocity.angular_velocity) / dt};
+ }
+
+ virtual TrajectoryTrackerVelocityOutput CalculateState(const TimedIterator& iterator,
+ const Pose2d& robot_pose) const = 0;
+
+ TrajectorySamplePoint> ReferencePoint() const {
+ return iterator_->CurrentState();
+ }
+
+ bool IsFinished() const { return iterator_->IsDone(); }
+
+ private:
+ TimedIterator* iterator_ = nullptr;
+ std::unique_ptr previous_velocity_;
+ double previous_time_ = -1.;
+};
+} // namespace frc5190
diff --git a/FalconLibraryCPP/src/mathematics/geometry/Pose2d.h b/FalconLibraryCPP/src/mathematics/geometry/Pose2d.h
index 2432a78..72375a7 100644
--- a/FalconLibraryCPP/src/mathematics/geometry/Pose2d.h
+++ b/FalconLibraryCPP/src/mathematics/geometry/Pose2d.h
@@ -59,6 +59,8 @@ class Pose2d final : public VaryInterpolatable {
return EpsilonEquals(twist.Dy(), 0.0) && EpsilonEquals(twist.Dtheta(), 0.0);
}
+ Pose2d InFrameOfReferenceOf(const Pose2d& other) const { return (-other) + *this; }
+
// Static Methods
static Twist2d ToTwist(const Pose2d& pose) {
const auto dtheta = pose.rotation_.Radians();
diff --git a/FalconLibraryCPP/src/mathematics/trajectory/TrajectoryIterator.h b/FalconLibraryCPP/src/mathematics/trajectory/TrajectoryIterator.h
index 2a8c828..23a08a7 100644
--- a/FalconLibraryCPP/src/mathematics/trajectory/TrajectoryIterator.h
+++ b/FalconLibraryCPP/src/mathematics/trajectory/TrajectoryIterator.h
@@ -29,8 +29,8 @@ class TrajectoryIterator {
return trajectory_->Sample(progress);
}
- bool IsDone() const { return progress_ >= trajectory_->LastInterpolant(); }
- TrajectoryPoint CurrentState() const { return sample_; }
+ bool IsDone() const { return progress_ >= trajectory_->LastInterpolant(); }
+ TrajectorySamplePoint CurrentState() const { return sample_; }
protected:
virtual U Addition(U a, U b) const = 0;
diff --git a/Tests/Tests.vcxproj b/Tests/Tests.vcxproj
index 490efae..f68abcb 100644
--- a/Tests/Tests.vcxproj
+++ b/Tests/Tests.vcxproj
@@ -43,6 +43,7 @@
Create
Create
+
diff --git a/Tests/ramsete-tests.cpp b/Tests/ramsete-tests.cpp
new file mode 100644
index 0000000..33eff15
--- /dev/null
+++ b/Tests/ramsete-tests.cpp
@@ -0,0 +1,21 @@
+#include "pch.h"
+#include "../FalconLibraryCPP/src/FalconLibrary.h"
+
+class RamseteTest : public ::testing::Test {
+ public:
+ void Run(frc5190::Pose2d initial, frc5190::Pose2d final, double max_velocity = 3,
+ double max_acceleration = 2, bool backwards = false) {
+ auto trajectory = frc5190::TrajectoryGenerator::GenerateTrajectory(
+ std::vector{initial, final},
+ std::vector*>{}, 0.0, 0.0, max_velocity,
+ max_acceleration, backwards);
+
+ frc5190::RamseteTracker tracker{2.0, 0.7};
+ tracker.Reset(trajectory);
+ }
+};
+
+TEST_F(RamseteTest, Test) {
+ Run(frc5190::Pose2d{0.0, 0.0, frc5190::Rotation2d::FromDegrees(0.0)},
+ frc5190::Pose2d{10.0, 10.0, frc5190::Rotation2d::FromDegrees(50.0)});
+}
\ No newline at end of file