Finish the place arm spline if pressed
This makes it so we don't bobble the cone when trying to score and
someone lets go of spit early.
Change-Id: Ib7375d78bd941f1229ee76e825540139b54df013
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/y2023/joystick_reader.cc b/y2023/joystick_reader.cc
index 8d04056..efd7537 100644
--- a/y2023/joystick_reader.cc
+++ b/y2023/joystick_reader.cc
@@ -382,6 +382,8 @@
GamePiece current_game_piece_ = GamePiece::CONE_UP;
+ bool has_scored_ = false;
+
void HandleTeleop(
const ::frc971::input::driver_station::Data &data) override {
superstructure_status_fetcher_.Fetch();
@@ -445,6 +447,7 @@
// Ok, no active setpoint. Search for the right one.
if (current_setpoint_ == nullptr) {
+ has_scored_ = false;
const Side current_side =
data.IsPressed(kBack) ? Side::BACK : Side::FRONT;
// Search for the active setpoint.
@@ -485,13 +488,16 @@
// spit.
if (std::abs(score_wrist_goal.value() -
superstructure_status_fetcher_->wrist()->goal_position()) <
- 0.1) {
+ 0.1 ||
+ has_scored_) {
if (place_index.has_value()) {
arm_goal_position_ = place_index.value();
- if (arm_goal_position_ ==
- superstructure_status_fetcher_->arm()->current_node() &&
- superstructure_status_fetcher_->arm()->path_distance_to_go() <
- 0.01) {
+ if ((arm_goal_position_ ==
+ superstructure_status_fetcher_->arm()->current_node() &&
+ superstructure_status_fetcher_->arm()->path_distance_to_go() <
+ 0.01) ||
+ has_scored_) {
+ has_scored_ = true;
roller_goal = RollerGoal::SPIT;
}
} else {