Add interface for target selector
This adds an interface to provide something to choose a target to guide
into based on the current robot state.
This does not actually add a real implementation for target choosing.
Change-Id: I73bfde640dbe0851847f9f77710614607bad96ea
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 2bfaaf5..b6b8b5d 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -41,7 +41,7 @@
dt_openloop_(dt_config_, &kf_),
dt_closedloop_(dt_config_, &kf_, localizer_),
dt_spline_(dt_config_),
- dt_line_follow_(dt_config_),
+ dt_line_follow_(dt_config_, localizer->target_selector()),
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),
@@ -248,11 +248,7 @@
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_line_follow_.SetGoal(*goal);
}
dt_openloop_.Update(robot_state().voltage_battery);
@@ -280,7 +276,11 @@
dt_spline_.SetOutput(output);
break;
case 3:
- dt_line_follow_.SetOutput(output);
+ if (!dt_line_follow_.SetOutput(output)) {
+ // If the line follow drivetrain was unable to execute (generally due to
+ // not having a target), execute the regular teleop drivetrain.
+ dt_openloop_.SetOutput(output);
+ }
break;
}