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/BUILD b/y2024/control_loops/superstructure/BUILD
index 28e4d00..0493c9b 100644
--- a/y2024/control_loops/superstructure/BUILD
+++ b/y2024/control_loops/superstructure/BUILD
@@ -81,6 +81,7 @@
data = [
],
deps = [
+ ":shooter",
":superstructure_goal_fbs",
":superstructure_output_fbs",
":superstructure_position_fbs",
@@ -140,6 +141,52 @@
],
)
+cc_library(
+ name = "shooter",
+ srcs = [
+ "shooter.cc",
+ ],
+ hdrs = [
+ "shooter.h",
+ ],
+ deps = [
+ ":aiming",
+ ":superstructure_can_position_fbs",
+ ":superstructure_goal_fbs",
+ ":superstructure_position_fbs",
+ ":superstructure_status_fbs",
+ "//frc971/control_loops:static_zeroing_single_dof_profiled_subsystem",
+ "//frc971/control_loops/catapult",
+ "//frc971/control_loops/catapult:catapult_goal_fbs",
+ "//frc971/shooter_interpolation:interpolation",
+ "//frc971/zeroing:pot_and_absolute_encoder",
+ "//y2024:constants",
+ "//y2024/constants:constants_fbs",
+ "//y2024/control_loops/superstructure/altitude:altitude_plants",
+ "//y2024/control_loops/superstructure/catapult:catapult_plants",
+ "//y2024/control_loops/superstructure/turret:turret_plants",
+ ],
+)
+
+cc_library(
+ name = "aiming",
+ srcs = [
+ "aiming.cc",
+ ],
+ hdrs = [
+ "aiming.h",
+ ],
+ deps = [
+ ":superstructure_status_fbs",
+ "//frc971/control_loops:static_zeroing_single_dof_profiled_subsystem",
+ "//frc971/control_loops/aiming",
+ "//frc971/control_loops/drivetrain:drivetrain_status_fbs",
+ "//y2024:constants",
+ "//y2024/constants:constants_fbs",
+ "//y2024/control_loops/drivetrain:drivetrain_base",
+ ],
+)
+
cc_binary(
name = "superstructure_replay",
srcs = ["superstructure_replay.cc"],
diff --git a/y2024/control_loops/superstructure/aiming.cc b/y2024/control_loops/superstructure/aiming.cc
new file mode 100644
index 0000000..980efba
--- /dev/null
+++ b/y2024/control_loops/superstructure/aiming.cc
@@ -0,0 +1,84 @@
+#include "y2024/control_loops/superstructure/aiming.h"
+
+#include "frc971/control_loops/aiming/aiming.h"
+#include "frc971/control_loops/pose.h"
+
+using frc971::control_loops::aiming::RobotState;
+using frc971::control_loops::aiming::ShotConfig;
+using frc971::control_loops::aiming::ShotMode;
+using y2024::control_loops::superstructure::Aimer;
+
+Aimer::Aimer(aos::EventLoop *event_loop,
+ const y2024::Constants *robot_constants)
+ : event_loop_(event_loop),
+ robot_constants_(CHECK_NOTNULL(robot_constants)),
+ drivetrain_config_(
+ frc971::control_loops::drivetrain::DrivetrainConfig<double>::
+ FromFlatbuffer(*robot_constants_->common()->drivetrain())),
+ interpolation_table_(
+ y2024::constants::Values::InterpolationTableFromFlatbuffer(
+ robot_constants_->common()->shooter_interpolation_table())),
+ joystick_state_fetcher_(
+ event_loop_->MakeFetcher<aos::JoystickState>("/aos")) {}
+
+void Aimer::Update(
+ const frc971::control_loops::drivetrain::Status *status,
+ frc971::control_loops::aiming::ShotMode shot_mode,
+ frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoalStatic
+ *turret_goal) {
+ const frc971::control_loops::Pose robot_pose({status->x(), status->y(), 0},
+ status->theta());
+ joystick_state_fetcher_.Fetch();
+ CHECK_NOTNULL(joystick_state_fetcher_.get());
+
+ aos::Alliance alliance = joystick_state_fetcher_->alliance();
+
+ const frc971::control_loops::Pose red_alliance_goal(
+ frc971::ToEigenOrDie<3, 1>(*robot_constants_->common()
+ ->shooter_targets()
+ ->red_alliance()
+ ->pos()),
+ robot_constants_->common()->shooter_targets()->red_alliance()->theta());
+
+ const frc971::control_loops::Pose blue_alliance_goal(
+ frc971::ToEigenOrDie<3, 1>(*robot_constants_->common()
+ ->shooter_targets()
+ ->blue_alliance()
+ ->pos()),
+ robot_constants_->common()->shooter_targets()->blue_alliance()->theta());
+
+ const frc971::control_loops::Pose goal =
+ alliance == aos::Alliance::kRed ? red_alliance_goal : blue_alliance_goal;
+
+ const Eigen::Vector2d linear_angular =
+ drivetrain_config_.Tlr_to_la() *
+ Eigen::Vector2d(status->estimated_left_velocity(),
+ status->estimated_right_velocity());
+ const double xdot = linear_angular(0) * std::cos(status->theta());
+ const double ydot = linear_angular(0) * std::sin(status->theta());
+
+ // Use the previous shot distance to estimate the speed-over-ground of the
+ // note.
+ current_goal_ = frc971::control_loops::aiming::AimerGoal(
+ ShotConfig{goal, shot_mode,
+ frc971::constants::Range::FromFlatbuffer(
+ robot_constants_->common()->turret()->range()),
+ interpolation_table_.Get(current_goal_.target_distance)
+ .shot_speed_over_ground,
+ /*wrap_mode=*/0.0, /*turret_zero_offset*/ 0.0},
+ RobotState{
+ robot_pose, {xdot, ydot}, linear_angular(1), current_goal_.position});
+
+ turret_goal->set_unsafe_goal(current_goal_.position);
+ turret_goal->set_goal_velocity(current_goal_.velocity);
+}
+
+flatbuffers::Offset<AimerStatus> Aimer::PopulateStatus(
+ flatbuffers::FlatBufferBuilder *fbb) const {
+ AimerStatus::Builder builder(*fbb);
+ 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());
+ return builder.Finish();
+}
diff --git a/y2024/control_loops/superstructure/aiming.h b/y2024/control_loops/superstructure/aiming.h
new file mode 100644
index 0000000..21d427a
--- /dev/null
+++ b/y2024/control_loops/superstructure/aiming.h
@@ -0,0 +1,51 @@
+#ifndef Y2024_CONTROL_LOOPS_SUPERSTRUCTURE_AIMING_H_
+#define Y2024_CONTROL_LOOPS_SUPERSTRUCTURE_AIMING_H_
+
+#include "frc971/control_loops/aiming/aiming.h"
+#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
+#include "frc971/control_loops/pose.h"
+#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h"
+#include "frc971/shooter_interpolation/interpolation.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_status_generated.h"
+
+using y2024::control_loops::superstructure::AimerStatus;
+
+namespace y2024::control_loops::superstructure {
+
+class Aimer {
+ public:
+ Aimer(aos::EventLoop *event_loop, const Constants *robot_constants);
+
+ void Update(
+ const frc971::control_loops::drivetrain::Status *status,
+ frc971::control_loops::aiming::ShotMode shot_mode,
+ frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystemGoalStatic
+ *turret_goal);
+
+ double DistanceToGoal() const { return current_goal_.virtual_shot_distance; }
+
+ flatbuffers::Offset<AimerStatus> PopulateStatus(
+ flatbuffers::FlatBufferBuilder *fbb) const;
+
+ private:
+ aos::EventLoop *event_loop_;
+
+ const Constants *robot_constants_;
+
+ frc971::control_loops::drivetrain::DrivetrainConfig<double>
+ drivetrain_config_;
+
+ frc971::shooter_interpolation::InterpolationTable<
+ y2024::constants::Values::ShotParams>
+ interpolation_table_;
+
+ aos::Fetcher<aos::JoystickState> joystick_state_fetcher_;
+
+ frc971::control_loops::aiming::TurretGoal current_goal_;
+};
+
+} // namespace y2024::control_loops::superstructure
+#endif // Y2024_CONTROL_LOOPS_SUPERSTRUCTURE_TURRET_AIMING_H_
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
diff --git a/y2024/control_loops/superstructure/shooter.h b/y2024/control_loops/superstructure/shooter.h
new file mode 100644
index 0000000..b381f3e
--- /dev/null
+++ b/y2024/control_loops/superstructure/shooter.h
@@ -0,0 +1,108 @@
+#ifndef Y2024_CONTROL_LOOPS_SUPERSTRUCTURE_SHOOTER_H_
+#define Y2024_CONTROL_LOOPS_SUPERSTRUCTURE_SHOOTER_H_
+
+#include "frc971/control_loops/catapult/catapult.h"
+#include "frc971/control_loops/catapult/catapult_goal_static.h"
+#include "frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h"
+#include "frc971/shooter_interpolation/interpolation.h"
+#include "frc971/zeroing/pot_and_absolute_encoder.h"
+#include "y2024/constants.h"
+#include "y2024/constants/constants_generated.h"
+#include "y2024/control_loops/superstructure/aiming.h"
+#include "y2024/control_loops/superstructure/superstructure_can_position_generated.h"
+#include "y2024/control_loops/superstructure/superstructure_goal_generated.h"
+#include "y2024/control_loops/superstructure/superstructure_position_generated.h"
+#include "y2024/control_loops/superstructure/superstructure_status_generated.h"
+
+namespace y2024::control_loops::superstructure {
+
+// The shooter class will control the various subsystems involved in the
+// shooter- the turret, altitude, and catapult.
+class Shooter {
+ public:
+ using PotAndAbsoluteEncoderSubsystem =
+ ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystem<
+ ::frc971::zeroing::PotAndAbsoluteEncoderZeroingEstimator,
+ ::frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus>;
+
+ using CatapultSubsystem =
+ ::frc971::control_loops::StaticZeroingSingleDOFProfiledSubsystem<
+ ::frc971::zeroing::PotAndAbsoluteEncoderZeroingEstimator,
+ ::frc971::control_loops::PotAndAbsoluteEncoderProfiledJointStatus,
+ ::frc971::zeroing::PotAndAbsoluteEncoderZeroingEstimator,
+ aos::util::AsymmetricTrapezoidProfile>;
+
+ Shooter(aos::EventLoop *event_loop, const Constants *robot_constants);
+
+ void Reset() {
+ catapult_.Reset();
+ turret_.Reset();
+ altitude_.Reset();
+ }
+
+ void Estop() {
+ catapult_.Estop();
+ turret_.Estop();
+ altitude_.Estop();
+ }
+
+ bool zeroed() {
+ return catapult_.zeroed() && turret_.zeroed() && altitude_.zeroed();
+ }
+
+ bool estopped() {
+ return catapult_.estopped() && turret_.estopped() && altitude_.estopped();
+ }
+
+ inline const PotAndAbsoluteEncoderSubsystem &turret() const {
+ return turret_;
+ }
+
+ inline const PotAndAbsoluteEncoderSubsystem &altitude() const {
+ return altitude_;
+ }
+
+ flatbuffers::Offset<ShooterStatus> Iterate(
+ const Position *position, const 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);
+
+ private:
+ CatapultState state_ = CatapultState::RETRACTING;
+
+ bool CatapultClose() const {
+ return (std::abs(catapult_.estimated_position() -
+ catapult_.unprofiled_goal(0, 0)) < 0.05 &&
+ std::abs(catapult_.estimated_velocity()) < 0.5);
+ }
+
+ aos::Fetcher<frc971::control_loops::drivetrain::Status>
+ drivetrain_status_fetcher_;
+
+ aos::Fetcher<y2024::control_loops::superstructure::CANPosition>
+ superstructure_can_position_fetcher_;
+
+ const Constants *robot_constants_;
+
+ CatapultSubsystem catapult_;
+
+ PotAndAbsoluteEncoderSubsystem turret_;
+ PotAndAbsoluteEncoderSubsystem altitude_;
+
+ Aimer aimer_;
+
+ frc971::shooter_interpolation::InterpolationTable<
+ y2024::constants::Values::ShotParams>
+ interpolation_table_;
+
+ double last_retention_position_;
+
+ aos::monotonic_clock::time_point last_timestamp_{
+ aos::monotonic_clock::min_time};
+};
+
+} // namespace y2024::control_loops::superstructure
+
+#endif
diff --git a/y2024/control_loops/superstructure/superstructure.cc b/y2024/control_loops/superstructure/superstructure.cc
index 43a2453..5ac705b 100644
--- a/y2024/control_loops/superstructure/superstructure.cc
+++ b/y2024/control_loops/superstructure/superstructure.cc
@@ -35,7 +35,8 @@
robot_constants_->robot()->intake_constants()),
climber_(
robot_constants_->common()->climber(),
- robot_constants_->robot()->climber_constants()->zeroing_constants()) {
+ robot_constants_->robot()->climber_constants()->zeroing_constants()),
+ shooter_(event_loop, robot_constants_) {
event_loop->SetRuntimeRealtimePriority(30);
}
@@ -46,13 +47,11 @@
const monotonic_clock::time_point timestamp =
event_loop()->context().monotonic_event_time;
- (void)timestamp;
- (void)position;
-
if (WasReset()) {
AOS_LOG(ERROR, "WPILib reset, restarting\n");
intake_pivot_.Reset();
climber_.Reset();
+ shooter_.Reset();
}
OutputT output_struct;
@@ -175,14 +174,26 @@
output != nullptr ? &output_struct.climber_voltage : nullptr,
status->fbb());
+ const flatbuffers::Offset<ShooterStatus> shooter_status_offset =
+ shooter_.Iterate(
+ position,
+ unsafe_goal != nullptr ? unsafe_goal->shooter_goal() : nullptr,
+ output != nullptr ? &output_struct.catapult_voltage : nullptr,
+ output != nullptr ? &output_struct.altitude_voltage : nullptr,
+ output != nullptr ? &output_struct.turret_voltage : nullptr,
+ output != nullptr ? &output_struct.retention_roller_voltage : nullptr,
+ robot_state().voltage_battery(), timestamp, status->fbb());
+
if (output) {
output->CheckOk(output->Send(Output::Pack(*output->fbb(), &output_struct)));
}
Status::Builder status_builder = status->MakeBuilder<Status>();
- const bool zeroed = intake_pivot_.zeroed() && climber_.zeroed();
- const bool estopped = intake_pivot_.estopped() || climber_.estopped();
+ const bool zeroed =
+ intake_pivot_.zeroed() && climber_.zeroed() && shooter_.zeroed();
+ const bool estopped =
+ intake_pivot_.estopped() || climber_.estopped() || shooter_.estopped();
status_builder.add_zeroed(zeroed);
status_builder.add_estopped(estopped);
@@ -190,6 +201,7 @@
status_builder.add_intake_pivot(intake_pivot_status_offset);
status_builder.add_transfer_roller(transfer_roller_state);
status_builder.add_climber(climber_status_offset);
+ status_builder.add_shooter(shooter_status_offset);
(void)status->Send(status_builder.Finish());
}
diff --git a/y2024/control_loops/superstructure/superstructure.h b/y2024/control_loops/superstructure/superstructure.h
index f85e0fc..6c43863 100644
--- a/y2024/control_loops/superstructure/superstructure.h
+++ b/y2024/control_loops/superstructure/superstructure.h
@@ -11,6 +11,7 @@
#include "frc971/zeroing/pot_and_absolute_encoder.h"
#include "y2024/constants.h"
#include "y2024/constants/constants_generated.h"
+#include "y2024/control_loops/superstructure/shooter.h"
#include "y2024/control_loops/superstructure/superstructure_goal_generated.h"
#include "y2024/control_loops/superstructure/superstructure_output_generated.h"
#include "y2024/control_loops/superstructure/superstructure_position_generated.h"
@@ -43,6 +44,8 @@
return climber_;
}
+ inline const Shooter &shooter() const { return shooter_; }
+
double robot_velocity() const;
protected:
@@ -64,6 +67,8 @@
AbsoluteEncoderSubsystem intake_pivot_;
PotAndAbsoluteEncoderSubsystem climber_;
+ Shooter shooter_;
+
DISALLOW_COPY_AND_ASSIGN(Superstructure);
};
diff --git a/y2024/control_loops/superstructure/superstructure_goal.fbs b/y2024/control_loops/superstructure/superstructure_goal.fbs
index 4d46d1e..39f108a 100644
--- a/y2024/control_loops/superstructure/superstructure_goal.fbs
+++ b/y2024/control_loops/superstructure/superstructure_goal.fbs
@@ -46,6 +46,9 @@
// Position for the altitude when we aren't auto aiming
altitude_position: frc971.control_loops.StaticZeroingSingleDOFProfiledSubsystemGoal (id: 4);
+
+ // If true, we started with the ball loaded and should proceed to that state.
+ preloaded:bool = false (id: 5);
}
// Represents goal for extend
diff --git a/y2024/control_loops/superstructure/superstructure_lib_test.cc b/y2024/control_loops/superstructure/superstructure_lib_test.cc
index 78bbbdd..fbabcfb 100644
--- a/y2024/control_loops/superstructure/superstructure_lib_test.cc
+++ b/y2024/control_loops/superstructure/superstructure_lib_test.cc
@@ -5,6 +5,7 @@
#include "aos/events/logging/log_writer.h"
#include "frc971/control_loops/capped_test_plant.h"
+#include "frc971/control_loops/catapult/catapult_goal_static.h"
#include "frc971/control_loops/control_loop_test.h"
#include "frc971/control_loops/position_sensor_sim.h"
#include "frc971/control_loops/subsystem_simulator.h"
@@ -12,9 +13,12 @@
#include "frc971/zeroing/absolute_encoder.h"
#include "y2024/constants/simulated_constants_sender.h"
#include "y2024/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
+#include "y2024/control_loops/superstructure/altitude/altitude_plant.h"
+#include "y2024/control_loops/superstructure/catapult/catapult_plant.h"
#include "y2024/control_loops/superstructure/climber/climber_plant.h"
#include "y2024/control_loops/superstructure/intake_pivot/intake_pivot_plant.h"
#include "y2024/control_loops/superstructure/superstructure.h"
+#include "y2024/control_loops/superstructure/turret/turret_plant.h"
DEFINE_string(output_folder, "",
"If set, logs all channels to the provided logfile.");
@@ -53,6 +57,8 @@
dt_(dt),
superstructure_position_sender_(
event_loop_->MakeSender<Position>("/superstructure")),
+ superstructure_can_position_sender_(
+ event_loop_->MakeSender<CANPosition>("/superstructure/rio")),
superstructure_status_fetcher_(
event_loop_->MakeFetcher<Status>("/superstructure")),
superstructure_output_fetcher_(
@@ -91,7 +97,67 @@
->climber_constants()
->zeroing_constants()
->measured_absolute_position(),
- dt_) {
+ dt_),
+ catapult_(new CappedTestPlant(catapult::MakeCatapultPlant()),
+ PositionSensorSimulator(simulated_robot_constants->robot()
+ ->catapult_constants()
+ ->zeroing_constants()
+ ->one_revolution_distance()),
+ {.subsystem_params =
+ {simulated_robot_constants->common()->catapult(),
+ simulated_robot_constants->robot()
+ ->catapult_constants()
+ ->zeroing_constants()},
+ .potentiometer_offset = simulated_robot_constants->robot()
+ ->catapult_constants()
+ ->potentiometer_offset()},
+ frc971::constants::Range::FromFlatbuffer(
+ simulated_robot_constants->common()->catapult()->range()),
+ simulated_robot_constants->robot()
+ ->catapult_constants()
+ ->zeroing_constants()
+ ->measured_absolute_position(),
+ dt_),
+ altitude_(new CappedTestPlant(altitude::MakeAltitudePlant()),
+ PositionSensorSimulator(simulated_robot_constants->robot()
+ ->altitude_constants()
+ ->zeroing_constants()
+ ->one_revolution_distance()),
+ {.subsystem_params =
+ {simulated_robot_constants->common()->altitude(),
+ simulated_robot_constants->robot()
+ ->altitude_constants()
+ ->zeroing_constants()},
+ .potentiometer_offset = simulated_robot_constants->robot()
+ ->altitude_constants()
+ ->potentiometer_offset()},
+ frc971::constants::Range::FromFlatbuffer(
+ simulated_robot_constants->common()->altitude()->range()),
+ simulated_robot_constants->robot()
+ ->altitude_constants()
+ ->zeroing_constants()
+ ->measured_absolute_position(),
+ dt_),
+ turret_(
+ new CappedTestPlant(turret::MakeTurretPlant()),
+ PositionSensorSimulator(simulated_robot_constants->robot()
+ ->turret_constants()
+ ->zeroing_constants()
+ ->one_revolution_distance()),
+ {.subsystem_params = {simulated_robot_constants->common()->turret(),
+ simulated_robot_constants->robot()
+ ->turret_constants()
+ ->zeroing_constants()},
+ .potentiometer_offset = simulated_robot_constants->robot()
+ ->turret_constants()
+ ->potentiometer_offset()},
+ frc971::constants::Range::FromFlatbuffer(
+ simulated_robot_constants->common()->turret()->range()),
+ simulated_robot_constants->robot()
+ ->turret_constants()
+ ->zeroing_constants()
+ ->measured_absolute_position(),
+ dt_) {
intake_pivot_.InitializePosition(
frc971::constants::Range::FromFlatbuffer(
simulated_robot_constants->common()->intake_pivot()->range())
@@ -100,6 +166,18 @@
frc971::constants::Range::FromFlatbuffer(
simulated_robot_constants->common()->climber()->range())
.middle());
+ catapult_.InitializePosition(
+ frc971::constants::Range::FromFlatbuffer(
+ simulated_robot_constants->common()->catapult()->range())
+ .middle());
+ altitude_.InitializePosition(
+ frc971::constants::Range::FromFlatbuffer(
+ simulated_robot_constants->common()->altitude()->range())
+ .middle());
+ turret_.InitializePosition(
+ frc971::constants::Range::FromFlatbuffer(
+ simulated_robot_constants->common()->turret()->range())
+ .middle());
phased_loop_handle_ = event_loop_->AddPhasedLoop(
[this](int) {
// Skip this the first time.
@@ -113,9 +191,21 @@
climber_.Simulate(superstructure_output_fetcher_->climber_voltage(),
superstructure_status_fetcher_->climber());
+ catapult_.Simulate(
+ superstructure_output_fetcher_->catapult_voltage(),
+ superstructure_status_fetcher_->shooter()->catapult());
+
+ altitude_.Simulate(
+ superstructure_output_fetcher_->altitude_voltage(),
+ superstructure_status_fetcher_->shooter()->altitude());
+
+ turret_.Simulate(
+ superstructure_output_fetcher_->turret_voltage(),
+ superstructure_status_fetcher_->shooter()->turret());
}
first_ = false;
SendPositionMessage();
+ SendCANPositionMessage();
},
dt);
}
@@ -132,25 +222,79 @@
frc971::PotAndAbsolutePosition::Builder climber_builder =
builder.MakeBuilder<frc971::PotAndAbsolutePosition>();
+
flatbuffers::Offset<frc971::PotAndAbsolutePosition> climber_offset =
climber_.encoder()->GetSensorValues(&climber_builder);
+ frc971::PotAndAbsolutePosition::Builder catapult_builder =
+ builder.MakeBuilder<frc971::PotAndAbsolutePosition>();
+
+ flatbuffers::Offset<frc971::PotAndAbsolutePosition> catapult_offset =
+ catapult_.encoder()->GetSensorValues(&catapult_builder);
+
+ frc971::PotAndAbsolutePosition::Builder altitude_builder =
+ builder.MakeBuilder<frc971::PotAndAbsolutePosition>();
+ flatbuffers::Offset<frc971::PotAndAbsolutePosition> altitude_offset =
+ altitude_.encoder()->GetSensorValues(&altitude_builder);
+
+ frc971::PotAndAbsolutePosition::Builder turret_builder =
+ builder.MakeBuilder<frc971::PotAndAbsolutePosition>();
+ flatbuffers::Offset<frc971::PotAndAbsolutePosition> turret_offset =
+ turret_.encoder()->GetSensorValues(&turret_builder);
Position::Builder position_builder = builder.MakeBuilder<Position>();
position_builder.add_transfer_beambreak(transfer_beambreak_);
position_builder.add_intake_pivot(intake_pivot_offset);
+ position_builder.add_catapult(catapult_offset);
+ position_builder.add_altitude(altitude_offset);
+ position_builder.add_turret(turret_offset);
position_builder.add_climber(climber_offset);
CHECK_EQ(builder.Send(position_builder.Finish()),
aos::RawSender::Error::kOk);
}
+ void SendCANPositionMessage() {
+ retention_position_ =
+ retention_velocity_ * std::chrono::duration<double>(dt_).count() +
+ retention_position_;
+
+ auto builder = superstructure_can_position_sender_.MakeBuilder();
+
+ frc971::control_loops::CANTalonFX::Builder retention_builder =
+ builder.MakeBuilder<frc971::control_loops::CANTalonFX>();
+
+ retention_builder.add_torque_current(retention_torque_current_);
+ retention_builder.add_position(retention_position_);
+
+ flatbuffers::Offset<frc971::control_loops::CANTalonFX> retention_offset =
+ retention_builder.Finish();
+
+ CANPosition::Builder can_position_builder =
+ builder.MakeBuilder<CANPosition>();
+
+ can_position_builder.add_retention_roller(retention_offset);
+
+ CHECK_EQ(builder.Send(can_position_builder.Finish()),
+ aos::RawSender::Error::kOk);
+ }
+
void set_transfer_beambreak(bool triggered) {
transfer_beambreak_ = triggered;
}
- AbsoluteEncoderSimulator *intake_pivot() { return &intake_pivot_; }
+ void set_retention_velocity(double velocity) {
+ retention_velocity_ = velocity;
+ }
+ void set_retention_torque_current(double torque_current) {
+ retention_torque_current_ = torque_current;
+ }
+
+ AbsoluteEncoderSimulator *intake_pivot() { return &intake_pivot_; }
+ PotAndAbsoluteEncoderSimulator *catapult() { return &catapult_; }
+ PotAndAbsoluteEncoderSimulator *altitude() { return &altitude_; }
+ PotAndAbsoluteEncoderSimulator *turret() { return &turret_; }
PotAndAbsoluteEncoderSimulator *climber() { return &climber_; }
private:
@@ -159,6 +303,7 @@
::aos::PhasedLoopHandler *phased_loop_handle_ = nullptr;
::aos::Sender<Position> superstructure_position_sender_;
+ ::aos::Sender<CANPosition> superstructure_can_position_sender_;
::aos::Fetcher<Status> superstructure_status_fetcher_;
::aos::Fetcher<Output> superstructure_output_fetcher_;
@@ -166,8 +311,15 @@
AbsoluteEncoderSimulator intake_pivot_;
PotAndAbsoluteEncoderSimulator climber_;
+ PotAndAbsoluteEncoderSimulator catapult_;
+ PotAndAbsoluteEncoderSimulator altitude_;
+ PotAndAbsoluteEncoderSimulator turret_;
bool first_ = true;
+
+ double retention_velocity_;
+ double retention_position_;
+ double retention_torque_current_;
};
class SuperstructureTest : public ::frc971::testing::ControlLoopTest {
@@ -199,6 +351,8 @@
test_event_loop_->MakeFetcher<Position>("/superstructure")),
superstructure_position_sender_(
test_event_loop_->MakeSender<Position>("/superstructure")),
+ superstructure_can_position_sender_(
+ test_event_loop_->MakeSender<CANPosition>("/superstructure/rio")),
drivetrain_status_sender_(
test_event_loop_->MakeSender<DrivetrainStatus>("/drivetrain")),
superstructure_plant_event_loop_(MakeEventLoop("plant", roborio_)),
@@ -244,6 +398,39 @@
superstructure_status_fetcher_->intake_pivot()->position(),
0.001);
+ if (superstructure_goal_fetcher_->has_shooter_goal()) {
+ if (superstructure_goal_fetcher_->shooter_goal()->has_turret_position() &&
+ !superstructure_goal_fetcher_->shooter_goal()->auto_aim()) {
+ EXPECT_NEAR(
+ superstructure_goal_fetcher_->shooter_goal()
+ ->turret_position()
+ ->unsafe_goal(),
+ superstructure_status_fetcher_->shooter()->turret()->position(),
+ 0.001);
+ }
+ }
+
+ if (superstructure_goal_fetcher_->has_shooter_goal()) {
+ if (superstructure_goal_fetcher_->shooter_goal()
+ ->has_altitude_position() &&
+ !superstructure_goal_fetcher_->shooter_goal()->auto_aim()) {
+ EXPECT_NEAR(
+ superstructure_goal_fetcher_->shooter_goal()
+ ->altitude_position()
+ ->unsafe_goal(),
+ superstructure_status_fetcher_->shooter()->altitude()->position(),
+ 0.001);
+ EXPECT_NEAR(superstructure_goal_fetcher_->shooter_goal()
+ ->altitude_position()
+ ->unsafe_goal(),
+ superstructure_plant_.altitude()->position(), 0.001);
+ }
+ }
+
+ EXPECT_NEAR(set_point,
+ superstructure_status_fetcher_->intake_pivot()->position(),
+ 0.001);
+
if (superstructure_status_fetcher_->intake_roller() ==
IntakeRollerStatus::NONE) {
EXPECT_EQ(superstructure_output_fetcher_->intake_roller_voltage(), 0.0);
@@ -291,6 +478,25 @@
!superstructure_status_fetcher_.get()->zeroed());
}
+ void WaitUntilNear(double turret_goal, double altitude_goal) {
+ int i = 0;
+ do {
+ i++;
+ RunFor(dt());
+ superstructure_status_fetcher_.Fetch();
+ // 2 Seconds
+
+ ASSERT_LE(i, 2.0 / ::aos::time::DurationInSeconds(dt()));
+
+ // Since there is a delay when sending running, make sure we have a
+ // status before checking it.
+ } while (superstructure_status_fetcher_.get() == nullptr ||
+ std::abs(superstructure_plant_.altitude()->position() -
+ altitude_goal) > 1e-3 ||
+ std::abs(superstructure_plant_.turret()->position() -
+ turret_goal) > 1e-3);
+ }
+
void SendRobotVelocity(double robot_velocity) {
SendDrivetrainStatus(robot_velocity, {0.0, 0.0}, 0.0);
}
@@ -329,6 +535,7 @@
::aos::Fetcher<Output> superstructure_output_fetcher_;
::aos::Fetcher<Position> superstructure_position_fetcher_;
::aos::Sender<Position> superstructure_position_sender_;
+ ::aos::Sender<CANPosition> superstructure_can_position_sender_;
::aos::Sender<DrivetrainStatus> drivetrain_status_sender_;
::std::unique_ptr<::aos::EventLoop> superstructure_plant_event_loop_;
@@ -347,12 +554,46 @@
SetEnabled(true);
WaitUntilZeroed();
+ superstructure_plant_.turret()->InitializePosition(
+ frc971::constants::Range::FromFlatbuffer(
+ simulated_robot_constants_->common()->turret()->range())
+ .middle());
+ superstructure_plant_.altitude()->InitializePosition(
+ frc971::constants::Range::FromFlatbuffer(
+ simulated_robot_constants_->common()->altitude()->range())
+ .middle());
+
{
auto builder = superstructure_goal_sender_.MakeBuilder();
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ turret_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(),
+ frc971::constants::Range::FromFlatbuffer(
+ simulated_robot_constants_->common()->turret()->range())
+ .middle());
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ altitude_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(),
+ frc971::constants::Range::FromFlatbuffer(
+ simulated_robot_constants_->common()->altitude()->range())
+ .middle());
+
+ ShooterGoal::Builder shooter_goal_builder =
+ builder.MakeBuilder<ShooterGoal>();
+
+ shooter_goal_builder.add_turret_position(turret_offset);
+ shooter_goal_builder.add_altitude_position(altitude_offset);
+ shooter_goal_builder.add_auto_aim(false);
+
+ flatbuffers::Offset<ShooterGoal> shooter_goal_offset =
+ shooter_goal_builder.Finish();
+
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_intake_pivot_goal(IntakePivotGoal::RETRACTED);
goal_builder.add_climber_goal(ClimberGoal::RETRACT);
+ goal_builder.add_shooter_goal(shooter_goal_offset);
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
@@ -369,18 +610,56 @@
superstructure_plant_.intake_pivot()->InitializePosition(
frc971::constants::Range::FromFlatbuffer(
simulated_robot_constants_->common()->intake_pivot()->range())
+ .lower);
+
+ superstructure_plant_.turret()->InitializePosition(
+ frc971::constants::Range::FromFlatbuffer(
+ simulated_robot_constants_->common()->turret()->range())
+ .lower);
+
+ superstructure_plant_.altitude()->InitializePosition(
+ frc971::constants::Range::FromFlatbuffer(
+ simulated_robot_constants_->common()->altitude()->range())
.middle());
+
superstructure_plant_.climber()->InitializePosition(
frc971::constants::Range::FromFlatbuffer(
simulated_robot_constants_->common()->climber()->range())
.lower);
+
WaitUntilZeroed();
+
{
auto builder = superstructure_goal_sender_.MakeBuilder();
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ turret_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(),
+ frc971::constants::Range::FromFlatbuffer(
+ simulated_robot_constants_->common()->turret()->range())
+ .upper);
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ altitude_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(),
+ frc971::constants::Range::FromFlatbuffer(
+ simulated_robot_constants_->common()->altitude()->range())
+ .upper);
+
+ ShooterGoal::Builder shooter_goal_builder =
+ builder.MakeBuilder<ShooterGoal>();
+
+ shooter_goal_builder.add_turret_position(turret_offset);
+ shooter_goal_builder.add_altitude_position(altitude_offset);
+ shooter_goal_builder.add_auto_aim(false);
+
+ flatbuffers::Offset<ShooterGoal> shooter_goal_offset =
+ shooter_goal_builder.Finish();
+
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_intake_pivot_goal(IntakePivotGoal::EXTENDED);
goal_builder.add_climber_goal(ClimberGoal::FULL_EXTEND);
+ goal_builder.add_shooter_goal(shooter_goal_offset);
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
@@ -401,9 +680,34 @@
{
auto builder = superstructure_goal_sender_.MakeBuilder();
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ turret_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(),
+ frc971::constants::Range::FromFlatbuffer(
+ simulated_robot_constants_->common()->turret()->range())
+ .upper);
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ altitude_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(),
+ frc971::constants::Range::FromFlatbuffer(
+ simulated_robot_constants_->common()->altitude()->range())
+ .upper);
+
+ ShooterGoal::Builder shooter_goal_builder =
+ builder.MakeBuilder<ShooterGoal>();
+
+ shooter_goal_builder.add_turret_position(turret_offset);
+ shooter_goal_builder.add_altitude_position(altitude_offset);
+ shooter_goal_builder.add_auto_aim(false);
+
+ flatbuffers::Offset<ShooterGoal> shooter_goal_offset =
+ shooter_goal_builder.Finish();
+
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_intake_pivot_goal(IntakePivotGoal::EXTENDED);
goal_builder.add_climber_goal(ClimberGoal::FULL_EXTEND);
+ goal_builder.add_shooter_goal(shooter_goal_offset);
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
@@ -415,9 +719,36 @@
{
auto builder = superstructure_goal_sender_.MakeBuilder();
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ turret_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(),
+ frc971::constants::Range::FromFlatbuffer(
+ simulated_robot_constants_->common()->turret()->range())
+ .lower,
+ CreateProfileParameters(*builder.fbb(), 20.0, 10));
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ altitude_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(),
+ frc971::constants::Range::FromFlatbuffer(
+ simulated_robot_constants_->common()->altitude()->range())
+ .lower,
+ CreateProfileParameters(*builder.fbb(), 20.0, 10));
+
+ ShooterGoal::Builder shooter_goal_builder =
+ builder.MakeBuilder<ShooterGoal>();
+
+ shooter_goal_builder.add_turret_position(turret_offset);
+ shooter_goal_builder.add_altitude_position(altitude_offset);
+ shooter_goal_builder.add_auto_aim(false);
+
+ flatbuffers::Offset<ShooterGoal> shooter_goal_offset =
+ shooter_goal_builder.Finish();
+
Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
goal_builder.add_intake_pivot_goal(IntakePivotGoal::RETRACTED);
goal_builder.add_climber_goal(ClimberGoal::RETRACT);
+ goal_builder.add_shooter_goal(shooter_goal_offset);
ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
}
@@ -437,6 +768,12 @@
EXPECT_EQ(PotAndAbsoluteEncoderSubsystem::State::RUNNING,
superstructure_.climber().state());
+
+ EXPECT_EQ(PotAndAbsoluteEncoderSubsystem::State::RUNNING,
+ superstructure_.shooter().turret().state());
+
+ EXPECT_EQ(PotAndAbsoluteEncoderSubsystem::State::RUNNING,
+ superstructure_.shooter().altitude().state());
}
// Tests that running disabled works
@@ -603,4 +940,399 @@
EXPECT_EQ(superstructure_output_fetcher_->transfer_roller_voltage(), 0.0);
}
+
+// Tests the full range of activities we need to be doing from loading ->
+// shooting
+TEST_F(SuperstructureTest, LoadingToShooting) {
+ SetEnabled(true);
+
+ WaitUntilZeroed();
+
+ constexpr double kTurretGoal = 2.0;
+ constexpr double kAltitudeGoal = 0.5;
+
+ set_alliance(aos::Alliance::kRed);
+ SendDrivetrainStatus(0.0, {0.0, 5.0}, 0.0);
+
+ // Auto aim, but don't fire.
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ ShooterGoal::Builder shooter_goal_builder =
+ builder.MakeBuilder<ShooterGoal>();
+
+ shooter_goal_builder.add_auto_aim(true);
+ shooter_goal_builder.add_fire(false);
+
+ flatbuffers::Offset<ShooterGoal> shooter_goal_offset =
+ shooter_goal_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_intake_pivot_goal(IntakePivotGoal::RETRACTED);
+ goal_builder.add_shooter_goal(shooter_goal_offset);
+
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
+ }
+
+ superstructure_plant_.set_retention_velocity(5);
+ superstructure_plant_.set_retention_torque_current(1);
+ superstructure_plant_.set_transfer_beambreak(false);
+
+ RunFor(chrono::seconds(5));
+
+ VerifyNearGoal();
+
+ EXPECT_NEAR(superstructure_status_fetcher_->shooter()->turret()->position(),
+ simulated_robot_constants_->common()->turret_loading_position(),
+ 0.01);
+
+ EXPECT_NEAR(superstructure_status_fetcher_->shooter()->altitude()->position(),
+ 0.0, 0.01);
+
+ EXPECT_EQ(superstructure_status_fetcher_->shooter()->catapult_state(),
+ CatapultState::READY);
+
+ // Now, extend the intake.
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ ShooterGoal::Builder shooter_goal_builder =
+ builder.MakeBuilder<ShooterGoal>();
+
+ shooter_goal_builder.add_auto_aim(true);
+ shooter_goal_builder.add_fire(false);
+
+ flatbuffers::Offset<ShooterGoal> shooter_goal_offset =
+ shooter_goal_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_intake_pivot_goal(IntakePivotGoal::EXTENDED);
+ goal_builder.add_intake_roller_goal(IntakeRollerGoal::INTAKE);
+ goal_builder.add_shooter_goal(shooter_goal_offset);
+
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
+ }
+
+ superstructure_plant_.set_transfer_beambreak(false);
+
+ RunFor(chrono::seconds(5));
+
+ VerifyNearGoal();
+
+ EXPECT_NEAR(superstructure_status_fetcher_->shooter()->turret()->position(),
+ simulated_robot_constants_->common()->turret_loading_position(),
+ 0.01);
+
+ EXPECT_NEAR(superstructure_status_fetcher_->shooter()->altitude()->position(),
+ 0.0, 0.01);
+
+ EXPECT_EQ(superstructure_status_fetcher_->shooter()->catapult_state(),
+ CatapultState::READY);
+
+ // Signal through the transfer roller that we got it.
+ superstructure_plant_.set_transfer_beambreak(true);
+
+ RunFor(chrono::seconds(5));
+
+ VerifyNearGoal();
+
+ EXPECT_EQ(superstructure_output_fetcher_->transfer_roller_voltage(), 0.0);
+
+ // Verify we are in the loading position.
+ EXPECT_NEAR(superstructure_status_fetcher_->shooter()->turret()->position(),
+ simulated_robot_constants_->common()->turret_loading_position(),
+ 0.01);
+
+ EXPECT_NEAR(superstructure_status_fetcher_->shooter()->altitude()->position(),
+ 0.0, 0.01);
+
+ EXPECT_EQ(superstructure_status_fetcher_->shooter()->catapult_state(),
+ CatapultState::READY);
+
+ // Set retention roller to show that we are loaded.
+ superstructure_plant_.set_retention_velocity(0.1);
+ superstructure_plant_.set_retention_torque_current(45);
+ superstructure_plant_.set_transfer_beambreak(true);
+
+ RunFor(chrono::seconds(5));
+
+ VerifyNearGoal();
+
+ // Should now be loaded.
+ EXPECT_EQ(superstructure_output_fetcher_->transfer_roller_voltage(), 0.0);
+
+ EXPECT_NEAR(superstructure_status_fetcher_->shooter()->altitude()->position(),
+ simulated_robot_constants_->common()->altitude_loading_position(),
+ 0.01);
+
+ EXPECT_EQ(superstructure_status_fetcher_->shooter()->catapult_state(),
+ CatapultState::LOADED);
+
+ // Fire. Start by triggering a motion and then firing all in 1 go.
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ turret_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), kTurretGoal);
+
+ flatbuffers::Offset<StaticZeroingSingleDOFProfiledSubsystemGoal>
+ altitude_offset = CreateStaticZeroingSingleDOFProfiledSubsystemGoal(
+ *builder.fbb(), kAltitudeGoal);
+
+ auto catapult_builder =
+ builder.MakeBuilder<frc971::control_loops::catapult::CatapultGoal>();
+ catapult_builder.add_shot_velocity(15.0);
+
+ const auto catapult_offset = catapult_builder.Finish();
+
+ ShooterGoal::Builder shooter_goal_builder =
+ builder.MakeBuilder<ShooterGoal>();
+
+ shooter_goal_builder.add_auto_aim(false);
+ shooter_goal_builder.add_fire(true);
+ shooter_goal_builder.add_catapult_goal(catapult_offset);
+ shooter_goal_builder.add_altitude_position(altitude_offset);
+ shooter_goal_builder.add_turret_position(turret_offset);
+
+ flatbuffers::Offset<ShooterGoal> shooter_goal_offset =
+ shooter_goal_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_intake_pivot_goal(IntakePivotGoal::RETRACTED);
+ goal_builder.add_shooter_goal(shooter_goal_offset);
+
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
+ }
+
+ superstructure_plant_.set_transfer_beambreak(false);
+
+ // Wait until the bot finishes auto-aiming.
+ WaitUntilNear(kTurretGoal, kAltitudeGoal);
+
+ RunFor(chrono::milliseconds(10));
+
+ // Make sure it stays at firing for a bit.
+ EXPECT_EQ(superstructure_status_fetcher_->shooter()->catapult_state(),
+ CatapultState::FIRING);
+
+ // Wheel should spin free again.
+ superstructure_plant_.set_retention_velocity(5);
+ superstructure_plant_.set_retention_torque_current(1);
+
+ RunFor(chrono::seconds(5));
+
+ // And we should be back to ready.
+ CHECK(superstructure_status_fetcher_.Fetch());
+ EXPECT_EQ(superstructure_status_fetcher_->shooter()->catapult_state(),
+ CatapultState::READY);
+
+ VerifyNearGoal();
+
+ EXPECT_NEAR(superstructure_status_fetcher_->shooter()->turret()->position(),
+ kTurretGoal, 0.001);
+
+ EXPECT_NEAR(superstructure_status_fetcher_->shooter()->altitude()->position(),
+ kAltitudeGoal, 0.001);
+
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ ShooterGoal::Builder shooter_goal_builder =
+ builder.MakeBuilder<ShooterGoal>();
+
+ shooter_goal_builder.add_auto_aim(false);
+ shooter_goal_builder.add_fire(false);
+
+ flatbuffers::Offset<ShooterGoal> shooter_goal_offset =
+ shooter_goal_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_shooter_goal(shooter_goal_offset);
+
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
+ }
+
+ superstructure_plant_.set_retention_velocity(5);
+ superstructure_plant_.set_retention_torque_current(45);
+ superstructure_plant_.set_transfer_beambreak(false);
+
+ RunFor(chrono::seconds(5));
+
+ VerifyNearGoal();
+
+ EXPECT_EQ(superstructure_status_fetcher_->shooter()->catapult_state(),
+ CatapultState::READY);
+
+ EXPECT_NEAR(superstructure_status_fetcher_->shooter()->turret()->position(),
+ simulated_robot_constants_->common()->turret_loading_position(),
+ 0.01);
+
+ EXPECT_NEAR(superstructure_status_fetcher_->shooter()->altitude()->position(),
+ simulated_robot_constants_->common()->altitude_loading_position(),
+ 0.01);
+}
+
+// Test that we are able to signal that the note was preloaded
+TEST_F(SuperstructureTest, Preloaded) {
+ // Put the bucket at the starting position.
+ superstructure_plant_.catapult()->InitializePosition(
+ simulated_robot_constants_->common()->catapult_return_position());
+
+ SetEnabled(true);
+ WaitUntilZeroed();
+
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ ShooterGoal::Builder shooter_goal_builder =
+ builder.MakeBuilder<ShooterGoal>();
+
+ shooter_goal_builder.add_preloaded(true);
+
+ flatbuffers::Offset<ShooterGoal> shooter_goal_offset =
+ shooter_goal_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_shooter_goal(shooter_goal_offset);
+
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
+ }
+
+ RunFor(dt());
+
+ superstructure_status_fetcher_.Fetch();
+ ASSERT_TRUE(superstructure_status_fetcher_.get() != nullptr);
+
+ EXPECT_EQ(superstructure_status_fetcher_->shooter()->catapult_state(),
+ CatapultState::LOADED);
+}
+
+// Tests that auto aim works properly for the shooter
+TEST_F(SuperstructureTest, AutoAim) {
+ superstructure_plant_.catapult()->InitializePosition(
+ simulated_robot_constants_->common()->catapult_return_position());
+ SetEnabled(true);
+ WaitUntilZeroed();
+
+ constexpr double kDistanceFromSpeaker = 5.0;
+
+ const double kRedSpeakerX = simulated_robot_constants_->common()
+ ->shooter_targets()
+ ->red_alliance()
+ ->pos()
+ ->data()
+ ->Get(0);
+ const double kRedSpeakerY = simulated_robot_constants_->common()
+ ->shooter_targets()
+ ->red_alliance()
+ ->pos()
+ ->data()
+ ->Get(1);
+
+ const double kBlueSpeakerX = simulated_robot_constants_->common()
+ ->shooter_targets()
+ ->blue_alliance()
+ ->pos()
+ ->data()
+ ->Get(0);
+ const double kBlueSpeakerY = simulated_robot_constants_->common()
+ ->shooter_targets()
+ ->blue_alliance()
+ ->pos()
+ ->data()
+ ->Get(1);
+
+ set_alliance(aos::Alliance::kRed);
+ // Set the robot facing 90 degrees away from the speaker
+ SendDrivetrainStatus(
+ 0.0, {(kRedSpeakerX - kDistanceFromSpeaker), kRedSpeakerY}, M_PI_2);
+
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ ShooterGoal::Builder shooter_goal_builder =
+ builder.MakeBuilder<ShooterGoal>();
+
+ shooter_goal_builder.add_auto_aim(true);
+ shooter_goal_builder.add_preloaded(true);
+
+ flatbuffers::Offset<ShooterGoal> shooter_goal_offset =
+ shooter_goal_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_shooter_goal(shooter_goal_offset);
+
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
+ }
+
+ superstructure_plant_.set_retention_velocity(0);
+ superstructure_plant_.set_retention_torque_current(45);
+
+ RunFor(chrono::seconds(5));
+
+ VerifyNearGoal();
+
+ EXPECT_NEAR(
+ -M_PI_2,
+ superstructure_status_fetcher_->shooter()->aimer()->turret_position(),
+ 5e-4);
+ EXPECT_NEAR(-M_PI_2,
+ superstructure_status_fetcher_->shooter()->turret()->position(),
+ 5e-4);
+ LOG(INFO) << aos::FlatbufferToJson(superstructure_status_fetcher_.get(),
+ {.multi_line = true});
+
+ EXPECT_EQ(
+ kDistanceFromSpeaker,
+ superstructure_status_fetcher_->shooter()->aimer()->target_distance());
+
+ set_alliance(aos::Alliance::kBlue);
+ // Set the robot facing 90 degrees away from the speaker
+ SendDrivetrainStatus(
+ 0.0, {(kBlueSpeakerX + kDistanceFromSpeaker), kBlueSpeakerY}, M_PI_2);
+
+ {
+ auto builder = superstructure_goal_sender_.MakeBuilder();
+
+ ShooterGoal::Builder shooter_goal_builder =
+ builder.MakeBuilder<ShooterGoal>();
+
+ shooter_goal_builder.add_auto_aim(true);
+
+ flatbuffers::Offset<ShooterGoal> shooter_goal_offset =
+ shooter_goal_builder.Finish();
+
+ Goal::Builder goal_builder = builder.MakeBuilder<Goal>();
+
+ goal_builder.add_shooter_goal(shooter_goal_offset);
+
+ ASSERT_EQ(builder.Send(goal_builder.Finish()), aos::RawSender::Error::kOk);
+ }
+
+ superstructure_plant_.set_retention_velocity(0);
+ superstructure_plant_.set_retention_torque_current(45);
+
+ RunFor(chrono::seconds(5));
+
+ VerifyNearGoal();
+
+ EXPECT_NEAR(
+ M_PI_2,
+ superstructure_status_fetcher_->shooter()->aimer()->turret_position(),
+ 5e-4);
+ EXPECT_NEAR(M_PI_2,
+ superstructure_status_fetcher_->shooter()->turret()->position(),
+ 5e-4);
+ EXPECT_EQ(
+ kDistanceFromSpeaker,
+ superstructure_status_fetcher_->shooter()->aimer()->target_distance());
+}
+
} // namespace y2024::control_loops::superstructure::testing
diff --git a/y2024/control_loops/superstructure/superstructure_status.fbs b/y2024/control_loops/superstructure/superstructure_status.fbs
index bda9bbf..7b7912e 100644
--- a/y2024/control_loops/superstructure/superstructure_status.fbs
+++ b/y2024/control_loops/superstructure/superstructure_status.fbs
@@ -10,13 +10,27 @@
INTAKING = 2,
}
-enum CatapultStatus: ubyte {
+enum CatapultState: ubyte {
// Means we are waiting for a game piece
- IDLE = 0,
+ READY = 0,
// Means we have a game piece
LOADED = 1,
// Means we are firing a game piece
FIRING = 2,
+ // We are retracting the bucket to do it again.
+ RETRACTING = 3,
+}
+
+table AimerStatus {
+ // The current goal angle for the turret auto-tracking, in radians.
+ turret_position:double (id: 0);
+ // The current goal velocity for the turret, in radians / sec.
+ turret_velocity:double (id: 1);
+ // The current distance to the target, in meters.
+ target_distance:double (id: 2);
+ // 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);
}
table ShooterStatus {
@@ -29,7 +43,10 @@
// Estimated angle and angular velocitiy of the altitude.
altitude:frc971.control_loops.PotAndAbsoluteEncoderProfiledJointStatus (id: 2);
- catapult_state: CatapultStatus (id: 3);
+ catapult_state: CatapultState (id: 3);
+
+ // Status of the aimer
+ aimer:AimerStatus (id: 4);
}
// Contains status of transfer rollers