Merge "Add a fixed cycle version of the motor interrupt"
diff --git a/aos/vision/math/BUILD b/aos/vision/math/BUILD
index 3233bc9..0fbdf7a 100644
--- a/aos/vision/math/BUILD
+++ b/aos/vision/math/BUILD
@@ -13,6 +13,7 @@
],
deps = [
"//third_party/eigen",
+ "@com_google_ceres_solver//:ceres",
],
)
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index d4c765f..05889e4 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -106,6 +106,13 @@
drivetrain_motor_plant_.GetRightPosition(), 1e-3);
}
+ void VerifyNearPosition(double x, double y) {
+ my_drivetrain_queue_.status.FetchLatest();
+ auto actual = drivetrain_motor_plant_.GetPosition();
+ EXPECT_NEAR(actual(0), x, 1e-2);
+ EXPECT_NEAR(actual(1), y, 1e-2);
+ }
+
void VerifyNearSplineGoal() {
my_drivetrain_queue_.status.FetchLatest();
double expected_x = my_drivetrain_queue_.status->trajectory_logging.x;
@@ -720,6 +727,29 @@
VerifyNearSplineGoal();
}
+//Tests that a trajectory can be executed after it is planned.
+TEST_F(DrivetrainTest, SplineExecuteAfterPlan) {
+ ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal> goal =
+ my_drivetrain_queue_.goal.MakeMessage();
+ goal->controller_type = 2;
+ goal->spline.spline_idx = 1;
+ goal->spline.spline_count = 1;
+ goal->spline.spline_x = {{0.0, 0.25, 0.5, 0.5, 0.75, 1.0}};
+ goal->spline.spline_y = {{0.0, 0.0, 0.25, 0.75, 1.0, 1.0}};
+ goal.Send();
+ WaitForTrajectoryPlan();
+ RunForTime(chrono::milliseconds(2000));
+
+ ::aos::ScopedMessagePtr<::frc971::control_loops::DrivetrainQueue::Goal>
+ start_goal = my_drivetrain_queue_.goal.MakeMessage();
+ start_goal->controller_type = 2;
+ start_goal->spline_handle = 1;
+ start_goal.Send();
+ RunForTime(chrono::milliseconds(2000));
+
+ VerifyNearPosition(1.0, 1.0);
+}
+
// The LineFollowDrivetrain logic is tested in line_follow_drivetrain_test. This
// tests that the integration with drivetrain_lib worked properly.
TEST_F(DrivetrainTest, BasicLineFollow) {
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.cc b/frc971/control_loops/drivetrain/splinedrivetrain.cc
index 0b6026d..a37ca0e 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.cc
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.cc
@@ -93,7 +93,7 @@
const ::frc971::control_loops::DrivetrainQueue::Goal &goal) {
current_spline_handle_ = goal.spline_handle;
// If told to stop, set the executing spline to an invalid index.
- if (current_spline_handle_ == 0) {
+ if (current_spline_handle_ == 0 && has_started_execution_) {
current_spline_idx_ = -1;
}
@@ -119,6 +119,7 @@
// Reset internal state to a trajectory start position.
current_xva_ = current_trajectory_->FFAcceleration(0);
current_xva_(1) = 0.0;
+ has_started_execution_ = false;
}
mutex_.Unlock();
}
@@ -133,6 +134,7 @@
::Eigen::Matrix<double, 2, 1> U_ff = ::Eigen::Matrix<double, 2, 1>::Zero();
if (!IsAtEnd() &&
current_spline_handle_ == current_spline_idx_) {
+ has_started_execution_ = true;
// TODO(alex): It takes about a cycle for the outputs to propagate to the
// motors. Consider delaying the output by a cycle.
U_ff = current_trajectory_->FFVoltage(current_xva_(0));
@@ -187,7 +189,7 @@
status->trajectory_logging.right_velocity = goal_state(4);
}
status->trajectory_logging.planning_state = static_cast<int8_t>(plan_state_.load());
- status->trajectory_logging.is_executing = !IsAtEnd();
+ status->trajectory_logging.is_executing = !IsAtEnd() && has_started_execution_;
status->trajectory_logging.goal_spline_handle = current_spline_handle_;
status->trajectory_logging.current_spline_idx = current_spline_idx_;
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.h b/frc971/control_loops/drivetrain/splinedrivetrain.h
index 3e47f5a..d70cb6d 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.h
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.h
@@ -70,6 +70,7 @@
int32_t current_spline_handle_ = 0; // Current spline told to excecute.
int32_t current_spline_idx_ = 0; // Current executing spline.
+ bool has_started_execution_ = false;
::std::unique_ptr<DistanceSpline> current_distance_spline_;
::std::unique_ptr<Trajectory> current_trajectory_;
diff --git a/frc971/control_loops/profiled_subsystem.h b/frc971/control_loops/profiled_subsystem.h
index d98b5c9..ef5ffde 100644
--- a/frc971/control_loops/profiled_subsystem.h
+++ b/frc971/control_loops/profiled_subsystem.h
@@ -10,12 +10,12 @@
#include "aos/controls/control_loop.h"
#include "aos/util/trapezoid_profile.h"
+#include "frc971/constants.h"
#include "frc971/control_loops/control_loops.q.h"
#include "frc971/control_loops/profiled_subsystem.q.h"
#include "frc971/control_loops/simple_capped_state_feedback_loop.h"
#include "frc971/control_loops/state_feedback_loop.h"
#include "frc971/zeroing/zeroing.h"
-#include "frc971/constants.h"
namespace frc971 {
namespace control_loops {
@@ -59,13 +59,15 @@
}
// Returns the controller.
- const StateFeedbackLoop<number_of_states, number_of_inputs, number_of_outputs> &
- controller() const {
+ const StateFeedbackLoop<number_of_states, number_of_inputs, number_of_outputs>
+ &controller() const {
return *loop_;
}
int controller_index() const { return loop_->index(); }
+ void set_controller_index(int index) { loop_->set_index(index); }
+
// Returns whether the estimators have been initialized and zeroed.
bool initialized() const { return initialized_; }
@@ -140,7 +142,8 @@
template <typename ZeroingEstimator =
::frc971::zeroing::PotAndIndexPulseZeroingEstimator>
class SingleDOFProfiledSubsystem
- : public ::frc971::control_loops::ProfiledSubsystem<3, 1, ZeroingEstimator> {
+ : public ::frc971::control_loops::ProfiledSubsystem<3, 1,
+ ZeroingEstimator> {
public:
SingleDOFProfiledSubsystem(
::std::unique_ptr<SimpleCappedStateFeedbackLoop<3, 1, 1>> loop,
diff --git a/frc971/control_loops/python/angular_system.py b/frc971/control_loops/python/angular_system.py
index a0a07d6..36da41a 100755
--- a/frc971/control_loops/python/angular_system.py
+++ b/frc971/control_loops/python/angular_system.py
@@ -375,26 +375,42 @@
"""Writes out the constants for a angular system to a file.
Args:
- params: AngularSystemParams, the parameters defining the system.
+ params: list of AngularSystemParams or AngularSystemParams, the
+ parameters defining the system.
plant_files: list of strings, the cc and h files for the plant.
controller_files: list of strings, the cc and h files for the integral
controller.
year_namespaces: list of strings, the namespace list to use.
"""
# Write the generated constants out to a file.
- angular_system = AngularSystem(params, params.name)
+ angular_systems = []
+ integral_angular_systems = []
+
+ if type(params) is list:
+ name = params[0].name
+ for index, param in enumerate(params):
+ angular_systems.append(
+ AngularSystem(param, param.name + str(index)))
+ integral_angular_systems.append(
+ IntegralAngularSystem(param, 'Integral' + param.name + str(
+ index)))
+ else:
+ name = params.name
+ angular_systems.append(AngularSystem(params, params.name))
+ integral_angular_systems.append(
+ IntegralAngularSystem(params, 'Integral' + params.name))
+
loop_writer = control_loop.ControlLoopWriter(
- angular_system.name, [angular_system], namespaces=year_namespaces)
+ name, angular_systems, namespaces=year_namespaces)
loop_writer.AddConstant(
- control_loop.Constant('kOutputRatio', '%f', angular_system.G))
+ control_loop.Constant('kOutputRatio', '%f', angular_systems[0].G))
loop_writer.AddConstant(
- control_loop.Constant('kFreeSpeed', '%f',
- angular_system.motor.free_speed))
+ control_loop.Constant('kFreeSpeed', '%f', angular_systems[0]
+ .motor.free_speed))
loop_writer.Write(plant_files[0], plant_files[1])
- integral_angular_system = IntegralAngularSystem(params,
- 'Integral' + params.name)
integral_loop_writer = control_loop.ControlLoopWriter(
- integral_angular_system.name, [integral_angular_system],
+ 'Integral' + name,
+ integral_angular_systems,
namespaces=year_namespaces)
integral_loop_writer.Write(controller_files[0], controller_files[1])
diff --git a/frc971/control_loops/python/linear_system.py b/frc971/control_loops/python/linear_system.py
index fe8dd85..105093b 100755
--- a/frc971/control_loops/python/linear_system.py
+++ b/frc971/control_loops/python/linear_system.py
@@ -381,29 +381,45 @@
"""Writes out the constants for a linear system to a file.
Args:
- params: LinearSystemParams, the parameters defining the system.
+ params: list of LinearSystemParams or LinearSystemParams, the
+ parameters defining the system.
plant_files: list of strings, the cc and h files for the plant.
controller_files: list of strings, the cc and h files for the integral
controller.
year_namespaces: list of strings, the namespace list to use.
"""
# Write the generated constants out to a file.
- linear_system = LinearSystem(params, params.name)
+ linear_systems = []
+ integral_linear_systems = []
+
+ if type(params) is list:
+ name = params[0].name
+ for index, param in enumerate(params):
+ linear_systems.append(
+ LinearSystem(param, param.name + str(index)))
+ integral_linear_systems.append(
+ IntegralLinearSystem(param, 'Integral' + param.name + str(
+ index)))
+ else:
+ name = params.name
+ linear_systems.append(LinearSystem(params, params.name))
+ integral_linear_systems.append(
+ IntegralLinearSystem(params, 'Integral' + params.name))
+
loop_writer = control_loop.ControlLoopWriter(
- linear_system.name, [linear_system], namespaces=year_namespaces)
+ name, linear_systems, namespaces=year_namespaces)
loop_writer.AddConstant(
- control_loop.Constant('kFreeSpeed', '%f',
- linear_system.motor.free_speed))
+ control_loop.Constant('kFreeSpeed', '%f', linear_systems[0]
+ .motor.free_speed))
loop_writer.AddConstant(
- control_loop.Constant('kOutputRatio', '%f',
- linear_system.G * linear_system.radius))
+ control_loop.Constant('kOutputRatio', '%f', linear_systems[0].G *
+ linear_systems[0].radius))
loop_writer.AddConstant(
- control_loop.Constant('kRadius', '%f', linear_system.radius))
+ control_loop.Constant('kRadius', '%f', linear_systems[0].radius))
loop_writer.Write(plant_files[0], plant_files[1])
- integral_linear_system = IntegralLinearSystem(params,
- 'Integral' + params.name)
integral_loop_writer = control_loop.ControlLoopWriter(
- integral_linear_system.name, [integral_linear_system],
+ 'Integral' + name,
+ integral_linear_systems,
namespaces=year_namespaces)
integral_loop_writer.Write(controller_files[0], controller_files[1])
diff --git a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h
index 2951f1a..7e8f4a7 100644
--- a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h
+++ b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h
@@ -74,6 +74,10 @@
void TriggerEstimatorError() { profiled_subsystem_.TriggerEstimatorError(); }
+ void set_controller_index(int index) {
+ profiled_subsystem_.set_controller_index(index);
+ }
+
enum class State : int32_t {
UNINITIALIZED,
DISABLED_INITIALIZED,
diff --git a/motors/BUILD b/motors/BUILD
index 9fe638c..ac65f37 100644
--- a/motors/BUILD
+++ b/motors/BUILD
@@ -144,6 +144,27 @@
)
hex_from_elf(
+ name = "simpler_receiver",
+ restricted_to = mcu_cpus,
+)
+
+cc_binary(
+ name = "simpler_receiver.elf",
+ srcs = [
+ "simpler_receiver.cc",
+ ],
+ restricted_to = mcu_cpus,
+ deps = [
+ ":util",
+ "//motors/core",
+ "//motors/peripheral:can",
+ "//motors/peripheral:configuration",
+ "//motors/print:usb",
+ "//motors/seems_reasonable:drivetrain_lib",
+ ],
+)
+
+hex_from_elf(
name = "simple_receiver",
restricted_to = mcu_cpus,
)
diff --git a/motors/peripheral/can.c b/motors/peripheral/can.c
index ccd8d7d..de593c7 100644
--- a/motors/peripheral/can.c
+++ b/motors/peripheral/can.c
@@ -86,6 +86,7 @@
// more stable than the PLL-based peripheral clock, which matters.
// We're going with a sample point fraction of 0.875 because that's what
// SocketCAN defaults to.
+ // This results in a baud rate of 500 kHz.
CAN0_CTRL1 = CAN_CTRL1_PRESDIV(
1) /* Divide the crystal frequency by 2 to get 8 MHz. */ |
CAN_CTRL1_RJW(0) /* RJW/SJW of 1, which is most common. */ |
diff --git a/motors/simpler_receiver.cc b/motors/simpler_receiver.cc
new file mode 100644
index 0000000..b3e5a36
--- /dev/null
+++ b/motors/simpler_receiver.cc
@@ -0,0 +1,512 @@
+// This file has the main for the Teensy on the simple receiver board v2 in the
+// new robot.
+
+#include <inttypes.h>
+#include <stdio.h>
+#include <atomic>
+#include <chrono>
+#include <cmath>
+
+#include "frc971/control_loops/drivetrain/polydrivetrain.h"
+#include "motors/core/kinetis.h"
+#include "motors/core/time.h"
+#include "motors/peripheral/can.h"
+#include "motors/peripheral/configuration.h"
+#include "motors/print/print.h"
+#include "motors/seems_reasonable/drivetrain_dog_motor_plant.h"
+#include "motors/seems_reasonable/polydrivetrain_dog_motor_plant.h"
+#include "motors/util.h"
+
+namespace frc971 {
+namespace motors {
+namespace {
+
+using ::frc971::control_loops::drivetrain::DrivetrainConfig;
+using ::frc971::control_loops::drivetrain::PolyDrivetrain;
+using ::frc971::constants::ShifterHallEffect;
+using ::frc971::control_loops::DrivetrainQueue_Goal;
+using ::frc971::control_loops::DrivetrainQueue_Output;
+
+namespace chrono = ::std::chrono;
+
+const ShifterHallEffect kThreeStateDriveShifter{0.0, 0.0, 0.25, 0.75};
+
+const DrivetrainConfig<float> &GetDrivetrainConfig() {
+ static DrivetrainConfig<float> kDrivetrainConfig{
+ ::frc971::control_loops::drivetrain::ShifterType::NO_SHIFTER,
+ ::frc971::control_loops::drivetrain::LoopType::OPEN_LOOP,
+ ::frc971::control_loops::drivetrain::GyroType::SPARTAN_GYRO,
+ ::frc971::control_loops::drivetrain::IMUType::IMU_X,
+
+ ::motors::seems_reasonable::MakeDrivetrainLoop,
+ ::motors::seems_reasonable::MakeVelocityDrivetrainLoop,
+ ::std::function<StateFeedbackLoop<7, 2, 4, float>()>(),
+
+ chrono::duration_cast<chrono::nanoseconds>(
+ chrono::duration<float>(::motors::seems_reasonable::kDt)),
+ ::motors::seems_reasonable::kRobotRadius,
+ ::motors::seems_reasonable::kWheelRadius, ::motors::seems_reasonable::kV,
+
+ ::motors::seems_reasonable::kHighGearRatio,
+ ::motors::seems_reasonable::kLowGearRatio, ::motors::seems_reasonable::kJ,
+ ::motors::seems_reasonable::kMass, kThreeStateDriveShifter,
+ kThreeStateDriveShifter, true /* default_high_gear */,
+ 0 /* down_offset */, 0.8 /* wheel_non_linearity */,
+ 1.2 /* quickturn_wheel_multiplier */, 1.5 /* wheel_multiplier */,
+ };
+
+ return kDrivetrainConfig;
+};
+
+
+::std::atomic<PolyDrivetrain<float> *> global_polydrivetrain{nullptr};
+
+// Last width we received on each channel.
+uint16_t pwm_input_widths[6];
+// When we received a pulse on each channel in milliseconds.
+uint32_t pwm_input_times[6];
+
+constexpr int kChannelTimeout = 100;
+
+bool lost_channel(int channel) {
+ DisableInterrupts disable_interrupts;
+ if (time_after(millis(),
+ time_add(pwm_input_times[channel], kChannelTimeout))) {
+ return true;
+ }
+ return false;
+}
+
+// Returns the most recently captured value for the specified input channel
+// scaled from -1 to 1, or 0 if it was captured over 100ms ago.
+float convert_input_width(int channel) {
+ uint16_t width;
+ {
+ DisableInterrupts disable_interrupts;
+ if (time_after(millis(),
+ time_add(pwm_input_times[channel], kChannelTimeout))) {
+ return 0;
+ }
+
+ width = pwm_input_widths[channel];
+ }
+
+ // Values measured with a channel mapped to a button.
+ static constexpr uint16_t kMinWidth = 4133;
+ static constexpr uint16_t kMaxWidth = 7177;
+ if (width < kMinWidth) {
+ width = kMinWidth;
+ } else if (width > kMaxWidth) {
+ width = kMaxWidth;
+ }
+ return (static_cast<float>(2 * (width - kMinWidth)) /
+ static_cast<float>(kMaxWidth - kMinWidth)) -
+ 1.0f;
+}
+
+// Sends a SET_RPM command to the specified VESC.
+// Note that sending 6 VESC commands every 1ms doesn't quite fit in the CAN
+// bandwidth.
+void vesc_set_rpm(int vesc_id, float rpm) {
+ const int32_t rpm_int = rpm;
+ uint32_t id = CAN_EFF_FLAG;
+ id |= vesc_id;
+ id |= (0x03 /* SET_RPM */) << 8;
+ uint8_t data[4] = {
+ static_cast<uint8_t>((rpm_int >> 24) & 0xFF),
+ static_cast<uint8_t>((rpm_int >> 16) & 0xFF),
+ static_cast<uint8_t>((rpm_int >> 8) & 0xFF),
+ static_cast<uint8_t>((rpm_int >> 0) & 0xFF),
+ };
+ can_send(id, data, sizeof(data), 2 + vesc_id);
+}
+
+// Sends a SET_CURRENT command to the specified VESC.
+// current is in amps.
+// Note that sending 6 VESC commands every 1ms doesn't quite fit in the CAN
+// bandwidth.
+void vesc_set_current(int vesc_id, float current) {
+ constexpr float kMaxCurrent = 80.0f;
+ const int32_t current_int =
+ ::std::max(-kMaxCurrent, ::std::min(kMaxCurrent, current)) * 1000.0f;
+ uint32_t id = CAN_EFF_FLAG;
+ id |= vesc_id;
+ id |= (0x01 /* SET_CURRENT */) << 8;
+ uint8_t data[4] = {
+ static_cast<uint8_t>((current_int >> 24) & 0xFF),
+ static_cast<uint8_t>((current_int >> 16) & 0xFF),
+ static_cast<uint8_t>((current_int >> 8) & 0xFF),
+ static_cast<uint8_t>((current_int >> 0) & 0xFF),
+ };
+ can_send(id, data, sizeof(data), 2 + vesc_id);
+}
+
+// Sends a SET_DUTY command to the specified VESC.
+// duty is from -1 to 1.
+// Note that sending 6 VESC commands every 1ms doesn't quite fit in the CAN
+// bandwidth.
+void vesc_set_duty(int vesc_id, float duty) {
+ constexpr int32_t kMaxDuty = 99999;
+ const int32_t duty_int = ::std::max(
+ -kMaxDuty, ::std::min(kMaxDuty, static_cast<int32_t>(duty * 100000.0f)));
+ uint32_t id = CAN_EFF_FLAG;
+ id |= vesc_id;
+ id |= (0x00 /* SET_DUTY */) << 8;
+ uint8_t data[4] = {
+ static_cast<uint8_t>((duty_int >> 24) & 0xFF),
+ static_cast<uint8_t>((duty_int >> 16) & 0xFF),
+ static_cast<uint8_t>((duty_int >> 8) & 0xFF),
+ static_cast<uint8_t>((duty_int >> 0) & 0xFF),
+ };
+ can_send(id, data, sizeof(data), 2 + vesc_id);
+}
+
+// TODO(Brian): Move these two test functions somewhere else.
+__attribute__((unused)) void DoVescTest() {
+ uint32_t time = micros();
+ while (true) {
+ for (int i = 0; i < 6; ++i) {
+ const uint32_t end = time_add(time, 500000);
+ while (true) {
+ const bool done = time_after(micros(), end);
+ float current;
+ if (done) {
+ current = -6;
+ } else {
+ current = 6;
+ }
+ vesc_set_current(i, current);
+ if (done) {
+ break;
+ }
+ delay(5);
+ }
+ time = end;
+ }
+ }
+}
+
+__attribute__((unused)) void DoReceiverTest2() {
+ static constexpr float kMaxRpm = 10000.0f;
+ while (true) {
+ const bool flip = convert_input_width(2) > 0;
+
+ {
+ const float value = convert_input_width(0);
+
+ {
+ float rpm = ::std::min(0.0f, value) * kMaxRpm;
+ if (flip) {
+ rpm *= -1.0f;
+ }
+ vesc_set_rpm(0, rpm);
+ }
+
+ {
+ float rpm = ::std::max(0.0f, value) * kMaxRpm;
+ if (flip) {
+ rpm *= -1.0f;
+ }
+ vesc_set_rpm(1, rpm);
+ }
+ }
+
+ {
+ const float value = convert_input_width(1);
+
+ {
+ float rpm = ::std::min(0.0f, value) * kMaxRpm;
+ if (flip) {
+ rpm *= -1.0f;
+ }
+ vesc_set_rpm(2, rpm);
+ }
+
+ {
+ float rpm = ::std::max(0.0f, value) * kMaxRpm;
+ if (flip) {
+ rpm *= -1.0f;
+ }
+ vesc_set_rpm(3, rpm);
+ }
+ }
+
+ {
+ const float value = convert_input_width(4);
+
+ {
+ float rpm = ::std::min(0.0f, value) * kMaxRpm;
+ if (flip) {
+ rpm *= -1.0f;
+ }
+ vesc_set_rpm(4, rpm);
+ }
+
+ {
+ float rpm = ::std::max(0.0f, value) * kMaxRpm;
+ if (flip) {
+ rpm *= -1.0f;
+ }
+ vesc_set_rpm(5, rpm);
+ }
+ }
+ // Give the CAN frames a chance to go out.
+ delay(5);
+ }
+}
+
+void SetupPwmFtm(BigFTM *ftm) {
+ ftm->MODE = FTM_MODE_WPDIS;
+ ftm->MODE = FTM_MODE_WPDIS | FTM_MODE_FTMEN;
+ ftm->SC = FTM_SC_CLKS(0) /* Disable counting for now */;
+
+ // Can't change MOD according to the reference manual ("The Dual Edge Capture
+ // mode must be used with ... the FTM counter in Free running counter.").
+ ftm->MOD = 0xFFFF;
+
+ // Capturing rising edge.
+ ftm->C0SC = FTM_CSC_MSA | FTM_CSC_ELSA;
+ // Capturing falling edge.
+ ftm->C1SC = FTM_CSC_CHIE | FTM_CSC_MSA | FTM_CSC_ELSB;
+
+ // Capturing rising edge.
+ ftm->C2SC = FTM_CSC_MSA | FTM_CSC_ELSA;
+ // Capturing falling edge.
+ ftm->C3SC = FTM_CSC_CHIE | FTM_CSC_MSA | FTM_CSC_ELSB;
+
+ // Capturing rising edge.
+ ftm->C4SC = FTM_CSC_MSA | FTM_CSC_ELSA;
+ // Capturing falling edge.
+ ftm->C5SC = FTM_CSC_CHIE | FTM_CSC_MSA | FTM_CSC_ELSB;
+
+ // Capturing rising edge.
+ ftm->C6SC = FTM_CSC_MSA | FTM_CSC_ELSA;
+ // Capturing falling edge.
+ ftm->C7SC = FTM_CSC_CHIE | FTM_CSC_MSA | FTM_CSC_ELSB;
+
+ (void)ftm->STATUS;
+ ftm->STATUS = 0x00;
+
+ ftm->COMBINE = FTM_COMBINE_DECAP3 | FTM_COMBINE_DECAPEN3 |
+ FTM_COMBINE_DECAP2 | FTM_COMBINE_DECAPEN2 |
+ FTM_COMBINE_DECAP1 | FTM_COMBINE_DECAPEN1 |
+ FTM_COMBINE_DECAP0 | FTM_COMBINE_DECAPEN0;
+
+ // 34.95ms max period before it starts wrapping and being weird.
+ ftm->SC = FTM_SC_CLKS(1) /* Use the system clock */ |
+ FTM_SC_PS(4) /* Prescaler=32 */;
+
+ ftm->MODE &= ~FTM_MODE_WPDIS;
+}
+
+extern "C" void ftm0_isr() {
+ while (true) {
+ const uint32_t status = FTM0->STATUS;
+ if (status == 0) {
+ return;
+ }
+
+ if (status & (1 << 1)) {
+ const uint32_t start = FTM0->C0V;
+ const uint32_t end = FTM0->C1V;
+ pwm_input_widths[1] = (end - start) & 0xFFFF;
+ pwm_input_times[1] = millis();
+ }
+ if (status & (1 << 5)) {
+ const uint32_t start = FTM0->C4V;
+ const uint32_t end = FTM0->C5V;
+ pwm_input_widths[3] = (end - start) & 0xFFFF;
+ pwm_input_times[3] = millis();
+ }
+ if (status & (1 << 3)) {
+ const uint32_t start = FTM0->C2V;
+ const uint32_t end = FTM0->C3V;
+ pwm_input_widths[4] = (end - start) & 0xFFFF;
+ pwm_input_times[4] = millis();
+ }
+
+ FTM0->STATUS = 0;
+ }
+}
+
+extern "C" void ftm3_isr() {
+ while (true) {
+ const uint32_t status = FTM3->STATUS;
+ if (status == 0) {
+ return;
+ }
+
+ FTM3->STATUS = 0;
+ }
+}
+
+extern "C" void pit3_isr() {
+ PIT_TFLG3 = 1;
+ PolyDrivetrain<float> *polydrivetrain =
+ global_polydrivetrain.load(::std::memory_order_acquire);
+
+ const bool lost_drive_channel = lost_channel(3) || lost_channel(1);
+
+ if (false) {
+ static int count = 0;
+ if (++count == 50) {
+ count = 0;
+ printf("0: %d 1: %d\n", (int)pwm_input_widths[3],
+ (int)pwm_input_widths[1]);
+ }
+ }
+
+ if (polydrivetrain != nullptr) {
+ DrivetrainQueue_Goal goal;
+ goal.control_loop_driving = false;
+ if (lost_drive_channel) {
+ goal.throttle = 0.0f;
+ goal.wheel = 0.0f;
+ } else {
+ goal.throttle = convert_input_width(1);
+ goal.wheel = -convert_input_width(3);
+ }
+ goal.quickturn = ::std::abs(polydrivetrain->velocity()) < 0.25f;
+
+ if (false) {
+ static int count = 0;
+ if (++count == 50) {
+ count = 0;
+ printf("throttle: %d wheel: %d\n", (int)(goal.throttle * 100),
+ (int)(goal.wheel * 100));
+ }
+ }
+
+ DrivetrainQueue_Output output;
+
+ polydrivetrain->SetGoal(goal);
+ polydrivetrain->Update(12.0f);
+ polydrivetrain->SetOutput(&output);
+
+ if (false) {
+ static int count = 0;
+ if (++count == 50) {
+ count = 0;
+ printf("l: %d r: %d\n", (int)(output.left_voltage * 100),
+ (int)(output.right_voltage * 100));
+ }
+ }
+ vesc_set_duty(0, -output.left_voltage / 12.0f);
+ vesc_set_duty(1, -output.left_voltage / 12.0f);
+
+ vesc_set_duty(2, -output.right_voltage / 12.0f);
+ vesc_set_duty(3, -output.right_voltage / 12.0f);
+ }
+}
+
+} // namespace
+
+extern "C" {
+
+void *__stack_chk_guard = (void *)0x67111971;
+void __stack_chk_fail(void);
+
+} // extern "C"
+
+extern "C" int main(void) {
+ // for background about this startup delay, please see these conversations
+ // https://forum.pjrc.com/threads/36606-startup-time-(400ms)?p=113980&viewfull=1#post113980
+ // https://forum.pjrc.com/threads/31290-Teensey-3-2-Teensey-Loader-1-24-Issues?p=87273&viewfull=1#post87273
+ delay(400);
+
+ // Set all interrupts to the second-lowest priority to start with.
+ for (int i = 0; i < NVIC_NUM_INTERRUPTS; i++) NVIC_SET_SANE_PRIORITY(i, 0xD);
+
+ // Now set priorities for all the ones we care about. They only have meaning
+ // relative to each other, which means centralizing them here makes it a lot
+ // more manageable.
+ NVIC_SET_SANE_PRIORITY(IRQ_USBOTG, 0x7);
+ NVIC_SET_SANE_PRIORITY(IRQ_FTM0, 0xa);
+ NVIC_SET_SANE_PRIORITY(IRQ_FTM3, 0xa);
+ NVIC_SET_SANE_PRIORITY(IRQ_PIT_CH3, 0x5);
+
+ // Builtin LED.
+ PERIPHERAL_BITBAND(GPIOC_PDOR, 5) = 1;
+ PORTC_PCR5 = PORT_PCR_DSE | PORT_PCR_SRE | PORT_PCR_MUX(1);
+ PERIPHERAL_BITBAND(GPIOC_PDDR, 5) = 1;
+
+ // Set up the CAN pins.
+ PORTA_PCR12 = PORT_PCR_DSE | PORT_PCR_MUX(2);
+ PORTA_PCR13 = PORT_PCR_DSE | PORT_PCR_MUX(2);
+
+ // PWM_IN0
+ // FTM0_CH1 (doesn't work)
+ // PORTC_PCR2 = PORT_PCR_MUX(4);
+
+ // PWM_IN1
+ // FTM0_CH0
+ PORTC_PCR1 = PORT_PCR_MUX(4);
+
+ // PWM_IN2
+ // FTM0_CH5 (doesn't work)
+ // PORTD_PCR5 = PORT_PCR_MUX(4);
+
+ // PWM_IN3
+ // FTM0_CH4
+ PORTD_PCR4 = PORT_PCR_MUX(4);
+
+ // PWM_IN4
+ // FTM0_CH2
+ PORTC_PCR3 = PORT_PCR_MUX(4);
+
+ // PWM_IN5
+ // FTM0_CH3 (doesn't work)
+ // PORTC_PCR4 = PORT_PCR_MUX(4);
+
+ delay(100);
+
+ PrintingParameters printing_parameters;
+ printing_parameters.dedicated_usb = true;
+ const ::std::unique_ptr<PrintingImplementation> printing =
+ CreatePrinting(printing_parameters);
+ printing->Initialize();
+
+ SIM_SCGC6 |= SIM_SCGC6_PIT;
+ // Workaround for errata e7914.
+ (void)PIT_MCR;
+ PIT_MCR = 0;
+ PIT_LDVAL3 = (BUS_CLOCK_FREQUENCY / 200) - 1;
+ PIT_TCTRL3 = PIT_TCTRL_TIE | PIT_TCTRL_TEN;
+
+ can_init(0, 1);
+ SetupPwmFtm(FTM0);
+ SetupPwmFtm(FTM3);
+
+ PolyDrivetrain<float> polydrivetrain(GetDrivetrainConfig(), nullptr);
+ global_polydrivetrain.store(&polydrivetrain, ::std::memory_order_release);
+
+ // Leave the LED on for a bit longer.
+ delay(300);
+ printf("Done starting up\n");
+
+ // Done starting up, now turn the LED off.
+ PERIPHERAL_BITBAND(GPIOC_PDOR, 5) = 0;
+
+ NVIC_ENABLE_IRQ(IRQ_FTM0);
+ NVIC_ENABLE_IRQ(IRQ_FTM3);
+ NVIC_ENABLE_IRQ(IRQ_PIT_CH3);
+ printf("Done starting up2\n");
+
+ while (true) {
+ }
+
+ return 0;
+}
+
+void __stack_chk_fail(void) {
+ while (true) {
+ GPIOC_PSOR = (1 << 5);
+ printf("Stack corruption detected\n");
+ delay(1000);
+ GPIOC_PCOR = (1 << 5);
+ delay(1000);
+ }
+}
+
+} // namespace motors
+} // namespace frc971
diff --git a/y2019/actors/auto_splines.cc b/y2019/actors/auto_splines.cc
index f316037..57aba4e 100644
--- a/y2019/actors/auto_splines.cc
+++ b/y2019/actors/auto_splines.cc
@@ -5,11 +5,72 @@
namespace y2019 {
namespace actors {
+::frc971::MultiSpline AutonomousSplines::HabToFarRocket() {
+ ::frc971::MultiSpline spline;
+ ::frc971::Constraint longitudinal_constraint;
+ ::frc971::Constraint lateral_constraint;
+ ::frc971::Constraint voltage_constraint;
+ ::frc971::Constraint velocity_constraint;
+
+ longitudinal_constraint.constraint_type = 1;
+ longitudinal_constraint.value = 1.5;
+
+ lateral_constraint.constraint_type = 2;
+ lateral_constraint.value = 1.0;
+
+ voltage_constraint.constraint_type = 3;
+ voltage_constraint.value = 11.0;
+
+ // Note: This velocity constraint is currently too late in the spline to
+ // actually do anything.
+ velocity_constraint.constraint_type = 4;
+ velocity_constraint.value = 0.5;
+ velocity_constraint.start_distance = 7.5;
+ velocity_constraint.end_distance = 10.0;
+
+ spline.spline_count = 1;
+ spline.spline_x = {{1.0, 2.0, 4.0, 7.8, 7.8, 6.53}};
+ spline.spline_y = {{1.5, 1.5, 1.5, 1.5, 3.0, 3.47}};
+ spline.constraints = {{longitudinal_constraint, lateral_constraint,
+ voltage_constraint, velocity_constraint}};
+ return spline;
+}
+
+::frc971::MultiSpline AutonomousSplines::FarRockettoHP() {
+ ::frc971::MultiSpline spline;
+ ::frc971::Constraint longitudinal_constraint;
+ ::frc971::Constraint lateral_constraint;
+ ::frc971::Constraint voltage_constraint;
+ ::frc971::Constraint velocity_constraint;
+
+ longitudinal_constraint.constraint_type = 1;
+ longitudinal_constraint.value = 1.5;
+
+ lateral_constraint.constraint_type = 2;
+ lateral_constraint.value = 1.0;
+
+ voltage_constraint.constraint_type = 3;
+ voltage_constraint.value = 11.0;
+
+ velocity_constraint.constraint_type = 4;
+ velocity_constraint.value = 0.5;
+ velocity_constraint.start_distance = 7.5;
+ velocity_constraint.end_distance = 10.0;
+
+ spline.spline_count = 1;
+ spline.spline_x = {{6.53, 7.8, 7.8, 4.0, 2.0, 0.4}};
+ spline.spline_y = {{3.47, 3.0, 1.5, 3.0, 3.4, 3.4}};
+ spline.constraints = {{longitudinal_constraint, lateral_constraint,
+ voltage_constraint, velocity_constraint}};
+ return spline;
+}
+
::frc971::MultiSpline AutonomousSplines::HPToNearRocket() {
::frc971::MultiSpline spline;
::frc971::Constraint longitudinal_constraint;
::frc971::Constraint lateral_constraint;
::frc971::Constraint voltage_constraint;
+ ::frc971::Constraint velocity_constraint;
longitudinal_constraint.constraint_type = 1;
longitudinal_constraint.value = 1.0;
@@ -18,13 +79,18 @@
lateral_constraint.value = 1.0;
voltage_constraint.constraint_type = 3;
- voltage_constraint.value = 12.0;
+ voltage_constraint.value = 11.0;
+
+ velocity_constraint.constraint_type = 4;
+ velocity_constraint.value = 0.5;
+ velocity_constraint.start_distance = 2.7;
+ velocity_constraint.end_distance = 10.0;
spline.spline_count = 1;
- spline.spline_x = {{0.4, 1.0, 3.0, 4.0, 4.5, 5.05}};
- spline.spline_y = {{3.4, 3.4, 3.4, 3.0, 3.0, 3.5}};
- spline.constraints = {
- {longitudinal_constraint, lateral_constraint, voltage_constraint}};
+ spline.spline_x = {{1.5, 2.0, 3.0, 4.0, 4.5, 5.12}};
+ spline.spline_y = {{3.4, 3.4, 3.4, 3.0, 3.0, 3.43}};
+ spline.constraints = {{longitudinal_constraint, lateral_constraint,
+ voltage_constraint, velocity_constraint}};
return spline;
}
diff --git a/y2019/actors/auto_splines.h b/y2019/actors/auto_splines.h
index 7e79d1f..d093b99 100644
--- a/y2019/actors/auto_splines.h
+++ b/y2019/actors/auto_splines.h
@@ -22,6 +22,9 @@
// HP to near side rocket
static ::frc971::MultiSpline HPToNearRocket();
+
+ static ::frc971::MultiSpline HabToFarRocket();
+ static ::frc971::MultiSpline FarRockettoHP();
};
} // namespace actors
diff --git a/y2019/constants.cc b/y2019/constants.cc
index 7406488..8872318 100644
--- a/y2019/constants.cc
+++ b/y2019/constants.cc
@@ -178,10 +178,10 @@
break;
case kCompTeamNumber:
- elevator_params->zeroing_constants.measured_absolute_position = 0.160736;
+ elevator_params->zeroing_constants.measured_absolute_position = 0.152217;
elevator->potentiometer_offset =
-0.075017 + 0.015837 + 0.009793 - 0.012017 + 0.019915 + 0.010126 +
- 0.005541 + 0.006088 - 0.039687 + 0.005843;
+ 0.005541 + 0.006088 - 0.039687 + 0.005843 + 0.009007;
intake->zeroing_constants.measured_absolute_position = 1.860016;
@@ -194,8 +194,8 @@
break;
case kPracticeTeamNumber:
- elevator_params->zeroing_constants.measured_absolute_position = 0.132216;
- elevator->potentiometer_offset = -0.022320 + 0.020567 - 0.022355 - 0.006497 + 0.019690 + 0.009151;
+ elevator_params->zeroing_constants.measured_absolute_position = 0.131806;
+ elevator->potentiometer_offset = -0.022320 + 0.020567 - 0.022355 - 0.006497 + 0.019690 + 0.009151 - 0.007513 + 0.007311;
intake->zeroing_constants.measured_absolute_position = 1.756847;
diff --git a/y2019/control_loops/drivetrain/localizer.h b/y2019/control_loops/drivetrain/localizer.h
index 58b6901..1571fec 100644
--- a/y2019/control_loops/drivetrain/localizer.h
+++ b/y2019/control_loops/drivetrain/localizer.h
@@ -100,7 +100,7 @@
// The threshold to use for completely rejecting potentially bad target
// matches.
// TODO(james): Tune
- static constexpr Scalar kRejectionScore = 1000000.0;
+ static constexpr Scalar kRejectionScore = 1.0;
// Checks that the targets coming in make some sense--mostly to prevent NaNs
// or the such from propagating.
diff --git a/y2019/jevois/structures.h b/y2019/jevois/structures.h
index 9c74b11..fcf1ebe 100644
--- a/y2019/jevois/structures.h
+++ b/y2019/jevois/structures.h
@@ -144,6 +144,8 @@
// Send As, which triggers the bootstrap script to drop directly into USB
// mode.
kAs,
+ // Log camera images
+ kLog,
};
// This is all the information sent from the Teensy to each camera.
diff --git a/y2019/jevois/teensy.cc b/y2019/jevois/teensy.cc
index 0f9f94d..58d1570 100644
--- a/y2019/jevois/teensy.cc
+++ b/y2019/jevois/teensy.cc
@@ -774,6 +774,10 @@
printf("Sending USB mode\n");
stdin_camera_command = CameraCommand::kUsb;
break;
+ case 'l':
+ printf("Log mode\n");
+ stdin_camera_command = CameraCommand::kLog;
+ break;
case 'n':
printf("Sending normal mode\n");
stdin_camera_command = CameraCommand::kNormal;
@@ -798,6 +802,7 @@
printf("UART board commands:\n");
printf(" p: Send passthrough mode\n");
printf(" u: Send USB mode\n");
+ printf(" l: Send Log mode\n");
printf(" n: Send normal mode\n");
printf(" a: Send all-'a' mode\n");
printf(" c: Dump camera configuration\n");
diff --git a/y2019/joystick_reader.cc b/y2019/joystick_reader.cc
index b403913..645582d 100644
--- a/y2019/joystick_reader.cc
+++ b/y2019/joystick_reader.cc
@@ -88,6 +88,8 @@
const ButtonLocation kMidCargoHint(3, 16);
const ButtonLocation kFarCargoHint(4, 2);
+const ButtonLocation kCameraLog(3, 7);
+
const ElevatorWristPosition kStowPos{0.36, 0.0};
const ElevatorWristPosition kClimbPos{0.0, M_PI / 4.0};
@@ -107,21 +109,21 @@
const ElevatorWristPosition kPanelCargoBackwardPos{0.0, -M_PI / 2.0};
const ElevatorWristPosition kBallForwardLowerPos{0.21, 1.27};
-const ElevatorWristPosition kBallBackwardLowerPos{0.40, -1.99};
+const ElevatorWristPosition kBallBackwardLowerPos{0.43, -1.99};
-const ElevatorWristPosition kBallForwardMiddlePos{0.90, 1.21};
-const ElevatorWristPosition kBallBackwardMiddlePos{1.17, -1.98};
+const ElevatorWristPosition kBallForwardMiddlePos{0.925, 1.21};
+const ElevatorWristPosition kBallBackwardMiddlePos{1.19, -1.98};
const ElevatorWristPosition kBallForwardUpperPos{1.51, 0.961};
const ElevatorWristPosition kBallBackwardUpperPos{1.44, -1.217};
-const ElevatorWristPosition kBallCargoForwardPos{0.739044, 1.353};
-const ElevatorWristPosition kBallCargoBackwardPos{0.868265, -1.999};
+const ElevatorWristPosition kBallCargoForwardPos{0.59, 1.2};
+const ElevatorWristPosition kBallCargoBackwardPos{0.868265, -2.1};
const ElevatorWristPosition kBallHPIntakeForwardPos{0.55, 1.097};
const ElevatorWristPosition kBallHPIntakeBackwardPos{0.89, -2.018};
-const ElevatorWristPosition kBallIntakePos{0.29, 2.14};
+const ElevatorWristPosition kBallIntakePos{0.309, 2.13};
class Reader : public ::aos::input::ActionJoystickInput {
public:
@@ -319,7 +321,7 @@
if (switch_ball_) {
if (kDoBallOutake ||
(kDoBallIntake &&
- monotonic_now < last_not_has_piece_ + chrono::milliseconds(100))) {
+ monotonic_now < last_not_has_piece_ + chrono::milliseconds(200))) {
new_superstructure_goal->intake.unsafe_goal = 0.959327;
}
@@ -397,6 +399,13 @@
video_tx_->Send(vision_control_);
last_vision_control_ = monotonic_now;
}
+
+ {
+ auto camera_log_message = camera_log.MakeMessage();
+ camera_log_message->log = data.IsPressed(kCameraLog);
+ LOG_STRUCT(DEBUG, "camera_log", *camera_log_message);
+ camera_log_message.Send();
+ }
}
private:
diff --git a/y2019/status_light.q b/y2019/status_light.q
index f84ed28..c26de88 100644
--- a/y2019/status_light.q
+++ b/y2019/status_light.q
@@ -8,3 +8,9 @@
};
queue StatusLight status_light;
+
+message CameraLog {
+ bool log;
+};
+
+queue CameraLog camera_log;
diff --git a/y2019/vision/BUILD b/y2019/vision/BUILD
index bf71116..c7f0009 100644
--- a/y2019/vision/BUILD
+++ b/y2019/vision/BUILD
@@ -34,6 +34,16 @@
tools = [":constants_formatting"],
)
+cc_library(
+ name = "image_writer",
+ srcs = ["image_writer.cc"],
+ hdrs = ["image_writer.h"],
+ deps = [
+ "//aos/vision/image:image_types",
+ "@com_github_google_glog//:glog",
+ ],
+)
+
sh_test(
name = "constants_formatting_test",
srcs = ["constants_formatting_test.sh"],
@@ -92,6 +102,7 @@
srcs = ["target_sender.cc"],
restricted_to = VISION_TARGETS,
deps = [
+ ":image_writer",
":target_finder",
"//aos/vision/blob:codec",
"//aos/vision/blob:find_blob",
diff --git a/y2019/vision/calibrate.sh b/y2019/vision/calibrate.sh
index 24118ed..7b7ba4f 100755
--- a/y2019/vision/calibrate.sh
+++ b/y2019/vision/calibrate.sh
@@ -21,9 +21,9 @@
--tape_direction_x=-1.0 \
--tape_direction_y=0.0 \
--beginning_tape_measure_reading=11 \
- --image_count=75 \
+ --image_count=69 \
--constants=constants.cc \
- --prefix data/cam10_0/debug_viewer_jpeg_
+ --prefix data/cam10_1/debug_viewer_jpeg_
calibration \
--camera_id 6 \
diff --git a/y2019/vision/constants.cc b/y2019/vision/constants.cc
index 36694f8..fabe75c 100644
--- a/y2019/vision/constants.cc
+++ b/y2019/vision/constants.cc
@@ -133,20 +133,20 @@
CameraCalibration camera_10 = {
{
- -0.190556 / 180.0 * M_PI, 345.022, 0.468494 / 180.0 * M_PI,
+ -0.305107 / 180.0 * M_PI, 336.952, -0.0804389 / 180.0 * M_PI,
},
{
- {{-4.83005 * kInchesToMeters, 2.95565 * kInchesToMeters,
- 33.3624 * kInchesToMeters}},
- 182.204 / 180.0 * M_PI,
+ {{-5.64467 * kInchesToMeters, 2.41348 * kInchesToMeters,
+ 33.1669 * kInchesToMeters}},
+ 181.598 / 180.0 * M_PI,
},
{
10,
{{-12.5 * kInchesToMeters, 0.0}},
{{-1 * kInchesToMeters, 0.0}},
11,
- "data/cam10_0/debug_viewer_jpeg_",
- 75,
+ "data/cam10_1/debug_viewer_jpeg_",
+ 69,
}};
CameraCalibration camera_14 = {
diff --git a/y2019/vision/image_writer.cc b/y2019/vision/image_writer.cc
new file mode 100644
index 0000000..51e2c60
--- /dev/null
+++ b/y2019/vision/image_writer.cc
@@ -0,0 +1,40 @@
+#include <fstream>
+#include <sys/stat.h>
+
+#include "glog/logging.h"
+#include "y2019/vision/image_writer.h"
+
+namespace y2019 {
+namespace vision {
+
+ImageWriter::ImageWriter() {
+ LOG(INFO) << "Initializing image writer";
+ SetDirPath();
+}
+
+void ImageWriter::WriteImage(::aos::vision::DataRef data) {
+ LOG(INFO) << "Writing image " << image_count_;
+ std::ofstream ofs(
+ dir_path_ + file_prefix_ + std::to_string(image_count_) + ".yuyv",
+ std::ofstream::out);
+ ofs << data;
+ ofs.close();
+ ++image_count_;
+}
+
+void ImageWriter::SetDirPath() {
+ ::std::string base_path = "/jevois/data/run_";
+ for (int i = 0;; ++i) {
+ struct stat st;
+ std::string option = base_path + std::to_string(i);
+ if (stat(option.c_str(), &st) != 0) {
+ file_prefix_ = option + "/";
+ LOG(INFO) << "Writing to " << file_prefix_.c_str();
+ mkdir(file_prefix_.c_str(), 0777);
+ break;
+ }
+ }
+}
+
+} // namespace vision
+} // namespace y2019
diff --git a/y2019/vision/image_writer.h b/y2019/vision/image_writer.h
new file mode 100644
index 0000000..f33cca7
--- /dev/null
+++ b/y2019/vision/image_writer.h
@@ -0,0 +1,29 @@
+#ifndef Y2019_VISION_IMAGE_WRITER_H_
+#define Y2019_VISION_IMAGE_WRITER_H_
+
+#include <string>
+
+#include "aos/vision/image/image_types.h"
+
+namespace y2019 {
+namespace vision {
+
+class ImageWriter {
+ public:
+ ImageWriter();
+
+ void WriteImage(::aos::vision::DataRef data);
+
+ private:
+ void SetDirPath();
+
+ ::std::string file_prefix_ = std::string("debug_viewer_jpeg_");
+ ::std::string dir_path_;
+
+ unsigned int image_count_ = 0;
+};
+
+} // namespace vision
+} // namespace y2017
+
+#endif // Y2019_VISION_IMAGE_WRITER_H_
diff --git a/y2019/vision/server/www/field.ts b/y2019/vision/server/www/field.ts
index b52eee8..591b291 100644
--- a/y2019/vision/server/www/field.ts
+++ b/y2019/vision/server/www/field.ts
@@ -1,4 +1,4 @@
-import {IN_TO_M, FT_TO_M} from './constants';
+import {FT_TO_M, IN_TO_M} from './constants';
const CENTER_FIELD_X = 27 * FT_TO_M + 1.125 * IN_TO_M;
@@ -37,12 +37,12 @@
const DEPOT_WIDTH = (12 + 10.625) * IN_TO_M;
-export function drawField(ctx : CanvasRenderingContext2D) : void {
+export function drawField(ctx: CanvasRenderingContext2D): void {
drawTargets(ctx);
drawHab(ctx);
}
-function drawHab(ctx : CanvasRenderingContext2D) : void {
+function drawHab(ctx: CanvasRenderingContext2D): void {
drawHalfHab(ctx);
ctx.save();
@@ -52,7 +52,7 @@
ctx.restore();
}
-function drawHalfHab(ctx : CanvasRenderingContext2D) : void {
+function drawHalfHab(ctx: CanvasRenderingContext2D): void {
ctx.fillStyle = 'rgb(50, 50, 50)';
ctx.fillRect(0, 0, HAB_LENGTH, HALF_HAB_3_WIDTH);
ctx.fillStyle = 'rgb(100, 100, 100)';
@@ -62,7 +62,7 @@
ctx.strokeRect(0, HALF_HAB_3_WIDTH + HAB_2_WIDTH, HAB_LENGTH, DEPOT_WIDTH);
}
-function drawTargets(ctx : CanvasRenderingContext2D) : void {
+function drawTargets(ctx: CanvasRenderingContext2D): void {
drawHalfCargo(ctx);
drawRocket(ctx);
drawHP(ctx);
@@ -76,11 +76,11 @@
ctx.restore();
}
-function drawHP(ctx : CanvasRenderingContext2D) : void {
+function drawHP(ctx: CanvasRenderingContext2D): void {
drawTarget(ctx, 0, HP_Y, HP_THETA);
}
-function drawRocket(ctx : CanvasRenderingContext2D) : void {
+function drawRocket(ctx: CanvasRenderingContext2D): void {
drawTarget(ctx, ROCKET_PORT_X, ROCKET_PORT_Y, ROCKET_PORT_THETA);
drawTarget(ctx, ROCKET_NEAR_X, ROCKET_HATCH_Y, ROCKET_NEAR_THETA);
@@ -88,7 +88,7 @@
drawTarget(ctx, ROCKET_FAR_X, ROCKET_HATCH_Y, ROCKET_FAR_THETA);
}
-function drawHalfCargo(ctx : CanvasRenderingContext2D) : void {
+function drawHalfCargo(ctx: CanvasRenderingContext2D): void {
drawTarget(ctx, FAR_CARGO_X, SIDE_CARGO_Y, SIDE_CARGO_THETA);
drawTarget(ctx, MID_CARGO_X, SIDE_CARGO_Y, SIDE_CARGO_THETA);
@@ -98,7 +98,8 @@
drawTarget(ctx, FACE_CARGO_X, FACE_CARGO_Y, FACE_CARGO_THETA);
}
-export function drawTarget(ctx : CanvasRenderingContext2D, x: number, y: number, theta: number) : void {
+export function drawTarget(
+ ctx: CanvasRenderingContext2D, x: number, y: number, theta: number): void {
ctx.save();
ctx.translate(x, y);
ctx.rotate(theta);
diff --git a/y2019/vision/server/www/main.ts b/y2019/vision/server/www/main.ts
index beb2734..a260787 100644
--- a/y2019/vision/server/www/main.ts
+++ b/y2019/vision/server/www/main.ts
@@ -1,4 +1,4 @@
-import {FT_TO_M, FIELD_WIDTH} from './constants';
+import {FIELD_WIDTH, FT_TO_M} from './constants';
import {drawField, drawTarget} from './field';
import {drawRobot} from './robot';
@@ -23,6 +23,11 @@
const ctx = canvas.getContext('2d');
const server = location.host;
+ this.initWebSocket(server);
+ window.requestAnimationFrame(() => this.draw(ctx));
+ }
+
+ initWebSocket(server: string): void {
const socket = new WebSocket(`ws://${server}/ws`);
const reader = new FileReader();
reader.addEventListener('loadend', (e) => {
@@ -32,8 +37,9 @@
this.y = j.robotPose.y;
this.theta = j.robotPose.theta;
- if(j.lineFollowDebug) {
- this.targetLocked = j.lineFollowDebug.frozen && j.lineFollowDebug.haveTarget;
+ if (j.lineFollowDebug) {
+ this.targetLocked =
+ j.lineFollowDebug.frozen && j.lineFollowDebug.haveTarget;
this.targetX = j.lineFollowDebug.goalTarget.x;
this.targetY = j.lineFollowDebug.goalTarget.y;
this.targetTheta = j.lineFollowDebug.goalTarget.theta;
@@ -49,17 +55,21 @@
socket.addEventListener('message', (event) => {
reader.readAsText(event.data);
});
- window.requestAnimationFrame(() => this.draw(ctx));
+ socket.addEventListener('close', (event) => {
+ setTimeout(() => {
+ this.initWebSocket(server);
+ }, 1000);
+ });
}
- reset(ctx : CanvasRenderingContext2D) : void {
- ctx.setTransform(1,0,0,1,0,0);
+ reset(ctx: CanvasRenderingContext2D): void {
+ ctx.setTransform(1, 0, 0, 1, 0, 0);
const size = Math.min(window.innerHeight, window.innerWidth) * 0.98;
ctx.canvas.height = size;
ctx.canvas.width = size;
- ctx.clearRect(0,0,size,size);
+ ctx.clearRect(0, 0, size, size);
- ctx.translate(size/2, size);
+ ctx.translate(size / 2, size);
ctx.rotate(-Math.PI / 2);
ctx.scale(1, -1);
const M_TO_PX = size / FIELD_WIDTH
@@ -67,7 +77,7 @@
ctx.lineWidth = 1 / M_TO_PX;
}
- draw(ctx : CanvasRenderingContext2D) : void {
+ draw(ctx: CanvasRenderingContext2D): void {
this.reset(ctx);
drawField(ctx);
diff --git a/y2019/vision/server/www/robot.ts b/y2019/vision/server/www/robot.ts
index 489112c..6329fb3 100644
--- a/y2019/vision/server/www/robot.ts
+++ b/y2019/vision/server/www/robot.ts
@@ -1,22 +1,28 @@
-import {IN_TO_M, FT_TO_M} from './constants';
import {CAMERA_POSES} from './camera_constants';
+import {FT_TO_M, IN_TO_M} from './constants';
const ROBOT_WIDTH = 25 * IN_TO_M;
const ROBOT_LENGTH = 31 * IN_TO_M;
const CAMERA_SCALE = 0.3;
-function drawCamera(ctx : canvasRenderingContext2d, pose : {x : number, y : number, theta : number}) : void {
+function drawCamera(
+ ctx: CanvasRenderingContext2D,
+ pose: {x: number, y: number, theta: number}): void {
ctx.beginPath();
ctx.moveTo(pose.x, pose.y);
- ctx.lineTo(pose.x + CAMERA_SCALE * Math.cos(pose.theta + Math.PI / 4.0),
- pose.y + CAMERA_SCALE * Math.sin(pose.theta + Math.PI / 4.0));
- ctx.lineTo(pose.x + CAMERA_SCALE * Math.cos(pose.theta - Math.PI / 4.0),
- pose.y + CAMERA_SCALE * Math.sin(pose.theta - Math.PI / 4.0));
+ ctx.lineTo(
+ pose.x + CAMERA_SCALE * Math.cos(pose.theta + Math.PI / 4.0),
+ pose.y + CAMERA_SCALE * Math.sin(pose.theta + Math.PI / 4.0));
+ ctx.lineTo(
+ pose.x + CAMERA_SCALE * Math.cos(pose.theta - Math.PI / 4.0),
+ pose.y + CAMERA_SCALE * Math.sin(pose.theta - Math.PI / 4.0));
ctx.closePath();
ctx.stroke();
}
-export function drawRobot(ctx : CanvasRenderingContext2D, x : number, y : number, theta : number, camera_colors : string[]) : void {
+export function drawRobot(
+ ctx: CanvasRenderingContext2D, x: number, y: number, theta: number,
+ camera_colors: string[]): void {
ctx.save();
ctx.translate(x, y);
ctx.rotate(theta);
@@ -26,9 +32,9 @@
ctx.beginPath();
ctx.strokeStyle = 'black';
- ctx.moveTo(ROBOT_LENGTH / 2, -ROBOT_WIDTH/2);
+ ctx.moveTo(ROBOT_LENGTH / 2, -ROBOT_WIDTH / 2);
ctx.lineTo(ROBOT_LENGTH / 2 + 0.1, 0);
- ctx.lineTo(ROBOT_LENGTH / 2, ROBOT_WIDTH/2);
+ ctx.lineTo(ROBOT_LENGTH / 2, ROBOT_WIDTH / 2);
ctx.closePath();
ctx.stroke();
ctx.lineWidth = 3.0 * ctx.lineWidth;
diff --git a/y2019/vision/target_finder.h b/y2019/vision/target_finder.h
index 3da5373..ff4c3f7 100644
--- a/y2019/vision/target_finder.h
+++ b/y2019/vision/target_finder.h
@@ -59,6 +59,8 @@
const std::vector<TargetComponent> component_list, bool verbose);
// Given a target solve for the transformation of the template target.
+ //
+ // This is safe to call concurrently.
IntermediateResult ProcessTargetToResult(const Target &target, bool verbose);
// Returns true if a target is good, and false otherwise. Picks the 4 vs 8
diff --git a/y2019/vision/target_geometry.cc b/y2019/vision/target_geometry.cc
index 00375c1..de76acb 100644
--- a/y2019/vision/target_geometry.cc
+++ b/y2019/vision/target_geometry.cc
@@ -6,7 +6,6 @@
#include "aos/util/math.h"
-using ceres::NumericDiffCostFunction;
using ceres::CENTRAL;
using ceres::CostFunction;
using ceres::Problem;
@@ -55,77 +54,6 @@
left.inside, left.bottom}};
}
-Vector<2> Project(Vector<2> pt, const IntrinsicParams &intrinsics,
- const ExtrinsicParams &extrinsics) {
- const double y = extrinsics.y; // height
- const double z = extrinsics.z; // distance
- const double r1 = extrinsics.r1; // skew
- const double r2 = extrinsics.r2; // heading
- const double rup = intrinsics.mount_angle;
- const double rbarrel = intrinsics.barrel_mount;
- const double fl = intrinsics.focal_length;
-
- // Start by translating point in target-space to be at correct height.
- ::Eigen::Matrix<double, 1, 3> pts{pt.x(), pt.y() + y, 0.0};
-
- {
- // Rotate to compensate for skew angle, to get into a frame still at the
- // same (x, y) position as the target but rotated to be facing straight
- // towards the camera.
- const double theta = r1;
- const double s = sin(theta);
- const double c = cos(theta);
- pts = (::Eigen::Matrix<double, 3, 3>() << c, 0, -s, 0, 1, 0, s, 0,
- c).finished() *
- pts.transpose();
- }
-
- // Translate the coordinate frame to have (x, y) centered at the camera, but
- // still oriented to be facing along the line from the camera to the target.
- pts(2) += z;
-
- {
- // Rotate out the heading so that the frame is oriented to line up with the
- // camera's viewpoint in the yaw-axis.
- const double theta = r2;
- const double s = sin(theta);
- const double c = cos(theta);
- pts = (::Eigen::Matrix<double, 3, 3>() << c, 0, -s, 0, 1, 0, s, 0,
- c).finished() *
- pts.transpose();
- }
-
- // TODO: Apply 15 degree downward rotation.
- {
- // Compensate for rotation in the pitch of the camera up/down to get into
- // the coordinate frame lined up with the plane of the camera sensor.
- const double theta = rup;
- const double s = sin(theta);
- const double c = cos(theta);
-
- pts = (::Eigen::Matrix<double, 3, 3>() << 1, 0, 0, 0, c, -s, 0, s,
- c).finished() *
- pts.transpose();
- }
-
- // Compensate for rotation of the barrel of the camera, i.e. about the axis
- // that points straight out from the camera lense, using an AngleAxis instead
- // of manually constructing the rotation matrices because once you get into
- // this frame you no longer need to be masochistic.
- // TODO: Maybe barrel should be extrinsics to allow rocking?
- // Also, in this case, barrel should go above the rotation above?
- pts = ::Eigen::AngleAxis<double>(rbarrel, ::Eigen::Vector3d(0.0, 0.0, 1.0)) *
- pts.transpose();
-
- // TODO: Final image projection.
- const ::Eigen::Matrix<double, 1, 3> res = pts;
-
- // Finally, scale to account for focal length and translate to get into
- // pixel-space.
- const float scale = fl / res.z();
- return Vector<2>(res.x() * scale + 320.0, 240.0 - res.y() * scale);
-}
-
Target Project(const Target &target, const IntrinsicParams &intrinsics,
const ExtrinsicParams &extrinsics) {
Target new_targ;
@@ -150,11 +78,13 @@
IntrinsicParams intrinsics)
: result(result), template_pt(template_pt), intrinsics(intrinsics) {}
- bool operator()(const double *const x, double *residual) const {
- auto extrinsics = ExtrinsicParams::get(x);
- auto pt = result - Project(template_pt, intrinsics, extrinsics);
- residual[0] = pt.x();
- residual[1] = pt.y();
+ template <typename T>
+ bool operator()(const T *const x, T *residual) const {
+ const auto extrinsics = TemplatedExtrinsicParams<T>::get(x);
+ ::Eigen::Matrix<T, 2, 1> pt =
+ Project<T>(ToEigenMatrix<T>(template_pt), intrinsics, extrinsics);
+ residual[0] = result.x() - pt(0, 0);
+ residual[1] = result.y() - pt(1, 0);
return true;
}
@@ -170,15 +100,18 @@
IntrinsicParams intrinsics)
: result(result), template_seg(template_seg), intrinsics(intrinsics) {}
- bool operator()(const double *const x, double *residual) const {
- const auto extrinsics = ExtrinsicParams::get(x);
- const Vector<2> p1 = Project(template_seg.A(), intrinsics, extrinsics);
- const Vector<2> p2 = Project(template_seg.B(), intrinsics, extrinsics);
- // distance from line (P1, P2) to point result
- double dx = p2.x() - p1.x();
- double dy = p2.y() - p1.y();
- double denom = p2.DistanceTo(p1);
- residual[0] = ::std::abs(dy * result.x() - dx * result.y() +
+ template <typename T>
+ bool operator()(const T *const x, T *residual) const {
+ const auto extrinsics = TemplatedExtrinsicParams<T>::get(x);
+ const ::Eigen::Matrix<T, 2, 1> p1 =
+ Project<T>(ToEigenMatrix<T>(template_seg.A()), intrinsics, extrinsics);
+ const ::Eigen::Matrix<T, 2, 1> p2 =
+ Project<T>(ToEigenMatrix<T>(template_seg.B()), intrinsics, extrinsics);
+ // Distance from line(P1, P2) to point result
+ T dx = p2.x() - p1.x();
+ T dy = p2.y() - p1.y();
+ T denom = (p2-p1).norm();
+ residual[0] = ceres::abs(dy * result.x() - dx * result.y() +
p2.x() * p1.y() - p2.y() * p1.x()) /
denom;
return true;
@@ -199,24 +132,28 @@
template_seg_(template_seg),
intrinsics_(intrinsics) {}
- bool operator()(const double *const x, double *residual) const {
- const ExtrinsicParams extrinsics = ExtrinsicParams::get(x);
- const Vector<2> p1 = Project(template_seg_.A(), intrinsics_, extrinsics);
- const Vector<2> p2 = Project(template_seg_.B(), intrinsics_, extrinsics);
+ template <typename T>
+ bool operator()(const T *const x, T *residual) const {
+ const auto extrinsics = TemplatedExtrinsicParams<T>::get(x);
+ const ::Eigen::Matrix<T, 2, 1> p1 = Project<T>(
+ ToEigenMatrix<T>(template_seg_.A()), intrinsics_, extrinsics);
+ const ::Eigen::Matrix<T, 2, 1> p2 = Project<T>(
+ ToEigenMatrix<T>(template_seg_.B()), intrinsics_, extrinsics);
// Construct a vector pointed perpendicular to the line. This vector is
// pointed down out the bottom of the target.
- ::Eigen::Vector2d down_axis(-(p1.y() - p2.y()), p1.x() - p2.x());
+ ::Eigen::Matrix<T, 2, 1> down_axis;
+ down_axis << -(p1.y() - p2.y()), p1.x() - p2.x();
down_axis.normalize();
// Positive means out.
- const double component =
- down_axis.transpose() * (bottom_point_ - p1.GetData().transpose());
+ const T component =
+ down_axis.transpose() * (bottom_point_ - p1);
- if (component > 0) {
+ if (component > T(0)) {
residual[0] = component * 1.0;
} else {
- residual[0] = 0.0;
+ residual[0] = T(0);
}
return true;
}
@@ -254,18 +191,18 @@
aos::vision::Segment<2> line = Segment<2>(a, a2);
problem_4point.AddResidualBlock(
- new NumericDiffCostFunction<LineCostFunctor, CENTRAL, 1, 4>(
+ new ceres::AutoDiffCostFunction<LineCostFunctor, 1, 4>(
new LineCostFunctor(b, line, intrinsics_)),
NULL, ¶ms_4point[0]);
} else {
problem_4point.AddResidualBlock(
- new NumericDiffCostFunction<PointCostFunctor, CENTRAL, 2, 4>(
+ new ceres::AutoDiffCostFunction<PointCostFunctor, 2, 4>(
new PointCostFunctor(b, a, intrinsics_)),
NULL, ¶ms_4point[0]);
}
problem_8point.AddResidualBlock(
- new NumericDiffCostFunction<PointCostFunctor, CENTRAL, 2, 4>(
+ new ceres::AutoDiffCostFunction<PointCostFunctor, 2, 4>(
new PointCostFunctor(b, a, intrinsics_)),
NULL, ¶ms_8point[0]);
}
@@ -284,7 +221,7 @@
// Now, add a large cost for the bottom point being below the bottom line.
problem_4point.AddResidualBlock(
- new NumericDiffCostFunction<BottomPointCostFunctor, CENTRAL, 1, 4>(
+ new ceres::AutoDiffCostFunction<BottomPointCostFunctor, 1, 4>(
new BottomPointCostFunctor(target.left.bottom_point,
Segment<2>(target_template_.left.outside,
target_template_.left.bottom),
@@ -293,7 +230,7 @@
// Make sure to point the segment the other way so when we do a -pi/2 rotation
// on the line, it points down in target space.
problem_4point.AddResidualBlock(
- new NumericDiffCostFunction<BottomPointCostFunctor, CENTRAL, 1, 4>(
+ new ceres::AutoDiffCostFunction<BottomPointCostFunctor, 1, 4>(
new BottomPointCostFunctor(target.right.bottom_point,
Segment<2>(target_template_.right.bottom,
target_template_.right.outside),
diff --git a/y2019/vision/target_sender.cc b/y2019/vision/target_sender.cc
index 12e0e09..79e2ca4 100644
--- a/y2019/vision/target_sender.cc
+++ b/y2019/vision/target_sender.cc
@@ -1,6 +1,10 @@
#include "y2019/vision/target_finder.h"
+#include <condition_variable>
#include <fstream>
+#include <mutex>
+#include <random>
+#include <thread>
#include "aos/vision/blob/codec.h"
#include "aos/vision/blob/find_blob.h"
@@ -8,10 +12,10 @@
#include "aos/vision/events/udp.h"
#include "y2019/jevois/camera/image_stream.h"
#include "y2019/jevois/camera/reader.h"
-
#include "y2019/jevois/serial.h"
#include "y2019/jevois/structures.h"
#include "y2019/jevois/uart.h"
+#include "y2019/vision/image_writer.h"
// This has to be last to preserve compatibility with other headers using AOS
// logging.
@@ -66,6 +70,96 @@
using aos::vision::ImageRange;
using aos::vision::RangeImage;
using aos::vision::ImageFormat;
+using y2019::vision::TargetFinder;
+using y2019::vision::IntermediateResult;
+using y2019::vision::Target;
+
+class TargetProcessPool {
+ public:
+ // The number of threads we'll use.
+ static constexpr int kThreads = 4;
+
+ TargetProcessPool(TargetFinder *finder);
+ ~TargetProcessPool();
+
+ std::vector<IntermediateResult> Process(std::vector<const Target *> &&inputs,
+ bool verbose);
+
+ private:
+ // The main function for a thread.
+ void RunThread();
+
+ std::array<std::thread, kThreads> threads_;
+ // Coordinates access to results_/inputs_ and coordinates with
+ // condition_variable_.
+ std::mutex mutex_;
+ // Signals changes to results_/inputs_ and quit_.
+ std::condition_variable condition_variable_;
+ bool quit_ = false;
+
+ bool verbose_ = false;
+ std::vector<const Target *> inputs_;
+ std::vector<IntermediateResult> results_;
+
+ TargetFinder *const finder_;
+};
+
+TargetProcessPool::TargetProcessPool(TargetFinder *finder) : finder_(finder) {
+ for (int i = 0; i < kThreads; ++i) {
+ threads_[i] = std::thread([this]() { RunThread(); });
+ }
+}
+
+TargetProcessPool::~TargetProcessPool() {
+ {
+ std::unique_lock<std::mutex> locker(mutex_);
+ quit_ = true;
+ condition_variable_.notify_all();
+ }
+ for (int i = 0; i < kThreads; ++i) {
+ threads_[i].join();
+ }
+}
+
+std::vector<IntermediateResult> TargetProcessPool::Process(
+ std::vector<const Target *> &&inputs, bool verbose) {
+ inputs_ = std::move(inputs);
+ results_.clear();
+ verbose_ = verbose;
+ const size_t number_targets = inputs_.size();
+ {
+ std::unique_lock<std::mutex> locker(mutex_);
+ condition_variable_.notify_all();
+ while (results_.size() < number_targets) {
+ condition_variable_.wait(locker);
+ }
+ }
+ return std::move(results_);
+}
+
+void TargetProcessPool::RunThread() {
+ while (true) {
+ const Target *my_input;
+ {
+ std::unique_lock<std::mutex> locker(mutex_);
+ while (inputs_.empty()) {
+ if (quit_) {
+ return;
+ }
+ condition_variable_.wait(locker);
+ }
+ my_input = inputs_.back();
+ inputs_.pop_back();
+ }
+ IntermediateResult my_output =
+ finder_->ProcessTargetToResult(*my_input, false);
+ {
+ std::unique_lock<std::mutex> locker(mutex_);
+ results_.emplace_back(std::move(my_output));
+ condition_variable_.notify_all();
+ }
+ }
+}
int main(int argc, char **argv) {
(void)argc;
@@ -82,17 +176,25 @@
// dup2(itsDev, 1);
// dup2(itsDev, 2);
- TargetFinder finder_;
+ TargetFinder finder;
+ TargetProcessPool process_pool(&finder);
+ ImageWriter writer;
+ uint32_t image_count = 0;
+ bool log_images = false;
aos::vision::CameraParams params0;
- params0.set_exposure(50);
+ params0.set_exposure(60);
params0.set_brightness(40);
params0.set_width(640);
- params0.set_fps(15);
+ params0.set_fps(25);
params0.set_height(480);
aos::vision::FastYuyvYPooledThresholder thresholder;
+ // A source of psuedorandom numbers which gives different numbers each time we
+ // need to drop targets.
+ std::minstd_rand random_engine;
+
::std::unique_ptr<CameraStream> camera0(
new CameraStream(params0, "/dev/video0"));
camera0->set_on_frame([&](DataRef data,
@@ -100,18 +202,18 @@
aos::vision::ImageFormat fmt{640, 480};
aos::vision::BlobList imgs =
aos::vision::FindBlobs(thresholder.Threshold(fmt, data.data(), 120));
- finder_.PreFilter(&imgs);
+ finder.PreFilter(&imgs);
LOG(INFO) << "Blobs: " << imgs.size();
constexpr bool verbose = false;
::std::vector<Polygon> raw_polys;
for (const RangeImage &blob : imgs) {
// Convert blobs to contours in the corrected space.
- ContourNode *contour = finder_.GetContour(blob);
+ ContourNode *contour = finder.GetContour(blob);
::std::vector<::Eigen::Vector2f> unwarped_contour =
- finder_.UnWarpContour(contour);
+ finder.UnWarpContour(contour);
const Polygon polygon =
- finder_.FindPolygon(::std::move(unwarped_contour), verbose);
+ finder.FindPolygon(::std::move(unwarped_contour), verbose);
if (!polygon.segments.empty()) {
raw_polys.push_back(polygon);
}
@@ -120,22 +222,44 @@
// Calculate each component side of a possible target.
::std::vector<TargetComponent> target_component_list =
- finder_.FillTargetComponentList(raw_polys, verbose);
+ finder.FillTargetComponentList(raw_polys, verbose);
LOG(INFO) << "Components: " << 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: " << target_list.size();
+ finder.FindTargetsFromComponents(target_component_list, verbose);
+ static constexpr size_t kMaximumPotentialTargets = 8;
+ LOG(INFO) << "Potential Targets (will filter to "
+ << kMaximumPotentialTargets << "): " << target_list.size();
+
+ // A list of all the indices into target_list which we're going to actually
+ // use.
+ std::vector<int> target_list_indices;
+ target_list_indices.resize(target_list.size());
+ for (size_t i = 0; i < target_list.size(); ++i) {
+ target_list_indices[i] = i;
+ }
+ // Drop random elements until we get sufficiently few of them. We drop
+ // different elements each time to ensure we will see different valid
+ // targets on successive frames, which provides more useful information to
+ // the localization.
+ while (target_list_indices.size() > kMaximumPotentialTargets) {
+ std::uniform_int_distribution<size_t> distribution(
+ 0, target_list_indices.size() - 1);
+ const size_t index = distribution(random_engine);
+ target_list_indices.erase(target_list_indices.begin() + index);
+ }
// 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));
+ std::vector<const Target *> inputs;
+ for (size_t index : target_list_indices) {
+ inputs.push_back(&target_list[index]);
}
+ std::vector<IntermediateResult> results =
+ process_pool.Process(std::move(inputs), verbose);
LOG(INFO) << "Raw Results: " << results.size();
- results = finder_.FilterResults(results, 30, verbose);
+ results = finder.FilterResults(results, 30, verbose);
LOG(INFO) << "Results: " << results.size();
// TODO: Select top 3 (randomly?)
@@ -167,6 +291,13 @@
LOG(INFO) << "Some problem happened";
}
}
+
+ if (log_images) {
+ if ((image_count % 5) == 0) {
+ writer.WriteImage(data);
+ }
+ ++image_count;
+ }
});
aos::events::EpollLoop loop;
@@ -187,7 +318,7 @@
frc971::jevois::UartUnpackToCamera(packet);
if (calibration_question) {
const auto &calibration = *calibration_question;
- IntrinsicParams *intrinsics = finder_.mutable_intrinsics();
+ IntrinsicParams *intrinsics = finder.mutable_intrinsics();
intrinsics->mount_angle = calibration.calibration(0, 0);
intrinsics->focal_length = calibration.calibration(0, 1);
intrinsics->barrel_mount = calibration.calibration(0, 2);
@@ -195,6 +326,10 @@
switch (calibration.camera_command) {
case CameraCommand::kNormal:
case CameraCommand::kAs:
+ log_images = false;
+ break;
+ case CameraCommand::kLog:
+ log_images = true;
break;
case CameraCommand::kUsb:
return 0;
diff --git a/y2019/vision/target_types.h b/y2019/vision/target_types.h
index e97c051..aba9300 100644
--- a/y2019/vision/target_types.h
+++ b/y2019/vision/target_types.h
@@ -1,6 +1,8 @@
#ifndef _Y2019_VISION_TARGET_TYPES_H_
#define _Y2019_VISION_TARGET_TYPES_H_
+#include "Eigen/Dense"
+
#include "aos/vision/math/segment.h"
#include "aos/vision/math/vector.h"
#include "y2019/vision/constants.h"
@@ -54,28 +56,29 @@
std::array<aos::vision::Vector<2>, 8> ToPointList() const;
};
-struct ExtrinsicParams {
+template <typename Scalar>
+struct TemplatedExtrinsicParams {
static constexpr size_t kNumParams = 4;
// Height of the target
- double y = 18.0 * 0.0254;
+ Scalar y = Scalar(18.0 * 0.0254);
// Distance to the target
- double z = 23.0 * 0.0254;
+ Scalar z = Scalar(23.0 * 0.0254);
// Skew of the target relative to the line-of-sight from the camera to the
// target.
- double r1 = 1.0 / 180 * M_PI;
+ Scalar r1 = Scalar(1.0 / 180 * M_PI);
// Heading from the camera to the target, relative to the center of the view
// from the camera.
- double r2 = -1.0 / 180 * M_PI;
+ Scalar r2 = Scalar(-1.0 / 180 * M_PI);
- void set(double *data) {
+ void set(Scalar *data) {
data[0] = y;
data[1] = z;
data[2] = r1;
data[3] = r2;
}
- static ExtrinsicParams get(const double *data) {
- ExtrinsicParams out;
+ static TemplatedExtrinsicParams get(const Scalar *data) {
+ TemplatedExtrinsicParams out;
out.y = data[0];
out.z = data[1];
out.r1 = data[2];
@@ -84,10 +87,18 @@
}
};
+using ExtrinsicParams = TemplatedExtrinsicParams<double>;
+
// Projects a point from idealized template space to camera space.
+template <typename Extrinsics>
aos::vision::Vector<2> Project(aos::vision::Vector<2> pt,
- const IntrinsicParams &intrinsics,
- const ExtrinsicParams &extrinsics);
+ const IntrinsicParams &intrinsics,
+ const Extrinsics &extrinsics);
+
+template <typename T, typename Extrinsics>
+::Eigen::Matrix<T, 2, 1> Project(::Eigen::Matrix<T, 2, 1> pt,
+ const IntrinsicParams &intrinsics,
+ const Extrinsics &extrinsics);
Target Project(const Target &target, const IntrinsicParams &intrinsics,
const ExtrinsicParams &extrinsics);
@@ -130,6 +141,96 @@
float skew;
};
+template<typename T>
+::Eigen::Matrix<T, 2, 1> ToEigenMatrix(aos::vision::Vector<2> pt) {
+ return (::Eigen::Matrix<T, 2, 1>() << T(pt.x()), T(pt.y())).finished();
+}
+
+template <typename Extrinsics>
+aos::vision::Vector<2> Project(aos::vision::Vector<2> pt,
+ const IntrinsicParams &intrinsics,
+ const Extrinsics &extrinsics) {
+ const ::Eigen::Matrix<double, 2, 1> eigen_pt = ToEigenMatrix<double>(pt);
+ const ::Eigen::Matrix<double, 2, 1> res =
+ Project(eigen_pt, intrinsics, extrinsics);
+ return aos::vision::Vector<2>(res(0, 0), res(1, 0));
+}
+
+template <typename T, typename Extrinsics>
+::Eigen::Matrix<T, 2, 1> Project(::Eigen::Matrix<T, 2, 1> pt,
+ const IntrinsicParams &intrinsics,
+ const Extrinsics &extrinsics) {
+ const T y = extrinsics.y; // height
+ const T z = extrinsics.z; // distance
+ const T r1 = extrinsics.r1; // skew
+ const T r2 = extrinsics.r2; // heading
+ const double rup = intrinsics.mount_angle;
+ const double rbarrel = intrinsics.barrel_mount;
+ const double fl = intrinsics.focal_length;
+
+ // Start by translating point in target-space to be at correct height.
+ ::Eigen::Matrix<T, 3, 1> pts{pt(0, 0), pt(1, 0) + y, T(0.0)};
+
+ {
+ // Rotate to compensate for skew angle, to get into a frame still at the
+ // same (x, y) position as the target but rotated to be facing straight
+ // towards the camera.
+ const T theta = r1;
+ const T s = sin(theta);
+ const T c = cos(theta);
+ pts = (::Eigen::Matrix<T, 3, 3>() << c, T(0), -s, T(0), T(1), T(0), s, T(0),
+ c).finished() *
+ pts;
+ }
+
+ // Translate the coordinate frame to have (x, y) centered at the camera, but
+ // still oriented to be facing along the line from the camera to the target.
+ pts(2) += z;
+
+ {
+ // Rotate out the heading so that the frame is oriented to line up with the
+ // camera's viewpoint in the yaw-axis.
+ const T theta = r2;
+ const T s = sin(theta);
+ const T c = cos(theta);
+ pts = (::Eigen::Matrix<T, 3, 3>() << c, T(0), -s, T(0), T(1), T(0), s, T(0),
+ c).finished() *
+ pts;
+ }
+
+ // TODO: Apply 15 degree downward rotation.
+ {
+ // Compensate for rotation in the pitch of the camera up/down to get into
+ // the coordinate frame lined up with the plane of the camera sensor.
+ const double theta = rup;
+ const T s = T(sin(theta));
+ const T c = T(cos(theta));
+
+ pts = (::Eigen::Matrix<T, 3, 3>() << T(1), T(0), T(0), T(0), c, -s, T(0), s,
+ c).finished() *
+ pts;
+ }
+
+ // Compensate for rotation of the barrel of the camera, i.e. about the axis
+ // that points straight out from the camera lense, using an AngleAxis instead
+ // of manually constructing the rotation matrices because once you get into
+ // this frame you no longer need to be masochistic.
+ // TODO: Maybe barrel should be extrinsics to allow rocking?
+ // Also, in this case, barrel should go above the rotation above?
+ pts = ::Eigen::AngleAxis<T>(T(rbarrel),
+ ::Eigen::Matrix<T, 3, 1>(T(0), T(0), T(1))) *
+ pts;
+
+ // TODO: Final image projection.
+ const ::Eigen::Matrix<T, 3, 1> res = pts;
+
+ // Finally, scale to account for focal length and translate to get into
+ // pixel-space.
+ const T scale = fl / res.z();
+ return ::Eigen::Matrix<T, 2, 1>(res.x() * scale + 320.0,
+ 240.0 - res.y() * scale);
+}
+
} // namespace vision
} // namespace y2019
diff --git a/y2019/wpilib_interface.cc b/y2019/wpilib_interface.cc
index e75b825..95ed28e 100644
--- a/y2019/wpilib_interface.cc
+++ b/y2019/wpilib_interface.cc
@@ -314,10 +314,13 @@
using namespace frc971::jevois;
RoborioToTeensy to_teensy{};
to_teensy.realtime_now = aos::realtime_clock::now();
+ camera_log.FetchLatest();
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 if (camera_log.get() && camera_log->log) {
+ to_teensy.camera_command = CameraCommand::kLog;
} else {
to_teensy.camera_command = CameraCommand::kNormal;
}