Add left/right switches for auto
Change-Id: Idde67276c32050fe7541bf621c4eb818a773a950
diff --git a/aos/input/action_joystick_input.cc b/aos/input/action_joystick_input.cc
index 81e277d..061f4dc 100644
--- a/aos/input/action_joystick_input.cc
+++ b/aos/input/action_joystick_input.cc
@@ -43,7 +43,8 @@
void ActionJoystickInput::StartAuto() {
LOG(INFO, "Starting auto mode\n");
- action_queue_.EnqueueAction(::frc971::autonomous::MakeAutonomousAction(0));
+ action_queue_.EnqueueAction(
+ ::frc971::autonomous::MakeAutonomousAction(GetAutonomousMode()));
}
void ActionJoystickInput::StopAuto() {
diff --git a/aos/input/action_joystick_input.h b/aos/input/action_joystick_input.h
index 87b980e..dca4d39 100644
--- a/aos/input/action_joystick_input.h
+++ b/aos/input/action_joystick_input.h
@@ -40,6 +40,10 @@
void StartAuto();
void StopAuto();
+ // Returns the current autonomous mode which has been selected by robot
+ // inputs.
+ virtual uint32_t GetAutonomousMode() { return 0; }
+
// True if the internal state machine thinks auto is running right now.
bool auto_running_ = false;
// True if an action was running last cycle.
diff --git a/y2019/actors/autonomous_actor.cc b/y2019/actors/autonomous_actor.cc
index 1647f04..41a70fd 100644
--- a/y2019/actors/autonomous_actor.cc
+++ b/y2019/actors/autonomous_actor.cc
@@ -27,10 +27,6 @@
.count();
}
-constexpr bool is_left = false;
-
-constexpr double turn_scalar = is_left ? 1.0 : -1.0;
-
} // namespace
AutonomousActor::AutonomousActor(
@@ -38,7 +34,8 @@
: frc971::autonomous::BaseAutonomousActor(
s, control_loops::drivetrain::GetDrivetrainConfig()) {}
-void AutonomousActor::Reset() {
+void AutonomousActor::Reset(bool is_left) {
+ const double turn_scalar = is_left ? 1.0 : -1.0;
elevator_goal_ = 0.01;
wrist_goal_ = -M_PI / 2.0;
intake_goal_ = -1.2;
@@ -86,8 +83,15 @@
bool AutonomousActor::RunAction(
const ::frc971::autonomous::AutonomousActionParams ¶ms) {
monotonic_clock::time_point start_time = monotonic_clock::now();
- LOG(INFO, "Starting autonomous action with mode %" PRId32 "\n", params.mode);
- Reset();
+ const bool is_left = params.mode == 0;
+
+ {
+ LOG(INFO, "Starting autonomous action with mode %" PRId32 " %s\n",
+ params.mode, is_left ? "left" : "right");
+ }
+ const double turn_scalar = is_left ? 1.0 : -1.0;
+
+ Reset(is_left);
// Grab the disk, wait until we have vacuum, then jump
set_elevator_goal(0.01);
diff --git a/y2019/actors/autonomous_actor.h b/y2019/actors/autonomous_actor.h
index 82e1aeb..38db070 100644
--- a/y2019/actors/autonomous_actor.h
+++ b/y2019/actors/autonomous_actor.h
@@ -26,7 +26,7 @@
const ::frc971::autonomous::AutonomousActionParams ¶ms) override;
private:
- void Reset();
+ void Reset(bool is_left);
double elevator_goal_ = 0.0;
double wrist_goal_ = 0.0;
diff --git a/y2019/joystick_reader.cc b/y2019/joystick_reader.cc
index 9d7117b..b403913 100644
--- a/y2019/joystick_reader.cc
+++ b/y2019/joystick_reader.cc
@@ -400,6 +400,15 @@
}
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;
+ }
+
// Current goals here.
ElevatorWristPosition elevator_wrist_pos_ = kStowPos;
bool grab_piece_ = false;
diff --git a/y2019/wpilib_interface.cc b/y2019/wpilib_interface.cc
index fd9df73..e75b825 100644
--- a/y2019/wpilib_interface.cc
+++ b/y2019/wpilib_interface.cc
@@ -203,6 +203,11 @@
vacuum_sensor_ = make_unique<frc::AnalogInput>(port);
}
+ // Auto mode switches.
+ void set_autonomous_mode(int i, ::std::unique_ptr<frc::DigitalInput> sensor) {
+ autonomous_modes_.at(i) = ::std::move(sensor);
+ }
+
void RunIteration() override {
{
auto drivetrain_message = drivetrain_queue.position.MakeMessage();
@@ -254,6 +259,18 @@
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:
@@ -262,6 +279,8 @@
::std::unique_ptr<frc::AnalogInput> vacuum_sensor_;
+ ::std::array<::std::unique_ptr<frc::DigitalInput>, 2> autonomous_modes_;
+
::frc971::wpilib::AbsoluteEncoder intake_encoder_;
// TODO(sabina): Add wrist and elevator hall effects.
};
@@ -646,6 +665,9 @@
reader.set_pwm_trigger(true);
reader.set_vacuum_sensor(7);
+ reader.set_autonomous_mode(0, make_unique<frc::DigitalInput>(22));
+ reader.set_autonomous_mode(0, make_unique<frc::DigitalInput>(23));
+
::std::thread reader_thread(::std::ref(reader));
CameraReader camera_reader;