commit c73bb22a8045b51ee4db0665641588de15647a8b
author James Kuszmaul <jabukuszmaul@gmail.com> Sun Apr 07 12:15:35 2019 -0700
committer James Kuszmaul <jabukuszmaul@gmail.com> Wed Apr 10 20:18:12 2019 -0700
tree 5c347abbe137728c094b629497b22b714272ac89
parent da049002f96104ead2ef55f3925b35da68f1ee03

Add ability to drive splines backwards

Change-Id: Ib34366ed155625fba56c4f72b9adce0615c6f12d

frc971/control_loops/drivetrain/splinedrivetrain.cc

diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.cc b/frc971/control_loops/drivetrain/splinedrivetrain.cc
index a37ca0e..18c8381 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.cc
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.cc
@@ -3,6 +3,7 @@
 #include "Eigen/Dense"
 
 #include "aos/init.h"
+#include "aos/util/math.h"
 #include "frc971/control_loops/drivetrain/drivetrain.q.h"
 #include "frc971/control_loops/drivetrain/drivetrain_config.h"
 
@@ -99,6 +100,7 @@
 
   ::aos::Mutex::State mutex_state = mutex_.TryLock();
   if (mutex_state == ::aos::Mutex::State::kLocked) {
+    drive_spline_backwards_ = goal_.drive_spline_backwards;
     if (goal.spline.spline_idx && future_spline_idx_ != goal.spline.spline_idx) {
       goal_ = goal;
       new_goal_.Broadcast();
@@ -143,7 +145,17 @@
     ::Eigen::Matrix<double, 2, 5> K =
         current_trajectory_->KForState(state, dt_config_.dt, Q, R);
     ::Eigen::Matrix<double, 5, 1> goal_state = CurrentGoalState();
+    if (drive_spline_backwards_) {
+      ::Eigen::Matrix<double, 2, 1> swapU(U_ff(1, 0), U_ff(0, 0));
+      U_ff = -swapU;
+      goal_state(2, 0) += M_PI;
+      double left_goal = goal_state(3, 0);
+      double right_goal = goal_state(4, 0);
+      goal_state(3, 0) = -right_goal;
+      goal_state(4, 0) = -left_goal;
+    }
     ::Eigen::Matrix<double, 5, 1> state_error = goal_state - state;
+    state_error(2, 0) = ::aos::math::NormalizeAngle(state_error(2, 0));
     ::Eigen::Matrix<double, 2, 1> U_fb = K * state_error;
 
     ::Eigen::Matrix<double, 2, 1> xv_state = current_xva_.block<2,1>(0,0);