commit 5e8ce3198ce682be95db999969b04ae9601faedc
author James Kuszmaul <jabukuszmaul+collab@gmail.com> Sat Mar 27 14:59:17 2021 -0700
committer James Kuszmaul <jabukuszmaul+collab@gmail.com> Sat Mar 27 15:07:13 2021 -0700
tree 1e75889dd24f5a2db520d3ad6f83ca63374d5a91
parent b2e840a04293c4397cac6f811a6b08b01c3bd190

Fix backwards spline feedback

We weren't handling the logic for feedback on backwards splines
correctly.

TODO: Clean up some of the backwards logic to make it possible to
follow.

Change-Id: I0c18b8c245010151356935dfcbf8babdb98d0846

frc971/control_loops/drivetrain/splinedrivetrain.cc

diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.cc b/frc971/control_loops/drivetrain/splinedrivetrain.cc
index cdedce0..fd0ad6e 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.cc
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.cc
@@ -117,7 +117,8 @@
     ::Eigen::Matrix<double, 2, 5> K =
         current_trajectory().GainForDistance(current_distance);
     ::Eigen::Matrix<double, 5, 1> goal_state = CurrentGoalState();
-    if (current_trajectory().drive_spline_backwards()) {
+    const bool backwards = current_trajectory().drive_spline_backwards();
+    if (backwards) {
       ::Eigen::Matrix<double, 2, 1> swapU(U_ff(1, 0), U_ff(0, 0));
       U_ff = -swapU;
       goal_state(2, 0) += M_PI;
@@ -128,13 +129,19 @@
     }
     const Eigen::Matrix<double, 5, 1> relative_goal =
         current_trajectory().StateToPathRelativeState(current_distance,
-                                                      goal_state);
+                                                      goal_state, backwards);
     const Eigen::Matrix<double, 5, 1> relative_state =
-        current_trajectory().StateToPathRelativeState(current_distance, state);
+        current_trajectory().StateToPathRelativeState(current_distance, state,
+                                                      backwards);
     Eigen::Matrix<double, 5, 1> state_error = relative_goal - relative_state;
     state_error(2, 0) = ::aos::math::NormalizeAngle(state_error(2, 0));
     ::Eigen::Matrix<double, 2, 1> U_fb = K * state_error;
 
+    if (backwards) {
+      Eigen::Matrix<double, 2, 1> swapU(U_fb(1), U_fb(0));
+      U_fb = -swapU;
+    }
+
     ::Eigen::Matrix<double, 2, 1> xv_state = current_xva_.block<2, 1>(0, 0);
     next_xva_ = current_trajectory().GetNextXVA(dt_config_.dt, &xv_state);
     next_U_ = U_ff + U_fb - voltage_error;
@@ -184,11 +191,17 @@
     ::Eigen::Matrix<double, 5, 1> goal_state = CurrentGoalState();
     trajectory_logging_builder.add_x(goal_state(0));
     trajectory_logging_builder.add_y(goal_state(1));
-    trajectory_logging_builder.add_theta(::aos::math::NormalizeAngle(
-        goal_state(2) +
-        (current_trajectory().drive_spline_backwards() ? M_PI : 0.0)));
-    trajectory_logging_builder.add_left_velocity(goal_state(3));
-    trajectory_logging_builder.add_right_velocity(goal_state(4));
+    if (current_trajectory().drive_spline_backwards()) {
+      trajectory_logging_builder.add_left_velocity(-goal_state(4));
+      trajectory_logging_builder.add_right_velocity(-goal_state(3));
+      trajectory_logging_builder.add_theta(
+          ::aos::math::NormalizeAngle(goal_state(2) + M_PI));
+    } else {
+      trajectory_logging_builder.add_theta(
+          ::aos::math::NormalizeAngle(goal_state(2)));
+      trajectory_logging_builder.add_left_velocity(goal_state(3));
+      trajectory_logging_builder.add_right_velocity(goal_state(4));
+    }
   }
   trajectory_logging_builder.add_is_executing(!IsAtEnd() &&
                                               executing_spline_);