From e29a5601465f94fcdd20382d6aea70eaecb1b32e Mon Sep 17 00:00:00 2001
From: shueja-personal <32416547+shueja-personal@users.noreply.github.com>
Date: Wed, 23 Aug 2023 19:34:32 -0700
Subject: [PATCH] more debug prints

---
 src/path/SwervePathBuilder.cpp | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/src/path/SwervePathBuilder.cpp b/src/path/SwervePathBuilder.cpp
index 9ddeba1b..fff50f26 100644
--- a/src/path/SwervePathBuilder.cpp
+++ b/src/path/SwervePathBuilder.cpp
@@ -78,11 +78,13 @@ void SwervePathBuilder::WptVelocityMagnitude(size_t idx, double v) {
   std::cout << v << std::endl;
   if (std::abs(v) < 0.0001) {
     WptZeroVelocity(idx);
+    
   } else {
     WptConstraint(idx,
                   HolonomicVelocityConstraint{EllipticalSet2d::CircularSet2d(v),
                                               CoordinateSystem::kField});
   }
+  std::cout << "Set constraint" << std::endl;
 }
 
 void SwervePathBuilder::WptZeroVelocity(size_t idx) {