| #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 |