commit e39cbcfd448414ac015f61fc7c5fe8fbd7dfce21
author James Kuszmaul <jabukuszmaul@gmail.com> Wed Feb 27 20:48:34 2019 -0800
committer James Kuszmaul <jabukuszmaul@gmail.com> Sat Mar 02 21:15:50 2019 -0800
tree 4424b699dde5397de02f13314970b0ad832cc6ba
parent 409993acf65b43f8960a4ac01d7923edce872fcc

Add basic line-following drivetrain

Works by drawing a Nth degree polynomial to guide us to the target and
guiding a lateral controller to follow it given a speed provided by the
driver.

Change-Id: I94fd31666769ca398b66f04665f39e01cee00627

frc971/control_loops/drivetrain/drivetrain.cc

diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 19e604d..2bfaaf5 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -41,6 +41,7 @@
       dt_openloop_(dt_config_, &kf_),
       dt_closedloop_(dt_config_, &kf_, localizer_),
       dt_spline_(dt_config_),
+      dt_line_follow_(dt_config_),
       down_estimator_(MakeDownEstimatorLoop()),
       left_gear_(dt_config_.default_high_gear ? Gear::HIGH : Gear::LOW),
       right_gear_(dt_config_.default_high_gear ? Gear::HIGH : Gear::LOW),
@@ -247,17 +248,26 @@
     dt_closedloop_.SetGoal(*goal);
     dt_openloop_.SetGoal(*goal);
     dt_spline_.SetGoal(*goal);
+    // TODO(james): Use a goal other than zero (which is what Pose default
+    // constructs to). Need to query the EKF in some way to determine what the
+    // logical target is.
+    const LineFollowDrivetrain::Pose goal_pose;
+    dt_line_follow_.SetGoal(*goal, goal_pose);
   }
 
   dt_openloop_.Update(robot_state().voltage_battery);
 
   dt_closedloop_.Update(output != NULL && controller_type == 1);
 
-  dt_spline_.Update(output != NULL && controller_type == 2,
-                    (Eigen::Matrix<double, 5, 1>() << localizer_->x(),
-                     localizer_->y(), localizer_->theta(),
-                     localizer_->left_velocity(), localizer_->right_velocity())
-                        .finished());
+  const Eigen::Matrix<double, 5, 1> trajectory_state =
+      (Eigen::Matrix<double, 5, 1>() << localizer_->x(), localizer_->y(),
+       localizer_->theta(), localizer_->left_velocity(),
+       localizer_->right_velocity())
+          .finished();
+
+  dt_spline_.Update(output != NULL && controller_type == 2, trajectory_state);
+
+  dt_line_follow_.Update(trajectory_state);
 
   switch (controller_type) {
     case 0:
@@ -269,6 +279,9 @@
     case 2:
       dt_spline_.SetOutput(output);
       break;
+    case 3:
+      dt_line_follow_.SetOutput(output);
+      break;
   }
 
   // The output should now contain the shift request.
@@ -310,6 +323,7 @@
     dt_openloop_.PopulateStatus(status);
     dt_closedloop_.PopulateStatus(status);
     dt_spline_.PopulateStatus(status);
+    dt_line_follow_.PopulateStatus(status);
   }
 
   double left_voltage = 0.0;