Add shooter superstructure code
Signed-off-by: Maxwell Henderson <mxwhenderson@gmail.com>
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
Change-Id: I559d79e1308c8ec6d434235e16f0273b4ce09f0d
diff --git a/y2024/control_loops/superstructure/shooter.cc b/y2024/control_loops/superstructure/shooter.cc
new file mode 100644
index 0000000..0666772
--- /dev/null
+++ b/y2024/control_loops/superstructure/shooter.cc
@@ -0,0 +1,271 @@
+#include "y2024/control_loops/superstructure/shooter.h"
+
+#include "aos/flatbuffers.h"
+#include "aos/flatbuffers/base.h"
+#include "frc971/control_loops/aiming/aiming.h"
+#include "y2024/control_loops/superstructure/catapult/catapult_plant.h"
+
+namespace y2024::control_loops::superstructure {
+
+using frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus;
+
+constexpr double kMinCurrent = 20.0;
+constexpr double kMaxVelocity = 1.0;
+constexpr double kCatapultActivationThreshold = 0.01;
+
+Shooter::Shooter(aos::EventLoop *event_loop, const Constants *robot_constants)
+ : drivetrain_status_fetcher_(
+ event_loop->MakeFetcher<frc971::control_loops::drivetrain::Status>(
+ "/drivetrain")),
+ superstructure_can_position_fetcher_(
+ event_loop
+ ->MakeFetcher<y2024::control_loops::superstructure::CANPosition>(
+ "/superstructure/rio")),
+ robot_constants_(robot_constants),
+ catapult_(
+ robot_constants->common()->catapult(),
+ robot_constants->robot()->catapult_constants()->zeroing_constants()),
+ turret_(
+ robot_constants_->common()->turret(),
+ robot_constants_->robot()->turret_constants()->zeroing_constants()),
+ altitude_(
+ robot_constants_->common()->altitude(),
+ robot_constants_->robot()->altitude_constants()->zeroing_constants()),
+ aimer_(event_loop, robot_constants_),
+ interpolation_table_(
+ y2024::constants::Values::InterpolationTableFromFlatbuffer(
+ robot_constants_->common()->shooter_interpolation_table())) {}
+
+flatbuffers::Offset<y2024::control_loops::superstructure::ShooterStatus>
+Shooter::Iterate(
+ const y2024::control_loops::superstructure::Position *position,
+ const y2024::control_loops::superstructure::ShooterGoal *shooter_goal,
+ double *catapult_output, double *altitude_output, double *turret_output,
+ double *retention_roller_output, double /*battery_voltage*/,
+ aos::monotonic_clock::time_point current_timestamp,
+ flatbuffers::FlatBufferBuilder *fbb) {
+ superstructure_can_position_fetcher_.Fetch();
+ drivetrain_status_fetcher_.Fetch();
+ CHECK(superstructure_can_position_fetcher_.get() != nullptr);
+
+ double current_retention_position =
+ superstructure_can_position_fetcher_->retention_roller()->position();
+
+ double torque_current =
+ superstructure_can_position_fetcher_->retention_roller()
+ ->torque_current();
+
+ double retention_velocity =
+ (current_retention_position - last_retention_position_) /
+ std::chrono::duration<double>(current_timestamp - last_timestamp_)
+ .count();
+
+ // If our current is over the minimum current and our velocity is under our
+ // maximum velocity, then set loaded to true. If we are preloaded set it to
+ // true as well.
+ //
+ // TODO(austin): Debounce piece_loaded?
+ bool piece_loaded =
+ (torque_current > kMinCurrent && retention_velocity < kMaxVelocity) ||
+ (shooter_goal != nullptr && shooter_goal->preloaded());
+
+ aos::fbs::FixedStackAllocator<aos::fbs::Builder<
+ frc971::control_loops::
+ StaticZeroingSingleDOFProfiledSubsystemGoalStatic>::kBufferSize>
+ turret_allocator;
+
+ aos::fbs::Builder<
+ frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoalStatic>
+ turret_goal_builder(&turret_allocator);
+
+ aos::fbs::FixedStackAllocator<aos::fbs::Builder<
+ frc971::control_loops::
+ StaticZeroingSingleDOFProfiledSubsystemGoalStatic>::kBufferSize>
+ altitude_allocator;
+
+ aos::fbs::Builder<
+ frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoalStatic>
+ altitude_goal_builder(&altitude_allocator);
+
+ const double distance_to_goal = aimer_.DistanceToGoal();
+
+ // Always retain the game piece if we are enabled.
+ if (retention_roller_output != nullptr) {
+ *retention_roller_output =
+ robot_constants_->common()->retention_roller_voltages()->retaining();
+ }
+
+ bool aiming = false;
+
+ // We don't have the note so we should be ready to intake it.
+ if (shooter_goal == nullptr || !shooter_goal->auto_aim() ||
+ (!piece_loaded && state_ == CatapultState::READY)) {
+ PopulateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ turret_goal_builder.get(),
+ robot_constants_->common()->turret_loading_position());
+
+ PopulateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ altitude_goal_builder.get(),
+ robot_constants_->common()->altitude_loading_position());
+ } else {
+ // We have a game piece, lets start aiming.
+ if (drivetrain_status_fetcher_.get() != nullptr) {
+ aiming = true;
+ aimer_.Update(drivetrain_status_fetcher_.get(),
+ frc971::control_loops::aiming::ShotMode::kShootOnTheFly,
+ turret_goal_builder.get());
+ }
+ }
+
+ // We have a game piece and are being asked to aim.
+ constants::Values::ShotParams shot_params;
+ if (piece_loaded && shooter_goal != nullptr && shooter_goal->auto_aim() &&
+ interpolation_table_.GetInRange(distance_to_goal, &shot_params)) {
+ PopulateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ altitude_goal_builder.get(), shot_params.shot_altitude_angle);
+ }
+
+ // The builder will contain either the auto-aim goal, or the loading goal. Use
+ // it if we have no goal, or no subsystem goal, or if we are auto-aiming.
+ const frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal
+ *turret_goal = (shooter_goal != nullptr && !shooter_goal->auto_aim() &&
+ shooter_goal->has_turret_position())
+ ? shooter_goal->turret_position()
+ : &turret_goal_builder->AsFlatbuffer();
+ const frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoal
+ *altitude_goal = (shooter_goal != nullptr && !shooter_goal->auto_aim() &&
+ shooter_goal->has_altitude_position())
+ ? shooter_goal->altitude_position()
+ : &altitude_goal_builder->AsFlatbuffer();
+
+ bool subsystems_in_range =
+ (std::abs(turret_.estimated_position() - turret_goal->unsafe_goal()) <
+ kCatapultActivationThreshold &&
+ std::abs(altitude_.estimated_position() - altitude_goal->unsafe_goal()) <
+ kCatapultActivationThreshold &&
+ altitude_.estimated_position() >
+ robot_constants_->common()->min_altitude_shooting_angle());
+
+ const flatbuffers::Offset<PotAndAbsoluteEncoderProfiledJointStatus>
+ turret_status_offset =
+ turret_.Iterate(turret_goal, position->turret(), turret_output, fbb);
+
+ const flatbuffers::Offset<PotAndAbsoluteEncoderProfiledJointStatus>
+ altitude_status_offset = altitude_.Iterate(
+ altitude_goal, position->altitude(), altitude_output, fbb);
+
+ flatbuffers::Offset<PotAndAbsoluteEncoderProfiledJointStatus>
+ catapult_status_offset;
+ {
+ // The catapult will never use a provided goal. We'll always fabricate one
+ // for it.
+ //
+ // Correct handles resetting our state when disabled.
+ const bool disabled = catapult_.Correct(nullptr, position->catapult(),
+ shooter_goal == nullptr);
+
+ catapult_.set_enable_profile(true);
+ // We want a trajectory which accelerates up over the first portion of the
+ // range of motion, holds top speed for a little bit, then decelerates
+ // before it swings too far.
+ //
+ // We can solve for these 3 parameters through the range of motion. Top
+ // speed is goverened by the voltage headroom we want to have for the
+ // controller.
+ //
+ // Accel can be tuned given the distance to accelerate over, and decel can
+ // be solved similarly.
+ //
+ // accel = v^2 / (2 * x)
+ catapult_.mutable_profile()->set_maximum_velocity(
+ catapult::kFreeSpeed * catapult::kOutputRatio * 4.0 / 12.0);
+
+ if (disabled) {
+ state_ = CatapultState::RETRACTING;
+ }
+
+ switch (state_) {
+ case CatapultState::READY:
+ case CatapultState::LOADED: {
+ if (piece_loaded) {
+ state_ = CatapultState::LOADED;
+ } else {
+ state_ = CatapultState::READY;
+ }
+
+ const bool catapult_close = CatapultClose();
+
+ if (subsystems_in_range && shooter_goal != nullptr &&
+ shooter_goal->fire() && catapult_close && piece_loaded) {
+ state_ = CatapultState::FIRING;
+ } else {
+ catapult_.set_controller_index(0);
+ catapult_.mutable_profile()->set_maximum_acceleration(100.0);
+ catapult_.mutable_profile()->set_maximum_deceleration(50.0);
+ catapult_.set_unprofiled_goal(0.0, 0.0);
+
+ if (!catapult_close) {
+ state_ = CatapultState::RETRACTING;
+ }
+ break;
+ }
+ [[fallthrough]];
+ }
+ case CatapultState::FIRING:
+ *retention_roller_output =
+ robot_constants_->common()->retention_roller_voltages()->spitting();
+ catapult_.set_controller_index(0);
+ catapult_.mutable_profile()->set_maximum_acceleration(400.0);
+ catapult_.mutable_profile()->set_maximum_deceleration(600.0);
+ catapult_.set_unprofiled_goal(2.0, 0.0);
+ if (CatapultClose()) {
+ state_ = CatapultState::RETRACTING;
+ } else {
+ break;
+ }
+ [[fallthrough]];
+ case CatapultState::RETRACTING:
+ catapult_.set_controller_index(0);
+ catapult_.mutable_profile()->set_maximum_acceleration(100.0);
+ catapult_.mutable_profile()->set_maximum_deceleration(50.0);
+ catapult_.set_unprofiled_goal(0.0, 0.0);
+
+ if (CatapultClose()) {
+ if (piece_loaded) {
+ state_ = CatapultState::LOADED;
+ } else {
+ state_ = CatapultState::READY;
+ }
+ }
+ break;
+ }
+
+ const double voltage = catapult_.UpdateController(disabled);
+ catapult_.UpdateObserver(voltage);
+ if (catapult_output != nullptr) {
+ *catapult_output = voltage;
+ }
+ catapult_status_offset = catapult_.MakeStatus(fbb);
+ }
+
+ flatbuffers::Offset<AimerStatus> aimer_offset;
+ if (aiming) {
+ aimer_offset = aimer_.PopulateStatus(fbb);
+ }
+
+ y2024::control_loops::superstructure::ShooterStatus::Builder status_builder(
+ *fbb);
+ status_builder.add_turret(turret_status_offset);
+ status_builder.add_altitude(altitude_status_offset);
+ status_builder.add_catapult(catapult_status_offset);
+ status_builder.add_catapult_state(state_);
+ if (aiming) {
+ status_builder.add_aimer(aimer_offset);
+ }
+
+ last_retention_position_ = current_retention_position;
+ last_timestamp_ = current_timestamp;
+ return status_builder.Finish();
+}
+
+} // namespace y2024::control_loops::superstructure