FollowTrajectory and TrajectoryGenerator update

src/frc846/cpp/frc846/swerve/follow_trajectory_command.cc

@@ -0,0 +1,88 @@
+#include "frc846/swerve/follow_trajectory_command.h"
+
+namespace frc846::swerve {
+
+FollowTrajectoryCommand::FollowTrajectoryCommand(
+    RobotContainer& container, std::vector<frc846::Waypoint> input_points)
+    : frc846::robot::GenericCommand<
+          RobotContainer, FollowTrajectoryCommand>{container,
+                                                   "follow_trajectory_command"},
+      input_points_(input_points) {
+  AddRequirements({&container_.drivetrain_});
+
+  wtcalculator.setConstants({container_.drivetrain_.max_speed_.value(),
+                             container_.drivetrain_.max_acceleration_.value(),
+                             2_in, 20_ms});
+}
+
+void FollowTrajectoryCommand::OnInit() {
+  Log("Starting Trajectory");
+  for (frc846::Waypoint i : input_points_) {
+    Log("points x{} y{} bearing {}", i.point[0], i.point[1], i.bearing);
+  }
+  Log("initial pose x{}, y{}, bearing {}",
+      container_.drivetrain_.GetReadings().pose.point[0],
+      container_.drivetrain_.GetReadings().pose.point[1],
+      container_.drivetrain_.GetReadings().pose.bearing);
+  is_done_ = false;
+
+  start_time_ = frc846::wpilib::CurrentFPGATime();
+
+  input_points_.insert(input_points_.begin(), 1,
+                       {container_.drivetrain_.GetReadings().pose});
+
+  target_idx_ = 1;
+
+  if (input_points_.size() < 2) {
+    Error("trajectory size ({}) is less than 2 - ending!",
+          input_points_.size());
+    is_done_ = true;
+  }
+}
+
+void FollowTrajectoryCommand::Periodic() {
+  // Just in case trajectory size was < 2
+  if (is_done_) {
+    return;
+  }
+
+  frc846::swerve::WTCInput lpcinput{
+      input_points_[target_idx_ - 1].point,
+      input_points_[target_idx_].point,
+      container_.drivetrain_.GetReadings().pose.point,
+      container_.drivetrain_.GetReadings().pose.bearing,
+      input_points_[target_idx_].bearing,
+      container_.drivetrain_.GetReadings().pose.velocity.magnitude(),
+      input_points_[target_idx_].velocity.magnitude()};
+
+  frc846::swerve::WTCOutput lpcoutput = wtcalculator.calculate(lpcinput);
+
+  if (lpcoutput.crossed_waypt) {
+    target_idx_++;
+    Log("Cross waypoint - now on {}/{}", target_idx_ + 1, input_points_.size());
+
+    if (target_idx_ == input_points_.size()) {
+      Log("Done!");
+      is_done_ = true;
+      return;
+    }
+  }
+
+  DrivetrainTarget target;
+  target.v_x = lpcoutput.target_vel[0];
+  target.v_y = lpcoutput.target_vel[1];
+  target.translation_reference = DrivetrainTranslationReference::kField;
+  target.rotation = DrivetrainRotationPosition(lpcoutput.bearing);
+  target.control = kClosedLoop;
+
+  container_.drivetrain_.SetTarget(target);
+}
+
+void FollowTrajectoryCommand::OnEnd(bool interrupted) {
+  (void)interrupted;
+  container_.drivetrain_.SetTargetZero();
+}
+
+bool FollowTrajectoryCommand::IsFinished() { return is_done_; }
+
+};  // namespace frc846::swerve

src/frc846/cpp/frc846/swerve/waypt_traversal_calculator.cc

