Merge changes Ia48a981e,Id42efc3e,If0076f30,I7c63ebcf,I77f4ba65
* changes:
Use cogging constants per pistol grip
Allow button boards to be used for the USB stack.
Calibrate based on pistol grip id
Added function to measure cogging
Move motor print debugging code to the application
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..86539bd
--- /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/control_loops/python/elevator.py b/y2019/control_loops/python/elevator.py
index 5859814..0a4e208 100755
--- a/y2019/control_loops/python/elevator.py
+++ b/y2019/control_loops/python/elevator.py
@@ -31,15 +31,20 @@
kalman_q_voltage=35.0,
kalman_r_position=0.05)
+kElevatorBall = copy.copy(kElevator)
+kElevatorBall.q_pos = 0.15
+kElevatorBall.q_vel = 1.5
+
kElevatorModel = copy.copy(kElevator)
kElevatorModel.mass = carriage_mass + first_stage_mass + 1.0
+
def main(argv):
if FLAGS.plot:
R = numpy.matrix([[1.5], [0.0]])
- linear_system.PlotKick(kElevator, R, plant_params=kElevatorModel)
+ linear_system.PlotKick(kElevatorBall, R, plant_params=kElevatorModel)
linear_system.PlotMotion(
- kElevator, R, max_velocity=5.0, plant_params=kElevatorModel)
+ kElevatorBall, R, max_velocity=5.0, plant_params=kElevatorModel)
# Write the generated constants out to a file.
if len(argv) != 5:
@@ -48,8 +53,8 @@
)
else:
namespaces = ['y2019', 'control_loops', 'superstructure', 'elevator']
- linear_system.WriteLinearSystem(kElevator, argv[1:3], argv[3:5],
- namespaces)
+ linear_system.WriteLinearSystem([kElevator, kElevatorBall, kElevator],
+ argv[1:3], argv[3:5], namespaces)
if __name__ == '__main__':
diff --git a/y2019/control_loops/python/wrist.py b/y2019/control_loops/python/wrist.py
index 66b7b26..127def7 100755
--- a/y2019/control_loops/python/wrist.py
+++ b/y2019/control_loops/python/wrist.py
@@ -29,7 +29,7 @@
name='Wrist',
motor=control_loop.BAG(),
G=(6.0 / 60.0) * (20.0 / 100.0) * (24.0 / 84.0),
- J=0.27,
+ J=0.30,
q_pos=0.20,
q_vel=5.0,
kalman_q_pos=0.12,
@@ -37,6 +37,14 @@
kalman_q_voltage=4.0,
kalman_r_position=0.05)
+kWristBall = copy.copy(kWrist)
+kWristBall.J = 0.4007
+kWristBall.q_pos = 0.55
+kWristBall.q_vel = 5.0
+
+kWristPanel = copy.copy(kWrist)
+kWristPanel.J = 0.446
+
kWristModel = copy.copy(kWrist)
kWristModel.J = 0.1348
@@ -44,8 +52,8 @@
def main(argv):
if FLAGS.plot:
R = numpy.matrix([[numpy.pi / 2.0], [0.0]])
- angular_system.PlotKick(kWrist, R, plant_params=kWristModel)
- angular_system.PlotMotion(kWrist, R, plant_params=kWristModel)
+ angular_system.PlotKick(kWristBall, R, plant_params=kWristBall)
+ angular_system.PlotMotion(kWristBall, R, plant_params=kWristBall)
# Write the generated constants out to a file.
if len(argv) != 5:
@@ -54,8 +62,8 @@
)
else:
namespaces = ['y2019', 'control_loops', 'superstructure', 'wrist']
- angular_system.WriteAngularSystem(kWrist, argv[1:3], argv[3:5],
- namespaces)
+ angular_system.WriteAngularSystem([kWrist, kWristBall, kWristPanel],
+ argv[1:3], argv[3:5], namespaces)
if __name__ == '__main__':
diff --git a/y2019/control_loops/superstructure/superstructure.cc b/y2019/control_loops/superstructure/superstructure.cc
index 8fe02a3..b6ad9e8 100644
--- a/y2019/control_loops/superstructure/superstructure.cc
+++ b/y2019/control_loops/superstructure/superstructure.cc
@@ -83,6 +83,19 @@
}
}
+ if (unsafe_goal) {
+ if (!unsafe_goal->suction.grab_piece) {
+ wrist_.set_controller_index(0);
+ elevator_.set_controller_index(0);
+ } else if (unsafe_goal->suction.gamepiece_mode == 0) {
+ wrist_.set_controller_index(1);
+ elevator_.set_controller_index(1);
+ } else {
+ wrist_.set_controller_index(2);
+ elevator_.set_controller_index(2);
+ }
+ }
+
// TODO(theo) move these up when Iterate() is split
// update the goals
collision_avoidance_.UpdateGoal(status, unsafe_goal);
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/vision/undistort.py b/y2019/vision/undistort.py
new file mode 100755
index 0000000..9a35d9c
--- /dev/null
+++ b/y2019/vision/undistort.py
@@ -0,0 +1,135 @@
+#!/usr/bin/python
+
+import cv2
+import glob
+import math
+import numpy as np
+import sys
+"""
+Usage:
+ undistort.py [display]
+
+Finds files in /tmp/*.yuyv to compute distortion constants for.
+"""
+
+
+def undist(orig, mtx, dist, newcameramtx, its=1):
+ """
+ This function runs a manual undistort over the entire image to compare to the
+ golden as proof that the algorithm works and the generated constants are correct.
+ """
+ output = np.full(orig.shape, 255, dtype=np.uint8)
+ for i in range(480):
+ for j in range(640):
+ x0 = (i - mtx[1, 2]) / mtx[1, 1]
+ y0 = (j - mtx[0, 2]) / mtx[0, 0]
+ x = x0
+ y = y0
+ for k in range(its):
+ r2 = x * x + y * y
+ coeff = (1 + dist[0, 0] * r2 + dist[0, 1] * math.pow(r2, 2) +
+ dist[0, 4] * math.pow(r2, 3))
+ x = x0 / coeff
+ y = y0 / coeff
+ ip = x * newcameramtx[1, 1] + newcameramtx[1, 2]
+ jp = y * newcameramtx[0, 0] + newcameramtx[0, 2]
+ if ip < 0 or jp < 0 or ip >= 480 or jp >= 640:
+ continue
+ output[int(ip), int(jp)] = orig[i, j]
+ return output
+
+
+def main(argv):
+ # prepare object points, like (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0)
+ objp = np.zeros((6 * 9, 3), np.float32)
+ objp[:, :2] = np.mgrid[0:9, 0:6].T.reshape(-1, 2)
+
+ # Arrays to store object points and image points from all the images.
+ objpoints = [] # 3d point in real world space
+ imgpoints = [] # 2d points in image plane.
+
+ images = glob.glob('/tmp/*.yuyv')
+
+ cols = 640
+ rows = 480
+
+ # Iterate through all the available images
+ for fname in images:
+ fd = open(fname, 'rb')
+ f = np.fromfile(fd, np.uint8, cols * rows * 2)
+ # Convert yuyv color space to single channel grey.
+ grey = f[::2]
+ grey = np.reshape(grey, (rows, cols))
+
+ ret, corners = cv2.findChessboardCorners(grey, (9, 6), None)
+ if ret:
+ objpoints.append(objp)
+ imgpoints.append(corners)
+ # Draw the chessboard with corners marked.
+ if len(argv) > 1 and argv[1] == 'display':
+ rgb = cv2.cvtColor(grey, cv2.COLOR_GRAY2RGB)
+ cv2.drawChessboardCorners(rgb, (9, 6), corners, ret)
+ cv2.imshow('', rgb)
+ cv2.waitKey(0)
+ cv2.destroyAllWindows()
+ fd.close()
+
+ ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(
+ objpoints, imgpoints, grey.shape[::-1], None, None)
+ newcameramtx, roi = cv2.getOptimalNewCameraMatrix(mtx, dist, (rows, cols),
+ 1, (rows, cols))
+
+ dist[0, 2] = 0
+ dist[0, 3] = 0
+ print("Formatted for Game Config:")
+ print("""distortion {
+ f_x: %f
+ c_x: %f
+ f_y: %f
+ c_y :%f
+ f_x_prime: %f
+ c_x_prime: %f
+ f_y_prime: %f
+ c_y_prime: %f
+ k_1: %f
+ k_2: %f
+ k_3: %f
+ distortion_iterations: 7
+}""" % (
+ # f_x c_x
+ mtx[0][0],
+ mtx[0][2],
+ # f_y c_y
+ mtx[1][1],
+ mtx[1][2],
+ # f_x c_x prime
+ newcameramtx[0][0],
+ newcameramtx[0][2],
+ # f_y c_y prime
+ newcameramtx[1][1],
+ newcameramtx[1][2],
+ # k_1, k_2, k_3
+ dist[0, 0],
+ dist[0, 1],
+ dist[0, 4]))
+
+ # Draw the original image, open-cv undistort, and our undistort in separate
+ # windows for each available image.
+ if len(argv) > 1 and argv[1] == 'display':
+ for fname in images:
+ fd = open(fname, 'rb')
+ f = np.fromfile(fd, np.uint8, cols * rows * 2)
+ grey_t = f[::2]
+ grey_t = np.reshape(grey_t, (rows, cols))
+ dst_expected = cv2.undistort(grey_t, mtx, dist, None, newcameramtx)
+ dst_actual = undist(grey_t, mtx, dist, newcameramtx, 5)
+ cv2.imshow('orig', grey_t)
+ cv2.imshow('opencv undistort', dst_expected)
+ cv2.imshow('our undistort', dst_actual)
+ cv2.waitKey(0)
+ cv2.destroyAllWindows()
+ fd.close()
+
+
+if __name__ == '__main__':
+ main(sys.argv)
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;
}