Add auto cancel button
Change-Id: I3f026dce7478edfdb71bde9683c42f204c765e58
diff --git a/aos/input/action_joystick_input.cc b/aos/input/action_joystick_input.cc
index 061f4dc..6a38967 100644
--- a/aos/input/action_joystick_input.cc
+++ b/aos/input/action_joystick_input.cc
@@ -23,7 +23,8 @@
}
}
- if (!auto_running_ || (run_teleop_in_auto_ && !action_queue_.Running())) {
+ if (!auto_running_ ||
+ (input_config_.run_teleop_in_auto && !action_queue_.Running())) {
if (auto_was_running_) {
AutoEnded();
auto_was_running_ = false;
@@ -36,6 +37,11 @@
HandleTeleop(data);
}
+ if (auto_action_running_ &&
+ data.IsPressed(input_config_.cancel_auto_button)) {
+ StopAuto();
+ }
+
// Process pending actions.
action_queue_.Tick();
was_running_ = action_queue_.Running();
@@ -45,11 +51,13 @@
LOG(INFO, "Starting auto mode\n");
action_queue_.EnqueueAction(
::frc971::autonomous::MakeAutonomousAction(GetAutonomousMode()));
+ auto_action_running_ = true;
}
void ActionJoystickInput::StopAuto() {
LOG(INFO, "Stopping auto mode\n");
action_queue_.CancelAllActions();
+ auto_action_running_ = false;
}
} // namespace input
diff --git a/aos/input/action_joystick_input.h b/aos/input/action_joystick_input.h
index dca4d39..bb4e563 100644
--- a/aos/input/action_joystick_input.h
+++ b/aos/input/action_joystick_input.h
@@ -14,21 +14,29 @@
// Turns out we do the same thing every year, so let's stop copying it.
class ActionJoystickInput : public ::aos::input::JoystickInput {
public:
+ // Configuration parameters that don't really belong in the DrivetrainConfig.
+ struct InputConfig {
+ // If true, we will run teleop during auto mode after auto mode ends. This
+ // is to support the 2019 sandstorm mode. Auto will run, and then when the
+ // action ends (either when it's done, or when the driver triggers it to
+ // finish early), we will run teleop regardless of the mode.
+ bool run_teleop_in_auto = false;
+ // A button, for use with the run_teleop_in_auto, that will cancel the auto
+ // mode, and if run_telop_in_auto is specified, resume teloperation.
+ const driver_station::ButtonLocation cancel_auto_button;
+ };
ActionJoystickInput(
::aos::EventLoop *event_loop,
- const ::frc971::control_loops::drivetrain::DrivetrainConfig<double>
- &dt_config)
+ const ::frc971::control_loops::drivetrain::DrivetrainConfig<double> &
+ dt_config,
+ const InputConfig &input_config)
: ::aos::input::JoystickInput(event_loop),
+ input_config_(input_config),
drivetrain_input_reader_(DrivetrainInputReader::Make(
DrivetrainInputReader::InputType::kPistol, dt_config)) {}
virtual ~ActionJoystickInput() {}
- protected:
- void set_run_teleop_in_auto(bool run_teleop_in_auto) {
- run_teleop_in_auto_ = run_teleop_in_auto;
- }
-
private:
// Handles anything that needs to be cleaned up when the auto action exits.
virtual void AutoEnded() {}
@@ -46,19 +54,16 @@
// True if the internal state machine thinks auto is running right now.
bool auto_running_ = false;
+ // True if we think that the auto *action* is running right now.
+ bool auto_action_running_ = false;
// True if an action was running last cycle.
bool was_running_ = false;
- // If true, we will run teleop during auto mode after auto mode ends. This is
- // to support the 2019 sandstorm mode. Auto will run, and then when the
- // action ends (either when it's done, or when the driver triggers it to
- // finish early), we will run teleop regardless of the mode.
- bool run_teleop_in_auto_ = false;
+ const InputConfig input_config_;
// Bool to track if auto was running the last cycle through. This lets us
// call AutoEnded when the auto mode function stops.
bool auto_was_running_ = false;
-
::std::unique_ptr<DrivetrainInputReader> drivetrain_input_reader_;
::aos::common::actions::ActionQueue action_queue_;
};
diff --git a/y2019/joystick_reader.cc b/y2019/joystick_reader.cc
index a82079b..e040ba6 100644
--- a/y2019/joystick_reader.cc
+++ b/y2019/joystick_reader.cc
@@ -78,6 +78,8 @@
const ButtonLocation kPanelHPIntakeBackward(5, 5);
const ButtonLocation kRelease(2, 4);
+// Reuse quickturn for the cancel button.
+const ButtonLocation kCancelAutoMode(2, 3);
const ButtonLocation kReleaseButtonBoard(3, 4);
const ButtonLocation kResetLocalizerLeftForwards(3, 10);
const ButtonLocation kResetLocalizerLeftBackwards(3, 9);
@@ -131,10 +133,9 @@
Reader(::aos::EventLoop *event_loop)
: ::aos::input::ActionJoystickInput(
event_loop,
- ::y2019::control_loops::drivetrain::GetDrivetrainConfig()) {
- // Set teleop to immediately resume after auto ends for sandstorm mode.
- set_run_teleop_in_auto(true);
-
+ ::y2019::control_loops::drivetrain::GetDrivetrainConfig(),
+ {.run_teleop_in_auto = true,
+ .cancel_auto_button = kCancelAutoMode}) {
const uint16_t team = ::aos::network::GetTeamNumber();
superstructure_queue.goal.FetchLatest();
if (superstructure_queue.goal.get()) {