Merge "Revert "Add binary checksum to starter applications.""
diff --git a/WORKSPACE b/WORKSPACE
index 398202c..f574965 100644
--- a/WORKSPACE
+++ b/WORKSPACE
@@ -12,6 +12,10 @@
apache2_debs = "files",
)
load(
+ "//debian:postgresql_amd64.bzl",
+ postgresql_amd64_debs = "files",
+)
+load(
"//debian:patch.bzl",
patch_debs = "files",
)
@@ -93,6 +97,8 @@
generate_repositories_for_debs(apache2_debs)
+generate_repositories_for_debs(postgresql_amd64_debs)
+
generate_repositories_for_debs(patch_debs)
generate_repositories_for_debs(pandoc_debs)
@@ -525,6 +531,13 @@
)
http_archive(
+ name = "postgresql_amd64",
+ build_file = "@//debian:postgresql_amd64.BUILD",
+ sha256 = "2b8bb77deaf58f798c296ce31ee7a32781395d55e05dcddc8a7da7e827f38d7f",
+ url = "https://www.frc971.org/Build-Dependencies/postgresql_amd64.tar.gz",
+)
+
+http_archive(
name = "patch",
build_file = "@//debian:patch.BUILD",
sha256 = "b5ce139648a2e04f5585948ddad2fdae24dd4ee7976ac5a22d6ae7bd5674631e",
@@ -1057,3 +1070,20 @@
"https://github.com/bazelbuild/buildtools/archive/refs/tags/4.2.4.tar.gz",
],
)
+
+http_archive(
+ name = "rules_pkg",
+ patch_args = ["-p1"],
+ patches = [
+ "//third_party:rules_pkg/0001-Fix-tree-artifacts.patch",
+ ],
+ sha256 = "62eeb544ff1ef41d786e329e1536c1d541bb9bcad27ae984d57f18f314018e66",
+ urls = [
+ "https://mirror.bazel.build/github.com/bazelbuild/rules_pkg/releases/download/0.6.0/rules_pkg-0.6.0.tar.gz",
+ "https://github.com/bazelbuild/rules_pkg/releases/download/0.6.0/rules_pkg-0.6.0.tar.gz",
+ ],
+)
+
+load("@rules_pkg//:deps.bzl", "rules_pkg_dependencies")
+
+rules_pkg_dependencies()
diff --git a/debian/BUILD b/debian/BUILD
index 2aeabf2..3f13244 100644
--- a/debian/BUILD
+++ b/debian/BUILD
@@ -7,6 +7,10 @@
apache2_debs = "files",
)
load(
+ ":postgresql_amd64.bzl",
+ postgresql_amd64_debs = "files",
+)
+load(
":patch.bzl",
patch_debs = "files",
)
@@ -173,6 +177,28 @@
)
download_packages(
+ name = "download_postgresql_deps",
+ excludes = [
+ "adduser",
+ "debconf",
+ "debconf-2.0",
+ "libsystemd0",
+ "lsb-base",
+ "libstdc++6",
+ "libc-bin",
+ "libc-l10n",
+ "netbase",
+ "ucf",
+ "locales",
+ "locales-all",
+ ],
+ packages = [
+ "postgresql",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+)
+
+download_packages(
name = "download_patch_deps",
packages = [
"patch",
@@ -287,6 +313,12 @@
)
generate_deb_tarball(
+ name = "postgresql_amd64",
+ files = postgresql_amd64_debs,
+ target_compatible_with = ["@platforms//os:linux"],
+)
+
+generate_deb_tarball(
name = "patch",
files = patch_debs,
target_compatible_with = ["@platforms//os:linux"],
diff --git a/debian/postgresql_amd64.BUILD b/debian/postgresql_amd64.BUILD
new file mode 100644
index 0000000..c2f8331
--- /dev/null
+++ b/debian/postgresql_amd64.BUILD
@@ -0,0 +1,56 @@
+load("@bazel_skylib//rules:write_file.bzl", "write_file")
+
+TEMPLATE = """\
+#!/bin/bash
+
+# --- begin runfiles.bash initialization v2 ---
+# Copy-pasted from the Bazel Bash runfiles library v2.
+set -uo pipefail; f=bazel_tools/tools/bash/runfiles/runfiles.bash
+source "${RUNFILES_DIR:-/dev/null}/$f" 2>/dev/null || \
+ source "$(grep -sm1 "^$f " "${RUNFILES_MANIFEST_FILE:-/dev/null}" | cut -f2- -d' ')" 2>/dev/null || \
+ source "$0.runfiles/$f" 2>/dev/null || \
+ source "$(grep -sm1 "^$f " "$0.runfiles_manifest" | cut -f2- -d' ')" 2>/dev/null || \
+ source "$(grep -sm1 "^$f " "$0.exe.runfiles_manifest" | cut -f2- -d' ')" 2>/dev/null || \
+ { echo>&2 "ERROR: cannot find $f"; exit 1; }; f=; set -e
+# --- end runfiles.bash initialization v2 ---
+
+add_ld_library_path_for() {
+ local file="$1"
+ local dir
+ local resolved_file
+ if ! resolved_file="$(rlocation "postgresql_amd64/$file")"; then
+ echo "Couldn't find file postgresql_amd64/${file}" >&2
+ exit 1
+ fi
+ dir="$(dirname "${resolved_file}")"
+ export LD_LIBRARY_PATH="${dir}${LD_LIBRARY_PATH:+:$LD_LIBRARY_PATH}"
+}
+
+add_ld_library_path_for usr/lib/x86_64-linux-gnu/libbsd.so.0.11.3
+add_ld_library_path_for lib/x86_64-linux-gnu/libreadline.so.8.1
+
+exec $(rlocation postgresql_amd64/usr/lib/postgresql/13/bin/%s) "$@"
+"""
+
+[(
+ write_file(
+ name = "generate_%s_wrapper" % binary,
+ out = "%s.sh" % binary,
+ content = [TEMPLATE % binary],
+ ),
+ sh_binary(
+ name = binary,
+ srcs = ["%s.sh" % binary],
+ data = glob([
+ "usr/lib/**/*",
+ "lib/**/*",
+ ]),
+ visibility = ["//visibility:public"],
+ deps = [
+ "@bazel_tools//tools/bash/runfiles",
+ ],
+ ),
+) for binary in (
+ "postgres",
+ "initdb",
+)]
diff --git a/debian/postgresql_amd64.bzl b/debian/postgresql_amd64.bzl
new file mode 100644
index 0000000..6fc9ba5
--- /dev/null
+++ b/debian/postgresql_amd64.bzl
@@ -0,0 +1,39 @@
+files = {
+ "libbsd0_0.11.3-1_amd64.deb": "284a7b8dcfcad74770f57360721365317448b38ab773db542bf630e94e60c13e",
+ "libedit2_3.1-20191231-2+b1_amd64.deb": "ac545f6ad10ba791aca24b09255ad1d6d943e6bc7c5511d5998e104aee51c943",
+ "libffi7_3.3-6_amd64.deb": "30ca89bfddae5fa6e0a2a044f22b6e50cd17c4bc6bc850c579819aeab7101f0f",
+ "libgdbm-compat4_1.19-2_amd64.deb": "e62caed68b0ffaa03b5fa539d6fdc08c4151f66236d5878949bead0b71b7bb09",
+ "libgdbm6_1.19-2_amd64.deb": "e54cfe4d8b8f209bb7df31a404ce040f7c2f9b1045114a927a7e1061cdf90727",
+ "libgnutls30_3.7.1-5_amd64.deb": "20b0189b72ad4c791cf5b280c111d41ce071a04dab0e9a9d7daa9504a7a7b543",
+ "libhogweed6_3.7.3-1_amd64.deb": "6aab2e892cdb2dfba45707601bc6c3b19aa228f70ae5841017f14c3b0ca3d22f",
+ "libicu67_67.1-7_amd64.deb": "2bf5c46254f527865bfd6368e1120908755fa57d83634bd7d316c9b3cfd57303",
+ "libidn2-0_2.3.0-5_amd64.deb": "cb80cd769171537bafbb4a16c12ec427065795946b3415781bc9792e92d60b59",
+ "libldap-2.4-2_2.4.57+dfsg-3_amd64.deb": "4186d0d3f086202d391da49d1bb5ced6dde5eafba1dbcffef9a8e1238a7ef7c3",
+ "libllvm11_11.0.1-2_amd64.deb": "eaff3c8dd6039af90b8b6bdbf33433e35d8c808a7aa195d0e3800ef5e61affff",
+ "libmd0_1.0.3-3_amd64.deb": "9e425b3c128b69126d95e61998e1b5ef74e862dd1fc953d91eebcc315aea62ea",
+ "libnettle8_3.7.3-1_amd64.deb": "e4f8ec31ed14518b241eb7b423ad5ed3f4a4e8ac50aae72c9fd475c569582764",
+ "libp11-kit0_0.23.22-1_amd64.deb": "bfef5f31ee1c730e56e16bb62cc5ff8372185106c75bf1ed1756c96703019457",
+ "libperl5.32_5.32.1-4+deb11u2_amd64.deb": "224cafe65968deb83168113b74dff2d2f13b115a41d99eb209ed3b8f981df0b3",
+ "libpq5_13.5-0+deb11u1_amd64.deb": "0bfa1dc24e1275963961efdcc6d2ff4d2eec390d7acd5a6aee3162569ae1886c",
+ "libreadline8_8.1-1_amd64.deb": "162ba9fdcde81b5502953ed4d84b24e8ad4e380bbd02990ab1a0e3edffca3c22",
+ "libsasl2-2_2.1.27+dfsg-2.1+deb11u1_amd64.deb": "2e86ab7a3329aad4b7350a9b067fe8f80b680302f2f82d94f73f9bf075404460",
+ "libsasl2-modules-db_2.1.27+dfsg-2.1+deb11u1_amd64.deb": "122bf3de4ca0ec873bc35bdde1f21ec9d91ace4f5245c3b1240e077f866e1ae9",
+ "libtasn1-6_4.16.0-2_amd64.deb": "fd7a200100298c2556e67bdc1a5faf5cf21c3136fa47f381d7e9769233ee88a1",
+ "libtinfo6_6.2+20201114-2_amd64.deb": "aeaf942c71ecc0ed081efdead1a1de304dcd513a9fc06791f26992e76986597b",
+ "libunistring2_0.9.10-4_amd64.deb": "654433ad02d3a8b05c1683c6c29a224500bf343039c34dcec4e5e9515345e3d4",
+ "libuuid1_2.36.1-8+deb11u1_amd64.deb": "31250af4dd3b7d1519326a9a6764d1466a93d8f498cf6545058761ebc38b2823",
+ "libxml2_2.9.10+dfsg-6.7_amd64.deb": "023296a15e1a28607609cb15c7ca0dd8a25160f3e89a0da58368319c7e17d4e0",
+ "libxslt1.1_1.1.34-4_amd64.deb": "17eb62d8973867b61e7f8b21b5c16ed33e151799656e49caf670081707853fb8",
+ "libz3-4_4.8.10-1_amd64.deb": "7a38c2dd985eb9315857588ee06ff297e2b16de159dec85bd2777a43ebe9f458",
+ "openssl_1.1.1k-1+deb11u1_amd64.deb": "ed998755dabb96ffe107c2d41ce685ecbb4fa200f7825ff82c1092f8334bf3cb",
+ "perl-modules-5.32_5.32.1-4+deb11u2_all.deb": "6fa15be322c3c89ec4a07d704ad58d4a2d1aabf866135a859f6d8d58c59e9df4",
+ "perl_5.32.1-4+deb11u2_amd64.deb": "1cebc4516ed7c240b812c7bdd7e6ea0810f513152717ca17ce139ee0dfbc7b0d",
+ "postgresql-13_13.5-0+deb11u1_amd64.deb": "e475540f43756dc1c64de0a8a3b33f2c0e45b39610f091afbfe3b6ef72573c7b",
+ "postgresql-client-13_13.5-0+deb11u1_amd64.deb": "cd1779abafdee712d9ea4ebae62d873b61540fd76beab1cc86e604c12813d005",
+ "postgresql-client-common_225_all.deb": "a867f301751692f9ad127c1dd921c3bce7f3969bdf58c6bf38c57303d1b51d2c",
+ "postgresql-common_225_all.deb": "90216c317fd9f247d8fb1597fb4677cbdf2bbb83811213ce4344a44820449e66",
+ "postgresql_13+225_all.deb": "c8791bd0fd7cce76341cbd2c6ba98991a206441fe948534394239e95d102b4b8",
+ "readline-common_8.1-1_all.deb": "3f947176ef949f93e4ad5d76c067d33fa97cf90b62ee0748acb4f5f64790edc8",
+ "ssl-cert_1.1.0+nmu1_all.deb": "6f3b0c20b0a37b2b196d832910a754cf784f96854daa02a16f4ac46d366cdcb8",
+ "tzdata_2021a-1+deb11u2_all.deb": "4a34cbe17d391e6351386f3530b7ffd096c2cc8582e970f745addc636fa7c397",
+}
diff --git a/frc971/analysis/BUILD b/frc971/analysis/BUILD
index cd6f729..9deec64 100644
--- a/frc971/analysis/BUILD
+++ b/frc971/analysis/BUILD
@@ -55,6 +55,7 @@
"//y2022/control_loops/superstructure:catapult_plotter",
"//y2022/control_loops/superstructure:climber_plotter",
"//y2022/control_loops/superstructure:intake_plotter",
+ "//y2022/control_loops/superstructure:superstructure_plotter",
"//y2022/control_loops/superstructure:turret_plotter",
"//y2022/localizer:localizer_plotter",
"//y2022/vision:vision_plotter",
diff --git a/frc971/analysis/plot_index.ts b/frc971/analysis/plot_index.ts
index b2a3efa..1c364c5 100644
--- a/frc971/analysis/plot_index.ts
+++ b/frc971/analysis/plot_index.ts
@@ -42,6 +42,8 @@
'org_frc971/y2021_bot3/control_loops/superstructure/superstructure_plotter';
import {plotTurret as plot2022Turret} from
'org_frc971/y2022/control_loops/superstructure/turret_plotter'
+import {plotSuperstructure as plot2022Superstructure} from
+ 'org_frc971/y2022/control_loops/superstructure/superstructure_plotter'
import {plotCatapult as plot2022Catapult} from
'org_frc971/y2022/control_loops/superstructure/catapult_plotter'
import {plotIntakeFront as plot2022IntakeFront, plotIntakeBack as plot2022IntakeBack} from
@@ -119,6 +121,7 @@
['2020 Localizer', new PlotState(plotDiv, plot2020Localizer)],
['2022 Localizer', new PlotState(plotDiv, plot2022Localizer)],
['2022 Vision', new PlotState(plotDiv, plot2022Vision)],
+ ['2022 Superstructure', new PlotState(plotDiv, plot2022Superstructure)],
['2022 Catapult', new PlotState(plotDiv, plot2022Catapult)],
['2022 Intake Front', new PlotState(plotDiv, plot2022IntakeFront)],
['2022 Intake Back', new PlotState(plotDiv, plot2022IntakeBack)],
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 66a3dcd..7d297e0 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -28,6 +28,11 @@
namespace control_loops {
namespace drivetrain {
+namespace {
+// Maximum variation to allow in the gyro when zeroing.
+constexpr double kMaxYawGyroZeroingRange = 0.05;
+}
+
DrivetrainFilters::DrivetrainFilters(const DrivetrainConfig<double> &dt_config,
::aos::EventLoop *event_loop,
LocalizerInterface *localizer)
@@ -132,8 +137,8 @@
if (last_imu_update_ == aos::monotonic_clock::min_time) {
last_imu_update_ = reading_time;
}
- down_estimator_.Predict(imu_zeroer_.ZeroedGyro(),
- imu_zeroer_.ZeroedAccel(),
+ down_estimator_.Predict(imu_zeroer_.ZeroedGyro().value(),
+ imu_zeroer_.ZeroedAccel().value(),
reading_time - last_imu_update_);
last_imu_update_ = reading_time;
}
@@ -165,26 +170,31 @@
}
// TODO(austin): Signal the current gear to both loops.
+ bool imu_zeroer_zeroed = imu_zeroer_.Zeroed();
switch (dt_config_.gyro_type) {
case GyroType::IMU_X_GYRO:
if (got_imu_reading) {
- last_gyro_rate_ = imu_zeroer_.ZeroedGyro().x();
+ last_gyro_rate_ =
+ imu_zeroer_zeroed ? imu_zeroer_.ZeroedGyro().value().x() : 0.0;
}
break;
case GyroType::IMU_Y_GYRO:
if (got_imu_reading) {
- last_gyro_rate_ = imu_zeroer_.ZeroedGyro().y();
+ last_gyro_rate_ =
+ imu_zeroer_zeroed ? imu_zeroer_.ZeroedGyro().value().y() : 0.0;
}
break;
case GyroType::IMU_Z_GYRO:
if (got_imu_reading) {
- last_gyro_rate_ = imu_zeroer_.ZeroedGyro().z();
+ last_gyro_rate_ =
+ imu_zeroer_zeroed ? imu_zeroer_.ZeroedGyro().value().z() : 0.0;
}
break;
case GyroType::FLIPPED_IMU_Z_GYRO:
if (got_imu_reading) {
- last_gyro_rate_ = -imu_zeroer_.ZeroedGyro().z();
+ last_gyro_rate_ =
+ imu_zeroer_zeroed ? -imu_zeroer_.ZeroedGyro().value().z() : 0.0;
}
break;
case GyroType::SPARTAN_GYRO:
@@ -204,9 +214,27 @@
break;
}
- ready_ = dt_config_.gyro_type == GyroType::SPARTAN_GYRO ||
- dt_config_.gyro_type == GyroType::FLIPPED_SPARTAN_GYRO ||
- imu_zeroer_.Zeroed();
+ switch (dt_config_.gyro_type) {
+ case GyroType::SPARTAN_GYRO:
+ case GyroType::FLIPPED_SPARTAN_GYRO:
+ if (!yaw_gyro_zero_.has_value()) {
+ yaw_gyro_zeroer_.AddData(last_gyro_rate_);
+ if (yaw_gyro_zeroer_.GetRange() < kMaxYawGyroZeroingRange) {
+ yaw_gyro_zero_ = yaw_gyro_zeroer_.GetAverage()(0);
+ }
+ }
+ ready_ = yaw_gyro_zero_.has_value();
+ if (ready_) {
+ last_gyro_rate_ = last_gyro_rate_ - yaw_gyro_zero_.value();
+ }
+ break;
+ case GyroType::IMU_X_GYRO:
+ case GyroType::IMU_Y_GYRO:
+ case GyroType::IMU_Z_GYRO:
+ case GyroType::FLIPPED_IMU_Z_GYRO:
+ ready_ = imu_zeroer_.Zeroed();
+ break;
+ }
// TODO(james): How aggressively can we fault here? If we fault to
// aggressively, we might have issues during startup if wpilib_interface takes
diff --git a/frc971/control_loops/drivetrain/drivetrain.h b/frc971/control_loops/drivetrain/drivetrain.h
index 52ae605..a06b965 100644
--- a/frc971/control_loops/drivetrain/drivetrain.h
+++ b/frc971/control_loops/drivetrain/drivetrain.h
@@ -138,6 +138,9 @@
// Last applied voltage.
Eigen::Matrix<double, 2, 1> last_voltage_;
Eigen::Matrix<double, 2, 1> last_last_voltage_;
+
+ std::optional<double> yaw_gyro_zero_;
+ zeroing::Averager<double, 200> yaw_gyro_zeroer_;
};
class DrivetrainLoop
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/frc971/control_loops/drivetrain/hybrid_ekf.h b/frc971/control_loops/drivetrain/hybrid_ekf.h
index 2a972b6..fcbed76 100644
--- a/frc971/control_loops/drivetrain/hybrid_ekf.h
+++ b/frc971/control_loops/drivetrain/hybrid_ekf.h
@@ -330,6 +330,12 @@
}
return X_hat();
}
+ std::optional<State> OldestState() {
+ if (observations_.empty()) {
+ return std::nullopt;
+ }
+ return observations_.begin()->X_hat;
+ }
// Returns the most recent input vector.
Input MostRecentInput() {
diff --git a/frc971/vision/charuco_lib.cc b/frc971/vision/charuco_lib.cc
index f14c9fd..fde5394 100644
--- a/frc971/vision/charuco_lib.cc
+++ b/frc971/vision/charuco_lib.cc
@@ -262,22 +262,31 @@
Eigen::Vector3d result = eigen_camera_matrix_ * camera_projection *
board_to_camera * Eigen::Vector3d::Zero();
- result /= result.z();
- cv::circle(rgb_image, cv::Point(result.x(), result.y()), 4,
- cv::Scalar(255, 255, 255), 0, cv::LINE_8);
+ // Found that drawAxis hangs if you try to draw with z values too small
+ // (trying to draw axes at inifinity)
+ // TODO<Jim>: Explore what real thresholds for this should be; likely
+ // Don't need to get rid of negative values
+ if (result.z() < 0.01) {
+ LOG(INFO) << "Skipping, due to z value too small: " << result.z();
+ valid = false;
+ } else {
+ result /= result.z();
+ cv::circle(rgb_image, cv::Point(result.x(), result.y()), 4,
+ cv::Scalar(255, 255, 255), 0, cv::LINE_8);
- cv::aruco::drawAxis(rgb_image, camera_matrix_, dist_coeffs_, rvec, tvec,
- 0.1);
+ cv::aruco::drawAxis(rgb_image, camera_matrix_, dist_coeffs_, rvec,
+ tvec, 0.1);
+ }
} else {
LOG(INFO) << "Age: " << age_double << ", invalid pose";
}
} else {
- LOG(INFO) << "Age: " << age_double << ", not enough charuco IDs, got "
- << charuco_ids.size() << ", needed " << FLAGS_min_targets;
+ VLOG(2) << "Age: " << age_double << ", not enough charuco IDs, got "
+ << charuco_ids.size() << ", needed " << FLAGS_min_targets;
}
} else {
- LOG(INFO) << "Age: " << age_double << ", not enough marker IDs, got "
- << marker_ids.size() << ", needed " << FLAGS_min_targets;
+ VLOG(2) << "Age: " << age_double << ", not enough marker IDs, got "
+ << marker_ids.size() << ", needed " << FLAGS_min_targets;
cv::aruco::drawDetectedMarkers(rgb_image, marker_corners, marker_ids);
}
diff --git a/frc971/zeroing/imu_zeroer.cc b/frc971/zeroing/imu_zeroer.cc
index a9b1045..6bc2d0e 100644
--- a/frc971/zeroing/imu_zeroer.cc
+++ b/frc971/zeroing/imu_zeroer.cc
@@ -6,7 +6,8 @@
bool DiagStatHasFaults(const ADIS16470DiagStat &diag) {
return diag.clock_error() || diag.memory_failure() || diag.sensor_failure() ||
diag.standby_mode() || diag.spi_communication_error() ||
- diag.flash_memory_update_error() || diag.data_path_overrun();
+ diag.flash_memory_update_error() || diag.data_path_overrun() ||
+ diag.checksum_mismatch();
}
bool ReadingHasFaults(const IMUValues &values) {
if (values.has_previous_reading_diag_stat() &&
@@ -17,11 +18,15 @@
DiagStatHasFaults(*values.self_test_diag_stat())) {
return true;
}
+ if (values.checksum_failed()) {
+ return true;
+ }
return false;
}
} // namespace
-ImuZeroer::ImuZeroer() {
+ImuZeroer::ImuZeroer(FaultBehavior fault_behavior)
+ : fault_behavior_(fault_behavior) {
gyro_average_.setZero();
accel_average_.setZero();
last_gyro_sample_.setZero();
@@ -32,14 +37,20 @@
return num_zeroes_ > kRequiredZeroPoints && !Faulted();
}
-bool ImuZeroer::Faulted() const { return faulted_; }
+bool ImuZeroer::Faulted() const { return reading_faulted_ || zeroing_faulted_; }
-Eigen::Vector3d ImuZeroer::ZeroedGyro() const {
- return last_gyro_sample_ - gyro_average_;
+std::optional<Eigen::Vector3d> ImuZeroer::ZeroedGyro() const {
+ return Faulted() ? std::nullopt
+ : std::make_optional<Eigen::Vector3d>(last_gyro_sample_ -
+ gyro_average_);
}
-Eigen::Vector3d ImuZeroer::ZeroedAccel() const {
- return last_accel_sample_ - accel_average_;
+
+std::optional<Eigen::Vector3d> ImuZeroer::ZeroedAccel() const {
+ return Faulted() ? std::nullopt
+ : std::make_optional<Eigen::Vector3d>(last_accel_sample_ -
+ accel_average_);
}
+
Eigen::Vector3d ImuZeroer::GyroOffset() const { return gyro_average_; }
bool ImuZeroer::GyroZeroReady() const {
@@ -55,20 +66,25 @@
}
void ImuZeroer::InsertAndProcessMeasurement(const IMUValues &values) {
- InsertMeasurement(values);
- ProcessMeasurements();
+ if (InsertMeasurement(values)) {
+ ProcessMeasurements();
+ }
}
-void ImuZeroer::InsertMeasurement(const IMUValues &values) {
+bool ImuZeroer::InsertMeasurement(const IMUValues &values) {
if (ReadingHasFaults(values)) {
- faulted_ = true;
- return;
+ reading_faulted_ = true;
+ return false;
+ }
+ if (fault_behavior_ == FaultBehavior::kTemporary) {
+ reading_faulted_ = false;
}
last_gyro_sample_ << values.gyro_x(), values.gyro_y(), values.gyro_z();
gyro_averager_.AddData(last_gyro_sample_);
last_accel_sample_ << values.accelerometer_x(), values.accelerometer_y(),
values.accelerometer_z();
accel_averager_.AddData(last_accel_sample_);
+ return true;
}
void ImuZeroer::ProcessMeasurements() {
@@ -88,7 +104,9 @@
// original zero, fault.
if ((current_gyro_average - gyro_average_).norm() >
kGyroFaultVariation) {
- faulted_ = true;
+ zeroing_faulted_ = true;
+ } else if (fault_behavior_ == FaultBehavior::kTemporary) {
+ zeroing_faulted_ = false;
}
}
++num_zeroes_;
diff --git a/frc971/zeroing/imu_zeroer.h b/frc971/zeroing/imu_zeroer.h
index f3af9c5..f9ae4d2 100644
--- a/frc971/zeroing/imu_zeroer.h
+++ b/frc971/zeroing/imu_zeroer.h
@@ -1,6 +1,8 @@
#ifndef FRC971_ZEROING_IMU_ZEROER_H_
#define FRC971_ZEROING_IMU_ZEROER_H_
+#include <optional>
+
#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
#include "frc971/wpilib/imu_generated.h"
#include "frc971/zeroing/averager.h"
@@ -19,16 +21,25 @@
static constexpr size_t kSamplesToAverage = 200;
static constexpr size_t kRequiredZeroPoints = 10;
- ImuZeroer();
+ enum class FaultBehavior {
+ // When we encounter a fault, latch and stay in an errored state
+ // indefinitely.
+ kLatch,
+ // When we encounter a fault, don't process the reading and return nullopt
+ // for all measurements.
+ kTemporary
+ };
+
+ explicit ImuZeroer(FaultBehavior fault_behavior = FaultBehavior::kLatch);
bool Zeroed() const;
bool Faulted() const;
- void InsertMeasurement(const IMUValues &values);
+ bool InsertMeasurement(const IMUValues &values);
// PErforms the heavier-duty processing for managing zeroing.
void ProcessMeasurements();
void InsertAndProcessMeasurement(const IMUValues &values);
Eigen::Vector3d GyroOffset() const;
- Eigen::Vector3d ZeroedGyro() const;
- Eigen::Vector3d ZeroedAccel() const;
+ std::optional<Eigen::Vector3d> ZeroedGyro() const;
+ std::optional<Eigen::Vector3d> ZeroedAccel() const;
flatbuffers::Offset<control_loops::drivetrain::ImuZeroerState> PopulateStatus(
flatbuffers::FlatBufferBuilder *fbb) const;
@@ -62,8 +73,11 @@
Eigen::Vector3d accel_average_;
Eigen::Vector3d last_gyro_sample_;
Eigen::Vector3d last_accel_sample_;
- // Whether the zeroing has faulted at any point thus far.
- bool faulted_ = false;
+
+ const FaultBehavior fault_behavior_;
+ bool reading_faulted_ = false;
+ bool zeroing_faulted_ = false;
+
size_t good_iters_ = 0;
size_t num_zeroes_ = 0;
};
diff --git a/frc971/zeroing/imu_zeroer_test.cc b/frc971/zeroing/imu_zeroer_test.cc
index 0a68541..deff1b6 100644
--- a/frc971/zeroing/imu_zeroer_test.cc
+++ b/frc971/zeroing/imu_zeroer_test.cc
@@ -42,8 +42,8 @@
ASSERT_FALSE(zeroer.Zeroed());
ASSERT_FALSE(zeroer.Faulted());
ASSERT_EQ(0.0, zeroer.GyroOffset().norm());
- ASSERT_EQ(0.0, zeroer.ZeroedGyro().norm());
- ASSERT_EQ(0.0, zeroer.ZeroedAccel().norm());
+ ASSERT_EQ(0.0, zeroer.ZeroedGyro().value().norm());
+ ASSERT_EQ(0.0, zeroer.ZeroedAccel().value().norm());
// A measurement before we are zeroed should just result in the measurement
// being passed through without modification.
zeroer.InsertAndProcessMeasurement(
@@ -51,12 +51,12 @@
ASSERT_FALSE(zeroer.Zeroed());
ASSERT_FALSE(zeroer.Faulted());
ASSERT_EQ(0.0, zeroer.GyroOffset().norm());
- ASSERT_FLOAT_EQ(0.01, zeroer.ZeroedGyro().x());
- ASSERT_FLOAT_EQ(0.02, zeroer.ZeroedGyro().y());
- ASSERT_FLOAT_EQ(0.03, zeroer.ZeroedGyro().z());
- ASSERT_EQ(4.0, zeroer.ZeroedAccel().x());
- ASSERT_EQ(5.0, zeroer.ZeroedAccel().y());
- ASSERT_EQ(6.0, zeroer.ZeroedAccel().z());
+ ASSERT_FLOAT_EQ(0.01, zeroer.ZeroedGyro().value().x());
+ ASSERT_FLOAT_EQ(0.02, zeroer.ZeroedGyro().value().y());
+ ASSERT_FLOAT_EQ(0.03, zeroer.ZeroedGyro().value().z());
+ ASSERT_EQ(4.0, zeroer.ZeroedAccel().value().x());
+ ASSERT_EQ(5.0, zeroer.ZeroedAccel().value().y());
+ ASSERT_EQ(6.0, zeroer.ZeroedAccel().value().z());
}
// Tests that we zero if we receive a bunch of identical measurements.
@@ -73,21 +73,21 @@
ASSERT_FLOAT_EQ(0.01, zeroer.GyroOffset().x());
ASSERT_FLOAT_EQ(0.02, zeroer.GyroOffset().y());
ASSERT_FLOAT_EQ(0.03, zeroer.GyroOffset().z());
- ASSERT_EQ(0.0, zeroer.ZeroedGyro().x());
- ASSERT_EQ(0.0, zeroer.ZeroedGyro().y());
- ASSERT_EQ(0.0, zeroer.ZeroedGyro().z());
+ ASSERT_EQ(0.0, zeroer.ZeroedGyro().value().x());
+ ASSERT_EQ(0.0, zeroer.ZeroedGyro().value().y());
+ ASSERT_EQ(0.0, zeroer.ZeroedGyro().value().z());
// Accelerometer readings should not be affected.
- ASSERT_EQ(4.0, zeroer.ZeroedAccel().x());
- ASSERT_EQ(5.0, zeroer.ZeroedAccel().y());
- ASSERT_EQ(6.0, zeroer.ZeroedAccel().z());
+ ASSERT_EQ(4.0, zeroer.ZeroedAccel().value().x());
+ ASSERT_EQ(5.0, zeroer.ZeroedAccel().value().y());
+ ASSERT_EQ(6.0, zeroer.ZeroedAccel().value().z());
// If we get another measurement offset by {1, 1, 1} we should read the result
// as {1, 1, 1}.
zeroer.InsertAndProcessMeasurement(
MakeMeasurement({0.02, 0.03, 0.04}, {0, 0, 0}).message());
ASSERT_FALSE(zeroer.Faulted());
- ASSERT_FLOAT_EQ(0.01, zeroer.ZeroedGyro().x());
- ASSERT_FLOAT_EQ(0.01, zeroer.ZeroedGyro().y());
- ASSERT_FLOAT_EQ(0.01, zeroer.ZeroedGyro().z());
+ ASSERT_FLOAT_EQ(0.01, zeroer.ZeroedGyro().value().x());
+ ASSERT_FLOAT_EQ(0.01, zeroer.ZeroedGyro().value().y());
+ ASSERT_FLOAT_EQ(0.01, zeroer.ZeroedGyro().value().z());
}
// Tests that we do not zero if the gyro is producing particularly high
@@ -125,12 +125,12 @@
zeroer.InsertAndProcessMeasurement(
MakeMeasurement({0.02, 0.03, 0.04}, {0, 0, 0}).message());
ASSERT_FALSE(zeroer.Faulted());
- ASSERT_NEAR(0.01, zeroer.ZeroedGyro().x(), 1e-3);
- ASSERT_NEAR(0.01, zeroer.ZeroedGyro().y(), 1e-3);
- ASSERT_NEAR(0.01, zeroer.ZeroedGyro().z(), 1e-3);
- ASSERT_EQ(0.0, zeroer.ZeroedAccel().x());
- ASSERT_EQ(0.0, zeroer.ZeroedAccel().y());
- ASSERT_EQ(0.0, zeroer.ZeroedAccel().z());
+ ASSERT_NEAR(0.01, zeroer.ZeroedGyro().value().x(), 1e-3);
+ ASSERT_NEAR(0.01, zeroer.ZeroedGyro().value().y(), 1e-3);
+ ASSERT_NEAR(0.01, zeroer.ZeroedGyro().value().z(), 1e-3);
+ ASSERT_EQ(0.0, zeroer.ZeroedAccel().value().x());
+ ASSERT_EQ(0.0, zeroer.ZeroedAccel().value().y());
+ ASSERT_EQ(0.0, zeroer.ZeroedAccel().value().z());
}
// Tests that we do not zero if there is too much noise in the input data.
diff --git a/scouting/BUILD b/scouting/BUILD
index 0ed540b..0c2c641 100644
--- a/scouting/BUILD
+++ b/scouting/BUILD
@@ -33,6 +33,7 @@
"//scouting/www:index.html",
"//scouting/www:zonejs_copy",
],
+ visibility = ["//visibility:public"],
)
protractor_ts_test(
@@ -41,5 +42,5 @@
":scouting_test.ts",
],
on_prepare = ":scouting_test.protractor.on-prepare.js",
- server = ":scouting",
+ server = "//scouting/testing:scouting_test_servers",
)
diff --git a/scouting/db/db.go b/scouting/db/db.go
index 788e6e3..94e9056 100644
--- a/scouting/db/db.go
+++ b/scouting/db/db.go
@@ -27,90 +27,192 @@
Climbing int32
}
+type NotesData struct {
+ TeamNumber int32
+ Notes []string
+}
+
// Opens a database at the specified path. If the path refers to a non-existent
// file, the database will be created and initialized with empty tables.
func NewDatabase(path string) (*Database, error) {
+ var err error
database := new(Database)
- database.DB, _ = sql.Open("sqlite3", path)
- statement, error_ := database.Prepare("CREATE TABLE IF NOT EXISTS matches " +
- "(id INTEGER PRIMARY KEY, MatchNumber INTEGER, Round INTEGER, CompLevel INTEGER, R1 INTEGER, R2 INTEGER, R3 INTEGER, B1 INTEGER, B2 INTEGER, B3 INTEGER, r1ID INTEGER, r2ID INTEGER, r3ID INTEGER, b1ID INTEGER, b2ID INTEGER, b3ID INTEGER)")
- defer statement.Close()
- if error_ != nil {
- fmt.Println(error_)
- return nil, error_
+ database.DB, err = sql.Open("sqlite3", path)
+ if err != nil {
+ return nil, errors.New(fmt.Sprint("Failed to create postgres db: ", err))
}
- _, error_ = statement.Exec()
- statement, error_ = database.Prepare("CREATE TABLE IF NOT EXISTS team_match_stats (id INTEGER PRIMARY KEY, TeamNumber INTEGER, MatchNumber DOUBLE, ShotsMissed INTEGER, UpperGoalShots INTEGER, LowerGoalShots INTEGER, ShotsMissedAuto INTEGER, UpperGoalAuto INTEGER, LowerGoalAuto INTEGER, PlayedDefense INTEGER, Climbing INTEGER)")
- defer statement.Close()
- if error_ != nil {
- fmt.Println(error_)
- return nil, error_
+
+ statement, err := database.Prepare("CREATE TABLE IF NOT EXISTS matches (" +
+ "id INTEGER PRIMARY KEY, " +
+ "MatchNumber INTEGER, " +
+ "Round INTEGER, " +
+ "CompLevel INTEGER, " +
+ "R1 INTEGER, " +
+ "R2 INTEGER, " +
+ "R3 INTEGER, " +
+ "B1 INTEGER, " +
+ "B2 INTEGER, " +
+ "B3 INTEGER, " +
+ "r1ID INTEGER, " +
+ "r2ID INTEGER, " +
+ "r3ID INTEGER, " +
+ "b1ID INTEGER, " +
+ "b2ID INTEGER, " +
+ "b3ID INTEGER)")
+ if err != nil {
+ return nil, errors.New(fmt.Sprint("Failed to prepare matches table creation: ", err))
}
- _, error_ = statement.Exec()
+ defer statement.Close()
+
+ _, err = statement.Exec()
+ if err != nil {
+ return nil, errors.New(fmt.Sprint("Failed to create matches table: ", err))
+ }
+
+ statement, err = database.Prepare("CREATE TABLE IF NOT EXISTS team_match_stats (" +
+ "id INTEGER PRIMARY KEY, " +
+ "TeamNumber INTEGER, " +
+ "MatchNumber INTEGER, " +
+ "ShotsMissed INTEGER, " +
+ "UpperGoalShots INTEGER, " +
+ "LowerGoalShots INTEGER, " +
+ "ShotsMissedAuto INTEGER, " +
+ "UpperGoalAuto INTEGER, " +
+ "LowerGoalAuto INTEGER, " +
+ "PlayedDefense INTEGER, " +
+ "Climbing INTEGER)")
+ if err != nil {
+ return nil, errors.New(fmt.Sprint("Failed to prepare stats table creation: ", err))
+ }
+ defer statement.Close()
+
+ _, err = statement.Exec()
+ if err != nil {
+ return nil, errors.New(fmt.Sprint("Failed to create team_match_stats table: ", err))
+ }
+
+ statement, err = database.Prepare("CREATE TABLE IF NOT EXISTS team_notes (" +
+ "id INTEGER PRIMARY KEY, " +
+ "TeamNumber INTEGER, " +
+ "Notes TEXT)")
+ if err != nil {
+ return nil, errors.New(fmt.Sprint("Failed to prepare notes table creation: ", err))
+ }
+ defer statement.Close()
+
+ _, err = statement.Exec()
+ if err != nil {
+ return nil, errors.New(fmt.Sprint("Failed to create notes table: ", err))
+ }
+
return database, nil
}
func (database *Database) Delete() error {
- statement, error_ := database.Prepare("DROP TABLE IF EXISTS matches")
- if error_ != nil {
- fmt.Println(error_)
- return (error_)
+ statement, err := database.Prepare("DROP TABLE IF EXISTS matches")
+ if err != nil {
+ return errors.New(fmt.Sprint("Failed to prepare dropping matches table: ", err))
}
- _, error_ = statement.Exec()
- statement, error_ = database.Prepare("DROP TABLE IF EXISTS team_match_stats")
- if error_ != nil {
- fmt.Println(error_)
- return (error_)
+ _, err = statement.Exec()
+ if err != nil {
+ return errors.New(fmt.Sprint("Failed to drop matches table: ", err))
}
- _, error_ = statement.Exec()
+
+ statement, err = database.Prepare("DROP TABLE IF EXISTS team_match_stats")
+ if err != nil {
+ return errors.New(fmt.Sprint("Failed to prepare dropping stats table: ", err))
+ }
+ _, err = statement.Exec()
+ if err != nil {
+ return errors.New(fmt.Sprint("Failed to drop stats table: ", err))
+ }
+
+ statement, err = database.Prepare("DROP TABLE IF EXISTS team_notes")
+ if err != nil {
+ return errors.New(fmt.Sprint("Failed to prepare dropping notes table: ", err))
+ }
+ _, err = statement.Exec()
+ if err != nil {
+ return errors.New(fmt.Sprint("Failed to drop notes table: ", err))
+ }
return nil
}
// This function will also populate the Stats table with six empty rows every time a match is added
func (database *Database) AddToMatch(m Match) error {
- statement, error_ := database.Prepare("INSERT INTO team_match_stats(TeamNumber, MatchNumber, ShotsMissed, UpperGoalShots, LowerGoalShots, ShotsMissedAuto, UpperGoalAuto, LowerGoalAuto, PlayedDefense, Climbing) VALUES (?, ?, ?, ?, ?, ?, ?, ?, ?, ?)")
- defer statement.Close()
- if error_ != nil {
- fmt.Println("failed to prepare stats database:", error_)
- return (error_)
+ statement, err := database.Prepare("INSERT INTO team_match_stats(" +
+ "TeamNumber, MatchNumber, " +
+ "ShotsMissed, UpperGoalShots, LowerGoalShots, " +
+ "ShotsMissedAuto, UpperGoalAuto, LowerGoalAuto, " +
+ "PlayedDefense, Climbing) " +
+ "VALUES (" +
+ "?, ?, " +
+ "?, ?, ?, " +
+ "?, ?, ?, " +
+ "?, ?)")
+ if err != nil {
+ return errors.New(fmt.Sprint("Failed to prepare insertion into stats database: ", err))
}
+ defer statement.Close()
+
var rowIds [6]int64
for i, TeamNumber := range []int32{m.R1, m.R2, m.R3, m.B1, m.B2, m.B3} {
- result, error_ := statement.Exec(TeamNumber, m.MatchNumber, 0, 0, 0, 0, 0, 0, 0, 0)
- if error_ != nil {
- fmt.Println("failed to execute statement 2:", error_)
- return (error_)
+ result, err := statement.Exec(TeamNumber, m.MatchNumber, 0, 0, 0, 0, 0, 0, 0, 0)
+ if err != nil {
+ return errors.New(fmt.Sprint("Failed to insert stats: ", err))
}
- rowIds[i], error_ = result.LastInsertId()
+ rowIds[i], err = result.LastInsertId()
+ if err != nil {
+ return errors.New(fmt.Sprint("Failed to get last insert ID: ", err))
+ }
}
- statement, error_ = database.Prepare("INSERT INTO matches(MatchNumber, Round, CompLevel, R1, R2, R3, B1, B2, B3, r1ID, r2ID, r3ID, b1ID, b2ID, b3ID) VALUES (?, ?, ?, ?, ?, ?, ?, ?, ?, ?, ?, ?, ?, ?, ?)")
+
+ statement, err = database.Prepare("INSERT INTO matches(" +
+ "MatchNumber, Round, CompLevel, " +
+ "R1, R2, R3, B1, B2, B3, " +
+ "r1ID, r2ID, r3ID, b1ID, b2ID, b3ID) " +
+ "VALUES (" +
+ "?, ?, ?, " +
+ "?, ?, ?, ?, ?, ?, " +
+ "?, ?, ?, ?, ?, ?)")
+ if err != nil {
+ return errors.New(fmt.Sprint("Failed to prepare insertion into match database: ", err))
+ }
defer statement.Close()
- if error_ != nil {
- fmt.Println("failed to prepare match database:", error_)
- return (error_)
- }
- _, error_ = statement.Exec(m.MatchNumber, m.Round, m.CompLevel, m.R1, m.R2, m.R3, m.B1, m.B2, m.B3, rowIds[0], rowIds[1], rowIds[2], rowIds[3], rowIds[4], rowIds[5])
- if error_ != nil {
- fmt.Println(error_)
- return (error_)
+
+ _, err = statement.Exec(m.MatchNumber, m.Round, m.CompLevel,
+ m.R1, m.R2, m.R3, m.B1, m.B2, m.B3,
+ rowIds[0], rowIds[1], rowIds[2], rowIds[3], rowIds[4], rowIds[5])
+ if err != nil {
+ return errors.New(fmt.Sprint("Failed to insert into match database: ", err))
}
return nil
}
func (database *Database) AddToStats(s Stats) error {
- statement, error_ := database.Prepare("UPDATE team_match_stats SET TeamNumber = ?, MatchNumber = ?, ShotsMissed = ?, UpperGoalShots = ?, LowerGoalShots = ?, ShotsMissedAuto = ?, UpperGoalAuto = ?, LowerGoalAuto = ?, PlayedDefense = ?, Climbing = ? WHERE MatchNumber = ? AND TeamNumber = ?")
- if error_ != nil {
- fmt.Println(error_)
- return (error_)
+ statement, err := database.Prepare("UPDATE team_match_stats SET " +
+ "TeamNumber = ?, MatchNumber = ?, " +
+ "ShotsMissed = ?, UpperGoalShots = ?, LowerGoalShots = ?, " +
+ "ShotsMissedAuto = ?, UpperGoalAuto = ?, LowerGoalAuto = ?, " +
+ "PlayedDefense = ?, Climbing = ? " +
+ "WHERE MatchNumber = ? AND TeamNumber = ?")
+ if err != nil {
+ return errors.New(fmt.Sprint("Failed to prepare stats update statement: ", err))
}
- result, error_ := statement.Exec(s.TeamNumber, s.MatchNumber, s.ShotsMissed, s.UpperGoalShots, s.LowerGoalShots, s.ShotsMissedAuto, s.UpperGoalAuto, s.LowerGoalAuto, s.PlayedDefense, s.Climbing, s.MatchNumber, s.TeamNumber)
- if error_ != nil {
- fmt.Println(error_)
- return (error_)
+ defer statement.Close()
+
+ result, err := statement.Exec(s.TeamNumber, s.MatchNumber,
+ s.ShotsMissed, s.UpperGoalShots, s.LowerGoalShots,
+ s.ShotsMissedAuto, s.UpperGoalAuto, s.LowerGoalAuto,
+ s.PlayedDefense, s.Climbing,
+ s.MatchNumber, s.TeamNumber)
+ if err != nil {
+ return errors.New(fmt.Sprint("Failed to update stats database: ", err))
}
- numRowsAffected, error_ := result.RowsAffected()
- if error_ != nil {
- return errors.New(fmt.Sprint("Failed to query rows affected: ", error_))
+
+ numRowsAffected, err := result.RowsAffected()
+ if err != nil {
+ return errors.New(fmt.Sprint("Failed to query rows affected: ", err))
}
if numRowsAffected == 0 {
return errors.New(fmt.Sprint(
@@ -121,16 +223,21 @@
}
func (database *Database) ReturnMatches() ([]Match, error) {
- matches := make([]Match, 0)
- rows, _ := database.Query("SELECT * FROM matches")
+ rows, err := database.Query("SELECT * FROM matches")
+ if err != nil {
+ return nil, errors.New(fmt.Sprint("Failed to select from matches: ", err))
+ }
defer rows.Close()
+
+ matches := make([]Match, 0)
for rows.Next() {
var match Match
var id int
- error_ := rows.Scan(&id, &match.MatchNumber, &match.Round, &match.CompLevel, &match.R1, &match.R2, &match.R3, &match.B1, &match.B2, &match.B3, &match.r1ID, &match.r2ID, &match.r3ID, &match.b1ID, &match.b2ID, &match.b3ID)
- if error_ != nil {
- fmt.Println(nil, error_)
- return nil, error_
+ err := rows.Scan(&id, &match.MatchNumber, &match.Round, &match.CompLevel,
+ &match.R1, &match.R2, &match.R3, &match.B1, &match.B2, &match.B3,
+ &match.r1ID, &match.r2ID, &match.r3ID, &match.b1ID, &match.b2ID, &match.b3ID)
+ if err != nil {
+ return nil, errors.New(fmt.Sprint("Failed to scan from matches: ", err))
}
matches = append(matches, match)
}
@@ -138,19 +245,22 @@
}
func (database *Database) ReturnStats() ([]Stats, error) {
- rows, error_ := database.Query("SELECT * FROM team_match_stats")
- if error_ != nil {
- return nil, errors.New(fmt.Sprint("Failed to SELECT * FROM team_match_stats: ", error_))
+ rows, err := database.Query("SELECT * FROM team_match_stats")
+ if err != nil {
+ return nil, errors.New(fmt.Sprint("Failed to SELECT * FROM team_match_stats: ", err))
}
defer rows.Close()
+
teams := make([]Stats, 0)
- var id int
for rows.Next() {
var team Stats
- error_ := rows.Scan(&id, &team.TeamNumber, &team.MatchNumber, &team.ShotsMissed, &team.UpperGoalShots, &team.LowerGoalShots, &team.ShotsMissedAuto, &team.UpperGoalAuto, &team.LowerGoalAuto, &team.PlayedDefense, &team.Climbing)
- if error_ != nil {
- fmt.Println(error_)
- return nil, error_
+ var id int
+ err = rows.Scan(&id, &team.TeamNumber, &team.MatchNumber,
+ &team.ShotsMissed, &team.UpperGoalShots, &team.LowerGoalShots,
+ &team.ShotsMissedAuto, &team.UpperGoalAuto, &team.LowerGoalAuto,
+ &team.PlayedDefense, &team.Climbing)
+ if err != nil {
+ return nil, errors.New(fmt.Sprint("Failed to scan from stats: ", err))
}
teams = append(teams, team)
}
@@ -158,41 +268,87 @@
}
func (database *Database) QueryMatches(teamNumber_ int32) ([]Match, error) {
- rows, error_ := database.Query("SELECT * FROM matches WHERE R1 = ? OR R2 = ? OR R3 = ? OR B1 = ? OR B2 = ? OR B3 = ?", teamNumber_, teamNumber_, teamNumber_, teamNumber_, teamNumber_, teamNumber_)
- if error_ != nil {
- fmt.Println("failed to execute statement 1:", error_)
- return nil, error_
+ rows, err := database.Query("SELECT * FROM matches WHERE "+
+ "R1 = ? OR R2 = ? OR R3 = ? OR B1 = ? OR B2 = ? OR B3 = ?",
+ teamNumber_, teamNumber_, teamNumber_, teamNumber_, teamNumber_, teamNumber_)
+ if err != nil {
+ return nil, errors.New(fmt.Sprint("Failed to select from matches for team: ", err))
}
defer rows.Close()
+
var matches []Match
- var id int
for rows.Next() {
var match Match
- rows.Scan(&id, &match.MatchNumber, &match.Round, &match.CompLevel, &match.R1, &match.R2, &match.R3, &match.B1, &match.B2, &match.B3, &match.r1ID, &match.r2ID, &match.r3ID, &match.b1ID, &match.b2ID, &match.b3ID)
+ var id int
+ err = rows.Scan(&id, &match.MatchNumber, &match.Round, &match.CompLevel,
+ &match.R1, &match.R2, &match.R3, &match.B1, &match.B2, &match.B3,
+ &match.r1ID, &match.r2ID, &match.r3ID, &match.b1ID, &match.b2ID, &match.b3ID)
+ if err != nil {
+ return nil, errors.New(fmt.Sprint("Failed to scan from matches: ", err))
+ }
matches = append(matches, match)
}
return matches, nil
}
func (database *Database) QueryStats(teamNumber_ int) ([]Stats, error) {
- rows, error_ := database.Query("SELECT * FROM team_match_stats WHERE TeamNumber = ?", teamNumber_)
- if error_ != nil {
- fmt.Println("failed to execute statement 3:", error_)
- return nil, error_
+ rows, err := database.Query("SELECT * FROM team_match_stats WHERE TeamNumber = ?", teamNumber_)
+ if err != nil {
+ return nil, errors.New(fmt.Sprint("Failed to select from stats: ", err))
}
defer rows.Close()
+
var teams []Stats
for rows.Next() {
var team Stats
var id int
- error_ = rows.Scan(&id, &team.TeamNumber, &team.MatchNumber, &team.ShotsMissed,
- &team.UpperGoalShots, &team.LowerGoalShots, &team.ShotsMissedAuto, &team.UpperGoalAuto,
- &team.LowerGoalAuto, &team.PlayedDefense, &team.Climbing)
+ err = rows.Scan(&id, &team.TeamNumber, &team.MatchNumber,
+ &team.ShotsMissed, &team.UpperGoalShots, &team.LowerGoalShots,
+ &team.ShotsMissedAuto, &team.UpperGoalAuto, &team.LowerGoalAuto,
+ &team.PlayedDefense, &team.Climbing)
+ if err != nil {
+ return nil, errors.New(fmt.Sprint("Failed to scan from stats: ", err))
+ }
teams = append(teams, team)
}
- if error_ != nil {
- fmt.Println("failed to execute statement 3:", error_)
- return nil, error_
- }
return teams, nil
}
+
+func (database *Database) QueryNotes(TeamNumber int32) (NotesData, error) {
+ rows, err := database.Query("SELECT * FROM team_notes WHERE TeamNumber = ?", TeamNumber)
+ if err != nil {
+ return NotesData{}, errors.New(fmt.Sprint("Failed to select from notes: ", err))
+ }
+ defer rows.Close()
+
+ var notes []string
+ for rows.Next() {
+ var id int32
+ var data string
+ err = rows.Scan(&id, &TeamNumber, &data)
+ if err != nil {
+ return NotesData{}, errors.New(fmt.Sprint("Failed to scan from notes: ", err))
+ }
+ notes = append(notes, data)
+ }
+ return NotesData{TeamNumber, notes}, nil
+}
+
+func (database *Database) AddNotes(data NotesData) error {
+ if len(data.Notes) > 1 {
+ return errors.New("Can only insert one row of notes at a time")
+ }
+ statement, err := database.Prepare("INSERT INTO " +
+ "team_notes(TeamNumber, Notes)" +
+ "VALUES (?, ?)")
+ if err != nil {
+ return errors.New(fmt.Sprint("Failed to prepare insertion into notes table: ", err))
+ }
+ defer statement.Close()
+
+ _, err = statement.Exec(data.TeamNumber, data.Notes[0])
+ if err != nil {
+ return errors.New(fmt.Sprint("Failed to insert into Notes database: ", err))
+ }
+ return nil
+}
diff --git a/scouting/db/db_test.go b/scouting/db/db_test.go
index 6e3e8c3..13a43f8 100644
--- a/scouting/db/db_test.go
+++ b/scouting/db/db_test.go
@@ -1,21 +1,28 @@
package db
import (
+ "fmt"
"os"
"path/filepath"
"reflect"
"testing"
)
+// Shortcut for error checking. If the specified error is non-nil, print the
+// error message and exit the test.
+func check(t *testing.T, err error, message string) {
+ if err != nil {
+ t.Fatal(message, ":", err)
+ }
+}
+
// Creates a database in TEST_TMPDIR so that we don't accidentally write it
// into the runfiles directory.
func createDatabase(t *testing.T) *Database {
// Get the path to our temporary writable directory.
testTmpdir := os.Getenv("TEST_TMPDIR")
db, err := NewDatabase(filepath.Join(testTmpdir, "scouting.db"))
- if err != nil {
- t.Fatal("Failed to create a new database: ", err)
- }
+ check(t, err, "Failed to create new database")
return db
}
@@ -23,12 +30,22 @@
db := createDatabase(t)
defer db.Delete()
- correct := []Match{Match{MatchNumber: 7, Round: 1, CompLevel: "quals", R1: 9999, R2: 1000, R3: 777, B1: 0000, B2: 4321, B3: 1234, r1ID: 1, r2ID: 2, r3ID: 3, b1ID: 4, b2ID: 5, b3ID: 6}}
- db.AddToMatch(correct[0])
- got, error_ := db.ReturnMatches()
- if error_ != nil {
- t.Fatalf(error_.Error())
+ correct := []Match{
+ Match{
+ MatchNumber: 7,
+ Round: 1,
+ CompLevel: "quals",
+ R1: 9999, R2: 1000, R3: 777, B1: 0000, B2: 4321, B3: 1234,
+ r1ID: 1, r2ID: 2, r3ID: 3, b1ID: 4, b2ID: 5, b3ID: 6,
+ },
}
+
+ err := db.AddToMatch(correct[0])
+ check(t, err, "Failed to add match data")
+
+ got, err := db.ReturnMatches()
+ check(t, err, "Failed ReturnMatches()")
+
if !reflect.DeepEqual(correct, got) {
t.Fatalf("Got %#v,\nbut expected %#v.", got, correct)
}
@@ -39,26 +56,58 @@
defer db.Delete()
correct := []Stats{
- Stats{TeamNumber: 1236, MatchNumber: 7, ShotsMissed: 9, UpperGoalShots: 5, LowerGoalShots: 4, ShotsMissedAuto: 3, UpperGoalAuto: 2, LowerGoalAuto: 1, PlayedDefense: 2, Climbing: 3},
- Stats{TeamNumber: 1001, MatchNumber: 7, ShotsMissed: 6, UpperGoalShots: 9, LowerGoalShots: 9, ShotsMissedAuto: 0, UpperGoalAuto: 0, LowerGoalAuto: 0, PlayedDefense: 0, Climbing: 0},
- Stats{TeamNumber: 777, MatchNumber: 7, ShotsMissed: 5, UpperGoalShots: 7, LowerGoalShots: 12, ShotsMissedAuto: 0, UpperGoalAuto: 4, LowerGoalAuto: 0, PlayedDefense: 0, Climbing: 0},
- Stats{TeamNumber: 1000, MatchNumber: 7, ShotsMissed: 12, UpperGoalShots: 6, LowerGoalShots: 10, ShotsMissedAuto: 0, UpperGoalAuto: 7, LowerGoalAuto: 0, PlayedDefense: 0, Climbing: 0},
- Stats{TeamNumber: 4321, MatchNumber: 7, ShotsMissed: 14, UpperGoalShots: 12, LowerGoalShots: 3, ShotsMissedAuto: 0, UpperGoalAuto: 7, LowerGoalAuto: 0, PlayedDefense: 0, Climbing: 0},
- Stats{TeamNumber: 1234, MatchNumber: 7, ShotsMissed: 3, UpperGoalShots: 4, LowerGoalShots: 0, ShotsMissedAuto: 0, UpperGoalAuto: 9, LowerGoalAuto: 0, PlayedDefense: 0, Climbing: 0},
+ Stats{
+ TeamNumber: 1236, MatchNumber: 7,
+ ShotsMissed: 9, UpperGoalShots: 5, LowerGoalShots: 4,
+ ShotsMissedAuto: 3, UpperGoalAuto: 2, LowerGoalAuto: 1,
+ PlayedDefense: 2, Climbing: 3,
+ },
+ Stats{
+ TeamNumber: 1001, MatchNumber: 7,
+ ShotsMissed: 6, UpperGoalShots: 9, LowerGoalShots: 9,
+ ShotsMissedAuto: 0, UpperGoalAuto: 0, LowerGoalAuto: 0,
+ PlayedDefense: 0, Climbing: 0,
+ },
+ Stats{
+ TeamNumber: 777, MatchNumber: 7,
+ ShotsMissed: 5, UpperGoalShots: 7, LowerGoalShots: 12,
+ ShotsMissedAuto: 0, UpperGoalAuto: 4, LowerGoalAuto: 0,
+ PlayedDefense: 0, Climbing: 0,
+ },
+ Stats{
+ TeamNumber: 1000, MatchNumber: 7,
+ ShotsMissed: 12, UpperGoalShots: 6, LowerGoalShots: 10,
+ ShotsMissedAuto: 0, UpperGoalAuto: 7, LowerGoalAuto: 0,
+ PlayedDefense: 0, Climbing: 0,
+ },
+ Stats{
+ TeamNumber: 4321, MatchNumber: 7,
+ ShotsMissed: 14, UpperGoalShots: 12, LowerGoalShots: 3,
+ ShotsMissedAuto: 0, UpperGoalAuto: 7, LowerGoalAuto: 0,
+ PlayedDefense: 0, Climbing: 0,
+ },
+ Stats{
+ TeamNumber: 1234, MatchNumber: 7,
+ ShotsMissed: 3, UpperGoalShots: 4, LowerGoalShots: 0,
+ ShotsMissedAuto: 0, UpperGoalAuto: 9, LowerGoalAuto: 0,
+ PlayedDefense: 0, Climbing: 0,
+ },
}
- err := db.AddToMatch(Match{MatchNumber: 7, Round: 1, CompLevel: "quals", R1: 1236, R2: 1001, R3: 777, B1: 1000, B2: 4321, B3: 1234, r1ID: 1, r2ID: 2, r3ID: 3, b1ID: 4, b2ID: 5, b3ID: 6})
- if err != nil {
- t.Fatal("Failed to add match: ", err)
- }
+
+ err := db.AddToMatch(Match{
+ MatchNumber: 7, Round: 1, CompLevel: "quals",
+ R1: 1236, R2: 1001, R3: 777, B1: 1000, B2: 4321, B3: 1234,
+ r1ID: 1, r2ID: 2, r3ID: 3, b1ID: 4, b2ID: 5, b3ID: 6})
+ check(t, err, "Failed to add match")
+
for i := 0; i < len(correct); i++ {
- if err := db.AddToStats(correct[i]); err != nil {
- t.Fatal("Failed to add stats to DB: ", err)
- }
+ err = db.AddToStats(correct[i])
+ check(t, err, "Failed to add stats to DB")
}
- got, error_ := db.ReturnStats()
- if error_ != nil {
- t.Fatalf(error_.Error())
- }
+
+ got, err := db.ReturnStats()
+ check(t, err, "Failed ReturnStats()")
+
if !reflect.DeepEqual(correct, got) {
t.Errorf("Got %#v,\nbut expected %#v.", got, correct)
}
@@ -76,18 +125,26 @@
}
for i := 0; i < len(testDatabase); i++ {
- db.AddToMatch(testDatabase[i])
+ err := db.AddToMatch(testDatabase[i])
+ check(t, err, fmt.Sprint("Failed to add match", i))
}
correct := []Match{
- Match{MatchNumber: 2, Round: 1, CompLevel: "quals", R1: 251, R2: 169, R3: 286, B1: 253, B2: 538, B3: 149, r1ID: 1, r2ID: 2, r3ID: 3, b1ID: 4, b2ID: 5, b3ID: 6},
- Match{MatchNumber: 3, Round: 1, CompLevel: "quals", R1: 147, R2: 421, R3: 538, B1: 126, B2: 448, B3: 262, r1ID: 13, r2ID: 14, r3ID: 15, b1ID: 16, b2ID: 17, b3ID: 18},
+ Match{
+ MatchNumber: 2, Round: 1, CompLevel: "quals",
+ R1: 251, R2: 169, R3: 286, B1: 253, B2: 538, B3: 149,
+ r1ID: 1, r2ID: 2, r3ID: 3, b1ID: 4, b2ID: 5, b3ID: 6,
+ },
+ Match{
+ MatchNumber: 3, Round: 1, CompLevel: "quals",
+ R1: 147, R2: 421, R3: 538, B1: 126, B2: 448, B3: 262,
+ r1ID: 13, r2ID: 14, r3ID: 15, b1ID: 16, b2ID: 17, b3ID: 18,
+ },
}
- got, error_ := db.QueryMatches(538)
- if error_ != nil {
- t.Fatal("Failed to query matches for 538: ", error_)
- }
+ got, err := db.QueryMatches(538)
+ check(t, err, "Failed to query matches for 538")
+
if !reflect.DeepEqual(correct, got) {
t.Fatalf("Got %#v,\nbut expected %#v.", got, correct)
}
@@ -98,24 +155,65 @@
defer db.Delete()
testDatabase := []Stats{
- Stats{TeamNumber: 1235, MatchNumber: 94, ShotsMissed: 2, UpperGoalShots: 2, LowerGoalShots: 2, ShotsMissedAuto: 2, UpperGoalAuto: 2, LowerGoalAuto: 2, PlayedDefense: 2, Climbing: 2},
- Stats{TeamNumber: 1234, MatchNumber: 94, ShotsMissed: 4, UpperGoalShots: 4, LowerGoalShots: 4, ShotsMissedAuto: 4, UpperGoalAuto: 4, LowerGoalAuto: 4, PlayedDefense: 7, Climbing: 2},
- Stats{TeamNumber: 1233, MatchNumber: 94, ShotsMissed: 3, UpperGoalShots: 3, LowerGoalShots: 3, ShotsMissedAuto: 3, UpperGoalAuto: 3, LowerGoalAuto: 3, PlayedDefense: 3, Climbing: 3},
- Stats{TeamNumber: 1232, MatchNumber: 94, ShotsMissed: 5, UpperGoalShots: 5, LowerGoalShots: 5, ShotsMissedAuto: 5, UpperGoalAuto: 5, LowerGoalAuto: 5, PlayedDefense: 7, Climbing: 1},
- Stats{TeamNumber: 1231, MatchNumber: 94, ShotsMissed: 6, UpperGoalShots: 6, LowerGoalShots: 6, ShotsMissedAuto: 6, UpperGoalAuto: 6, LowerGoalAuto: 6, PlayedDefense: 7, Climbing: 1},
- Stats{TeamNumber: 1239, MatchNumber: 94, ShotsMissed: 7, UpperGoalShots: 7, LowerGoalShots: 7, ShotsMissedAuto: 7, UpperGoalAuto: 7, LowerGoalAuto: 3, PlayedDefense: 7, Climbing: 1},
+ Stats{
+ TeamNumber: 1235, MatchNumber: 94,
+ ShotsMissed: 2, UpperGoalShots: 2, LowerGoalShots: 2,
+ ShotsMissedAuto: 2, UpperGoalAuto: 2, LowerGoalAuto: 2,
+ PlayedDefense: 2, Climbing: 2},
+ Stats{
+ TeamNumber: 1234, MatchNumber: 94,
+ ShotsMissed: 4, UpperGoalShots: 4, LowerGoalShots: 4,
+ ShotsMissedAuto: 4, UpperGoalAuto: 4, LowerGoalAuto: 4,
+ PlayedDefense: 7, Climbing: 2,
+ },
+ Stats{
+ TeamNumber: 1233, MatchNumber: 94,
+ ShotsMissed: 3, UpperGoalShots: 3, LowerGoalShots: 3,
+ ShotsMissedAuto: 3, UpperGoalAuto: 3, LowerGoalAuto: 3,
+ PlayedDefense: 3, Climbing: 3,
+ },
+ Stats{
+ TeamNumber: 1232, MatchNumber: 94,
+ ShotsMissed: 5, UpperGoalShots: 5, LowerGoalShots: 5,
+ ShotsMissedAuto: 5, UpperGoalAuto: 5, LowerGoalAuto: 5,
+ PlayedDefense: 7, Climbing: 1,
+ },
+ Stats{
+ TeamNumber: 1231, MatchNumber: 94,
+ ShotsMissed: 6, UpperGoalShots: 6, LowerGoalShots: 6,
+ ShotsMissedAuto: 6, UpperGoalAuto: 6, LowerGoalAuto: 6,
+ PlayedDefense: 7, Climbing: 1,
+ },
+ Stats{
+ TeamNumber: 1239, MatchNumber: 94,
+ ShotsMissed: 7, UpperGoalShots: 7, LowerGoalShots: 7,
+ ShotsMissedAuto: 7, UpperGoalAuto: 7, LowerGoalAuto: 3,
+ PlayedDefense: 7, Climbing: 1,
+ },
}
- db.AddToMatch(Match{MatchNumber: 94, Round: 1, CompLevel: "quals", R1: 1235, R2: 1234, R3: 1233, B1: 1232, B2: 1231, B3: 1239})
+
+ err := db.AddToMatch(Match{
+ MatchNumber: 94, Round: 1, CompLevel: "quals",
+ R1: 1235, R2: 1234, R3: 1233, B1: 1232, B2: 1231, B3: 1239})
+ check(t, err, "Failed to add match")
+
for i := 0; i < len(testDatabase); i++ {
- db.AddToStats(testDatabase[i])
+ err = db.AddToStats(testDatabase[i])
+ check(t, err, fmt.Sprint("Failed to add stats", i))
}
+
correct := []Stats{
- Stats{TeamNumber: 1235, MatchNumber: 94, ShotsMissed: 2, UpperGoalShots: 2, LowerGoalShots: 2, ShotsMissedAuto: 2, UpperGoalAuto: 2, LowerGoalAuto: 2, PlayedDefense: 2, Climbing: 2},
+ Stats{
+ TeamNumber: 1235, MatchNumber: 94,
+ ShotsMissed: 2, UpperGoalShots: 2, LowerGoalShots: 2,
+ ShotsMissedAuto: 2, UpperGoalAuto: 2, LowerGoalAuto: 2,
+ PlayedDefense: 2, Climbing: 2,
+ },
}
- got, error_ := db.QueryStats(1235)
- if error_ != nil {
- t.Fatalf(error_.Error())
- }
+
+ got, err := db.QueryStats(1235)
+ check(t, err, "Failed QueryStats()")
+
if !reflect.DeepEqual(correct, got) {
t.Errorf("Got %#v,\nbut expected %#v.", got, correct)
}
@@ -126,19 +224,41 @@
defer db.Delete()
correct := []Match{
- Match{MatchNumber: 2, Round: 1, CompLevel: "quals", R1: 251, R2: 169, R3: 286, B1: 253, B2: 538, B3: 149, r1ID: 1, r2ID: 2, r3ID: 3, b1ID: 4, b2ID: 5, b3ID: 6},
- Match{MatchNumber: 3, Round: 1, CompLevel: "quals", R1: 147, R2: 421, R3: 538, B1: 126, B2: 448, B3: 262, r1ID: 7, r2ID: 8, r3ID: 9, b1ID: 10, b2ID: 11, b3ID: 12},
- Match{MatchNumber: 4, Round: 1, CompLevel: "quals", R1: 251, R2: 169, R3: 286, B1: 653, B2: 538, B3: 149, r1ID: 13, r2ID: 14, r3ID: 15, b1ID: 16, b2ID: 17, b3ID: 18},
- Match{MatchNumber: 5, Round: 1, CompLevel: "quals", R1: 198, R2: 1421, R3: 538, B1: 26, B2: 448, B3: 262, r1ID: 19, r2ID: 20, r3ID: 21, b1ID: 22, b2ID: 23, b3ID: 24},
- Match{MatchNumber: 6, Round: 1, CompLevel: "quals", R1: 251, R2: 188, R3: 286, B1: 555, B2: 538, B3: 149, r1ID: 25, r2ID: 26, r3ID: 27, b1ID: 28, b2ID: 29, b3ID: 30},
+ Match{
+ MatchNumber: 2, Round: 1, CompLevel: "quals",
+ R1: 251, R2: 169, R3: 286, B1: 253, B2: 538, B3: 149,
+ r1ID: 1, r2ID: 2, r3ID: 3, b1ID: 4, b2ID: 5, b3ID: 6,
+ },
+ Match{
+ MatchNumber: 3, Round: 1, CompLevel: "quals",
+ R1: 147, R2: 421, R3: 538, B1: 126, B2: 448, B3: 262,
+ r1ID: 7, r2ID: 8, r3ID: 9, b1ID: 10, b2ID: 11, b3ID: 12,
+ },
+ Match{
+ MatchNumber: 4, Round: 1, CompLevel: "quals",
+ R1: 251, R2: 169, R3: 286, B1: 653, B2: 538, B3: 149,
+ r1ID: 13, r2ID: 14, r3ID: 15, b1ID: 16, b2ID: 17, b3ID: 18,
+ },
+ Match{
+ MatchNumber: 5, Round: 1, CompLevel: "quals",
+ R1: 198, R2: 1421, R3: 538, B1: 26, B2: 448, B3: 262,
+ r1ID: 19, r2ID: 20, r3ID: 21, b1ID: 22, b2ID: 23, b3ID: 24,
+ },
+ Match{
+ MatchNumber: 6, Round: 1, CompLevel: "quals",
+ R1: 251, R2: 188, R3: 286, B1: 555, B2: 538, B3: 149,
+ r1ID: 25, r2ID: 26, r3ID: 27, b1ID: 28, b2ID: 29, b3ID: 30,
+ },
}
+
for i := 0; i < len(correct); i++ {
- db.AddToMatch(correct[i])
+ err := db.AddToMatch(correct[i])
+ check(t, err, fmt.Sprint("Failed to add match", i))
}
- got, error_ := db.ReturnMatches()
- if error_ != nil {
- t.Fatalf(error_.Error())
- }
+
+ got, err := db.ReturnMatches()
+ check(t, err, "Failed ReturnMatches()")
+
if !reflect.DeepEqual(correct, got) {
t.Errorf("Got %#v,\nbut expected %#v.", got, correct)
}
@@ -149,22 +269,82 @@
defer db.Delete()
correct := []Stats{
- Stats{TeamNumber: 1235, MatchNumber: 94, ShotsMissed: 2, UpperGoalShots: 2, LowerGoalShots: 2, ShotsMissedAuto: 2, UpperGoalAuto: 2, LowerGoalAuto: 2, PlayedDefense: 2, Climbing: 2},
- Stats{TeamNumber: 1236, MatchNumber: 94, ShotsMissed: 4, UpperGoalShots: 4, LowerGoalShots: 4, ShotsMissedAuto: 4, UpperGoalAuto: 4, LowerGoalAuto: 4, PlayedDefense: 7, Climbing: 2},
- Stats{TeamNumber: 1237, MatchNumber: 94, ShotsMissed: 3, UpperGoalShots: 3, LowerGoalShots: 3, ShotsMissedAuto: 3, UpperGoalAuto: 3, LowerGoalAuto: 3, PlayedDefense: 3, Climbing: 3},
- Stats{TeamNumber: 1238, MatchNumber: 94, ShotsMissed: 5, UpperGoalShots: 5, LowerGoalShots: 5, ShotsMissedAuto: 5, UpperGoalAuto: 5, LowerGoalAuto: 5, PlayedDefense: 7, Climbing: 1},
- Stats{TeamNumber: 1239, MatchNumber: 94, ShotsMissed: 6, UpperGoalShots: 6, LowerGoalShots: 6, ShotsMissedAuto: 6, UpperGoalAuto: 6, LowerGoalAuto: 6, PlayedDefense: 7, Climbing: 1},
- Stats{TeamNumber: 1233, MatchNumber: 94, ShotsMissed: 7, UpperGoalShots: 7, LowerGoalShots: 7, ShotsMissedAuto: 7, UpperGoalAuto: 7, LowerGoalAuto: 3, PlayedDefense: 7, Climbing: 1},
+ Stats{
+ TeamNumber: 1235, MatchNumber: 94,
+ ShotsMissed: 2, UpperGoalShots: 2, LowerGoalShots: 2,
+ ShotsMissedAuto: 2, UpperGoalAuto: 2, LowerGoalAuto: 2,
+ PlayedDefense: 2, Climbing: 2,
+ },
+ Stats{
+ TeamNumber: 1236, MatchNumber: 94,
+ ShotsMissed: 4, UpperGoalShots: 4, LowerGoalShots: 4,
+ ShotsMissedAuto: 4, UpperGoalAuto: 4, LowerGoalAuto: 4,
+ PlayedDefense: 7, Climbing: 2,
+ },
+ Stats{
+ TeamNumber: 1237, MatchNumber: 94,
+ ShotsMissed: 3, UpperGoalShots: 3, LowerGoalShots: 3,
+ ShotsMissedAuto: 3, UpperGoalAuto: 3, LowerGoalAuto: 3,
+ PlayedDefense: 3, Climbing: 3,
+ },
+ Stats{
+ TeamNumber: 1238, MatchNumber: 94,
+ ShotsMissed: 5, UpperGoalShots: 5, LowerGoalShots: 5,
+ ShotsMissedAuto: 5, UpperGoalAuto: 5, LowerGoalAuto: 5,
+ PlayedDefense: 7, Climbing: 1,
+ },
+ Stats{
+ TeamNumber: 1239, MatchNumber: 94,
+ ShotsMissed: 6, UpperGoalShots: 6, LowerGoalShots: 6,
+ ShotsMissedAuto: 6, UpperGoalAuto: 6, LowerGoalAuto: 6,
+ PlayedDefense: 7, Climbing: 1,
+ },
+ Stats{
+ TeamNumber: 1233, MatchNumber: 94,
+ ShotsMissed: 7, UpperGoalShots: 7, LowerGoalShots: 7,
+ ShotsMissedAuto: 7, UpperGoalAuto: 7, LowerGoalAuto: 3,
+ PlayedDefense: 7, Climbing: 1,
+ },
}
- db.AddToMatch(Match{MatchNumber: 94, Round: 1, CompLevel: "quals", R1: 1235, R2: 1236, R3: 1237, B1: 1238, B2: 1239, B3: 1233})
+
+ err := db.AddToMatch(Match{
+ MatchNumber: 94, Round: 1, CompLevel: "quals",
+ R1: 1235, R2: 1236, R3: 1237, B1: 1238, B2: 1239, B3: 1233})
+ check(t, err, "Failed to add match")
+
for i := 0; i < len(correct); i++ {
- db.AddToStats(correct[i])
+ err = db.AddToStats(correct[i])
+ check(t, err, fmt.Sprint("Failed to add stats", i))
}
- got, error_ := db.ReturnStats()
- if error_ != nil {
- t.Fatalf(error_.Error())
- }
+
+ got, err := db.ReturnStats()
+ check(t, err, "Failed ReturnStats()")
+
if !reflect.DeepEqual(correct, got) {
t.Errorf("Got %#v,\nbut expected %#v.", got, correct)
}
}
+
+func TestNotes(t *testing.T) {
+ db := createDatabase(t)
+ defer db.Delete()
+
+ expected := NotesData{
+ TeamNumber: 1234,
+ Notes: []string{"Note 1", "Note 3"},
+ }
+
+ err := db.AddNotes(NotesData{1234, []string{"Note 1"}})
+ check(t, err, "Failed to add Note")
+ err = db.AddNotes(NotesData{1235, []string{"Note 2"}})
+ check(t, err, "Failed to add Note")
+ err = db.AddNotes(NotesData{1234, []string{"Note 3"}})
+ check(t, err, "Failed to add Note")
+
+ actual, err := db.QueryNotes(1234)
+ check(t, err, "Failed to get Notes")
+
+ if !reflect.DeepEqual(expected, actual) {
+ t.Errorf("Got %#v,\nbut expected %#v.", actual, expected)
+ }
+}
diff --git a/scouting/deploy/BUILD b/scouting/deploy/BUILD
new file mode 100644
index 0000000..ed4b9cd
--- /dev/null
+++ b/scouting/deploy/BUILD
@@ -0,0 +1,60 @@
+load("@rules_pkg//pkg:pkg.bzl", "pkg_deb", "pkg_tar")
+load("@rules_pkg//pkg:mappings.bzl", "pkg_files")
+
+pkg_files(
+ name = "systemd_files",
+ srcs = [
+ "scouting.service",
+ ],
+ prefix = "etc/systemd/system",
+)
+
+pkg_tar(
+ name = "server_files",
+ srcs = [
+ "//scouting",
+ ],
+ include_runfiles = True,
+ package_dir = "opt/frc971/scouting_server",
+ strip_prefix = ".",
+)
+
+pkg_tar(
+ name = "deploy_tar",
+ srcs = [
+ ":systemd_files",
+ ],
+ deps = [
+ ":server_files",
+ ],
+)
+
+pkg_deb(
+ name = "frc971-scouting-server",
+ architecture = "amd64",
+ data = ":deploy_tar",
+ description = "The FRC971 scouting web server.",
+ # TODO(phil): What's a good email address for this?
+ maintainer = "frc971@frc971.org",
+ package = "frc971-scouting-server",
+ postinst = "postinst",
+ predepends = [
+ "systemd",
+ ],
+ prerm = "prerm",
+ version = "1",
+)
+
+py_binary(
+ name = "deploy",
+ srcs = [
+ "deploy.py",
+ ],
+ args = [
+ "--deb",
+ "$(location :frc971-scouting-server)",
+ ],
+ data = [
+ ":frc971-scouting-server",
+ ],
+)
diff --git a/scouting/deploy/README.md b/scouting/deploy/README.md
new file mode 100644
index 0000000..6d223da
--- /dev/null
+++ b/scouting/deploy/README.md
@@ -0,0 +1,37 @@
+Deploying the scouting application
+================================================================================
+The scouting application is deployed to `scouting.frc971.org` via `bazel`:
+```console
+$ bazel run //scouting/deploy
+(Reading database ... 119978 files and directories currently installed.)
+Preparing to unpack .../frc971-scouting-server_1_amd64.deb ...
+Removed /etc/systemd/system/multi-user.target.wants/scouting.service.
+Unpacking frc971-scouting-server (1) over (1) ...
+Setting up frc971-scouting-server (1) ...
+Created symlink /etc/systemd/system/multi-user.target.wants/scouting.service → /etc/systemd/system/scouting.service.
+Connection to scouting.frc971.org closed.
+```
+
+You will need SSH access to the scouting server. You can customize the SSH host
+with the `--host` argument.
+
+The Blue Alliance API key
+--------------------------------------------------------------------------------
+You need to set up an API key on the scouting server so that the scraping logic
+can use it. It needs to live in `/var/frc971/scouting/tba_config.json` and look
+as follows:
+```json
+{
+ "api_key": "..."
+}
+```
+
+Starting and stopping the application
+--------------------------------------------------------------------------------
+When you SSH into the scouting server, use `systemctl` to manage
+`scouting.service` like any other service.
+```console
+$ sudo systemctl stop scouting.service
+$ sudo systemctl start scouting.service
+$ sudo systemctl restart scouting.service
+```
diff --git a/scouting/deploy/deploy.py b/scouting/deploy/deploy.py
new file mode 100644
index 0000000..c9886fb
--- /dev/null
+++ b/scouting/deploy/deploy.py
@@ -0,0 +1,34 @@
+import argparse
+from pathlib import Path
+import subprocess
+import sys
+
+def main(argv):
+ """Installs the scouting application on the scouting server."""
+ parser = argparse.ArgumentParser()
+ parser.add_argument(
+ "--deb",
+ type=str,
+ required=True,
+ help="The .deb file to deploy.",
+ )
+ parser.add_argument(
+ "--host",
+ type=str,
+ default="scouting.frc971.org",
+ help="The SSH host to install the scouting web server to.",
+ )
+ args = parser.parse_args(argv[1:])
+ deb = Path(args.deb)
+
+ # Copy the .deb to the scouting server, install it, and delete it again.
+ subprocess.run(["rsync", "-L", args.deb, f"{args.host}:/tmp/{deb.name}"],
+ check=True, stdin=sys.stdin)
+ subprocess.run(f"ssh -tt {args.host} sudo dpkg -i /tmp/{deb.name}",
+ shell=True, check=True, stdin=sys.stdin)
+ subprocess.run(f"ssh {args.host} rm -f /tmp/{deb.name}",
+ shell=True, check=True, stdin=sys.stdin)
+
+
+if __name__ == "__main__":
+ sys.exit(main(sys.argv))
diff --git a/scouting/deploy/postinst b/scouting/deploy/postinst
new file mode 100644
index 0000000..a7a8b16
--- /dev/null
+++ b/scouting/deploy/postinst
@@ -0,0 +1,24 @@
+#!/bin/bash
+
+# This script runs after the frc971-scouting-server package is installed. This
+# script is responsible for making sure the webserver has everything it needs,
+# then starts the webserver.
+
+set -o errexit
+set -o nounset
+set -o pipefail
+
+# Create a directory for the database to live in.
+mkdir -p /var/frc971/scouting/
+
+# Create an empty The Blue Alliance configuration file.
+if [[ ! -e /var/frc971/scouting/tba_config.json ]]; then
+ echo '{}' > /var/frc971/scouting/tba_config.json
+fi
+
+# Make sure it's all usable by the user.
+chown -R www-data:www-data /var/frc971/scouting/
+
+systemctl daemon-reload
+systemctl enable scouting.service
+systemctl start scouting.service || :
diff --git a/scouting/deploy/prerm b/scouting/deploy/prerm
new file mode 100644
index 0000000..31e3fbc
--- /dev/null
+++ b/scouting/deploy/prerm
@@ -0,0 +1,13 @@
+#!/bin/bash
+
+# This script gets run before the frc971-scouting-server package gets removed
+# or upgraded. This script is responsible for stopping the webserver before the
+# underlying files are removed by dpkg.
+
+set -o errexit
+set -o nounset
+set -o pipefail
+
+systemctl stop scouting.service
+systemctl disable scouting.service
+systemctl daemon-reload
diff --git a/scouting/deploy/scouting.service b/scouting/deploy/scouting.service
new file mode 100644
index 0000000..2990f42
--- /dev/null
+++ b/scouting/deploy/scouting.service
@@ -0,0 +1,17 @@
+[Unit]
+Description=FRC971 Scouting Server
+After=systemd-networkd-wait-online.service
+
+[Service]
+User=www-data
+Group=www-data
+Type=simple
+WorkingDirectory=/opt/frc971/scouting_server
+ExecStart=/opt/frc971/scouting_server/scouting/scouting \
+ -port 8080 \
+ -database /var/frc971/scouting/scouting.db \
+ -tba_config /var/frc971/scouting/tba_config.json
+Restart=always
+
+[Install]
+WantedBy=multi-user.target
diff --git a/scouting/scouting_test.ts b/scouting/scouting_test.ts
index c3a1c6e..f400430 100644
--- a/scouting/scouting_test.ts
+++ b/scouting/scouting_test.ts
@@ -71,8 +71,8 @@
await expectReviewFieldToBe('Attempted to Climb', 'No');
// Validate Defense.
- await expectReviewFieldToBe('Defense Played On Rating', '3');
- await expectReviewFieldToBe('Defense Played Rating', '3');
+ await expectReviewFieldToBe('Defense Played On Rating', '0');
+ await expectReviewFieldToBe('Defense Played Rating', '0');
// TODO(phil): Submit data and make sure it made its way to the database
// correctly. Right now the /requests/submit/data_scouting endpoint is not
diff --git a/scouting/testing/BUILD b/scouting/testing/BUILD
new file mode 100644
index 0000000..43bbe06
--- /dev/null
+++ b/scouting/testing/BUILD
@@ -0,0 +1,12 @@
+py_binary(
+ name = "scouting_test_servers",
+ testonly = True,
+ srcs = [
+ "scouting_test_servers.py",
+ ],
+ data = [
+ "//scouting",
+ "//scouting/scraping:test_data",
+ ],
+ visibility = ["//visibility:public"],
+)
diff --git a/scouting/testing/scouting_test_servers.py b/scouting/testing/scouting_test_servers.py
new file mode 100644
index 0000000..3447815
--- /dev/null
+++ b/scouting/testing/scouting_test_servers.py
@@ -0,0 +1,108 @@
+"""This library is here to run the various servers involved in the scouting app.
+
+The servers are:
+ - The fake TBA server
+ - The actual web server
+"""
+
+import argparse
+import json
+import os
+from pathlib import Path
+import shutil
+import signal
+import socket
+import subprocess
+import sys
+import time
+from typing import List
+
+def wait_for_server(port: int):
+ """Waits for the server at the specified port to respond to TCP connections."""
+ while True:
+ try:
+ connection = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+ connection.connect(("localhost", port))
+ connection.close()
+ break
+ except ConnectionRefusedError:
+ connection.close()
+ time.sleep(0.01)
+
+def create_tba_config(tmpdir: Path) -> Path:
+ # Configure the scouting webserver to scrape data from our fake TBA
+ # server.
+ config = tmpdir / "scouting_config.json"
+ config.write_text(json.dumps({
+ "api_key": "dummy_key_that_is_not_actually_used_in_this_test",
+ "base_url": "http://localhost:7000",
+ }))
+ return config
+
+def set_up_tba_api_dir(tmpdir: Path, year: int, event_code: str):
+ tba_api_dir = tmpdir / "api" / "v3" / "event" / f"{year}{event_code}"
+ tba_api_dir.mkdir(parents=True, exist_ok=True)
+ (tba_api_dir / "matches").write_text(
+ Path(f"scouting/scraping/test_data/{year}_{event_code}.json").read_text()
+ )
+
+class Runner:
+ """Helps manage the services we need for testing the scouting app."""
+
+ def start(self, port: int):
+ """Starts the services needed for testing the scouting app."""
+ self.tmpdir = Path(os.environ["TEST_TMPDIR"]) / "servers"
+ self.tmpdir.mkdir(exist_ok=True)
+
+ db_path = self.tmpdir / "scouting.db"
+ tba_config = create_tba_config(self.tmpdir)
+
+ self.webserver = subprocess.Popen([
+ "scouting/scouting",
+ f"--port={port}",
+ f"--database={db_path}",
+ f"--tba_config={tba_config}",
+ ])
+
+ # Create a fake TBA server to serve the static match list.
+ set_up_tba_api_dir(self.tmpdir, year=2016, event_code="nytr")
+ set_up_tba_api_dir(self.tmpdir, year=2020, event_code="fake")
+ self.fake_tba_api = subprocess.Popen(
+ ["python3", "-m", "http.server", "7000"],
+ cwd=self.tmpdir,
+ )
+
+ # Wait for the TBA server and the scouting webserver to start up.
+ wait_for_server(7000)
+ wait_for_server(port)
+
+ def stop(self):
+ """Stops the services needed for testing the scouting app."""
+ servers = (self.webserver, self.fake_tba_api)
+ for server in servers:
+ server.terminate()
+ for server in servers:
+ server.wait()
+
+ try:
+ shutil.rmtree(self.tmpdir)
+ except FileNotFoundError:
+ pass
+
+
+def main(argv: List[str]):
+ parser = argparse.ArgumentParser()
+ parser.add_argument("--port", type=int, help="The port for the actual web server.")
+ args = parser.parse_args(argv[1:])
+
+ runner = Runner()
+ runner.start(args.port)
+
+ # Wait until we're asked to shut down via CTRL-C or SIGTERM.
+ signal.pause()
+
+ runner.stop()
+
+
+if __name__ == "__main__":
+ sys.exit(main(sys.argv))
diff --git a/scouting/webserver/requests/debug/cli/BUILD b/scouting/webserver/requests/debug/cli/BUILD
index 903f8c8..371f66e 100644
--- a/scouting/webserver/requests/debug/cli/BUILD
+++ b/scouting/webserver/requests/debug/cli/BUILD
@@ -30,7 +30,8 @@
],
data = [
":cli",
- "//scouting/scraping:test_data",
- "//scouting/webserver",
+ ],
+ deps = [
+ "//scouting/testing:scouting_test_servers",
],
)
diff --git a/scouting/webserver/requests/debug/cli/cli_test.py b/scouting/webserver/requests/debug/cli/cli_test.py
index 64d79a6..f4b82b4 100644
--- a/scouting/webserver/requests/debug/cli/cli_test.py
+++ b/scouting/webserver/requests/debug/cli/cli_test.py
@@ -7,11 +7,15 @@
import shutil
import socket
import subprocess
+import sys
import textwrap
import time
from typing import Any, Dict, List
import unittest
+import scouting.testing.scouting_test_servers
+
+
def write_json_request(content: Dict[str, Any]):
"""Writes a JSON file with the specified dict content."""
json_path = Path(os.environ["TEST_TMPDIR"]) / "test.json"
@@ -31,72 +35,15 @@
run_result.stderr.decode("utf-8"),
)
-def wait_for_server(port: int):
- """Waits for the server at the specified port to respond to TCP connections."""
- while True:
- try:
- connection = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
- connection.connect(("localhost", port))
- connection.close()
- break
- except ConnectionRefusedError:
- connection.close()
- time.sleep(0.01)
-
class TestDebugCli(unittest.TestCase):
def setUp(self):
- tmpdir = Path(os.environ["TEST_TMPDIR"]) / "temp"
- try:
- shutil.rmtree(tmpdir)
- except FileNotFoundError:
- pass
- os.mkdir(tmpdir)
-
- # Copy the test data into place so that the final API call can be
- # emulated.
- self.set_up_tba_api_dir(tmpdir, year=2016, event_code="nytr")
- self.set_up_tba_api_dir(tmpdir, year=2020, event_code="fake")
-
- # Create a fake TBA server to serve the static match list.
- self.fake_tba_api = subprocess.Popen(
- ["python3", "-m", "http.server", "7000"],
- cwd=tmpdir,
- )
-
- # Configure the scouting webserver to scrape data from our fake TBA
- # server.
- scouting_config = tmpdir / "scouting_config.json"
- scouting_config.write_text(json.dumps({
- "api_key": "dummy_key_that_is_not_actually_used_in_this_test",
- "base_url": "http://localhost:7000",
- }))
-
- # Run the scouting webserver.
- self.webserver = subprocess.Popen([
- "scouting/webserver/webserver_/webserver",
- "-port=8080",
- "-database=%s/database.db" % tmpdir,
- "-tba_config=%s/scouting_config.json" % tmpdir,
- ])
-
- # Wait for the servers to be reachable.
- wait_for_server(7000)
- wait_for_server(8080)
+ self.servers = scouting.testing.scouting_test_servers.Runner()
+ self.servers.start(8080)
def tearDown(self):
- self.fake_tba_api.terminate()
- self.webserver.terminate()
- self.fake_tba_api.wait()
- self.webserver.wait()
-
- def set_up_tba_api_dir(self, tmpdir, year, event_code):
- tba_api_dir = tmpdir / "api" / "v3" / "event" / f"{year}{event_code}"
- os.makedirs(tba_api_dir)
- (tba_api_dir / "matches").write_text(
- Path(f"scouting/scraping/test_data/{year}_{event_code}.json").read_text()
- )
+ self.servers.stop()
def refresh_match_list(self, year=2016, event_code="nytr"):
"""Triggers the webserver to fetch the match list."""
diff --git a/scouting/www/app.ts b/scouting/www/app.ts
index 285d306..79c1094 100644
--- a/scouting/www/app.ts
+++ b/scouting/www/app.ts
@@ -10,11 +10,26 @@
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;
}
switchTabTo(tab: Tab) {
- this.tab = tab;
+ let shouldSwitch = true;
+ if (tab === 'ImportMatchList') {
+ shouldSwitch = window.confirm('Leave data scouting page?');
+ }
+ if (shouldSwitch) {
+ this.tab = tab;
+ }
}
}
diff --git a/scouting/www/counter_button/BUILD b/scouting/www/counter_button/BUILD
new file mode 100644
index 0000000..1dbcdfc
--- /dev/null
+++ b/scouting/www/counter_button/BUILD
@@ -0,0 +1,21 @@
+load("@npm//@bazel/typescript:index.bzl", "ts_library")
+
+ts_library(
+ name = "counter_button",
+ srcs = [
+ "counter_button.component.ts",
+ "counter_button.module.ts",
+ ],
+ angular_assets = [
+ "counter_button.component.css",
+ "counter_button.ng.html",
+ ],
+ compiler = "//tools:tsc_wrapped_with_angular",
+ target_compatible_with = ["@platforms//cpu:x86_64"],
+ use_angular_plugin = True,
+ visibility = ["//visibility:public"],
+ deps = [
+ "@npm//@angular/common",
+ "@npm//@angular/core",
+ ],
+)
diff --git a/scouting/www/counter_button/counter_button.component.css b/scouting/www/counter_button/counter_button.component.css
new file mode 100644
index 0000000..39faea8
--- /dev/null
+++ b/scouting/www/counter_button/counter_button.component.css
@@ -0,0 +1,11 @@
+:host {
+ display: flex;
+ align-items: stretch;
+ flex-direction: column;
+ text-align: center;
+}
+
+* {
+ padding: 10px;
+}
+
diff --git a/scouting/www/counter_button/counter_button.component.ts b/scouting/www/counter_button/counter_button.component.ts
new file mode 100644
index 0000000..fa84dc6
--- /dev/null
+++ b/scouting/www/counter_button/counter_button.component.ts
@@ -0,0 +1,17 @@
+import {Component, Input, Output, EventEmitter} from '@angular/core';
+
+@Component({
+ selector: 'frc971-counter-button',
+ templateUrl: './counter_button.ng.html',
+ styleUrls: ['./counter_button.component.css'],
+})
+export class CounterButton {
+ @Input() value: number = 0;
+ @Output() valueChange = new EventEmitter<number>();
+
+ update(delta: number) {
+ this.value = Math.max(this.value + delta, 0);
+
+ this.valueChange.emit(this.value);
+ }
+}
diff --git a/scouting/www/counter_button/counter_button.module.ts b/scouting/www/counter_button/counter_button.module.ts
new file mode 100644
index 0000000..25bafcb
--- /dev/null
+++ b/scouting/www/counter_button/counter_button.module.ts
@@ -0,0 +1,10 @@
+import {NgModule} from '@angular/core';
+
+import {CounterButton} from './counter_button.component';
+
+@NgModule({
+ declarations: [CounterButton],
+ exports: [CounterButton],
+})
+export class CounterButtonModule {
+}
diff --git a/scouting/www/counter_button/counter_button.ng.html b/scouting/www/counter_button/counter_button.ng.html
new file mode 100644
index 0000000..4abf415
--- /dev/null
+++ b/scouting/www/counter_button/counter_button.ng.html
@@ -0,0 +1,4 @@
+<h4><ng-content></ng-content></h4>
+<button (click)="update(1)" class="btn btn-secondary btn-block">+</button>
+<h3>{{value}}</h3>
+<button (click)="update(-1)" class="btn btn-secondary btn-block">-</button>
diff --git a/scouting/www/entry/BUILD b/scouting/www/entry/BUILD
index 2c394de..17eed23 100644
--- a/scouting/www/entry/BUILD
+++ b/scouting/www/entry/BUILD
@@ -19,6 +19,7 @@
"//scouting/webserver/requests/messages:error_response_ts_fbs",
"//scouting/webserver/requests/messages:submit_data_scouting_response_ts_fbs",
"//scouting/webserver/requests/messages:submit_data_scouting_ts_fbs",
+ "//scouting/www/counter_button",
"@com_github_google_flatbuffers//ts:flatbuffers_ts",
"@npm//@angular/common",
"@npm//@angular/core",
diff --git a/scouting/www/entry/entry.component.css b/scouting/www/entry/entry.component.css
index 76a3c29..e6f81b3 100644
--- a/scouting/www/entry/entry.component.css
+++ b/scouting/www/entry/entry.component.css
@@ -2,13 +2,6 @@
padding: 10px;
}
-.center-column {
- display: flex;
- align-items: stretch;
- flex-direction: column;
- text-align: center;
-}
-
.buttons {
display: flex;
justify-content: space-between;
diff --git a/scouting/www/entry/entry.component.ts b/scouting/www/entry/entry.component.ts
index fb4a1b1..3db5f72 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',
@@ -27,8 +27,8 @@
teleUpperShotsMade: number = 0;
teleLowerShotsMade: number = 0;
teleShotsMissed: number = 0;
- defensePlayedOnScore: number = 3;
- defensePlayedScore: number = 3;
+ defensePlayedOnScore: number = 0;
+ defensePlayedScore: number = 0;
level: Level;
proper: boolean = false;
climbed: boolean = false;
@@ -38,6 +38,10 @@
this.proper = !this.proper;
}
+ setFailed() {
+ this.level = 'Failed';
+ }
+
setLow() {
this.level = 'Low';
}
@@ -54,22 +58,6 @@
this.level = 'Transversal';
}
- defensePlayedOnSlider(event) {
- this.defensePlayedOnScore = event.target.value;
- }
-
- defensePlayedSlider(event) {
- this.defensePlayedScore = event.target.value;
- }
-
- setClimbedTrue() {
- this.climbed = true;
- }
-
- setClimbedFalse() {
- this.climbed = false;
- }
-
nextSection() {
if (this.section === 'Team Selection') {
this.section = 'Auto';
@@ -100,30 +88,6 @@
}
}
- adjustAutoUpper(by: number) {
- this.autoUpperShotsMade = Math.max(0, this.autoUpperShotsMade + by);
- }
-
- adjustAutoLower(by: number) {
- this.autoLowerShotsMade = Math.max(0, this.autoLowerShotsMade + by);
- }
-
- adjustAutoMissed(by: number) {
- this.autoShotsMissed = Math.max(0, this.autoShotsMissed + by);
- }
-
- adjustTeleUpper(by: number) {
- this.teleUpperShotsMade = Math.max(0, this.teleUpperShotsMade + by);
- }
-
- adjustTeleLower(by: number) {
- this.teleLowerShotsMade = Math.max(0, this.teleLowerShotsMade + by);
- }
-
- adjustTeleMissed(by: number) {
- this.teleShotsMissed = Math.max(0, this.teleShotsMissed + by);
- }
-
async submitDataScouting() {
const builder = new flatbuffer_builder.Builder() as unknown as flatbuffers.Builder;
SubmitDataScouting.startSubmitDataScouting(builder);
diff --git a/scouting/www/entry/entry.module.ts b/scouting/www/entry/entry.module.ts
index 35ecd26..b4d81c0 100644
--- a/scouting/www/entry/entry.module.ts
+++ b/scouting/www/entry/entry.module.ts
@@ -1,12 +1,14 @@
import {NgModule} from '@angular/core';
import {CommonModule} from '@angular/common';
-import {EntryComponent} from './entry.component';
import {FormsModule} from '@angular/forms';
+import {CounterButtonModule} from '../counter_button/counter_button.module';
+import {EntryComponent} from './entry.component';
+
@NgModule({
declarations: [EntryComponent],
exports: [EntryComponent],
- imports: [CommonModule, FormsModule],
+ imports: [CommonModule, FormsModule, CounterButtonModule],
})
export class EntryModule {
}
diff --git a/scouting/www/entry/entry.ng.html b/scouting/www/entry/entry.ng.html
index b5783d2..c5cf233 100644
--- a/scouting/www/entry/entry.ng.html
+++ b/scouting/www/entry/entry.ng.html
@@ -12,8 +12,10 @@
<label for="team_number">Team Number</label>
<input [(ngModel)]="teamNumber" type="number" id="team_number" min="1" max="9999">
</div>
- <div class="text-right">
- <button class="btn btn-primary" (click)="nextSection()">Next</button>
+ <div class="buttons">
+ <!-- hack to right align the next button -->
+ <div></div>
+ <button class="btn btn-primary" (click)="nextSection()">Next</button>
</div>
</div>
@@ -23,10 +25,12 @@
<h4>Image</h4>
<form>
<!--Choice for each ball location-->
- <input type="radio" name="balls" value="1" id="ball-1"><label for="ball-1">Ball 1</label>
- <input type="radio" name="balls" value="2" id="ball-2"><label for="ball-2">Ball 2</label><br>
- <input type="radio" name="balls" value="3" id="ball-3"><label for="ball-3">Ball 3</label>
- <input type="radio" name="balls" value="4" id="ball-4"><label for="ball-4">Ball 4</label>
+ <input type="checkbox" name="balls" value="1" id="ball-1"><label for="ball-1">Ball 1</label>
+ <input type="checkbox" name="balls" value="2" id="ball-2"><label for="ball-2">Ball 2</label><br>
+ <input type="checkbox" name="balls" value="3" id="ball-3"><label for="ball-3">Ball 3</label>
+ <input type="checkbox" name="balls" value="4" id="ball-4"><label for="ball-4">Ball 4</label><br>
+ <input type="checkbox" name="balls" value="5" id="ball-5"><label for="ball-5">Ball 5</label>
+ <input type="checkbox" name="balls" value="6" id="ball-6"><label for="ball-6">Ball 6</label>
</form>
</div>
<div class="row">
@@ -44,56 +48,21 @@
</form>
</div>
<div class="row justify-content-center">
- <span class="col-4 center-column">
- <h4>Upper</h4>
- <button (click)="adjustAutoUpper(1)" class="btn btn-secondary btn-block">+</button>
- <h3>{{autoUpperShotsMade}}</h3>
- <button (click)="adjustAutoUpper(-1)" class="btn btn-secondary btn-block">-</button>
- </span>
-
- <span class="col-4 center-column">
- <h4>Lower</h4>
- <button (click)="adjustAutoLower(1)" class="btn btn-secondary btn-block">+</button>
- <h3>{{autoLowerShotsMade}}</h3>
- <button (click)="adjustAutoLower(-1)" class="btn btn-secondary btn-block">-</button>
- </span>
-
- <span class="col-4 center-column">
- <h4>Missed</h4>
- <button (click)="adjustAutoMissed(1)" class="btn btn-secondary btn-block">+</button>
- <h3>{{autoShotsMissed}}</h3>
- <button (click)="adjustAutoMissed(-1)" class="btn btn-secondary btn-block">-</button>
- </span>
+ <frc971-counter-button class="col-4" [(value)]="autoUpperShotsMade">Upper</frc971-counter-button>
+ <frc971-counter-button class="col-4" [(value)]="autoLowerShotsMade">Lower</frc971-counter-button>
+ <frc971-counter-button class="col-4" [(value)]="autoShotsMissed">Missed</frc971-counter-button>
</div>
<div class="buttons">
- <!-- hack to right align the next button -->
- <div></div>
+ <button class="btn btn-primary" (click)="prevSection()">Back</button>
<button class="btn btn-primary" (click)="nextSection()">Next</button>
</div>
</div>
<div *ngSwitchCase="'TeleOp'" id="teleop" class="container-fluid">
<div class="row justify-content-center">
- <span class="col-4 center-column">
- <h4>Upper</h4>
- <button (click)="adjustTeleUpper(1)" class="btn btn-secondary btn-block">+</button>
- <h3>{{teleUpperShotsMade}}</h3>
- <button (click)="adjustTeleUpper(-1)" class="btn btn-secondary btn-block">-</button>
- </span>
-
- <span class="col-4 center-column">
- <h4>Lower</h4>
- <button (click)="adjustTeleLower(1)" class="btn btn-secondary btn-block">+</button>
- <h3>{{teleLowerShotsMade}}</h3>
- <button (click)="adjustTeleLower(-1)" class="btn btn-secondary btn-block">-</button>
- </span>
-
- <span class="col-4 center-column">
- <h4>Missed</h4>
- <button (click)="adjustTeleMissed(1)" class="btn btn-secondary btn-block">+</button>
- <h3>{{teleShotsMissed}}</h3>
- <button (click)="adjustTeleMissed(-1)" class="btn btn-secondary btn-block">-</button>
- </span>
+ <frc971-counter-button class="col-4" [(value)]="teleUpperShotsMade">Upper</frc971-counter-button>
+ <frc971-counter-button class="col-4" [(value)]="teleLowerShotsMade">Lower</frc971-counter-button>
+ <frc971-counter-button class="col-4" [(value)]="teleShotsMissed">Missed</frc971-counter-button>
</div>
<div class="buttons">
<button class="btn btn-primary" (click)="prevSection()">Back</button>
@@ -104,18 +73,28 @@
<div *ngSwitchCase="'Climb'" id="climb" class="container-fluid">
<div class="row">
<form>
- <input (click)="setClimbedFalse()" type="radio" name="climbing" id="continue"><label for="continue">Kept Shooting</label><br>
- <input (click)="setClimbedTrue()" type="radio" name="climbing" id="climbed"><label for="climbed">Attempted to Climb</label><br>
+ <input [(ngModel)]="climbed" type="radio" name="climbing" id="continue" [value]="false">
+ <label for="continue">Kept Shooting</label><br>
+ <input [(ngModel)]="climbed" type="radio" name="climbing" id="climbed" [value]="true">
+ <label for="climbed">Attempted to Climb</label><br>
</form>
</div>
<div *ngIf="climbed">
<h4>Bar Made</h4>
<form>
- <input (click)="setLow()" type="radio" name="level" id="low"><label for="low">Low</label><br>
- <input (click)="setMedium()" type="radio" name="level" id="medium"><label for="medium">Medium</label><br>
- <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 [ngModel]="level" (click)="setLow()" type="radio" name="level" id="low" value="Low">
+ <label for="low">Low</label><br>
+ <input [ngModel]="level" (click)="setMedium()" type="radio" name="level" id="medium" value="Medium">
+ <label for="medium">Medium</label><br>
+ <input [ngModel]="level" (click)="setHigh()" type="radio" name="level" id="high" value="High">
+ <label for="high">High</label><br>
+ <input [ngModel]="level" (click)="setTransversal()" type="radio" name="level" id="transversal" value="Transversal">
+ <label for="transversal">Transversal</label><br>
+ <input [ngModel]="level" (click)="setFailed()" type="radio" name="level" id="failed" value="Failed">
+ <label for="failed">Failed</label><br>
+
+ <input (click)="toggleProper()" type="checkbox" id="proper">
+ <label for="proper">~10 seconds to attempt next level?</label>
</form>
</div>
<div class="row">
@@ -137,7 +116,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" [(ngModel)]="defensePlayedOnScore">
</div>
<div class="col">
@@ -156,7 +135,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" [(ngModel)]="defensePlayedScore">
</div>
<div class="col">
diff --git a/scouting/www/import_match_list/import_match_list.component.ts b/scouting/www/import_match_list/import_match_list.component.ts
index b2b15e5..7ad4ab6 100644
--- a/scouting/www/import_match_list/import_match_list.component.ts
+++ b/scouting/www/import_match_list/import_match_list.component.ts
@@ -10,44 +10,48 @@
import ErrorResponse = error_response.scouting.webserver.requests.ErrorResponse;
@Component({
- selector: 'app-import-match-list',
- templateUrl: './import_match_list.ng.html',
- styleUrls: ['../common.css', './import_match_list.component.css']
+ selector: 'app-import-match-list',
+ templateUrl: './import_match_list.ng.html',
+ styleUrls: ['../common.css', './import_match_list.component.css']
})
export class ImportMatchListComponent {
- year: number = new Date().getFullYear();
- eventCode: string = '';
- progressMessage: string = '';
- errorMessage: string = '';
+ year: number = new Date().getFullYear();
+ eventCode: string = '';
+ progressMessage: string = '';
+ errorMessage: string = '';
- async importMatchList() {
- this.errorMessage = '';
-
- const builder = new flatbuffer_builder.Builder() as unknown as flatbuffers.Builder;
- const eventCode = builder.createString(this.eventCode);
- RefreshMatchList.startRefreshMatchList(builder);
- RefreshMatchList.addYear(builder, this.year);
- RefreshMatchList.addEventCode(builder, eventCode);
- builder.finish(RefreshMatchList.endRefreshMatchList(builder));
-
- this.progressMessage = 'Importing match list. Please be patient.';
-
- const buffer = builder.asUint8Array();
- const res = await fetch(
- '/requests/refresh_match_list', {method: 'POST', body: buffer});
-
- if (res.ok) {
- // We successfully submitted the data.
- this.progressMessage = 'Successfully imported match list.';
- } else {
- this.progressMessage = '';
- const resBuffer = await res.arrayBuffer();
- const fbBuffer = new ByteBuffer(new Uint8Array(resBuffer));
- const parsedResponse = ErrorResponse.getRootAsErrorResponse(
- fbBuffer as unknown as flatbuffers.ByteBuffer);
-
- const errorMessage = parsedResponse.errorMessage();
- this.errorMessage = `Received ${res.status} ${res.statusText}: "${errorMessage}"`;
- }
+ async importMatchList() {
+ if (!window.confirm('Actually import new matches?')) {
+ return;
}
+
+ this.errorMessage = '';
+
+ const builder = new flatbuffer_builder.Builder() as unknown as flatbuffers.Builder;
+ const eventCode = builder.createString(this.eventCode);
+ RefreshMatchList.startRefreshMatchList(builder);
+ RefreshMatchList.addYear(builder, this.year);
+ RefreshMatchList.addEventCode(builder, eventCode);
+ builder.finish(RefreshMatchList.endRefreshMatchList(builder));
+
+ this.progressMessage = 'Importing match list. Please be patient.';
+
+ const buffer = builder.asUint8Array();
+ const res = await fetch(
+ '/requests/refresh_match_list', {method: 'POST', body: buffer});
+
+ if (res.ok) {
+ // We successfully submitted the data.
+ this.progressMessage = 'Successfully imported match list.';
+ } else {
+ this.progressMessage = '';
+ const resBuffer = await res.arrayBuffer();
+ const fbBuffer = new ByteBuffer(new Uint8Array(resBuffer));
+ const parsedResponse = ErrorResponse.getRootAsErrorResponse(
+ fbBuffer as unknown as flatbuffers.ByteBuffer);
+
+ const errorMessage = parsedResponse.errorMessage();
+ this.errorMessage = `Received ${res.status} ${res.statusText}: "${errorMessage}"`;
+ }
+ }
}
diff --git a/third_party/rules_pkg/0001-Fix-tree-artifacts.patch b/third_party/rules_pkg/0001-Fix-tree-artifacts.patch
new file mode 100644
index 0000000..567aba7
--- /dev/null
+++ b/third_party/rules_pkg/0001-Fix-tree-artifacts.patch
@@ -0,0 +1,28 @@
+From d654cc64ae71366ea82ac492106e9b2c8fa532d5 Mon Sep 17 00:00:00 2001
+From: Philipp Schrader <philipp.schrader@gmail.com>
+Date: Thu, 10 Mar 2022 23:25:21 -0800
+Subject: [PATCH] Fix tree artifacts
+
+For some reason the upstream code strips the directory names from the
+`babel()` rule that we use. This patch makes it so the directory is
+not stripped. This makes runfiles layout in the tarball match the
+runfiles layout in `bazel-bin`.
+---
+ pkg/pkg.bzl | 4 ++--
+ 1 file changed, 2 insertions(+), 2 deletions(-)
+
+diff --git a/pkg/pkg.bzl b/pkg/pkg.bzl
+index d7adbbc..a241b26 100644
+--- a/pkg/pkg.bzl
++++ b/pkg/pkg.bzl
+@@ -157,8 +157,8 @@ def _pkg_tar_impl(ctx):
+ # Tree artifacts need a name, but the name is never really
+ # the important part. The likely behavior people want is
+ # just the content, so we strip the directory name.
+- dest = "/".join(d_path.split("/")[0:-1])
+- add_tree_artifact(content_map, dest, f, src.label)
++ #dest = "/".join(d_path.split("/")[0:-1])
++ add_tree_artifact(content_map, d_path, f, src.label)
+ else:
+ # Note: This extra remap is the bottleneck preventing this
+ # large block from being a utility method as shown below.
diff --git a/y2020/vision/calibration.cc b/y2020/vision/calibration.cc
index 3f08be9..b5d1b32 100644
--- a/y2020/vision/calibration.cc
+++ b/y2020/vision/calibration.cc
@@ -72,6 +72,12 @@
cv::imshow("Display undist", undistorted_rgb_image);
}
+ int keystroke = cv::waitKey(1);
+
+ // If we haven't got a valid pose estimate, don't use these points
+ if (!valid) {
+ return;
+ }
// Calibration calculates rotation and translation delta from last image
// stored to automatically capture next image
@@ -89,6 +95,10 @@
double t_norm = delta_t.norm();
bool store_image = false;
+ double percent_motion =
+ std::max<double>(r_norm / kDeltaRThreshold, t_norm / kDeltaTThreshold);
+ LOG(INFO) << all_charuco_ids_.size() << ": Moved " << percent_motion
+ << "% of what's needed";
// Verify that camera has moved enough from last stored image
if (r_norm > kDeltaRThreshold || t_norm > kDeltaTThreshold) {
// frame_ refers to deltas between current and last captured image
@@ -106,11 +116,14 @@
// Make sure camera has stopped moving before storing image
store_image =
frame_r_norm < kFrameDeltaRLimit && frame_t_norm < kFrameDeltaTLimit;
+ double percent_stop = std::max<double>(frame_r_norm / kFrameDeltaRLimit,
+ frame_t_norm / kFrameDeltaTLimit);
+ LOG(INFO) << all_charuco_ids_.size() << ": Moved enough ("
+ << percent_motion << "%); Need to stop (last motion was "
+ << percent_stop << "%";
}
-
prev_image_H_camera_board_ = H_camera_board_;
- int keystroke = cv::waitKey(1);
if (store_image) {
if (valid) {
prev_H_camera_board_ = H_camera_board_;
@@ -126,20 +139,16 @@
LOG(INFO) << "Triggered by translation delta = " << t_norm << " > "
<< kDeltaTThreshold;
}
-
- LOG(INFO) << "Image count " << all_charuco_corners_.size();
}
- if (all_charuco_ids_.size() >= 50) {
- LOG(INFO) << "Got enough images to calibrate";
- event_loop_->Exit();
- }
} else if ((keystroke & 0xFF) == static_cast<int>('q')) {
event_loop_->Exit();
}
}
void MaybeCalibrate() {
+ // TODO: This number should depend on coarse vs. fine pattern
+ // Maybe just on total # of ids found, not just images
if (all_charuco_ids_.size() >= 50) {
LOG(INFO) << "Beginning calibration on " << all_charuco_ids_.size()
<< " images";
diff --git a/y2022/BUILD b/y2022/BUILD
index 488026e..f2bdb8b 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",
@@ -91,6 +92,7 @@
"//aos/network:timestamp_fbs",
"//aos/network:remote_message_fbs",
"//frc971/vision:vision_fbs",
+ "//y2022/localizer:localizer_output_fbs",
"//y2022/vision:calibration_fbs",
"//y2022/vision:target_estimate_fbs",
],
@@ -201,6 +203,7 @@
"//frc971:constants",
"//frc971/control_loops:pose",
"//frc971/control_loops:static_zeroing_single_dof_profiled_subsystem",
+ "//frc971/shooter_interpolation:interpolation",
"//y2022/control_loops/drivetrain:polydrivetrain_plants",
"//y2022/control_loops/superstructure/catapult:catapult_plants",
"//y2022/control_loops/superstructure/climber:climber_plants",
diff --git a/y2022/actors/splines/spline_4.json b/y2022/actors/splines/spline_4.json
index 5c93fa0..d9e872f 100644
--- a/y2022/actors/splines/spline_4.json
+++ b/y2022/actors/splines/spline_4.json
@@ -1 +1 @@
-{"spline_count": 1, "spline_x": [6.720451116452686, 5.982811423475535, 5.227386296005803, 4.608975667524007, 3.06982032552487, 2.5343458588073577], "spline_y": [2.847133953965564, 2.5520780767747038, 2.2949124753895456, 1.6352744716756296, 1.6627593884970424, 1.9804073147174122], "constraints": [{"constraint_type": "LONGITUDINAL_ACCELERATION", "value": 3}, {"constraint_type": "LATERAL_ACCELERATION", "value": 2}, {"constraint_type": "VOLTAGE", "value": 10}]}
\ No newline at end of file
+{"spline_count": 1, "spline_x": [6.720451116452686, 5.982811423475535, 5.766318367346939, 4.702628571428573, 3.5269714285714286, 3.313095574784694], "spline_y": [2.847133953965564, 2.5520780767747038, 2.435289795918367, 1.4275836734693876, 1.9874204081632652, 2.148963926647223], "constraints": [{"constraint_type": "LONGITUDINAL_ACCELERATION", "value": 3}, {"constraint_type": "LATERAL_ACCELERATION", "value": 2}, {"constraint_type": "VOLTAGE", "value": 10}]}
\ No newline at end of file
diff --git a/y2022/actors/splines/spline_5.json b/y2022/actors/splines/spline_5.json
index 9771c34..a37911b 100644
--- a/y2022/actors/splines/spline_5.json
+++ b/y2022/actors/splines/spline_5.json
@@ -1 +1 @@
-{"spline_count": 1, "spline_x": [2.5343458588073577, 1.9811160890744945, 1.3910043346927745, 0.6718056340400533, -0.48997688239895876, -0.821914744238676], "spline_y": [1.9804073147174122, 2.367668153530416, 2.75492899234342, 3.381922731373998, 3.5663326546182854, 3.5663326546182854], "constraints": [{"constraint_type": "LONGITUDINAL_ACCELERATION", "value": 3}, {"constraint_type": "LATERAL_ACCELERATION", "value": 2}, {"constraint_type": "VOLTAGE", "value": 10}]}
+{"spline_count": 1, "spline_x": [3.313095574784694, 2.4072979591836745, 1.5115591836734694, 0.6718056340400533, -0.48997688239895876, -0.821914744238676], "spline_y": [2.148963926647223, 2.911151020408163, 3.638938775510204, 3.381922731373998, 3.5663326546182854, 3.5663326546182854], "constraints": [{"constraint_type": "LONGITUDINAL_ACCELERATION", "value": 3}, {"constraint_type": "LATERAL_ACCELERATION", "value": 2}, {"constraint_type": "VOLTAGE", "value": 10}]}
\ No newline at end of file
diff --git a/y2022/constants.cc b/y2022/constants.cc
index a380a4e..0b4a591 100644
--- a/y2022/constants.cc
+++ b/y2022/constants.cc
@@ -68,7 +68,7 @@
auto *const turret_params = &turret->subsystem_params;
turret_params->zeroing_voltage = 4.0;
- turret_params->operating_voltage = 4.0;
+ turret_params->operating_voltage = 12.0;
turret_params->zeroing_profile_params = {0.5, 2.0};
turret_params->default_profile_params = {15.0, 40.0};
turret_params->range = Values::kTurretRange();
@@ -88,7 +88,7 @@
climber->subsystem_params.zeroing_voltage = 3.0;
climber->subsystem_params.operating_voltage = 12.0;
climber->subsystem_params.zeroing_profile_params = {0.5, 0.1};
- climber->subsystem_params.default_profile_params = {6.0, 1.0};
+ climber->subsystem_params.default_profile_params = {5.0, 1.0};
climber->subsystem_params.range = Values::kClimberRange();
climber->subsystem_params.make_integral_loop =
control_loops::superstructure::climber::MakeIntegralClimberLoop;
@@ -130,9 +130,20 @@
catapult_params->zeroing_constants.moving_buffer_size = 20;
catapult_params->zeroing_constants.allowable_encoder_error = 0.9;
+ // Interpolation table for comp and practice robots
+ r.shot_interpolation_table = InterpolationTable<Values::ShotParams>({
+ {2, {0.08, 8.0}},
+ {5, {0.6, 10.0}},
+ });
+
switch (team) {
// A set of constants for tests.
case 1:
+ r.shot_interpolation_table = InterpolationTable<Values::ShotParams>({
+ {2, {0.08, 8.0}},
+ {5, {0.6, 10.0}},
+ });
+
climber->potentiometer_offset = 0.0;
intake_front->potentiometer_offset = 0.0;
intake_front->subsystem_params.zeroing_constants
@@ -151,7 +162,7 @@
break;
case kCompTeamNumber:
- climber->potentiometer_offset = 0.0;
+ climber->potentiometer_offset = -0.0463847608752;
intake_front->potentiometer_offset = 2.79628370453323;
intake_front->subsystem_params.zeroing_constants
@@ -161,13 +172,13 @@
intake_back->subsystem_params.zeroing_constants
.measured_absolute_position = 0.280099007470002;
- turret->potentiometer_offset =
- -9.99970387166721 + 0.06415943 + 0.073290115367682;
+ turret->potentiometer_offset = -9.99970387166721 + 0.06415943 +
+ 0.073290115367682 - 0.0634440443622909;
turret->subsystem_params.zeroing_constants.measured_absolute_position =
- 0.511895084051468;
+ 0.568649928102931;
flipper_arm_left->potentiometer_offset = -6.4;
- flipper_arm_right->potentiometer_offset = 5.66;
+ flipper_arm_right->potentiometer_offset = 5.56;
catapult_params->zeroing_constants.measured_absolute_position =
1.71723370408082;
@@ -193,6 +204,11 @@
break;
case kCodingRobotTeamNumber:
+ r.shot_interpolation_table = InterpolationTable<Values::ShotParams>({
+ {2, {0.08, 8.0}},
+ {5, {0.6, 10.0}},
+ });
+
climber->potentiometer_offset = 0.0;
intake_front->potentiometer_offset = 0.0;
intake_front->subsystem_params.zeroing_constants
diff --git a/y2022/constants.h b/y2022/constants.h
index c824076..2c0c5a1 100644
--- a/y2022/constants.h
+++ b/y2022/constants.h
@@ -8,12 +8,15 @@
#include "frc971/constants.h"
#include "frc971/control_loops/pose.h"
#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h"
+#include "frc971/shooter_interpolation/interpolation.h"
#include "y2022/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
#include "y2022/control_loops/superstructure/catapult/catapult_plant.h"
#include "y2022/control_loops/superstructure/climber/climber_plant.h"
#include "y2022/control_loops/superstructure/intake/intake_plant.h"
#include "y2022/control_loops/superstructure/turret/turret_plant.h"
+using ::frc971::shooter_interpolation::InterpolationTable;
+
namespace y2022 {
namespace constants {
@@ -42,7 +45,7 @@
// Climber
static constexpr ::frc971::constants::Range kClimberRange() {
return ::frc971::constants::Range{
- .lower_hard = -0.01, .upper_hard = 0.6, .lower = 0.0, .upper = 0.5};
+ .lower_hard = -0.01, .upper_hard = 0.55, .lower = 0.005, .upper = 0.48};
}
static constexpr double kClimberPotMetersPerRevolution() {
return 22 * 0.25 * 0.0254;
@@ -113,7 +116,7 @@
.lower_hard = -6.0, // Back Hard
.upper_hard = 4.0, // Front Hard
.lower = -5.0, // Back Soft
- .upper = 3.7 // Front Soft
+ .upper = 3.3 // Front Soft
};
}
@@ -168,7 +171,7 @@
.lower_hard = -0.01, .upper_hard = 0.4, .lower = 0.0, .upper = 0.5};
}
// Position of the flippers when they are open
- static constexpr double kFlipperOpenPosition() { return 0.15; }
+ static constexpr double kFlipperOpenPosition() { return 0.20; }
// If the flippers were open but now moved back, reseat the ball if they go
// below this position
static constexpr double kReseatFlipperPosition() { return 0.1; }
@@ -205,6 +208,23 @@
// TODO(milind): set this
static constexpr double kImuHeight() { return 0.0; }
+
+ struct ShotParams {
+ // Measured in radians
+ double shot_angle;
+ // Muzzle velocity (m/s) of the ball as it is released from the catapult.
+ double shot_velocity;
+
+ static ShotParams BlendY(double coefficient, ShotParams a1, ShotParams a2) {
+ using ::frc971::shooter_interpolation::Blend;
+ return ShotParams{
+ Blend(coefficient, a1.shot_angle, a2.shot_angle),
+ Blend(coefficient, a1.shot_velocity, a2.shot_velocity),
+ };
+ }
+ };
+
+ InterpolationTable<ShotParams> shot_interpolation_table;
};
// Creates and returns a Values instance for the constants.
diff --git a/y2022/control_loops/drivetrain/localizer.cc b/y2022/control_loops/drivetrain/localizer.cc
index 3d02312..0735534 100644
--- a/y2022/control_loops/drivetrain/localizer.cc
+++ b/y2022/control_loops/drivetrain/localizer.cc
@@ -71,8 +71,15 @@
monotonic_offset);
// TODO: Finish implementing simple x/y/theta updater with state_at_capture.
// TODO: Implement turret/camera processing logic on pi side.
- const std::optional<State> state_at_capture =
+ std::optional<State> state_at_capture =
ekf_.LastStateBeforeTime(capture_time);
+ if (!state_at_capture.has_value()) {
+ state_at_capture = ekf_.OldestState();
+ if (!state_at_capture.has_value()) {
+ return;
+ }
+ }
+
Eigen::Matrix<float, HybridEkf::kNOutputs, HybridEkf::kNStates> H;
H.setZero();
H(0, StateIdx::kX) = 1;
diff --git a/y2022/control_loops/python/catapult.py b/y2022/control_loops/python/catapult.py
index ad0e25a..2d0588a 100755
--- a/y2022/control_loops/python/catapult.py
+++ b/y2022/control_loops/python/catapult.py
@@ -46,7 +46,7 @@
kCatapultWithBall = catapult_lib.CatapultParams(
name='Catapult',
- motor=AddResistance(control_loop.NMotor(control_loop.Falcon(), 2), 0.03),
+ motor=AddResistance(control_loop.NMotor(control_loop.Falcon(), 2), 0.01),
G=G,
J=J,
radius=lever,
diff --git a/y2022/control_loops/superstructure/BUILD b/y2022/control_loops/superstructure/BUILD
index e7b6559..4739cd1 100644
--- a/y2022/control_loops/superstructure/BUILD
+++ b/y2022/control_loops/superstructure/BUILD
@@ -82,6 +82,7 @@
":superstructure_output_fbs",
":superstructure_position_fbs",
":superstructure_status_fbs",
+ "//aos:flatbuffer_merge",
"//aos/events:event_loop",
"//frc971/control_loops:control_loop",
"//frc971/control_loops/drivetrain:drivetrain_status_fbs",
diff --git a/y2022/control_loops/superstructure/catapult/catapult.cc b/y2022/control_loops/superstructure/catapult/catapult.cc
index b99f59b..8b5c7eb 100644
--- a/y2022/control_loops/superstructure/catapult/catapult.cc
+++ b/y2022/control_loops/superstructure/catapult/catapult.cc
@@ -312,25 +312,28 @@
const flatbuffers::Offset<
frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus>
-Catapult::Iterate(const Goal *unsafe_goal, const Position *position,
- double *catapult_voltage, bool fire,
+Catapult::Iterate(const CatapultGoal *catapult_goal, const Position *position,
+ double battery_voltage, double *catapult_voltage, bool fire,
flatbuffers::FlatBufferBuilder *fbb) {
const frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal
- *catapult_goal = unsafe_goal != nullptr && unsafe_goal->has_catapult()
- ? (unsafe_goal->catapult()->return_position())
- : nullptr;
+ *return_goal =
+ catapult_goal != nullptr && catapult_goal->has_return_position()
+ ? catapult_goal->return_position()
+ : nullptr;
const bool catapult_disabled = catapult_.Correct(
- catapult_goal, position->catapult(), catapult_voltage == nullptr);
+ return_goal, position->catapult(), catapult_voltage == nullptr);
if (catapult_disabled) {
catapult_state_ = CatapultState::PROFILE;
- } else if (catapult_.running() && unsafe_goal &&
- unsafe_goal->has_catapult() && fire && !last_firing_) {
+ } else if (catapult_.running() && catapult_goal != nullptr && fire &&
+ !last_firing_) {
catapult_state_ = CatapultState::FIRING;
+ latched_shot_position = catapult_goal->shot_position();
+ latched_shot_velocity = catapult_goal->shot_velocity();
}
- if (catapult_.running() && unsafe_goal && unsafe_goal->has_catapult()) {
+ if (catapult_.running()) {
last_firing_ = fire;
}
@@ -354,8 +357,7 @@
catapult_mpc_.SetState(
next_X.block<2, 1>(0, 0),
- Eigen::Vector2d(unsafe_goal->catapult()->shot_position(),
- unsafe_goal->catapult()->shot_velocity()));
+ Eigen::Vector2d(latched_shot_position, latched_shot_velocity));
const bool solved = catapult_mpc_.Solve();
@@ -373,25 +375,33 @@
} else {
// TODO(austin): Voltage error?
CHECK_NOTNULL(catapult_voltage);
- *catapult_voltage =
- std::max(0.0, std::min(12.0, *solution - 0.0 * next_X(2, 0)));
+ *catapult_voltage = std::max(
+ 0.0, std::min(12.0, (*solution - 0.0 * next_X(2, 0)) * 12.0 /
+ std::max(battery_voltage, 8.0)));
use_profile_ = false;
}
} else {
- if (unsafe_goal && unsafe_goal->has_catapult() && !fire) {
+ if (!fire) {
// Eh, didn't manage to solve before it was time to fire. Give up.
catapult_state_ = CatapultState::PROFILE;
}
}
- if (!use_profile_ || catapult_state_ == CatapultState::RESETTING) {
+ if (!use_profile_) {
catapult_.ForceGoal(catapult_.estimated_position(),
catapult_.estimated_velocity());
}
- } break;
+ }
+ if (catapult_state_ != CatapultState::RESETTING) {
+ break;
+ } else {
+ [[fallthrough]];
+ }
case CatapultState::RESETTING:
- if (catapult_.controller().R(1, 0) > 0.0) {
+ if (catapult_.controller().R(1, 0) > 7.0) {
+ catapult_.AdjustProfile(7.0, 2000.0);
+ } else if (catapult_.controller().R(1, 0) > 0.0) {
catapult_.AdjustProfile(7.0, 1000.0);
} else {
catapult_state_ = CatapultState::PROFILE;
@@ -415,8 +425,9 @@
}
}
- catapult_.UpdateObserver(catapult_voltage != nullptr ? *catapult_voltage
- : 0.0);
+ catapult_.UpdateObserver(catapult_voltage != nullptr
+ ? (*catapult_voltage * battery_voltage / 12.0)
+ : 0.0);
return catapult_.MakeStatus(fbb);
}
diff --git a/y2022/control_loops/superstructure/catapult/catapult.h b/y2022/control_loops/superstructure/catapult/catapult.h
index ccd4b06..6a2c834 100644
--- a/y2022/control_loops/superstructure/catapult/catapult.h
+++ b/y2022/control_loops/superstructure/catapult/catapult.h
@@ -210,14 +210,11 @@
// shooting or not. Returns the status.
const flatbuffers::Offset<
frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus>
- Iterate(const Goal *unsafe_goal, const Position *position,
- double *catapult_voltage, bool fire,
+ Iterate(const CatapultGoal *unsafe_goal, const Position *position,
+ double battery_voltage, double *catapult_voltage, bool fire,
flatbuffers::FlatBufferBuilder *fbb);
private:
- // TODO(austin): Prototype is just an encoder. Catapult has both an encoder
- // and pot. Switch back once we have a catapult.
- // PotAndAbsoluteEncoderSubsystem catapult_;
PotAndAbsoluteEncoderSubsystem catapult_;
catapult::CatapultController catapult_mpc_;
@@ -226,6 +223,9 @@
CatapultState catapult_state_ = CatapultState::PROFILE;
+ double latched_shot_position = 0.0;
+ double latched_shot_velocity = 0.0;
+
bool last_firing_ = false;
bool use_profile_ = true;
diff --git a/y2022/control_loops/superstructure/collision_avoidance.h b/y2022/control_loops/superstructure/collision_avoidance.h
index 3cd5344..04b8e8a 100644
--- a/y2022/control_loops/superstructure/collision_avoidance.h
+++ b/y2022/control_loops/superstructure/collision_avoidance.h
@@ -48,7 +48,7 @@
// TODO(henry): put actual constants here.
// Reference angles between which the turret will be careful
- static constexpr double kCollisionZoneTurret = M_PI * 5.0 / 18.0;
+ static constexpr double kCollisionZoneTurret = M_PI * 7.0 / 18.0;
// For the turret, 0 rad is pointing straight forwards
static constexpr double kMinCollisionZoneFrontTurret =
@@ -61,7 +61,7 @@
static constexpr double kMaxCollisionZoneBackTurret = kCollisionZoneTurret;
// Maximum position of the intake to avoid collisions
- static constexpr double kCollisionZoneIntake = 1.4;
+ static constexpr double kCollisionZoneIntake = 1.33;
// Tolerances for the subsystems
static constexpr double kEpsTurret = 0.05;
diff --git a/y2022/control_loops/superstructure/superstructure.cc b/y2022/control_loops/superstructure/superstructure.cc
index e45917a..1168b88 100644
--- a/y2022/control_loops/superstructure/superstructure.cc
+++ b/y2022/control_loops/superstructure/superstructure.cc
@@ -1,6 +1,8 @@
#include "y2022/control_loops/superstructure/superstructure.h"
#include "aos/events/event_loop.h"
+#include "aos/flatbuffer_merge.h"
+#include "frc971/zeroing/wrap.h"
#include "y2022/control_loops/superstructure/collision_avoidance.h"
namespace y2022 {
@@ -47,7 +49,8 @@
aos::FlatbufferFixedAllocatorArray<
frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal, 64>
- turret_goal_buffer;
+ turret_loading_goal_buffer;
+ aos::FlatbufferFixedAllocatorArray<CatapultGoal, 64> catapult_goal_buffer;
const aos::monotonic_clock::time_point timestamp =
event_loop()->context().monotonic_event_time;
@@ -64,11 +67,13 @@
const frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal
*turret_goal = nullptr;
+ 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 flipper_arms_voltage = 0.0;
+ bool have_active_intake_request = false;
if (unsafe_goal != nullptr) {
roller_speed_compensated_front =
@@ -84,6 +89,35 @@
turret_goal =
unsafe_goal->auto_aim() ? auto_aim_goal : unsafe_goal->turret();
+
+ catapult_goal = unsafe_goal->catapult();
+
+ constants::Values::ShotParams shot_params;
+ const double distance_to_goal = aimer_.DistanceToGoal();
+ if (unsafe_goal->auto_aim() && values_->shot_interpolation_table.GetInRange(
+ distance_to_goal, &shot_params)) {
+ std::optional<flatbuffers::Offset<
+ frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal>>
+ return_position_offset;
+ if (unsafe_goal != nullptr && unsafe_goal->has_catapult() &&
+ unsafe_goal->catapult()->has_return_position()) {
+ return_position_offset = {
+ aos::CopyFlatBuffer(unsafe_goal->catapult()->return_position(),
+ catapult_goal_buffer.fbb())};
+ }
+ CatapultGoal::Builder builder(*catapult_goal_buffer.fbb());
+ builder.add_shot_position(shot_params.shot_angle);
+ builder.add_shot_velocity(shot_params.shot_velocity);
+ if (return_position_offset.has_value()) {
+ builder.add_return_position(return_position_offset.value());
+ }
+ catapult_goal_buffer.Finish(builder.Finish());
+ catapult_goal = &catapult_goal_buffer.message();
+ }
+
+ if (unsafe_goal->has_turret_intake()) {
+ have_active_intake_request = true;
+ }
}
// Superstructure state machine:
@@ -129,65 +163,94 @@
}
}
- const bool is_spitting = ((intake_state_ == IntakeState::INTAKE_FRONT_BALL &&
- transfer_roller_speed_front < 0) ||
- (intake_state_ == IntakeState::INTAKE_BACK_BALL &&
- transfer_roller_speed_back < 0));
-
- // Intake handling should happen regardless of the turret state
- if (position->intake_beambreak_front() || position->intake_beambreak_back()) {
- if (intake_state_ == IntakeState::NO_BALL) {
- if (position->intake_beambreak_front()) {
- intake_state_ = IntakeState::INTAKE_FRONT_BALL;
- } else if (position->intake_beambreak_back()) {
- intake_state_ = IntakeState::INTAKE_BACK_BALL;
- }
- }
-
- intake_beambreak_timer_ = timestamp;
+ if (position->intake_beambreak_front()) {
+ front_intake_has_ball_ = true;
+ front_intake_beambreak_timer_ = timestamp;
}
- if (timestamp >
- intake_beambreak_timer_ + constants::Values::kBallLostTime()) {
- intake_state_ = IntakeState::NO_BALL;
+ if (position->intake_beambreak_back()) {
+ back_intake_has_ball_ = true;
+ back_intake_beambreak_timer_ = timestamp;
}
- if (intake_state_ != IntakeState::NO_BALL) {
+ // Check if we're either spitting of have lost the ball.
+ if (transfer_roller_speed_front < 0.0 ||
+ timestamp >
+ front_intake_beambreak_timer_ + constants::Values::kBallLostTime()) {
+ front_intake_has_ball_ = false;
+ }
+
+ if (transfer_roller_speed_back < 0.0 ||
+ timestamp >
+ back_intake_beambreak_timer_ + constants::Values::kBallLostTime()) {
+ back_intake_has_ball_ = false;
+ }
+
+ // Wiggle transfer rollers: send the ball back and forth while waiting
+ // for the turret or waiting for another shot to be completed
+ const double wiggle_voltage =
+ constants::Values::kTransferRollerWiggleVoltage();
+ if (front_intake_has_ball_) {
roller_speed_compensated_front = 0.0;
- roller_speed_compensated_back = 0.0;
-
- const double wiggle_voltage =
- constants::Values::kTransferRollerWiggleVoltage();
- // Wiggle transfer rollers: send the ball back and forth while waiting
- // for the turret or waiting for another shot to be completed
- if (intake_state_ == IntakeState::INTAKE_FRONT_BALL) {
- if (position->intake_beambreak_front()) {
- transfer_roller_speed_front = -wiggle_voltage;
- transfer_roller_speed_back = wiggle_voltage;
- } else {
- transfer_roller_speed_front = wiggle_voltage;
- transfer_roller_speed_back = -wiggle_voltage;
- }
+ if (position->intake_beambreak_front()) {
+ transfer_roller_speed_front = -wiggle_voltage;
} else {
- if (position->intake_beambreak_back()) {
- transfer_roller_speed_back = -wiggle_voltage;
- transfer_roller_speed_front = wiggle_voltage;
- } else {
- transfer_roller_speed_back = wiggle_voltage;
- transfer_roller_speed_front = -wiggle_voltage;
- }
+ transfer_roller_speed_front = wiggle_voltage;
}
}
+ if (back_intake_has_ball_) {
+ roller_speed_compensated_back = 0.0;
+ if (position->intake_beambreak_back()) {
+ transfer_roller_speed_back = -wiggle_voltage;
+ } else {
+ transfer_roller_speed_back = wiggle_voltage;
+ }
+ }
+
+ // Create the goal message for putting the turret in its loading position.
+ // This will then get used in the state machine for the states (IDLE and
+ // TRANSFERRING) that use it.
+ double turret_loading_position =
+ (turret_intake_state_ == RequestedIntake::kFront
+ ? constants::Values::kTurretFrontIntakePos()
+ : constants::Values::kTurretBackIntakePos());
+ // Turn to the loading position as close to the current position as
+ // possible.
+ 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 =
+ frc971::zeroing::Wrap(constants::Values::kTurretRange().middle(),
+ turret_loading_position, 2.0 * M_PI);
+ }
+
+ turret_loading_goal_buffer.Finish(
+ frc971::control_loops::CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *turret_loading_goal_buffer.fbb(), turret_loading_position));
+
switch (state_) {
case SuperstructureState::IDLE: {
- if (is_spitting) {
- intake_state_ = IntakeState::NO_BALL;
+ // Only change the turret's goal loading position when idle, to prevent us
+ // spinning the turret around when TRANSFERRING...
+ if (front_intake_has_ball_ != back_intake_has_ball_) {
+ if (have_active_intake_request) {
+ turret_intake_state_ = unsafe_goal->turret_intake();
+ }
+ turret_intake_state_ = front_intake_has_ball_ ? RequestedIntake::kFront
+ : RequestedIntake::kBack;
+ }
+ // When IDLE with no specific intake button pressed, allow the goal
+ // message to override the intaking stuff.
+ if (have_active_intake_request || turret_goal == nullptr) {
+ turret_goal = &turret_loading_goal_buffer.message();
}
- if (intake_state_ == IntakeState::NO_BALL ||
- !(position->intake_beambreak_front() ||
- position->intake_beambreak_back())) {
+ if (!front_intake_has_ball_ && !back_intake_has_ball_) {
break;
}
@@ -197,36 +260,16 @@
break;
}
case SuperstructureState::TRANSFERRING: {
- // If we've been transferring for too long, the ball probably got lost
- if (intake_state_ == IntakeState::NO_BALL) {
+ // If we've been transferring for too long, the ball probably got lost.
+ if ((turret_intake_state_ == RequestedIntake::kFront &&
+ !front_intake_has_ball_) ||
+ (turret_intake_state_ == RequestedIntake::kBack &&
+ !back_intake_has_ball_)) {
state_ = SuperstructureState::IDLE;
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();
+ turret_goal = &turret_loading_goal_buffer.message();
const bool turret_near_goal =
std::abs(turret_.estimated_position() - turret_loading_position) <
@@ -236,22 +279,22 @@
}
// Transfer rollers and flipper arm belt on
- if (intake_state_ == IntakeState::INTAKE_FRONT_BALL) {
+ if (turret_intake_state_ == RequestedIntake::kFront) {
transfer_roller_speed_front =
constants::Values::kTransferRollerVoltage();
- transfer_roller_speed_back =
- -constants::Values::kTransferRollerVoltage();
} else {
transfer_roller_speed_back =
constants::Values::kTransferRollerVoltage();
- transfer_roller_speed_front =
- -constants::Values::kTransferRollerVoltage();
}
flipper_arms_voltage = constants::Values::kFlipperFeedVoltage();
// Ball is in catapult
if (position->turret_beambreak()) {
- intake_state_ = IntakeState::NO_BALL;
+ if (turret_intake_state_ == RequestedIntake::kFront) {
+ front_intake_has_ball_ = false;
+ } else {
+ back_intake_has_ball_ = false;
+ }
state_ = SuperstructureState::LOADING;
loading_timer_ = timestamp;
}
@@ -381,12 +424,12 @@
// Disable the catapult if we want to restart to prevent damage with
// flippers
const flatbuffers::Offset<PotAndAbsoluteEncoderProfiledJointStatus>
- catapult_status_offset =
- catapult_.Iterate(unsafe_goal, position,
- output != nullptr && !catapult_.estopped()
- ? &(output_struct.catapult_voltage)
- : nullptr,
- fire_, status->fbb());
+ catapult_status_offset = catapult_.Iterate(
+ catapult_goal, position, robot_state().voltage_battery(),
+ output != nullptr && !catapult_.estopped()
+ ? &(output_struct.catapult_voltage)
+ : nullptr,
+ fire_, status->fbb());
const flatbuffers::Offset<RelativeEncoderProfiledJointStatus>
climber_status_offset = climber_.Iterate(
@@ -446,12 +489,29 @@
status_builder.add_solve_time(catapult_.solve_time());
status_builder.add_shot_count(catapult_.shot_count());
status_builder.add_mpc_active(catapult_.mpc_active());
+ if (catapult_goal != nullptr) {
+ status_builder.add_shot_position(catapult_goal->shot_position());
+ status_builder.add_shot_velocity(catapult_goal->shot_velocity());
+ }
status_builder.add_flippers_open(flippers_open_);
status_builder.add_reseating_in_catapult(reseating_in_catapult_);
status_builder.add_fire(fire_);
status_builder.add_state(state_);
- status_builder.add_intake_state(intake_state_);
+ if (!front_intake_has_ball_ && !back_intake_has_ball_) {
+ status_builder.add_intake_state(IntakeState::NO_BALL);
+ } else if (front_intake_has_ball_ && back_intake_has_ball_) {
+ status_builder.add_intake_state(turret_intake_state_ ==
+ RequestedIntake::kFront
+ ? IntakeState::INTAKE_FRONT_BALL
+ : IntakeState::INTAKE_BACK_BALL);
+ } else {
+ status_builder.add_intake_state(front_intake_has_ball_
+ ? IntakeState::INTAKE_FRONT_BALL
+ : IntakeState::INTAKE_BACK_BALL);
+ }
+ status_builder.add_front_intake_has_ball(front_intake_has_ball_);
+ status_builder.add_back_intake_has_ball(back_intake_has_ball_);
status_builder.add_aimer(aimer_offset);
diff --git a/y2022/control_loops/superstructure/superstructure.h b/y2022/control_loops/superstructure/superstructure.h
index e9afcc1..11cd38f 100644
--- a/y2022/control_loops/superstructure/superstructure.h
+++ b/y2022/control_loops/superstructure/superstructure.h
@@ -81,7 +81,9 @@
bool reseating_in_catapult_ = false;
bool fire_ = false;
- aos::monotonic_clock::time_point intake_beambreak_timer_ =
+ aos::monotonic_clock::time_point front_intake_beambreak_timer_ =
+ aos::monotonic_clock::min_time;
+ aos::monotonic_clock::time_point back_intake_beambreak_timer_ =
aos::monotonic_clock::min_time;
aos::monotonic_clock::time_point transferring_timer_ =
aos::monotonic_clock::min_time;
@@ -90,7 +92,9 @@
aos::monotonic_clock::time_point flipper_opening_start_time_ =
aos::monotonic_clock::min_time;
SuperstructureState state_ = SuperstructureState::IDLE;
- IntakeState intake_state_ = IntakeState::NO_BALL;
+ bool front_intake_has_ball_ = false;
+ bool back_intake_has_ball_ = false;
+ RequestedIntake turret_intake_state_ = RequestedIntake::kFront;
DISALLOW_COPY_AND_ASSIGN(Superstructure);
};
diff --git a/y2022/control_loops/superstructure/superstructure_goal.fbs b/y2022/control_loops/superstructure/superstructure_goal.fbs
index 8862d22..7227dc2 100644
--- a/y2022/control_loops/superstructure/superstructure_goal.fbs
+++ b/y2022/control_loops/superstructure/superstructure_goal.fbs
@@ -2,6 +2,12 @@
namespace y2022.control_loops.superstructure;
+// Which intake to transfer to the turret from.
+enum RequestedIntake : ubyte {
+ kFront = 0,
+ kBack = 1,
+}
+
table CatapultGoal {
// Old fire flag, only kept for backwards-compatability with logs.
// Use the fire flag in the root Goal instead
@@ -26,8 +32,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);
@@ -51,6 +60,9 @@
// If true, we started with the ball loaded and should proceed to that state.
preloaded:bool (id: 13);
+
+ // Specifies which intake the turret should move to to intake.
+ turret_intake:RequestedIntake (id: 14);
}
diff --git a/y2022/control_loops/superstructure/superstructure_lib_test.cc b/y2022/control_loops/superstructure/superstructure_lib_test.cc
index 66a615e..389c01a 100644
--- a/y2022/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2022/control_loops/superstructure/superstructure_lib_test.cc
@@ -766,6 +766,7 @@
goal_builder.add_roller_speed_back(12.0);
goal_builder.add_roller_speed_compensation(0.0);
goal_builder.add_turret(turret_offset);
+ goal_builder.add_turret_intake(RequestedIntake::kFront);
builder.CheckOk(builder.Send(goal_builder.Finish()));
}
RunFor(std::chrono::seconds(2));
@@ -776,6 +777,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 +785,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 requested the front intake, 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);
@@ -793,7 +797,7 @@
// Make sure that the turret goal is set to be loading from the front intake
// and the supersturcture is transferring from the front intake, since that
- // beambreak was trigerred. Also, the outside rollers should be stopped
+ // beambreak was trigerred. Also, the front outside rollers should be stopped.
ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
EXPECT_EQ(superstructure_status_fetcher_->state(),
@@ -801,7 +805,7 @@
EXPECT_EQ(superstructure_status_fetcher_->intake_state(),
IntakeState::INTAKE_FRONT_BALL);
EXPECT_EQ(superstructure_output_fetcher_->roller_voltage_front(), 0.0);
- EXPECT_EQ(superstructure_output_fetcher_->roller_voltage_back(), 0.0);
+ EXPECT_EQ(superstructure_output_fetcher_->roller_voltage_back(), 12.0);
RunFor(chrono::seconds(2));
@@ -811,7 +815,7 @@
ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
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_EQ(superstructure_output_fetcher_->roller_voltage_back(), 12.0);
EXPECT_EQ(superstructure_status_fetcher_->state(),
SuperstructureState::TRANSFERRING);
EXPECT_EQ(superstructure_status_fetcher_->intake_state(),
@@ -821,7 +825,7 @@
EXPECT_EQ(superstructure_output_fetcher_->transfer_roller_voltage_front(),
constants::Values::kTransferRollerVoltage());
EXPECT_EQ(superstructure_output_fetcher_->transfer_roller_voltage_back(),
- -constants::Values::kTransferRollerVoltage());
+ 0.0);
EXPECT_NEAR(superstructure_status_fetcher_->turret()->position(),
constants::Values::kTurretFrontIntakePos(), 0.001);
EXPECT_EQ(superstructure_status_fetcher_->shot_count(), 0);
@@ -902,20 +906,22 @@
// move the turret.
ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
- EXPECT_EQ(superstructure_output_fetcher_->roller_voltage_front(), 0.0);
+ 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_front() !=
+ EXPECT_TRUE(superstructure_output_fetcher_->transfer_roller_voltage_back() !=
0.0 &&
- superstructure_output_fetcher_->transfer_roller_voltage_front() <=
+ superstructure_output_fetcher_->transfer_roller_voltage_back() <=
constants::Values::kTransferRollerWiggleVoltage() &&
- superstructure_output_fetcher_->transfer_roller_voltage_front() >=
+ superstructure_output_fetcher_->transfer_roller_voltage_back() >=
-constants::Values::kTransferRollerWiggleVoltage());
- EXPECT_EQ(superstructure_output_fetcher_->transfer_roller_voltage_back(),
- -superstructure_output_fetcher_->transfer_roller_voltage_front());
+ 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(),
IntakeState::INTAKE_BACK_BALL);
+ EXPECT_FALSE(superstructure_status_fetcher_->front_intake_has_ball());
+ EXPECT_TRUE(superstructure_status_fetcher_->back_intake_has_ball());
EXPECT_NEAR(superstructure_status_fetcher_->turret()->position(), kTurretGoal,
0.001);
@@ -935,7 +941,7 @@
auto catapult_offset = catapult_builder.Finish();
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
- goal_builder.add_roller_speed_front(12.0);
+ goal_builder.add_roller_speed_front(0.0);
goal_builder.add_roller_speed_back(12.0);
goal_builder.add_roller_speed_compensation(0.0);
goal_builder.add_catapult(catapult_offset);
@@ -952,7 +958,7 @@
// Now that we were asked to fire and the flippers are open,
// we should be shooting the ball and holding the flippers open.
// The turret should still be at its goal, and we should still be wiggling the
- // transfer rollers to keep the ball in the back intake
+ // transfer rollers to keep the ball in the back intake.
ASSERT_TRUE(superstructure_output_fetcher_.Fetch());
ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
EXPECT_EQ(superstructure_output_fetcher_->roller_voltage_front(), 0.0);
@@ -963,8 +969,8 @@
constants::Values::kTransferRollerWiggleVoltage() &&
superstructure_output_fetcher_->transfer_roller_voltage_back() >=
-constants::Values::kTransferRollerWiggleVoltage());
- EXPECT_EQ(superstructure_output_fetcher_->transfer_roller_voltage_front(),
- -superstructure_output_fetcher_->transfer_roller_voltage_back());
+ 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(),
@@ -983,17 +989,18 @@
superstructure_plant_.set_intake_beambreak_back(false);
RunFor(std::chrono::seconds(2));
- // After a bit, we should have completed the shot and be idle.
+ // After a bit, we should have completed the shot and be transferring.
// Since the beambreak was triggered a bit ago, it should still think a ball
- // is there
+ // is there.
ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
EXPECT_EQ(superstructure_status_fetcher_->shot_count(), 1);
- EXPECT_EQ(superstructure_status_fetcher_->state(), SuperstructureState::IDLE);
+ EXPECT_EQ(superstructure_status_fetcher_->state(),
+ SuperstructureState::TRANSFERRING);
EXPECT_EQ(superstructure_status_fetcher_->intake_state(),
IntakeState::INTAKE_BACK_BALL);
// Since the intake beambreak hasn't triggered in a while, it should realize
- // the ball was lost
+ // the ball was lost.
RunFor(std::chrono::seconds(1));
ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
EXPECT_EQ(superstructure_status_fetcher_->shot_count(), 1);
@@ -1196,7 +1203,15 @@
EXPECT_NEAR(superstructure_status_fetcher_->catapult()->position(),
constants::Values::kCatapultRange().lower, 1e-3);
EXPECT_EQ(superstructure_status_fetcher_->shot_count(), 1);
- EXPECT_EQ(superstructure_status_fetcher_->state(), SuperstructureState::IDLE);
+ // Will still be in TRANSFERRING because we left the front beambreak on after
+ // we entered loaded, so we queued up another ball.
+ EXPECT_EQ(superstructure_status_fetcher_->state(),
+ SuperstructureState::TRANSFERRING);
+
+ RunFor(chrono::milliseconds(2000));
+ ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
+ EXPECT_EQ(superstructure_status_fetcher_->state(),
+ SuperstructureState::IDLE);
}
// Test that we are able to signal that the ball was preloaded
@@ -1251,6 +1266,42 @@
superstructure_status_fetcher_->aimer()->turret_velocity());
}
+TEST_F(SuperstructureTest, InterpolationTableTest) {
+ SetEnabled(true);
+ WaitUntilZeroed();
+
+ constexpr double kDistance = 3.0;
+
+ SendDrivetrainStatus(0.0, {0.0, kDistance}, 0.0);
+
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_auto_aim(true);
+
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
+ }
+
+ // Give it time to stabilize.
+ RunFor(chrono::seconds(2));
+
+ ASSERT_TRUE(superstructure_status_fetcher_.Fetch());
+
+ EXPECT_NEAR(superstructure_status_fetcher_->aimer()->target_distance(),
+ kDistance, 0.01);
+
+ constants::Values::ShotParams shot_params;
+ EXPECT_TRUE(
+ values_->shot_interpolation_table.GetInRange(kDistance, &shot_params));
+
+ EXPECT_EQ(superstructure_status_fetcher_->shot_velocity(),
+ shot_params.shot_velocity);
+ EXPECT_EQ(superstructure_status_fetcher_->shot_position(),
+ shot_params.shot_angle);
+}
+
} // namespace testing
} // namespace superstructure
} // namespace control_loops
diff --git a/y2022/control_loops/superstructure/superstructure_plotter.ts b/y2022/control_loops/superstructure/superstructure_plotter.ts
index 36f0c92..f8c3a7c 100644
--- a/y2022/control_loops/superstructure/superstructure_plotter.ts
+++ b/y2022/control_loops/superstructure/superstructure_plotter.ts
@@ -6,7 +6,7 @@
import Connection = proxy.Connection;
const TIME = AosPlotter.TIME;
-const DEFAULT_WIDTH = AosPlotter.DEFAULT_WIDTH;
+const DEFAULT_WIDTH = AosPlotter.DEFAULT_WIDTH * 2;
const DEFAULT_HEIGHT = AosPlotter.DEFAULT_HEIGHT * 3;
export function plotSuperstructure(conn: Connection, element: Element): void {
@@ -78,4 +78,7 @@
otherPlot.addMessageLine(position, ['flipper_arm_right', 'encoder'])
.setColor(CYAN)
.setPointSize(4.0);
+ otherPlot.addMessageLine(output, ['flipper_arms_voltage'])
+ .setColor(BROWN)
+ .setPointSize(4.0);
}
diff --git a/y2022/control_loops/superstructure/superstructure_status.fbs b/y2022/control_loops/superstructure/superstructure_status.fbs
index 4b215ac..92f3dbd 100644
--- a/y2022/control_loops/superstructure/superstructure_status.fbs
+++ b/y2022/control_loops/superstructure/superstructure_status.fbs
@@ -48,6 +48,9 @@
state:SuperstructureState (id: 10);
// Intaking state
intake_state:IntakeState (id: 11);
+ // Whether the front/rear intakes currently are holding balls.
+ front_intake_has_ball:bool (id: 18);
+ back_intake_has_ball:bool (id: 19);
// Whether the flippers are open for shooting
flippers_open:bool (id: 12);
// Whether the flippers failed to open and we are retrying
@@ -70,6 +73,8 @@
solve_time:double (id: 7);
mpc_active:bool (id: 8);
+ shot_position:double (id: 16);
+ shot_velocity:double (id: 17);
// The number of shots we have taken.
shot_count:int32 (id: 9);
diff --git a/y2022/control_loops/superstructure/turret/aiming.cc b/y2022/control_loops/superstructure/turret/aiming.cc
index 4c0309c..2a600d8 100644
--- a/y2022/control_loops/superstructure/turret/aiming.cc
+++ b/y2022/control_loops/superstructure/turret/aiming.cc
@@ -22,13 +22,22 @@
// straight out the back of the robot.
constexpr double kTurretZeroOffset = M_PI;
+constexpr double kMaxProfiledVelocity = 10.0;
+constexpr double kMaxProfiledAccel = 20.0;
+
flatbuffers::DetachedBuffer MakePrefilledGoal() {
flatbuffers::FlatBufferBuilder fbb;
fbb.ForceDefaults(true);
+ frc971::ProfileParameters::Builder profile_builder(fbb);
+ profile_builder.add_max_velocity(kMaxProfiledVelocity);
+ profile_builder.add_max_acceleration(kMaxProfiledAccel);
+ const flatbuffers::Offset<frc971::ProfileParameters> profile_offset =
+ profile_builder.Finish();
Aimer::Goal::Builder builder(fbb);
builder.add_unsafe_goal(0);
builder.add_goal_velocity(0);
- builder.add_ignore_profile(true);
+ builder.add_ignore_profile(false);
+ builder.add_profile_params(profile_offset);
fbb.Finish(builder.Finish());
return fbb.Release();
}
diff --git a/y2022/control_loops/superstructure/turret_plotter.ts b/y2022/control_loops/superstructure/turret_plotter.ts
index 10fc10e..6753880 100644
--- a/y2022/control_loops/superstructure/turret_plotter.ts
+++ b/y2022/control_loops/superstructure/turret_plotter.ts
@@ -29,6 +29,12 @@
positionPlot.addMessageLine(status, ['turret', 'goal_position']).setColor(RED).setPointSize(4.0);
positionPlot.addMessageLine(status, ['turret', 'goal_velocity']).setColor(ORANGE).setPointSize(4.0);
positionPlot.addMessageLine(status, ['turret', 'estimator_state', 'position']).setColor(CYAN).setPointSize(1.0);
+ positionPlot.addMessageLine(status, ['aimer', 'turret_velocity'])
+ .setColor(WHITE)
+ .setPointSize(1.0);
+ positionPlot.addMessageLine(status, ['aimer', 'turret_position'])
+ .setColor(BROWN)
+ .setPointSize(1.0);
const voltagePlot =
aosPlotter.addPlot(element, [DEFAULT_WIDTH, DEFAULT_HEIGHT / 2]);
diff --git a/y2022/joystick_reader.cc b/y2022/joystick_reader.cc
index 2f69758..a911e4b 100644
--- a/y2022/joystick_reader.cc
+++ b/y2022/joystick_reader.cc
@@ -37,15 +37,15 @@
namespace superstructure = y2022::control_loops::superstructure;
-// TODO(henry) put actually button locations here
-// TODO(milind): integrate with shooting statemachine and aimer
#if 0
const ButtonLocation kCatapultPos(4, 3);
const ButtonLocation kFire(3, 4);
const ButtonLocation kTurret(4, 15);
+const ButtonLocation kAutoAim(4, 2);
const ButtonLocation kIntakeFrontOut(4, 10);
const ButtonLocation kIntakeBackOut(4, 9);
+const ButtonLocation kSpit(3, 3);
const ButtonLocation kRedLocalizerReset(3, 13);
const ButtonLocation kBlueLocalizerReset(3, 14);
@@ -55,12 +55,16 @@
const ButtonLocation kCatapultPos(4, 3);
const ButtonLocation kFire(4, 1);
const ButtonLocation kTurret(4, 15);
+const ButtonLocation kAutoAim(4, 2);
+
+const ButtonLocation kClimberExtend(4, 6);
const ButtonLocation kIntakeFrontOut(4, 10);
const ButtonLocation kIntakeBackOut(4, 9);
+const ButtonLocation kSpit(3, 3);
-const ButtonLocation kRedLocalizerReset(3, 13);
-const ButtonLocation kBlueLocalizerReset(3, 14);
+const ButtonLocation kRedLocalizerReset(4, 14);
+const ButtonLocation kBlueLocalizerReset(4, 13);
const ButtonLocation kLocalizerReset(3, 8);
#endif
@@ -82,16 +86,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) {
@@ -105,10 +111,10 @@
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(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) {
@@ -159,11 +165,15 @@
double intake_back_pos = 1.47;
double transfer_roller_front_speed = 0.0;
double transfer_roller_back_speed = 0.0;
+ std::optional<control_loops::superstructure::RequestedIntake>
+ requested_intake;
double roller_front_speed = 0.0;
double roller_back_speed = 0.0;
- double turret_pos = 0.0;
+ std::optional<double> turret_pos = 0.0;
+
+ double climber_position = 0.01;
double catapult_pos = 0.03;
double catapult_speed = 18.0;
@@ -181,14 +191,18 @@
BlueResetLocalizer();
}
+ if (data.IsPressed(kClimberExtend)) {
+ climber_position = 0.50;
+ } else {
+ climber_position = 0.01;
+ }
+
if (data.IsPressed(kTurret)) {
if (setpoint_fetcher_.get()) {
turret_pos = setpoint_fetcher_->turret();
} else {
turret_pos = -1.5;
}
- } else {
- turret_pos = 0.0;
}
if (setpoint_fetcher_.get()) {
@@ -204,17 +218,42 @@
catapult_return_pos = -0.908;
}
+ constexpr double kRollerSpeed = 8.0;
+ constexpr double kTransferRollerSpeed = 12.0;
+ constexpr double kIntakePosition = -0.02;
+ constexpr size_t kIntakeCounterIterations = 25;
+
// Extend the intakes and spin the rollers
if (data.IsPressed(kIntakeFrontOut)) {
- intake_front_pos = 0.0;
- roller_front_speed = 12.0;
- transfer_roller_front_speed = 12.0;
- transfer_roller_back_speed = -transfer_roller_front_speed;
+ intake_front_pos = kIntakePosition;
+ transfer_roller_front_speed = kTransferRollerSpeed;
+
+ intake_front_counter_ = kIntakeCounterIterations;
+ intake_back_counter_ = 0;
} else if (data.IsPressed(kIntakeBackOut)) {
- roller_back_speed = 12.0;
- intake_back_pos = 0.0;
- transfer_roller_back_speed = 12.0;
- transfer_roller_front_speed = -transfer_roller_back_speed;
+ intake_back_pos = kIntakePosition;
+ transfer_roller_back_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;
+
+ intake_front_counter_ = 0;
+ intake_back_counter_ = 0;
+ }
+
+ // Keep spinning the rollers a bit after they let go
+ if (intake_front_counter_ > 0) {
+ intake_front_counter_--;
+ roller_front_speed = kRollerSpeed;
+ requested_intake = control_loops::superstructure::RequestedIntake::kFront;
+ }
+ if (intake_back_counter_ > 0) {
+ intake_back_counter_--;
+ roller_back_speed = kRollerSpeed;
+ requested_intake = control_loops::superstructure::RequestedIntake::kBack;
}
if (data.IsPressed(kFire)) {
@@ -236,9 +275,12 @@
CreateProfileParameters(*builder.fbb(), 8.0, 40.0));
flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
- turret_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
- *builder.fbb(), turret_pos,
- CreateProfileParameters(*builder.fbb(), 1.0, 10.0));
+ turret_offset;
+ if (turret_pos.has_value()) {
+ CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), turret_pos.value(),
+ CreateProfileParameters(*builder.fbb(), 12.0, 20.0));
+ }
flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
catapult_return_offset =
@@ -246,6 +288,11 @@
*builder.fbb(), catapult_return_pos,
frc971::CreateProfileParameters(*builder.fbb(), 9.0, 50.0));
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ climber_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), climber_position,
+ frc971::CreateProfileParameters(*builder.fbb(), 1.0, 5.0));
+
superstructure::CatapultGoal::Builder catapult_builder =
builder.MakeBuilder<superstructure::CatapultGoal>();
catapult_builder.add_return_position(catapult_return_offset);
@@ -260,6 +307,7 @@
superstructure_goal_builder.add_intake_front(intake_front_offset);
superstructure_goal_builder.add_intake_back(intake_back_offset);
superstructure_goal_builder.add_turret(turret_offset);
+ superstructure_goal_builder.add_climber(climber_offset);
superstructure_goal_builder.add_catapult(catapult_offset);
superstructure_goal_builder.add_fire(fire);
@@ -269,6 +317,10 @@
transfer_roller_front_speed);
superstructure_goal_builder.add_transfer_roller_speed_back(
transfer_roller_back_speed);
+ superstructure_goal_builder.add_auto_aim(data.IsPressed(kAutoAim));
+ if (requested_intake.has_value()) {
+ superstructure_goal_builder.add_turret_intake(requested_intake.value());
+ }
if (builder.Send(superstructure_goal_builder.Finish()) !=
aos::RawSender::Error::kOk) {
@@ -286,6 +338,9 @@
::aos::Fetcher<superstructure::Status> superstructure_status_fetcher_;
::aos::Fetcher<Setpoint> setpoint_fetcher_;
+
+ size_t intake_front_counter_ = 0;
+ size_t intake_back_counter_ = 0;
};
} // namespace joysticks
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..1971312 100644
--- a/y2022/localizer/localizer.cc
+++ b/y2022/localizer/localizer.cc
@@ -1,9 +1,12 @@
#include "y2022/localizer/localizer.h"
+#include "aos/json_to_flatbuffer.h"
#include "frc971/control_loops/c2d.h"
#include "frc971/wpilib/imu_batch_generated.h"
#include "y2022/constants.h"
-#include "aos/json_to_flatbuffer.h"
+
+DEFINE_bool(ignore_accelerometer, false,
+ "If set, ignores the accelerometer readings.");
namespace frc971::controls {
@@ -17,7 +20,7 @@
constexpr double kVisionTargetY = 0.0;
// Minimum confidence to require to use a target match.
-constexpr double kMinTargetEstimateConfidence = 0.2;
+constexpr double kMinTargetEstimateConfidence = 0.75;
template <int N>
Eigen::Matrix<double, N, 1> MakeState(std::vector<double> values) {
@@ -39,8 +42,9 @@
.coefficients()),
down_estimator_(dt_config) {
statistics_.rejection_counts.fill(0);
- CHECK_EQ(branches_.capacity(), static_cast<size_t>(std::chrono::seconds(1) /
- kNominalDt / kBranchPeriod));
+ CHECK_EQ(branches_.capacity(),
+ static_cast<size_t>(std::chrono::seconds(1) / kNominalDt /
+ kBranchPeriod));
if (dt_config_.is_simulated) {
down_estimator_.assume_perfect_gravity();
}
@@ -101,6 +105,8 @@
DiscretizeQAFast(Q_continuous_accel_, A_continuous_accel_, kNominalDt,
&Q_discrete_accel_, &A_discrete_accel_);
P_accel_ = Q_discrete_accel_;
+
+ led_outputs_.fill(LedOutput::ON);
}
Eigen::Matrix<double, ModelBasedLocalizer::kNModelStates,
@@ -170,7 +176,7 @@
const ModelBasedLocalizer::ModelState &state) const {
const double robot_speed =
(state(kLeftVelocity) + state(kRightVelocity)) / 2.0;
- const double lat_speed = (AModel(state) * state)(kTheta) * long_offset_;
+ const double lat_speed = (AModel(state) * state)(kTheta)*long_offset_;
const double velocity_x = std::cos(state(kTheta)) * robot_speed -
std::sin(state(kTheta)) * lat_speed;
const double velocity_y = std::sin(state(kTheta)) * robot_speed +
@@ -369,7 +375,8 @@
constexpr size_t kShareStates = kNModelStates;
static_assert(kUseModelThreshold < kUseAccelThreshold);
if (using_model_) {
- if (filtered_residual_ > kUseAccelThreshold) {
+ if (!FLAGS_ignore_accelerometer &&
+ filtered_residual_ > kUseAccelThreshold) {
hysteresis_count_++;
} else {
hysteresis_count_ = 0;
@@ -438,7 +445,7 @@
VLOG(2) << "Model state " << current_state_.model_state.transpose();
VLOG(2) << "Accel state " << current_state_.accel_state.transpose();
VLOG(2) << "Accel state for model "
- << AccelStateForModelState(current_state_.model_state).transpose();
+ << AccelStateForModelState(current_state_.model_state).transpose();
VLOG(2) << "Input acce " << accel.transpose();
VLOG(2) << "Input gyro " << gyro.transpose();
VLOG(2) << "Input voltage " << voltage.transpose();
@@ -486,8 +493,9 @@
}
// Node names of the pis to listen for cameras from.
-const std::array<std::string_view, 4> kPisToUse{"pi1", "pi2", "pi3", "pi4"};
-}
+constexpr std::array<std::string_view, ModelBasedLocalizer::kNumPis> kPisToUse{
+ "pi1", "pi2", "pi3", "pi4"};
+} // namespace
const Eigen::Matrix<double, 4, 4> ModelBasedLocalizer::CameraTransform(
const OldPosition &state,
@@ -647,9 +655,9 @@
// And now we have to correct *everything* on all the branches:
for (CombinedState &state : branches_) {
state.model_state += K_model * (measured_robot_position.value() -
- H_model * state.model_state);
+ H_model * state.model_state);
state.accel_state += K_accel * (measured_robot_position.value() -
- H_accel * state.accel_state);
+ H_accel * state.accel_state);
}
current_state_.model_state +=
K_model *
@@ -666,6 +674,9 @@
const double camera_yaw =
std::atan2(camera_z_in_field.y(), camera_z_in_field.x());
+ // TODO(milind): actually control this
+ led_outputs_[camera_index] = LedOutput::ON;
+
TargetEstimateDebugT debug;
debug.camera = static_cast<uint8_t>(camera_index);
debug.camera_x = H_field_camera_measured(0, 3);
@@ -678,6 +689,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);
}
@@ -692,7 +704,7 @@
void ModelBasedLocalizer::HandleReset(aos::monotonic_clock::time_point now,
const Eigen::Vector3d &xytheta) {
branches_.Reset();
- t_ = now;
+ t_ = now;
using_model_ = true;
current_state_.model_state << xytheta(0), xytheta(1), xytheta(2),
current_state_.model_state(kLeftEncoder), 0.0, 0.0,
@@ -754,7 +766,7 @@
const CombinedState &state = current_state_;
const flatbuffers::Offset<ModelBasedState> model_state_offset =
- BuildModelState(fbb, state.model_state);
+ BuildModelState(fbb, state.model_state);
const flatbuffers::Offset<AccelBasedState> accel_state_offset =
BuildAccelState(fbb, state.accel_state);
@@ -799,7 +811,7 @@
aos::SizedArray<flatbuffers::Offset<TargetEstimateDebug>, kDebugBufferSize>
debug_offsets;
- for (const TargetEstimateDebugT& debug : image_debugs_) {
+ for (const TargetEstimateDebugT &debug : image_debugs_) {
debug_offsets.push_back(PackTargetEstimateDebug(debug, fbb));
}
@@ -822,6 +834,7 @@
TargetEstimateDebugT debug;
debug.accepted = false;
debug.rejection_reason = reason;
+ CHECK_LT(image_debugs_.size(), kDebugBufferSize);
image_debugs_.push_back(debug);
}
@@ -847,12 +860,13 @@
static double DrivetrainWrapPeriod() {
return y2022::constants::Values::DrivetrainEncoderToMeters(1 << 16);
}
-}
+} // namespace
EventLoopLocalizer::EventLoopLocalizer(
aos::EventLoop *event_loop,
const control_loops::drivetrain::DrivetrainConfig<double> &dt_config)
: event_loop_(event_loop),
+ dt_config_(dt_config),
model_based_(dt_config),
status_sender_(event_loop_->MakeSender<LocalizerStatus>("/localizer")),
output_sender_(event_loop_->MakeSender<LocalizerOutput>("/localizer")),
@@ -864,6 +878,10 @@
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 +894,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);
- 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())));
- }
- });
+ for (size_t camera_index = 0; camera_index < kPisToUse.size();
+ ++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())));
+ }
+ 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());
@@ -924,7 +967,7 @@
const Eigen::Vector2d encoders{
left_encoder_.Unwrap(value->left_encoder()),
right_encoder_.Unwrap(value->right_encoder())};
- if (zeroer_.Zeroed()) {
+ {
const aos::monotonic_clock::time_point pico_timestamp{
std::chrono::microseconds(value->pico_timestamp_us())};
// TODO(james): If we get large enough drift off of the pico,
@@ -946,9 +989,14 @@
(output_fetcher_.context().monotonic_event_time +
std::chrono::milliseconds(10) <
event_loop_->context().monotonic_event_time);
+ const bool zeroed = zeroer_.Zeroed();
model_based_.HandleImu(
sample_timestamp,
- zeroer_.ZeroedGyro(), zeroer_.ZeroedAccel(), encoders,
+ zeroed ? zeroer_.ZeroedGyro().value() : Eigen::Vector3d::Zero(),
+ zeroed ? zeroer_.ZeroedAccel().value()
+ : dt_config_.imu_transform.transpose() *
+ Eigen::Vector3d::UnitZ(),
+ encoders,
disabled ? Eigen::Vector2d::Zero()
: Eigen::Vector2d{output_fetcher_->left_voltage(),
output_fetcher_->right_voltage()});
@@ -978,6 +1026,11 @@
if (last_output_send_ + std::chrono::milliseconds(5) <
event_loop_->context().monotonic_event_time) {
auto builder = output_sender_.MakeBuilder();
+
+ const auto led_outputs_offset =
+ builder.fbb()->CreateVector(model_based_.led_outputs().data(),
+ model_based_.led_outputs().size());
+
LocalizerOutput::Builder output_builder =
builder.MakeBuilder<LocalizerOutput>();
// TODO(james): Should we bother to try to estimate time offsets for
@@ -995,6 +1048,7 @@
quaternion.mutate_z(orientation.z());
quaternion.mutate_w(orientation.w());
output_builder.add_orientation(&quaternion);
+ output_builder.add_led_outputs(led_outputs_offset);
builder.CheckOk(builder.Send(output_builder.Finish()));
last_output_send_ = event_loop_->monotonic_now();
}
diff --git a/y2022/localizer/localizer.h b/y2022/localizer/localizer.h
index 4b7eff0..be14b45 100644
--- a/y2022/localizer/localizer.h
+++ b/y2022/localizer/localizer.h
@@ -63,6 +63,8 @@
// * Tune for ADIS16505/real robot.
class ModelBasedLocalizer {
public:
+ static constexpr size_t kNumPis = 4;
+
static constexpr size_t kX = 0;
static constexpr size_t kY = 1;
static constexpr size_t kTheta = 2;
@@ -141,6 +143,8 @@
size_t NumQueuedImageDebugs() const { return image_debugs_.size(); }
+ std::array<LedOutput, kNumPis> led_outputs() const { return led_outputs_; }
+
private:
struct CombinedState {
AccelState accel_state = AccelState::Zero();
@@ -298,6 +302,7 @@
Statistics statistics_;
aos::SizedArray<TargetEstimateDebugT, kDebugBufferSize> image_debugs_;
+ std::array<LedOutput, kNumPis> led_outputs_;
friend class testing::LocalizerTest;
};
@@ -316,12 +321,18 @@
std::optional<aos::monotonic_clock::duration> ClockOffset(
std::string_view pi);
aos::EventLoop *event_loop_;
+ const control_loops::drivetrain::DrivetrainConfig<double> &dt_config_;
ModelBasedLocalizer model_based_;
aos::Sender<LocalizerStatus> status_sender_;
aos::Sender<LocalizerOutput> output_sender_;
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>,
+ ModelBasedLocalizer::kNumPis>
+ 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_output.fbs b/y2022/localizer/localizer_output.fbs
index b07278b..ec3302a 100644
--- a/y2022/localizer/localizer_output.fbs
+++ b/y2022/localizer/localizer_output.fbs
@@ -10,6 +10,12 @@
z:double (id: 3);
}
+// Used to tell different LEDs to be on or off
+enum LedOutput : byte {
+ ON,
+ OFF
+}
+
table LocalizerOutput {
// Timestamp (on the source node) that this sample corresponds with. This
// may be older than the sent time to account for delays in sensor readings.
@@ -24,6 +30,10 @@
// Whether we have zeroed the IMU (may go false if we observe a fault with the
// IMU).
zeroed:bool (id: 5);
+
+ // Whether each led should be on.
+ // Indices correspond to pi number.
+ led_outputs:[LedOutput] (id: 6);
}
root_type LocalizerOutput;
diff --git a/y2022/localizer/localizer_test.cc b/y2022/localizer/localizer_test.cc
index 791adf3..6e9cd1e 100644
--- a/y2022/localizer/localizer_test.cc
+++ b/y2022/localizer/localizer_test.cc
@@ -2,12 +2,12 @@
#include "aos/events/logging/log_writer.h"
#include "aos/events/simulated_event_loop.h"
-#include "gtest/gtest.h"
#include "frc971/control_loops/drivetrain/drivetrain_test_lib.h"
#include "frc971/control_loops/pose.h"
-#include "y2022/vision/target_estimate_generated.h"
-#include "y2022/control_loops/superstructure/superstructure_status_generated.h"
+#include "gtest/gtest.h"
#include "y2022/control_loops/drivetrain/drivetrain_base.h"
+#include "y2022/control_loops/superstructure/superstructure_status_generated.h"
+#include "y2022/vision/target_estimate_generated.h"
DEFINE_string(output_folder, "",
"If set, logs all channels to the provided logfile.");
@@ -18,11 +18,11 @@
typedef ModelBasedLocalizer::ModelInput ModelInput;
typedef ModelBasedLocalizer::AccelInput AccelInput;
-using frc971::vision::calibration::CameraCalibrationT;
-using frc971::vision::calibration::TransformationMatrixT;
+using frc971::control_loops::Pose;
using frc971::control_loops::drivetrain::DrivetrainConfig;
using frc971::control_loops::drivetrain::LocalizerControl;
-using frc971::control_loops::Pose;
+using frc971::vision::calibration::CameraCalibrationT;
+using frc971::vision::calibration::TransformationMatrixT;
using y2022::vision::TargetEstimate;
using y2022::vision::TargetEstimateT;
@@ -82,14 +82,12 @@
config.is_simulated = true;
return config;
}
-}
+} // namespace
class LocalizerTest : public ::testing::Test {
protected:
LocalizerTest()
- : dt_config_(
- GetTest2022DrivetrainConfig()),
- localizer_(dt_config_) {
+ : dt_config_(GetTest2022DrivetrainConfig()), localizer_(dt_config_) {
localizer_.set_longitudinal_offset(0.0);
}
ModelState CallDiffModel(const ModelState &state, const ModelInput &U) {
@@ -102,7 +100,6 @@
const control_loops::drivetrain::DrivetrainConfig<double> dt_config_;
ModelBasedLocalizer localizer_;
-
};
TEST_F(LocalizerTest, AccelIntegrationTest) {
@@ -233,7 +230,8 @@
const Eigen::Vector2d encoders{0.0, 0.0};
const Eigen::Vector2d voltages{0.0, 0.0};
Eigen::Vector3d accel{5.0, 2.0, 9.80665};
- Eigen::Vector3d accel_gs = dt_config_.imu_transform.inverse() * accel / 9.80665;
+ Eigen::Vector3d accel_gs =
+ dt_config_.imu_transform.inverse() * accel / 9.80665;
while (t < start) {
// Spin to fill up the buffer.
localizer_.HandleImu(t, gyro, Eigen::Vector3d::UnitZ(), encoders, voltages);
@@ -744,10 +742,10 @@
ASSERT_FALSE(status_fetcher_->model_based()->using_model());
ASSERT_LT(0.1, status_fetcher_->model_based()->residual())
<< aos::FlatbufferToJson(status_fetcher_.get(), {.multi_line = true});
- ASSERT_NEAR(drivetrain_plant_.state()(0),
- status_fetcher_->model_based()->x(), 1.0);
- ASSERT_NEAR(drivetrain_plant_.state()(1),
- status_fetcher_->model_based()->y(), 1e-6);
+ ASSERT_NEAR(drivetrain_plant_.state()(0), status_fetcher_->model_based()->x(),
+ 1.0);
+ ASSERT_NEAR(drivetrain_plant_.state()(1), status_fetcher_->model_based()->y(),
+ 1e-6);
}
// Tests that image corrections in the nominal case (no errors) causes no
@@ -849,22 +847,23 @@
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);
+ ASSERT_NEAR(drivetrain_plant_.state()(1), status_fetcher_->model_based()->y(),
+ 0.1);
ASSERT_TRUE(status_fetcher_->model_based()->has_statistics());
ASSERT_LT(10,
status_fetcher_->model_based()->statistics()->total_candidates());
@@ -872,4 +871,16 @@
status_fetcher_->model_based()->statistics()->total_accepted());
}
-} // namespace frc91::controls::testing
+TEST_F(EventLoopLocalizerTest, LedOutputs) {
+ send_targets_ = true;
+
+ event_loop_factory_.RunFor(std::chrono::milliseconds(10));
+ CHECK(output_fetcher_.Fetch());
+ EXPECT_EQ(output_fetcher_->led_outputs()->size(),
+ ModelBasedLocalizer::kNumPis);
+ for (LedOutput led_output : *output_fetcher_->led_outputs()) {
+ EXPECT_EQ(led_output, LedOutput::ON);
+ }
+}
+
+} // namespace frc971::controls::testing
diff --git a/y2022/setpoint_setter.cc b/y2022/setpoint_setter.cc
index 7dbd4b8..b588636 100644
--- a/y2022/setpoint_setter.cc
+++ b/y2022/setpoint_setter.cc
@@ -8,13 +8,15 @@
DEFINE_double(catapult_velocity, 18.0, "Catapult shot velocity");
DEFINE_double(turret, 0.0, "Turret setpoint");
+DEFINE_string(config, "aos_config.json", "Path to the config file to use.");
+
using y2022::input::joysticks::Setpoint;
int main(int argc, char **argv) {
aos::InitGoogle(&argc, &argv);
aos::FlatbufferDetachedBuffer<aos::Configuration> config =
- aos::configuration::ReadConfig("config.json");
+ aos::configuration::ReadConfig(FLAGS_config);
aos::ShmEventLoop event_loop(&config.message());
diff --git a/y2022/vision/BUILD b/y2022/vision/BUILD
index 4ca8561..02f7fc7 100644
--- a/y2022/vision/BUILD
+++ b/y2022/vision/BUILD
@@ -120,6 +120,7 @@
"//frc971/vision:v4l2_reader",
"//frc971/vision:vision_fbs",
"//third_party:opencv",
+ "//y2022/localizer:localizer_output_fbs",
],
)
@@ -238,6 +239,26 @@
)
cc_binary(
+ name = "viewer_replay",
+ srcs = [
+ "viewer_replay.cc",
+ ],
+ data = [
+ "//y2022:aos_config",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//y2022:__subpackages__"],
+ deps = [
+ ":blob_detector_lib",
+ "//aos:init",
+ "//aos/events:simulated_event_loop",
+ "//aos/events/logging:log_reader",
+ "//frc971/vision:vision_fbs",
+ "//third_party:opencv",
+ ],
+)
+
+cc_binary(
name = "extrinsics_calibration",
srcs = [
"extrinsics_calibration.cc",
@@ -252,3 +273,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/blob_detector.cc b/y2022/vision/blob_detector.cc
index 7bbd1f3..873cf43 100644
--- a/y2022/vision/blob_detector.cc
+++ b/y2022/vision/blob_detector.cc
@@ -13,7 +13,7 @@
DEFINE_uint64(red_delta, 100,
"Required difference between green pixels vs. red");
-DEFINE_uint64(blue_delta, 30,
+DEFINE_uint64(blue_delta, 1,
"Required difference between green pixels vs. blue");
DEFINE_bool(use_outdoors, false,
@@ -52,6 +52,14 @@
}
}
+ // Fill in the contours on the binarized image so that we don't detect
+ // multiple blobs in one
+ const auto blobs = FindBlobs(binarized_image);
+ for (auto it = blobs.begin(); it < blobs.end(); it++) {
+ cv::drawContours(binarized_image, blobs, it - blobs.begin(),
+ cv::Scalar(255), cv::FILLED);
+ }
+
return binarized_image;
}
@@ -226,6 +234,10 @@
cv::Scalar(0, 100, 0), cv::FILLED);
}
+ for (const auto &blob : blob_result.filtered_blobs) {
+ cv::polylines(view_image, blob, true, cv::Scalar(0, 255, 0));
+ }
+
static constexpr double kCircleRadius = 2.0;
// Draw blob centroids
for (auto stats : blob_result.blob_stats) {
diff --git a/y2022/vision/calib_files/calibration_pi-971-4_cam-21-04_2022-01-28_06-51-31.012263392.json b/y2022/vision/calib_files/calibration_pi-971-4_cam-21-04_2022-01-28_06-51-31.012263392.json
new file mode 100644
index 0000000..00059f9
--- /dev/null
+++ b/y2022/vision/calib_files/calibration_pi-971-4_cam-21-04_2022-01-28_06-51-31.012263392.json
@@ -0,0 +1,24 @@
+{
+ "node_name": "pi4",
+ "team_number": 971,
+ "intrinsics": [
+ 391.82019,
+ 0.0,
+ 351.66806,
+ 0.0,
+ 391.380554,
+ 261.502167,
+ 0.0,
+ 0.0,
+ 1.0
+ ],
+ "dist_coeffs": [
+ 0.147662,
+ -0.258165,
+ 0.000671,
+ -0.00027,
+ 0.09542
+ ],
+ "calibration_timestamp": 1643352691012263392,
+ "camera_id": "21-03"
+}
diff --git a/y2022/vision/calib_files/calibration_pi-9971-1_cam-21-01_2022-01-28_07-06-07.187479188.json b/y2022/vision/calib_files/calibration_pi-9971-1_cam-21-01_2022-01-28_07-06-07.187479188.json
new file mode 100755
index 0000000..01935b8
--- /dev/null
+++ b/y2022/vision/calib_files/calibration_pi-9971-1_cam-21-01_2022-01-28_07-06-07.187479188.json
@@ -0,0 +1,24 @@
+{
+ "node_name": "pi1",
+ "team_number": 9971,
+ "intrinsics": [
+ 390.600739,
+ 0.0,
+ 346.584473,
+ 0.0,
+ 390.302429,
+ 234.478241,
+ 0.0,
+ 0.0,
+ 1.0
+ ],
+ "dist_coeffs": [
+ 0.134674,
+ -0.239105,
+ -0.000435,
+ 0.000515,
+ 0.087243
+ ],
+ "calibration_timestamp": 1643353567187479188,
+ "camera_id": "21-01"
+}
\ No newline at end of file
diff --git a/y2022/vision/calib_files/calibration_pi-9971-2_cam-21-02_2022-01-28_06-56-21.541962185.json b/y2022/vision/calib_files/calibration_pi-9971-2_cam-21-02_2022-01-28_06-56-21.541962185.json
new file mode 100755
index 0000000..3025298
--- /dev/null
+++ b/y2022/vision/calib_files/calibration_pi-9971-2_cam-21-02_2022-01-28_06-56-21.541962185.json
@@ -0,0 +1,24 @@
+{
+ "node_name": "pi2",
+ "team_number": 9971,
+ "intrinsics": [
+ 392.045471,
+ 0.0,
+ 299.976349,
+ 0.0,
+ 391.858948,
+ 240.437485,
+ 0.0,
+ 0.0,
+ 1.0
+ ],
+ "dist_coeffs": [
+ 0.140083,
+ -0.243777,
+ -0.000455,
+ 0.00004,
+ 0.084187
+ ],
+ "calibration_timestamp": 1643352981541962185,
+ "camera_id": "21-02"
+}
diff --git a/y2022/vision/calib_files/calibration_pi-9971-3_cam-21-03_2022-01-28_06-40-19.061887367.json b/y2022/vision/calib_files/calibration_pi-9971-3_cam-21-03_2022-01-28_06-40-19.061887367.json
new file mode 100755
index 0000000..22ad253
--- /dev/null
+++ b/y2022/vision/calib_files/calibration_pi-9971-3_cam-21-03_2022-01-28_06-40-19.061887367.json
@@ -0,0 +1,24 @@
+{
+ "node_name": "pi3",
+ "team_number": 9971,
+ "intrinsics": [
+ 390.202698,
+ 0.0,
+ 323.895874,
+ 0.0,
+ 390.134705,
+ 240.809784,
+ 0.0,
+ 0.0,
+ 1.0
+ ],
+ "dist_coeffs": [
+ 0.128804,
+ -0.215377,
+ -0.000073,
+ -0.000593,
+ 0.062635
+ ],
+ "calibration_timestamp": 1643352019061887367,
+ "camera_id": "21-03"
+}
diff --git a/y2022/vision/calib_files/calibration_pi-971-4_cam-22-04_2022-01-28_05-26-43.135661745.json b/y2022/vision/calib_files/calibration_pi-9971-4_cam-22-04_2022-01-28_05-26-43.135661745.json
similarity index 92%
rename from y2022/vision/calib_files/calibration_pi-971-4_cam-22-04_2022-01-28_05-26-43.135661745.json
rename to y2022/vision/calib_files/calibration_pi-9971-4_cam-22-04_2022-01-28_05-26-43.135661745.json
index cb0c66d..5466224 100755
--- a/y2022/vision/calib_files/calibration_pi-971-4_cam-22-04_2022-01-28_05-26-43.135661745.json
+++ b/y2022/vision/calib_files/calibration_pi-9971-4_cam-22-04_2022-01-28_05-26-43.135661745.json
@@ -1,6 +1,6 @@
{
"node_name": "pi4",
- "team_number": 971,
+ "team_number": 9971,
"intrinsics": [
386.619232,
0.0,
diff --git a/y2022/vision/calib_files/calibration_pi-9971-5_cam-21-05_2022-01-28_06-35-06.089551572.json b/y2022/vision/calib_files/calibration_pi-9971-5_cam-21-05_2022-01-28_06-35-06.089551572.json
new file mode 100755
index 0000000..d089faa
--- /dev/null
+++ b/y2022/vision/calib_files/calibration_pi-9971-5_cam-21-05_2022-01-28_06-35-06.089551572.json
@@ -0,0 +1,24 @@
+{
+ "node_name": "pi5",
+ "team_number": 9971,
+ "intrinsics": [
+ 393.222015,
+ 0.0,
+ 283.454346,
+ 0.0,
+ 393.15332,
+ 257.545563,
+ 0.0,
+ 0.0,
+ 1.0
+ ],
+ "dist_coeffs": [
+ 0.145289,
+ -0.26649,
+ 0.000127,
+ 0.000057,
+ 0.106988
+ ],
+ "calibration_timestamp": 1643351706089551572,
+ "camera_id": "21-05"
+}
diff --git a/y2022/vision/calib_files/calibration_pi-9971-6_cam-21-06_2022-01-28_06-34-47.705289435.json b/y2022/vision/calib_files/calibration_pi-9971-6_cam-21-06_2022-01-28_06-34-47.705289435.json
new file mode 100644
index 0000000..bb08ce2
--- /dev/null
+++ b/y2022/vision/calib_files/calibration_pi-9971-6_cam-21-06_2022-01-28_06-34-47.705289435.json
@@ -0,0 +1,24 @@
+{
+ "node_name": "pi6",
+ "team_number": 9971,
+ "intrinsics": [
+ 392.598694,
+ 0.0,
+ 338.288269,
+ 0.0,
+ 392.689697,
+ 244.718338,
+ 0.0,
+ 0.0,
+ 1.0
+ ],
+ "dist_coeffs": [
+ 0.138265,
+ -0.232579,
+ -0.000478,
+ 0.000089,
+ 0.075434
+ ],
+ "calibration_timestamp": 1643351687705289435,
+ "camera_id": "21-06"
+}
diff --git a/y2022/vision/calib_files/calibration_pi-9971-1_cam-22-07_2022-02-16_21-20-00.000000000.json b/y2022/vision/calib_files/calibration_pi-9971-7_cam-22-07_2022-02-16_21-20-00.000000000.json
similarity index 93%
rename from y2022/vision/calib_files/calibration_pi-9971-1_cam-22-07_2022-02-16_21-20-00.000000000.json
rename to y2022/vision/calib_files/calibration_pi-9971-7_cam-22-07_2022-02-16_21-20-00.000000000.json
index 35efa45..6dbd3b2 100755
--- a/y2022/vision/calib_files/calibration_pi-9971-1_cam-22-07_2022-02-16_21-20-00.000000000.json
+++ b/y2022/vision/calib_files/calibration_pi-9971-7_cam-22-07_2022-02-16_21-20-00.000000000.json
@@ -1,5 +1,5 @@
{
- "node_name": "pi1",
+ "node_name": "pi7",
"team_number": 9971,
"intrinsics": [
388.062378,
diff --git a/y2022/vision/camera_definition.py b/y2022/vision/camera_definition.py
index 7ad2c0b..68549a4 100644
--- a/y2022/vision/camera_definition.py
+++ b/y2022/vision/camera_definition.py
@@ -76,8 +76,8 @@
0]])
if is_turret:
- # Turret camera has default heading 180 deg away from the robot x
- base_cam_ext.R = np.array([[-1.0, 0.0, 0.0], [0.0, -1.0, 0.0],
+ # Turret is just an identity matrix in the middle of the robot.
+ base_cam_ext.R = np.array([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0],
[0.0, 0.0, 1.0]])
base_cam_ext.T = np.array([0.0, 0.0, 0.0])
turret_cam_ext.R = camera_yaw_matrix @ camera_pitch_matrix @ robot_to_camera_rotation
@@ -91,24 +91,25 @@
def compute_extrinsic_by_pi(pi_number):
- # Defaults for non-turret camera
- camera_pitch = -20.0 * np.pi / 180.0
+ # Defaults for all cameras
+ camera_pitch = -35.0 * np.pi / 180.0
camera_yaw = 0.0
- is_turret = False
+ is_turret = True
# Default camera location to robot origin
T = np.array([0.0, 0.0, 0.0])
if pi_number == "pi1":
- camera_pitch = -35.0 * np.pi / 180.0
- T = np.array([0.0, 0.0, 37.0 * 0.0254])
- elif pi_number == "pi2":
- T = np.array([4.5 * 0.0254, 3.75 * 0.0254, 26.0 * 0.0254])
- elif pi_number == "pi3":
camera_yaw = 90.0 * np.pi / 180.0
- T = np.array([-2.75 * 0.0254, 5.25 * 0.0254, 25.25 * 0.0254])
+ T = np.array([-7.0 * 0.0254, 3.5 * 0.0254, 32.0 * 0.0254])
+ elif pi_number == "pi2":
+ camera_yaw = 0.0
+ T = np.array([-7.0 * 0.0254, -3.0 * 0.0254, 34.0 * 0.0254])
+ elif pi_number == "pi3":
+ camera_yaw = 180.0 * np.pi / 180.0
+ T = np.array([-1.0 * 0.0254, 8.5 * 0.0254, 34.0 * 0.0254])
elif pi_number == "pi4":
camera_yaw = -90.0 * np.pi / 180.0
- T = np.array([-2.75 * 0.0254, -6 * 0.0254, 26 * 0.0254])
+ T = np.array([-9.0 * 0.0254, -5 * 0.0254, 27.5 * 0.0254])
return compute_extrinsic(camera_pitch, camera_yaw, T, is_turret)
diff --git a/y2022/vision/camera_reader.cc b/y2022/vision/camera_reader.cc
index f1cb4e8..94b2d96 100644
--- a/y2022/vision/camera_reader.cc
+++ b/y2022/vision/camera_reader.cc
@@ -211,6 +211,26 @@
reader_->SendLatestImage();
read_image_timer_->Setup(event_loop_->monotonic_now());
+
+ // Disable the LEDs based on localizer output
+ if (localizer_output_fetcher_.Fetch()) {
+ const auto node_name = event_loop_->node()->name()->string_view();
+ const size_t pi_number =
+ std::atol(node_name.substr(node_name.size() - 1).data());
+
+ CHECK(localizer_output_fetcher_->has_led_outputs() &&
+ localizer_output_fetcher_->led_outputs()->size() > pi_number);
+
+ const LedOutput led_output =
+ localizer_output_fetcher_->led_outputs()->Get(pi_number);
+
+ if (led_output != prev_led_output_) {
+ gpio_disable_control_.GPIOWrite(led_output == LedOutput::OFF ? kGPIOHigh
+ : kGPIOLow);
+
+ prev_led_output_ = led_output;
+ }
+ }
}
} // namespace vision
diff --git a/y2022/vision/camera_reader.h b/y2022/vision/camera_reader.h
index edc3a1a..707e04e 100644
--- a/y2022/vision/camera_reader.h
+++ b/y2022/vision/camera_reader.h
@@ -13,6 +13,7 @@
#include "aos/network/team_number.h"
#include "frc971/vision/v4l2_reader.h"
#include "frc971/vision/vision_generated.h"
+#include "y2022/localizer/localizer_output_generated.h"
#include "y2022/vision/calibration_data.h"
#include "y2022/vision/calibration_generated.h"
#include "y2022/vision/gpio.h"
@@ -23,6 +24,7 @@
namespace vision {
using namespace frc971::vision;
+using frc971::controls::LedOutput;
// TODO<jim>: Probably need to break out LED control to separate process
class CameraReader {
@@ -38,6 +40,9 @@
target_estimator_(CameraIntrinsics(), CameraExtrinsics()),
target_estimate_sender_(
event_loop->MakeSender<TargetEstimate>("/camera")),
+ localizer_output_fetcher_(
+ event_loop->MakeFetcher<frc971::controls::LocalizerOutput>(
+ "/localizer")),
read_image_timer_(event_loop->AddTimer([this]() { ReadImage(); })),
gpio_imu_pin_(GPIOControl(GPIO_PIN_SCLK_IMU, kGPIOIn)),
gpio_pwm_control_(GPIOPWMControl(GPIO_PIN_SCK_PWM, duty_cycle_)),
@@ -99,6 +104,9 @@
TargetEstimator target_estimator_;
aos::Sender<TargetEstimate> target_estimate_sender_;
+ LedOutput prev_led_output_ = LedOutput::ON;
+ aos::Fetcher<frc971::controls::LocalizerOutput> localizer_output_fetcher_;
+
// We schedule this immediately to read an image. Having it on a timer
// means other things can run on the event loop in between.
aos::TimerHandler *const read_image_timer_;
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/vision/target_estimator.cc b/y2022/vision/target_estimator.cc
index 130759c..b495b23 100644
--- a/y2022/vision/target_estimator.cc
+++ b/y2022/vision/target_estimator.cc
@@ -440,11 +440,11 @@
double angle_to_camera, double roll, double pitch,
double yaw, double confidence, cv::Mat view_image) {
constexpr int kTextX = 10;
- int text_y = 250;
- constexpr int kTextSpacing = 35;
+ int text_y = 0;
+ constexpr int kTextSpacing = 25;
const auto kTextColor = cv::Scalar(0, 255, 255);
- constexpr double kFontScale = 1.0;
+ constexpr double kFontScale = 0.6;
cv::putText(view_image, absl::StrFormat("Distance: %.3f", distance),
cv::Point(kTextX, text_y += kTextSpacing),
diff --git a/y2022/vision/viewer.cc b/y2022/vision/viewer.cc
index 24c8fc6..f847dbd 100644
--- a/y2022/vision/viewer.cc
+++ b/y2022/vision/viewer.cc
@@ -212,7 +212,7 @@
TargetEstimator estimator(intrinsics, extrinsics);
- for (auto it = file_list.begin() + FLAGS_skip; it != file_list.end(); it++) {
+ for (auto it = file_list.begin() + FLAGS_skip; it < file_list.end(); it++) {
LOG(INFO) << "Reading file " << *it;
cv::Mat image_mat = cv::imread(it->c_str());
BlobDetector::BlobResult blob_result;
diff --git a/y2022/vision/viewer_replay.cc b/y2022/vision/viewer_replay.cc
new file mode 100644
index 0000000..c1bf88b
--- /dev/null
+++ b/y2022/vision/viewer_replay.cc
@@ -0,0 +1,81 @@
+#include "aos/events/logging/log_reader.h"
+#include "aos/events/simulated_event_loop.h"
+#include "aos/init.h"
+#include "frc971/vision/vision_generated.h"
+#include "opencv2/calib3d.hpp"
+#include "opencv2/features2d.hpp"
+#include "opencv2/highgui/highgui.hpp"
+#include "opencv2/imgproc.hpp"
+#include "y2022/vision/blob_detector.h"
+
+DEFINE_string(node, "pi1", "Node name to replay.");
+DEFINE_string(image_save_prefix, "/tmp/img",
+ "Prefix to use for saving images from the logfile.");
+DEFINE_bool(display, false, "If true, display the images with a timeout.");
+DEFINE_bool(detected_only, false,
+ "If true, only write images which had blobs (unfiltered) detected");
+DEFINE_bool(filtered_only, false,
+ "If true, only write images which had blobs (filtered) detected");
+
+namespace y2022 {
+namespace vision {
+namespace {
+
+void ViewerMain(int argc, char *argv[]) {
+ std::vector<std::string> unsorted_logfiles =
+ aos::logger::FindLogs(argc, argv);
+
+ // Open logfiles
+ aos::logger::LogReader reader(aos::logger::SortParts(unsorted_logfiles));
+ reader.Register();
+ const aos::Node *node = nullptr;
+ if (aos::configuration::MultiNode(reader.configuration())) {
+ node = aos::configuration::GetNode(reader.configuration(), FLAGS_node);
+ }
+ std::unique_ptr<aos::EventLoop> event_loop =
+ reader.event_loop_factory()->MakeEventLoop("player", node);
+
+ int image_count = 0;
+ event_loop->MakeWatcher(
+ "/camera/decimated",
+ [&image_count](const frc971::vision::CameraImage &image) {
+ // Create color image:
+ cv::Mat image_color_mat(cv::Size(image.cols(), image.rows()), CV_8UC2,
+ (void *)image.data()->data());
+ cv::Mat image_mat(cv::Size(image.cols(), image.rows()), CV_8UC3);
+ cv::cvtColor(image_color_mat, image_mat, cv::COLOR_YUV2BGR_YUYV);
+
+ bool use_image = true;
+ if (FLAGS_detected_only || FLAGS_filtered_only) {
+ BlobDetector::BlobResult blob_result;
+ BlobDetector::ExtractBlobs(image_mat, &blob_result);
+
+ use_image =
+ ((FLAGS_filtered_only ? blob_result.filtered_blobs.size()
+ : blob_result.unfiltered_blobs.size()) > 0);
+ }
+ if (use_image) {
+ if (!FLAGS_image_save_prefix.empty()) {
+ cv::imwrite(FLAGS_image_save_prefix +
+ std::to_string(image_count++) + ".png",
+ image_mat);
+ }
+ if (FLAGS_display) {
+ cv::imshow("Display", image_mat);
+ cv::waitKey(FLAGS_detected_only || FLAGS_filtered_only ? 10 : 1);
+ }
+ }
+ });
+
+ reader.event_loop_factory()->Run();
+}
+
+} // namespace
+} // namespace vision
+} // namespace y2022
+
+// Quick and lightweight viewer for image logs
+int main(int argc, char **argv) {
+ aos::InitGoogle(&argc, &argv);
+ y2022::vision::ViewerMain(argc, argv);
+}
diff --git a/y2022/wpilib_interface.cc b/y2022/wpilib_interface.cc
index a1b1202..f23d3e2 100644
--- a/y2022/wpilib_interface.cc
+++ b/y2022/wpilib_interface.cc
@@ -79,7 +79,7 @@
double climber_pot_translate(double voltage) {
return voltage * Values::kClimberPotRatio() *
- (10.0 /*turns*/ / 5.0 /*volts*/) *
+ (5.0 /*turns*/ / 5.0 /*volts*/) *
Values::kClimberPotMetersPerRevolution();
}
@@ -510,7 +510,7 @@
}
void Write(const superstructure::Output &output) override {
- WritePwm(output.climber_voltage(), climber_falcon_.get());
+ WritePwm(-output.climber_voltage(), climber_falcon_.get());
WritePwm(output.intake_voltage_front(), intake_falcon_front_.get());
WritePwm(output.intake_voltage_back(), intake_falcon_back_.get());
@@ -701,13 +701,6 @@
AddLoop(&output_event_loop);
- // Thread 5
- ::aos::ShmEventLoop can_sensor_reader_event_loop(&config.message());
- CANSensorReader can_sensor_reader(&can_sensor_reader_event_loop);
- can_sensor_reader.set_flipper_arms_falcon(
- superstructure_writer.flipper_arms_falcon());
- AddLoop(&can_sensor_reader_event_loop);
-
RunLoops();
}
};
diff --git a/y2022/y2022_imu.json b/y2022/y2022_imu.json
index 3d97c3c..06a7955 100644
--- a/y2022/y2022_imu.json
+++ b/y2022/y2022_imu.json
@@ -223,22 +223,7 @@
"type": "frc971.controls.LocalizerStatus",
"source_node": "imu",
"frequency": 2200,
- "max_size": 2000,
- "logger": "LOCAL_AND_REMOTE_LOGGER",
- "logger_nodes": [
- "logger"
- ],
- "destination_nodes": [
- {
- "name": "logger",
- "priority": 5,
- "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
- "timestamp_logger_nodes": [
- "imu"
- ],
- "time_to_live": 5000000
- }
- ]
+ "max_size": 2000
},
{
"name": "/imu/aos/remote_timestamps/logger/localizer/frc971-controls-LocalizerStatus",
@@ -374,6 +359,8 @@
{
"name": "localizer",
"executable_name": "localizer_main",
+ /* TODO(james): Remove this once confident in the accelerometer code. */
+ "args": ["--ignore_accelerometer"],
"nodes": [
"imu"
]
@@ -395,7 +382,7 @@
{
"name": "localizer_logger",
"executable_name": "logger_main",
- "args": ["--snappy_compress"],
+ "args": ["--logging_folder", "", "--snappy_compress"],
"nodes": [
"imu"
]
diff --git a/y2022/y2022_logger.json b/y2022/y2022_logger.json
index 2407b9e..b72abeb 100644
--- a/y2022/y2022_logger.json
+++ b/y2022/y2022_logger.json
@@ -35,6 +35,22 @@
]
},
{
+ "name": "/drivetrain",
+ "type": "frc971.control_loops.drivetrain.Output",
+ "source_node": "roborio",
+ "logger": "LOCAL_AND_REMOTE_LOGGER",
+ "logger_nodes": [
+ "logger"
+ ],
+ "destination_nodes": [
+ {
+ "name": "logger",
+ "priority": 2,
+ "time_to_live": 500000000
+ }
+ ]
+ },
+ {
"name": "/pi1/aos",
"type": "aos.message_bridge.Timestamp",
"source_node": "pi1",
@@ -306,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",
@@ -322,7 +338,7 @@
]
},
{
- "name": "/pi2/camera",
+ "name": "/pi2/camera/decimated",
"type": "frc971.vision.CameraImage",
"source_node": "pi2",
"logger": "LOCAL_AND_REMOTE_LOGGER",
@@ -333,12 +349,12 @@
{
"name": "logger",
"priority": 3,
- "time_to_live": 5000000
+ "time_to_live": 500000000
}
]
},
{
- "name": "/pi3/camera",
+ "name": "/pi3/camera/decimated",
"type": "frc971.vision.CameraImage",
"source_node": "pi3",
"logger": "LOCAL_AND_REMOTE_LOGGER",
@@ -354,7 +370,7 @@
]
},
{
- "name": "/pi4/camera",
+ "name": "/pi4/camera/decimated",
"type": "frc971.vision.CameraImage",
"source_node": "pi4",
"logger": "LOCAL_AND_REMOTE_LOGGER",
@@ -383,16 +399,17 @@
],
"applications": [
{
- "name": "message_bridge_client",
+ "name": "logger_message_bridge_client",
"executable_name": "message_bridge_client",
- "args": ["--rmem=8388608"],
+ "args": ["--rmem=8388608", "--rt_priority=16"],
"nodes": [
"logger"
]
},
{
- "name": "message_bridge_server",
+ "name": "logger_message_bridge_server",
"executable_name": "message_bridge_server",
+ "args": ["--rt_priority=16"],
"nodes": [
"logger"
]
diff --git a/y2022/y2022_pi_template.json b/y2022/y2022_pi_template.json
index 7f49e41..3dff81e 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 }}",
@@ -200,6 +208,18 @@
"max_size": 208
},
{
+ "name": "/localizer",
+ "type": "frc971.controls.LocalizerOutput",
+ "source_node": "imu",
+ "destination_nodes": [
+ {
+ "name": "pi{{ NUM }}",
+ "priority": 5,
+ "time_to_live": 5000000
+ }
+ ]
+ },
+ {
"name": "/logger/aos",
"type": "aos.starter.StarterRpc",
"source_node": "logger",
@@ -328,6 +348,13 @@
"nodes": [
"pi{{ NUM }}"
]
+ },
+ {
+ "name": "image_decimator",
+ "executable_name": "image_decimator",
+ "nodes": [
+ "pi{{ NUM }}"
+ ]
}
],
"maps": [
diff --git a/y2022/y2022_roborio.json b/y2022/y2022_roborio.json
index b16f0cb..e342094 100644
--- a/y2022/y2022_roborio.json
+++ b/y2022/y2022_roborio.json
@@ -211,13 +211,19 @@
"num_senders": 2,
"logger": "LOCAL_AND_REMOTE_LOGGER",
"logger_nodes": [
- "imu"
+ "imu",
+ "logger"
],
"destination_nodes": [
{
"name": "imu",
"priority": 5,
- "time_to_live": 5000000
+ "time_to_live": 50000000
+ },
+ {
+ "name": "logger",
+ "priority": 5,
+ "time_to_live": 50000000
}
]
},