Merge "De-warp contours before starting the polygon finding."
diff --git a/frc971/control_loops/drivetrain/libspline.cc b/frc971/control_loops/drivetrain/libspline.cc
index f931e59..1d7aa0b 100644
--- a/frc971/control_loops/drivetrain/libspline.cc
+++ b/frc971/control_loops/drivetrain/libspline.cc
@@ -197,9 +197,9 @@
void TrajectoryGetPlanXVA(::std::vector<::Eigen::Matrix<double, 3, 1>> *vec,
double *X, double *V, double *A) {
for (size_t i = 0; i < vec->size(); ++i) {
- X[i] = (*vec)[0][0];
- V[i] = (*vec)[0][1];
- A[i] = (*vec)[0][2];
+ X[i] = (*vec)[i][0];
+ V[i] = (*vec)[i][1];
+ A[i] = (*vec)[i][2];
}
}
diff --git a/motors/print/BUILD b/motors/print/BUILD
index 26ee976..26d8e45 100644
--- a/motors/print/BUILD
+++ b/motors/print/BUILD
@@ -8,6 +8,7 @@
restricted_to = mcu_cpus,
visibility = ["//visibility:public"],
deps = [
+ "//aos/containers:sized_array",
"//motors/core",
"//third_party/GSL",
],
diff --git a/motors/print/print.h b/motors/print/print.h
index d51a2c3..89da913 100644
--- a/motors/print/print.h
+++ b/motors/print/print.h
@@ -3,6 +3,7 @@
#include <memory>
+#include "aos/containers/sized_array.h"
#include "motors/core/kinetis.h"
#include "third_party/GSL/include/gsl/gsl"
@@ -29,6 +30,11 @@
// Writes something to a separate debug stream. Some implementations will
// always ignore this, and others will ignore it under some conditions.
virtual int WriteDebug(gsl::span<const char> buffer) { return buffer.size(); }
+
+ // Reads any characters which are available (never blocks).
+ //
+ // The default never returns any data.
+ virtual aos::SizedArray<char, 4> ReadStdin() { return {}; }
};
// A trivial printing "implementation" which simply does nothing. This is used
diff --git a/motors/print/usb.h b/motors/print/usb.h
index 7e63184..981c5d3 100644
--- a/motors/print/usb.h
+++ b/motors/print/usb.h
@@ -28,6 +28,12 @@
return debug_tty_->Write(buffer.data(), buffer.size());
}
+ aos::SizedArray<char, 4> ReadStdin() override {
+ aos::SizedArray<char, 4> result;
+ result.set_size(stdout_tty_->Read(result.data(), result.max_size()));
+ return result;
+ }
+
private:
teensy::AcmTty *const stdout_tty_;
teensy::AcmTty *const debug_tty_;
@@ -50,6 +56,12 @@
return debug_tty_.Write(buffer.data(), buffer.size());
}
+ aos::SizedArray<char, 4> ReadStdin() override {
+ aos::SizedArray<char, 4> result;
+ result.set_size(stdout_tty_.Read(result.data(), result.max_size()));
+ return result;
+ }
+
private:
teensy::UsbDevice usb_device_;
teensy::AcmTty stdout_tty_;
diff --git a/y2019/jevois/spi.cc b/y2019/jevois/spi.cc
index 2d3e2f2..b6e6632 100644
--- a/y2019/jevois/spi.cc
+++ b/y2019/jevois/spi.cc
@@ -252,6 +252,8 @@
memcpy(remaining_space.data(), &realtime_now, sizeof(realtime_now));
remaining_space = remaining_space.subspan(sizeof(realtime_now));
}
+ memcpy(remaining_space.data(), &message.camera_command, 1);
+ remaining_space = remaining_space.subspan(1);
{
uint16_t crc = jevois_crc_init();
crc = jevois_crc_update(crc, transfer.data(),
@@ -281,6 +283,8 @@
aos::realtime_clock::duration(realtime_now));
remaining_input = remaining_input.subspan(sizeof(realtime_now));
}
+ memcpy(&message.camera_command, remaining_input.data(), 1);
+ remaining_input = remaining_input.subspan(1);
{
uint16_t calculated_crc = jevois_crc_init();
calculated_crc =
diff --git a/y2019/jevois/spi_test.cc b/y2019/jevois/spi_test.cc
index 8f08fcb..32551db 100644
--- a/y2019/jevois/spi_test.cc
+++ b/y2019/jevois/spi_test.cc
@@ -107,7 +107,28 @@
// Tests packing and then unpacking an empty message.
TEST(SpiToTeensyPackTest, Empty) {
+ RoborioToTeensy input_message{};
+ const SpiTransfer transfer = SpiPackToTeensy(input_message);
+ const auto output_message = SpiUnpackToTeensy(transfer);
+ ASSERT_TRUE(output_message);
+ EXPECT_EQ(input_message, output_message.value());
+}
+
+// Tests packing and then unpacking a message with all the fields set.
+TEST(SpiToTeensyPackTest, Full) {
RoborioToTeensy input_message;
+ input_message.beacon_brightness[0] = 9;
+ input_message.beacon_brightness[1] = 7;
+ input_message.beacon_brightness[2] = 1;
+ input_message.light_rings[0] = 1;
+ input_message.light_rings[1] = 0;
+ input_message.light_rings[2] = 0;
+ input_message.light_rings[3] = 1;
+ input_message.light_rings[4] = 0;
+ input_message.realtime_now =
+ aos::realtime_clock::epoch() + std::chrono::seconds(971254);
+ input_message.camera_command = CameraCommand::kUsb;
+
const SpiTransfer transfer = SpiPackToTeensy(input_message);
const auto output_message = SpiUnpackToTeensy(transfer);
ASSERT_TRUE(output_message);
diff --git a/y2019/jevois/structures.h b/y2019/jevois/structures.h
index 055f201..1f10dc0 100644
--- a/y2019/jevois/structures.h
+++ b/y2019/jevois/structures.h
@@ -106,17 +106,17 @@
camera_duration age;
};
+enum class CameraCommand : char {
+ // Stay in normal mode.
+ kNormal,
+ // Go to camera passthrough mode.
+ kCameraPassthrough,
+ // Go to being a useful USB device.
+ kUsb,
+};
+
// This is all the information sent from the Teensy to each camera.
struct CameraCalibration {
- enum class CameraCommand : char {
- // Stay in normal mode.
- kNormal,
- // Go to camera passthrough mode.
- kCameraPassthrough,
- // Go to being a useful USB device.
- kUsb,
- };
-
bool operator==(const CameraCalibration &other) const {
if (other.calibration != calibration) {
return false;
@@ -197,6 +197,9 @@
// The current time.
aos::realtime_clock::time_point realtime_now;
+
+ // A command to send to all the cameras.
+ CameraCommand camera_command;
};
} // namespace jevois
diff --git a/y2019/jevois/teensy.cc b/y2019/jevois/teensy.cc
index 44b2505..a3b93d2 100644
--- a/y2019/jevois/teensy.cc
+++ b/y2019/jevois/teensy.cc
@@ -577,7 +577,8 @@
// claiming that it's impossible to queue up the first byte for the slave end of
// an SPI connection properly. Instead, we just accept there will be a garbage
// byte and the other end ignores it.
-__attribute__((unused)) void TransferData() {
+__attribute__((unused)) void TransferData(
+ frc971::motors::PrintingImplementation *printing) {
Uarts *const uarts = Uarts::global_instance;
std::array<CobsPacketizer<uart_to_teensy_size()>, 5> packetizers;
std::array<TransmitBuffer, 5> transmit_buffers{
@@ -585,13 +586,18 @@
FrameQueue frame_queue;
aos::monotonic_clock::time_point last_camera_send =
aos::monotonic_clock::min_time;
+ CameraCommand stdin_camera_command = CameraCommand::kNormal;
+ CameraCommand last_roborio_camera_command = CameraCommand::kNormal;
+
bool first = true;
while (true) {
{
const auto received_transfer = SpiQueue::global_instance->Tick();
if (received_transfer) {
const auto unpacked = SpiUnpackToTeensy(*received_transfer);
- if (!unpacked) {
+ if (unpacked) {
+ last_roborio_camera_command = unpacked->camera_command;
+ } else {
printf("UART decode error\n");
}
}
@@ -639,7 +645,11 @@
CameraCalibration calibration{};
calibration.teensy_now = aos::monotonic_clock::now();
calibration.realtime_now = aos::realtime_clock::min_time;
- calibration.camera_command = CameraCalibration::CameraCommand::kNormal;
+ if (last_roborio_camera_command != CameraCommand::kNormal) {
+ calibration.camera_command = last_roborio_camera_command;
+ } else {
+ calibration.camera_command = stdin_camera_command;
+ }
// TODO(Brian): Actually fill out the calibration field.
transmit_buffers[0].MaybeWritePacket(calibration);
transmit_buffers[1].MaybeWritePacket(calibration);
@@ -652,6 +662,29 @@
}
}
+ {
+ const auto stdin_data = printing->ReadStdin();
+ if (!stdin_data.empty()) {
+ switch (stdin_data.back()) {
+ case 'p':
+ printf("Entering passthrough mode\n");
+ stdin_camera_command = CameraCommand::kCameraPassthrough;
+ break;
+ case 'u':
+ printf("Entering USB mode\n");
+ stdin_camera_command = CameraCommand::kUsb;
+ break;
+ case 'n':
+ printf("Entering normal mode\n");
+ stdin_camera_command = CameraCommand::kNormal;
+ break;
+ default:
+ printf("Unrecognized character\n");
+ break;
+ }
+ }
+ }
+
first = false;
}
}
@@ -817,7 +850,7 @@
NVIC_ENABLE_IRQ(IRQ_SPI0);
NVIC_ENABLE_IRQ(IRQ_PORTA);
- TransferData();
+ TransferData(printing.get());
while (true) {
}
diff --git a/y2019/jevois/uart_test.cc b/y2019/jevois/uart_test.cc
index 1e90d84..d669688 100644
--- a/y2019/jevois/uart_test.cc
+++ b/y2019/jevois/uart_test.cc
@@ -66,8 +66,7 @@
input_message.teensy_now =
aos::monotonic_clock::time_point(std::chrono::seconds(1678));
input_message.realtime_now = aos::realtime_clock::min_time;
- input_message.camera_command =
- CameraCalibration::CameraCommand::kCameraPassthrough;
+ input_message.camera_command = CameraCommand::kCameraPassthrough;
const UartToCameraBuffer buffer = UartPackToCamera(input_message);
const auto output_message = UartUnpackToCamera(buffer);
ASSERT_TRUE(output_message);
diff --git a/y2019/vision/debug_serial.cc b/y2019/vision/debug_serial.cc
index a095b14..857f20f 100644
--- a/y2019/vision/debug_serial.cc
+++ b/y2019/vision/debug_serial.cc
@@ -68,10 +68,9 @@
if (n >= 1) {
CameraCalibration calibration{};
if (data[0] == 'p') {
- calibration.camera_command =
- CameraCalibration::CameraCommand::kCameraPassthrough;
+ calibration.camera_command = CameraCommand::kCameraPassthrough;
} else {
- calibration.camera_command = CameraCalibration::CameraCommand::kUsb;
+ calibration.camera_command = CameraCommand::kUsb;
}
if (write(itsDev, "\0", 1) == 1) {
const auto out_data = frc971::jevois::UartPackToCamera(calibration);
diff --git a/y2019/vision/target_sender.cc b/y2019/vision/target_sender.cc
index 0afeb47..2c7d8e2 100644
--- a/y2019/vision/target_sender.cc
+++ b/y2019/vision/target_sender.cc
@@ -275,7 +275,7 @@
(void)argc;
(void)argv;
using namespace y2019::vision;
- using frc971::jevois::CameraCalibration;
+ using frc971::jevois::CameraCommand;
// gflags::ParseCommandLineFlags(&argc, &argv, false);
::aos::logging::Init();
::aos::logging::AddImplementation(
@@ -389,11 +389,11 @@
intrinsics->barrel_mount = calibration.calibration(0, 2);
switch (calibration.camera_command) {
- case CameraCalibration::CameraCommand::kNormal:
+ case CameraCommand::kNormal:
break;
- case CameraCalibration::CameraCommand::kUsb:
+ case CameraCommand::kUsb:
return 0;
- case CameraCalibration::CameraCommand::kCameraPassthrough:
+ case CameraCommand::kCameraPassthrough:
return system("touch /tmp/do_not_export_sd_card");
}
} else {
diff --git a/y2019/wpilib_interface.cc b/y2019/wpilib_interface.cc
index 29377c8..02269eb 100644
--- a/y2019/wpilib_interface.cc
+++ b/y2019/wpilib_interface.cc
@@ -279,10 +279,26 @@
spi_->SetMSBFirst();
}
+ void set_activate_usb(std::unique_ptr<frc::DigitalInput> activate_usb) {
+ activate_usb_ = std::move(activate_usb);
+ }
+
+ void set_activate_passthrough(
+ std::unique_ptr<frc::DigitalInput> activate_passthrough) {
+ activate_passthrough_ = std::move(activate_passthrough);
+ }
+
void DoSpiTransaction() {
using namespace frc971::jevois;
RoborioToTeensy to_teensy{};
to_teensy.realtime_now = aos::realtime_clock::now();
+ if (activate_usb_ && !activate_usb_->Get()) {
+ to_teensy.camera_command = CameraCommand::kUsb;
+ } else if (activate_passthrough_ && !activate_passthrough_->Get()) {
+ to_teensy.camera_command = CameraCommand::kCameraPassthrough;
+ } else {
+ to_teensy.camera_command = CameraCommand::kNormal;
+ }
std::array<char, spi_transfer_size() + 1> to_send{};
{
@@ -343,6 +359,9 @@
frc::SPI *spi_ = nullptr;
::std::unique_ptr<frc::SPI> dummy_spi_;
+ std::unique_ptr<frc::DigitalInput> activate_usb_;
+ std::unique_ptr<frc::DigitalInput> activate_passthrough_;
+
frc971::wpilib::SpiRxClearer rx_clearer_;
};
@@ -553,6 +572,9 @@
frc::SPI camera_spi(frc::SPI::Port::kOnboardCS3);
camera_reader.set_spi(&camera_spi);
camera_reader.SetDummySPI(frc::SPI::Port::kOnboardCS2);
+ // Austin says 8, 9, 24, and 25 are good options to choose from for these.
+ camera_reader.set_activate_usb(make_unique<frc::DigitalInput>(24));
+ camera_reader.set_activate_passthrough(make_unique<frc::DigitalInput>(25));
auto imu_trigger = make_unique<frc::DigitalInput>(0);
::frc971::wpilib::ADIS16448 imu(frc::SPI::Port::kOnboardCS1,