Read auto mode sensors
Change-Id: Ib8a3716022805bf02258637f23635c71578a7105
diff --git a/y2016/actors/autonomous_action.q b/y2016/actors/autonomous_action.q
index e2bfd81..5f87a8a 100644
--- a/y2016/actors/autonomous_action.q
+++ b/y2016/actors/autonomous_action.q
@@ -2,8 +2,16 @@
import "aos/common/actions/actions.q";
+message AutonomousMode {
+ // Mode read from the mode setting sensors.
+ int32_t mode;
+};
+
+queue AutonomousMode auto_mode;
+
struct AutonomousActionParams {
- int32_t mode; // 0 = single ball auto
+ // The mode from the sensors when auto starts.
+ int32_t mode;
};
queue_group AutonomousActionQueueGroup {
diff --git a/y2016/joystick_reader.cc b/y2016/joystick_reader.cc
index ab23df1..75f48a4 100644
--- a/y2016/joystick_reader.cc
+++ b/y2016/joystick_reader.cc
@@ -378,7 +378,13 @@
LOG(INFO, "Starting auto mode\n");
actors::AutonomousActionParams params;
- params.mode = 0;
+ actors::auto_mode.FetchLatest();
+ if (actors::auto_mode.get() != nullptr) {
+ params.mode = actors::auto_mode->mode;
+ } else {
+ LOG(WARNING, "no auto mode values\n");
+ params.mode = 0;
+ }
action_queue_.EnqueueAction(actors::MakeAutonomousAction(params));
}
diff --git a/y2016/wpilib/BUILD b/y2016/wpilib/BUILD
index 2cb2937..0379dd0 100644
--- a/y2016/wpilib/BUILD
+++ b/y2016/wpilib/BUILD
@@ -38,5 +38,6 @@
'//y2016/control_loops/shooter:shooter_queue',
'//y2016/control_loops/superstructure:superstructure_queue',
'//y2016/queues:ball_detector',
+ '//y2016/actors:autonomous_action_queue',
],
)
diff --git a/y2016/wpilib/wpilib_interface.cc b/y2016/wpilib/wpilib_interface.cc
index 7c58f15..9393b5d 100644
--- a/y2016/wpilib/wpilib_interface.cc
+++ b/y2016/wpilib/wpilib_interface.cc
@@ -6,6 +6,7 @@
#include <thread>
#include <mutex>
#include <functional>
+#include <array>
#include "Encoder.h"
#include "Talon.h"
@@ -37,6 +38,7 @@
#include "y2016/control_loops/shooter/shooter.q.h"
#include "y2016/control_loops/superstructure/superstructure.q.h"
#include "y2016/queues/ball_detector.q.h"
+#include "y2016/actors/autonomous_action.q.h"
#include "frc971/wpilib/joystick_sender.h"
#include "frc971/wpilib/loop_output_handler.h"
@@ -249,11 +251,15 @@
ball_detector_ = ::std::move(analog);
}
+ // Autonomous mode switch setter.
+ void set_autonomous_mode(int i, ::std::unique_ptr<DigitalInput> sensor) {
+ autonomous_modes_.at(i) = ::std::move(sensor);
+ }
+
// All of the DMA-related set_* calls must be made before this, and it doesn't
// hurt to do all of them.
- // TODO(comran): Add 2016 things down below for dma synchronization.
void set_dma(::std::unique_ptr<DMA> dma) {
dma_synchronizer_.reset(
new ::frc971::wpilib::DMASynchronizer(::std::move(dma)));
@@ -348,6 +354,18 @@
LOG_STRUCT(DEBUG, "ball detector", *ball_detector_message);
ball_detector_message.Send();
}
+
+ {
+ auto auto_mode_message = ::y2016::actors::auto_mode.MakeMessage();
+ auto_mode_message->mode = 0;
+ for (size_t i = 0; i < autonomous_modes_.size(); ++i) {
+ if (autonomous_modes_[i]->Get()) {
+ auto_mode_message->mode |= 1 << i;
+ }
+ }
+ LOG_STRUCT(DEBUG, "auto mode", *auto_mode_message);
+ auto_mode_message.Send();
+ }
}
void Quit() { run_ = false; }
@@ -388,6 +406,8 @@
shoulder_encoder_, wrist_encoder_;
::std::unique_ptr<AnalogInput> ball_detector_;
+ ::std::array<::std::unique_ptr<DigitalInput>, 4> autonomous_modes_;
+
::std::atomic<bool> run_{true};
DigitalGlitchFilter drivetrain_shooter_encoder_filter_, hall_filter_,
superstructure_encoder_filter_;
@@ -649,6 +669,11 @@
reader.set_ball_detector(make_unique<AnalogInput>(7));
+ reader.set_autonomous_mode(0, make_unique<DigitalInput>(9));
+ reader.set_autonomous_mode(1, make_unique<DigitalInput>(8));
+ reader.set_autonomous_mode(2, make_unique<DigitalInput>(7));
+ reader.set_autonomous_mode(3, make_unique<DigitalInput>(6));
+
reader.set_dma(make_unique<DMA>());
::std::thread reader_thread(::std::ref(reader));