Merge "Send turret to load before a ball is detected"
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/scouting/www/app.ts b/scouting/www/app.ts
index e22fad0..79c1094 100644
--- a/scouting/www/app.ts
+++ b/scouting/www/app.ts
@@ -10,6 +10,15 @@
export class App {
tab: Tab = 'Entry';
+ constructor() {
+ window.addEventListener('beforeunload', (e) => {
+ // Based on https://developer.mozilla.org/en-US/docs/Web/API/WindowEventHandlers/onbeforeunload#example
+ // This combination ensures a dialog will be shown on most browsers.
+ e.preventDefault();
+ e.returnValue = '';
+ });
+ }
+
tabIs(tab: Tab) {
return this.tab == tab;
}
diff --git a/scouting/www/entry/entry.component.ts b/scouting/www/entry/entry.component.ts
index af0ce97..1fb672f 100644
--- a/scouting/www/entry/entry.component.ts
+++ b/scouting/www/entry/entry.component.ts
@@ -10,7 +10,7 @@
import ErrorResponse = error_response.scouting.webserver.requests.ErrorResponse;
type Section = 'Team Selection'|'Auto'|'TeleOp'|'Climb'|'Defense'|'Review and Submit'|'Home'
-type Level = 'Low'|'Medium'|'High'|'Transversal'
+type Level = 'Failed'|'Low'|'Medium'|'High'|'Transversal'
@Component({
selector: 'app-entry',
@@ -38,6 +38,10 @@
this.proper = !this.proper;
}
+ setFailed() {
+ this.level = 'Failed';
+ }
+
setLow() {
this.level = 'Low';
}
diff --git a/scouting/www/entry/entry.ng.html b/scouting/www/entry/entry.ng.html
index ceaaac2..257b40a 100644
--- a/scouting/www/entry/entry.ng.html
+++ b/scouting/www/entry/entry.ng.html
@@ -83,6 +83,7 @@
<input (click)="setHigh()" type="radio" name="level" id="high"><label for="high">High</label><br>
<input (click)="setTransversal()" type="radio" name="level" id="transversal"><label for="transversal">Transversal</label><br>
<input (click)="toggleProper()" type="checkbox" id="proper"><label for="proper">~10 seconds to attempt next level?</label>
+ <input (click)="setFailed()" type="radio" name="level" id="failed"><label for="failed">Failed</label><br>
</form>
</div>
<div class="row">
@@ -104,7 +105,7 @@
</div>
<div class="col">
- <input type="range" min="1" max="5" value="3" (input)="defensePlayedOnSlider($event)">
+ <input type="range" min="0" max="5" value="0" (input)="defensePlayedOnSlider($event)">
</div>
<div class="col">
@@ -123,7 +124,7 @@
</div>
<div class="col">
- <input type="range" min="1" max="5" value="3" (input)="defensePlayedSlider($event)">
+ <input type="range" min="0" max="5" value="0" (input)="defensePlayedSlider($event)">
</div>
<div class="col">
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/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": [