Make target selector use throttle instead of velocity
Change-Id: I7f3c1cc593854f957ffe7f9c29e6dd5769ab0f69
diff --git a/frc971/control_loops/drivetrain/line_follow_drivetrain.cc b/frc971/control_loops/drivetrain/line_follow_drivetrain.cc
index d085d17..b5417af 100644
--- a/frc971/control_loops/drivetrain/line_follow_drivetrain.cc
+++ b/frc971/control_loops/drivetrain/line_follow_drivetrain.cc
@@ -182,7 +182,8 @@
// Because we assume the target selector may have some internal state (e.g.,
// not confirming a target until some time as passed), we should call
// UpdateSelection every time.
- bool new_target = target_selector_->UpdateSelection(abs_state);
+ bool new_target =
+ target_selector_->UpdateSelection(abs_state, goal_velocity_);
if (freeze_target_) {
// When freezing the target, only make changes if we didn't have a good
// target before.
diff --git a/frc971/control_loops/drivetrain/localizer.h b/frc971/control_loops/drivetrain/localizer.h
index f0d81cb..f0685dd 100644
--- a/frc971/control_loops/drivetrain/localizer.h
+++ b/frc971/control_loops/drivetrain/localizer.h
@@ -16,9 +16,12 @@
// Take the state as [x, y, theta, left_vel, right_vel]
// If unable to determine what target to go for, returns false. If a viable
// target is selected, then returns true and sets target_pose.
+ // command_speed is the goal speed of the current drivetrain, generally
+ // generated from the throttle and meant to signify driver intent.
// TODO(james): Some implementations may also want a drivetrain goal so that
// driver intent can be divined more directly.
- virtual bool UpdateSelection(const ::Eigen::Matrix<double, 5, 1> &state) = 0;
+ virtual bool UpdateSelection(const ::Eigen::Matrix<double, 5, 1> &state,
+ double command_speed) = 0;
// Gets the current target pose. Should only be called if UpdateSelection has
// returned true.
virtual TypedPose<double> TargetPose() const = 0;
@@ -59,7 +62,7 @@
// manually set the target selector state.
class TrivialTargetSelector : public TargetSelectorInterface {
public:
- bool UpdateSelection(const ::Eigen::Matrix<double, 5, 1> &) override {
+ bool UpdateSelection(const ::Eigen::Matrix<double, 5, 1> &, double) override {
return has_target_;
}
TypedPose<double> TargetPose() const override { return pose_; }