Created a spline drivetrain.
Change-Id: Icfbfcec930d17d1a2ca075fc2eb67594d35113df
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index a50b429..ec9faaa 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -38,6 +38,7 @@
kf_(dt_config_.make_kf_drivetrain_loop()),
dt_openloop_(dt_config_, &kf_),
dt_closedloop_(dt_config_, &kf_, &integrated_kf_heading_),
+ dt_spline_(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),
@@ -329,6 +330,7 @@
dt_closedloop_.SetGoal(*goal);
dt_openloop_.SetGoal(*goal);
+ dt_spline_.SetGoal(*goal);
}
dt_openloop_.Update(robot_state().voltage_battery);
@@ -342,6 +344,9 @@
case 1:
dt_closedloop_.SetOutput(output);
break;
+ case 2:
+ dt_spline_.SetOutput(output);
+ break;
}
// The output should now contain the shift request.