Merge "Add y2022 WebRTC image_streamer."
diff --git a/aos/network/www/aos_plotter.ts b/aos/network/www/aos_plotter.ts
index b7d8771..cd2e131 100644
--- a/aos/network/www/aos_plotter.ts
+++ b/aos/network/www/aos_plotter.ts
@@ -28,7 +28,7 @@
import {Connection} from 'org_frc971/aos/network/www/proxy';
import {SubscriberRequest, ChannelRequest, TransferMethod} from 'org_frc971/aos/network/web_proxy_generated';
import {Parser, Table} from 'org_frc971/aos/network/www/reflection'
-import {Schema} from 'org_frc971/external/com_github_google_flatbuffers/reflection/reflection_generated';
+import {Schema} from 'flatbuffers_reflection/reflection_generated';
import {ByteBuffer} from 'flatbuffers';
export class TimestampedMessage {
diff --git a/aos/network/www/proxy.ts b/aos/network/www/proxy.ts
index cf8e972..0bda8b9 100644
--- a/aos/network/www/proxy.ts
+++ b/aos/network/www/proxy.ts
@@ -1,7 +1,7 @@
import {Builder, ByteBuffer, Offset} from 'flatbuffers';
import {Channel as ChannelFb, Configuration} from 'org_frc971/aos/configuration_generated';
import {ChannelRequest as ChannelRequestFb, ChannelState, MessageHeader, Payload, SdpType, SubscriberRequest, TransferMethod, WebSocketIce, WebSocketMessage, WebSocketSdp} from 'org_frc971/aos/network/web_proxy_generated';
-import {Schema} from 'org_frc971/external/com_github_google_flatbuffers/reflection/reflection_generated';
+import {Schema} from 'flatbuffers_reflection/reflection_generated';
// There is one handler for each DataChannel, it maintains the state of
// multi-part messages and delegates to a callback when the message is fully
diff --git a/frc971/analysis/plot_data_utils.ts b/frc971/analysis/plot_data_utils.ts
index 230e370..80c4cca 100644
--- a/frc971/analysis/plot_data_utils.ts
+++ b/frc971/analysis/plot_data_utils.ts
@@ -5,7 +5,7 @@
import {ByteBuffer} from 'flatbuffers';
import {Plot, Point} from 'org_frc971/aos/network/www/plotter';
import {Connection} from 'org_frc971/aos/network/www/proxy';
-import {Schema} from 'org_frc971/external/com_github_google_flatbuffers/reflection/reflection_generated';
+import {Schema} from 'flatbuffers_reflection/reflection_generated';
export function plotData(conn: Connection, parentDiv: Element) {
// Set up a selection box to allow the user to choose between plots to show.
diff --git a/frc971/wpilib/imu_plot_utils.ts b/frc971/wpilib/imu_plot_utils.ts
index 08b3c13..c4b516b 100644
--- a/frc971/wpilib/imu_plot_utils.ts
+++ b/frc971/wpilib/imu_plot_utils.ts
@@ -5,7 +5,7 @@
import {Point} from 'org_frc971/aos/network/www/plotter';
import {Table} from 'org_frc971/aos/network/www/reflection';
import {ByteBuffer} from 'flatbuffers';
-import {Schema} from 'org_frc971/external/com_github_google_flatbuffers/reflection/reflection_generated';
+import {Schema} from 'flatbuffers_reflection/reflection_generated';
const FILTER_WINDOW_SIZE = 100;
diff --git a/scouting/BUILD b/scouting/BUILD
index c543c17..f58a157 100644
--- a/scouting/BUILD
+++ b/scouting/BUILD
@@ -1,10 +1,5 @@
load("//tools/build_rules:apache.bzl", "apache_wrapper")
-load("//tools/build_rules:js.bzl", "protractor_ts_test", "turn_files_into_runfiles")
-
-turn_files_into_runfiles(
- name = "main_bundle_compiled_runfiles",
- files = "//scouting/www:main_bundle_compiled",
-)
+load("//tools/build_rules:js.bzl", "protractor_ts_test")
sh_binary(
name = "scouting",
@@ -12,7 +7,6 @@
"scouting.sh",
],
data = [
- ":main_bundle_compiled_runfiles",
"//scouting/webserver",
"//scouting/www:static_files",
],
diff --git a/scouting/webserver/static/static.go b/scouting/webserver/static/static.go
index 57824d9..92d8086 100644
--- a/scouting/webserver/static/static.go
+++ b/scouting/webserver/static/static.go
@@ -32,8 +32,11 @@
return http.HandlerFunc(fn)
}
-// Serve pages given a port, directory to serve from, and an channel to pass the errors back to the caller.
+// Serve pages in the specified directory.
func ServePages(scoutingServer server.ScoutingServer, directory string) {
// Serve the / endpoint given a folder of pages.
scoutingServer.Handle("/", NoCache(http.FileServer(http.Dir(directory))))
+ // Make an exception for pictures. We don't want the pictures to be
+ // pulled every time the page is refreshed.
+ scoutingServer.Handle("/pictures/", http.FileServer(http.Dir(directory)))
}
diff --git a/scouting/www/BUILD b/scouting/www/BUILD
index 99dcde6..726749b 100644
--- a/scouting/www/BUILD
+++ b/scouting/www/BUILD
@@ -1,6 +1,5 @@
load("@npm//@bazel/typescript:index.bzl", "ts_library")
load("//tools/build_rules:js.bzl", "rollup_bundle")
-load("@npm//@bazel/concatjs:index.bzl", "concatjs_devserver")
load("@npm//@babel/cli:index.bzl", "babel")
ts_library(
@@ -21,6 +20,7 @@
"//scouting/www/import_match_list",
"//scouting/www/match_list",
"//scouting/www/notes",
+ "//scouting/www/shift_schedule",
"@npm//@angular/animations",
"@npm//@angular/common",
"@npm//@angular/core",
@@ -39,19 +39,31 @@
babel(
name = "main_bundle_compiled",
args = [
- "$(execpath :main_bundle)",
+ "$(execpath :main_bundle.min.js)",
"--no-babelrc",
"--source-maps",
+ "--minified",
+ "--no-comments",
"--plugins=@angular/compiler-cli/linker/babel",
"--out-dir",
"$(@D)",
],
data = [
- ":main_bundle",
+ ":main_bundle.min.js",
"@npm//@angular/compiler-cli",
],
output_dir = True,
- visibility = ["//visibility:public"],
+)
+
+# The babel() rule above puts everything into a directory without telling bazel
+# what's in the directory. That makes it annoying to work with from other
+# rules. This genrule() here copies the one file in the directory we care about
+# so that other rules have an easier time using the file.
+genrule(
+ name = "main_bundle_file",
+ srcs = [":main_bundle_compiled"],
+ outs = ["main_bundle_file.js"],
+ cmd = "cp $(location :main_bundle_compiled)/main_bundle.min.js $(OUTS)",
)
# Create a copy of zone.js here so that we can have a predictable path to
@@ -82,20 +94,12 @@
srcs = [
"index.html",
":field_pictures_copy",
+ ":main_bundle_file.js",
":zonejs_copy",
],
visibility = ["//visibility:public"],
)
-concatjs_devserver(
- name = "devserver",
- serving_path = "/main_bundle.js",
- static_files = [
- ":static_files",
- ],
- deps = [":main_bundle_compiled"],
-)
-
filegroup(
name = "common_css",
srcs = ["common.css"],
diff --git a/scouting/www/app.ng.html b/scouting/www/app.ng.html
index ab1e433..1357228 100644
--- a/scouting/www/app.ng.html
+++ b/scouting/www/app.ng.html
@@ -46,6 +46,15 @@
Import Match List
</a>
</li>
+ <li class="nav-item">
+ <a
+ class="nav-link"
+ [class.active]="tabIs('ShiftSchedule')"
+ (click)="switchTabToGuarded('ShiftSchedule')"
+ >
+ Shift Schedule
+ </a>
+ </li>
</ul>
<ng-container [ngSwitch]="tab">
@@ -63,4 +72,5 @@
<app-import-match-list
*ngSwitchCase="'ImportMatchList'"
></app-import-match-list>
+ <shift-schedule *ngSwitchCase="'ShiftSchedule'"></shift-schedule>
</ng-container>
diff --git a/scouting/www/app.ts b/scouting/www/app.ts
index 02af125..85620f1 100644
--- a/scouting/www/app.ts
+++ b/scouting/www/app.ts
@@ -1,6 +1,11 @@
import {Component, ElementRef, ViewChild} from '@angular/core';
-type Tab = 'MatchList' | 'Notes' | 'Entry' | 'ImportMatchList';
+type Tab =
+ | 'MatchList'
+ | 'Notes'
+ | 'Entry'
+ | 'ImportMatchList'
+ | 'ShiftSchedule';
type TeamInMatch = {
teamNumber: number;
matchNumber: number;
diff --git a/scouting/www/app_module.ts b/scouting/www/app_module.ts
index 9c762f6..b3c788f 100644
--- a/scouting/www/app_module.ts
+++ b/scouting/www/app_module.ts
@@ -7,6 +7,7 @@
import {ImportMatchListModule} from './import_match_list/import_match_list.module';
import {MatchListModule} from './match_list/match_list.module';
import {NotesModule} from './notes/notes.module';
+import {ShiftScheduleModule} from './shift_schedule/shift_schedule.module';
@NgModule({
declarations: [App],
@@ -17,6 +18,7 @@
NotesModule,
ImportMatchListModule,
MatchListModule,
+ ShiftScheduleModule,
],
exports: [App],
bootstrap: [App],
diff --git a/scouting/www/index.html b/scouting/www/index.html
index 84777d1..c9e2bb3 100644
--- a/scouting/www/index.html
+++ b/scouting/www/index.html
@@ -14,6 +14,6 @@
</head>
<body>
<my-app></my-app>
- <script src="./main_bundle_compiled/main_bundle.js"></script>
+ <script src="./main_bundle_file.js"></script>
</body>
</html>
diff --git a/scouting/www/shift_schedule/BUILD b/scouting/www/shift_schedule/BUILD
new file mode 100644
index 0000000..8fe99e4
--- /dev/null
+++ b/scouting/www/shift_schedule/BUILD
@@ -0,0 +1,27 @@
+load("@npm//@bazel/typescript:index.bzl", "ts_library")
+
+ts_library(
+ name = "shift_schedule",
+ srcs = [
+ "shift_schedule.component.ts",
+ "shift_schedule.module.ts",
+ ],
+ angular_assets = [
+ "shift_schedule.component.css",
+ "shift_schedule.ng.html",
+ "//scouting/www:common_css",
+ ],
+ compiler = "//tools:tsc_wrapped_with_angular",
+ target_compatible_with = ["@platforms//cpu:x86_64"],
+ use_angular_plugin = True,
+ visibility = ["//visibility:public"],
+ deps = [
+ "//scouting/webserver/requests/messages:error_response_ts_fbs",
+ "//scouting/webserver/requests/messages:request_all_matches_response_ts_fbs",
+ "//scouting/webserver/requests/messages:request_all_matches_ts_fbs",
+ "@com_github_google_flatbuffers//ts:flatbuffers_ts",
+ "@npm//@angular/common",
+ "@npm//@angular/core",
+ "@npm//@angular/forms",
+ ],
+)
diff --git a/scouting/www/shift_schedule/shift_schedule.component.css b/scouting/www/shift_schedule/shift_schedule.component.css
new file mode 100644
index 0000000..bafeb4c
--- /dev/null
+++ b/scouting/www/shift_schedule/shift_schedule.component.css
@@ -0,0 +1,17 @@
+* {
+ padding: 5px;
+}
+
+.badge {
+ height: 20px;
+}
+
+.red {
+ background-color: #dc3545;
+ border-radius: 5px;
+}
+
+.blue {
+ background-color: #0d6efd;
+ border-radius: 5px;
+}
diff --git a/scouting/www/shift_schedule/shift_schedule.component.ts b/scouting/www/shift_schedule/shift_schedule.component.ts
new file mode 100644
index 0000000..2f22653
--- /dev/null
+++ b/scouting/www/shift_schedule/shift_schedule.component.ts
@@ -0,0 +1,15 @@
+import {Component, OnInit} from '@angular/core';
+import {Builder, ByteBuffer} from 'flatbuffers';
+import {ErrorResponse} from 'org_frc971/scouting/webserver/requests/messages/error_response_generated';
+
+@Component({
+ selector: 'shift-schedule',
+ templateUrl: './shift_schedule.ng.html',
+ styleUrls: ['../common.css', './shift_schedule.component.css'],
+})
+export class ShiftsComponent {
+ progressMessage: string = '';
+ errorMessage: string = '';
+ // used to calculate shift blocks from starting match to ending match
+ numMatches: number[] = [20, 40, 60, 80, 100, 120, 140, 160, 180, 200];
+}
diff --git a/scouting/www/shift_schedule/shift_schedule.module.ts b/scouting/www/shift_schedule/shift_schedule.module.ts
new file mode 100644
index 0000000..0e5aa48
--- /dev/null
+++ b/scouting/www/shift_schedule/shift_schedule.module.ts
@@ -0,0 +1,12 @@
+import {CommonModule} from '@angular/common';
+import {NgModule} from '@angular/core';
+import {FormsModule} from '@angular/forms';
+
+import {ShiftsComponent} from './shift_schedule.component';
+
+@NgModule({
+ declarations: [ShiftsComponent],
+ exports: [ShiftsComponent],
+ imports: [CommonModule, FormsModule],
+})
+export class ShiftScheduleModule {}
diff --git a/scouting/www/shift_schedule/shift_schedule.ng.html b/scouting/www/shift_schedule/shift_schedule.ng.html
new file mode 100644
index 0000000..3a9cdf0
--- /dev/null
+++ b/scouting/www/shift_schedule/shift_schedule.ng.html
@@ -0,0 +1,33 @@
+<div class="header">
+ <h2>Shift Schedule</h2>
+</div>
+
+<div class="container-fluid">
+ <div class="row">
+ <div *ngFor="let num of numMatches">
+ <span class="badge bg-secondary rounded-left">
+ Scouting matches from {{num-19}} to {{num}}
+ </span>
+ <div class="list-group list-group-horizontal-sm">
+ <div class="redColumn">
+ <div *ngFor="let allianceNum of [1, 2, 3]">
+ <input
+ class="red text-center text-white fw-bold list-group-item list-group-item-action"
+ type="text"
+ />
+ </div>
+ </div>
+ <div class="blueColumn">
+ <div *ngFor="let allianceNum of [1, 2, 3]">
+ <input
+ class="blue text-center text-white fw-bold list-group-item list-group-item-action"
+ type="text"
+ />
+ </div>
+ </div>
+ </div>
+ </div>
+ </div>
+ <button class="btn btn-primary">Save</button>
+ <span class="error_message" role="alert">{{ errorMessage }}</span>
+</div>
diff --git a/third_party/flatbuffers/build_defs.bzl b/third_party/flatbuffers/build_defs.bzl
index cb50726..7eaa4d3 100644
--- a/third_party/flatbuffers/build_defs.bzl
+++ b/third_party/flatbuffers/build_defs.bzl
@@ -379,7 +379,8 @@
flatc_args = DEFAULT_FLATC_TS_ARGS,
visibility = None,
restricted_to = None,
- include_reflection = True):
+ include_reflection = True,
+ package_name = None):
"""Generates a ts_library rule for a given flatbuffer definition.
Args:
@@ -401,6 +402,7 @@
include_reflection: Optional, Whether to depend on the flatbuffer
reflection library automatically. Only really relevant for the
target that builds the reflection library itself.
+ package_name: Optional, Package name to use for the generated code.
"""
srcs_lib = "%s_srcs" % (name)
@@ -428,7 +430,7 @@
"SRCS=($(SRCS));",
"OUTS=($(OUTS));",
"for i in $${!SRCS[@]}; do",
- "sed 's/third_party\\/flatbuffers/external\\/com_github_google_flatbuffers/' $${SRCS[i]} > $${OUTS[i]};",
+ "sed \"s/'.*reflection\\/reflection_pregenerated/'flatbuffers_reflection\\/reflection_generated/\" $${SRCS[i]} > $${OUTS[i]};",
"sed -i 's/_pregenerated/_generated/' $${OUTS[i]};",
"done",
])
@@ -469,6 +471,7 @@
restricted_to = restricted_to,
target_compatible_with = target_compatible_with,
deps = [name + "_ts"],
+ package_name = package_name,
)
native.filegroup(
name = "%s_includes" % (name),
diff --git a/third_party/flatbuffers/reflection/BUILD.bazel b/third_party/flatbuffers/reflection/BUILD.bazel
index 7948e12..aa421db 100644
--- a/third_party/flatbuffers/reflection/BUILD.bazel
+++ b/third_party/flatbuffers/reflection/BUILD.bazel
@@ -8,6 +8,7 @@
flatbuffer_ts_library(
name = "reflection_ts_fbs",
+ package_name = "flatbuffers_reflection",
srcs = ["reflection.fbs"],
include_reflection = False,
visibility = ["//visibility:public"],
diff --git a/tools/build_rules/js.bzl b/tools/build_rules/js.bzl
index eeb5594..f5a7543 100644
--- a/tools/build_rules/js.bzl
+++ b/tools/build_rules/js.bzl
@@ -63,25 +63,6 @@
},
)
-# Some rules (e.g. babel()) do not expose their files as runfiles. So we need
-# to do this step manually.
-def _turn_files_into_runfiles_impl(ctx):
- files = ctx.attr.files.files
- return [DefaultInfo(
- files = files,
- runfiles = ctx.runfiles(transitive_files = files),
- )]
-
-turn_files_into_runfiles = rule(
- implementation = _turn_files_into_runfiles_impl,
- attrs = {
- "files": attr.label(
- mandatory = True,
- doc = "The target whose files should be turned into runfiles.",
- ),
- },
-)
-
def protractor_ts_test(name, srcs, deps = None, **kwargs):
"""Wraps upstream protractor_web_test_suite() to reduce boilerplate.
diff --git a/y2020/control_loops/drivetrain/localizer_plotter.ts b/y2020/control_loops/drivetrain/localizer_plotter.ts
index 9fe80de..b2120ef 100644
--- a/y2020/control_loops/drivetrain/localizer_plotter.ts
+++ b/y2020/control_loops/drivetrain/localizer_plotter.ts
@@ -6,7 +6,7 @@
import {Point} from 'org_frc971/aos/network/www/plotter';
import {Table} from 'org_frc971/aos/network/www/reflection';
import {ByteBuffer} from 'flatbuffers';
-import {Schema} from 'org_frc971/external/com_github_google_flatbuffers/reflection/reflection_generated';
+import {Schema} from 'flatbuffers_reflection/reflection_generated';
const TIME = AosPlotter.TIME;
diff --git a/y2020/control_loops/superstructure/turret_plotter.ts b/y2020/control_loops/superstructure/turret_plotter.ts
index 2c3fb01..5cb15c6 100644
--- a/y2020/control_loops/superstructure/turret_plotter.ts
+++ b/y2020/control_loops/superstructure/turret_plotter.ts
@@ -7,7 +7,7 @@
import {Point} from 'org_frc971/aos/network/www/plotter';
import {Table} from 'org_frc971/aos/network/www/reflection';
import {ByteBuffer} from 'flatbuffers';
-import {Schema} from 'org_frc971/external/com_github_google_flatbuffers/reflection/reflection_generated';
+import {Schema} from 'flatbuffers_reflection/reflection_generated';
import Connection = proxy.Connection;
diff --git a/y2022/actors/autonomous_actor.cc b/y2022/actors/autonomous_actor.cc
index ecc2a98..a8a2905 100644
--- a/y2022/actors/autonomous_actor.cc
+++ b/y2022/actors/autonomous_actor.cc
@@ -351,10 +351,7 @@
if (requested_intake_.has_value()) {
superstructure_builder.add_turret_intake(*requested_intake_);
}
- superstructure_builder.add_transfer_roller_speed_front(
- transfer_roller_front_voltage_);
- superstructure_builder.add_transfer_roller_speed_back(
- transfer_roller_back_voltage_);
+ superstructure_builder.add_transfer_roller_speed(transfer_roller_voltage_);
superstructure_builder.add_turret(turret_offset);
superstructure_builder.add_catapult(catapult_goal_offset);
superstructure_builder.add_fire(fire_);
@@ -371,8 +368,7 @@
set_requested_intake(RequestedIntake::kFront);
set_intake_front_goal(kExtendIntakeGoal);
set_roller_front_voltage(kIntakeRollerVoltage);
- set_transfer_roller_front_voltage(kRollerVoltage);
- set_transfer_roller_back_voltage(-kRollerVoltage);
+ set_transfer_roller_voltage(kRollerVoltage);
SendSuperstructureGoal();
}
@@ -380,8 +376,7 @@
set_requested_intake(std::nullopt);
set_intake_front_goal(kRetractIntakeGoal);
set_roller_front_voltage(0.0);
- set_transfer_roller_front_voltage(0.0);
- set_transfer_roller_back_voltage(0.0);
+ set_transfer_roller_voltage(0.0);
SendSuperstructureGoal();
}
@@ -389,8 +384,7 @@
set_requested_intake(RequestedIntake::kBack);
set_intake_back_goal(kExtendIntakeGoal);
set_roller_back_voltage(kIntakeRollerVoltage);
- set_transfer_roller_back_voltage(kRollerVoltage);
- set_transfer_roller_front_voltage(-kRollerVoltage);
+ set_transfer_roller_voltage(-kRollerVoltage);
SendSuperstructureGoal();
}
@@ -398,8 +392,7 @@
set_requested_intake(std::nullopt);
set_intake_back_goal(kRetractIntakeGoal);
set_roller_back_voltage(0.0);
- set_transfer_roller_front_voltage(0.0);
- set_transfer_roller_back_voltage(0.0);
+ set_transfer_roller_voltage(0.0);
SendSuperstructureGoal();
}
diff --git a/y2022/actors/autonomous_actor.h b/y2022/actors/autonomous_actor.h
index 0168630..ec66fb3 100644
--- a/y2022/actors/autonomous_actor.h
+++ b/y2022/actors/autonomous_actor.h
@@ -41,11 +41,8 @@
void set_roller_back_voltage(double roller_back_voltage) {
roller_back_voltage_ = roller_back_voltage;
}
- void set_transfer_roller_front_voltage(double voltage) {
- transfer_roller_front_voltage_ = voltage;
- }
- void set_transfer_roller_back_voltage(double voltage) {
- transfer_roller_back_voltage_ = voltage;
+ void set_transfer_roller_voltage(double voltage) {
+ transfer_roller_voltage_ = voltage;
}
void set_requested_intake(std::optional<RequestedIntake> requested_intake) {
requested_intake_ = requested_intake;
@@ -78,8 +75,7 @@
double intake_back_goal_ = 0.0;
double roller_front_voltage_ = 0.0;
double roller_back_voltage_ = 0.0;
- double transfer_roller_front_voltage_ = 0.0;
- double transfer_roller_back_voltage_ = 0.0;
+ double transfer_roller_voltage_ = 0.0;
std::optional<RequestedIntake> requested_intake_ = std::nullopt;
double turret_goal_ = 0.0;
bool fire_ = false;
diff --git a/y2022/control_loops/superstructure/superstructure.cc b/y2022/control_loops/superstructure/superstructure.cc
index 266ebfa..3d5284e 100644
--- a/y2022/control_loops/superstructure/superstructure.cc
+++ b/y2022/control_loops/superstructure/superstructure.cc
@@ -70,8 +70,7 @@
const CatapultGoal *catapult_goal = nullptr;
double roller_speed_compensated_front = 0.0;
double roller_speed_compensated_back = 0.0;
- double transfer_roller_speed_front = 0.0;
- double transfer_roller_speed_back = 0.0;
+ double transfer_roller_speed = 0.0;
double flipper_arms_voltage = 0.0;
bool have_active_intake_request = false;
@@ -84,8 +83,7 @@
unsafe_goal->roller_speed_back() -
std::min(velocity * unsafe_goal->roller_speed_compensation(), 0.0);
- transfer_roller_speed_front = unsafe_goal->transfer_roller_speed_front();
- transfer_roller_speed_back = unsafe_goal->transfer_roller_speed_back();
+ transfer_roller_speed = unsafe_goal->transfer_roller_speed();
turret_goal =
unsafe_goal->auto_aim() ? auto_aim_goal : unsafe_goal->turret();
@@ -175,13 +173,13 @@
}
// Check if we're either spitting of have lost the ball.
- if (transfer_roller_speed_front < 0.0 ||
+ if ((transfer_roller_speed < 0.0 && front_intake_has_ball_) ||
timestamp >
front_intake_beambreak_timer_ + constants::Values::kBallLostTime()) {
front_intake_has_ball_ = false;
}
- if (transfer_roller_speed_back < 0.0 ||
+ if ((transfer_roller_speed > 0.0 && back_intake_has_ball_) ||
timestamp >
back_intake_beambreak_timer_ + constants::Values::kBallLostTime()) {
back_intake_has_ball_ = false;
@@ -194,18 +192,18 @@
if (front_intake_has_ball_) {
roller_speed_compensated_front = 0.0;
if (position->intake_beambreak_front()) {
- transfer_roller_speed_front = -wiggle_voltage;
+ transfer_roller_speed = -wiggle_voltage;
} else {
- transfer_roller_speed_front = wiggle_voltage;
+ transfer_roller_speed = wiggle_voltage;
}
}
if (back_intake_has_ball_) {
roller_speed_compensated_back = 0.0;
if (position->intake_beambreak_back()) {
- transfer_roller_speed_back = -wiggle_voltage;
+ transfer_roller_speed = wiggle_voltage;
} else {
- transfer_roller_speed_back = wiggle_voltage;
+ transfer_roller_speed = -wiggle_voltage;
}
}
@@ -296,11 +294,9 @@
// Transfer rollers and flipper arm belt on
if (turret_intake_state_ == RequestedIntake::kFront) {
- transfer_roller_speed_front =
- constants::Values::kTransferRollerVoltage();
+ transfer_roller_speed = constants::Values::kTransferRollerVoltage();
} else {
- transfer_roller_speed_back =
- constants::Values::kTransferRollerVoltage();
+ transfer_roller_speed = -constants::Values::kTransferRollerVoltage();
}
flipper_arms_voltage = constants::Values::kFlipperFeedVoltage();
@@ -495,8 +491,7 @@
if (output != nullptr) {
output_struct.roller_voltage_front = roller_speed_compensated_front;
output_struct.roller_voltage_back = roller_speed_compensated_back;
- output_struct.transfer_roller_voltage_front = transfer_roller_speed_front;
- output_struct.transfer_roller_voltage_back = transfer_roller_speed_back;
+ output_struct.transfer_roller_voltage = transfer_roller_speed;
output_struct.flipper_arms_voltage = flipper_arms_voltage;
output->CheckOk(output->Send(Output::Pack(*output->fbb(), &output_struct)));
diff --git a/y2022/control_loops/superstructure/superstructure_goal.fbs b/y2022/control_loops/superstructure/superstructure_goal.fbs
index 7227dc2..0bb51e1 100644
--- a/y2022/control_loops/superstructure/superstructure_goal.fbs
+++ b/y2022/control_loops/superstructure/superstructure_goal.fbs
@@ -37,8 +37,10 @@
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);
+ // Positive is intaking front and spitting back, negative is the opposite
+ transfer_roller_speed:double (id: 5);
+ // Not used anymore - just one transfer roller speed to control both
+ transfer_roller_speed_back:double (id: 12, deprecated);
// Factor to multiply robot velocity by and add to roller voltage.
roller_speed_compensation:double (id: 6);
diff --git a/y2022/control_loops/superstructure/superstructure_lib_test.cc b/y2022/control_loops/superstructure/superstructure_lib_test.cc
index 341d315..02fef22 100644
--- a/y2022/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2022/control_loops/superstructure/superstructure_lib_test.cc
@@ -778,10 +778,7 @@
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(),
- 0.0);
+ EXPECT_EQ(superstructure_output_fetcher_->transfer_roller_voltage(), 0.0);
EXPECT_EQ(superstructure_status_fetcher_->state(), SuperstructureState::IDLE);
EXPECT_EQ(superstructure_status_fetcher_->intake_state(),
IntakeState::NO_BALL);
@@ -822,10 +819,8 @@
IntakeState::INTAKE_FRONT_BALL);
EXPECT_EQ(superstructure_output_fetcher_->flipper_arms_voltage(),
constants::Values::kFlipperFeedVoltage());
- EXPECT_EQ(superstructure_output_fetcher_->transfer_roller_voltage_front(),
+ EXPECT_EQ(superstructure_output_fetcher_->transfer_roller_voltage(),
constants::Values::kTransferRollerVoltage());
- EXPECT_EQ(superstructure_output_fetcher_->transfer_roller_voltage_back(),
- 0.0);
EXPECT_NEAR(superstructure_status_fetcher_->turret()->position(),
constants::Values::kTurretFrontIntakePos(), 0.001);
EXPECT_EQ(superstructure_status_fetcher_->shot_count(), 0);
@@ -843,10 +838,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(),
- 0.0);
+ EXPECT_EQ(superstructure_output_fetcher_->transfer_roller_voltage(), 0.0);
EXPECT_EQ(superstructure_status_fetcher_->state(),
SuperstructureState::LOADING);
EXPECT_EQ(superstructure_status_fetcher_->intake_state(),
@@ -865,10 +857,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(),
- 0.0);
+ EXPECT_EQ(superstructure_output_fetcher_->transfer_roller_voltage(), 0.0);
EXPECT_EQ(superstructure_status_fetcher_->state(),
SuperstructureState::LOADED);
EXPECT_EQ(superstructure_status_fetcher_->intake_state(),
@@ -884,10 +873,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(),
- 0.0);
+ EXPECT_EQ(superstructure_output_fetcher_->transfer_roller_voltage(), 0.0);
EXPECT_EQ(superstructure_status_fetcher_->state(),
SuperstructureState::LOADED);
EXPECT_EQ(superstructure_status_fetcher_->intake_state(),
@@ -908,14 +894,12 @@
ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
EXPECT_EQ(superstructure_output_fetcher_->roller_voltage_front(), 12.0);
EXPECT_EQ(superstructure_output_fetcher_->roller_voltage_back(), 0.0);
- EXPECT_TRUE(superstructure_output_fetcher_->transfer_roller_voltage_back() !=
+ EXPECT_TRUE(superstructure_output_fetcher_->transfer_roller_voltage() !=
0.0 &&
- superstructure_output_fetcher_->transfer_roller_voltage_back() <=
+ superstructure_output_fetcher_->transfer_roller_voltage() <=
constants::Values::kTransferRollerWiggleVoltage() &&
- superstructure_output_fetcher_->transfer_roller_voltage_back() >=
+ superstructure_output_fetcher_->transfer_roller_voltage() >=
-constants::Values::kTransferRollerWiggleVoltage());
- EXPECT_EQ(0.0,
- superstructure_output_fetcher_->transfer_roller_voltage_front());
EXPECT_EQ(superstructure_status_fetcher_->state(),
SuperstructureState::LOADED);
EXPECT_EQ(superstructure_status_fetcher_->intake_state(),
@@ -963,14 +947,12 @@
ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
EXPECT_EQ(superstructure_output_fetcher_->roller_voltage_front(), 0.0);
EXPECT_EQ(superstructure_output_fetcher_->roller_voltage_back(), 0.0);
- EXPECT_TRUE(superstructure_output_fetcher_->transfer_roller_voltage_back() !=
+ EXPECT_TRUE(superstructure_output_fetcher_->transfer_roller_voltage() !=
0.0 &&
- superstructure_output_fetcher_->transfer_roller_voltage_back() <=
+ superstructure_output_fetcher_->transfer_roller_voltage() <=
constants::Values::kTransferRollerWiggleVoltage() &&
- superstructure_output_fetcher_->transfer_roller_voltage_back() >=
+ superstructure_output_fetcher_->transfer_roller_voltage() >=
-constants::Values::kTransferRollerWiggleVoltage());
- EXPECT_EQ(0.0,
- superstructure_output_fetcher_->transfer_roller_voltage_front());
EXPECT_EQ(superstructure_status_fetcher_->state(),
SuperstructureState::SHOOTING);
EXPECT_EQ(superstructure_status_fetcher_->intake_state(),
@@ -1212,8 +1194,7 @@
RunFor(chrono::milliseconds(2000));
ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
- EXPECT_EQ(superstructure_status_fetcher_->state(),
- SuperstructureState::IDLE);
+ EXPECT_EQ(superstructure_status_fetcher_->state(), SuperstructureState::IDLE);
}
// Test that we are able to signal that the ball was preloaded
diff --git a/y2022/control_loops/superstructure/superstructure_output.fbs b/y2022/control_loops/superstructure/superstructure_output.fbs
index af6c292..a4460e2 100644
--- a/y2022/control_loops/superstructure/superstructure_output.fbs
+++ b/y2022/control_loops/superstructure/superstructure_output.fbs
@@ -26,8 +26,9 @@
// positive is pulling into the robot
roller_voltage_front:double (id: 6);
roller_voltage_back:double (id: 7);
- transfer_roller_voltage_front:double (id: 8);
- transfer_roller_voltage_back:double (id: 9);
+ transfer_roller_voltage:double (id: 8);
+ // Only using one transfer roller voltage now
+ transfer_roller_voltage_back:double (id: 9, deprecated);
}
root_type Output;
diff --git a/y2022/joystick_reader.cc b/y2022/joystick_reader.cc
index cfd8952..7e7a735 100644
--- a/y2022/joystick_reader.cc
+++ b/y2022/joystick_reader.cc
@@ -45,7 +45,8 @@
const ButtonLocation kIntakeFrontOut(4, 10);
const ButtonLocation kIntakeBackOut(4, 9);
-const ButtonLocation kSpit(3, 3);
+const ButtonLocation kSpitFront(3, 3);
+const ButtonLocation kSpitBack(2, 3);
const ButtonLocation kRedLocalizerReset(3, 13);
const ButtonLocation kBlueLocalizerReset(3, 14);
@@ -62,7 +63,8 @@
const ButtonLocation kIntakeFrontOut(4, 10);
const ButtonLocation kIntakeBackOut(4, 9);
-const ButtonLocation kSpit(3, 3);
+const ButtonLocation kSpitFront(3, 3);
+const ButtonLocation kSpitBack(2, 3);
const ButtonLocation kRedLocalizerReset(4, 14);
const ButtonLocation kBlueLocalizerReset(4, 13);
@@ -165,8 +167,7 @@
constexpr double kIntakeUpPosition = 1.47;
double intake_front_pos = kIntakeUpPosition;
double intake_back_pos = kIntakeUpPosition;
- double transfer_roller_front_speed = 0.0;
- double transfer_roller_back_speed = 0.0;
+ double transfer_roller_speed = 0.0;
std::optional<control_loops::superstructure::RequestedIntake>
requested_intake;
@@ -228,21 +229,21 @@
// Extend the intakes and spin the rollers
if (data.IsPressed(kIntakeFrontOut)) {
intake_front_pos = kIntakePosition;
- transfer_roller_front_speed = kTransferRollerSpeed;
+ transfer_roller_speed = kTransferRollerSpeed;
intake_front_counter_ = kIntakeCounterIterations;
intake_back_counter_ = 0;
} else if (data.IsPressed(kIntakeBackOut)) {
intake_back_pos = kIntakePosition;
- transfer_roller_back_speed = kTransferRollerSpeed;
+ transfer_roller_speed = -kTransferRollerSpeed;
intake_back_counter_ = kIntakeCounterIterations;
intake_front_counter_ = 0;
- } else if (data.IsPressed(kSpit)) {
- transfer_roller_front_speed = -kTransferRollerSpeed;
- transfer_roller_back_speed = -kTransferRollerSpeed;
-
+ } else if (data.IsPressed(kSpitFront)) {
+ transfer_roller_speed = -kTransferRollerSpeed;
intake_front_counter_ = 0;
+ } else if (data.IsPressed(kSpitBack)) {
+ transfer_roller_speed = kTransferRollerSpeed;
intake_back_counter_ = 0;
}
@@ -323,10 +324,8 @@
superstructure_goal_builder.add_roller_speed_front(roller_front_speed);
superstructure_goal_builder.add_roller_speed_back(roller_back_speed);
- superstructure_goal_builder.add_transfer_roller_speed_front(
- transfer_roller_front_speed);
- superstructure_goal_builder.add_transfer_roller_speed_back(
- transfer_roller_back_speed);
+ superstructure_goal_builder.add_transfer_roller_speed(
+ transfer_roller_speed);
superstructure_goal_builder.add_auto_aim(data.IsPressed(kAutoAim));
if (requested_intake.has_value()) {
superstructure_goal_builder.add_turret_intake(requested_intake.value());
diff --git a/y2022/vision/blob_detector.cc b/y2022/vision/blob_detector.cc
index 9800ace..60af1f8 100644
--- a/y2022/vision/blob_detector.cc
+++ b/y2022/vision/blob_detector.cc
@@ -12,7 +12,7 @@
#include "y2022/vision/geometry.h"
DEFINE_bool(
- use_outdoors, false,
+ use_outdoors, true,
"If set, use the color filters and exposure for an outdoor setting.");
DEFINE_int32(red_delta, 50, "Required difference between green pixels vs. red");
DEFINE_int32(blue_delta, -20,
@@ -247,10 +247,6 @@
cv::circle(view_image, stats.centroid, kCircleRadius, cv::Scalar(0, 255, 0),
cv::FILLED);
}
-
- // Draw average centroid
- cv::circle(view_image, blob_result.centroid, kCircleRadius,
- cv::Scalar(255, 255, 0), cv::FILLED);
}
void BlobDetector::ExtractBlobs(cv::Mat bgr_image,
diff --git a/y2022/vision/camera_reader.h b/y2022/vision/camera_reader.h
index 6a00351..7128890 100644
--- a/y2022/vision/camera_reader.h
+++ b/y2022/vision/camera_reader.h
@@ -97,7 +97,6 @@
const cv::Mat result(5, 1, CV_32F,
const_cast<void *>(static_cast<const void *>(
camera_calibration_->dist_coeffs()->data())));
- result.convertTo(result, CV_64F);
CHECK_EQ(result.total(), camera_calibration_->dist_coeffs()->size());
return result;
}
diff --git a/y2022/vision/target_estimator.cc b/y2022/vision/target_estimator.cc
index 14f9342..377778d 100644
--- a/y2022/vision/target_estimator.cc
+++ b/y2022/vision/target_estimator.cc
@@ -38,7 +38,7 @@
// Height of the center of the tape (m)
constexpr double kTapeCenterHeight = 2.58 + (kTapePieceHeight / 2);
// Horizontal distance from tape to center of hub (m)
-constexpr double kUpperHubRadius = 1.22 / 2;
+constexpr double kUpperHubRadius = 1.36 / 2;
std::vector<cv::Point3d> ComputeTapePoints() {
std::vector<cv::Point3d> tape_points;
@@ -185,8 +185,9 @@
ceres::DO_NOT_TAKE_OWNERSHIP);
// TODO(milind): add loss function when we get more noisy data
- problem.AddResidualBlock(cost_function, nullptr, &roll_, &pitch_, &yaw_,
- &distance_, &angle_to_camera_, &camera_height_);
+ problem.AddResidualBlock(cost_function, new ceres::HuberLoss(2.0), &roll_,
+ &pitch_, &yaw_, &distance_, &angle_to_camera_,
+ &camera_height_);
// Compute the estimated rotation of the camera using the robot rotation.
const Eigen::Matrix3d extrinsics_rot =
@@ -202,10 +203,16 @@
// Constrain the rotation to be around the localizer's, otherwise there can be
// multiple solutions. There shouldn't be too much roll or pitch
+ if (FLAGS_freeze_roll) {
+ roll_ = roll_seed;
+ }
constexpr double kMaxRollDelta = 0.1;
SetBoundsOrFreeze(&roll_, FLAGS_freeze_roll, roll_seed - kMaxRollDelta,
roll_seed + kMaxRollDelta, &problem);
+ if (FLAGS_freeze_pitch) {
+ pitch_ = pitch_seed;
+ }
constexpr double kMaxPitchDelta = 0.15;
SetBoundsOrFreeze(&pitch_, FLAGS_freeze_pitch, pitch_seed - kMaxPitchDelta,
pitch_seed + kMaxPitchDelta, &problem);
@@ -257,12 +264,12 @@
// Use a sigmoid to convert the deviation into a confidence for the
// localizer. Fit a sigmoid to the points of (0, 1) and two other
// reasonable deviation-confidence combinations using
- // https://www.desmos.com/calculator/try0pgx1qw
- constexpr double kSigmoidCapacity = 1.045;
+ // https://www.desmos.com/calculator/ha6fh9yw44
+ constexpr double kSigmoidCapacity = 1.065;
// Stretch the sigmoid out correctly.
- // Currently, good estimates have deviations of around 2 pixels.
- constexpr double kSigmoidScalar = 0.04452;
- constexpr double kSigmoidGrowthRate = -0.4021;
+ // Currently, good estimates have deviations of 1 or less pixels.
+ constexpr double kSigmoidScalar = 0.06496;
+ constexpr double kSigmoidGrowthRate = -0.6221;
confidence_ =
kSigmoidCapacity /
(1.0 + kSigmoidScalar * std::exp(-kSigmoidGrowthRate * std_dev));
@@ -326,38 +333,127 @@
ProjectToImage(*tape_piece_it, H_hub_camera);
}
+ // Now, find the closest tape for each blob.
+ // We don't normally see tape without matching blobs in the center. So we
+ // want to compress any gaps in the matched tape blobs. This makes it so it
+ // doesn't want to make the goal super small and skip tape blobs. The
+ // resulting accuracy is then pretty good.
+
+ // Mapping from tape index to blob index.
+ std::vector<std::pair<size_t, size_t>> tape_indices;
for (size_t i = 0; i < blob_stats_.size(); i++) {
- const auto distance = DistanceFromTape(i, tape_points_proj);
+ tape_indices.emplace_back(ClosestTape(i, tape_points_proj), i);
+ VLOG(2) << "Tape indices were " << tape_indices.back().first;
+ }
+
+ std::sort(
+ tape_indices.begin(), tape_indices.end(),
+ [](const std::pair<size_t, size_t> &a,
+ const std::pair<size_t, size_t> &b) { return a.first < b.first; });
+
+ size_t middle_tape_index = 1000;
+ for (size_t i = 0; i < tape_indices.size(); ++i) {
+ if (tape_indices[i].second == middle_blob_index_) {
+ middle_tape_index = i;
+ }
+ }
+ CHECK_NE(middle_tape_index, 1000) << "Failed to find middle tape";
+
+ if (VLOG_IS_ON(2)) {
+ LOG(INFO) << "Middle tape is " << middle_tape_index << ", blob "
+ << middle_blob_index_;
+ for (size_t i = 0; i < tape_indices.size(); ++i) {
+ const auto distance = DistanceFromTapeIndex(
+ tape_indices[i].second, tape_indices[i].first, tape_points_proj);
+ LOG(INFO) << "Blob index " << tape_indices[i].second << " maps to "
+ << tape_indices[i].first << " distance " << distance.x << " "
+ << distance.y;
+ }
+ }
+
+ {
+ size_t offset = 0;
+ for (size_t i = middle_tape_index + 1; i < tape_indices.size(); ++i) {
+ tape_indices[i].first -= offset;
+
+ if (tape_indices[i].first > tape_indices[i - 1].first + 1) {
+ offset += tape_indices[i].first - (tape_indices[i - 1].first + 1);
+ VLOG(2) << "Offset now " << offset;
+ tape_indices[i].first = tape_indices[i - 1].first + 1;
+ }
+ }
+ }
+
+ if (VLOG_IS_ON(2)) {
+ LOG(INFO) << "Middle tape is " << middle_tape_index << ", blob "
+ << middle_blob_index_;
+ for (size_t i = 0; i < tape_indices.size(); ++i) {
+ const auto distance = DistanceFromTapeIndex(
+ tape_indices[i].second, tape_indices[i].first, tape_points_proj);
+ LOG(INFO) << "Blob index " << tape_indices[i].second << " maps to "
+ << tape_indices[i].first << " distance " << distance.x << " "
+ << distance.y;
+ }
+ }
+
+ {
+ size_t offset = 0;
+ for (size_t i = middle_tape_index; i > 0; --i) {
+ tape_indices[i - 1].first -= offset;
+
+ if (tape_indices[i - 1].first + 1 < tape_indices[i].first) {
+ VLOG(2) << "Too big a gap. " << tape_indices[i].first << " and "
+ << tape_indices[i - 1].first;
+
+ offset += tape_indices[i].first - (tape_indices[i - 1].first + 1);
+ tape_indices[i - 1].first = tape_indices[i].first - 1;
+ VLOG(2) << "Offset now " << offset;
+ }
+ }
+ }
+
+ if (VLOG_IS_ON(2)) {
+ LOG(INFO) << "Middle tape is " << middle_tape_index << ", blob "
+ << middle_blob_index_;
+ for (size_t i = 0; i < tape_indices.size(); ++i) {
+ const auto distance = DistanceFromTapeIndex(
+ tape_indices[i].second, tape_indices[i].first, tape_points_proj);
+ LOG(INFO) << "Blob index " << tape_indices[i].second << " maps to "
+ << tape_indices[i].first << " distance " << distance.x << " "
+ << distance.y;
+ }
+ }
+
+ for (size_t i = 0; i < tape_indices.size(); ++i) {
+ const auto distance = DistanceFromTapeIndex(
+ tape_indices[i].second, tape_indices[i].first, tape_points_proj);
+ VLOG(2) << "Blob index " << tape_indices[i].second << " maps to "
+ << tape_indices[i].first << " distance " << distance.x << " "
+ << distance.y;
// Set the residual to the (x, y) distance of the centroid from the
- // nearest projected piece of tape
+ // matched projected piece of tape
residual[i * 2] = distance.x;
residual[(i * 2) + 1] = distance.y;
}
// Penalize based on the difference between the size of the projected piece of
- // tape and that of the detected blobs. Use the squared size to avoid taking a
- // norm, which ceres can't handle well
- const S middle_tape_piece_width_squared =
- ceres::pow(middle_tape_piece_points_proj[2].x -
- middle_tape_piece_points_proj[3].x,
- 2) +
- ceres::pow(middle_tape_piece_points_proj[2].y -
- middle_tape_piece_points_proj[3].y,
- 2);
- const S middle_tape_piece_height_squared =
- ceres::pow(middle_tape_piece_points_proj[1].x -
- middle_tape_piece_points_proj[2].x,
- 2) +
- ceres::pow(middle_tape_piece_points_proj[1].y -
- middle_tape_piece_points_proj[2].y,
- 2);
+ // tape and that of the detected blobs.
+ const S middle_tape_piece_width = ceres::hypot(
+ middle_tape_piece_points_proj[2].x - middle_tape_piece_points_proj[3].x,
+ middle_tape_piece_points_proj[2].y - middle_tape_piece_points_proj[3].y);
+ const S middle_tape_piece_height = ceres::hypot(
+ middle_tape_piece_points_proj[1].x - middle_tape_piece_points_proj[2].x,
+ middle_tape_piece_points_proj[1].y - middle_tape_piece_points_proj[2].y);
+ constexpr double kCenterBlobSizeScalar = 0.1;
residual[blob_stats_.size() * 2] =
- middle_tape_piece_width_squared -
- std::pow(blob_stats_[middle_blob_index_].size.width, 2);
+ kCenterBlobSizeScalar *
+ (middle_tape_piece_width -
+ static_cast<S>(blob_stats_[middle_blob_index_].size.width));
residual[(blob_stats_.size() * 2) + 1] =
- middle_tape_piece_height_squared -
- std::pow(blob_stats_[middle_blob_index_].size.height, 2);
+ kCenterBlobSizeScalar *
+ (middle_tape_piece_height -
+ static_cast<S>(blob_stats_[middle_blob_index_].size.height));
if (image_.has_value()) {
// Draw the current stage of the solving
@@ -427,27 +523,41 @@
} // namespace
template <typename S>
-cv::Point_<S> TargetEstimator::DistanceFromTape(
+cv::Point_<S> TargetEstimator::DistanceFromTapeIndex(
+ size_t blob_index, size_t tape_index,
+ const std::vector<cv::Point_<S>> &tape_points) const {
+ return Distance(blob_stats_[blob_index].centroid, tape_points[tape_index]);
+}
+
+template <typename S>
+size_t TargetEstimator::ClosestTape(
size_t blob_index, const std::vector<cv::Point_<S>> &tape_points) const {
auto distance = cv::Point_<S>(std::numeric_limits<S>::infinity(),
std::numeric_limits<S>::infinity());
+ size_t final_match = 255;
if (blob_index == middle_blob_index_) {
// Fix the middle blob so the solver can't go too far off
- distance = Distance(blob_stats_[middle_blob_index_].centroid,
- tape_points[tape_points.size() / 2]);
+ final_match = tape_points.size() / 2;
+ distance = DistanceFromTapeIndex(blob_index, final_match, tape_points);
} else {
// Give the other blob_stats some freedom in case some are split into pieces
for (auto it = tape_points.begin(); it < tape_points.end(); it++) {
+ const size_t tape_index = std::distance(tape_points.begin(), it);
const auto current_distance =
- Distance(blob_stats_[blob_index].centroid, *it);
- if ((it != tape_points.begin() + (tape_points.size() / 2)) &&
+ DistanceFromTapeIndex(blob_index, tape_index, tape_points);
+ if ((tape_index != (tape_points.size() / 2)) &&
Less(current_distance, distance)) {
+ final_match = tape_index;
distance = current_distance;
}
}
}
- return distance;
+ VLOG(2) << "Matched index " << blob_index << " to " << final_match
+ << " distance " << distance.x << " " << distance.y;
+ CHECK_NE(final_match, 255);
+
+ return final_match;
}
void TargetEstimator::DrawProjectedHub(
diff --git a/y2022/vision/target_estimator.h b/y2022/vision/target_estimator.h
index 174f774..f158626 100644
--- a/y2022/vision/target_estimator.h
+++ b/y2022/vision/target_estimator.h
@@ -68,8 +68,13 @@
const Eigen::Transform<S, 3, Eigen::Affine> &H_hub_camera) const;
template <typename S>
- cv::Point_<S> DistanceFromTape(
+ size_t ClosestTape(size_t centroid_index,
+ const std::vector<cv::Point_<S>> &tape_points) const;
+
+ template <typename S>
+ cv::Point_<S> DistanceFromTapeIndex(
size_t centroid_index,
+ size_t tape_index,
const std::vector<cv::Point_<S>> &tape_points) const;
void DrawProjectedHub(const std::vector<cv::Point2d> &tape_points_proj,
diff --git a/y2022/vision/viewer.cc b/y2022/vision/viewer.cc
index afca4cb..a21c09f 100644
--- a/y2022/vision/viewer.cc
+++ b/y2022/vision/viewer.cc
@@ -279,8 +279,10 @@
FLAGS_display_estimation ? std::make_optional(ret_image)
: std::nullopt);
estimator.DrawEstimate(ret_image);
+ LOG(INFO) << "Read file " << (it - file_list.begin()) << ": " << *it;
}
+
cv::imshow("image", image_mat);
cv::imshow("mask", blob_result.binarized_image);
cv::imshow("blobs", ret_image);
diff --git a/y2022/vision/vision_plotter.ts b/y2022/vision/vision_plotter.ts
index 8b9fa7f..adbf900 100644
--- a/y2022/vision/vision_plotter.ts
+++ b/y2022/vision/vision_plotter.ts
@@ -4,7 +4,7 @@
import {BLUE, BROWN, CYAN, GREEN, PINK, RED, WHITE} from 'org_frc971/aos/network/www/colors';
import {Connection} from 'org_frc971/aos/network/www/proxy';
import {Table} from 'org_frc971/aos/network/www/reflection';
-import {Schema} from 'org_frc971/external/com_github_google_flatbuffers/reflection/reflection_generated';
+import {Schema} from 'flatbuffers_reflection/reflection_generated';
import {TargetEstimate} from 'org_frc971/y2022/vision/target_estimate_generated';
diff --git a/y2022/wpilib_interface.cc b/y2022/wpilib_interface.cc
index 3a03a2a..56eba95 100644
--- a/y2022/wpilib_interface.cc
+++ b/y2022/wpilib_interface.cc
@@ -437,7 +437,6 @@
::std::shared_ptr<::ctre::phoenix::motorcontrol::can::TalonFX>
catapult_falcon_1_can_, catapult_falcon_2_can_;
-
};
class SuperstructureWriter
@@ -473,9 +472,12 @@
falcon->ConfigStatorCurrentLimit(
{false, Values::kIntakeRollerStatorCurrentLimit(),
Values::kIntakeRollerStatorCurrentLimit(), 0});
- falcon->SetStatusFramePeriod(ctre::phoenix::motorcontrol::Status_1_General, 1);
- falcon->SetControlFramePeriod(ctre::phoenix::motorcontrol::Control_3_General, 1);
- falcon->SetStatusFramePeriod(ctre::phoenix::motorcontrol::Status_Brushless_Current, 50);
+ falcon->SetStatusFramePeriod(
+ ctre::phoenix::motorcontrol::Status_1_General, 1);
+ falcon->SetControlFramePeriod(
+ ctre::phoenix::motorcontrol::Control_3_General, 1);
+ falcon->SetStatusFramePeriod(
+ ctre::phoenix::motorcontrol::Status_Brushless_Current, 50);
falcon->ConfigOpenloopRamp(0.0);
falcon->ConfigClosedloopRamp(0.0);
falcon->ConfigVoltageMeasurementFilter(1);
@@ -528,12 +530,8 @@
return flipper_arms_falcon_;
}
- void set_transfer_roller_victor_front(::std::unique_ptr<::frc::VictorSP> t) {
- transfer_roller_victor_front_ = ::std::move(t);
- }
-
- void set_transfer_roller_victor_back(::std::unique_ptr<::frc::VictorSP> t) {
- transfer_roller_victor_back_ = ::std::move(t);
+ void set_transfer_roller_victor(::std::unique_ptr<::frc::VictorSP> t) {
+ transfer_roller_victor_ = ::std::move(t);
}
std::shared_ptr<frc::DigitalOutput> superstructure_reading_;
@@ -555,8 +553,7 @@
ctre::phoenix::motorcontrol::ControlMode::Disabled, 0);
intake_falcon_front_->SetDisabled();
intake_falcon_back_->SetDisabled();
- transfer_roller_victor_front_->SetDisabled();
- transfer_roller_victor_back_->SetDisabled();
+ transfer_roller_victor_->SetDisabled();
if (catapult_falcon_1_) {
catapult_falcon_1_->SetDisabled();
}
@@ -576,10 +573,7 @@
WritePwm(output.intake_voltage_back(), intake_falcon_back_.get());
WriteCan(output.roller_voltage_front(), roller_falcon_front_.get());
WriteCan(output.roller_voltage_back(), roller_falcon_back_.get());
- WritePwm(output.transfer_roller_voltage_front(),
- transfer_roller_victor_front_.get());
- WritePwm(-output.transfer_roller_voltage_back(),
- transfer_roller_victor_back_.get());
+ WritePwm(output.transfer_roller_voltage(), transfer_roller_victor_.get());
WriteCan(-output.flipper_arms_voltage(), flipper_arms_falcon_.get());
@@ -626,8 +620,7 @@
::std::unique_ptr<::frc::TalonFX> turret_falcon_, catapult_falcon_1_,
climber_falcon_;
- ::std::unique_ptr<::frc::VictorSP> transfer_roller_victor_front_,
- transfer_roller_victor_back_;
+ ::std::unique_ptr<::frc::VictorSP> transfer_roller_victor_;
std::unique_ptr<frc::DigitalOutput> catapult_reversal_;
};
@@ -768,9 +761,7 @@
superstructure_writer.set_roller_falcon_back(
make_unique<::ctre::phoenix::motorcontrol::can::TalonFX>(1));
- superstructure_writer.set_transfer_roller_victor_front(
- make_unique<::frc::VictorSP>(6));
- superstructure_writer.set_transfer_roller_victor_back(
+ superstructure_writer.set_transfer_roller_victor(
make_unique<::frc::VictorSP>(5));
superstructure_writer.set_intake_falcon_front(make_unique<frc::TalonFX>(2));