Adjust shot angle offset depending on retention roller current
This is functionally disabled currently, as it always outputs the same
shot angle offset.
Change-Id: I7bc4b3bc0ba2779d00a3d3203e67e97748b759b1
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>
diff --git a/y2024/control_loops/superstructure/BUILD b/y2024/control_loops/superstructure/BUILD
index 4e33f20..b824fbf 100644
--- a/y2024/control_loops/superstructure/BUILD
+++ b/y2024/control_loops/superstructure/BUILD
@@ -220,11 +220,13 @@
"aiming.h",
],
deps = [
+ ":superstructure_can_position_fbs",
":superstructure_goal_fbs",
":superstructure_status_fbs",
"//frc971/control_loops:static_zeroing_single_dof_profiled_subsystem",
"//frc971/control_loops/aiming",
"//frc971/control_loops/drivetrain:drivetrain_status_fbs",
+ "//frc971/zeroing:averager",
"//y2024:constants",
"//y2024/constants:constants_fbs",
"//y2024/control_loops/drivetrain:drivetrain_base",
diff --git a/y2024/control_loops/superstructure/aiming.cc b/y2024/control_loops/superstructure/aiming.cc
index 200029e..a2e92fd 100644
--- a/y2024/control_loops/superstructure/aiming.cc
+++ b/y2024/control_loops/superstructure/aiming.cc
@@ -22,8 +22,22 @@
y2024::constants::Values::InterpolationTableFromFlatbuffer(
robot_constants_->common()
->shooter_shuttle_interpolation_table())),
+ note_interpolation_table_(
+ y2024::constants::Values::NoteInterpolationTableFromFlatbuffer(
+ robot_constants_->common()->note_interpolation_table())),
joystick_state_fetcher_(
- event_loop_->MakeFetcher<aos::JoystickState>("/aos")) {}
+ event_loop_->MakeFetcher<aos::JoystickState>("/aos")) {
+ event_loop_->MakeWatcher(
+ "/superstructure/rio",
+ [this](const y2024::control_loops::superstructure::CANPosition &msg) {
+ if (latch_current_) return;
+
+ if (msg.has_retention_roller()) {
+ note_current_average_.AddData(
+ msg.retention_roller()->torque_current());
+ }
+ });
+}
void Aimer::Update(
const frc971::control_loops::drivetrain::Status *status,
@@ -73,7 +87,14 @@
robot_constants_->common()->turret()->range()),
current_interpolation_table->Get(current_goal_.target_distance)
.shot_speed_over_ground,
- /*wrap_mode=*/0.15, M_PI - kTurretZeroOffset},
+ /*wrap_mode=*/0.15,
+ // If we don't have any retention roller data, the averager
+ // will return 0. If we get a current that is out-of-range of
+ // the interpolation table, we will use the terminal values of
+ // the interpolation table.
+ M_PI - note_interpolation_table_
+ .Get(note_current_average_.GetAverage()(0))
+ .turret_offset},
RobotState{
robot_pose, {xdot, ydot}, linear_angular(1), current_goal_.position});
@@ -86,6 +107,9 @@
builder.add_turret_position(current_goal_.position);
builder.add_turret_velocity(current_goal_.velocity);
builder.add_target_distance(current_goal_.target_distance);
- builder.add_shot_distance(DistanceToGoal());
+ builder.add_note_current(note_current_average_.GetAverage()(0));
+ builder.add_current_turret_offset(
+ note_interpolation_table_.Get(note_current_average_.GetAverage()(0))
+ .turret_offset);
return builder.Finish();
}
diff --git a/y2024/control_loops/superstructure/aiming.h b/y2024/control_loops/superstructure/aiming.h
index 4560071..808a175 100644
--- a/y2024/control_loops/superstructure/aiming.h
+++ b/y2024/control_loops/superstructure/aiming.h
@@ -8,9 +8,11 @@
#include "frc971/control_loops/pose.h"
#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h"
#include "frc971/shooter_interpolation/interpolation.h"
+#include "frc971/zeroing/averager.h"
#include "y2024/constants.h"
#include "y2024/constants/constants_generated.h"
#include "y2024/control_loops/drivetrain/drivetrain_base.h"
+#include "y2024/control_loops/superstructure/superstructure_can_position_static.h"
#include "y2024/control_loops/superstructure/superstructure_goal_generated.h"
#include "y2024/control_loops/superstructure/superstructure_status_generated.h"
using y2024::control_loops::superstructure::AimerStatus;
@@ -19,9 +21,6 @@
class Aimer {
public:
- // When the turret is at 0 the note will be leaving the robot at PI.
- static constexpr double kTurretZeroOffset = 0.11;
-
Aimer(aos::EventLoop *event_loop, const Constants *robot_constants);
void Update(
@@ -33,6 +32,12 @@
double DistanceToGoal() const { return current_goal_.virtual_shot_distance; }
+ // Indicate that we are ready so that we can reset the note current filter to
+ // avoid taking values from when we were still seating the note.
+ void IndicateReady() { note_current_average_.Reset(); }
+
+ void latch_note_current(bool latch) { latch_current_ = latch; }
+
flatbuffers::Offset<AimerStatus> PopulateStatus(
flatbuffers::FlatBufferBuilder *fbb) const;
@@ -41,6 +46,8 @@
const Constants *robot_constants_;
+ bool latch_current_ = false;
+
frc971::control_loops::drivetrain::DrivetrainConfig<double>
drivetrain_config_;
@@ -52,6 +59,10 @@
y2024::constants::Values::ShotParams>
interpolation_table_shuttle_;
+ frc971::shooter_interpolation::InterpolationTable<
+ y2024::constants::Values::NoteParams>
+ note_interpolation_table_;
+
std::map<AutoAimMode, frc971::shooter_interpolation::InterpolationTable<
y2024::constants::Values::ShotParams> *>
interpolation_tables_ = {
@@ -108,6 +119,8 @@
aos::Fetcher<aos::JoystickState> joystick_state_fetcher_;
+ frc971::zeroing::Averager<double, 50> note_current_average_;
+
frc971::control_loops::aiming::TurretGoal current_goal_;
bool received_joystick_state_ = false;
diff --git a/y2024/control_loops/superstructure/shooter.cc b/y2024/control_loops/superstructure/shooter.cc
index 0c52360..7fb8973 100644
--- a/y2024/control_loops/superstructure/shooter.cc
+++ b/y2024/control_loops/superstructure/shooter.cc
@@ -93,9 +93,11 @@
// Always retain the game piece if we are enabled.
if (retention_roller_output != nullptr) {
*retention_roller_output =
- robot_constants_->common()->retention_roller_voltages()->retaining();
+ robot_constants_->common()->retention_roller_voltages()->intaking();
if (piece_loaded) {
+ *retention_roller_output =
+ robot_constants_->common()->retention_roller_voltages()->retaining();
*retention_roller_stator_current_limit =
robot_constants_->common()
->current_limits()
@@ -283,9 +285,11 @@
case CatapultState::READY:
[[fallthrough]];
case CatapultState::LOADED: {
+ aimer_.latch_note_current(false);
if (piece_loaded) {
state_ = CatapultState::LOADED;
} else {
+ aimer_.IndicateReady();
state_ = CatapultState::READY;
}
@@ -311,6 +315,7 @@
[[fallthrough]];
}
case CatapultState::FIRING:
+ aimer_.latch_note_current(true);
*retention_roller_output =
robot_constants_->common()->retention_roller_voltages()->spitting();
*retention_roller_stator_current_limit =
@@ -336,6 +341,7 @@
}
[[fallthrough]];
case CatapultState::RETRACTING:
+ aimer_.latch_note_current(false);
catapult_.set_controller_index(0);
catapult_.mutable_profile()->set_maximum_acceleration(
kLoadingAcceleration);
diff --git a/y2024/control_loops/superstructure/superstructure_lib_test.cc b/y2024/control_loops/superstructure/superstructure_lib_test.cc
index 512ccab..59398ab 100644
--- a/y2024/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2024/control_loops/superstructure/superstructure_lib_test.cc
@@ -48,6 +48,10 @@
AbsoluteEncoderSubsystem::State,
constants::Values::AbsoluteEncoderConstants>;
+// Value to make the auto-aim tests pass because they use a current-based
+// interpolation table. This should be the zero-value.
+static constexpr double kTurretZeroOffset = 0.09;
+
class SuperstructureSimulation {
public:
SuperstructureSimulation(::aos::EventLoop *event_loop,
@@ -1389,11 +1393,11 @@
EXPECT_NEAR(
-M_PI_2,
superstructure_status_fetcher_->shooter()->aimer()->turret_position() -
- M_PI - Aimer::kTurretZeroOffset,
+ M_PI - kTurretZeroOffset,
5e-4);
EXPECT_NEAR(-M_PI_2,
superstructure_status_fetcher_->shooter()->turret()->position() -
- M_PI - Aimer::kTurretZeroOffset,
+ M_PI - kTurretZeroOffset,
5e-4);
EXPECT_EQ(
@@ -1435,11 +1439,11 @@
EXPECT_NEAR(
M_PI_2,
superstructure_status_fetcher_->shooter()->aimer()->turret_position() +
- M_PI - Aimer::kTurretZeroOffset,
+ M_PI - kTurretZeroOffset,
5e-4);
EXPECT_NEAR(M_PI_2,
superstructure_status_fetcher_->shooter()->turret()->position() +
- M_PI - Aimer::kTurretZeroOffset,
+ M_PI - kTurretZeroOffset,
5e-4);
EXPECT_EQ(
kDistanceFromSpeaker,
diff --git a/y2024/control_loops/superstructure/superstructure_status.fbs b/y2024/control_loops/superstructure/superstructure_status.fbs
index 7a508dd..c73115f 100644
--- a/y2024/control_loops/superstructure/superstructure_status.fbs
+++ b/y2024/control_loops/superstructure/superstructure_status.fbs
@@ -52,6 +52,10 @@
// The current "shot distance." When shooting on the fly, this may be
// different from the static distance to the target.
shot_distance:double (id: 3);
+ // Estimate of the retention roller current.
+ note_current:double (id: 4);
+ // Turret offset applied due to retention roller current.
+ current_turret_offset:double (id: 5);
}
// Enum representing where the superstructure
@@ -140,7 +144,7 @@
transfer_roller:TransferRollerStatus (id: 5);
// Estimated angle and angular velocitiy of the climber.
- // Deprecated now because climber no longer has an encoder
+ // Deprecated now because climber no longer has an encoder
// and climber status is not used
climber:frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus (id: 6, deprecated);