more debug prints

SleipnirGroup · Aug 24, 2023 · e29a560 · e29a560
1 parent 51b176b
commit e29a560
Showing 1 changed file with 2 additions and 0 deletions.
diff --git 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) {