commit a71badbe4d2fbeb642db20920bf72d78f64e9a20
author Alex Perry <alex.perry96@gmail.com> Wed Feb 06 19:40:41 2019 -0800
committer Alex Perry <alex.perry96@gmail.com> Sun Feb 17 13:09:57 2019 -0800
tree 678639e71a9cde4fb333fca336ddb8223bb260cc
parent 731b4600a3bfbc7025d2fae422116fcdbe94777d

Use iterative XVA in spline drivetrain.

Change-Id: Ib027c99b5fdba7de98bb825d8cd64d1abd7a96a9

frc971/control_loops/drivetrain/splinedrivetrain.cc

diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.cc b/frc971/control_loops/drivetrain/splinedrivetrain.cc
index 11a4f69..0d6b6ae 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.cc
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.cc
@@ -63,7 +63,15 @@
     }
 
     current_trajectory_->Plan();
-    current_xva_plan_ = current_trajectory_->PlanXVA(dt_config_.dt);
+    current_xva_ = current_trajectory_->FFAcceleration(0);
+    current_xva_(1) = 0.0;
+  }
+}
+
+void SplineDrivetrain::Update(bool enable) {
+  if (enable && current_trajectory_ &&
+      !current_trajectory_->is_at_end(current_state_)) {
+    next_xva_ = current_trajectory_->GetNextXVA(dt_config_.dt, &current_state_);
   }
 }
 
@@ -73,14 +81,17 @@
   if (!output) {
     return;
   }
+  if (!current_trajectory_) {
+    return;
+  }
   if (current_spline_handle_ == current_spline_idx_) {
-    if (current_xva_idx_ < current_xva_plan_.size()) {
-      double current_distance = current_xva_plan_[current_xva_idx_](0);
+    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);
-      ++current_xva_idx_;
+      current_xva_ = next_xva_;
     }
   }
 }