@@ -0,0 +1,64 @@
+#include "frc846/swerve/waypt_traversal_calculator.h"
+
+namespace frc846::swerve {
+
+bool WayptTraversalCalculator::HasCrossedWaypt(WTCInput input) {
+  auto disp_vec = (input.current_pos - input.start_pos);
+
+  auto target_vec = (input.target_pos - input.start_pos);
+
+  if (target_vec.magnitude() <= constants_.loc_tolerance) {
+    return true;
+  }
+
+  return disp_vec.projectOntoAnother(target_vec).magnitude() >=
+         target_vec.magnitude();
+};
+
+WTCOutput WayptTraversalCalculator::calculate(WTCInput input) {
+  WTCOutput result{};
+  result.target_vel = {0.0_fps, 0.0_fps};
+  result.bearing = 0_deg;
+
+  auto delta_vec = (input.target_pos - input.current_pos);
+
+  if (delta_vec.magnitude() < constants_.loc_tolerance) {
+    result.crossed_waypt = true;
+
+    return result;
+  }
+  result.crossed_waypt = HasCrossedWaypt(input);
+
+  if (result.crossed_waypt) return result;
+
+  auto dir_vec = delta_vec.unit();
+
+  units::second_t stopping_time =
+      units::math::abs(input.current_vel - input.target_vel) /
+      constants_.max_acceleration;
+  units::foot_t stopping_distance =
+      (input.current_vel + input.target_vel) / 2 * stopping_time;
+
+  result.bearing = (input.target_bearing - input.current_bearing) *
+                   constants_.loop_time / stopping_time;
+
+  auto target_vel_mag = constants_.max_speed;
+
+  if (stopping_distance > delta_vec.magnitude() - constants_.loc_tolerance) {
+    target_vel_mag =
+        input.current_vel + (input.target_vel - input.current_vel) *
+                                constants_.loop_time / stopping_time;
+
+  } else if (input.current_vel < constants_.max_speed) {
+    target_vel_mag =
+        input.current_vel + constants_.max_acceleration * constants_.loop_time;
+  }
+  target_vel_mag = units::math::min(target_vel_mag, constants_.max_speed);
+
+  result.target_vel = {dir_vec[0].to<double>() * target_vel_mag,
+                       dir_vec[1].to<double>() * target_vel_mag};
+
+  return result;
+}
+
+};  // namespace frc846::swerve

src/frc846/include/frc846/math/vectors.h

@@ -156,7 +156,7 @@ class VectorND {
   T dot(const VectorND<T, N>& other) const {
     T result = T{};
     for (size_t i = 0; i < N; ++i) {
-      result += data[i] * other[i];
+      result += data[i] * other[i].template to<double>();
     }
     return result;
   }
@@ -186,12 +186,12 @@ class VectorND {
 
   // Projects this vector onto another and returns
   VectorND<T, N> projectOntoAnother(const VectorND<T, N>& other) const {
-    return other.unit() * dot(other.unit()).template to<double>();
+    return other.projectOntoThis(*this);
   }
 
   // Projects another vector onto this and returns
   VectorND<T, N> projectOntoThis(const VectorND<T, N>& other) const {
-    return unit() * other.dot(*this);
+    return unit() * dot(other).template to<double>();
   }
 
   // Returns the angle of this vector

src/frc846/include/frc846/robot/GenericCommand.h

@@ -91,20 +91,19 @@ class GenericCommandGroup
       : Loggable{name}, container_{container} {
     frc2::Command::SetName(name);
 
-    auto start_command_added = frc2::InstantCommand([&] {
-      Log("Command group {} starting.", name);
-      command_start_time_ = frc846::wpilib::CurrentFPGATime();
-    });
-    auto end_command_added = frc2::InstantCommand([&] {
-      units::millisecond_t elapsed_time =
-          frc846::wpilib::CurrentFPGATime() - command_start_time_;
-      Log("Command group {} ending. Took {} ms to complete.", name,
-          elapsed_time.to<double>());
-    });
+    // auto start_command_added = frc2::InstantCommand([&] {
+    //   Log("Command group {} starting.", name);
+    //   command_start_time_ = frc846::wpilib::CurrentFPGATime();
+    // });
+    // auto end_command_added = frc2::InstantCommand([&] {
+    //   units::millisecond_t elapsed_time =
+    //       frc846::wpilib::CurrentFPGATime() - command_start_time_;
+    //   Log("Command group {} ending. Took {} ms to complete.", name,
+    //       elapsed_time.to<double>());
+    // });
 
     frc2::SequentialCommandGroup::AddCommands(
-        start_command_added, std::forward<Commands>(commands)...,
-        end_command_added);
+        std::forward<Commands>(commands)...);
 
     Log("Constructing instance of command group {}.", name);
   }