Merge "Add parsed driver ranking scores to the scouting database"
diff --git a/aos/actions/actions.h b/aos/actions/actions.h
index bda29cc..1b5d607 100644
--- a/aos/actions/actions.h
+++ b/aos/actions/actions.h
@@ -25,6 +25,8 @@
class ActionQueue {
public:
// Queues up an action for sending.
+ // TODO(james): Allow providing something other than a unique_ptr to avoid
+ // malloc's.
void EnqueueAction(::std::unique_ptr<Action> action);
// Cancels the current action, and runs the next one after the current one has
diff --git a/frc971/control_loops/drivetrain/line_follow_drivetrain.cc b/frc971/control_loops/drivetrain/line_follow_drivetrain.cc
index f231b47..101c0f7 100644
--- a/frc971/control_loops/drivetrain/line_follow_drivetrain.cc
+++ b/frc971/control_loops/drivetrain/line_follow_drivetrain.cc
@@ -165,18 +165,21 @@
const ::Eigen::Matrix<double, 5, 1> &abs_state, double relative_y_offset,
double velocity_sign) {
// Calculates the goal angle for the drivetrain given our position.
- // The calculated goal will be such that a point disc_rad to one side of the
- // drivetrain (the side depends on where we approach from) will end up hitting
- // the plane of the target exactly disc_rad from the center of the target.
- // This allows us to better approach targets in the 2019 game from an
- // angle--radii of zero imply driving straight in.
- const double disc_rad = target_selector_->TargetRadius();
+ // The calculated goal will be such that a point piece_rad to one side of the
+ // drivetrain (the side depends on where we approach from and SignedRadii())
+ // will end up hitting the plane of the target exactly target_rad from the
+ // center of the target. This allows us to better approach targets in the 2019
+ // game from an angle--radii of zero imply driving straight in.
+ const double target_rad = target_selector_->TargetRadius();
+ const double piece_rad = target_selector_->GamePieceRadius();
// Depending on whether we are to the right or left of the target, we work off
// of a different side of the robot.
- const double edge_sign = relative_y_offset > 0 ? 1.0 : -1.0;
+ const double edge_sign = target_selector_->SignedRadii()
+ ? 1.0
+ : (relative_y_offset > 0 ? 1.0 : -1.0);
// Note side_adjust which is the input from the driver's wheel to allow
// shifting the goal target left/right.
- const double edge_offset = edge_sign * disc_rad - side_adjust_;
+ const double edge_offset = edge_sign * target_rad - side_adjust_;
// The point that we are trying to get the disc to hit.
const Pose corner = Pose(&target_pose_, {0.0, edge_offset, 0.0}, 0.0);
// A pose for the current robot position that is square to the target.
@@ -185,14 +188,17 @@
// To prevent numerical issues, we limit x so that when the localizer isn't
// working properly and ends up driving past the target, we still get sane
// results.
- square_robot.mutable_pos()->x() =
- ::std::min(::std::min(square_robot.mutable_pos()->x(), -disc_rad), -0.01);
+ // The min() with -piece_rad ensures that we past well-conditioned numbers
+ // to acos() (we must have piece_rad <= dist_to_corner); the min with -0.01
+ // ensures that dist_to_corner doesn't become near zero.
+ square_robot.mutable_pos()->x() = ::std::min(
+ ::std::min(square_robot.mutable_pos()->x(), -std::abs(piece_rad)), -0.01);
// Distance from the edge of the disc on the robot to the velcro we want to
// hit on the target.
const double dist_to_corner = square_robot.xy_norm();
// The following actually handles calculating the heading we need the robot to
// take (relative to the plane of the target).
- const double alpha = ::std::acos(disc_rad / dist_to_corner);
+ const double alpha = ::std::acos(piece_rad / dist_to_corner);
const double heading_to_robot = edge_sign * square_robot.heading();
double theta = -edge_sign * (M_PI - alpha - (heading_to_robot - M_PI_2));
if (velocity_sign < 0) {
diff --git a/frc971/control_loops/drivetrain/line_follow_drivetrain_test.cc b/frc971/control_loops/drivetrain/line_follow_drivetrain_test.cc
index 3ea0b0b..5fe6ec3 100644
--- a/frc971/control_loops/drivetrain/line_follow_drivetrain_test.cc
+++ b/frc971/control_loops/drivetrain/line_follow_drivetrain_test.cc
@@ -190,6 +190,7 @@
for (double v : {-3.0, -2.0, -1.0, 0.0, 1.0, 2.0, 3.0}) {
for (double throttle : {-1.0, 1.0}) {
target_selector_.set_target_radius(0.0);
+ target_selector_.set_game_piece_radius(0.0);
const double zero_rad_theta = GoalTheta(x, y, v, throttle);
EXPECT_NEAR(
0.0,
@@ -198,6 +199,7 @@
zero_rad_theta),
1e-14);
target_selector_.set_target_radius(0.05);
+ target_selector_.set_game_piece_radius(0.05);
const double small_rad_theta = GoalTheta(x, y, v, throttle);
if (y > 0) {
EXPECT_LT(small_rad_theta, zero_rad_theta);
diff --git a/frc971/control_loops/drivetrain/localizer.h b/frc971/control_loops/drivetrain/localizer.h
index 6e53138..867b3c0 100644
--- a/frc971/control_loops/drivetrain/localizer.h
+++ b/frc971/control_loops/drivetrain/localizer.h
@@ -29,11 +29,26 @@
// Gets the current target pose. Should only be called if UpdateSelection has
// returned true.
virtual TypedPose<double> TargetPose() const = 0;
+ // For the "radii" below, we have two possible modes:
+ // 1) Akin to 2019, we can place with either edge of the game piece, so
+ // the line following code will have to automatically detect which edge
+ // (right or left) to aim to have intersect the target.
+ // 2) As in 2023, the game piece itself is offset in the robot and so we care
+ // which of left vs. right we are using.
+ // In situation (1), SignedRadii() should return false and the *Radius()
+ // functions should return a non-negative number (technically I think the
+ // math may work for negative numbers, but may have weird implications
+ // physically...). For (2) SignedRadii()
+ // should return true and the sign of the *Radius() functions will be
+ // respected by the line following code.
+ virtual bool SignedRadii() const = 0;
// The "radius" of the target--for y2019, we wanted to drive in so that a disc
// with radius r would hit the plane of the target at an offset of exactly r
// from the TargetPose--this is distinct from wanting the center of the
// robot to project straight onto the center of the target.
virtual double TargetRadius() const = 0;
+ // the "radius" of the robot/game piece to place.
+ virtual double GamePieceRadius() const = 0;
// Which direction we want the robot to drive to get to the target.
virtual Side DriveDirection() const = 0;
// Indicates that the line following *must* drive to the currently selected
@@ -120,12 +135,15 @@
return has_target_;
}
TypedPose<double> TargetPose() const override { return pose_; }
+ bool SignedRadii() const override { return signed_radii_; }
double TargetRadius() const override { return target_radius_; }
+ double GamePieceRadius() const override { return game_piece_radius_; }
Side DriveDirection() const override { return drive_direction_; }
bool ForceReselectTarget() const override { return force_reselect_; }
void set_pose(const TypedPose<double> &pose) { pose_ = pose; }
void set_target_radius(double radius) { target_radius_ = radius; }
+ void set_game_piece_radius(double radius) { game_piece_radius_ = radius; }
void set_has_target(bool has_target) { has_target_ = has_target; }
void set_drive_direction(Side side) { drive_direction_ = side; }
void set_force_reselect(bool force_reselect) {
@@ -137,7 +155,9 @@
bool has_target_ = true;
bool force_reselect_ = false;
TypedPose<double> pose_;
+ bool signed_radii_ = false;
double target_radius_ = 0.0;
+ double game_piece_radius_ = 0.0;
Side drive_direction_ = Side::DONT_CARE;
};
diff --git a/scouting/www/BUILD b/scouting/www/BUILD
index 042a9c3..4c2f931 100644
--- a/scouting/www/BUILD
+++ b/scouting/www/BUILD
@@ -15,12 +15,12 @@
],
deps = [
"//:node_modules/@angular/animations",
- "//scouting/www/driver_ranking:_lib",
- "//scouting/www/entry:_lib",
- "//scouting/www/match_list:_lib",
- "//scouting/www/notes:_lib",
- "//scouting/www/shift_schedule:_lib",
- "//scouting/www/view:_lib",
+ "//scouting/www/driver_ranking",
+ "//scouting/www/entry",
+ "//scouting/www/match_list",
+ "//scouting/www/notes",
+ "//scouting/www/shift_schedule",
+ "//scouting/www/view",
],
)
diff --git a/scouting/www/match_list/BUILD b/scouting/www/match_list/BUILD
index c713dda..b2128db 100644
--- a/scouting/www/match_list/BUILD
+++ b/scouting/www/match_list/BUILD
@@ -13,7 +13,7 @@
"//scouting/webserver/requests/messages:error_response_ts_fbs",
"//scouting/webserver/requests/messages:request_all_matches_response_ts_fbs",
"//scouting/webserver/requests/messages:request_all_matches_ts_fbs",
- "//scouting/www/rpc:_lib",
+ "//scouting/www/rpc",
"@com_github_google_flatbuffers//ts:flatbuffers_ts",
],
)
diff --git a/scouting/www/view/BUILD b/scouting/www/view/BUILD
index 168feb6..25aa9fa 100644
--- a/scouting/www/view/BUILD
+++ b/scouting/www/view/BUILD
@@ -17,7 +17,7 @@
"//scouting/webserver/requests/messages:request_all_notes_ts_fbs",
"//scouting/webserver/requests/messages:request_data_scouting_response_ts_fbs",
"//scouting/webserver/requests/messages:request_data_scouting_ts_fbs",
- "//scouting/www/rpc:_lib",
+ "//scouting/www/rpc",
"@com_github_google_flatbuffers//ts:flatbuffers_ts",
],
)
diff --git a/tools/build_rules/js.bzl b/tools/build_rules/js.bzl
index ceb67aa..eb95510 100644
--- a/tools/build_rules/js.bzl
+++ b/tools/build_rules/js.bzl
@@ -1,7 +1,5 @@
load("@aspect_rules_js//js:providers.bzl", "JsInfo")
load("@bazel_skylib//rules:write_file.bzl", "write_file")
-load("@aspect_rules_js//js:defs.bzl", "js_library")
-load("@aspect_rules_js//npm:defs.bzl", "npm_package")
load("@aspect_bazel_lib//lib:copy_to_directory.bzl", "copy_to_directory")
load("@aspect_bazel_lib//lib:copy_file.bzl", "copy_file")
load("@aspect_rules_esbuild//esbuild:defs.bzl", "esbuild")
@@ -271,7 +269,7 @@
srcs.append(":_public_api")
ng_project(
- name = "_lib",
+ name = name,
srcs = srcs + [":_index"],
deps = deps + PACKAGE_DEPS,
#visibility = ["//visibility:private"],
@@ -279,20 +277,6 @@
**kwargs
)
- js_library(
- name = name + "_js",
- srcs = [":_lib"],
- visibility = ["//visibility:public"],
- )
-
- npm_package(
- name = name,
- srcs = ["package.json", ":_lib"],
- # This is a perf improvement; the default will be flipped to False in rules_js 2.0
- include_runfiles = False,
- visibility = ["//visibility:public"],
- )
-
def rollup_bundle(name, entry_point, deps = [], visibility = None, **kwargs):
"""Calls the upstream rollup_bundle() and exposes a .min.js file.
diff --git a/y2019/control_loops/drivetrain/target_selector.h b/y2019/control_loops/drivetrain/target_selector.h
index 1fdd691..b8d89c7 100644
--- a/y2019/control_loops/drivetrain/target_selector.h
+++ b/y2019/control_loops/drivetrain/target_selector.h
@@ -40,6 +40,8 @@
Pose TargetPose() const override { return target_pose_; }
double TargetRadius() const override { return target_radius_; }
+ double GamePieceRadius() const override { return target_radius_; }
+ bool SignedRadii() const override { return false; }
Side DriveDirection() const override { return Side::DONT_CARE; }
diff --git a/y2023/BUILD b/y2023/BUILD
index 7121809..8108a39 100644
--- a/y2023/BUILD
+++ b/y2023/BUILD
@@ -28,11 +28,13 @@
],
dirs = [
"//y2023/www:www_files",
+ "//y2023/autonomous:splines",
],
start_binaries = [
"//aos/events/logging:logger_main",
"//aos/network:web_proxy_main",
"//aos/starter:irq_affinity",
+ "//y2023/autonomous:binaries",
":joystick_reader",
":wpilib_interface",
"//aos/network:message_bridge_client",
diff --git a/y2023/autonomous/auto_splines.cc b/y2023/autonomous/auto_splines.cc
index 07250e4..0cc98a2 100644
--- a/y2023/autonomous/auto_splines.cc
+++ b/y2023/autonomous/auto_splines.cc
@@ -1,26 +1,37 @@
#include "y2023/autonomous/auto_splines.h"
#include "frc971/control_loops/control_loops_generated.h"
+#include "aos/flatbuffer_merge.h"
namespace y2023 {
namespace actors {
-void MaybeFlipSpline(
- aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder,
- flatbuffers::Offset<flatbuffers::Vector<float>> spline_y_offset,
- bool is_left) {
- flatbuffers::Vector<float> *spline_y =
- GetMutableTemporaryPointer(*builder->fbb(), spline_y_offset);
+namespace {
+flatbuffers::Offset<frc971::MultiSpline> FixSpline(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ flatbuffers::Offset<frc971::MultiSpline> spline_offset,
+ aos::Alliance alliance) {
+ frc971::MultiSpline *spline =
+ GetMutableTemporaryPointer(*builder->fbb(), spline_offset);
+ flatbuffers::Vector<float> *spline_x = spline->mutable_spline_x();
- if (!is_left) {
- for (size_t i = 0; i < spline_y->size(); i++) {
- spline_y->Mutate(i, -spline_y->Get(i));
+ // For 2023: The field is mirrored across the center line, and is not
+ // rotationally symmetric. As such, we only flip the X coordinates when
+ // changing side of the field.
+ if (alliance == aos::Alliance::kBlue) {
+ for (size_t ii = 0; ii < spline_x->size(); ++ii) {
+ spline_x->Mutate(ii, -spline_x->Get(ii));
}
}
+ return spline_offset;
}
+} // namespace
flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::BasicSSpline(
- aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder) {
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ aos::Alliance alliance) {
flatbuffers::Offset<frc971::Constraint> longitudinal_constraint_offset;
flatbuffers::Offset<frc971::Constraint> lateral_constraint_offset;
flatbuffers::Offset<frc971::Constraint> voltage_constraint_offset;
@@ -78,11 +89,13 @@
multispline_builder.add_spline_x(spline_x_offset);
multispline_builder.add_spline_y(spline_y_offset);
- return multispline_builder.Finish();
+ return FixSpline(builder, multispline_builder.Finish(), alliance);
}
flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::StraightLine(
- aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder) {
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ aos::Alliance alliance) {
flatbuffers::Offset<flatbuffers::Vector<float>> spline_x_offset =
builder->fbb()->CreateVector<float>(
{-12.3, -11.9, -11.5, -11.1, -10.6, -10.0});
@@ -96,7 +109,17 @@
multispline_builder.add_spline_x(spline_x_offset);
multispline_builder.add_spline_y(spline_y_offset);
- return multispline_builder.Finish();
+ return FixSpline(builder, multispline_builder.Finish(), alliance);
+}
+
+flatbuffers::Offset<frc971::MultiSpline> AutonomousSplines::TestSpline(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ aos::Alliance alliance) {
+ return FixSpline(
+ builder,
+ aos::CopyFlatBuffer<frc971::MultiSpline>(test_spline_, builder->fbb()),
+ alliance);
}
} // namespace actors
diff --git a/y2023/autonomous/auto_splines.h b/y2023/autonomous/auto_splines.h
index 68795e6..1280693 100644
--- a/y2023/autonomous/auto_splines.h
+++ b/y2023/autonomous/auto_splines.h
@@ -3,6 +3,7 @@
#include "aos/events/event_loop.h"
#include "frc971/control_loops/control_loops_generated.h"
+#include "frc971/input/joystick_state_generated.h"
#include "frc971/control_loops/drivetrain/drivetrain_goal_generated.h"
/*
@@ -16,10 +17,24 @@
class AutonomousSplines {
public:
+ AutonomousSplines()
+ : test_spline_(aos::JsonFileToFlatbuffer<frc971::MultiSpline>(
+ "splines/test_spline.json")) {}
static flatbuffers::Offset<frc971::MultiSpline> BasicSSpline(
- aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder);
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ aos::Alliance alliance);
static flatbuffers::Offset<frc971::MultiSpline> StraightLine(
- aos::Sender<frc971::control_loops::drivetrain::Goal>::Builder *builder);
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ aos::Alliance alliance);
+
+ flatbuffers::Offset<frc971::MultiSpline> TestSpline(
+ aos::Sender<frc971::control_loops::drivetrain::SplineGoal>::Builder
+ *builder,
+ aos::Alliance alliance);
+ private:
+ aos::FlatbufferDetachedBuffer<frc971::MultiSpline> test_spline_;
};
} // namespace actors
diff --git a/y2023/autonomous/autonomous_actor.cc b/y2023/autonomous/autonomous_actor.cc
index a815b25..8e99af6 100644
--- a/y2023/autonomous/autonomous_actor.cc
+++ b/y2023/autonomous/autonomous_actor.cc
@@ -8,6 +8,8 @@
#include "frc971/control_loops/drivetrain/localizer_generated.h"
#include "y2023/control_loops/drivetrain/drivetrain_base.h"
+DEFINE_bool(spline_auto, true, "Run simple test S-spline auto mode.");
+
namespace y2023 {
namespace actors {
@@ -18,11 +20,86 @@
AutonomousActor::AutonomousActor(::aos::EventLoop *event_loop)
: frc971::autonomous::BaseAutonomousActor(
- event_loop, control_loops::drivetrain::GetDrivetrainConfig()) {}
+ event_loop, control_loops::drivetrain::GetDrivetrainConfig()),
+ localizer_control_sender_(
+ event_loop->MakeSender<
+ ::frc971::control_loops::drivetrain::LocalizerControl>(
+ "/drivetrain")),
+ joystick_state_fetcher_(
+ event_loop->MakeFetcher<aos::JoystickState>("/aos")),
+ robot_state_fetcher_(event_loop->MakeFetcher<aos::RobotState>("/aos")),
+ auto_splines_() {
+ replan_timer_ = event_loop->AddTimer([this]() { Replan(); });
+
+ event_loop->OnRun([this, event_loop]() {
+ replan_timer_->Setup(event_loop->monotonic_now());
+ button_poll_->Setup(event_loop->monotonic_now(), chrono::milliseconds(50));
+ });
+
+ // TODO(james): Really need to refactor this code since we keep using it.
+ button_poll_ = event_loop->AddTimer([this]() {
+ const aos::monotonic_clock::time_point now =
+ this->event_loop()->context().monotonic_event_time;
+ if (robot_state_fetcher_.Fetch()) {
+ if (robot_state_fetcher_->user_button()) {
+ user_indicated_safe_to_reset_ = true;
+ MaybeSendStartingPosition();
+ }
+ }
+ if (joystick_state_fetcher_.Fetch()) {
+ if (joystick_state_fetcher_->has_alliance() &&
+ (joystick_state_fetcher_->alliance() != alliance_)) {
+ alliance_ = joystick_state_fetcher_->alliance();
+ is_planned_ = false;
+ // Only kick the planning out by 2 seconds. If we end up enabled in that
+ // second, then we will kick it out further based on the code below.
+ replan_timer_->Setup(now + std::chrono::seconds(2));
+ }
+ if (joystick_state_fetcher_->enabled()) {
+ if (!is_planned_) {
+ // Only replan once we've been disabled for 5 seconds.
+ replan_timer_->Setup(now + std::chrono::seconds(5));
+ }
+ }
+ }
+ });
+}
+
+void AutonomousActor::Replan() {
+ if (alliance_ == aos::Alliance::kInvalid) {
+ return;
+ }
+ sent_starting_position_ = false;
+ if (FLAGS_spline_auto) {
+ test_spline_ =
+ PlanSpline(std::bind(&AutonomousSplines::TestSpline, &auto_splines_,
+ std::placeholders::_1, alliance_),
+ SplineDirection::kForward);
+
+ starting_position_ = test_spline_->starting_position();
+ }
+
+ is_planned_ = true;
+
+ MaybeSendStartingPosition();
+}
+
+void AutonomousActor::MaybeSendStartingPosition() {
+ if (is_planned_ && user_indicated_safe_to_reset_ &&
+ !sent_starting_position_) {
+ CHECK(starting_position_);
+ SendStartingPosition(starting_position_.value());
+ }
+}
void AutonomousActor::Reset() {
InitializeEncoders();
ResetDrivetrain();
+
+ joystick_state_fetcher_.Fetch();
+ CHECK(joystick_state_fetcher_.get() != nullptr)
+ << "Expect at least one JoystickState message before running auto...";
+ alliance_ = joystick_state_fetcher_->alliance();
}
bool AutonomousActor::RunAction(
@@ -30,8 +107,58 @@
Reset();
AOS_LOG(INFO, "Params are %d\n", params->mode());
+
+ if (!user_indicated_safe_to_reset_) {
+ AOS_LOG(WARNING, "Didn't send starting position prior to starting auto.");
+ CHECK(starting_position_);
+ SendStartingPosition(starting_position_.value());
+ }
+ // Clear this so that we don't accidentally resend things as soon as we replan
+ // later.
+ user_indicated_safe_to_reset_ = false;
+ is_planned_ = false;
+ starting_position_.reset();
+
+ AOS_LOG(INFO, "Params are %d\n", params->mode());
+ if (alliance_ == aos::Alliance::kInvalid) {
+ AOS_LOG(INFO, "Aborting autonomous due to invalid alliance selection.");
+ return false;
+ }
+ if (FLAGS_spline_auto) {
+ SplineAuto();
+ } else {
+ AOS_LOG(WARNING, "No auto mode selected.");
+ }
return true;
}
+void AutonomousActor::SplineAuto() {
+ CHECK(test_spline_);
+
+ if (!test_spline_->WaitForPlan()) return;
+ test_spline_->Start();
+
+ if (!test_spline_->WaitForSplineDistanceRemaining(0.02)) return;
+}
+
+void AutonomousActor::SendStartingPosition(const Eigen::Vector3d &start) {
+ // Set up the starting position for the blue alliance.
+
+ auto builder = localizer_control_sender_.MakeBuilder();
+
+ LocalizerControl::Builder localizer_control_builder =
+ builder.MakeBuilder<LocalizerControl>();
+ localizer_control_builder.add_x(start(0));
+ localizer_control_builder.add_y(start(1));
+ localizer_control_builder.add_theta(start(2));
+ localizer_control_builder.add_theta_uncertainty(0.00001);
+ AOS_LOG(INFO, "User button pressed, x: %f y: %f theta: %f", start(0),
+ start(1), start(2));
+ if (builder.Send(localizer_control_builder.Finish()) !=
+ aos::RawSender::Error::kOk) {
+ AOS_LOG(ERROR, "Failed to reset localizer.\n");
+ }
+}
+
} // namespace actors
} // namespace y2023
diff --git a/y2023/autonomous/autonomous_actor.h b/y2023/autonomous/autonomous_actor.h
index 6eb8f90..cf0b458 100644
--- a/y2023/autonomous/autonomous_actor.h
+++ b/y2023/autonomous/autonomous_actor.h
@@ -6,6 +6,8 @@
#include "frc971/autonomous/base_autonomous_actor.h"
#include "frc971/control_loops/control_loops_generated.h"
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
+#include "frc971/control_loops/drivetrain/localizer_generated.h"
+#include "y2023/autonomous/auto_splines.h"
namespace y2023 {
namespace actors {
@@ -19,6 +21,29 @@
private:
void Reset();
+
+ void SendStartingPosition(const Eigen::Vector3d &start);
+ void MaybeSendStartingPosition();
+ void SplineAuto();
+ void Replan();
+
+ aos::Sender<frc971::control_loops::drivetrain::LocalizerControl>
+ localizer_control_sender_;
+ aos::Fetcher<aos::JoystickState> joystick_state_fetcher_;
+ aos::Fetcher<aos::RobotState> robot_state_fetcher_;
+
+ aos::TimerHandler *replan_timer_;
+ aos::TimerHandler *button_poll_;
+
+ std::optional<SplineHandle> test_spline_;
+ aos::Alliance alliance_ = aos::Alliance::kInvalid;
+ AutonomousSplines auto_splines_;
+ bool user_indicated_safe_to_reset_ = false;
+ bool sent_starting_position_ = false;
+
+ bool is_planned_ = false;
+
+ std::optional<Eigen::Vector3d> starting_position_;
};
} // namespace actors
diff --git a/y2023/autonomous/splines/test_spline.json b/y2023/autonomous/splines/test_spline.json
new file mode 100644
index 0000000..7672596
--- /dev/null
+++ b/y2023/autonomous/splines/test_spline.json
@@ -0,0 +1 @@
+{"spline_count": 1, "spline_x": [6.22420997455908, 6.1347950111487386, 6.080329974810555, 6.023577036950107, 5.9617203084135255, 5.81469341092744], "spline_y": [-2.63127733767268, -2.63127733767268, -2.656484781970896, -2.656484781970896, -2.6668098529078925, -2.6448802602350456], "constraints": [{"constraint_type": "LONGITUDINAL_ACCELERATION", "value": 2}, {"constraint_type": "LATERAL_ACCELERATION", "value": 1}, {"constraint_type": "VOLTAGE", "value": 4}]}
diff --git a/y2023/constants.cc b/y2023/constants.cc
index d46d812..eed17c4 100644
--- a/y2023/constants.cc
+++ b/y2023/constants.cc
@@ -125,7 +125,7 @@
0.0257708772364788 - 0.0395076737853459 - 6.87914956118006;
wrist->subsystem_params.zeroing_constants.measured_absolute_position =
- 2.51265911579648;
+ 0.0227022553749391;
wrist->subsystem_params.zeroing_constants.one_revolution_distance =
M_PI * 2.0 * constants::Values::kPracticeWristEncoderRatio();
diff --git a/y2023/constants/9971.json b/y2023/constants/9971.json
index 743db03..b21c012 100644
--- a/y2023/constants/9971.json
+++ b/y2023/constants/9971.json
@@ -1,16 +1,16 @@
{
"cameras": [
{
- "calibration": {% include 'y2023/vision/calib_files/calibration_pi-9971-1_cam-23-09_ext_from971_2023-02-23.json' %}
+ "calibration": {% include 'y2023/vision/calib_files/calibration_pi-9971-1_cam-23-09_ext_2023-03-05.json' %}
},
{
"calibration": {% include 'y2023/vision/calib_files/calibration_pi-9971-2_cam-23-10_ext_from971_2023-02-23.json' %}
},
{
- "calibration": {% include 'y2023/vision/calib_files/calibration_pi-9971-3_cam-23-11_ext_from971_2023-02-23.json' %}
+ "calibration": {% include 'y2023/vision/calib_files/calibration_pi-9971-3_cam-23-11_ext_2023-03-05.json' %}
},
{
- "calibration": {% include 'y2023/vision/calib_files/calibration_pi-9971-4_cam-23-12_ext_from971_2023-02-23.json' %}
+ "calibration": {% include 'y2023/vision/calib_files/calibration_pi-9971-4_cam-23-12_ext_2023-03-05.json' %}
}
],
"target_map": {% include 'y2023/vision/maps/target_map.json' %},
diff --git a/y2023/control_loops/drivetrain/target_selector.h b/y2023/control_loops/drivetrain/target_selector.h
index 5afc846..b1a9a39 100644
--- a/y2023/control_loops/drivetrain/target_selector.h
+++ b/y2023/control_loops/drivetrain/target_selector.h
@@ -37,6 +37,8 @@
}
double TargetRadius() const override { return 0.0; }
+ double GamePieceRadius() const override { return 0.0; }
+ bool SignedRadii() const override { return true; }
Side DriveDirection() const override { return drive_direction_; }
// We will manage any desired hysteresis in the target selection.
bool ForceReselectTarget() const override { return true; }
diff --git a/y2023/vision/calib_files/calibration_pi-9971-1_cam-23-09_ext_2023-03-05.json b/y2023/vision/calib_files/calibration_pi-9971-1_cam-23-09_ext_2023-03-05.json
new file mode 100644
index 0000000..c79f7d3
--- /dev/null
+++ b/y2023/vision/calib_files/calibration_pi-9971-1_cam-23-09_ext_2023-03-05.json
@@ -0,0 +1 @@
+{ "node_name": "pi1", "team_number": 9971, "intrinsics": [ 893.617798, 0.0, 612.44397, 0.0, 893.193115, 375.196381, 0.0, 0.0, 1.0 ], "fixed_extrinsics": { "data": [ -0.483961, 0.220781, 0.84678, 0.176109, 0.868846, 0.005849, 0.495048, -0.191149, 0.104344, 0.975306, -0.194656, 0.550508, 0.0, 0.0, 0.0, 1.0 ] }, "dist_coeffs": [ -0.443805, 0.238734, 0.000133, 0.000448, -0.071068 ], "calibration_timestamp": 1358499779650270322, "camera_id": "23-09" }
\ No newline at end of file
diff --git a/y2023/vision/calib_files/calibration_pi-9971-1_cam-23-09_ext_from971_2023-02-23.json b/y2023/vision/calib_files/calibration_pi-9971-1_cam-23-09_ext_from971_2023-02-23.json
deleted file mode 100644
index b6e0def..0000000
--- a/y2023/vision/calib_files/calibration_pi-9971-1_cam-23-09_ext_from971_2023-02-23.json
+++ /dev/null
@@ -1,25 +0,0 @@
-{
- "node_name": "pi1",
- "team_number": 9971,
- "intrinsics": [
- 893.617798,
- 0.0,
- 612.44397,
- 0.0,
- 893.193115,
- 375.196381,
- 0.0,
- 0.0,
- 1.0
- ],
- "dist_coeffs": [
- -0.443805,
- 0.238734,
- 0.000133,
- 0.000448,
- -0.071068
- ],
- "fixed_extrinsics": { "data": [ -0.487722, 0.222354, 0.844207, 0.025116, 0.864934, -0.008067, 0.501821, -0.246003, 0.118392, 0.974933, -0.188387, 0.532497, 0.0, 0.0, 0.0, 1.0 ] },
- "calibration_timestamp": 1358499779650270322,
- "camera_id": "23-09"
-}
diff --git a/y2023/vision/calib_files/calibration_pi-9971-3_cam-23-11_ext_2023-03-05.json b/y2023/vision/calib_files/calibration_pi-9971-3_cam-23-11_ext_2023-03-05.json
new file mode 100644
index 0000000..d4b8bd2
--- /dev/null
+++ b/y2023/vision/calib_files/calibration_pi-9971-3_cam-23-11_ext_2023-03-05.json
@@ -0,0 +1 @@
+{ "node_name": "pi3", "team_number": 9971, "intrinsics": [ 891.026001, 0.0, 620.086731, 0.0, 890.566895, 385.035126, 0.0, 0.0, 1.0 ], "fixed_extrinsics": { "data": [ 0.503726, -0.160611, -0.848802, 0.064621, -0.857988, 0.021383, -0.513224, -0.195154, 0.100579, 0.986786, -0.127032, 0.653821, 0.0, 0.0, 0.0, 1.0 ] }, "dist_coeffs": [ -0.448299, 0.250123, -0.00042, -0.000127, -0.078433 ], "calibration_timestamp": 1358503290177115986, "camera_id": "23-11" }
\ No newline at end of file
diff --git a/y2023/vision/calib_files/calibration_pi-9971-3_cam-23-11_ext_from971_2023-02-23.json b/y2023/vision/calib_files/calibration_pi-9971-3_cam-23-11_ext_from971_2023-02-23.json
deleted file mode 100644
index ec4c69c..0000000
--- a/y2023/vision/calib_files/calibration_pi-9971-3_cam-23-11_ext_from971_2023-02-23.json
+++ /dev/null
@@ -1,25 +0,0 @@
-{
- "node_name": "pi3",
- "team_number": 9971,
- "intrinsics": [
- 891.026001,
- 0.0,
- 620.086731,
- 0.0,
- 890.566895,
- 385.035126,
- 0.0,
- 0.0,
- 1.0
- ],
- "dist_coeffs": [
- -0.448299,
- 0.250123,
- -0.00042,
- -0.000127,
- -0.078433
- ],
- "fixed_extrinsics": { "data": [ 0.492232, -0.163335, -0.855002, 0.020122, -0.866067, 0.006706, -0.499883, -0.174518, 0.087382, 0.986548, -0.138158, 0.645307, 0.0, 0.0, 0.0, 1.0 ] },
- "calibration_timestamp": 1358503290177115986,
- "camera_id": "23-11"
-}
diff --git a/y2023/vision/calib_files/calibration_pi-9971-4_cam-23-12_ext_2023-03-05.json b/y2023/vision/calib_files/calibration_pi-9971-4_cam-23-12_ext_2023-03-05.json
new file mode 100644
index 0000000..7ea2f6d
--- /dev/null
+++ b/y2023/vision/calib_files/calibration_pi-9971-4_cam-23-12_ext_2023-03-05.json
@@ -0,0 +1 @@
+{ "node_name": "pi4", "team_number": 9971, "intrinsics": [ 891.127197, 0.0, 640.291321, 0.0, 891.176453, 359.578705, 0.0, 0.0, 1.0 ], "fixed_extrinsics": { "data": [ -0.845241, -0.169204, -0.506891, 0.008046, -0.511496, -0.018479, 0.859087, -0.243442, -0.154727, 0.985408, -0.070928, 0.69704, 0.0, 0.0, 0.0, 1.0 ] }, "dist_coeffs": [ -0.452948, 0.262567, 0.00088, -0.000253, -0.089368 ], "calibration_timestamp": 1358499579812698894, "camera_id": "23-12" }
\ No newline at end of file
diff --git a/y2023/vision/calib_files/calibration_pi-9971-4_cam-23-12_ext_from971_2023-02-23.json b/y2023/vision/calib_files/calibration_pi-9971-4_cam-23-12_ext_from971_2023-02-23.json
deleted file mode 100644
index 3d0f566..0000000
--- a/y2023/vision/calib_files/calibration_pi-9971-4_cam-23-12_ext_from971_2023-02-23.json
+++ /dev/null
@@ -1,25 +0,0 @@
-{
- "node_name": "pi4",
- "team_number": 9971,
- "intrinsics": [
- 891.127197,
- 0.0,
- 640.291321,
- 0.0,
- 891.176453,
- 359.578705,
- 0.0,
- 0.0,
- 1.0
- ],
- "dist_coeffs": [
- -0.452948,
- 0.262567,
- 0.00088,
- -0.000253,
- -0.089368
- ],
- "fixed_extrinsics": { "data": [ -0.865915, -0.186983, -0.463928, -0.014873, -0.473362, 0.006652, 0.880843, -0.215738, -0.161617, 0.982341, -0.094271, 0.676433, 0.0, 0.0, 0.0, 1.0 ] },
- "calibration_timestamp": 1358499579812698894,
- "camera_id": "23-12"
-}
diff --git a/y2023/y2023_roborio.json b/y2023/y2023_roborio.json
index ac7845f..1c0beac 100644
--- a/y2023/y2023_roborio.json
+++ b/y2023/y2023_roborio.json
@@ -509,7 +509,7 @@
"name": "joystick_reader",
"executable_name": "joystick_reader",
"args": [
- "--die_on_malloc"
+ "--nodie_on_malloc"
],
"nodes": [
"roborio"
@@ -526,9 +526,9 @@
"name": "autonomous_action",
"executable_name": "autonomous_action",
"args": [
- "--die_on_malloc"
+ "--nodie_on_malloc"
],
- "autostart": false,
+ "autostart": true,
"nodes": [
"roborio"
]