Added status light and logic.
Change-Id: Idf192f1e6af3cb4f8a8e659fce5269cbd78bd4bd
diff --git a/y2019/BUILD b/y2019/BUILD
index 3fae9e5..f4ddb2f 100644
--- a/y2019/BUILD
+++ b/y2019/BUILD
@@ -1,4 +1,5 @@
load("//frc971:downloader.bzl", "robot_downloader")
+load("//aos/build:queues.bzl", "queue_library")
robot_downloader(
start_binaries = [
@@ -43,6 +44,7 @@
restricted_to = ["//tools:roborio"],
deps = [
":constants",
+ ":status_light",
"//aos:init",
"//aos:make_unique",
"//aos:math",
@@ -109,6 +111,7 @@
":joystick_reader.cc",
],
deps = [
+ ":status_light",
"//aos:init",
"//aos/actions:action_lib",
"//aos/input:action_joystick_input",
@@ -128,6 +131,14 @@
],
)
+queue_library(
+ name = "status_light",
+ srcs = [
+ "status_light.q",
+ ],
+ visibility = ["//visibility:public"],
+)
+
py_library(
name = "python_init",
srcs = ["__init__.py"],
diff --git a/y2019/control_loops/superstructure/BUILD b/y2019/control_loops/superstructure/BUILD
index 00bacae..524a1ba 100644
--- a/y2019/control_loops/superstructure/BUILD
+++ b/y2019/control_loops/superstructure/BUILD
@@ -24,10 +24,11 @@
],
deps = [
":collision_avoidance",
- ":vacuum",
":superstructure_queue",
+ ":vacuum",
"//aos/controls:control_loop",
"//y2019:constants",
+ "//y2019:status_light",
],
)
@@ -47,6 +48,7 @@
"//frc971/control_loops:capped_test_plant",
"//frc971/control_loops:position_sensor_sim",
"//frc971/control_loops:team_number_test_environment",
+ "//y2019:status_light",
"//y2019/control_loops/superstructure/intake:intake_plants",
],
)
@@ -88,7 +90,7 @@
],
deps = [
":superstructure_queue",
- "//aos/controls:control_loop"
+ "//aos/controls:control_loop",
],
)
diff --git a/y2019/control_loops/superstructure/superstructure.cc b/y2019/control_loops/superstructure/superstructure.cc
index 3443d63..dd332b8 100644
--- a/y2019/control_loops/superstructure/superstructure.cc
+++ b/y2019/control_loops/superstructure/superstructure.cc
@@ -4,10 +4,26 @@
#include "frc971/control_loops/control_loops.q.h"
#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h"
+#include "y2019/status_light.q.h"
+
namespace y2019 {
namespace control_loops {
namespace superstructure {
+namespace {
+
+void SendColors(float red, float green, float blue) {
+ auto new_status_light = status_light.MakeMessage();
+ new_status_light->red = red;
+ new_status_light->green = green;
+ new_status_light->blue = blue;
+
+ if (!new_status_light.Send()) {
+ LOG(ERROR, "Failed to send lights.\n");
+ }
+}
+} // namespace
+
Superstructure::Superstructure(::aos::EventLoop *event_loop,
const ::std::string &name)
: aos::controls::ControlLoop<SuperstructureQueue>(event_loop, name),
@@ -75,6 +91,22 @@
wrist_.set_max_position(collision_avoidance_.max_wrist_goal());
intake_.set_min_position(collision_avoidance_.min_intake_goal());
intake_.set_max_position(collision_avoidance_.max_intake_goal());
+
+ if (status && unsafe_goal) {
+ // Light Logic
+ if (status->estopped) {
+ // Estop is red
+ SendColors(0.5, 0.0, 0.0);
+ } else if (unsafe_goal->suction.gamepiece_mode == 0) {
+ // Ball mode is blue
+ SendColors(0.0, 0.0, 0.5);
+ } else if (unsafe_goal->suction.gamepiece_mode == 1) {
+ // Disk mode is yellow
+ SendColors(0.5, 0.5, 0.0);
+ } else {
+ SendColors(0.0, 0.0, 0.0);
+ }
+ }
}
} // namespace superstructure
diff --git a/y2019/control_loops/superstructure/superstructure.q b/y2019/control_loops/superstructure/superstructure.q
index f87e4f2..d5c8a73 100644
--- a/y2019/control_loops/superstructure/superstructure.q
+++ b/y2019/control_loops/superstructure/superstructure.q
@@ -4,10 +4,13 @@
import "frc971/control_loops/profiled_subsystem.q";
struct SuctionGoal {
- // True = open solenoid (apply suction)
- // Top/bottom are when wrist is forward
- bool top;
- bool bottom;
+ // True = apply suction
+ bool grab_piece;
+
+ // 0 = ball mode
+ // 1 = disk mode
+
+ int32_t gamepiece_mode;
};
queue_group SuperstructureQueue {
diff --git a/y2019/control_loops/superstructure/superstructure_lib_test.cc b/y2019/control_loops/superstructure/superstructure_lib_test.cc
index 0cce4a6..4d3de51 100644
--- a/y2019/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2019/control_loops/superstructure/superstructure_lib_test.cc
@@ -10,6 +10,7 @@
#include "frc971/control_loops/team_number_test_environment.h"
#include "gtest/gtest.h"
#include "y2019/constants.h"
+#include "y2019/status_light.q.h"
#include "y2019/control_loops/superstructure/elevator/elevator_plant.h"
#include "y2019/control_loops/superstructure/intake/intake_plant.h"
#include "y2019/control_loops/superstructure/stilts/stilts_plant.h"
@@ -288,6 +289,7 @@
".y2019.control_loops.superstructure.superstructure_queue."
"position"),
superstructure_(&event_loop_) {
+ status_light.Clear();
set_team_id(::frc971::control_loops::testing::kTeamNumber);
}
@@ -712,8 +714,7 @@
// Turn on suction
{
auto goal = superstructure_queue_.goal.MakeMessage();
- goal->suction.top = true;
- goal->suction.bottom = true;
+ goal->suction.grab_piece = true;
ASSERT_TRUE(goal.Send());
}
@@ -734,8 +735,7 @@
// Turn on suction
{
auto goal = superstructure_queue_.goal.MakeMessage();
- goal->suction.top = true;
- goal->suction.bottom = true;
+ goal->suction.grab_piece = true;
ASSERT_TRUE(goal.Send());
}
@@ -761,8 +761,7 @@
// Turn on suction
{
auto goal = superstructure_queue_.goal.MakeMessage();
- goal->suction.top = true;
- goal->suction.bottom = true;
+ goal->suction.grab_piece = true;
ASSERT_TRUE(goal.Send());
}
@@ -776,8 +775,7 @@
// Turn off suction
{
auto goal = superstructure_queue_.goal.MakeMessage();
- goal->suction.top = false;
- goal->suction.bottom = false;
+ goal->suction.grab_piece = false;
ASSERT_TRUE(goal.Send());
}
diff --git a/y2019/control_loops/superstructure/vacuum.cc b/y2019/control_loops/superstructure/vacuum.cc
index 7bf2e94..497ae5b 100644
--- a/y2019/control_loops/superstructure/vacuum.cc
+++ b/y2019/control_loops/superstructure/vacuum.cc
@@ -32,7 +32,7 @@
low_pump_voltage = *has_piece;
if (unsafe_goal && output) {
- const bool release = !unsafe_goal->top && !unsafe_goal->bottom;
+ const bool release = !unsafe_goal->grab_piece;
if (release) {
last_release_time_ = monotonic_now;
@@ -44,8 +44,16 @@
output->pump_voltage =
release ? 0 : (low_pump_voltage ? kPumpHasPieceVoltage : kPumpVoltage);
- output->intake_suction_top = unsafe_goal->top;
- output->intake_suction_bottom = unsafe_goal->bottom;
+ if (unsafe_goal->grab_piece && unsafe_goal->gamepiece_mode == 0) {
+ output->intake_suction_top = false;
+ output->intake_suction_bottom = true;
+ } else if (unsafe_goal->grab_piece && unsafe_goal->gamepiece_mode == 1) {
+ output->intake_suction_top = true;
+ output->intake_suction_bottom = true;
+ } else {
+ output->intake_suction_top = false;
+ output->intake_suction_bottom = false;
+ }
// If we intend to release, or recently released, set has_piece to false so
// that we give the part of the vacuum circuit with the pressure sensor time
diff --git a/y2019/joystick_reader.cc b/y2019/joystick_reader.cc
index 9290a2f..887ac5e 100644
--- a/y2019/joystick_reader.cc
+++ b/y2019/joystick_reader.cc
@@ -18,6 +18,7 @@
#include "y2019/control_loops/drivetrain/drivetrain_base.h"
#include "y2019/control_loops/superstructure/superstructure.q.h"
+#include "y2019/status_light.q.h"
using ::y2019::control_loops::superstructure::superstructure_queue;
using ::aos::input::driver_station::ButtonLocation;
@@ -102,8 +103,7 @@
::y2019::control_loops::drivetrain::GetDrivetrainConfig()) {
superstructure_queue.goal.FetchLatest();
if (superstructure_queue.goal.get()) {
- top_ = superstructure_queue.goal->suction.top;
- bottom_ = superstructure_queue.goal->suction.bottom;
+ grab_piece_ = superstructure_queue.goal->suction.grab_piece;
}
}
@@ -119,13 +119,12 @@
auto new_superstructure_goal = superstructure_queue.goal.MakeMessage();
if (data.IsPressed(kSuctionBall)) {
- Ball();
+ grab_piece_ = true;
} else if (data.IsPressed(kSuctionHatch)) {
- Disc();
+ grab_piece_ = true;
} else if (data.IsPressed(kRelease) ||
!superstructure_queue.status->has_piece) {
- top_ = false;
- bottom_ = false;
+ grab_piece_ = false;
}
if (data.IsPressed(kRocketBackwardUnpressed)) {
@@ -175,10 +174,10 @@
// Go to intake position and apply vacuum
if (data.IsPressed(kBallHPIntakeForward)) {
- Ball();
+ grab_piece_ = true;
elevator_wrist_pos_ = kBallHPIntakeForwardPos;
} else if (data.IsPressed(kBallHPIntakeBackward)) {
- Ball();
+ grab_piece_ = true;
elevator_wrist_pos_ = kBallHPIntakeBackwardPos;
}
@@ -203,10 +202,10 @@
}
} else {
if (data.IsPressed(kPanelHPIntakeForward)) {
- Disc();
+ grab_piece_ = true;
elevator_wrist_pos_ = kPanelHPIntakeForwrdPos;
} else if (data.IsPressed(kPanelHPIntakeBackward)) {
- Disc();
+ grab_piece_ = true;
elevator_wrist_pos_ = kPanelHPIntakeBackwardPos;
}
@@ -236,7 +235,7 @@
if (kDoBallIntake && !superstructure_queue.status->has_piece) {
elevator_wrist_pos_ = kBallIntakePos;
new_superstructure_goal->roller_voltage = 9.0;
- Ball();
+ grab_piece_ = true;
} else {
if (kDoBallOutake) {
new_superstructure_goal->roller_voltage = -6.0;
@@ -248,12 +247,16 @@
}
if (data.IsPressed(kRelease)) {
- top_ = false;
- bottom_ = false;
+ grab_piece_ = false;
}
- new_superstructure_goal->suction.top = top_;
- new_superstructure_goal->suction.bottom = bottom_;
+ if (switch_ball_) {
+ new_superstructure_goal->suction.gamepiece_mode = 0;
+ } else {
+ new_superstructure_goal->suction.gamepiece_mode = 1;
+ }
+
+ new_superstructure_goal->suction.grab_piece = grab_piece_;
new_superstructure_goal->elevator.unsafe_goal =
elevator_wrist_pos_.elevator;
@@ -265,20 +268,10 @@
}
}
- void Disc() {
- top_ = true;
- bottom_ = true;
- }
- void Ball() {
- top_ = false;
- bottom_ = true;
- }
-
private:
// Current goals here.
ElevatorWristPosition elevator_wrist_pos_ = kStowPos;
- bool top_ = false;
- bool bottom_ = false;
+ bool grab_piece_ = false;
bool switch_ball_ = false;
bool stilts_was_above_ = false;
diff --git a/y2019/status_light.q b/y2019/status_light.q
new file mode 100644
index 0000000..f84ed28
--- /dev/null
+++ b/y2019/status_light.q
@@ -0,0 +1,10 @@
+package y2019;
+
+message StatusLight {
+ // How bright to make each one. 0 is off, 1 is full on.
+ float red;
+ float green;
+ float blue;
+};
+
+queue StatusLight status_light;
diff --git a/y2019/wpilib_interface.cc b/y2019/wpilib_interface.cc
index 564e8e1..a29a51c 100644
--- a/y2019/wpilib_interface.cc
+++ b/y2019/wpilib_interface.cc
@@ -16,6 +16,7 @@
#include "frc971/wpilib/ahal/DriverStation.h"
#include "frc971/wpilib/ahal/Encoder.h"
#include "frc971/wpilib/ahal/VictorSP.h"
+#include "ctre/phoenix/CANifier.h"
#undef ERROR
#include "aos/commonmath.h"
@@ -47,6 +48,7 @@
#include "y2019/control_loops/drivetrain/camera.q.h"
#include "y2019/control_loops/superstructure/superstructure.q.h"
#include "y2019/jevois/spi.h"
+#include "y2019/status_light.q.h"
#ifndef M_PI
#define M_PI 3.14159265358979323846
@@ -522,6 +524,59 @@
to_log.read_solenoids = pcm_.GetAll();
LOG_STRUCT(DEBUG, "pneumatics info", to_log);
}
+
+ status_light.FetchLatest();
+ // If we don't have a light request (or it's an old one), we are borked.
+ // Flash the red light slowly.
+ if (!status_light.get() ||
+ status_light.Age() > chrono::milliseconds(100)) {
+ StatusLight color;
+ color.red = 0.0;
+ color.green = 0.0;
+ color.blue = 0.0;
+
+ ++light_flash_;
+ if (light_flash_ > 10) {
+ color.red = 0.5;
+ }
+
+ if (light_flash_ > 20) {
+ light_flash_ = 0;
+ }
+
+ LOG_STRUCT(DEBUG, "color", color);
+ SetColor(color);
+ } else {
+ LOG_STRUCT(DEBUG, "color", *status_light);
+ SetColor(*status_light);
+ }
+ }
+ }
+
+ void SetColor(const StatusLight &status_light) {
+ // Save CAN bandwidth and CPU at the cost of RT. Only change the light when
+ // it actually changes. This is pretty low priority anyways.
+ static int time_since_last_send = 0;
+ ++time_since_last_send;
+ if (time_since_last_send > 10) {
+ time_since_last_send = 0;
+ }
+ if (status_light.green != last_green_ || time_since_last_send == 0) {
+ canifier_.SetLEDOutput(1.0 - status_light.green,
+ ::ctre::phoenix::CANifier::LEDChannelB);
+ last_green_ = status_light.green;
+ }
+
+ if (status_light.blue != last_blue_ || time_since_last_send == 0) {
+ canifier_.SetLEDOutput(1.0 - status_light.blue,
+ ::ctre::phoenix::CANifier::LEDChannelA);
+ last_blue_ = status_light.blue;
+ }
+
+ if (status_light.red != last_red_ || time_since_last_send == 0) {
+ canifier_.SetLEDOutput(1.0 - status_light.red,
+ ::ctre::phoenix::CANifier::LEDChannelC);
+ last_red_ = status_light.red;
}
}
@@ -540,7 +595,15 @@
::y2019::control_loops::superstructure::SuperstructureQueue::Output>
superstructure_;
+ ::ctre::phoenix::CANifier canifier_{0};
+
::std::atomic<bool> run_{true};
+
+ double last_red_ = -1.0;
+ double last_green_ = -1.0;
+ double last_blue_ = -1.0;
+
+ int light_flash_ = 0;
};
class WPILibRobot : public ::frc971::wpilib::WPILibRobotBase {