commit e32eabc9a3c6e198b9bff7d0176efbe7c53acce9
author Alex Perry <alex.perry96@gmail.com> Fri Feb 08 19:51:19 2019 -0800
committer Alex Perry <alex.perry96@gmail.com> Sun Feb 17 15:46:28 2019 -0800
tree e29418afe11d48580c13fd5ef718917933c753bd
parent f11ef5c5cf88a2a94a77e2b48481aa32a0e7803e

Add feedback to the spline drivetrain.

Change-Id: I11bc55bd6d8ca866dbd85d2b1efcd8e278f8021f

frc971/control_loops/drivetrain/splinedrivetrain.cc

diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.cc b/frc971/control_loops/drivetrain/splinedrivetrain.cc
index 0d6b6ae..b45f218 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.cc
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.cc
@@ -1,5 +1,7 @@
 #include "frc971/control_loops/drivetrain/splinedrivetrain.h"
 
+#include <iostream>
+
 #include "Eigen/Dense"
 
 #include "frc971/control_loops/drivetrain/drivetrain.q.h"
@@ -12,7 +14,17 @@
 namespace drivetrain {
 
 SplineDrivetrain::SplineDrivetrain(const DrivetrainConfig<double> &dt_config)
-    : dt_config_(dt_config) {}
+    : dt_config_(dt_config),
+      current_state_(::Eigen::Matrix<double, 2, 1>::Zero()) {}
+
+void SplineDrivetrain::ScaleCapU(Eigen::Matrix<double, 2, 1> *U) {
+  bool output_was_capped = ::std::abs((*U)(0, 0)) > 12.0 ||
+                       ::std::abs((*U)(1, 0)) > 12.0;
+
+  if (output_was_capped) {
+    *U *= 12.0 / U->lpNorm<Eigen::Infinity>();
+  }
+}
 
 // TODO(alex): put in another thread to avoid malloc in RT.
 void SplineDrivetrain::SetGoal(
@@ -65,17 +77,36 @@
     current_trajectory_->Plan();
     current_xva_ = current_trajectory_->FFAcceleration(0);
     current_xva_(1) = 0.0;
+    current_state_ = ::Eigen::Matrix<double, 2, 1>::Zero();
   }
 }
 
-void SplineDrivetrain::Update(bool enable) {
-  if (enable && current_trajectory_ &&
-      !current_trajectory_->is_at_end(current_state_)) {
+// TODO(alex): Hold position when done following the spline.
+// TODO(Austin): Compensate for voltage error.
+void SplineDrivetrain::Update(bool enable,
+                              const ::Eigen::Matrix<double, 5, 1> &state) {
+  enable_ = enable;
+  if (enable && current_trajectory_) {
+    ::Eigen::Matrix<double, 2, 1> U_ff = ::Eigen::Matrix<double, 2, 1>::Zero();
+    if (!current_trajectory_->is_at_end(current_state_)) {
+      // TODO(alex): It takes about a cycle for the outputs to propagate to the
+      // motors. Consider delaying the output by a cycle.
+      U_ff = current_trajectory_->FFVoltage(current_xva_(0));
+    }
+    ::Eigen::Matrix<double, 2, 5> K =
+        current_trajectory_->KForState(state, dt_config_.dt, Q, R);
+    ::Eigen::Matrix<double, 5, 1> goal_state =
+        current_trajectory_->GoalState(current_xva_(0), current_xva_(1));
+    ::Eigen::Matrix<double, 5, 1> state_error = goal_state - state;
+    ::Eigen::Matrix<double, 2, 1> U_fb = K * state_error;
+    next_U_ = U_ff + U_fb;
+    uncapped_U_ = next_U_;
+    ScaleCapU(&next_U_);
+
     next_xva_ = current_trajectory_->GetNextXVA(dt_config_.dt, &current_state_);
   }
 }
 
-// TODO(alex): Handle drift.
 void SplineDrivetrain::SetOutput(
     ::frc971::control_loops::DrivetrainQueue::Output *output) {
   if (!output) {
@@ -86,16 +117,22 @@
   }
   if (current_spline_handle_ == current_spline_idx_) {
     if (!current_trajectory_->is_at_end(current_state_)) {
-      double current_distance = current_xva_(0);
-      ::Eigen::Matrix<double, 2, 1> FFVoltage =
-          current_trajectory_->FFVoltage(current_distance);
-      output->left_voltage = FFVoltage(0);
-      output->right_voltage = FFVoltage(1);
+      output->left_voltage = next_U_(0);
+      output->right_voltage = next_U_(1);
       current_xva_ = next_xva_;
     }
   }
 }
 
+void SplineDrivetrain::PopulateStatus(
+    ::frc971::control_loops::DrivetrainQueue::Status *status) const {
+  if (status && enable_) {
+    status->uncapped_left_voltage = uncapped_U_(0);
+    status->uncapped_right_voltage = uncapped_U_(1);
+    status->robot_speed = current_xva_(1);
+  }
+}
+
 }  // namespace drivetrain
 }  // namespace control_loops
 }  // namespace frc971