Convert joystick readers to ActionJoystickInput
This removes most direct users of the autonomous queues. Convert the
rest over to EventLoops.
Change-Id: I47ac7d5583d597cc9244573d810d512210d1e111
diff --git a/aos/input/action_joystick_input.h b/aos/input/action_joystick_input.h
index 0160468..17f1363 100644
--- a/aos/input/action_joystick_input.h
+++ b/aos/input/action_joystick_input.h
@@ -35,23 +35,51 @@
input_config_(input_config),
drivetrain_input_reader_(
DrivetrainInputReader::Make(input_type, dt_config)),
+ dt_config_(dt_config),
autonomous_action_factory_(
- ::frc971::autonomous::BaseAutonomousActor::MakeFactory(
- event_loop)) {}
+ ::frc971::autonomous::BaseAutonomousActor::MakeFactory(event_loop)),
+ autonomous_mode_fetcher_(
+ event_loop->MakeFetcher<::frc971::autonomous::AutonomousMode>(
+ ".frc971.autonomous.auto_mode")) {}
virtual ~ActionJoystickInput() {}
protected:
bool was_running_action() { return was_running_; }
+ // Returns true if an action is running.
bool ActionRunning() { return action_queue_.Running(); }
+ // Cancels all actions.
void CancelAllActions() { action_queue_.CancelAllActions(); }
+ // Cancels the current action.
void CancelCurrentAction() { action_queue_.CancelCurrentAction(); }
+ // Enqueues an action.
void EnqueueAction(::std::unique_ptr<::aos::common::actions::Action> action) {
action_queue_.EnqueueAction(::std::move(action));
}
+ // Returns the current robot velocity.
+ double robot_velocity() const {
+ return drivetrain_input_reader_->robot_velocity();
+ }
+
+ // Returns the drivetrain config.
+ const ::frc971::control_loops::drivetrain::DrivetrainConfig<double>
+ dt_config() const {
+ return dt_config_;
+ }
+
+ // Sets the vision align function. This function runs before the normal
+ // drivetrain code runs. If it returns true, we are in vision align mode and
+ // no drivetain code is run. If it returns false, the vision align function
+ // is assumed to be disabled and normal drive code is run.
+ void set_vision_align_fn(
+ ::std::function<bool(const ::aos::input::driver_station::Data &data)>
+ vision_align_fn) {
+ drivetrain_input_reader_->set_vision_align_fn(vision_align_fn);
+ }
+
private:
// Handles anything that needs to be cleaned up when the auto action exits.
virtual void AutoEnded() {}
@@ -65,7 +93,14 @@
// Returns the current autonomous mode which has been selected by robot
// inputs.
- virtual uint32_t GetAutonomousMode() { return 0; }
+ virtual uint32_t GetAutonomousMode() {
+ autonomous_mode_fetcher_.Fetch();
+ if (autonomous_mode_fetcher_.get() == nullptr) {
+ LOG(WARNING, "no auto mode values\n");
+ return 0;
+ }
+ return autonomous_mode_fetcher_->mode;
+ }
// True if the internal state machine thinks auto is running right now.
bool auto_running_ = false;
@@ -82,7 +117,12 @@
::std::unique_ptr<DrivetrainInputReader> drivetrain_input_reader_;
::aos::common::actions::ActionQueue action_queue_;
+ const ::frc971::control_loops::drivetrain::DrivetrainConfig<double>
+ dt_config_;
+
::frc971::autonomous::BaseAutonomousActor::Factory autonomous_action_factory_;
+
+ ::aos::Fetcher<::frc971::autonomous::AutonomousMode> autonomous_mode_fetcher_;
};
} // namespace input
diff --git a/aos/input/drivetrain_input.cc b/aos/input/drivetrain_input.cc
index 631f97d..8c0fa6b 100644
--- a/aos/input/drivetrain_input.cc
+++ b/aos/input/drivetrain_input.cc
@@ -37,6 +37,14 @@
robot_velocity_ = drivetrain_queue.status->robot_speed;
}
+ // If we have a vision align function, and it is in control, don't run the
+ // normal driving code.
+ if (vision_align_fn_) {
+ if (vision_align_fn_(data)) {
+ return;
+ }
+ }
+
bool is_control_loop_driving = false;
bool is_line_following = false;
diff --git a/aos/input/drivetrain_input.h b/aos/input/drivetrain_input.h
index ec52368..2b7d73d 100644
--- a/aos/input/drivetrain_input.h
+++ b/aos/input/drivetrain_input.h
@@ -85,6 +85,12 @@
// Returns the current robot velocity in m/s.
double robot_velocity() const { return robot_velocity_; }
+ void set_vision_align_fn(
+ ::std::function<bool(const ::aos::input::driver_station::Data &data)>
+ vision_align_fn) {
+ vision_align_fn_ = vision_align_fn;
+ }
+
protected:
const driver_station::JoystickAxis wheel_;
const driver_station::JoystickAxis throttle_;
@@ -122,6 +128,9 @@
// The scale for the joysticks for closed loop mode converting
// joysticks to meters displacement on the two wheels.
double wheel_multiplier_ = 0.5;
+
+ ::std::function<bool(const ::aos::input::driver_station::Data &data)>
+ vision_align_fn_;
};
// Implements DrivetrainInputReader for the original steering wheel.