Merge "Keep scouting data when returning to a page."
diff --git a/frc971/control_loops/drivetrain/drivetrain_plotter.ts b/frc971/control_loops/drivetrain/drivetrain_plotter.ts
index deb300f..4b59dc0 100644
--- a/frc971/control_loops/drivetrain/drivetrain_plotter.ts
+++ b/frc971/control_loops/drivetrain/drivetrain_plotter.ts
@@ -20,6 +20,8 @@
'/drivetrain', 'frc971.control_loops.drivetrain.Status');
const output = aosPlotter.addMessageSource(
'/drivetrain', 'frc971.control_loops.drivetrain.Output');
+ const gyroReading = aosPlotter.addMessageSource(
+ '/drivetrain', 'frc971.sensors.GyroReading');
const imu = aosPlotter.addRawMessageSource(
'/drivetrain', 'frc971.IMUValuesBatch',
new ImuMessageHandler(conn.getSchema('frc971.IMUValuesBatch')));
@@ -323,6 +325,7 @@
gyroY.setColor(GREEN);
const gyroZ = gyroPlot.addMessageLine(imu, ['gyro_z']);
gyroZ.setColor(BLUE);
+ gyroPlot.addMessageLine(gyroReading, ['velocity']).setColor(BLUE);
// IMU States
const imuStatePlot =
diff --git a/y2022/BUILD b/y2022/BUILD
index 488026e..b16067d 100644
--- a/y2022/BUILD
+++ b/y2022/BUILD
@@ -39,6 +39,7 @@
"//y2022/vision:viewer",
"//y2022/localizer:imu_main",
"//y2022/localizer:localizer_main",
+ "//y2022/vision:image_decimator",
],
data = [
":aos_config",
diff --git a/y2022/control_loops/superstructure/superstructure.cc b/y2022/control_loops/superstructure/superstructure.cc
index e45917a..c87cc95 100644
--- a/y2022/control_loops/superstructure/superstructure.cc
+++ b/y2022/control_loops/superstructure/superstructure.cc
@@ -179,6 +179,44 @@
}
}
+ // Send the turret to the loading position if we have a ball in the intake, or
+ // are trying to intake one
+ double turret_loading_position = std::numeric_limits<double>::quiet_NaN();
+ if (state_ == SuperstructureState::TRANSFERRING &&
+ intake_state_ != IntakeState::NO_BALL) {
+ turret_loading_position = (intake_state_ == IntakeState::INTAKE_FRONT_BALL
+ ? constants::Values::kTurretFrontIntakePos()
+ : constants::Values::kTurretBackIntakePos());
+ } else if (state_ == SuperstructureState::IDLE && unsafe_goal != nullptr) {
+ if (unsafe_goal->roller_speed_front() > 0) {
+ turret_loading_position = constants::Values::kTurretFrontIntakePos();
+ } else if (unsafe_goal->roller_speed_back() > 0) {
+ turret_loading_position = constants::Values::kTurretBackIntakePos();
+ }
+ }
+
+ if (!std::isnan(turret_loading_position)) {
+ // Turn to the loading position as close to the current position as
+ // possible
+ // Strategy is copied from frc971/control_loops/aiming/aiming.cc
+ turret_loading_position =
+ turret_.estimated_position() +
+ aos::math::NormalizeAngle(turret_loading_position -
+ turret_.estimated_position());
+ // if out of range, reset back to within +/- pi of zero.
+ if (turret_loading_position > constants::Values::kTurretRange().upper ||
+ turret_loading_position < constants::Values::kTurretRange().lower) {
+ turret_loading_position =
+ aos::math::NormalizeAngle(turret_loading_position);
+ }
+
+ turret_goal_buffer.Finish(
+ frc971::control_loops::
+ CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *turret_goal_buffer.fbb(), turret_loading_position));
+ turret_goal = &turret_goal_buffer.message();
+ }
+
switch (state_) {
case SuperstructureState::IDLE: {
if (is_spitting) {
@@ -203,31 +241,6 @@
break;
}
- double turret_loading_position =
- (intake_state_ == IntakeState::INTAKE_FRONT_BALL
- ? constants::Values::kTurretFrontIntakePos()
- : constants::Values::kTurretBackIntakePos());
-
- // Turn to the loading position as close to the current position as
- // possible
- // Strategy is copied from frc971/control_loops/aiming/aiming.cc
- turret_loading_position =
- turret_.estimated_position() +
- aos::math::NormalizeAngle(turret_loading_position -
- turret_.estimated_position());
- // if out of range, reset back to within +/- pi of zero.
- if (turret_loading_position > constants::Values::kTurretRange().upper ||
- turret_loading_position < constants::Values::kTurretRange().lower) {
- turret_loading_position =
- aos::math::NormalizeAngle(turret_loading_position);
- }
-
- turret_goal_buffer.Finish(
- frc971::control_loops::
- CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
- *turret_goal_buffer.fbb(), turret_loading_position));
- turret_goal = &turret_goal_buffer.message();
-
const bool turret_near_goal =
std::abs(turret_.estimated_position() - turret_loading_position) <
kTurretGoalThreshold;
diff --git a/y2022/control_loops/superstructure/superstructure_goal.fbs b/y2022/control_loops/superstructure/superstructure_goal.fbs
index 8862d22..7816563 100644
--- a/y2022/control_loops/superstructure/superstructure_goal.fbs
+++ b/y2022/control_loops/superstructure/superstructure_goal.fbs
@@ -26,8 +26,11 @@
intake_back:frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal (id: 2);
// Positive is rollers intaking.
+ // When spinning the rollers, the turret will be moved to that side,
+ // so both shouldn't be positive at the same time.
roller_speed_front:double (id: 3);
roller_speed_back:double (id: 4);
+
transfer_roller_speed_front:double (id: 5);
transfer_roller_speed_back:double (id: 12);
diff --git a/y2022/control_loops/superstructure/superstructure_lib_test.cc b/y2022/control_loops/superstructure/superstructure_lib_test.cc
index 66a615e..73ceee7 100644
--- a/y2022/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2022/control_loops/superstructure/superstructure_lib_test.cc
@@ -776,6 +776,7 @@
ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
EXPECT_EQ(superstructure_output_fetcher_->roller_voltage_front(), 12.0);
EXPECT_EQ(superstructure_output_fetcher_->roller_voltage_back(), 12.0);
+
EXPECT_EQ(superstructure_output_fetcher_->transfer_roller_voltage_front(),
0.0);
EXPECT_EQ(superstructure_output_fetcher_->transfer_roller_voltage_back(),
@@ -783,8 +784,10 @@
EXPECT_EQ(superstructure_status_fetcher_->state(), SuperstructureState::IDLE);
EXPECT_EQ(superstructure_status_fetcher_->intake_state(),
IntakeState::NO_BALL);
- EXPECT_NEAR(superstructure_status_fetcher_->turret()->position(), kTurretGoal,
- 0.001);
+ // Since we spun the front rollers, the turret should be trying to intake from
+ // there
+ EXPECT_NEAR(superstructure_status_fetcher_->turret()->position(),
+ constants::Values::kTurretFrontIntakePos(), 0.001);
EXPECT_EQ(superstructure_status_fetcher_->shot_count(), 0);
superstructure_plant_.set_intake_beambreak_front(true);
diff --git a/y2022/joystick_reader.cc b/y2022/joystick_reader.cc
index 4b2f082..2f707d7 100644
--- a/y2022/joystick_reader.cc
+++ b/y2022/joystick_reader.cc
@@ -60,8 +60,8 @@
const ButtonLocation kIntakeFrontOut(4, 10);
const ButtonLocation kIntakeBackOut(4, 9);
-const ButtonLocation kRedLocalizerReset(4, 13);
-const ButtonLocation kBlueLocalizerReset(4, 14);
+const ButtonLocation kRedLocalizerReset(4, 14);
+const ButtonLocation kBlueLocalizerReset(4, 13);
const ButtonLocation kLocalizerReset(3, 8);
#endif
@@ -83,16 +83,18 @@
setpoint_fetcher_(
event_loop->MakeFetcher<Setpoint>("/superstructure")) {}
+ // Localizer reset positions are with front of robot pressed up against driver
+ // station in the middle of the field side-to-side.
void BlueResetLocalizer() {
auto builder = localizer_control_sender_.MakeBuilder();
frc971::control_loops::drivetrain::LocalizerControl::Builder
localizer_control_builder = builder.MakeBuilder<
frc971::control_loops::drivetrain::LocalizerControl>();
- localizer_control_builder.add_x(7.4);
- localizer_control_builder.add_y(-1.7);
+ localizer_control_builder.add_x(-7.9);
+ localizer_control_builder.add_y(0.0);
localizer_control_builder.add_theta_uncertainty(10.0);
- localizer_control_builder.add_theta(0.0);
+ localizer_control_builder.add_theta(M_PI);
localizer_control_builder.add_keep_current_theta(false);
if (builder.Send(localizer_control_builder.Finish()) !=
aos::RawSender::Error::kOk) {
@@ -106,10 +108,10 @@
frc971::control_loops::drivetrain::LocalizerControl::Builder
localizer_control_builder = builder.MakeBuilder<
frc971::control_loops::drivetrain::LocalizerControl>();
- localizer_control_builder.add_x(-7.9);
- localizer_control_builder.add_y(0.7);
+ localizer_control_builder.add_x(7.9);
+ localizer_control_builder.add_y(0.0);
localizer_control_builder.add_theta_uncertainty(10.0);
- localizer_control_builder.add_theta(M_PI);
+ localizer_control_builder.add_theta(0.0);
localizer_control_builder.add_keep_current_theta(false);
if (builder.Send(localizer_control_builder.Finish()) !=
aos::RawSender::Error::kOk) {
diff --git a/y2022/localizer/imu.cc b/y2022/localizer/imu.cc
index fed9ceb..2310e99 100644
--- a/y2022/localizer/imu.cc
+++ b/y2022/localizer/imu.cc
@@ -124,9 +124,9 @@
// extra data from the pico
imu_builder.add_pico_timestamp_us(pico_timestamp);
imu_builder.add_left_encoder(
- constants::Values::DrivetrainEncoderToMeters(encoder1_count));
+ -constants::Values::DrivetrainEncoderToMeters(encoder2_count));
imu_builder.add_right_encoder(
- constants::Values::DrivetrainEncoderToMeters(encoder2_count));
+ constants::Values::DrivetrainEncoderToMeters(encoder1_count));
imu_builder.add_previous_reading_diag_stat(diag_stat_offset);
}
diff --git a/y2022/localizer/localizer.cc b/y2022/localizer/localizer.cc
index e7b6421..14de8af 100644
--- a/y2022/localizer/localizer.cc
+++ b/y2022/localizer/localizer.cc
@@ -678,6 +678,7 @@
aos::math::NormalizeAngle(camera_yaw + target->angle_to_target());
debug.accepted = true;
debug.image_age_sec = aos::time::DurationInSeconds(t_ - sample_time);
+ CHECK_LT(image_debugs_.size(), kDebugBufferSize);
image_debugs_.push_back(debug);
}
@@ -822,6 +823,7 @@
TargetEstimateDebugT debug;
debug.accepted = false;
debug.rejection_reason = reason;
+ CHECK_LT(image_debugs_.size(), kDebugBufferSize);
image_debugs_.push_back(debug);
}
@@ -864,6 +866,8 @@
clock_offset_fetcher_(
event_loop_->MakeFetcher<aos::message_bridge::ServerStatistics>(
"/aos")),
+ superstructure_fetcher_(event_loop_->MakeFetcher<y2022::control_loops::superstructure::Status>(
+ "/superstructure")),
left_encoder_(-DrivetrainWrapPeriod() / 2.0, DrivetrainWrapPeriod()),
right_encoder_(-DrivetrainWrapPeriod() / 2.0, DrivetrainWrapPeriod()) {
event_loop_->MakeWatcher(
@@ -876,45 +880,70 @@
model_based_.HandleReset(event_loop_->monotonic_now(),
{control.x(), control.y(), theta});
});
- event_loop_->MakeWatcher(
- "/superstructure",
- [this](const y2022::control_loops::superstructure::Status &status) {
- if (!status.has_turret()) {
- return;
- }
- CHECK(status.has_turret());
- model_based_.HandleTurret(event_loop_->context().monotonic_event_time,
- status.turret()->position(),
- status.turret()->velocity());
- });
+ aos::TimerHandler *superstructure_timer = event_loop_->AddTimer([this]() {
+ if (superstructure_fetcher_.Fetch()) {
+ const y2022::control_loops::superstructure::Status &status =
+ *superstructure_fetcher_.get();
+ if (!status.has_turret()) {
+ return;
+ }
+ CHECK(status.has_turret());
+ model_based_.HandleTurret(
+ superstructure_fetcher_.context().monotonic_event_time,
+ status.turret()->position(), status.turret()->velocity());
+ }
+ });
+ event_loop_->OnRun([this, superstructure_timer]() {
+ superstructure_timer->Setup(event_loop_->monotonic_now(),
+ std::chrono::milliseconds(20));
+ });
for (size_t camera_index = 0; camera_index < kPisToUse.size(); ++camera_index) {
- event_loop_->MakeWatcher(
- absl::StrCat("/", kPisToUse[camera_index], "/camera"),
- [this, camera_index](const y2022::vision::TargetEstimate &target) {
- const std::optional<aos::monotonic_clock::duration> monotonic_offset =
- ClockOffset(kPisToUse[camera_index]);
- if (!monotonic_offset.has_value()) {
- return;
- }
- // TODO(james): Get timestamp from message contents.
- aos::monotonic_clock::time_point capture_time(
- event_loop_->context().monotonic_remote_time - monotonic_offset.value());
- if (capture_time > event_loop_->context().monotonic_event_time) {
- model_based_.TallyRejection(RejectionReason::IMAGE_FROM_FUTURE);
- return;
- }
- model_based_.HandleImageMatch(capture_time, &target, camera_index);
+ CHECK_LT(camera_index, target_estimate_fetchers_.size());
+ target_estimate_fetchers_[camera_index] =
+ event_loop_->MakeFetcher<y2022::vision::TargetEstimate>(
+ absl::StrCat("/", kPisToUse[camera_index], "/camera"));
+ }
+ aos::TimerHandler *estimate_timer = event_loop_->AddTimer(
+ [this]() {
+ for (size_t camera_index = 0; camera_index < kPisToUse.size();
+ ++camera_index) {
if (model_based_.NumQueuedImageDebugs() ==
ModelBasedLocalizer::kDebugBufferSize ||
(last_visualization_send_ + kMinVisualizationPeriod <
event_loop_->monotonic_now())) {
auto builder = visualization_sender_.MakeBuilder();
- visualization_sender_.CheckOk(
- builder.Send(model_based_.PopulateVisualization(builder.fbb())));
+ visualization_sender_.CheckOk(builder.Send(
+ model_based_.PopulateVisualization(builder.fbb())));
}
- });
- }
+ if (target_estimate_fetchers_[camera_index].Fetch()) {
+ const std::optional<aos::monotonic_clock::duration>
+ monotonic_offset = ClockOffset(kPisToUse[camera_index]);
+ if (!monotonic_offset.has_value()) {
+ continue;
+ }
+ // TODO(james): Get timestamp from message contents.
+ aos::monotonic_clock::time_point capture_time(
+ target_estimate_fetchers_[camera_index]
+ .context()
+ .monotonic_remote_time -
+ monotonic_offset.value());
+ if (capture_time > target_estimate_fetchers_[camera_index]
+ .context()
+ .monotonic_event_time) {
+ model_based_.TallyRejection(RejectionReason::IMAGE_FROM_FUTURE);
+ continue;
+ }
+ model_based_.HandleImageMatch(
+ capture_time, target_estimate_fetchers_[camera_index].get(),
+ camera_index);
+ }
+ }
+ });
+ event_loop_->OnRun([this, estimate_timer]() {
+ estimate_timer->Setup(event_loop_->monotonic_now(),
+ std::chrono::milliseconds(100));
+ });
event_loop_->MakeWatcher(
"/localizer", [this](const frc971::IMUValuesBatch &values) {
CHECK(values.has_readings());
diff --git a/y2022/localizer/localizer.h b/y2022/localizer/localizer.h
index 4b7eff0..c19bf05 100644
--- a/y2022/localizer/localizer.h
+++ b/y2022/localizer/localizer.h
@@ -322,6 +322,10 @@
aos::Sender<LocalizerVisualization> visualization_sender_;
aos::Fetcher<frc971::control_loops::drivetrain::Output> output_fetcher_;
aos::Fetcher<aos::message_bridge::ServerStatistics> clock_offset_fetcher_;
+ std::array<aos::Fetcher<y2022::vision::TargetEstimate>, 4>
+ target_estimate_fetchers_;
+ aos::Fetcher<y2022::control_loops::superstructure::Status>
+ superstructure_fetcher_;
zeroing::ImuZeroer zeroer_;
aos::monotonic_clock::time_point last_output_send_ =
aos::monotonic_clock::min_time;
diff --git a/y2022/localizer/localizer_test.cc b/y2022/localizer/localizer_test.cc
index 791adf3..bf75446 100644
--- a/y2022/localizer/localizer_test.cc
+++ b/y2022/localizer/localizer_test.cc
@@ -849,19 +849,19 @@
drivetrain_plant_.set_accel_sin_magnitude(0.01);
drivetrain_plant_.mutable_state()->x() = 2.0;
drivetrain_plant_.mutable_state()->y() = 2.0;
- SendLocalizerControl(5.0, 3.0, 0.0);
+ SendLocalizerControl(6.0, 3.0, 0.0);
event_loop_factory_.RunFor(std::chrono::seconds(1));
CHECK(output_fetcher_.Fetch());
CHECK(status_fetcher_.Fetch());
ASSERT_FALSE(status_fetcher_->model_based()->using_model());
- EXPECT_FALSE(VerifyEstimatorAccurate(0.5));
+ EXPECT_FALSE(VerifyEstimatorAccurate(3.0));
send_targets_ = true;
event_loop_factory_.RunFor(std::chrono::seconds(4));
CHECK(status_fetcher_.Fetch());
ASSERT_FALSE(status_fetcher_->model_based()->using_model());
- EXPECT_TRUE(VerifyEstimatorAccurate(0.5));
+ EXPECT_TRUE(VerifyEstimatorAccurate(3.0));
// y should be noticeably more accurate than x, since we are just driving
// straight.
ASSERT_NEAR(drivetrain_plant_.state()(1), status_fetcher_->model_based()->y(), 0.1);
diff --git a/y2022/vision/BUILD b/y2022/vision/BUILD
index 4ca8561..bede03a 100644
--- a/y2022/vision/BUILD
+++ b/y2022/vision/BUILD
@@ -252,3 +252,15 @@
"//y2022/control_loops/superstructure:superstructure_status_fbs",
],
)
+
+cc_binary(
+ name = "image_decimator",
+ srcs = ["image_decimator.cc"],
+ visibility = ["//y2022:__subpackages__"],
+ deps = [
+ "//aos:flatbuffers",
+ "//aos:init",
+ "//aos/events:shm_event_loop",
+ "//frc971/vision:vision_fbs",
+ ],
+)
diff --git a/y2022/vision/image_decimator.cc b/y2022/vision/image_decimator.cc
new file mode 100644
index 0000000..5fda423
--- /dev/null
+++ b/y2022/vision/image_decimator.cc
@@ -0,0 +1,52 @@
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+#include "aos/flatbuffers.h"
+#include "frc971/vision/vision_generated.h"
+
+DEFINE_string(config, "aos_config.json", "Path to the config file to use.");
+
+namespace frc971::vision {
+// Reads images from /camera and resends them in /camera/decimated at a fixed
+// rate (1 Hz, in this case).
+class ImageDecimator {
+ public:
+ ImageDecimator(aos::EventLoop *event_loop)
+ : slow_image_sender_(
+ event_loop->MakeSender<CameraImage>("/camera/decimated")),
+ image_fetcher_(event_loop->MakeFetcher<CameraImage>("/camera")) {
+ aos::TimerHandler *timer =
+ event_loop->AddTimer(
+ [this]() {
+ if (image_fetcher_.Fetch()) {
+ const aos::FlatbufferSpan<CameraImage> image(
+ {reinterpret_cast<const uint8_t *>(
+ image_fetcher_.context().data),
+ image_fetcher_.context().size});
+ slow_image_sender_.CheckOk(slow_image_sender_.Send(image));
+ }
+ });
+ event_loop->OnRun([event_loop, timer]() {
+ timer->Setup(event_loop->monotonic_now(),
+ std::chrono::milliseconds(1000));
+ });
+ }
+
+ private:
+ aos::Sender<CameraImage> slow_image_sender_;
+ aos::Fetcher<CameraImage> image_fetcher_;
+};
+}
+
+int main(int argc, char *argv[]) {
+ aos::InitGoogle(&argc, &argv);
+
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig(FLAGS_config);
+
+ aos::ShmEventLoop event_loop(&config.message());
+ frc971::vision::ImageDecimator decimator(&event_loop);
+
+ event_loop.Run();
+
+ return 0;
+}
diff --git a/y2022/y2022_logger.json b/y2022/y2022_logger.json
index d6fa4a4..edf52ab 100644
--- a/y2022/y2022_logger.json
+++ b/y2022/y2022_logger.json
@@ -322,7 +322,7 @@
"max_size": 200
},
{
- "name": "/pi1/camera",
+ "name": "/pi1/camera/decimated",
"type": "frc971.vision.CameraImage",
"source_node": "pi1",
"logger": "LOCAL_AND_REMOTE_LOGGER",
@@ -338,7 +338,7 @@
]
},
{
- "name": "/pi2/camera",
+ "name": "/pi2/camera/decimated",
"type": "frc971.vision.CameraImage",
"source_node": "pi2",
"logger": "LOCAL_AND_REMOTE_LOGGER",
@@ -354,7 +354,7 @@
]
},
{
- "name": "/pi3/camera",
+ "name": "/pi3/camera/decimated",
"type": "frc971.vision.CameraImage",
"source_node": "pi3",
"logger": "LOCAL_AND_REMOTE_LOGGER",
@@ -370,7 +370,7 @@
]
},
{
- "name": "/pi4/camera",
+ "name": "/pi4/camera/decimated",
"type": "frc971.vision.CameraImage",
"source_node": "pi4",
"logger": "LOCAL_AND_REMOTE_LOGGER",
diff --git a/y2022/y2022_pi_template.json b/y2022/y2022_pi_template.json
index 7f49e41..27bc251 100644
--- a/y2022/y2022_pi_template.json
+++ b/y2022/y2022_pi_template.json
@@ -146,6 +146,14 @@
"num_senders": 18
},
{
+ "name": "/pi{{ NUM }}/camera/decimated",
+ "type": "frc971.vision.CameraImage",
+ "source_node": "pi{{ NUM }}",
+ "frequency": 2,
+ "max_size": 620000,
+ "num_senders": 18
+ },
+ {
"name": "/pi{{ NUM }}/camera",
"type": "frc971.vision.calibration.CalibrationData",
"source_node": "pi{{ NUM }}",
@@ -328,6 +336,13 @@
"nodes": [
"pi{{ NUM }}"
]
+ },
+ {
+ "name": "image_decimator",
+ "executable_name": "image_decimator",
+ "nodes": [
+ "pi{{ NUM }}"
+ ]
}
],
"maps": [