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/constants.h b/y2024/constants.h
index 4cb5378..98fe468 100644
--- a/y2024/constants.h
+++ b/y2024/constants.h
@@ -161,6 +161,23 @@
// 20 -> 28 reduction to a 0.5" radius roller
static constexpr double kExtendRollerOutputRatio = (20.0 / 28.0) * 0.0127;
+ struct NoteParams {
+ // Measured in radians
+ double turret_offset = 0.0;
+
+ static NoteParams BlendY(double coefficient, NoteParams a1, NoteParams a2) {
+ using ::frc971::shooter_interpolation::Blend;
+ return NoteParams{.turret_offset = Blend(coefficient, a1.turret_offset,
+ a2.turret_offset)};
+ }
+
+ static NoteParams FromFlatbuffer(const y2024::NoteParams *note_params) {
+ return NoteParams{
+ .turret_offset = note_params->turret_offset(),
+ };
+ }
+ };
+
struct ShotParams {
// Measured in radians
double shot_altitude_angle = 0.0;
@@ -197,6 +214,21 @@
}
};
+ static frc971::shooter_interpolation::InterpolationTable<NoteParams>
+ NoteInterpolationTableFromFlatbuffer(
+ const flatbuffers::Vector<
+ flatbuffers::Offset<y2024::NoteInterpolationTablePoint>> *table) {
+ std::vector<std::pair<double, NoteParams>> interpolation_table;
+
+ for (const NoteInterpolationTablePoint *point : *table) {
+ interpolation_table.emplace_back(
+ point->amperage(), NoteParams::FromFlatbuffer(point->note_params()));
+ }
+
+ return frc971::shooter_interpolation::InterpolationTable<NoteParams>(
+ interpolation_table);
+ }
+
static frc971::shooter_interpolation::InterpolationTable<ShotParams>
InterpolationTableFromFlatbuffer(
const flatbuffers::Vector<
diff --git a/y2024/constants/common.json b/y2024/constants/common.json
index d99f17a..87a43cf 100644
--- a/y2024/constants/common.json
+++ b/y2024/constants/common.json
@@ -101,6 +101,20 @@
}
}
],
+ "note_interpolation_table": [
+ {
+ "amperage": 6.0,
+ "note_params": {
+ "turret_offset": 0.09
+ }
+ },
+ {
+ "amperage": 10.0,
+ "note_params": {
+ "turret_offset": 0.09
+ }
+ }
+ ],
"intake_roller_voltages": {
"spitting": -6.0,
"intaking": 9.0
@@ -299,7 +313,8 @@
"min_altitude_shooting_angle": 0.4,
"max_altitude_shooting_angle": 1.15,
"retention_roller_voltages": {
- "retaining": 1.5,
+ "intaking": 1.5,
+ "retaining": 0.6,
"spitting": 6.0
},
// TODO(Filip): Update the speaker and amp shooter setpoints
diff --git a/y2024/constants/constants.fbs b/y2024/constants/constants.fbs
index 4cc3502..b1f0b1f 100644
--- a/y2024/constants/constants.fbs
+++ b/y2024/constants/constants.fbs
@@ -29,6 +29,15 @@
shot_params: ShotParams (id: 1);
}
+table NoteParams {
+ turret_offset: double (id: 0);
+}
+
+table NoteInterpolationTablePoint {
+ amperage: double (id: 0);
+ note_params: NoteParams (id: 1);
+}
+
// Amount of voltage to give to the intake rollers when:
// spitting, which represents voltage when IntakeRollerGoal is SPITTING
// and intaking, which represents voltage when IntakeRollerGoal is INTAKING
@@ -161,6 +170,8 @@
table RetentionRollerVoltages {
retaining:double (id: 0);
spitting:double (id: 1);
+ // Used for pulling the game-piece in while it is not yet loaded.
+ intaking:double (id: 2);
}
// Set of april tag targets, by april tag ID, to ignore when on a
@@ -175,6 +186,7 @@
target_map:frc971.vision.TargetMap (id: 0);
shooter_interpolation_table: [InterpolationTablePoint] (id: 1);
shooter_shuttle_interpolation_table: [InterpolationTablePoint] (id: 30);
+ note_interpolation_table: [NoteInterpolationTablePoint] (id: 31);
intake_roller_voltages:IntakeRollerVoltages (id : 2);
intake_pivot_set_points:IntakePivotSetPoints (id: 3);
intake_pivot:frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemCommonParams (id: 4);
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);