Merge "Be more robust to arbitrary incoming frames"
diff --git a/y2019/BUILD b/y2019/BUILD
index e3bac02..4f11c36 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 = [
@@ -44,6 +45,7 @@
restricted_to = ["//tools:roborio"],
deps = [
":constants",
+ ":status_light",
"//aos:init",
"//aos:make_unique",
"//aos:math",
@@ -110,6 +112,7 @@
":joystick_reader.cc",
],
deps = [
+ ":status_light",
"//aos:init",
"//aos/actions:action_lib",
"//aos/input:action_joystick_input",
@@ -129,6 +132,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/constants.cc b/y2019/constants.cc
index f84e945..0a9a429 100644
--- a/y2019/constants.cc
+++ b/y2019/constants.cc
@@ -196,8 +196,8 @@
intake->zeroing_constants.measured_absolute_position = 1.756847;
- wrist_params->zeroing_constants.measured_absolute_position = 0.357394;
- wrist->potentiometer_offset = -1.479097 - 2.740303;
+ wrist_params->zeroing_constants.measured_absolute_position = 0.192576;
+ wrist->potentiometer_offset = -4.200894 - 0.187134;
stilts_params->zeroing_constants.measured_absolute_position = 0.043580;
stilts->potentiometer_offset = -0.093820 + 0.0124 - 0.008334 + 0.004507;
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 bfc13e4..26a8ce1 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;
@@ -105,8 +106,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;
}
}
@@ -122,13 +122,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)) {
@@ -178,10 +177,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;
}
@@ -206,10 +205,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;
}
@@ -243,7 +242,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;
@@ -255,12 +254,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;
@@ -272,20 +275,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/vision/calibrate.sh b/y2019/vision/calibrate.sh
new file mode 100755
index 0000000..24118ed
--- /dev/null
+++ b/y2019/vision/calibrate.sh
@@ -0,0 +1,70 @@
+#!/bin/bash
+
+set -e
+
+cd "$(dirname "${BASH_SOURCE[0]}")"
+
+# We need to rebuild to save the old constants since the .cc file is baked into
+# the executable.
+calibration() {
+ bazel build -c opt //y2019/vision:global_calibration
+
+ ../../bazel-bin/y2019/vision/global_calibration "$@"
+}
+
+# Calibrate the comp robot. This is assuming the data file captured has been
+# extracted into the data folder in //y2019/vision/
+calibration \
+ --camera_id 10 \
+ --tape_start_x=-12.5 \
+ --tape_start_y=0.0 \
+ --tape_direction_x=-1.0 \
+ --tape_direction_y=0.0 \
+ --beginning_tape_measure_reading=11 \
+ --image_count=75 \
+ --constants=constants.cc \
+ --prefix data/cam10_0/debug_viewer_jpeg_
+
+calibration \
+ --camera_id 6 \
+ --tape_start_x=12.5 \
+ --tape_start_y=0.0 \
+ --tape_direction_x=1.0 \
+ --tape_direction_y=0.0 \
+ --beginning_tape_measure_reading=11 \
+ --image_count=75 \
+ --constants=constants.cc \
+ --prefix data/cam6_0/debug_viewer_jpeg_
+
+calibration \
+ --camera_id 7 \
+ --tape_start_x=12.5 \
+ --tape_start_y=0.0 \
+ --tape_direction_x=1.0 \
+ --tape_direction_y=0.0 \
+ --beginning_tape_measure_reading=21 \
+ --image_count=65 \
+ --constants=constants.cc \
+ --prefix data/cam7_0/debug_viewer_jpeg_
+
+calibration \
+ --camera_id 9 \
+ --tape_start_x=-6.5 \
+ --tape_start_y=11.0 \
+ --tape_direction_x=0.0 \
+ --tape_direction_y=-1.0 \
+ --beginning_tape_measure_reading=30 \
+ --image_count=56 \
+ --constants=constants.cc \
+ --prefix data/cam9_0/debug_viewer_jpeg_
+
+calibration \
+ --camera_id 8 \
+ --tape_start_x=6.5 \
+ --tape_start_y=-11.0 \
+ --tape_direction_x=0.0 \
+ --tape_direction_y=1.0 \
+ --beginning_tape_measure_reading=25 \
+ --image_count=61 \
+ --constants=constants.cc \
+ --prefix data/cam8_0/debug_viewer_jpeg_
diff --git a/y2019/vision/calibrate_9971.sh b/y2019/vision/calibrate_9971.sh
new file mode 100755
index 0000000..9a05ce2
--- /dev/null
+++ b/y2019/vision/calibrate_9971.sh
@@ -0,0 +1,70 @@
+#!/bin/bash
+
+set -e
+
+cd "$(dirname "${BASH_SOURCE[0]}")"
+
+# We need to rebuild to save the old constants since the .cc file is baked into
+# the executable.
+calibration() {
+ bazel build -c opt //y2019/vision:global_calibration
+
+ ../../bazel-bin/y2019/vision/global_calibration "$@"
+}
+
+# Calibrate the comp robot. This is assuming the data file captured has been
+# extracted into the users home directory.
+calibration \
+ --camera_id 1 \
+ --tape_start_x=-12.5 \
+ --tape_start_y=0.0 \
+ --tape_direction_x=-1.0 \
+ --tape_direction_y=0.0 \
+ --beginning_tape_measure_reading=16 \
+ --image_count=45 \
+ --constants=constants.cc \
+ --prefix ~/cam1/debug_viewer_jpeg_
+
+calibration \
+ --camera_id 14 \
+ --tape_start_x=12.5 \
+ --tape_start_y=0.0 \
+ --tape_direction_x=1.0 \
+ --tape_direction_y=0.0 \
+ --beginning_tape_measure_reading=16 \
+ --image_count=71 \
+ --constants=constants.cc \
+ --prefix ~/cam14/debug_viewer_jpeg_
+
+calibration \
+ --camera_id 15 \
+ --tape_start_x=12.5 \
+ --tape_start_y=0.0 \
+ --tape_direction_x=1.0 \
+ --tape_direction_y=0.0 \
+ --beginning_tape_measure_reading=25 \
+ --image_count=62 \
+ --constants=constants.cc \
+ --prefix ~/cam15/debug_viewer_jpeg_
+
+calibration \
+ --camera_id 17 \
+ --tape_start_x=-6.5 \
+ --tape_start_y=11.0 \
+ --tape_direction_x=0.0 \
+ --tape_direction_y=-1.0 \
+ --beginning_tape_measure_reading=29 \
+ --image_count=58 \
+ --constants=constants.cc \
+ --prefix ~/cam17/debug_viewer_jpeg_
+
+calibration \
+ --camera_id 18 \
+ --tape_start_x=6.5 \
+ --tape_start_y=-11.0 \
+ --tape_direction_x=0.0 \
+ --tape_direction_y=1.0 \
+ --beginning_tape_measure_reading=27 \
+ --image_count=60 \
+ --constants=constants.cc \
+ --prefix ~/cam18/debug_viewer_jpeg_
diff --git a/y2019/vision/constants.cc b/y2019/vision/constants.cc
index e41a53b..0fa8eda 100644
--- a/y2019/vision/constants.cc
+++ b/y2019/vision/constants.cc
@@ -5,6 +5,24 @@
static constexpr double kInchesToMeters = 0.0254;
+CameraCalibration camera_1 = {
+ {
+ -0.874694 / 180.0 * M_PI, 338.619, 2.14651 / 180.0 * M_PI,
+ },
+ {
+ {{-5.44063 * kInchesToMeters, 2.83405 * kInchesToMeters,
+ 33.0386 * kInchesToMeters}},
+ 181.723 / 180.0 * M_PI,
+ },
+ {
+ 1,
+ {{-12.5 * kInchesToMeters, 0.0}},
+ {{-1 * kInchesToMeters, 0.0}},
+ 16,
+ "/home/alex/cam1/debug_viewer_jpeg_",
+ 45,
+ }};
+
CameraCalibration camera_4 = {
{
3.73623 / 180.0 * M_PI, 588.1, 0.269291 / 180.0 * M_PI,
@@ -41,22 +59,130 @@
59,
}};
-CameraCalibration camera_10 = {
+CameraCalibration camera_6 = {
{
- 0.0429164 / 180.0 * M_PI, 337.247, 0.865625 / 180.0 * M_PI,
+ -1.17595 / 180.0 * M_PI, 346.997, 0.987547 / 180.0 * M_PI,
},
{
- {{20.5486 * kInchesToMeters, -2.47638 * kInchesToMeters,
- 33.1395 * kInchesToMeters}},
- 2.08049 / 180.0 * M_PI,
+ {{4.88124 * kInchesToMeters, 2.15528 * kInchesToMeters,
+ 33.1686 * kInchesToMeters}},
+ -12.0018 / 180.0 * M_PI,
+ },
+ {
+ 6,
+ {{12.5 * kInchesToMeters, 0.0}},
+ {{1 * kInchesToMeters, 0.0}},
+ 11,
+ "data/cam6_0/debug_viewer_jpeg_",
+ 75,
+ }};
+
+CameraCalibration camera_7 = {
+ {
+ -2.30729 / 180.0 * M_PI, 339.894, 1.16684 / 180.0 * M_PI,
+ },
+ {
+ {{3.62399 * kInchesToMeters, 3.94792 * kInchesToMeters,
+ 33.3196 * kInchesToMeters}},
+ 18.5828 / 180.0 * M_PI,
+ },
+ {
+ 7,
+ {{12.5 * kInchesToMeters, 0.0}},
+ {{1 * kInchesToMeters, 0.0}},
+ 21,
+ "data/cam7_0/debug_viewer_jpeg_",
+ 65,
+ }};
+
+CameraCalibration camera_8 = {
+ {
+ 37.0966 / 180.0 * M_PI, 339.997, 0.265968 / 180.0 * M_PI,
+ },
+ {
+ {{3.53674 * kInchesToMeters, 5.25891 * kInchesToMeters,
+ 12.6869 * kInchesToMeters}},
+ 92.4773 / 180.0 * M_PI,
+ },
+ {
+ 8,
+ {{6.5 * kInchesToMeters, -11 * kInchesToMeters}},
+ {{0.0, 1 * kInchesToMeters}},
+ 25,
+ "data/cam8_0/debug_viewer_jpeg_",
+ 61,
+ }};
+
+CameraCalibration camera_9 = {
+ {
+ 35.3461 / 180.0 * M_PI, 337.599, 3.34351 / 180.0 * M_PI,
+ },
+ {
+ {{4.24216 * kInchesToMeters, -2.97032 * kInchesToMeters,
+ 11.323 * kInchesToMeters}},
+ -93.3026 / 180.0 * M_PI,
+ },
+ {
+ 9,
+ {{-6.5 * kInchesToMeters, 11 * kInchesToMeters}},
+ {{0.0, -1 * kInchesToMeters}},
+ 30,
+ "data/cam9_0/debug_viewer_jpeg_",
+ 56,
+ }};
+
+CameraCalibration camera_10 = {
+ {
+ -0.165199 / 180.0 * M_PI, 340.666, 0.596842 / 180.0 * M_PI,
+ },
+ {
+ {{-5.23103 * kInchesToMeters, 2.96098 * kInchesToMeters,
+ 33.2867 * kInchesToMeters}},
+ 182.121 / 180.0 * M_PI,
},
{
10,
- {{12.5 * kInchesToMeters, 0.5 * kInchesToMeters}},
- {{1 * kInchesToMeters, 0.0}},
- 26,
+ {{-12.5 * kInchesToMeters, 0.0}},
+ {{-1 * kInchesToMeters, 0.0}},
+ 11,
"data/cam10_0/debug_viewer_jpeg_",
- 59,
+ 75,
+ }};
+
+CameraCalibration camera_14 = {
+ {
+ -0.0729684 / 180.0 * M_PI, 343.569, 0.685893 / 180.0 * M_PI,
+ },
+ {
+ {{5.53867 * kInchesToMeters, 2.08897 * kInchesToMeters,
+ 33.1766 * kInchesToMeters}},
+ -12.4121 / 180.0 * M_PI,
+ },
+ {
+ 14,
+ {{12.5 * kInchesToMeters, 0.0}},
+ {{1 * kInchesToMeters, 0.0}},
+ 16,
+ "/home/alex/cam14/debug_viewer_jpeg_",
+ 71,
+ }};
+
+CameraCalibration camera_15 = {
+ {
+ -0.715538 / 180.0 * M_PI, 336.509, 1.54169 / 180.0 * M_PI,
+ },
+ {
+ {{4.57935 * kInchesToMeters, 4.16624 * kInchesToMeters,
+ 33.433 * kInchesToMeters}},
+ 20.9856 / 180.0 * M_PI,
+ },
+ {
+ 15,
+ {{12.5 * kInchesToMeters, 0.0}},
+ {{1 * kInchesToMeters, 0.0}},
+ 25,
+ "/home/alex/cam15/debug_viewer_jpeg_",
+ 62,
}};
CameraCalibration camera_16 = {
@@ -77,6 +203,42 @@
55,
}};
+CameraCalibration camera_17 = {
+ {
+ 34.7631 / 180.0 * M_PI, 337.946, 2.48943 / 180.0 * M_PI,
+ },
+ {
+ {{3.15808 * kInchesToMeters, -2.5734 * kInchesToMeters,
+ 11.8507 * kInchesToMeters}},
+ -92.1953 / 180.0 * M_PI,
+ },
+ {
+ 17,
+ {{-6.5 * kInchesToMeters, 11 * kInchesToMeters}},
+ {{0.0, -1 * kInchesToMeters}},
+ 29,
+ "/home/alex/cam17/debug_viewer_jpeg_",
+ 58,
+ }};
+
+CameraCalibration camera_18 = {
+ {
+ 33.9292 / 180.0 * M_PI, 338.44, -1.71889 / 180.0 * M_PI,
+ },
+ {
+ {{3.82945 * kInchesToMeters, 5.51444 * kInchesToMeters,
+ 12.3803 * kInchesToMeters}},
+ 96.0112 / 180.0 * M_PI,
+ },
+ {
+ 18,
+ {{6.5 * kInchesToMeters, -11 * kInchesToMeters}},
+ {{0.0, 1 * kInchesToMeters}},
+ 27,
+ "/home/alex/cam18/debug_viewer_jpeg_",
+ 60,
+ }};
+
CameraCalibration camera_19 = {
{
-0.341036 / 180.0 * M_PI, 324.626, 1.2545 / 180.0 * M_PI,
@@ -97,14 +259,32 @@
const CameraCalibration *GetCamera(int camera_id) {
switch (camera_id) {
+ case 1:
+ return &camera_1;
case 4:
return &camera_4;
case 5:
return &camera_5;
+ case 6:
+ return &camera_6;
+ case 7:
+ return &camera_7;
+ case 8:
+ return &camera_8;
+ case 9:
+ return &camera_9;
case 10:
return &camera_10;
+ case 14:
+ return &camera_14;
+ case 15:
+ return &camera_15;
case 16:
return &camera_16;
+ case 17:
+ return &camera_17;
+ case 18:
+ return &camera_18;
case 19:
return &camera_19;
default:
diff --git a/y2019/vision/constants.h b/y2019/vision/constants.h
index 3f07f87..faa16e1 100644
--- a/y2019/vision/constants.h
+++ b/y2019/vision/constants.h
@@ -12,14 +12,14 @@
struct CameraGeometry {
static constexpr size_t kNumParams = 4;
// In Meters from floor under imu center.
- std::array<double, 3> location{{0, 0, 0}};
+ ::std::array<double, 3> location{{0.0, 0.0, 0.0}};
double heading = 0.0;
void set(double *data) {
- location[0] = data[0];
- location[1] = data[1];
- location[2] = data[2];
- heading = data[3];
+ data[0] = location[0];
+ data[1] = location[1];
+ data[2] = location[2];
+ data[3] = heading;
}
static CameraGeometry get(const double *data) {
CameraGeometry out;
@@ -60,9 +60,9 @@
struct DatasetInfo {
int camera_id;
// In meters from IMU start.
- std::array<double, 2> to_tape_measure_start;
+ ::std::array<double, 2> to_tape_measure_start;
// In meters,
- std::array<double, 2> tape_measure_direction;
+ ::std::array<double, 2> tape_measure_direction;
// This will multiply tape_measure_direction and thus has no units.
double beginning_tape_measure_reading;
const char *filename_prefix;
@@ -81,14 +81,17 @@
// Serial number of the teensy for each robot.
constexpr uint32_t CodeBotTeensyId() { return 0xffff322e; }
+constexpr uint32_t PracticeBotTeensyId() { return 0xffff3215; }
// Get the IDs of the cameras in each port for a particular teensy board.
// inlined so that we don't have to deal with including it in the autogenerated
// constants.cc.
-inline std::array<int, 5> CameraSerialNumbers(uint32_t processor_id) {
+inline ::std::array<int, 5> CameraSerialNumbers(uint32_t processor_id) {
switch (processor_id) {
case CodeBotTeensyId():
return {{0, 0, 0, 16, 19}};
+ case PracticeBotTeensyId():
+ return {{14, 15, 18, 17, 1}};
default:
return {{0, 0, 0, 0, 0}};
}
diff --git a/y2019/vision/global_calibration.cc b/y2019/vision/global_calibration.cc
index c962fc9..d21e184 100644
--- a/y2019/vision/global_calibration.cc
+++ b/y2019/vision/global_calibration.cc
@@ -18,6 +18,27 @@
#include "ceres/ceres.h"
DEFINE_int32(camera_id, -1, "The camera ID to calibrate");
+DEFINE_string(prefix, "", "The image filename prefix");
+
+DEFINE_string(constants, "y2019/vision/constants.cc",
+ "Path to the constants file to update");
+
+DEFINE_double(beginning_tape_measure_reading, 11,
+ "The tape measure measurement (in inches) of the first image.");
+DEFINE_int32(image_count, 75, "The number of images to capture");
+DEFINE_double(
+ tape_start_x, -12.5,
+ "The starting location of the tape measure in x relative to the CG in inches.");
+DEFINE_double(
+ tape_start_y, -0.5,
+ "The starting location of the tape measure in y relative to the CG in inches.");
+
+DEFINE_double(
+ tape_direction_x, -1.0,
+ "The x component of \"1\" inch along the tape measure in meters.");
+DEFINE_double(
+ tape_direction_y, 0.0,
+ "The y component of \"1\" inch along the tape measure in meters.");
using ::aos::events::DataSocket;
using ::aos::events::RXUdpSocket;
@@ -36,19 +57,17 @@
namespace y2019 {
namespace vision {
+namespace {
constexpr double kInchesToMeters = 0.0254;
-template <size_t k, size_t n, size_t n2, typename T>
-T *MakeFunctor(T &&t) {
- return new T(std::move(t));
-}
+} // namespace
-std::array<double, 3> GetError(const DatasetInfo &info,
- const double *const extrinsics,
- const double *const geometry, int i) {
- auto extrinsic_params = ExtrinsicParams::get(extrinsics);
- auto geo = CameraGeometry::get(geometry);
+::std::array<double, 3> GetError(const DatasetInfo &info,
+ const double *const extrinsics,
+ const double *const geometry, int i) {
+ const ExtrinsicParams extrinsic_params = ExtrinsicParams::get(extrinsics);
+ const CameraGeometry geo = CameraGeometry::get(geometry);
const double s = sin(geo.heading + extrinsic_params.r2);
const double c = cos(geo.heading + extrinsic_params.r2);
@@ -56,142 +75,168 @@
// Take the tape measure starting point (this will be at the perimeter of the
// robot), add the offset to the first sample, and then add the per sample
// offset.
- const double dx =
- (info.to_tape_measure_start[0] +
- (info.beginning_tape_measure_reading + i) *
- info.tape_measure_direction[0]) /
- kInchesToMeters -
- (geo.location[0] + c * extrinsic_params.z) / kInchesToMeters;
- const double dy =
- (info.to_tape_measure_start[1] +
- (info.beginning_tape_measure_reading + i) *
- info.tape_measure_direction[1]) /
- kInchesToMeters -
- (geo.location[1] + s * extrinsic_params.z) / kInchesToMeters;
+ const double dx = (info.to_tape_measure_start[0] +
+ (info.beginning_tape_measure_reading + i) *
+ info.tape_measure_direction[0]) -
+ (geo.location[0] + c * extrinsic_params.z);
+ const double dy = (info.to_tape_measure_start[1] +
+ (info.beginning_tape_measure_reading + i) *
+ info.tape_measure_direction[1]) -
+ (geo.location[1] + s * extrinsic_params.z);
- constexpr double kCalibrationTargetHeight = 28.5;
- const double dz = kCalibrationTargetHeight -
- (geo.location[2] + extrinsic_params.y) / kInchesToMeters;
+ constexpr double kCalibrationTargetHeight = 28.5 * kInchesToMeters;
+ const double dz =
+ kCalibrationTargetHeight - (geo.location[2] + extrinsic_params.y);
return {{dx, dy, dz}};
}
void main(int argc, char **argv) {
using namespace y2019::vision;
- gflags::ParseCommandLineFlags(&argc, &argv, false);
-
- const char *base_directory = "/home/parker/data/frc/2019_calibration/";
+ ::gflags::ParseCommandLineFlags(&argc, &argv, false);
DatasetInfo info = {
FLAGS_camera_id,
- {{12.5 * kInchesToMeters, 0.5 * kInchesToMeters}},
- {{kInchesToMeters, 0.0}},
- 26,
- "cam5_0/debug_viewer_jpeg_",
- 59,
+ {{FLAGS_tape_start_x * kInchesToMeters,
+ FLAGS_tape_start_y * kInchesToMeters}},
+ {{FLAGS_tape_direction_x * kInchesToMeters,
+ FLAGS_tape_direction_y * kInchesToMeters}},
+ FLAGS_beginning_tape_measure_reading,
+ FLAGS_prefix.c_str(),
+ FLAGS_image_count,
};
::aos::logging::Init();
::aos::logging::AddImplementation(
new ::aos::logging::StreamLogImplementation(stderr));
- TargetFinder finder_;
+ TargetFinder target_finder;
ceres::Problem problem;
struct Sample {
- std::unique_ptr<double[]> extrinsics;
+ ::std::unique_ptr<double[]> extrinsics;
int i;
};
- std::vector<Sample> all_extrinsics;
+ ::std::vector<Sample> all_extrinsics;
double intrinsics[IntrinsicParams::kNumParams];
IntrinsicParams().set(&intrinsics[0]);
double geometry[CameraGeometry::kNumParams];
- CameraGeometry().set(&geometry[0]);
+ {
+ // Assume the camera is near the center of the robot, and start by pointing
+ // it from the center of the robot to the location of the first target.
+ CameraGeometry camera_geometry;
+ const double x =
+ info.to_tape_measure_start[0] +
+ info.beginning_tape_measure_reading * info.tape_measure_direction[0];
+ const double y =
+ info.to_tape_measure_start[1] +
+ info.beginning_tape_measure_reading * info.tape_measure_direction[1];
+ camera_geometry.heading = ::std::atan2(y, x);
+ printf("Inital heading is %f\n", camera_geometry.heading);
+ camera_geometry.set(&geometry[0]);
+ }
Solver::Options options;
options.minimizer_progress_to_stdout = false;
Solver::Summary summary;
- std::cout << summary.BriefReport() << "\n";
+ ::std::cout << summary.BriefReport() << "\n";
IntrinsicParams intrinsics_ = IntrinsicParams::get(&intrinsics[0]);
- std::cout << "rup = " << intrinsics_.mount_angle * 180 / M_PI << ";\n";
- std::cout << "fl = " << intrinsics_.focal_length << ";\n";
- std::cout << "error = " << summary.final_cost << ";\n";
+ ::std::cout << "rup = " << intrinsics_.mount_angle * 180 / M_PI << ";\n";
+ ::std::cout << "fl = " << intrinsics_.focal_length << ";\n";
+ ::std::cout << "error = " << summary.final_cost << ";\n";
for (int i = 0; i < info.num_images; ++i) {
- auto frame = aos::vision::LoadFile(std::string(base_directory) +
- info.filename_prefix +
- std::to_string(i) + ".yuyv");
+ ::aos::vision::DatasetFrame frame =
+ aos::vision::LoadFile(FLAGS_prefix + std::to_string(i) + ".yuyv");
- aos::vision::ImageFormat fmt{640, 480};
- aos::vision::BlobList imgs =
- aos::vision::FindBlobs(aos::vision::DoThresholdYUYV(
+ const ::aos::vision::ImageFormat fmt{640, 480};
+ ::aos::vision::BlobList imgs =
+ ::aos::vision::FindBlobs(aos::vision::DoThresholdYUYV(
fmt, frame.data.data(), TargetFinder::GetThresholdValue()));
- finder_.PreFilter(&imgs);
+ target_finder.PreFilter(&imgs);
- bool verbose = false;
- std::vector<std::vector<Segment<2>>> raw_polys;
+ constexpr bool verbose = false;
+ ::std::vector<std::vector<Segment<2>>> raw_polys;
for (const RangeImage &blob : imgs) {
// Convert blobs to contours in the corrected space.
- ContourNode* contour = finder_.GetContour(blob);
- finder_.UnWarpContour(contour);
- std::vector<Segment<2>> polygon = finder_.FillPolygon(contour, verbose);
- if (polygon.empty()) {
- } else {
+ ContourNode *contour = target_finder.GetContour(blob);
+ target_finder.UnWarpContour(contour);
+ const ::std::vector<Segment<2>> polygon =
+ target_finder.FillPolygon(contour, verbose);
+ if (!polygon.empty()) {
raw_polys.push_back(polygon);
}
}
// Calculate each component side of a possible target.
- std::vector<TargetComponent> target_component_list =
- finder_.FillTargetComponentList(raw_polys);
+ const ::std::vector<TargetComponent> target_component_list =
+ target_finder.FillTargetComponentList(raw_polys);
// Put the compenents together into targets.
- std::vector<Target> target_list =
- finder_.FindTargetsFromComponents(target_component_list, verbose);
+ const ::std::vector<Target> target_list =
+ target_finder.FindTargetsFromComponents(target_component_list, verbose);
- // Use the solver to generate an intermediate version of our results.
- std::vector<IntermediateResult> results;
+ // Now, iterate over the targets (in pixel space). Generate a residual
+ // block for each valid target which compares the actual pixel locations
+ // with the expected pixel locations given the intrinsics and extrinsics.
for (const Target &target : target_list) {
- ::std::array<aos::vision::Vector<2>, 8> target_value =
+ const ::std::array<::aos::vision::Vector<2>, 8> target_value =
target.ToPointList();
- ::std::array<aos::vision::Vector<2>, 8> template_value =
- finder_.GetTemplateTarget().ToPointList();
+ const ::std::array<::aos::vision::Vector<2>, 8> template_value =
+ target_finder.GetTemplateTarget().ToPointList();
- // TODO(austin): Memory leak below, fix.
- double *extrinsics = new double[ExtrinsicParams::kNumParams];
+ all_extrinsics.push_back(
+ {::std::unique_ptr<double[]>(new double[ExtrinsicParams::kNumParams]),
+ i});
+ double *extrinsics = all_extrinsics.back().extrinsics.get();
ExtrinsicParams().set(extrinsics);
- all_extrinsics.push_back({std::unique_ptr<double[]>(extrinsics), i});
for (size_t j = 0; j < 8; ++j) {
- aos::vision::Vector<2> temp = template_value[j];
- aos::vision::Vector<2> targ = target_value[j];
+ // Template target in target space as documented by GetTemplateTarget()
+ const ::aos::vision::Vector<2> template_point = template_value[j];
+ // Target in pixel space.
+ const ::aos::vision::Vector<2> target_point = target_value[j];
- auto ftor = [temp, targ, i](const double *const intrinsics,
- const double *const extrinsics,
- double *residual) {
- auto in = IntrinsicParams::get(intrinsics);
- auto extrinsic_params = ExtrinsicParams::get(extrinsics);
- auto pt = targ - Project(temp, in, extrinsic_params);
- residual[0] = pt.x();
- residual[1] = pt.y();
+ // Now build up the residual block.
+ auto ftor = [template_point, target_point, i](
+ const double *const intrinsics, const double *const extrinsics,
+ double *residual) {
+ const IntrinsicParams intrinsic_params =
+ IntrinsicParams::get(intrinsics);
+ const ExtrinsicParams extrinsic_params =
+ ExtrinsicParams::get(extrinsics);
+ // Project the target location into pixel coordinates.
+ const ::aos::vision::Vector<2> projected_point =
+ Project(template_point, intrinsic_params, extrinsic_params);
+ const ::aos::vision::Vector<2> residual_point =
+ target_point - projected_point;
+ residual[0] = residual_point.x();
+ residual[1] = residual_point.y();
return true;
};
problem.AddResidualBlock(
new NumericDiffCostFunction<decltype(ftor), CENTRAL, 2,
IntrinsicParams::kNumParams,
ExtrinsicParams::kNumParams>(
- new decltype(ftor)(std::move(ftor))),
+ new decltype(ftor)(::std::move(ftor))),
NULL, &intrinsics[0], extrinsics);
}
+ // Now, compare the estimated location of the target that we just solved
+ // for with the extrinsic params above with the known location of the
+ // target.
auto ftor = [&info, i](const double *const extrinsics,
const double *const geometry, double *residual) {
- std::array<double, 3> err = GetError(info, extrinsics, geometry, i);
- residual[0] = 32.0 * err[0];
- residual[1] = 32.0 * err[1];
- residual[2] = 32.0 * err[2];
+ const ::std::array<double, 3> err =
+ GetError(info, extrinsics, geometry, i);
+ // We care a lot more about geometry than pixels. Especially since
+ // pixels are small and meters are big, so there's a small penalty to
+ // being off by meters.
+ residual[0] = err[0] * 1259.0;
+ residual[1] = err[1] * 1259.0;
+ residual[2] = err[2] * 1259.0;
return true;
};
@@ -199,7 +244,7 @@
new NumericDiffCostFunction<decltype(ftor), CENTRAL, 3,
ExtrinsicParams::kNumParams,
CameraGeometry::kNumParams>(
- new decltype(ftor)(std::move(ftor))),
+ new decltype(ftor)(::std::move(ftor))),
NULL, extrinsics, &geometry[0]);
}
}
@@ -209,41 +254,41 @@
Solve(options, &problem, &summary);
{
- std::cout << summary.BriefReport() << "\n";
+ ::std::cout << summary.BriefReport() << ::std::endl;
IntrinsicParams intrinsics_ = IntrinsicParams::get(&intrinsics[0]);
CameraGeometry geometry_ = CameraGeometry::get(&geometry[0]);
- std::cout << "rup = " << intrinsics_.mount_angle * 180 / M_PI << ";\n";
- std::cout << "fl = " << intrinsics_.focal_length << ";\n";
- std::cout << "error = " << summary.final_cost << ";\n";
+ ::std::cout << "rup = " << intrinsics_.mount_angle * 180 / M_PI << ";"
+ << ::std::endl;
+ ::std::cout << "fl = " << intrinsics_.focal_length << ";" << ::std::endl;
+ ::std::cout << "error = " << summary.final_cost << ";" << ::std::endl;
- std::cout << "camera_angle = " << geometry_.heading * 180 / M_PI << "\n";
- std::cout << "camera_x = " << geometry_.location[0] / kInchesToMeters
- << "\n";
- std::cout << "camera_y = " << geometry_.location[1] / kInchesToMeters
- << "\n";
- std::cout << "camera_z = " << geometry_.location[2] / kInchesToMeters
- << "\n";
- std::cout << "camera_barrel = " << intrinsics_.barrel_mount * 180.0 / M_PI
- << "\n";
+ ::std::cout << "camera_angle = " << geometry_.heading * 180 / M_PI
+ << ::std::endl;
+ ::std::cout << "camera_x = " << geometry_.location[0] << ::std::endl;
+ ::std::cout << "camera_y = " << geometry_.location[1] << ::std::endl;
+ ::std::cout << "camera_z = " << geometry_.location[2] << ::std::endl;
+ ::std::cout << "camera_barrel = " << intrinsics_.barrel_mount * 180.0 / M_PI
+ << ::std::endl;
for (const Sample &sample : all_extrinsics) {
- int i = sample.i;
+ const int i = sample.i;
double *data = sample.extrinsics.get();
- ExtrinsicParams extn = ExtrinsicParams::get(data);
+ const ExtrinsicParams extrinsic_params = ExtrinsicParams::get(data);
- auto err = GetError(info, data, &geometry[0], i);
+ const ::std::array<double, 3> error =
+ GetError(info, data, &geometry[0], i);
- std::cout << i << ", ";
- std::cout << extn.z / kInchesToMeters << ", ";
- std::cout << extn.y / kInchesToMeters << ", ";
- std::cout << extn.r1 * 180 / M_PI << ", ";
- std::cout << extn.r2 * 180 / M_PI << ", ";
+ ::std::cout << i << ", ";
+ ::std::cout << extrinsic_params.z << "m, ";
+ ::std::cout << extrinsic_params.y << "m, ";
+ ::std::cout << extrinsic_params.r1 * 180 / M_PI << ", ";
+ ::std::cout << extrinsic_params.r2 * 180 / M_PI << ", ";
// TODO: Methodology problem: This should really have a separate solve for
// extrinsics...
- std::cout << err[0] << ", ";
- std::cout << err[1] << ", ";
- std::cout << err[2] << "\n";
+ ::std::cout << error[0] << "m, ";
+ ::std::cout << error[1] << "m, ";
+ ::std::cout << error[2] << "m" << ::std::endl;
}
}
@@ -251,7 +296,7 @@
results.dataset = info;
results.intrinsics = IntrinsicParams::get(&intrinsics[0]);
results.geometry = CameraGeometry::get(&geometry[0]);
- DumpCameraConstants("y2019/vision/constants.cc", info.camera_id, results);
+ DumpCameraConstants(FLAGS_constants.c_str(), info.camera_id, results);
}
} // namespace y2019
diff --git a/y2019/vision/target_finder.cc b/y2019/vision/target_finder.cc
index d39764a..45678eb 100644
--- a/y2019/vision/target_finder.cc
+++ b/y2019/vision/target_finder.cc
@@ -7,7 +7,7 @@
namespace y2019 {
namespace vision {
-TargetFinder::TargetFinder() { target_template_ = Target::MakeTemplate(); }
+TargetFinder::TargetFinder() : target_template_(Target::MakeTemplate()) {}
aos::vision::RangeImage TargetFinder::Threshold(aos::vision::ImagePtr image) {
const uint8_t threshold_value = GetThresholdValue();
diff --git a/y2019/vision/target_finder.h b/y2019/vision/target_finder.h
index 666ae2f..1ba79ba 100644
--- a/y2019/vision/target_finder.h
+++ b/y2019/vision/target_finder.h
@@ -73,7 +73,7 @@
aos::vision::AnalysisAllocator alloc_;
// The template for the default target in the standard space.
- Target target_template_;
+ const Target target_template_;
IntrinsicParams intrinsics_;
diff --git a/y2019/vision/target_geometry.cc b/y2019/vision/target_geometry.cc
index 1dd6a1a..d4b8a54 100644
--- a/y2019/vision/target_geometry.cc
+++ b/y2019/vision/target_geometry.cc
@@ -124,20 +124,18 @@
Target Project(const Target &target, const IntrinsicParams &intrinsics,
const ExtrinsicParams &extrinsics) {
- auto project = [&](Vector<2> pt) {
- return Project(pt, intrinsics, extrinsics);
- };
Target new_targ;
new_targ.right.is_right = true;
- new_targ.right.top = project(target.right.top);
- new_targ.right.inside = project(target.right.inside);
- new_targ.right.bottom = project(target.right.bottom);
- new_targ.right.outside = project(target.right.outside);
+ new_targ.right.top = Project(target.right.top, intrinsics, extrinsics);
+ new_targ.right.inside = Project(target.right.inside, intrinsics, extrinsics);
+ new_targ.right.bottom = Project(target.right.bottom, intrinsics, extrinsics);
+ new_targ.right.outside =
+ Project(target.right.outside, intrinsics, extrinsics);
- new_targ.left.top = project(target.left.top);
- new_targ.left.inside = project(target.left.inside);
- new_targ.left.bottom = project(target.left.bottom);
- new_targ.left.outside = project(target.left.outside);
+ new_targ.left.top = Project(target.left.top, intrinsics, extrinsics);
+ new_targ.left.inside = Project(target.left.inside, intrinsics, extrinsics);
+ new_targ.left.bottom = Project(target.left.bottom, intrinsics, extrinsics);
+ new_targ.left.outside = Project(target.left.outside, intrinsics, extrinsics);
return new_targ;
}
diff --git a/y2019/vision/target_types.h b/y2019/vision/target_types.h
index d662316..3174b90 100644
--- a/y2019/vision/target_types.h
+++ b/y2019/vision/target_types.h
@@ -38,6 +38,9 @@
TargetComponent left;
TargetComponent right;
+ // Returns a target. The resulting target is in meters with 0, 0 centered
+ // between the upper inner corners of the two pieces of tape, y being up and x
+ // being to the right.
static Target MakeTemplate();
// Get the points in some order (will match against the template).
std::array<aos::vision::Vector<2>, 8> ToPointList() const;
@@ -72,6 +75,7 @@
return out;
}
};
+
// Projects a point from idealized template space to camera space.
aos::vision::Vector<2> Project(aos::vision::Vector<2> pt,
const IntrinsicParams &intrinsics,
diff --git a/y2019/wpilib_interface.cc b/y2019/wpilib_interface.cc
index 2b21eb8..2a21274 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
@@ -523,6 +525,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;
}
}
@@ -541,7 +596,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 {