Merge "Updated extrinsic calibration for 9971 cameras 1, 3, 4"
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/drivetrain_status.fbs b/frc971/control_loops/drivetrain/drivetrain_status.fbs
index f886bb2..89dc3d5 100644
--- a/frc971/control_loops/drivetrain/drivetrain_status.fbs
+++ b/frc971/control_loops/drivetrain/drivetrain_status.fbs
@@ -82,6 +82,12 @@
available_splines:[int] (id: 12);
}
+enum RobotSide : ubyte {
+ FRONT = 0,
+ BACK = 1,
+ DONT_CARE = 2,
+}
+
// For logging state of the line follower.
table LineFollowLogging {
// Whether we are currently freezing target choice.
@@ -100,6 +106,8 @@
goal_theta:float (id: 7);
// Current relative heading.
rel_theta:float (id: 8);
+ // Current goal drive direction.
+ drive_direction:RobotSide = DONT_CARE (id: 9);
}
// Current states of the EKF. See hybrid_ekf.h for detailed comments.
diff --git a/frc971/control_loops/drivetrain/line_follow_drivetrain.cc b/frc971/control_loops/drivetrain/line_follow_drivetrain.cc
index 95f14a3..101c0f7 100644
--- a/frc971/control_loops/drivetrain/line_follow_drivetrain.cc
+++ b/frc971/control_loops/drivetrain/line_follow_drivetrain.cc
@@ -87,6 +87,19 @@
return Kff;
}
+double VelocitySignForSide(TargetSelectorInterface::Side side,
+ double goal_velocity) {
+ switch (side) {
+ case TargetSelectorInterface::Side::FRONT:
+ return 1.0;
+ case TargetSelectorInterface::Side::BACK:
+ return -1.0;
+ case TargetSelectorInterface::Side::DONT_CARE:
+ return goal_velocity >= 0.0 ? 1.0 : -1.0;
+ }
+ return 1.0;
+}
+
} // namespace
// When we create A/B, we do recompute A/B, but we don't really care about
@@ -152,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.
@@ -172,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) {
@@ -196,13 +215,14 @@
// UpdateSelection every time.
bool new_target =
target_selector_->UpdateSelection(abs_state, goal_velocity_);
- if (freeze_target_) {
+ if (freeze_target_ && !target_selector_->ForceReselectTarget()) {
// When freezing the target, only make changes if we didn't have a good
// target before.
if (!have_target_ && new_target) {
have_target_ = true;
start_of_target_acquire_ = now;
- velocity_sign_ = goal_velocity_ >= 0.0 ? 1.0 : -1.0;
+ velocity_sign_ = VelocitySignForSide(target_selector_->DriveDirection(),
+ goal_velocity_);
target_pose_ = target_selector_->TargetPose();
}
} else {
@@ -211,9 +231,11 @@
have_target_ = new_target;
if (have_target_) {
target_pose_ = target_selector_->TargetPose();
- velocity_sign_ = goal_velocity_ >= 0.0 ? 1.0 : -1.0;
+ velocity_sign_ = VelocitySignForSide(target_selector_->DriveDirection(),
+ goal_velocity_);
}
}
+
// Get the robot pose in the target coordinate frame.
relative_pose_ = Pose({abs_state.x(), abs_state.y(), 0.0}, abs_state(2, 0))
.Rebase(&target_pose_);
@@ -276,6 +298,8 @@
-relative_pose_.rel_pos().x());
line_follow_logging_builder.add_goal_theta(controls_goal_(0, 0));
line_follow_logging_builder.add_rel_theta(relative_pose_.rel_theta());
+ line_follow_logging_builder.add_drive_direction(
+ target_selector_->DriveDirection());
return line_follow_logging_builder.Finish();
}
diff --git a/frc971/control_loops/drivetrain/line_follow_drivetrain_test.cc b/frc971/control_loops/drivetrain/line_follow_drivetrain_test.cc
index bb872dc..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);
@@ -260,6 +262,27 @@
<< "Expected state of zero, got: " << state_.transpose();
}
+// Tests that, when we explicitly require that the controller use a new target
+// that we obey it, even during trying to do line following.
+TEST_F(LineFollowDrivetrainTest, IgnoreFreezeWhenRequested) {
+ state_.setZero();
+ set_goal_pose({{0.0, 0.0, 0.0}, 0.0});
+ // Do one iteration to get the target into the drivetrain:
+ Iterate();
+
+ freeze_target_ = true;
+ target_selector_.set_force_reselect(true);
+
+ // Set a goal pose that we will end up trying to go to.
+ set_goal_pose({{1.0, 0.0, 0.0}, 0.0});
+ driver_model_ = [](const ::Eigen::Matrix<double, 5, 1> &) { return 0.25; };
+
+ RunForTime(::std::chrono::seconds(5));
+ // Should've driven a decent distance in X.
+ EXPECT_LT(1.0, state_.x());
+ EXPECT_NEAR(0.0, state_.y(), 1e-6);
+}
+
// Tests that when we freeze the controller without having acquired a target, we
// don't do anything until a target arrives.
TEST_F(LineFollowDrivetrainTest, FreezeWithoutAcquiringTarget) {
@@ -309,12 +332,12 @@
VerifyNearGoal();
}
INSTANTIATE_TEST_SUITE_P(TargetPosTest, LineFollowDrivetrainTargetParamTest,
- ::testing::Values(Pose({0.0, 0.0, 0.0}, 0.0),
- Pose({1.0, 0.0, 0.0}, 0.0),
- Pose({3.0, 1.0, 0.0}, 0.0),
- Pose({3.0, 0.0, 0.0}, 0.5),
- Pose({3.0, 0.0, 0.0}, -0.5),
- Pose({-3.0, -1.0, 0.0}, -2.5)));
+ ::testing::Values(Pose({0.0, 0.0, 0.0}, 0.0),
+ Pose({1.0, 0.0, 0.0}, 0.0),
+ Pose({3.0, 1.0, 0.0}, 0.0),
+ Pose({3.0, 0.0, 0.0}, 0.5),
+ Pose({3.0, 0.0, 0.0}, -0.5),
+ Pose({-3.0, -1.0, 0.0}, -2.5)));
class LineFollowDrivetrainParamTest
: public LineFollowDrivetrainTest,
diff --git a/frc971/control_loops/drivetrain/localizer.h b/frc971/control_loops/drivetrain/localizer.h
index d590f1d..867b3c0 100644
--- a/frc971/control_loops/drivetrain/localizer.h
+++ b/frc971/control_loops/drivetrain/localizer.h
@@ -15,6 +15,7 @@
// state updates and then determine what poes we should be driving to.
class TargetSelectorInterface {
public:
+ typedef RobotSide Side;
virtual ~TargetSelectorInterface() {}
// Take the state as [x, y, theta, left_vel, right_vel]
// If unable to determine what target to go for, returns false. If a viable
@@ -28,11 +29,31 @@
// 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
+ // target, regardless of any hysteresis we try to use to protect the driver.
+ virtual bool ForceReselectTarget() const = 0;
};
// Defines an interface for classes that provide field-global localization.
@@ -114,17 +135,30 @@
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) {
+ force_reselect_ = force_reselect;
+ }
bool has_target() const { return has_target_; }
private:
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;
};
// Uses the generic HybridEkf implementation to provide a basic field estimator.
diff --git a/frc971/vision/target_mapper.h b/frc971/vision/target_mapper.h
index 7701fd1..9d943c9 100644
--- a/frc971/vision/target_mapper.h
+++ b/frc971/vision/target_mapper.h
@@ -122,7 +122,7 @@
// too much
static ceres::examples::VectorOfConstraints MatchTargetDetections(
const std::vector<TimestampedDetection> ×tamped_target_detections,
- aos::distributed_clock::duration max_dt = std::chrono::milliseconds(1));
+ aos::distributed_clock::duration max_dt = std::chrono::milliseconds(10));
// Computes inverse of covariance matrix, assuming there was a target
// detection between robot movement over the given time period. Ceres calls
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 19ac52e..b8d89c7 100644
--- a/y2019/control_loops/drivetrain/target_selector.h
+++ b/y2019/control_loops/drivetrain/target_selector.h
@@ -1,10 +1,10 @@
#ifndef Y2019_CONTROL_LOOPS_DRIVETRAIN_TARGET_SELECTOR_H_
#define Y2019_CONTROL_LOOPS_DRIVETRAIN_TARGET_SELECTOR_H_
-#include "frc971/control_loops/pose.h"
-#include "frc971/control_loops/drivetrain/localizer.h"
-#include "y2019/constants.h"
#include "frc971/control_loops/drivetrain/camera.h"
+#include "frc971/control_loops/drivetrain/localizer.h"
+#include "frc971/control_loops/pose.h"
+#include "y2019/constants.h"
#include "y2019/control_loops/drivetrain/target_selector_generated.h"
#include "y2019/control_loops/superstructure/superstructure_goal_generated.h"
@@ -29,7 +29,8 @@
// obstacles and just assume that we have perfect field of view.
typedef frc971::control_loops::TypedCamera<
y2019::constants::Field::kNumTargets,
- /*num_obstacles=*/0, double> FakeCamera;
+ /*num_obstacles=*/0, double>
+ FakeCamera;
TargetSelector(::aos::EventLoop *event_loop);
virtual ~TargetSelector() {}
@@ -39,6 +40,12 @@
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; }
+
+ bool ForceReselectTarget() const override { return false; }
private:
static constexpr double kFakeFov = M_PI * 0.9;
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 6a263d3..eed17c4 100644
--- a/y2023/constants.cc
+++ b/y2023/constants.cc
@@ -25,6 +25,7 @@
auto *const arm_distal = &r.arm_distal;
auto *const wrist = &r.wrist;
auto *const roll_joint = &r.roll_joint;
+ r.wrist_flipped = true;
arm_proximal->zeroing.average_filter_size = Values::kZeroingSampleSize;
arm_proximal->zeroing.one_revolution_distance =
@@ -51,18 +52,18 @@
wrist->subsystem_params.operating_voltage = 12.0;
wrist->subsystem_params.zeroing_profile_params = {0.5, 3.0};
wrist->subsystem_params.default_profile_params = {0.5, 5.0};
- wrist->subsystem_params.range = Values::kWristRange();
+ wrist->subsystem_params.range = Values::kCompWristRange();
wrist->subsystem_params.make_integral_loop =
control_loops::superstructure::wrist::MakeIntegralWristLoop;
wrist->subsystem_params.zeroing_constants.average_filter_size =
Values::kZeroingSampleSize;
wrist->subsystem_params.zeroing_constants.one_revolution_distance =
- M_PI * 2.0 * constants::Values::kWristEncoderRatio();
+ M_PI * 2.0 * constants::Values::kCompWristEncoderRatio();
wrist->subsystem_params.zeroing_constants.zeroing_threshold = 0.0005;
wrist->subsystem_params.zeroing_constants.moving_buffer_size = 20;
wrist->subsystem_params.zeroing_constants.allowable_encoder_error = 0.9;
wrist->subsystem_params.zeroing_constants.middle_position =
- Values::kWristRange().middle();
+ Values::kCompWristRange().middle();
switch (team) {
// A set of constants for tests.
@@ -82,17 +83,17 @@
break;
case kCompTeamNumber:
- arm_proximal->zeroing.measured_absolute_position = 0.132182297391884;
+ arm_proximal->zeroing.measured_absolute_position = 0.138453705930275;
arm_proximal->potentiometer_offset =
0.931355973012855 + 8.6743197253382 - 0.101200335326309 -
- 0.0820901660993467 - 0.0703733798337964;
+ 0.0820901660993467 - 0.0703733798337964 - 0.0294645384848748;
- arm_distal->zeroing.measured_absolute_position = 0.597004611319487;
+ arm_distal->zeroing.measured_absolute_position = 0.562947209110251;
arm_distal->potentiometer_offset =
0.436664933370656 + 0.49457213779426 + 6.78213223139724 -
0.0220711555235029 - 0.0162945074111813 + 0.00630344935527365 -
0.0164398318919943 - 0.145833494945215 + 0.234878799868491 +
- 0.125924230298394;
+ 0.125924230298394 + 0.147136306208754;
roll_joint->zeroing.measured_absolute_position = 0.593975883699743;
roll_joint->potentiometer_offset =
@@ -104,7 +105,7 @@
0.0201047336425017 - 1.0173426655158 - 0.186085272847293 - 0.0317706563397807;
wrist->subsystem_params.zeroing_constants.measured_absolute_position =
- 1.54674994866259;
+ 5.78862525947414;
break;
@@ -124,7 +125,14 @@
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();
+ wrist->subsystem_params.range = Values::kPracticeWristRange();
+ wrist->subsystem_params.zeroing_constants.middle_position =
+ Values::kPracticeWristRange().middle();
+ r.wrist_flipped = false;
break;
diff --git a/y2023/constants.h b/y2023/constants.h
index de665d9..8996597 100644
--- a/y2023/constants.h
+++ b/y2023/constants.h
@@ -121,17 +121,34 @@
// Wrist
static constexpr double kWristEncoderCountsPerRevolution() { return 4096.0; }
- static constexpr double kWristEncoderRatio() {
+ static constexpr double kCompWristEncoderRatio() {
+ return 1.0;
+ }
+ static constexpr double kPracticeWristEncoderRatio() {
return (24.0 / 36.0) * (36.0 / 60.0);
}
- static constexpr double kMaxWristEncoderPulsesPerSecond() {
+ static constexpr double kMaxCompWristEncoderPulsesPerSecond() {
return control_loops::superstructure::wrist::kFreeSpeed / (2.0 * M_PI) *
control_loops::superstructure::wrist::kOutputRatio /
- kWristEncoderRatio() * kWristEncoderCountsPerRevolution();
+ kCompWristEncoderRatio() * kWristEncoderCountsPerRevolution();
+ }
+ static constexpr double kMaxPracticeWristEncoderPulsesPerSecond() {
+ return control_loops::superstructure::wrist::kFreeSpeed / (2.0 * M_PI) *
+ control_loops::superstructure::wrist::kOutputRatio /
+ kPracticeWristEncoderRatio() * kWristEncoderCountsPerRevolution();
}
- static constexpr ::frc971::constants::Range kWristRange() {
+ static constexpr ::frc971::constants::Range kCompWristRange() {
+ return ::frc971::constants::Range{
+ .lower_hard = -0.10, // Back Hard
+ .upper_hard = 4.90, // Front Hard
+ .lower = 0.0, // Back Soft
+ .upper = 4.0, // Front Soft
+ };
+ }
+
+ static constexpr ::frc971::constants::Range kPracticeWristRange() {
return ::frc971::constants::Range{
.lower_hard = -0.10, // Back Hard
.upper_hard = 2.30, // Front Hard
@@ -210,6 +227,8 @@
ArmJointConstants roll_joint;
AbsEncoderConstants wrist;
+
+ bool wrist_flipped;
};
// Creates and returns a Values instance for the constants.
diff --git a/y2023/control_loops/drivetrain/BUILD b/y2023/control_loops/drivetrain/BUILD
index 32b8c15..a12326c 100644
--- a/y2023/control_loops/drivetrain/BUILD
+++ b/y2023/control_loops/drivetrain/BUILD
@@ -131,6 +131,9 @@
],
gen_reflections = 1,
visibility = ["//visibility:public"],
+ deps = [
+ "//frc971/control_loops/drivetrain:drivetrain_status_fbs",
+ ],
)
cc_library(
diff --git a/y2023/control_loops/drivetrain/target_selector.cc b/y2023/control_loops/drivetrain/target_selector.cc
index 99805cb..20e5e25 100644
--- a/y2023/control_loops/drivetrain/target_selector.cc
+++ b/y2023/control_loops/drivetrain/target_selector.cc
@@ -3,6 +3,12 @@
#include "aos/containers/sized_array.h"
namespace y2023::control_loops::drivetrain {
+namespace {
+// If we already have a target selected, require the robot to be closer than
+// this distance (in meters) to one target than another before swapping.
+constexpr double kGridHysteresisDistance = 0.1;
+} // namespace
+
TargetSelector::TargetSelector(aos::EventLoop *event_loop)
: joystick_state_fetcher_(
event_loop->MakeFetcher<aos::JoystickState>("/aos")),
@@ -44,6 +50,17 @@
// We don't know where to go, wait on a hint.
return false;
}
+ // Keep track of when the hint changes (note that this will not detect default
+ // vs. not populated default values); when it changes, force us to reselect
+ // the target.
+ {
+ TargetSelectorHintT hint_object;
+ hint_fetcher_.get()->UnPackTo(&hint_object);
+ if (!last_hint_.has_value() || hint_object != last_hint_) {
+ target_pose_.reset();
+ }
+ last_hint_ = hint_object;
+ }
aos::SizedArray<const localizer::ScoringGrid *, 3> possible_grids;
if (hint_fetcher_->has_grid()) {
possible_grids = {[this]() -> const localizer::ScoringGrid * {
@@ -102,19 +119,36 @@
return positions;
}();
CHECK_LT(0u, possible_positions.size());
+ aos::SizedArray<double, 3> distances;
std::optional<double> closest_distance;
std::optional<Eigen::Vector3d> closest_position;
const Eigen::Vector3d robot_position(state.x(), state.y(), 0.0);
for (const frc971::vision::Position *position : possible_positions) {
const Eigen::Vector3d target(position->x(), position->y(), position->z());
double distance = (target - robot_position).norm();
+ distances.push_back(distance);
if (!closest_distance.has_value() || distance < closest_distance.value()) {
closest_distance = distance;
closest_position = target;
}
}
- CHECK(closest_position.has_value());
- target_pose_ = Pose(closest_position.value(), /*theta=*/0.0);
+ std::sort(distances.begin(), distances.end());
+ CHECK_EQ(distances.at(0), closest_distance.value());
+ // Only change the target pose if one grid is clearly better than the other.
+ // This prevents us from dithering between two grids if we happen to be on the
+ // boundary.
+ if (!target_pose_.has_value() ||
+ distances.at(1) - distances.at(0) > kGridHysteresisDistance) {
+ CHECK(closest_position.has_value());
+ target_pose_ = Pose(closest_position.value(), /*theta=*/0.0);
+ if (hint_fetcher_->has_robot_side()) {
+ drive_direction_ = hint_fetcher_->robot_side();
+ } else {
+ drive_direction_ = Side::DONT_CARE;
+ }
+ }
+ CHECK(target_pose_.has_value());
return true;
}
+
} // namespace y2023::control_loops::drivetrain
diff --git a/y2023/control_loops/drivetrain/target_selector.h b/y2023/control_loops/drivetrain/target_selector.h
index 0290425..b1a9a39 100644
--- a/y2023/control_loops/drivetrain/target_selector.h
+++ b/y2023/control_loops/drivetrain/target_selector.h
@@ -23,6 +23,7 @@
: public frc971::control_loops::drivetrain::TargetSelectorInterface {
public:
typedef frc971::control_loops::TypedPose<double> Pose;
+ typedef frc971::control_loops::drivetrain::RobotSide Side;
TargetSelector(aos::EventLoop *event_loop);
@@ -36,14 +37,21 @@
}
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; }
private:
void UpdateAlliance();
std::optional<Pose> target_pose_;
aos::Fetcher<aos::JoystickState> joystick_state_fetcher_;
aos::Fetcher<TargetSelectorHint> hint_fetcher_;
+ std::optional<TargetSelectorHintT> last_hint_;
frc971::constants::ConstantsFetcher<Constants> constants_fetcher_;
const localizer::HalfField *scoring_map_ = nullptr;
+ Side drive_direction_ = Side::DONT_CARE;
};
} // namespace y2023::control_loops::drivetrain
#endif // Y2023_CONTROL_LOOPS_DRIVETRAIN_TARGET_SELECTOR_H_
diff --git a/y2023/control_loops/drivetrain/target_selector_hint.fbs b/y2023/control_loops/drivetrain/target_selector_hint.fbs
index 518d302..399d772 100644
--- a/y2023/control_loops/drivetrain/target_selector_hint.fbs
+++ b/y2023/control_loops/drivetrain/target_selector_hint.fbs
@@ -1,3 +1,5 @@
+include "frc971/control_loops/drivetrain/drivetrain_status.fbs";
+
namespace y2023.control_loops.drivetrain;
// Which of the grids we are going for.
@@ -23,13 +25,12 @@
RIGHT,
}
-
table TargetSelectorHint {
grid:GridSelectionHint (id: 0);
row:RowSelectionHint (id: 1);
spot:SpotSelectionHint (id: 2);
+ robot_side:frc971.control_loops.drivetrain.RobotSide = DONT_CARE (id: 3);
// TODO: support human player pickup auto-align?
- // TODO: add flag for forwards vs. backwards.
}
root_type TargetSelectorHint;
diff --git a/y2023/control_loops/drivetrain/target_selector_test.cc b/y2023/control_loops/drivetrain/target_selector_test.cc
index fdbd904..c28c14d 100644
--- a/y2023/control_loops/drivetrain/target_selector_test.cc
+++ b/y2023/control_loops/drivetrain/target_selector_test.cc
@@ -4,6 +4,8 @@
#include "gtest/gtest.h"
#include "y2023/constants/simulated_constants_sender.h"
+using Side = frc971::control_loops::drivetrain::RobotSide;
+
namespace y2023::control_loops::drivetrain {
class TargetSelectorTest : public ::testing::Test {
protected:
@@ -38,17 +40,18 @@
}
void SendHint(GridSelectionHint grid, RowSelectionHint row,
- SpotSelectionHint spot) {
+ SpotSelectionHint spot, Side side) {
auto builder = hint_sender_.MakeBuilder();
builder.CheckOk(builder.Send(
- CreateTargetSelectorHint(*builder.fbb(), grid, row, spot)));
+ CreateTargetSelectorHint(*builder.fbb(), grid, row, spot, side)));
}
- void SendHint(RowSelectionHint row, SpotSelectionHint spot) {
+ void SendHint(RowSelectionHint row, SpotSelectionHint spot, Side side) {
auto builder = hint_sender_.MakeBuilder();
TargetSelectorHint::Builder hint_builder =
builder.MakeBuilder<TargetSelectorHint>();
hint_builder.add_row(row);
hint_builder.add_spot(spot);
+ hint_builder.add_robot_side(side);
builder.CheckOk(builder.Send(hint_builder.Finish()));
}
@@ -95,13 +98,17 @@
{row->left_cone(), SpotSelectionHint::LEFT},
{row->cube(), SpotSelectionHint::MIDDLE},
{row->right_cone(), SpotSelectionHint::RIGHT}}) {
- SendHint(grid_hint, row_hint, spot_hint);
+ SendHint(grid_hint, row_hint, spot_hint, Side::FRONT);
EXPECT_TRUE(target_selector_.UpdateSelection(
Eigen::Matrix<double, 5, 1>::Zero(), 0.0));
EXPECT_EQ(0.0, target_selector_.TargetRadius());
EXPECT_EQ(spot->x(), target_selector_.TargetPose().abs_pos().x());
EXPECT_EQ(spot->y(), target_selector_.TargetPose().abs_pos().y());
EXPECT_EQ(spot->z(), target_selector_.TargetPose().abs_pos().z());
+ EXPECT_EQ(frc971::control_loops::drivetrain::TargetSelectorInterface::
+ Side::FRONT,
+ target_selector_.DriveDirection());
+ EXPECT_TRUE(target_selector_.ForceReselectTarget());
}
}
}
@@ -125,13 +132,57 @@
SpotSelectionHint::MIDDLE},
{scoring_map()->left_grid()->bottom()->right_cone(),
SpotSelectionHint::RIGHT}}) {
- SendHint(RowSelectionHint::BOTTOM, spot_hint);
+ SendHint(RowSelectionHint::BOTTOM, spot_hint, Side::BACK);
EXPECT_TRUE(target_selector_.UpdateSelection(
Eigen::Matrix<double, 5, 1>::Zero(), 0.0));
+ EXPECT_TRUE(target_selector_.ForceReselectTarget());
EXPECT_EQ(spot->x(), target_selector_.TargetPose().abs_pos().x());
EXPECT_EQ(spot->y(), target_selector_.TargetPose().abs_pos().y());
EXPECT_EQ(spot->z(), target_selector_.TargetPose().abs_pos().z());
+ EXPECT_EQ(
+ frc971::control_loops::drivetrain::TargetSelectorInterface::Side::BACK,
+ target_selector_.DriveDirection());
}
}
+// Tests that if we are on the boundary of two grids that we do apply some
+// hysteresis.
+TEST_F(TargetSelectorTest, GridHysteresis) {
+ SendJoystickState();
+ // We will leave the robot at (0, 0). This means that if we are going for the
+ // left cone we should go for the middle grid, and if we are going for the
+ // cube (middle) or right cone positions we should prefer the left grid.
+ // Note that the grids are not centered on the field (hence the middle isn't
+ // always preferred when at (0, 0)).
+
+ const frc971::vision::Position *left_pos =
+ scoring_map()->left_grid()->bottom()->cube();
+ const frc971::vision::Position *middle_pos =
+ scoring_map()->middle_grid()->bottom()->cube();
+ Eigen::Matrix<double, 5, 1> split_position;
+ split_position << 0.0, (left_pos->y() + middle_pos->y()) / 2.0, 0.0, 0.0, 0.0;
+ Eigen::Matrix<double, 5, 1> slightly_left = split_position;
+ slightly_left.y() += 0.01;
+ Eigen::Matrix<double, 5, 1> slightly_middle = split_position;
+ slightly_middle.y() -= 0.01;
+ Eigen::Matrix<double, 5, 1> very_middle = split_position;
+ very_middle.y() -= 1.0;
+
+ SendHint(RowSelectionHint::BOTTOM, SpotSelectionHint::MIDDLE, Side::BACK);
+ EXPECT_TRUE(target_selector_.UpdateSelection(slightly_left, 0.0));
+ Eigen::Vector3d target = target_selector_.TargetPose().abs_pos();
+ EXPECT_EQ(target.x(), left_pos->x());
+ EXPECT_EQ(target.y(), left_pos->y());
+ // A slight movement should *not* reset things.
+ EXPECT_TRUE(target_selector_.UpdateSelection(slightly_middle, 0.0));
+ target = target_selector_.TargetPose().abs_pos();
+ EXPECT_EQ(target.x(), left_pos->x());
+ EXPECT_EQ(target.y(), left_pos->y());
+ // A large movement *should* reset things.
+ EXPECT_TRUE(target_selector_.UpdateSelection(very_middle, 0.0));
+ target = target_selector_.TargetPose().abs_pos();
+ EXPECT_EQ(target.x(), middle_pos->x());
+ EXPECT_EQ(target.y(), middle_pos->y());
+}
+
} // namespace y2023::control_loops::drivetrain
diff --git a/y2023/control_loops/python/graph_edit.py b/y2023/control_loops/python/graph_edit.py
index 1948edc..f18a0b6 100644
--- a/y2023/control_loops/python/graph_edit.py
+++ b/y2023/control_loops/python/graph_edit.py
@@ -373,16 +373,21 @@
for i in range(len(self.segments)):
color = None
if i == self.index:
- # Draw current spline in black
- color = [0, 0, 0]
- else:
- color = [0, random.random(), 1]
- random.shuffle(color)
+ continue
+ color = [0, random.random(), 1]
+ random.shuffle(color)
set_color(cr, Color(color[0], color[1], color[2]))
self.segments[i].DrawTo(cr, self.theta_version)
with px(cr):
cr.stroke()
+ # Draw current spline in black
+ color = [0, 0, 0]
+ set_color(cr, Color(color[0], color[1], color[2]))
+ self.segments[self.index].DrawTo(cr, self.theta_version)
+ with px(cr):
+ cr.stroke()
+
set_color(cr, Color(0.0, 1.0, 0.5))
# Create the plots
diff --git a/y2023/control_loops/python/graph_paths.py b/y2023/control_loops/python/graph_paths.py
index 6c74a74..f7571a6 100644
--- a/y2023/control_loops/python/graph_paths.py
+++ b/y2023/control_loops/python/graph_paths.py
@@ -298,7 +298,7 @@
))
points['ScoreBackLowCube'] = to_theta_with_circular_index_and_roll(
- -1.102, 0.30, -np.pi / 2.0, circular_index=1)
+ -1.102, 0.3212121, -np.pi / 2.0, circular_index=1)
named_segments.append(
ThetaSplineSegment(
diff --git a/y2023/control_loops/python/wrist.py b/y2023/control_loops/python/wrist.py
index 60ae8a6..fe2fcf6 100644
--- a/y2023/control_loops/python/wrist.py
+++ b/y2023/control_loops/python/wrist.py
@@ -20,7 +20,7 @@
kWrist = angular_system.AngularSystemParams(
name='Wrist',
motor=control_loop.BAG(),
- G=(6.0 / 48.0) * (20.0 / 100.0) * (24.0 / 36.0) * (36.0 / 60.0),
+ G=(6.0 / 48.0) * (20.0 / 100.0) * (18.0 / 24.0) * (24.0 / 44.0),
# Use parallel axis theorem to get the moment of inertia around
# the joint (I = I_cm + mh^2 = 0.001877 + 0.8332 * 0.0407162^2)
J=0.003258,
diff --git a/y2023/control_loops/superstructure/superstructure_lib_test.cc b/y2023/control_loops/superstructure/superstructure_lib_test.cc
index 4eeb0f6..ee440ed 100644
--- a/y2023/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2023/control_loops/superstructure/superstructure_lib_test.cc
@@ -181,7 +181,7 @@
PositionSensorSimulator(
values->wrist.subsystem_params.zeroing_constants
.one_revolution_distance),
- values->wrist, constants::Values::kWristRange(),
+ values->wrist, constants::Values::kCompWristRange(),
values->wrist.subsystem_params.zeroing_constants
.measured_absolute_position,
dt_),
@@ -449,7 +449,7 @@
flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
wrist_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
- *builder.fbb(), constants::Values::kWristRange().middle());
+ *builder.fbb(), constants::Values::kCompWristRange().middle());
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_arm_goal_position(arm::NeutralIndex());
@@ -474,7 +474,7 @@
flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
wrist_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
- *builder.fbb(), constants::Values::kWristRange().upper);
+ *builder.fbb(), constants::Values::kCompWristRange().upper);
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
@@ -505,7 +505,7 @@
flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
wrist_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
- *builder.fbb(), constants::Values::kWristRange().upper);
+ *builder.fbb(), constants::Values::kCompWristRange().upper);
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
@@ -524,7 +524,7 @@
flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
wrist_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
- *builder.fbb(), constants::Values::kWristRange().lower,
+ *builder.fbb(), constants::Values::kCompWristRange().lower,
CreateProfileParameters(*builder.fbb(), 20.0, 0.1));
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
diff --git a/y2023/joystick_reader.cc b/y2023/joystick_reader.cc
index 135c987..43962dd 100644
--- a/y2023/joystick_reader.cc
+++ b/y2023/joystick_reader.cc
@@ -31,11 +31,12 @@
using frc971::input::driver_station::ControlBit;
using frc971::input::driver_station::JoystickAxis;
using frc971::input::driver_station::POVLocation;
-using y2023::control_loops::superstructure::RollerGoal;
-using y2023::control_loops::drivetrain::RowSelectionHint;
using y2023::control_loops::drivetrain::GridSelectionHint;
+using y2023::control_loops::drivetrain::RowSelectionHint;
using y2023::control_loops::drivetrain::SpotSelectionHint;
using y2023::control_loops::drivetrain::TargetSelectorHint;
+using y2023::control_loops::superstructure::RollerGoal;
+using Side = frc971::control_loops::drivetrain::RobotSide;
namespace y2023 {
namespace input {
@@ -77,11 +78,6 @@
CUBE = 2,
};
-enum class Side {
- FRONT = 0,
- BACK = 1,
-};
-
struct ButtonData {
ButtonLocation button;
std::optional<SpotSelectionHint> spot = std::nullopt;
@@ -439,8 +435,7 @@
auto hint_builder = builder.MakeBuilder<TargetSelectorHint>();
hint_builder.add_row(placing_row.value());
hint_builder.add_spot(placing_spot.value());
- // TODO: Add field to TargetSelector hint for forwards vs. backwards
- // placement.
+ hint_builder.add_robot_side(CHECK_NOTNULL(current_setpoint_)->side);
if (builder.Send(hint_builder.Finish()) != aos::RawSender::Error::kOk) {
AOS_LOG(ERROR, "Sending target selector hint failed.\n");
}
diff --git a/y2023/vision/target_mapping.cc b/y2023/vision/target_mapping.cc
index c59414b..e399470 100644
--- a/y2023/vision/target_mapping.cc
+++ b/y2023/vision/target_mapping.cc
@@ -32,6 +32,8 @@
"Use the calibration for a node with this team number");
DEFINE_string(mcap_output_path, "/tmp/log.mcap", "Log to output.");
DEFINE_string(pi, "pi1", "Pi name to generate mcap log for; defaults to pi1.");
+DEFINE_double(max_pose_error, 1e-6,
+ "Throw out target poses with a higher pose error than this");
namespace y2023 {
namespace vision {
@@ -52,12 +54,17 @@
// Add detected apriltag poses relative to the robot to
// timestamped_target_detections
-void HandleAprilTag(const TargetMap &map,
- aos::distributed_clock::time_point pi_distributed_time,
- std::vector<DataAdapter::TimestampedDetection>
- *timestamped_target_detections,
- Eigen::Affine3d extrinsics) {
+void HandleAprilTags(const TargetMap &map,
+ aos::distributed_clock::time_point pi_distributed_time,
+ std::vector<DataAdapter::TimestampedDetection>
+ *timestamped_target_detections,
+ Eigen::Affine3d extrinsics) {
for (const auto *target_pose_fbs : *map.target_poses()) {
+ // Skip detections with high pose errors
+ if (target_pose_fbs->pose_error() > FLAGS_max_pose_error) {
+ continue;
+ }
+
const TargetMapper::TargetPose target_pose =
PoseUtils::TargetPoseFromFbs(*target_pose_fbs);
@@ -82,7 +89,7 @@
}
}
-// Get images from pi and pass apriltag positions to HandleAprilTag()
+// Get images from pi and pass apriltag positions to HandleAprilTags()
void HandlePiCaptures(
aos::EventLoop *detection_event_loop, aos::EventLoop *mapping_event_loop,
aos::logger::LogReader *reader,
@@ -107,8 +114,8 @@
->ToDistributedClock(aos::monotonic_clock::time_point(
aos::monotonic_clock::duration(map.monotonic_timestamp_ns())));
- HandleAprilTag(map, pi_distributed_time, timestamped_target_detections,
- extrinsics);
+ HandleAprilTags(map, pi_distributed_time, timestamped_target_detections,
+ extrinsics);
});
}
diff --git a/y2023/wpilib_interface.cc b/y2023/wpilib_interface.cc
index 921531e..033d3d7 100644
--- a/y2023/wpilib_interface.cc
+++ b/y2023/wpilib_interface.cc
@@ -104,7 +104,8 @@
Values::kMaxProximalEncoderPulsesPerSecond(),
Values::kMaxDistalEncoderPulsesPerSecond(),
Values::kMaxRollJointEncoderPulsesPerSecond(),
- Values::kMaxWristEncoderPulsesPerSecond(),
+ Values::kMaxCompWristEncoderPulsesPerSecond(),
+ Values::kMaxPracticeWristEncoderPulsesPerSecond(),
});
static_assert(kMaxFastEncoderPulsesPerSecond <= 1300000,
"fast encoders are too fast");
@@ -461,7 +462,10 @@
frc971::AbsolutePositionT wrist;
CopyPosition(wrist_encoder_, &wrist,
Values::kWristEncoderCountsPerRevolution(),
- Values::kWristEncoderRatio(), false);
+ values_->wrist.subsystem_params.zeroing_constants
+ .one_revolution_distance /
+ (M_PI * 2.0),
+ values_->wrist_flipped);
flatbuffers::Offset<frc971::PotAndAbsolutePosition> proximal_offset =
frc971::PotAndAbsolutePosition::Pack(*builder.fbb(), &proximal);
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"
]