Merge "Add constants for camera 16 and 19 (code bot 1 front and back)"
diff --git a/y2019/BUILD b/y2019/BUILD
index f00f5ba..3fae9e5 100644
--- a/y2019/BUILD
+++ b/y2019/BUILD
@@ -72,6 +72,7 @@
"//frc971/wpilib:wpilib_robot_base",
"//third_party:phoenix",
"//third_party:wpilib",
+ "//y2019/control_loops/drivetrain:camera_queue",
"//y2019/control_loops/superstructure:superstructure_queue",
"//y2019/jevois:spi",
],
diff --git a/y2019/control_loops/drivetrain/BUILD b/y2019/control_loops/drivetrain/BUILD
index 677726e..de52215 100644
--- a/y2019/control_loops/drivetrain/BUILD
+++ b/y2019/control_loops/drivetrain/BUILD
@@ -88,6 +88,7 @@
srcs = [
"camera.q",
],
+ visibility = ["//visibility:public"],
)
cc_library(
diff --git a/y2019/control_loops/drivetrain/camera.q b/y2019/control_loops/drivetrain/camera.q
index 5add759..6ef6f49 100644
--- a/y2019/control_loops/drivetrain/camera.q
+++ b/y2019/control_loops/drivetrain/camera.q
@@ -1,7 +1,6 @@
package y2019.control_loops.drivetrain;
-// These structures have a nearly one-to-one correspondence to those in
-// //y2019/jevois:structures.h. Please refer to that file for details.
+// See the Target structure in //y2019/jevois:structures.h for documentation.
struct CameraTarget {
float distance;
float height;
@@ -10,17 +9,17 @@
};
message CameraFrame {
- // monotonic time in nanoseconds at which frame was taken (note structure.h
- // uses age).
+ // Number of nanoseconds since the aos::monotonic_clock epoch at which this
+ // frame was captured.
int64_t timestamp;
// Number of targets actually in this frame.
uint8_t num_targets;
- // Buffer for the targets
+ // Buffer for the targets.
CameraTarget[3] targets;
- // Index of the camera with which this frame was taken:
+ // Index of the camera position (not serial number) which this frame is from.
uint8_t camera;
};
diff --git a/y2019/jevois/BUILD b/y2019/jevois/BUILD
index 3f5890a..afca4ef 100644
--- a/y2019/jevois/BUILD
+++ b/y2019/jevois/BUILD
@@ -246,6 +246,7 @@
"//motors/peripheral:uart",
"//motors/print:usb",
"//third_party/GSL",
+ "//y2019/vision:constants",
],
)
diff --git a/y2019/jevois/spi.cc b/y2019/jevois/spi.cc
index b6e6632..a987ee7 100644
--- a/y2019/jevois/spi.cc
+++ b/y2019/jevois/spi.cc
@@ -32,9 +32,10 @@
// 2. 8 bits distance
// 3. 6 bits skew
// 4. 6 bits height
-// 5. 1 bit target valid (a present frame has all-valid targets)
-// 6. 1 bit target present (a present frame can have from 0 to 3
-// targets, depending on how many were found)
+// 5. 2 bits of quantity+index
+// The 6 bits of quantity+index (between all three targets) are:
+// 1. 4 bits camera index + 1 (0 means this isn't a valid frame)
+// 2. 2 bits target count
// Note that empty frames are still sent to indicate that the camera is
// still working even though it doesn't see any targets.
@@ -110,26 +111,29 @@
integer);
}
-constexpr int valid_bits() { return 1; }
-constexpr int valid_offset() { return height_offset() + height_bits(); }
-void valid_pack(bool valid, gsl::span<char> destination) {
- aos::PackBits<uint32_t, valid_bits(), valid_offset()>(valid, destination);
+constexpr int quantity_index_offset() { return height_offset() + height_bits(); }
+void camera_index_pack(int camera_index, gsl::span<char> destination) {
+ aos::PackBits<uint32_t, 2, quantity_index_offset()>(camera_index & 3,
+ destination);
+ aos::PackBits<uint32_t, 2, quantity_index_offset() + 32>(
+ (camera_index >> 2) & 3, destination);
}
-bool valid_unpack(gsl::span<const char> source) {
- return aos::UnpackBits<uint32_t, valid_bits(), valid_offset()>(source);
+int camera_index_unpack(gsl::span<const char> source) {
+ int result = 0;
+ result |= aos::UnpackBits<uint32_t, 2, quantity_index_offset()>(source);
+ result |= aos::UnpackBits<uint32_t, 2, quantity_index_offset() + 32>(source)
+ << 2;
+ return result;
+}
+void target_count_pack(int target_count, gsl::span<char> destination) {
+ aos::PackBits<uint32_t, 2, quantity_index_offset() + 32 * 2>(target_count,
+ destination);
+}
+int target_count_unpack(gsl::span<const char> source) {
+ return aos::UnpackBits<uint32_t, 2, quantity_index_offset() + 32 * 2>(source);
}
-constexpr int present_bits() { return 1; }
-constexpr int present_offset() { return valid_offset() + valid_bits(); }
-void present_pack(bool present, gsl::span<char> destination) {
- aos::PackBits<uint32_t, present_bits(), present_offset()>(present,
- destination);
-}
-bool present_unpack(gsl::span<const char> source) {
- return aos::UnpackBits<uint32_t, present_bits(), present_offset()>(source);
-}
-
-constexpr int next_offset() { return present_offset() + present_bits(); }
+constexpr int next_offset() { return quantity_index_offset() + 2; }
static_assert(next_offset() <= 32, "Target is too big");
} // namespace
@@ -138,14 +142,17 @@
SpiTransfer transfer;
gsl::span<char> remaining_space = transfer;
for (int frame = 0; frame < 3; ++frame) {
- for (int target = 0; target < 3; ++target) {
- remaining_space[0] = 0;
- remaining_space[1] = 0;
- remaining_space[2] = 0;
- remaining_space[3] = 0;
+ // Zero out all three targets and the age.
+ for (int i = 0; i < 3 * 4 + 1; ++i) {
+ remaining_space[i] = 0;
+ }
- if (static_cast<int>(message.frames.size()) > frame) {
- valid_pack(true, remaining_space);
+ if (static_cast<int>(message.frames.size()) > frame) {
+ camera_index_pack(message.frames[frame].camera_index + 1,
+ remaining_space);
+ target_count_pack(message.frames[frame].targets.size(), remaining_space);
+
+ for (int target = 0; target < 3; ++target) {
if (static_cast<int>(message.frames[frame].targets.size()) > target) {
heading_pack(message.frames[frame].targets[target].heading,
remaining_space);
@@ -155,23 +162,16 @@
remaining_space);
height_pack(message.frames[frame].targets[target].height,
remaining_space);
- present_pack(true, remaining_space);
- } else {
- present_pack(false, remaining_space);
}
- } else {
- valid_pack(false, remaining_space);
+ remaining_space = remaining_space.subspan(4);
}
- remaining_space = remaining_space.subspan(4);
- }
- if (static_cast<int>(message.frames.size()) > frame) {
const uint8_t age_count = message.frames[frame].age.count();
memcpy(&remaining_space[0], &age_count, 1);
+ remaining_space = remaining_space.subspan(1);
} else {
- remaining_space[0] = 0;
+ remaining_space = remaining_space.subspan(4 * 3 + 1);
}
- remaining_space = remaining_space.subspan(1);
}
{
uint16_t crc = jevois_crc_init();
@@ -191,13 +191,14 @@
TeensyToRoborio message;
gsl::span<const char> remaining_input = transfer;
for (int frame = 0; frame < 3; ++frame) {
- const bool have_frame = valid_unpack(remaining_input);
- if (have_frame) {
+ const int camera_index_plus = camera_index_unpack(remaining_input);
+ if (camera_index_plus > 0) {
message.frames.push_back({});
- }
- for (int target = 0; target < 3; ++target) {
- if (present_unpack(remaining_input)) {
- if (have_frame) {
+ message.frames.back().camera_index = camera_index_plus - 1;
+
+ const int target_count = target_count_unpack(remaining_input);
+ for (int target = 0; target < 3; ++target) {
+ if (target < target_count) {
message.frames.back().targets.push_back({});
message.frames.back().targets.back().heading =
heading_unpack(remaining_input);
@@ -208,16 +209,18 @@
message.frames.back().targets.back().height =
height_unpack(remaining_input);
}
+ remaining_input = remaining_input.subspan(4);
}
- remaining_input = remaining_input.subspan(4);
+ {
+ uint8_t age_count;
+ memcpy(&age_count, &remaining_input[0], 1);
+ message.frames.back().age = camera_duration(age_count);
+ }
+ remaining_input = remaining_input.subspan(1);
+ } else {
+ remaining_input = remaining_input.subspan(4 * 3 + 1);
}
- if (have_frame) {
- uint8_t age_count;
- memcpy(&age_count, &remaining_input[0], 1);
- message.frames.back().age = camera_duration(age_count);
- }
- remaining_input = remaining_input.subspan(1);
}
{
uint16_t calculated_crc = jevois_crc_init();
diff --git a/y2019/jevois/spi_test.cc b/y2019/jevois/spi_test.cc
index 32551db..84938f0 100644
--- a/y2019/jevois/spi_test.cc
+++ b/y2019/jevois/spi_test.cc
@@ -60,10 +60,13 @@
TeensyToRoborio input_message;
input_message.frames.push_back({});
input_message.frames.back().age = camera_duration(9);
+ input_message.frames.back().camera_index = 2;
input_message.frames.push_back({});
input_message.frames.back().age = camera_duration(7);
+ input_message.frames.back().camera_index = 5;
input_message.frames.push_back({});
input_message.frames.back().age = camera_duration(1);
+ input_message.frames.back().camera_index = 4;
const SpiTransfer transfer = SpiPackToRoborio(input_message);
const auto output_message = SpiUnpackToRoborio(transfer);
@@ -80,13 +83,16 @@
input_message.frames.back().targets.back().height = 1;
input_message.frames.back().targets.back().heading = 0.5;
input_message.frames.back().targets.back().skew = -0.5;
+ input_message.frames.back().camera_index = 0;
input_message.frames.push_back({});
input_message.frames.back().targets.push_back({});
input_message.frames.back().targets.push_back({});
+ input_message.frames.back().camera_index = 2;
input_message.frames.push_back({});
input_message.frames.back().targets.push_back({});
input_message.frames.back().targets.push_back({});
input_message.frames.back().targets.push_back({});
+ input_message.frames.back().camera_index = 3;
const SpiTransfer transfer = SpiPackToRoborio(input_message);
const auto output_message = SpiUnpackToRoborio(transfer);
@@ -103,6 +109,93 @@
output_message->frames.back().targets.back().heading, 0.1);
EXPECT_NEAR(input_message.frames.back().targets.back().skew,
output_message->frames.back().targets.back().skew, 0.1);
+ for (int i = 0; i < 3; ++i) {
+ EXPECT_EQ(input_message.frames[i].camera_index,
+ output_message->frames[i].camera_index);
+ }
+}
+
+// Tests that packing and unpacking two targets results in the same number on
+// the other side.
+TEST(SpiToRoborioPackTest, TwoTargets) {
+ TeensyToRoborio input_message;
+ input_message.frames.push_back({});
+ input_message.frames.back().targets.push_back({});
+ input_message.frames.back().targets.back().distance = 9;
+ input_message.frames.back().targets.back().height = 1;
+ input_message.frames.back().targets.back().heading = 0.5;
+ input_message.frames.back().targets.back().skew = -0.5;
+ input_message.frames.back().targets.push_back({});
+ input_message.frames.back().targets.back().distance = 1;
+ input_message.frames.back().targets.back().height = 0.9;
+ input_message.frames.back().targets.back().heading = 0.4;
+ input_message.frames.back().targets.back().skew = -0.4;
+ input_message.frames.back().age = camera_duration(9);
+ input_message.frames.back().camera_index = 2;
+
+ const SpiTransfer transfer = SpiPackToRoborio(input_message);
+ const auto output_message = SpiUnpackToRoborio(transfer);
+ ASSERT_TRUE(output_message);
+ ASSERT_EQ(1u, output_message->frames.size());
+ ASSERT_EQ(2u, output_message->frames[0].targets.size());
+ for (int i = 0; i < 2; ++i) {
+ EXPECT_NEAR(input_message.frames.back().targets[i].distance,
+ output_message->frames.back().targets[i].distance, 0.1);
+ EXPECT_NEAR(input_message.frames.back().targets[i].height,
+ output_message->frames.back().targets[i].height, 0.1);
+ EXPECT_NEAR(input_message.frames.back().targets[i].heading,
+ output_message->frames.back().targets[i].heading, 0.1);
+ EXPECT_NEAR(input_message.frames.back().targets[i].skew,
+ output_message->frames.back().targets[i].skew, 0.1);
+ EXPECT_EQ(input_message.frames.back().age,
+ output_message->frames.back().age);
+ EXPECT_EQ(input_message.frames.back().camera_index,
+ output_message->frames.back().camera_index);
+ }
+}
+
+// Tests that packing and unpacking three targets results in the same number on
+// the other side.
+TEST(SpiToRoborioPackTest, ThreeTargets) {
+ TeensyToRoborio input_message;
+ input_message.frames.push_back({});
+ input_message.frames.back().targets.push_back({});
+ input_message.frames.back().targets.back().distance = 9;
+ input_message.frames.back().targets.back().height = 1;
+ input_message.frames.back().targets.back().heading = 0.5;
+ input_message.frames.back().targets.back().skew = -0.5;
+ input_message.frames.back().targets.push_back({});
+ input_message.frames.back().targets.back().distance = 1;
+ input_message.frames.back().targets.back().height = 0.9;
+ input_message.frames.back().targets.back().heading = 0.4;
+ input_message.frames.back().targets.back().skew = -0.4;
+ input_message.frames.back().targets.push_back({});
+ input_message.frames.back().targets.back().distance = 2;
+ input_message.frames.back().targets.back().height = 0.7;
+ input_message.frames.back().targets.back().heading = 0.3;
+ input_message.frames.back().targets.back().skew = -0.3;
+ input_message.frames.back().age = camera_duration(1);
+ input_message.frames.back().camera_index = 1;
+
+ const SpiTransfer transfer = SpiPackToRoborio(input_message);
+ const auto output_message = SpiUnpackToRoborio(transfer);
+ ASSERT_TRUE(output_message);
+ ASSERT_EQ(1u, output_message->frames.size());
+ ASSERT_EQ(3u, output_message->frames[0].targets.size());
+ for (int i = 0; i < 3; ++i) {
+ EXPECT_NEAR(input_message.frames.back().targets[i].distance,
+ output_message->frames.back().targets[i].distance, 0.1);
+ EXPECT_NEAR(input_message.frames.back().targets[i].height,
+ output_message->frames.back().targets[i].height, 0.1);
+ EXPECT_NEAR(input_message.frames.back().targets[i].heading,
+ output_message->frames.back().targets[i].heading, 0.1);
+ EXPECT_NEAR(input_message.frames.back().targets[i].skew,
+ output_message->frames.back().targets[i].skew, 0.1);
+ EXPECT_EQ(input_message.frames.back().age,
+ output_message->frames.back().age);
+ EXPECT_EQ(input_message.frames.back().camera_index,
+ output_message->frames.back().camera_index);
+ }
}
// Tests packing and then unpacking an empty message.
diff --git a/y2019/jevois/structures.h b/y2019/jevois/structures.h
index 1f10dc0..676b46c 100644
--- a/y2019/jevois/structures.h
+++ b/y2019/jevois/structures.h
@@ -85,8 +85,8 @@
// The information extracted from a single camera frame.
//
// This is all the information sent from each camera to the Teensy.
-struct Frame {
- bool operator==(const Frame &other) const {
+struct CameraFrame {
+ bool operator==(const CameraFrame &other) const {
if (other.targets != targets) {
return false;
}
@@ -95,7 +95,7 @@
}
return true;
}
- bool operator!=(const Frame &other) const {
+ bool operator!=(const CameraFrame &other) const {
return !(*this == other);
}
@@ -106,6 +106,34 @@
camera_duration age;
};
+// The information extracted from a single camera frame, from a given camera.
+struct RoborioFrame {
+ bool operator==(const RoborioFrame &other) const {
+ if (other.targets != targets) {
+ return false;
+ }
+ if (other.age != age) {
+ return false;
+ }
+ if (other.camera_index != camera_index) {
+ return false;
+ }
+ return true;
+ }
+ bool operator!=(const RoborioFrame &other) const {
+ return !(*this == other);
+ }
+
+ // The top most interesting targets found in this frame.
+ aos::SizedArray<Target, 3> targets;
+
+ // How long ago from the current time this frame was captured.
+ camera_duration age;
+ // Which camera this is from (which position on the robot, not a serial
+ // number).
+ int camera_index;
+};
+
enum class CameraCommand : char {
// Stay in normal mode.
kNormal,
@@ -113,6 +141,9 @@
kCameraPassthrough,
// Go to being a useful USB device.
kUsb,
+ // Send As, which triggers the bootstrap script to drop directly into USB
+ // mode.
+ kAs,
};
// This is all the information sent from the Teensy to each camera.
@@ -170,7 +201,7 @@
// The newest frames received from up to three cameras. These will be the
// three earliest-received of all buffered frames.
- aos::SizedArray<Frame, 3> frames;
+ aos::SizedArray<RoborioFrame, 3> frames;
};
// This is all the information the RoboRIO sends to the Teensy.
@@ -182,6 +213,12 @@
if (other.light_rings != light_rings) {
return false;
}
+ if (other.realtime_now != realtime_now) {
+ return false;
+ }
+ if (other.camera_command != camera_command) {
+ return false;
+ }
return true;
}
bool operator!=(const RoborioToTeensy &other) const {
diff --git a/y2019/jevois/teensy.cc b/y2019/jevois/teensy.cc
index 539d7b6..30cfc81 100644
--- a/y2019/jevois/teensy.cc
+++ b/y2019/jevois/teensy.cc
@@ -1,3 +1,6 @@
+#include <inttypes.h>
+#include <stdio.h>
+
#include "aos/time/time.h"
#include "motors/core/kinetis.h"
#include "motors/core/time.h"
@@ -10,6 +13,7 @@
#include "y2019/jevois/cobs.h"
#include "y2019/jevois/spi.h"
#include "y2019/jevois/uart.h"
+#include "y2019/vision/constants.h"
using frc971::teensy::InterruptBufferedUart;
using frc971::teensy::InterruptBufferedSpi;
@@ -87,6 +91,12 @@
buffer.PushSingle(0);
}
}
+
+ void FillAs() {
+ while (!buffer.full()) {
+ buffer.PushSingle('a');
+ }
+ }
};
InterruptBufferedSpi *global_spi_instance = nullptr;
@@ -269,9 +279,10 @@
FrameQueue(const FrameQueue &) = delete;
FrameQueue &operator=(const FrameQueue &) = delete;
- void UpdateFrame(int camera, const Frame &frame) {
+ void UpdateFrame(int camera, const CameraFrame &frame) {
frames_[camera].targets = frame.targets;
frames_[camera].capture_time = aos::monotonic_clock::now() - frame.age;
+ frames_[camera].camera_index = camera;
const aos::SizedArray<int, 3> old_last_frames = last_frames_;
last_frames_.clear();
for (int index : old_last_frames) {
@@ -302,6 +313,7 @@
aos::SizedArray<Target, 3> targets;
aos::monotonic_clock::time_point capture_time =
aos::monotonic_clock::min_time;
+ int camera_index;
};
std::array<FrameData, 5> frames_;
@@ -327,7 +339,7 @@
const FrameData &frame = frames_[index];
const auto age = aos::monotonic_clock::now() - frame.capture_time;
const auto rounded_age = aos::time::round<camera_duration>(age);
- message.frames.push_back({frame.targets, rounded_age});
+ message.frames.push_back({frame.targets, rounded_age, frame.camera_index});
last_frames_.push_back(index);
}
return SpiPackToRoborio(message);
@@ -366,11 +378,31 @@
aos::monotonic_clock::time_point last_cycle_start_ =
aos::monotonic_clock::min_time;
- aos::monotonic_clock::duration next_off_time_ = std::chrono::milliseconds(100);
+ aos::monotonic_clock::duration next_off_time_ =
+ std::chrono::milliseconds(100);
aos::monotonic_clock::time_point current_off_point_ =
aos::monotonic_clock::min_time;
};
+// Returns an identifier for the processor we're running on.
+uint32_t ProcessorIdentifier() {
+ uint32_t r = 0;
+ r |= SIM_UIDH << 24;
+ r |= SIM_UIDMH << 16;
+ r |= SIM_UIDML << 8;
+ r |= SIM_UIDL << 0;
+ return r;
+}
+
+std::array<int, 5> CameraSerialNumbers() {
+ switch (ProcessorIdentifier()) {
+ case 0xffff322e: // CODE bot
+ return {{0, 0, 0, 16, 19}};
+ default:
+ return {{0, 0, 0, 0, 0}};
+ }
+}
+
extern "C" {
void *__stack_chk_guard = (void *)0x67111971;
@@ -658,7 +690,6 @@
UartUnpackToTeensy(packetizers[i].received_packet());
packetizers[i].clear_received_packet();
if (decoded) {
- printf("got one with %d\n", (int)decoded->targets.size());
frame_queue.UpdateFrame(i, *decoded);
}
}
@@ -681,30 +712,42 @@
}
{
const auto now = aos::monotonic_clock::now();
- if (last_camera_send + std::chrono::milliseconds(1000) < now) {
- last_camera_send = now;
- CameraCalibration calibration{};
- calibration.teensy_now = aos::monotonic_clock::now();
- calibration.realtime_now = aos::realtime_clock::min_time;
- if (last_roborio_camera_command != CameraCommand::kNormal) {
- calibration.camera_command = last_roborio_camera_command;
- } else {
- calibration.camera_command = stdin_camera_command;
+ CameraCommand current_camera_command = CameraCommand::kNormal;
+ if (last_roborio_camera_command != CameraCommand::kNormal) {
+ current_camera_command = last_roborio_camera_command;
+ } else {
+ current_camera_command = stdin_camera_command;
+ }
+ if (current_camera_command == CameraCommand::kUsb) {
+ debug_light.set_next_off_time(std::chrono::milliseconds(900));
+ } else if (current_camera_command == CameraCommand::kCameraPassthrough) {
+ debug_light.set_next_off_time(std::chrono::milliseconds(500));
+ } else {
+ debug_light.set_next_off_time(std::chrono::milliseconds(100));
+ }
+
+ if (current_camera_command == CameraCommand::kAs) {
+ for (size_t i = 0; i < transmit_buffers.size(); ++i) {
+ transmit_buffers[i].FillAs();
}
- if (calibration.camera_command == CameraCommand::kUsb) {
- debug_light.set_next_off_time(std::chrono::milliseconds(900));
- } else if (calibration.camera_command ==
- CameraCommand::kCameraPassthrough) {
- debug_light.set_next_off_time(std::chrono::milliseconds(500));
- } else {
- debug_light.set_next_off_time(std::chrono::milliseconds(100));
+ } else {
+ if (last_camera_send + std::chrono::milliseconds(1000) < now) {
+ last_camera_send = now;
+ CameraCalibration calibration{};
+ calibration.teensy_now = aos::monotonic_clock::now();
+ calibration.realtime_now = aos::realtime_clock::min_time;
+ calibration.camera_command = current_camera_command;
+
+ for (int i = 0; i < 5; ++i) {
+ const y2019::vision::CameraCalibration *const constants =
+ y2019::vision::GetCamera(CameraSerialNumbers()[i]);
+ (void)constants;
+ calibration.calibration(0, 0) = constants->intrinsics.mount_angle;
+ calibration.calibration(0, 1) = constants->intrinsics.focal_length;
+ calibration.calibration(0, 2) = constants->intrinsics.barrel_mount;
+ transmit_buffers[i].MaybeWritePacket(calibration);
+ }
}
- // TODO(Brian): Actually fill out the calibration field.
- transmit_buffers[0].MaybeWritePacket(calibration);
- transmit_buffers[1].MaybeWritePacket(calibration);
- transmit_buffers[2].MaybeWritePacket(calibration);
- transmit_buffers[3].MaybeWritePacket(calibration);
- transmit_buffers[4].MaybeWritePacket(calibration);
}
for (TransmitBuffer &transmit_buffer : transmit_buffers) {
transmit_buffer.Tick(now);
@@ -716,17 +759,36 @@
if (!stdin_data.empty()) {
switch (stdin_data.back()) {
case 'p':
- printf("Entering passthrough mode\n");
+ printf("Sending passthrough mode\n");
stdin_camera_command = CameraCommand::kCameraPassthrough;
break;
case 'u':
- printf("Entering USB mode\n");
+ printf("Sending USB mode\n");
stdin_camera_command = CameraCommand::kUsb;
break;
case 'n':
- printf("Entering normal mode\n");
+ printf("Sending normal mode\n");
stdin_camera_command = CameraCommand::kNormal;
break;
+ case 'a':
+ printf("Sending all 'a's\n");
+ stdin_camera_command = CameraCommand::kAs;
+ break;
+ case 'c':
+ printf("This UART board is 0x%" PRIx32 "\n", ProcessorIdentifier());
+ for (int i = 0; i < 5; ++i) {
+ printf("Camera slot %d's serial number is %d\n", i,
+ CameraSerialNumbers()[i]);
+ }
+ break;
+ case 'h':
+ printf("UART board commands:\n");
+ printf(" p: Send passthrough mode\n");
+ printf(" u: Send USB mode\n");
+ printf(" n: Send normal mode\n");
+ printf(" a: Send all-'a' mode\n");
+ printf(" c: Dump camera configuration\n");
+ break;
default:
printf("Unrecognized character\n");
break;
diff --git a/y2019/jevois/uart.cc b/y2019/jevois/uart.cc
index 3621da8..63138a8 100644
--- a/y2019/jevois/uart.cc
+++ b/y2019/jevois/uart.cc
@@ -15,7 +15,7 @@
namespace frc971 {
namespace jevois {
-UartToTeensyBuffer UartPackToTeensy(const Frame &message) {
+UartToTeensyBuffer UartPackToTeensy(const CameraFrame &message) {
std::array<char, uart_to_teensy_size()> buffer;
gsl::span<char> remaining_space = buffer;
remaining_space[0] = message.targets.size();
@@ -55,7 +55,7 @@
return result;
}
-tl::optional<Frame> UartUnpackToTeensy(gsl::span<const char> encoded_buffer) {
+tl::optional<CameraFrame> UartUnpackToTeensy(gsl::span<const char> encoded_buffer) {
std::array<char, uart_to_teensy_size()> buffer;
if (static_cast<size_t>(
CobsDecode<uart_to_teensy_size()>(encoded_buffer, &buffer).size()) !=
@@ -63,7 +63,7 @@
return tl::nullopt;
}
- Frame message;
+ CameraFrame message;
gsl::span<const char> remaining_input = buffer;
const int number_targets = remaining_input[0];
remaining_input = remaining_input.subspan(1);
diff --git a/y2019/jevois/uart.h b/y2019/jevois/uart.h
index 2a4acec..b9e784b 100644
--- a/y2019/jevois/uart.h
+++ b/y2019/jevois/uart.h
@@ -29,8 +29,8 @@
using UartToCameraBuffer =
aos::SizedArray<char, CobsMaxEncodedSize(uart_to_camera_size())>;
-UartToTeensyBuffer UartPackToTeensy(const Frame &message);
-tl::optional<Frame> UartUnpackToTeensy(gsl::span<const char> buffer);
+UartToTeensyBuffer UartPackToTeensy(const CameraFrame &message);
+tl::optional<CameraFrame> UartUnpackToTeensy(gsl::span<const char> buffer);
UartToCameraBuffer UartPackToCamera(const CameraCalibration &message);
tl::optional<CameraCalibration> UartUnpackToCamera(
diff --git a/y2019/jevois/uart_test.cc b/y2019/jevois/uart_test.cc
index d669688..b8f25c1 100644
--- a/y2019/jevois/uart_test.cc
+++ b/y2019/jevois/uart_test.cc
@@ -10,7 +10,7 @@
// Tests packing and then unpacking a message with arbitrary values.
TEST(UartToTeensyTest, Basic) {
- Frame input_message;
+ CameraFrame input_message;
for (int i = 0; i < 3; ++i) {
input_message.targets.push_back({});
Target *const target = &input_message.targets.back();
@@ -29,7 +29,7 @@
// Tests packing and then unpacking a message with arbitrary values and no
// frames.
TEST(UartToTeensyTest, NoFrames) {
- Frame input_message;
+ CameraFrame input_message;
input_message.age = camera_duration(123);
const UartToTeensyBuffer buffer = UartPackToTeensy(input_message);
const auto output_message = UartUnpackToTeensy(buffer);
@@ -39,7 +39,7 @@
// Tests packing and then unpacking a message with just one frame.
TEST(UartToTeensyTest, OneFrame) {
- Frame input_message;
+ CameraFrame input_message;
{
input_message.targets.push_back({});
Target *const target = &input_message.targets.back();
@@ -75,7 +75,7 @@
// Tests that corrupting the data in various ways is handled properly.
TEST(UartToTeensyTest, CorruptData) {
- Frame input_message{};
+ CameraFrame input_message{};
{
UartToTeensyBuffer buffer = UartPackToTeensy(input_message);
buffer[0]++;
diff --git a/y2019/joystick_reader.cc b/y2019/joystick_reader.cc
index 40c9027..9290a2f 100644
--- a/y2019/joystick_reader.cc
+++ b/y2019/joystick_reader.cc
@@ -71,23 +71,23 @@
const ElevatorWristPosition kPanelForwardLowerPos{0.0, M_PI / 2.0};
const ElevatorWristPosition kPanelBackwardLowerPos{0.0, -M_PI / 2.0};
-const ElevatorWristPosition kPanelForwardMiddlePos{0.7412, M_PI / 2.0};
-const ElevatorWristPosition kPanelBackwardMiddlePos{0.7412, -M_PI / 2.0};
+const ElevatorWristPosition kPanelForwardMiddlePos{0.75, M_PI / 2.0};
+const ElevatorWristPosition kPanelBackwardMiddlePos{0.78, -M_PI / 2.0};
-const ElevatorWristPosition kPanelForwardUpperPos{1.4524, M_PI / 2.0};
-const ElevatorWristPosition kPanelBackwardUpperPos{1.4524, -M_PI / 2.0};
+const ElevatorWristPosition kPanelForwardUpperPos{1.51, M_PI / 2.0};
+const ElevatorWristPosition kPanelBackwardUpperPos{1.50, -M_PI / 2.0};
-const ElevatorWristPosition kBallForwardLowerPos{0.0, 0.98};
-const ElevatorWristPosition kBallBackwardLowerPos{0.607, -2.281};
+const ElevatorWristPosition kBallForwardLowerPos{0.4598, 1.5863};
+const ElevatorWristPosition kBallBackwardLowerPos{0.175, -1.61};
-const ElevatorWristPosition kBallForwardMiddlePos{0.67945, 1.22};
-const ElevatorWristPosition kBallBackwardMiddlePos{0.93345, -1.83};
+const ElevatorWristPosition kBallForwardMiddlePos{1.16, 1.546};
+const ElevatorWristPosition kBallBackwardMiddlePos{0.876021, -1.546};
-const ElevatorWristPosition kBallForwardUpperPos{1.41605, 1.22};
-const ElevatorWristPosition kBallBackwardUpperPos{1.41605, -1.36};
+const ElevatorWristPosition kBallForwardUpperPos{1.50, 0.961};
+const ElevatorWristPosition kBallBackwardUpperPos{1.41, -1.217};
-const ElevatorWristPosition kBallCargoForwardPos{0.73025, M_PI / 2};
-const ElevatorWristPosition kBallCargoBackwardPos{0.80645, -1.92};
+const ElevatorWristPosition kBallCargoForwardPos{0.699044, 1.353};
+const ElevatorWristPosition kBallCargoBackwardPos{0.828265, -1.999};
const ElevatorWristPosition kBallHPIntakeForwardPos{0.340, 0.737};
const ElevatorWristPosition kBallHPIntakeBackwardPos{0.52, -1.1};
diff --git a/y2019/vision/BUILD b/y2019/vision/BUILD
index 0221254..2bdfd9c 100644
--- a/y2019/vision/BUILD
+++ b/y2019/vision/BUILD
@@ -1,6 +1,7 @@
load("//aos/build:queues.bzl", "queue_library")
load("//tools/build_rules:gtk_dependent.bzl", "gtk_dependent_cc_binary", "gtk_dependent_cc_library")
load("@com_google_protobuf//:protobuf.bzl", "cc_proto_library")
+load("//tools:environments.bzl", "mcu_cpus")
package(default_visibility = ["//visibility:public"])
@@ -13,6 +14,8 @@
name = "constants",
srcs = ["constants.cc"],
hdrs = ["constants.h"],
+ compatible_with = mcu_cpus,
+ visibility = ["//visibility:public"],
)
cc_library(
diff --git a/y2019/vision/debug_viewer.cc b/y2019/vision/debug_viewer.cc
index 533dcf6..1db8e33 100644
--- a/y2019/vision/debug_viewer.cc
+++ b/y2019/vision/debug_viewer.cc
@@ -150,15 +150,14 @@
// Use the solver to generate an intermediate version of our results.
std::vector<IntermediateResult> results;
for (const Target &target : target_list) {
- results.emplace_back(finder_.ProcessTargetToResult(target, true));
+ results.emplace_back(finder_.ProcessTargetToResult(target, draw_raw_IR_));
if (draw_raw_IR_) DrawResult(results.back(), {255, 128, 0});
}
// Check that our current results match possible solutions.
- results = finder_.FilterResults(results);
+ results = finder_.FilterResults(results, 0);
if (draw_results_) {
for (const IntermediateResult &res : results) {
- DrawResult(res, {0, 255, 0});
DrawTarget(res, {0, 255, 0});
}
}
@@ -194,7 +193,7 @@
printf(" n: Toggle drawing countours before and after warping.\n");
printf(" m: Toggle drawing raw blob data (may need to change image to toggle a redraw).\n");
printf(" h: Print this message.\n");
- printf(" a: May log camera image to /tmp/debug_viewer_jpeg_<#>.yuyv"
+ printf(" a: May log camera image to /tmp/debug_viewer_jpeg_<#>.yuyv\n");
printf(" q: Exit the application.\n");
} else if (key == 'q') {
printf("User requested shutdown.\n");
diff --git a/y2019/vision/global_calibration.cc b/y2019/vision/global_calibration.cc
index bf64310..94048ad 100644
--- a/y2019/vision/global_calibration.cc
+++ b/y2019/vision/global_calibration.cc
@@ -17,6 +17,8 @@
// CERES Clashes with logging symbols...
#include "ceres/ceres.h"
+DEFINE_int32(camera_id, -1, "The camera ID to calibrate");
+
using ::aos::events::DataSocket;
using ::aos::events::RXUdpSocket;
using ::aos::events::TCPServer;
@@ -74,16 +76,13 @@
}
void main(int argc, char **argv) {
- (void)argc;
- (void)argv;
using namespace y2019::vision;
- // gflags::ParseCommandLineFlags(&argc, &argv, false);
+ gflags::ParseCommandLineFlags(&argc, &argv, false);
- int camera_id = 5;
const char *base_directory = "/home/parker/data/frc/2019_calibration/";
DatasetInfo info = {
- camera_id,
+ FLAGS_camera_id,
{{12.5 * kInchesToMeters, 0.5 * kInchesToMeters}},
{{kInchesToMeters, 0.0}},
26,
@@ -248,7 +247,7 @@
results.dataset = info;
results.intrinsics = IntrinsicParams::get(&intrinsics[0]);
results.geometry = CameraGeometry::get(&geometry[0]);
- DumpCameraConstants(camera_id, results);
+ DumpCameraConstants(info.camera_id, results);
}
} // namespace y2019
diff --git a/y2019/vision/target_finder.cc b/y2019/vision/target_finder.cc
index b46d802..be2e262 100644
--- a/y2019/vision/target_finder.cc
+++ b/y2019/vision/target_finder.cc
@@ -419,13 +419,24 @@
}
std::vector<IntermediateResult> TargetFinder::FilterResults(
- const std::vector<IntermediateResult> &results) {
+ const std::vector<IntermediateResult> &results, uint64_t print_rate) {
std::vector<IntermediateResult> filtered;
for (const IntermediateResult &res : results) {
if (res.solver_error < 75.0) {
filtered.emplace_back(res);
}
}
+ frame_count_++;
+ if (!filtered.empty()) {
+ valid_result_count_++;
+ }
+ if (print_rate > 0 && frame_count_ > print_rate) {
+ LOG(INFO, "Found (%zu / %zu)(%.2f) targets.\n", valid_result_count_,
+ frame_count_, (double)valid_result_count_ / (double)frame_count_);
+ frame_count_ = 0;
+ valid_result_count_ = 0;
+ }
+
return filtered;
}
diff --git a/y2019/vision/target_finder.h b/y2019/vision/target_finder.h
index bdf67f2..4583690 100644
--- a/y2019/vision/target_finder.h
+++ b/y2019/vision/target_finder.h
@@ -49,7 +49,7 @@
IntermediateResult ProcessTargetToResult(const Target &target, bool verbose);
std::vector<IntermediateResult> FilterResults(
- const std::vector<IntermediateResult> &results);
+ const std::vector<IntermediateResult> &results, uint64_t print_rate);
// Get the local overlay for debug if we are doing that.
aos::vision::PixelLinesOverlay *GetOverlay() { return &overlay_; }
@@ -78,6 +78,10 @@
IntrinsicParams intrinsics_;
ExtrinsicParams default_extrinsics_;
+
+ // Counts for logging.
+ size_t frame_count_;
+ size_t valid_result_count_;
};
} // namespace vision
diff --git a/y2019/vision/target_sender.cc b/y2019/vision/target_sender.cc
index 2c7d8e2..06235ff 100644
--- a/y2019/vision/target_sender.cc
+++ b/y2019/vision/target_sender.cc
@@ -289,12 +289,11 @@
TargetFinder finder_;
- // Check that our current results match possible solutions.
aos::vision::CameraParams params0;
- params0.set_exposure(400);
+ params0.set_exposure(50);
params0.set_brightness(40);
params0.set_width(640);
- // params0.set_fps(10);
+ params0.set_fps(15);
params0.set_height(480);
::std::unique_ptr<CameraStream> camera0(
@@ -302,9 +301,11 @@
camera0->set_on_frame([&](DataRef data,
monotonic_clock::time_point monotonic_now) {
aos::vision::ImageFormat fmt{640, 480};
+ // Use threshold from aos::vision. This will run at 15 FPS.
aos::vision::BlobList imgs =
- aos::vision::FindBlobs(::DoThresholdYUYV(fmt, data.data(), 120));
+ aos::vision::FindBlobs(aos::vision::DoThresholdYUYV(fmt, data.data(), 120));
finder_.PreFilter(&imgs);
+ LOG(INFO, "Blobs: (%zu).\n", imgs.size());
bool verbose = false;
std::vector<std::vector<Segment<2>>> raw_polys;
@@ -317,26 +318,31 @@
raw_polys.push_back(polygon);
}
}
+ LOG(INFO, "Polygons: (%zu).\n", raw_polys.size());
// Calculate each component side of a possible target.
std::vector<TargetComponent> target_component_list =
finder_.FillTargetComponentList(raw_polys);
+ LOG(INFO, "Components: (%zu).\n", target_component_list.size());
// Put the compenents together into targets.
std::vector<Target> target_list =
finder_.FindTargetsFromComponents(target_component_list, verbose);
+ LOG(INFO, "Potential Target: (%zu).\n", target_list.size());
// Use the solver to generate an intermediate version of our results.
std::vector<IntermediateResult> results;
for (const Target &target : target_list) {
results.emplace_back(finder_.ProcessTargetToResult(target, verbose));
}
+ LOG(INFO, "Raw Results: (%zu).\n", results.size());
- results = finder_.FilterResults(results);
+ results = finder_.FilterResults(results, 30);
+ LOG(INFO, "Results: (%zu).\n", results.size());
// TODO: Select top 3 (randomly?)
- frc971::jevois::Frame frame{};
+ frc971::jevois::CameraFrame frame{};
for (size_t i = 0; i < results.size() && i < frame.targets.max_size();
++i) {
@@ -390,6 +396,7 @@
switch (calibration.camera_command) {
case CameraCommand::kNormal:
+ case CameraCommand::kAs:
break;
case CameraCommand::kUsb:
return 0;
diff --git a/y2019/vision/tools/deploy.sh b/y2019/vision/tools/deploy.sh
index ff3ee01..3c16b03 100755
--- a/y2019/vision/tools/deploy.sh
+++ b/y2019/vision/tools/deploy.sh
@@ -1,32 +1,38 @@
#!/bin/sh
+set -e
+
echo "Building executables"
readonly BAZEL_OPTIONS="-c opt --cpu=armhf-debian"
readonly BAZEL_BIN="$(bazel info ${BAZEL_OPTIONS} bazel-bin)"
+readonly TARGET_DIR=/media/$USER/JEVOIS
bazel build ${BAZEL_OPTIONS} \
//y2019/vision:target_sender \
//y2019/vision:serial_waiter
-echo "Mount jevois ..."
-./jevois-cmd usbsd
+if [ ! -d "${TARGET_DIR}" ]
+then
+ echo "Mount jevois at ${TARGET_DIR} ..."
+ ./jevois-cmd usbsd
+fi
echo "Waiting for fs ..."
-while [ ! -d /media/$USER/JEVOIS ]
+while [ ! -d "${TARGET_DIR}" ]
do
sleep 1
done
echo "OK"
echo "Copying files ..."
-cp ./austin_cam.sh /media/$USER/JEVOIS/
-cp ./launch.sh /media/$USER/JEVOIS/deploy/
+cp ./austin_cam.sh "${TARGET_DIR}"/
+cp ./launch.sh "${TARGET_DIR}"/deploy/
cp "${BAZEL_BIN}/y2019/vision/target_sender" \
"${BAZEL_BIN}/y2019/vision/serial_waiter" \
- /media/$USER/JEVOIS/deploy/
+ "${TARGET_DIR}"/deploy/
echo "Unmount sd card ..."
-umount /media/$USER/JEVOIS
+umount "${TARGET_DIR}"
echo "OK"
echo "Rebooting Jevois."
diff --git a/y2019/vision/tools/flash.sh b/y2019/vision/tools/flash.sh
index 5daad73..d3c55ae 100755
--- a/y2019/vision/tools/flash.sh
+++ b/y2019/vision/tools/flash.sh
@@ -6,6 +6,19 @@
exit
fi
+echo "Pulling Binaries ..."
+if [ ! -f "./libjevoisbase.so" ]
+then
+ wget http://www.frc971.org/Build-Dependencies/libjevoisbase.so
+fi
+echo "Got libjevoisbase.so"
+
+if [ ! -f "./PassThrough.so" ]
+then
+ wget http://www.frc971.org/Build-Dependencies/PassThrough.so
+fi
+echo "Got PassThrough.so"
+
DEV_BASE=/dev/mmcblk0p
echo "Please check that ${DEV_BASE}3 is the correct drive to format. This is a destructive command so please be sure."
@@ -18,7 +31,7 @@
fi
# need to disable some new features on 17.x
-sudo mkfs.ext4 -L JEVOIS -O ^64bit,uninit_bg,^metadata_csum ${DEV_BASE}3
+sudo mkfs.ext3 -L JEVOIS ${DEV_BASE}3
echo "Mounting JEVOIS."
mkdir -p /tmp/JEVOIS
@@ -30,15 +43,20 @@
echo "Make JEVOIS directories."
mkdir -p /tmp/JEVOIS/packages
-mkdir -p /tmp/JEVOIS/modules
+mkdir -p /tmp/JEVOIS/modules/JeVois/PassThrough/
mkdir -p /tmp/JEVOIS/config
-mkdir -p /tmp/JEVOIS/lib
+mkdir -p /tmp/JEVOIS/lib/JeVois/
+mkdir -p /tmp/JEVOIS/deploy/
+mkdir -p /tmp/JEVOIS/data/
echo "Copy configs."
cp ./austin_cam.sh /tmp/JEVOIS
cp ./videomappings.cfg /tmp/JEVOIS/config/
cp ./rcS_script.txt /tmp/LINUX/etc/init.d/rcS
cp Jevois_fstab /tmp/LINUX/etc/fstab
+cp ./PassThrough.so /tmp/JEVOIS/modules/JeVois/PassThrough/
+cp ./libjevoisbase.so /tmp/JEVOIS/lib/JeVois/
+cp ./launch.sh /tmp/JEVOIS/deploy/
echo "Un-mounting JEVOIS"
sudo umount /tmp/JEVOIS
diff --git a/y2019/wpilib_interface.cc b/y2019/wpilib_interface.cc
index 02269eb..564e8e1 100644
--- a/y2019/wpilib_interface.cc
+++ b/y2019/wpilib_interface.cc
@@ -28,6 +28,7 @@
#include "aos/util/log_interval.h"
#include "aos/util/phased_loop.h"
#include "aos/util/wrapping_counter.h"
+#include "ctre/phoenix/motorcontrol/can/TalonSRX.h"
#include "frc971/autonomous/auto.q.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/wpilib/ADIS16448.h"
@@ -42,8 +43,8 @@
#include "frc971/wpilib/pdp_fetcher.h"
#include "frc971/wpilib/sensor_reader.h"
#include "frc971/wpilib/wpilib_robot_base.h"
-#include "ctre/phoenix/motorcontrol/can/TalonSRX.h"
#include "y2019/constants.h"
+#include "y2019/control_loops/drivetrain/camera.q.h"
#include "y2019/control_loops/superstructure/superstructure.q.h"
#include "y2019/jevois/spi.h"
@@ -319,7 +320,22 @@
return;
}
- // TODO(Brian): Do something useful with the targets.
+ const auto now = aos::monotonic_clock::now();
+ for (const auto &received : unpacked->frames) {
+ auto to_send = control_loops::drivetrain::camera_frames.MakeMessage();
+ to_send->timestamp =
+ std::chrono::nanoseconds((now + received.age).time_since_epoch())
+ .count();
+ to_send->num_targets = received.targets.size();
+ for (size_t i = 0; i < received.targets.size(); ++i) {
+ to_send->targets[i].distance = received.targets[i].distance;
+ to_send->targets[i].height = received.targets[i].height;
+ to_send->targets[i].heading = received.targets[i].heading;
+ to_send->targets[i].skew = received.targets[i].skew;
+ }
+ to_send->camera = received.camera_index;
+ to_send.Send();
+ }
if (dummy_spi_) {
uint8_t dummy_send, dummy_receive;