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/drivetrain.cc

diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index ec9faaa..168f81d 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -337,6 +337,8 @@
 
   dt_closedloop_.Update(output != NULL && controller_type == 1);
 
+  dt_spline_.Update(output != NULL && controller_type == 2);
+
   switch (controller_type) {
     case 0:
       dt_openloop_.SetOutput(output);

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_;
     }
   }
 }

frc971/control_loops/drivetrain/splinedrivetrain.h

diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.h b/frc971/control_loops/drivetrain/splinedrivetrain.h
index b311e14..4bb7ac9 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.h
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.h
@@ -1,6 +1,8 @@
 #ifndef FRC971_CONTROL_LOOPS_DRIVETRAIN_SPLINEDRIVETRAIN_H_
 #define FRC971_CONTROL_LOOPS_DRIVETRAIN_SPLINEDRIVETRAIN_H_
 
+#include "Eigen/Dense"
+
 #include "frc971/control_loops/drivetrain/distance_spline.h"
 #include "frc971/control_loops/drivetrain/drivetrain.q.h"
 #include "frc971/control_loops/drivetrain/drivetrain_config.h"
@@ -17,6 +19,8 @@
 
   void SetGoal(const ::frc971::control_loops::DrivetrainQueue::Goal &goal);
 
+  void Update(bool enabled);
+
   void SetOutput(
       ::frc971::control_loops::DrivetrainQueue::Output *output);
   // TODO(alex): What status do we need?
@@ -29,8 +33,8 @@
   uint32_t current_spline_idx_;  // Current excecuting spline.
   ::std::unique_ptr<DistanceSpline> distance_spline_;
   ::std::unique_ptr<Trajectory> current_trajectory_;
-  ::std::vector<::Eigen::Matrix<double, 3, 1>> current_xva_plan_;
-  size_t current_xva_idx_;
+  ::Eigen::Matrix<double, 3, 1> current_xva_, next_xva_;
+  ::Eigen::Matrix<double, 2, 1> current_state_;
 };
 
 }  // namespace drivetrain