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.
diff --git a/frc971/autonomous/auto.q b/frc971/autonomous/auto.q
index a72514c..f107c90 100644
--- a/frc971/autonomous/auto.q
+++ b/frc971/autonomous/auto.q
@@ -2,20 +2,12 @@
import "aos/actions/actions.q";
-message AutoControl {
- // True if auto mode should be running, false otherwise.
- bool run_auto;
-};
-
-queue AutoControl autonomous;
-
+// Published on ".frc971.autonomous.auto_mode"
message AutonomousMode {
// Mode read from the mode setting sensors.
int32_t mode;
};
-queue AutonomousMode auto_mode;
-
struct AutonomousActionParams {
// The mode from the sensors when auto starts.
int32_t mode;
diff --git a/y2014/joystick_reader.cc b/y2014/joystick_reader.cc
index 2b32d2d..6680e8c 100644
--- a/y2014/joystick_reader.cc
+++ b/y2014/joystick_reader.cc
@@ -401,16 +401,6 @@
}
private:
- void StartAuto() {
- LOG(INFO, "Starting auto mode\n");
- ::frc971::autonomous::autonomous.MakeWithBuilder().run_auto(true).Send();
- }
-
- void StopAuto() {
- LOG(INFO, "Stopping auto mode\n");
- ::frc971::autonomous::autonomous.MakeWithBuilder().run_auto(false).Send();
- }
-
double shot_power_;
double goal_angle_;
double separation_angle_, shot_separation_angle_;
diff --git a/y2016/BUILD b/y2016/BUILD
index 1a00044..1f426c6 100644
--- a/y2016/BUILD
+++ b/y2016/BUILD
@@ -30,7 +30,7 @@
":constants",
"//aos:init",
"//aos/actions:action_lib",
- "//aos/input:joystick_input",
+ "//aos/input:action_joystick_input",
"//aos/logging",
"//aos/time",
"//aos/util:log_interval",
diff --git a/y2016/dashboard/dashboard.cc b/y2016/dashboard/dashboard.cc
index af6019c..98fcd7b 100644
--- a/y2016/dashboard/dashboard.cc
+++ b/y2016/dashboard/dashboard.cc
@@ -18,9 +18,7 @@
#include "aos/seasocks/seasocks_logger.h"
#include "aos/time/time.h"
#include "aos/util/phased_loop.h"
-
#include "frc971/autonomous/auto.q.h"
-
#include "y2016/control_loops/superstructure/superstructure.q.h"
#include "y2016/queues/ball_detector.q.h"
#include "y2016/vision/vision.q.h"
@@ -57,6 +55,9 @@
ball_detector_fetcher_(
event_loop->MakeFetcher<::y2016::sensors::BallDetector>(
".y2016.sensors.ball_detector")),
+ autonomous_mode_fetcher_(
+ event_loop->MakeFetcher<::frc971::autonomous::AutonomousMode>(
+ ".frc971.autonomous.auto_mode")),
cur_raw_data_("no data"),
sample_id_(0),
measure_index_(0),
@@ -88,7 +89,7 @@
// gone wrong with reading the auto queue.
int auto_mode_indicator = -1;
- ::frc971::autonomous::auto_mode.FetchLatest();
+ autonomous_mode_fetcher_.Fetch();
::y2016::control_loops::superstructure_queue.status.FetchLatest();
ball_detector_fetcher_.Fetch();
vision_status_fetcher_.Fetch();
@@ -129,8 +130,8 @@
}
}
- if (::frc971::autonomous::auto_mode.get()) {
- auto_mode_indicator = ::frc971::autonomous::auto_mode->mode;
+ if (autonomous_mode_fetcher_.get()) {
+ auto_mode_indicator = autonomous_mode_fetcher_->mode;
}
AddPoint("big indicator", big_indicator);
diff --git a/y2016/dashboard/dashboard.h b/y2016/dashboard/dashboard.h
index 665d656..2e01f40 100644
--- a/y2016/dashboard/dashboard.h
+++ b/y2016/dashboard/dashboard.h
@@ -17,6 +17,7 @@
#include "aos/events/event-loop.h"
#include "aos/mutex/mutex.h"
#include "aos/time/time.h"
+#include "frc971/autonomous/auto.q.h"
#include "y2016/queues/ball_detector.q.h"
#include "y2016/vision/vision.q.h"
@@ -66,6 +67,7 @@
::aos::Fetcher<::y2016::vision::VisionStatus> vision_status_fetcher_;
::aos::Fetcher<::y2016::sensors::BallDetector> ball_detector_fetcher_;
+ ::aos::Fetcher<::frc971::autonomous::AutonomousMode> autonomous_mode_fetcher_;
// Storage vector that is written and overwritten with data in a FIFO fashion.
::std::vector<SampleItem> sample_items_;
diff --git a/y2016/joystick_reader.cc b/y2016/joystick_reader.cc
index bc0e319..aec9c0c 100644
--- a/y2016/joystick_reader.cc
+++ b/y2016/joystick_reader.cc
@@ -3,13 +3,13 @@
#include <unistd.h>
#include <math.h>
+#include "aos/actions/actions.h"
#include "aos/init.h"
-#include "aos/input/joystick_input.h"
+#include "aos/input/action_joystick_input.h"
#include "aos/input/driver_station_data.h"
#include "aos/logging/logging.h"
-#include "aos/util/log_interval.h"
#include "aos/time/time.h"
-#include "aos/actions/actions.h"
+#include "aos/util/log_interval.h"
#include "frc971/autonomous/auto.q.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
@@ -69,49 +69,32 @@
const ButtonLocation kExpand(3, 6);
const ButtonLocation kWinch(3, 5);
-// TODO(austin): ActionJoystickInput
-class Reader : public ::aos::input::JoystickInput {
+class Reader : public ::aos::input::ActionJoystickInput {
public:
Reader(::aos::EventLoop *event_loop)
- : ::aos::input::JoystickInput(event_loop),
+ : ::aos::input::ActionJoystickInput(
+ event_loop, control_loops::drivetrain::GetDrivetrainConfig(),
+ ::aos::input::DrivetrainInputReader::InputType::kSteeringWheel, {}),
vision_status_fetcher_(
event_loop->MakeFetcher<::y2016::vision::VisionStatus>(
".y2016.vision.vision_status")),
ball_detector_fetcher_(
event_loop->MakeFetcher<::y2016::sensors::BallDetector>(
".y2016.sensors.ball_detector")),
- is_high_gear_(true),
intake_goal_(0.0),
shoulder_goal_(M_PI / 2.0),
wrist_goal_(0.0),
- dt_config_(control_loops::drivetrain::GetDrivetrainConfig()),
- autonomous_action_factory_(
- ::frc971::autonomous::BaseAutonomousActor::MakeFactory(event_loop)),
vision_align_action_factory_(
actors::VisionAlignActor::MakeFactory(event_loop)),
superstructure_action_factory_(
- actors::SuperstructureActor::MakeFactory(event_loop)) {}
+ actors::SuperstructureActor::MakeFactory(event_loop)) {
+ set_vision_align_fn([this](const ::aos::input::driver_station::Data &data) {
+ return VisionAlign(data);
+ });
+ }
- // TODO(austin): Move this to the ActionJoystickInput class.
- void RunIteration(const ::aos::input::driver_station::Data &data) override {
- bool last_auto_running = auto_running_;
- auto_running_ = data.GetControlBit(ControlBit::kAutonomous) &&
- data.GetControlBit(ControlBit::kEnabled);
- if (auto_running_ != last_auto_running) {
- if (auto_running_) {
- StartAuto();
- } else {
- StopAuto();
- }
- }
-
- if (!data.GetControlBit(ControlBit::kEnabled)) {
- // If we are not enabled, reset the waiting for zero bit.
- LOG(DEBUG, "Waiting for zero.\n");
- waiting_for_zero_ = true;
- is_high_gear_ = true;
- }
-
+ // Returns true when we are vision aligning
+ bool VisionAlign(const ::aos::input::driver_station::Data &data) {
vision_valid_ = false;
vision_status_fetcher_.Fetch();
@@ -122,27 +105,10 @@
last_angle_ = vision_status_fetcher_->horizontal_angle;
}
- if (!auto_running_) {
- HandleDrivetrain(data);
- HandleTeleop(data);
- }
-
- // Process any pending actions.
- action_queue_.Tick();
- was_running_ = action_queue_.Running();
- }
-
- void HandleDrivetrain(const ::aos::input::driver_station::Data &data) {
- bool is_control_loop_driving = false;
-
- const double wheel = -data.GetAxis(kSteeringWheel);
- const double throttle = -data.GetAxis(kDriveThrottle);
- drivetrain_queue.status.FetchLatest();
-
if (data.IsPressed(kVisionAlign)) {
if (vision_valid_ && !vision_action_running_) {
actors::VisionAlignActionParams params;
- action_queue_.EnqueueAction(vision_align_action_factory_.Make(params));
+ EnqueueAction(vision_align_action_factory_.Make(params));
vision_action_running_ = true;
LOG(INFO, "Starting vision align\n");
} else {
@@ -153,55 +119,29 @@
}
if (data.NegEdge(kVisionAlign)) {
- action_queue_.CancelAllActions();
+ CancelAllActions();
}
if (!data.IsPressed(kVisionAlign)) {
vision_action_running_ = false;
}
- // Don't do any normal drivetrain stuff if vision is in charge.
- if (vision_action_running_) {
- return;
- }
-
- if (data.PosEdge(kTurn1) || data.PosEdge(kTurn2)) {
- if (drivetrain_queue.status.get()) {
- left_goal_ = drivetrain_queue.status->estimated_left_position;
- right_goal_ = drivetrain_queue.status->estimated_right_position;
- }
- }
- if (data.IsPressed(kTurn1) || data.IsPressed(kTurn2)) {
- is_control_loop_driving = true;
- }
- if (!drivetrain_queue.goal.MakeWithBuilder()
- .wheel(wheel)
- .throttle(throttle)
- .highgear(is_high_gear_)
- .quickturn(data.IsPressed(kQuickTurn))
- .controller_type(is_control_loop_driving ? 1 : 0)
- .left_goal(left_goal_ - wheel * 0.5 + throttle * 0.3)
- .right_goal(right_goal_ + wheel * 0.5 + throttle * 0.3)
- .Send()) {
- LOG(WARNING, "sending stick values failed\n");
- }
-
- if (data.PosEdge(kShiftLow)) {
- is_high_gear_ = false;
- }
-
- if (data.PosEdge(kShiftHigh) || data.PosEdge(kShiftHigh2)) {
- is_high_gear_ = true;
- }
+ return vision_action_running_;
}
void HandleTeleop(const ::aos::input::driver_station::Data &data) {
+ if (!data.GetControlBit(ControlBit::kEnabled)) {
+ // If we are not enabled, reset the waiting for zero bit.
+ LOG(DEBUG, "Waiting for zero.\n");
+ waiting_for_zero_ = true;
+ }
+
float voltage_climber = 0.0;
// Default the intake to up.
intake_goal_ = constants::Values::kIntakeRange.upper - 0.04;
bool force_lights_on = false;
if (!data.GetControlBit(ControlBit::kEnabled)) {
- action_queue_.CancelAllActions();
+ CancelAllActions();
LOG(DEBUG, "Canceling\n");
}
@@ -272,8 +212,7 @@
params.delay_time = 0.7;
params.full_angle = shoulder_goal_;
params.shooter_angle = wrist_goal_;
- action_queue_.EnqueueAction(
- superstructure_action_factory_.Make(params));
+ EnqueueAction(superstructure_action_factory_.Make(params));
}
if (data.IsPressed(kWinch)) {
voltage_climber = 12.0;
@@ -285,7 +224,7 @@
}
if (data.NegEdge(kExpand) || voltage_climber > 1.0) {
is_expanding_ = false;
- action_queue_.CancelAllActions();
+ CancelAllActions();
}
bool ball_detected = false;
@@ -326,7 +265,7 @@
if (vision_action_running_ && ::std::abs(last_angle_) < 0.02 &&
::std::abs((left_goal - right_goal) -
(left_current - right_current)) /
- dt_config_.robot_radius / 2.0 <
+ dt_config().robot_radius / 2.0 <
0.02 &&
::std::abs(left_velocity - right_velocity) < 0.01) {
++ready_to_fire_;
@@ -440,42 +379,15 @@
}
private:
- void StartAuto() {
- LOG(INFO, "Starting auto mode\n");
-
- ::frc971::autonomous::AutonomousActionParams params;
- ::frc971::autonomous::auto_mode.FetchLatest();
- if (::frc971::autonomous::auto_mode.get() != nullptr) {
- params.mode = ::frc971::autonomous::auto_mode->mode;
- } else {
- LOG(WARNING, "no auto mode values\n");
- params.mode = 0;
- }
- action_queue_.EnqueueAction(autonomous_action_factory_.Make(params));
- }
-
- void StopAuto() {
- LOG(INFO, "Stopping auto mode\n");
- action_queue_.CancelAllActions();
- }
-
::aos::Fetcher<::y2016::vision::VisionStatus> vision_status_fetcher_;
::aos::Fetcher<::y2016::sensors::BallDetector> ball_detector_fetcher_;
- bool is_high_gear_;
// Whatever these are set to are our default goals to send out after zeroing.
double intake_goal_;
double shoulder_goal_;
double wrist_goal_;
double shooter_velocity_ = 0.0;
- // Turning goals
- double left_goal_;
- double right_goal_;
-
- bool was_running_ = false;
- bool auto_running_ = false;
-
bool traverse_unlatched_ = false;
bool traverse_down_ = false;
@@ -497,13 +409,8 @@
int ready_to_fire_ = 0;
- ::aos::common::actions::ActionQueue action_queue_;
-
- const ::frc971::control_loops::drivetrain::DrivetrainConfig<double> dt_config_;
-
bool is_expanding_ = false;
- ::frc971::autonomous::BaseAutonomousActor::Factory autonomous_action_factory_;
actors::VisionAlignActor::Factory vision_align_action_factory_;
actors::SuperstructureActor::Factory superstructure_action_factory_;
diff --git a/y2016/wpilib_interface.cc b/y2016/wpilib_interface.cc
index 6dd9b51..3b00248 100644
--- a/y2016/wpilib_interface.cc
+++ b/y2016/wpilib_interface.cc
@@ -152,7 +152,10 @@
: ::frc971::wpilib::SensorReader(event_loop),
ball_detector_sender_(
event_loop->MakeSender<::y2016::sensors::BallDetector>(
- ".y2016.sensors.ball_detector")) {
+ ".y2016.sensors.ball_detector")),
+ auto_mode_sender_(
+ event_loop->MakeSender<::frc971::autonomous::AutonomousMode>(
+ ".frc971.autonomous.auto_mode")) {
// Set it to filter out anything shorter than 1/4 of the minimum pulse width
// we should ever see.
UpdateFastEncoderFilterHz(kMaxDrivetrainShooterEncoderPulsesPerSecond);
@@ -299,7 +302,7 @@
}
{
- auto auto_mode_message = ::frc971::autonomous::auto_mode.MakeMessage();
+ auto auto_mode_message = auto_mode_sender_.MakeMessage();
auto_mode_message->mode = 0;
for (size_t i = 0; i < autonomous_modes_.size(); ++i) {
if (autonomous_modes_[i]->Get()) {
@@ -313,6 +316,7 @@
private:
::aos::Sender<::y2016::sensors::BallDetector> ball_detector_sender_;
+ ::aos::Sender<::frc971::autonomous::AutonomousMode> auto_mode_sender_;
::std::unique_ptr<AnalogInput> drivetrain_left_hall_, drivetrain_right_hall_;
diff --git a/y2017/BUILD b/y2017/BUILD
index 8637eed..30da3b9 100644
--- a/y2017/BUILD
+++ b/y2017/BUILD
@@ -33,8 +33,8 @@
":constants",
"//aos:init",
"//aos/actions:action_lib",
+ "//aos/input:action_joystick_input",
"//aos/input:drivetrain_input",
- "//aos/input:joystick_input",
"//aos/logging",
"//aos/time",
"//aos/util:log_interval",
diff --git a/y2017/joystick_reader.cc b/y2017/joystick_reader.cc
index 963c7d1..894d1da 100644
--- a/y2017/joystick_reader.cc
+++ b/y2017/joystick_reader.cc
@@ -9,7 +9,7 @@
#include "aos/time/time.h"
#include "aos/util/log_interval.h"
#include "aos/input/drivetrain_input.h"
-#include "aos/input/joystick_input.h"
+#include "aos/input/action_joystick_input.h"
#include "aos/init.h"
#include "frc971/autonomous/auto.q.h"
#include "frc971/autonomous/base_autonomous_actor.h"
@@ -49,63 +49,24 @@
const ButtonLocation kExtra2(3, 10);
const ButtonLocation kHang(3, 2);
-std::unique_ptr<DrivetrainInputReader> drivetrain_input_reader_;
-
-class Reader : public ::aos::input::JoystickInput {
+class Reader : public ::aos::input::ActionJoystickInput {
public:
Reader(::aos::EventLoop *event_loop)
- : ::aos::input::JoystickInput(event_loop),
- autonomous_action_factory_(
- ::frc971::autonomous::BaseAutonomousActor::MakeFactory(
- event_loop)) {
- drivetrain_input_reader_ = DrivetrainInputReader::Make(
- DrivetrainInputReader::InputType::kSteeringWheel,
- ::y2017::control_loops::drivetrain::GetDrivetrainConfig());
- }
+ : ::aos::input::ActionJoystickInput(
+ event_loop,
+ ::y2017::control_loops::drivetrain::GetDrivetrainConfig(),
+ DrivetrainInputReader::InputType::kSteeringWheel, {}) {}
enum class ShotDistance { VISION_SHOT, MIDDLE_SHOT, FAR_SHOT };
ShotDistance last_shot_distance_ = ShotDistance::VISION_SHOT;
- void RunIteration(const ::aos::input::driver_station::Data &data) override {
- bool last_auto_running = auto_running_;
- auto_running_ = data.GetControlBit(ControlBit::kAutonomous) &&
- data.GetControlBit(ControlBit::kEnabled);
- if (auto_running_ != last_auto_running) {
- if (auto_running_) {
- StartAuto();
- } else {
- StopAuto();
- }
- }
-
- if (!auto_running_) {
- HandleDrivetrain(data);
- HandleTeleop(data);
- }
-
- // Process any pending actions.
- action_queue_.Tick();
- was_running_ = action_queue_.Running();
- }
- int intake_accumulator_ = 0;
-
- void HandleDrivetrain(const ::aos::input::driver_station::Data &data) {
- drivetrain_input_reader_->HandleDrivetrain(data);
- robot_velocity_ = drivetrain_input_reader_->robot_velocity();
- }
-
void HandleTeleop(const ::aos::input::driver_station::Data &data) {
// Default the intake to in.
intake_goal_ = 0.07;
bool lights_on = false;
bool vision_track = false;
- if (!data.GetControlBit(ControlBit::kEnabled)) {
- action_queue_.CancelAllActions();
- LOG(DEBUG, "Canceling\n");
- }
-
superstructure_queue.status.FetchLatest();
if (!superstructure_queue.status.get()) {
LOG(ERROR, "Got no superstructure status packet.\n");
@@ -245,7 +206,7 @@
new_superstructure_goal->intake.voltage_rollers = -10.0;
new_superstructure_goal->intake.disable_intake = true;
} else if (data.IsPressed(kIntakeIn) || data.IsPressed(kFire)) {
- if (robot_velocity_ > 2.0) {
+ if (robot_velocity() > 2.0) {
new_superstructure_goal->intake.voltage_rollers = 12.0;
} else {
new_superstructure_goal->intake.voltage_rollers = 11.0;
@@ -285,42 +246,16 @@
}
private:
- void StartAuto() {
- LOG(INFO, "Starting auto mode\n");
-
- ::frc971::autonomous::AutonomousActionParams params;
- ::frc971::autonomous::auto_mode.FetchLatest();
- if (::frc971::autonomous::auto_mode.get() != nullptr) {
- params.mode = ::frc971::autonomous::auto_mode->mode;
- } else {
- LOG(WARNING, "no auto mode values\n");
- params.mode = 0;
- }
- action_queue_.EnqueueAction(autonomous_action_factory_.Make(params));
- }
-
- void StopAuto() {
- LOG(INFO, "Stopping auto mode\n");
- action_queue_.CancelAllActions();
- }
-
// Current goals to send to the robot.
double intake_goal_ = 0.0;
double turret_goal_ = 0.0;
double hood_goal_ = 0.3;
double shooter_velocity_ = 0.0;
-
- bool was_running_ = false;
- bool auto_running_ = false;
+ int intake_accumulator_ = 0;
bool vision_distance_shot_ = false;
bool fire_ = false;
- double robot_velocity_ = 0.0;
-
- ::aos::common::actions::ActionQueue action_queue_;
-
- ::frc971::autonomous::BaseAutonomousActor::Factory autonomous_action_factory_;
};
} // namespace joysticks
diff --git a/y2017/wpilib_interface.cc b/y2017/wpilib_interface.cc
index 5dbf29d..6fd02fc 100644
--- a/y2017/wpilib_interface.cc
+++ b/y2017/wpilib_interface.cc
@@ -126,7 +126,10 @@
class SensorReader : public ::frc971::wpilib::SensorReader {
public:
SensorReader(::aos::EventLoop *event_loop)
- : ::frc971::wpilib::SensorReader(event_loop) {
+ : ::frc971::wpilib::SensorReader(event_loop),
+ auto_mode_sender_(
+ event_loop->MakeSender<::frc971::autonomous::AutonomousMode>(
+ ".frc971.autonomous.auto_mode")) {
// Set to filter out anything shorter than 1/4 of the minimum pulse width
// we should ever see.
UpdateFastEncoderFilterHz(kMaxFastEncoderPulsesPerSecond);
@@ -244,7 +247,7 @@
}
{
- auto auto_mode_message = ::frc971::autonomous::auto_mode.MakeMessage();
+ auto auto_mode_message = auto_mode_sender_.MakeMessage();
auto_mode_message->mode = 0;
for (size_t i = 0; i < autonomous_modes_.size(); ++i) {
if (autonomous_modes_[i] && autonomous_modes_[i]->Get()) {
@@ -257,6 +260,8 @@
}
private:
+ ::aos::Sender<::frc971::autonomous::AutonomousMode> auto_mode_sender_;
+
DigitalGlitchFilter hall_filter_;
::frc971::wpilib::AbsoluteEncoderAndPotentiometer intake_encoder_;
diff --git a/y2018/BUILD b/y2018/BUILD
index 5285116..fc2aae8 100644
--- a/y2018/BUILD
+++ b/y2018/BUILD
@@ -22,8 +22,8 @@
":vision_proto",
"//aos:init",
"//aos/actions:action_lib",
+ "//aos/input:action_joystick_input",
"//aos/input:drivetrain_input",
- "//aos/input:joystick_input",
"//aos/logging",
"//aos/network:team_number",
"//aos/stl_mutex",
diff --git a/y2018/joystick_reader.cc b/y2018/joystick_reader.cc
index d4d49c2..6e1aaa5 100644
--- a/y2018/joystick_reader.cc
+++ b/y2018/joystick_reader.cc
@@ -13,7 +13,7 @@
#include "aos/time/time.h"
#include "aos/util/log_interval.h"
#include "aos/input/drivetrain_input.h"
-#include "aos/input/joystick_input.h"
+#include "aos/input/action_joystick_input.h"
#include "aos/init.h"
#include "aos/vision/events/udp.h"
#include "frc971/autonomous/auto.q.h"
@@ -85,55 +85,23 @@
const ButtonLocation kClawOpen(4, 4);
const ButtonLocation kDriverClawOpen(2, 4);
-class Reader : public ::aos::input::JoystickInput {
+class Reader : public ::aos::input::ActionJoystickInput {
public:
Reader(::aos::EventLoop *event_loop)
- : ::aos::input::JoystickInput(event_loop),
- autonomous_action_factory_(
- ::frc971::autonomous::BaseAutonomousActor::MakeFactory(
- event_loop)) {
+ : ::aos::input::ActionJoystickInput(
+ event_loop,
+ ::y2018::control_loops::drivetrain::GetDrivetrainConfig(),
+ ::aos::network::GetTeamNumber() == 971
+ ? DrivetrainInputReader::InputType::kPistol
+ : DrivetrainInputReader::InputType::kSteeringWheel,
+ {}) {
const uint16_t team = ::aos::network::GetTeamNumber();
- drivetrain_input_reader_ = DrivetrainInputReader::Make(
- team == 971 ? DrivetrainInputReader::InputType::kPistol
- : DrivetrainInputReader::InputType::kSteeringWheel,
- ::y2018::control_loops::drivetrain::GetDrivetrainConfig());
video_tx_.reset(new ProtoTXUdpSocket<VisionControl>(
StringPrintf("10.%d.%d.179", team / 100, team % 100), 5000));
}
- void RunIteration(const ::aos::input::driver_station::Data &data) override {
- bool last_auto_running = auto_running_;
- auto_running_ = data.GetControlBit(ControlBit::kAutonomous) &&
- data.GetControlBit(ControlBit::kEnabled);
- if (auto_running_ != last_auto_running) {
- if (auto_running_) {
- StartAuto();
- } else {
- StopAuto();
- }
- }
-
- if (!auto_running_) {
- HandleDrivetrain(data);
- HandleTeleop(data);
- }
-
- // Process any pending actions.
- action_queue_.Tick();
- was_running_ = action_queue_.Running();
- }
-
- void HandleDrivetrain(const ::aos::input::driver_station::Data &data) {
- drivetrain_input_reader_->HandleDrivetrain(data);
- }
-
- void HandleTeleop(const ::aos::input::driver_station::Data &data) {
- if (!data.GetControlBit(ControlBit::kEnabled)) {
- action_queue_.CancelAllActions();
- LOG(DEBUG, "Canceling\n");
- }
-
+ void HandleTeleop(const ::aos::input::driver_station::Data &data) override {
superstructure_queue.position.FetchLatest();
superstructure_queue.status.FetchLatest();
if (!superstructure_queue.status.get() ||
@@ -191,7 +159,7 @@
} else {
intake_goal_ = -0.10;
}
- if (drivetrain_input_reader_->robot_velocity() < -0.1 &&
+ if (robot_velocity() < -0.1 &&
superstructure_queue.position->box_distance > 0.15) {
intake_goal_ += 0.10;
}
@@ -363,32 +331,14 @@
}
private:
- void StartAuto() {
- LOG(INFO, "Starting auto mode\n");
-
- ::frc971::autonomous::AutonomousActionParams params;
- ::frc971::autonomous::auto_mode.FetchLatest();
- if (::frc971::autonomous::auto_mode.get() != nullptr) {
- params.mode = ::frc971::autonomous::auto_mode->mode << 2;
- } else {
- LOG(WARNING, "no auto mode values\n");
- params.mode = 0;
- }
+ uint32_t GetAutonomousMode() override {
// Low bit is switch, high bit is scale. 1 means left, 0 means right.
- params.mode = mode();
- action_queue_.EnqueueAction(autonomous_action_factory_.Make(params));
- }
-
- void StopAuto() {
- LOG(INFO, "Stopping auto mode\n");
- action_queue_.CancelAllActions();
+ return mode();
}
// Current goals to send to the robot.
double intake_goal_ = 0.0;
- bool was_running_ = false;
- bool auto_running_ = false;
bool never_disabled_ = true;
uint32_t arm_goal_position_ = 0;
@@ -397,13 +347,7 @@
decltype(FrontPoints()) front_points_ = FrontPoints();
decltype(BackPoints()) back_points_ = BackPoints();
- ::aos::common::actions::ActionQueue action_queue_;
-
::std::unique_ptr<ProtoTXUdpSocket<VisionControl>> video_tx_;
-
- ::std::unique_ptr<DrivetrainInputReader> drivetrain_input_reader_;
-
- ::frc971::autonomous::BaseAutonomousActor::Factory autonomous_action_factory_;
};
} // namespace joysticks
diff --git a/y2018/wpilib_interface.cc b/y2018/wpilib_interface.cc
index d6bda66..4e18114 100644
--- a/y2018/wpilib_interface.cc
+++ b/y2018/wpilib_interface.cc
@@ -32,7 +32,6 @@
#include "aos/util/log_interval.h"
#include "aos/util/phased_loop.h"
#include "aos/util/wrapping_counter.h"
-#include "frc971/autonomous/auto.q.h"
#include "frc971/control_loops/control_loops.q.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/wpilib/ADIS16448.h"
@@ -258,11 +257,6 @@
box_back_beambreak_ = ::std::move(input);
}
- // Auto mode switches.
- void set_autonomous_mode(int i, ::std::unique_ptr<frc::DigitalInput> sensor) {
- autonomous_modes_.at(i) = ::std::move(sensor);
- }
-
void set_lidar_lite_input(::std::unique_ptr<frc::DigitalInput> lidar_lite_input) {
lidar_lite_input_ = ::std::move(lidar_lite_input);
lidar_lite_.set_input(lidar_lite_input_.get());
@@ -342,18 +336,6 @@
superstructure_message.Send();
}
-
- {
- auto auto_mode_message = ::frc971::autonomous::auto_mode.MakeMessage();
- auto_mode_message->mode = 0;
- for (size_t i = 0; i < autonomous_modes_.size(); ++i) {
- if (autonomous_modes_[i] && autonomous_modes_[i]->Get()) {
- auto_mode_message->mode |= 1 << i;
- }
- }
- LOG_STRUCT(DEBUG, "auto mode", *auto_mode_message);
- auto_mode_message.Send();
- }
}
private:
@@ -374,8 +356,6 @@
::std::unique_ptr<frc::DigitalInput> claw_beambreak_;
::std::unique_ptr<frc::DigitalInput> box_back_beambreak_;
- ::std::array<::std::unique_ptr<frc::DigitalInput>, 4> autonomous_modes_;
-
::std::unique_ptr<frc::DigitalInput> lidar_lite_input_;
::frc971::wpilib::DMAPulseWidthReader lidar_lite_;
};
diff --git a/y2019/joystick_reader.cc b/y2019/joystick_reader.cc
index 3c028c1..37dba72 100644
--- a/y2019/joystick_reader.cc
+++ b/y2019/joystick_reader.cc
@@ -495,15 +495,6 @@
}
private:
- uint32_t GetAutonomousMode() override {
- ::frc971::autonomous::auto_mode.FetchLatest();
- if (::frc971::autonomous::auto_mode.get() == nullptr) {
- LOG(WARNING, "no auto mode values\n");
- return 0;
- }
- return ::frc971::autonomous::auto_mode->mode;
- }
-
::aos::Sender<::y2019::control_loops::drivetrain::TargetSelectorHint>
target_selector_hint_sender_;
diff --git a/y2019/wpilib_interface.cc b/y2019/wpilib_interface.cc
index dec9b9c..e6dbb1f 100644
--- a/y2019/wpilib_interface.cc
+++ b/y2019/wpilib_interface.cc
@@ -131,7 +131,10 @@
class SensorReader : public ::frc971::wpilib::SensorReader {
public:
SensorReader(::aos::EventLoop *event_loop)
- : ::frc971::wpilib::SensorReader(event_loop) {
+ : ::frc971::wpilib::SensorReader(event_loop),
+ auto_mode_sender_(
+ event_loop->MakeSender<::frc971::autonomous::AutonomousMode>(
+ ".frc971.autonomous.auto_mode")) {
// Set to filter out anything shorter than 1/4 of the minimum pulse width
// we should ever see.
UpdateFastEncoderFilterHz(kMaxFastEncoderPulsesPerSecond);
@@ -279,7 +282,7 @@
}
{
- auto auto_mode_message = ::frc971::autonomous::auto_mode.MakeMessage();
+ auto auto_mode_message = auto_mode_sender_.MakeMessage();
auto_mode_message->mode = 0;
for (size_t i = 0; i < autonomous_modes_.size(); ++i) {
if (autonomous_modes_[i] && autonomous_modes_[i]->Get()) {
@@ -292,6 +295,8 @@
}
private:
+ ::aos::Sender<::frc971::autonomous::AutonomousMode> auto_mode_sender_;
+
::frc971::wpilib::AbsoluteEncoderAndPotentiometer elevator_encoder_,
wrist_encoder_, stilts_encoder_;