Added traverse code.
Change-Id: I22ab8c9133a7b646be4dd06a5aa90aa27a570918
diff --git a/y2016/control_loops/superstructure/superstructure.cc b/y2016/control_loops/superstructure/superstructure.cc
index e85657c..97fe0dd 100644
--- a/y2016/control_loops/superstructure/superstructure.cc
+++ b/y2016/control_loops/superstructure/superstructure.cc
@@ -659,6 +659,8 @@
output->voltage_bottom_rollers = ::std::max(
-kMaxIntakeBottomVoltage,
::std::min(unsafe_goal->voltage_bottom_rollers, kMaxIntakeBottomVoltage));
+ output->traverse_unlatched = unsafe_goal->traverse_unlatched;
+ output->traverse_down = unsafe_goal->traverse_down;
}
}
diff --git a/y2016/control_loops/superstructure/superstructure.q b/y2016/control_loops/superstructure/superstructure.q
index 5d17fb6..f550512 100644
--- a/y2016/control_loops/superstructure/superstructure.q
+++ b/y2016/control_loops/superstructure/superstructure.q
@@ -64,6 +64,12 @@
// Voltage to send to the rollers. Positive is sucking in.
float voltage_top_rollers;
float voltage_bottom_rollers;
+
+ // If true, release the latch which holds the traverse mechanism in the
+ // middle.
+ bool traverse_unlatched;
+ // If true, fire the traverse mechanism down.
+ bool traverse_down;
};
message Status {
@@ -109,6 +115,11 @@
float voltage_top_rollers;
float voltage_bottom_rollers;
+
+ // If true, release the latch to hold the traverse mechanism in the middle.
+ bool traverse_unlatched;
+ // If true, fire the traverse mechanism down.
+ bool traverse_down;
};
queue Goal goal;
diff --git a/y2016/joystick_reader.cc b/y2016/joystick_reader.cc
index ab23df1..3226d06 100644
--- a/y2016/joystick_reader.cc
+++ b/y2016/joystick_reader.cc
@@ -58,12 +58,11 @@
const POVLocation kBackLong(3, 0);
const POVLocation kBackFender(3, 90);
const POVLocation kFrontFender(3, 270);
-const ButtonLocation kTest3(3, 7);
const ButtonLocation kIntakeIn(3, 12);
-const ButtonLocation kTest5(3, 8);
const ButtonLocation kFire(3, 3);
-const ButtonLocation kTest7(3, 5);
const ButtonLocation kIntakeOut(3, 9);
+const ButtonLocation kPortcullis(3, 7);
+const ButtonLocation kChevalDeFrise(3, 8);
const ButtonLocation kVisionAlign(3, 4);
@@ -239,10 +238,6 @@
shooter_velocity_ = 0.0;
}
- if (data.IsPressed(kTest3)) {
- wrist_goal_ = 0.0;
- }
-
bool ball_detected = false;
::y2016::sensors::ball_detector.FetchLatest();
if (::y2016::sensors::ball_detector.get()) {
@@ -297,9 +292,6 @@
}
}
- if (data.PosEdge(kTest7)) {
- }
-
is_outtaking_ = data.IsPressed(kIntakeOut);
if (is_intaking_ || is_outtaking_) {
@@ -318,6 +310,17 @@
--recently_intaking_accumulator_;
}
+ if (data.IsPressed(kPortcullis)) {
+ traverse_unlatched_ = true;
+ traverse_down_ = true;
+ } else if (data.IsPressed(kChevalDeFrise)) {
+ traverse_unlatched_ = false;
+ traverse_down_ = true;
+ } else {
+ traverse_unlatched_ = true;
+ traverse_down_ = false;
+ }
+
if (!waiting_for_zero_) {
auto new_superstructure_goal = superstructure_queue.goal.MakeMessage();
new_superstructure_goal->angle_intake = intake_goal_;
@@ -355,6 +358,9 @@
new_superstructure_goal->voltage_bottom_rollers = 0.0;
}
+ new_superstructure_goal->traverse_unlatched = traverse_unlatched_;
+ new_superstructure_goal->traverse_down = traverse_down_;
+
if (!new_superstructure_goal.Send()) {
LOG(ERROR, "Sending superstructure goal failed.\n");
} else {
@@ -397,6 +403,9 @@
bool was_running_ = false;
bool auto_running_ = false;
+ bool traverse_unlatched_ = false;
+ bool traverse_down_ = false;
+
// If we're waiting for the subsystems to zero.
bool waiting_for_zero_ = true;
diff --git a/y2016/wpilib/wpilib_interface.cc b/y2016/wpilib/wpilib_interface.cc
index 7c58f15..caf34b2 100644
--- a/y2016/wpilib/wpilib_interface.cc
+++ b/y2016/wpilib/wpilib_interface.cc
@@ -398,7 +398,10 @@
SolenoidWriter(const ::std::unique_ptr<::frc971::wpilib::BufferedPcm> &pcm)
: pcm_(pcm),
drivetrain_(".frc971.control_loops.drivetrain_queue.output"),
- shooter_(".y2016.control_loops.shooter.shooter_queue.output") {}
+ shooter_(".y2016.control_loops.shooter.shooter_queue.output"),
+ superstructure_(
+ ".y2016.control_loops.superstructure_queue.output") {
+ }
void set_compressor(::std::unique_ptr<Compressor> compressor) {
compressor_ = ::std::move(compressor);
@@ -414,6 +417,16 @@
drivetrain_right_ = ::std::move(s);
}
+ void set_traverse(
+ ::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> s) {
+ traverse_ = ::std::move(s);
+ }
+
+ void set_traverse_latch(
+ ::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> s) {
+ traverse_latch_ = ::std::move(s);
+ }
+
void set_shooter_clamp(
::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> s) {
shooter_clamp_ = ::std::move(s);
@@ -465,6 +478,15 @@
}
{
+ superstructure_.FetchLatest();
+ if (superstructure_.get()) {
+ LOG_STRUCT(DEBUG, "solenoids", *shooter_);
+ traverse_->Set(superstructure_->traverse_down);
+ traverse_latch_->Set(superstructure_->traverse_unlatched);
+ }
+ }
+
+ {
::frc971::wpilib::PneumaticsToLog to_log;
{
to_log.compressor_on = compressor_->Enabled();
@@ -483,11 +505,15 @@
const ::std::unique_ptr<::frc971::wpilib::BufferedPcm> &pcm_;
::std::unique_ptr<::frc971::wpilib::BufferedSolenoid> drivetrain_left_,
- drivetrain_right_, shooter_clamp_, shooter_pusher_, lights_;
+ drivetrain_right_, shooter_clamp_, shooter_pusher_, lights_, traverse_,
+ traverse_latch_;
::std::unique_ptr<Compressor> compressor_;
::aos::Queue<::frc971::control_loops::DrivetrainQueue::Output> drivetrain_;
::aos::Queue<::y2016::control_loops::shooter::ShooterQueue::Output> shooter_;
+ ::aos::Queue<
+ ::y2016::control_loops::SuperstructureQueue::Output>
+ superstructure_;
::std::atomic<bool> run_{true};
};
@@ -694,6 +720,8 @@
SolenoidWriter solenoid_writer(pcm);
solenoid_writer.set_drivetrain_left(pcm->MakeSolenoid(1));
solenoid_writer.set_drivetrain_right(pcm->MakeSolenoid(0));
+ solenoid_writer.set_traverse_latch(pcm->MakeSolenoid(2));
+ solenoid_writer.set_traverse(pcm->MakeSolenoid(3));
solenoid_writer.set_shooter_clamp(pcm->MakeSolenoid(4));
solenoid_writer.set_shooter_pusher(pcm->MakeSolenoid(5));
solenoid_writer.set_lights(pcm->MakeSolenoid(6));