diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 90136d26..37632535 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -13,10 +13,13 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.commands.AddTunableApriltagReading; +import frc.robot.commands.ShootButton; import frc.robot.commands.AddApriltagReading; import frc.robot.commands.AddGarbageReading; import frc.robot.commands.climber.ClimberDown; import frc.robot.commands.climber.ClimberUp; +import frc.robot.commands.feeder.DefaultSpinFeeder; +import frc.robot.commands.hopper.DefaultSpinHopper; import frc.robot.commands.hopper.SpinHopper; import frc.robot.commands.drive.DriveDirectionTime; import frc.robot.commands.feeder.SpinFeeder; @@ -30,9 +33,12 @@ import frc.robot.commands.sequences.IntakeUpSequence; import frc.robot.autochooser.AutoChooser; import frc.robot.commands.angler.AimAngler; +import frc.robot.commands.angler.DefaultAnglerControl; import frc.robot.commands.angler.RunAnglerToReverseLimit; +import frc.robot.commands.shooter.DefaultShooterControl; import frc.robot.commands.auto.ExampleAuto; import frc.robot.commands.shooter.SetShootingState; +import frc.robot.commands.turret.DefaultTurretControl; import frc.robot.commands.turret.RunTurretToFwdLimit; import frc.robot.commands.turret.RunTurretToRevLimit; import frc.robot.commands.turret.SetTurretAngle; @@ -43,6 +49,7 @@ import frc.robot.constants.enums.ShootingState.ShootState; import frc.robot.subsystems.AnglerSubsystem; import frc.robot.subsystems.ApriltagSubsystem; +import frc.robot.subsystems.ControllerSubsystem; import frc.robot.subsystems.HopperSubsystem; import frc.robot.subsystems.IntakeDeployerSubsystem; import frc.robot.subsystems.FeederSubsystem; @@ -94,9 +101,11 @@ public class RobotContainer { private final FeederSubsystem feederSubsystem; private final ApriltagSubsystem apriltagSubsystem; private final ShooterSubsystem shooterSubsystem; + private final ControllerSubsystem controllerSubsystem; private RobotVisualizer robotVisualizer = null; private final HopperSubsystem hopperSubsystem; - private final TurretSubsystem turretSubsystem; + + private final TurretSubsystem turretSubsystem; private final IntakeDeployerSubsystem intakeDeployer; private SwerveSubsystem drivebase = null; private GyroSubsystem gyroSubsystem = null; @@ -142,7 +151,9 @@ public RobotContainer() { drivebase = !Constants.TESTBED ? new SwerveSubsystem( new File(Filesystem.getDeployDirectory(), "YAGSL"), swerveIMU) : null; apriltagSubsystem = !Constants.TESTBED ? new ApriltagSubsystem(ApriltagSubsystem.createRealIo(), drivebase) : null; - } + controllerSubsystem = !Constants.TESTBED ? new ControllerSubsystem(drivebase, this) : null; + + } case REPLAY -> { // rollerSubsystem = new RollerSubsystem(RollerSubsystem.createMockIo()); // tiltSubsystem = new TiltSubsystem(TiltSubsystem.createMockIo()); @@ -159,6 +170,8 @@ public RobotContainer() { // create the drive subsystem with null gyro (use default json) drivebase = !Constants.TESTBED ? new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "YAGSL"), null) : null; apriltagSubsystem = !Constants.TESTBED ? new ApriltagSubsystem(ApriltagSubsystem.createMockIo(), drivebase) : null; + controllerSubsystem = !Constants.TESTBED ? new ControllerSubsystem(drivebase, this) : null; + } case SIM -> { robotVisualizer = new RobotVisualizer(); @@ -177,6 +190,7 @@ public RobotContainer() { IntakeDeployerSubsystem.createSimIo(robotVisualizer));// No GyroSubsystem in REPLAY for now // create the drive subsystem with null gyro (use default json) drivebase = !Constants.TESTBED ? new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "YAGSL"), null) : null; + controllerSubsystem = !Constants.TESTBED ? new ControllerSubsystem(drivebase, this) : null; apriltagSubsystem = !Constants.TESTBED ? new ApriltagSubsystem(ApriltagSubsystem.createSimIo(), drivebase) : null; } @@ -275,9 +289,10 @@ private void configureBindings() { controller.povRight().onTrue(new SetShootingState(shootState, ShootState.FIXED_2)); controller.povDown().onTrue(new SetShootingState(shootState, ShootState.SHOOTING_HUB)); controller.povLeft().onTrue(new SetShootingState(shootState, ShootState.SHUTTLING)); - steerJoystick.trigger().whileTrue((new RunHopperAndFeeder(hopperSubsystem, feederSubsystem))); driveJoystick.trigger().whileTrue((new SetShootingState(shootState, ShootState.STOPPED))); - + if (controllerSubsystem != null) { + steerJoystick.trigger().whileTrue(new ShootButton(controllerSubsystem)); + } // Schedule `ExampleCommand` when `exampleCondition` changes to `true` // new Trigger(m_exampleSubsystem::exampleCondition) @@ -290,28 +305,32 @@ private void configureBindings() { // TODO: Clean this up a little - create command in method and only create the // one actually needed - //example default command for angler- disabled for now - if (false) { - new AimAngler( - anglerSubsystem, - () -> drivebase != null ? drivebase.getPose() : null, - shootState); - } + // example default command for angler- disabled for now + if (false) { + new AimAngler( + anglerSubsystem, + () -> drivebase != null ? drivebase.getPose() : null, + shootState); + } - { - if (!Constants.TESTBED) { - SwerveInputStream driveAngularVelocity = SwerveInputStream - .of(drivebase.getSwerveDrive(), - () -> driveJoystick.getY() * -1, - () -> driveJoystick.getX() * -1) - .withControllerRotationAxis(steerJoystick::getX) - .deadband(Constants.DEADBAND) - .scaleTranslation(0.8) - .allianceRelativeControl(true); - Command driveFieldOrientedAnglularVelocity = drivebase - .driveFieldOriented(driveAngularVelocity); - drivebase.setDefaultCommand(driveFieldOrientedAnglularVelocity); - } + if (controllerSubsystem != null) { + anglerSubsystem.setDefaultCommand(new DefaultAnglerControl(anglerSubsystem, controllerSubsystem)); + shooterSubsystem.setDefaultCommand(new DefaultShooterControl(shooterSubsystem, controllerSubsystem)); + turretSubsystem.setDefaultCommand(new DefaultTurretControl(turretSubsystem, controllerSubsystem)); + hopperSubsystem.setDefaultCommand(new DefaultSpinHopper(hopperSubsystem, controllerSubsystem)); + feederSubsystem.setDefaultCommand(new DefaultSpinFeeder(feederSubsystem, controllerSubsystem)); + } + + if (!Constants.TESTBED) { + SwerveInputStream driveAngularVelocity = SwerveInputStream.of(drivebase.getSwerveDrive(), + () -> driveJoystick.getY() * -1, + () -> driveJoystick.getX() * -1) + .withControllerRotationAxis(steerJoystick::getX) + .deadband(Constants.DEADBAND) + .scaleTranslation(0.8) + .allianceRelativeControl(true); + Command driveFieldOrientedAnglularVelocity = drivebase.driveFieldOriented(driveAngularVelocity); + drivebase.setDefaultCommand(driveFieldOrientedAnglularVelocity); } } diff --git a/src/main/java/frc/robot/commands/ShootButton.java b/src/main/java/frc/robot/commands/ShootButton.java new file mode 100644 index 00000000..26fa28e6 --- /dev/null +++ b/src/main/java/frc/robot/commands/ShootButton.java @@ -0,0 +1,31 @@ +package frc.robot.commands; + +import frc.robot.subsystems.ControllerSubsystem; +import frc.robot.utils.logging.commands.LoggableCommand; + +public class ShootButton extends LoggableCommand{ + + private final ControllerSubsystem controllerSubsystem; + + public ShootButton(ControllerSubsystem controllerSubsystem) { + this.controllerSubsystem = controllerSubsystem; + } + + @Override + public void end(boolean interrupted) { + controllerSubsystem.setDriverActivatedShooting(false); + } + + @Override + public void initialize() { + controllerSubsystem.setDriverActivatedShooting(true); + } + + @Override + public boolean isFinished() { + return false; + } + + + +} diff --git a/src/main/java/frc/robot/commands/angler/DefaultAnglerControl.java b/src/main/java/frc/robot/commands/angler/DefaultAnglerControl.java new file mode 100644 index 00000000..474b6a62 --- /dev/null +++ b/src/main/java/frc/robot/commands/angler/DefaultAnglerControl.java @@ -0,0 +1,27 @@ +package frc.robot.commands.angler; + +import frc.robot.subsystems.AnglerSubsystem; +import frc.robot.subsystems.ControllerSubsystem; +import frc.robot.utils.logging.commands.LoggableCommand; + +public class DefaultAnglerControl extends LoggableCommand { + + private final AnglerSubsystem anglerSubsystem; + private final ControllerSubsystem controllerSubsystem; + + public DefaultAnglerControl(AnglerSubsystem anglerSubsystem, ControllerSubsystem controllerSubsystem) { + this.anglerSubsystem = anglerSubsystem; + this.controllerSubsystem = controllerSubsystem; + addRequirements(anglerSubsystem); + } + + @Override + public void execute() { + anglerSubsystem.setAngle(controllerSubsystem.getTargetAnglerAngleDegrees()); + } + + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/feeder/DefaultSpinFeeder.java b/src/main/java/frc/robot/commands/feeder/DefaultSpinFeeder.java new file mode 100644 index 00000000..d413869f --- /dev/null +++ b/src/main/java/frc/robot/commands/feeder/DefaultSpinFeeder.java @@ -0,0 +1,46 @@ +package frc.robot.commands.feeder; + +import edu.wpi.first.wpilibj.Timer; +import frc.robot.constants.Constants; +import frc.robot.subsystems.ControllerSubsystem; +import frc.robot.subsystems.FeederSubsystem; +import frc.robot.utils.logging.commands.LoggableCommand; + +public class DefaultSpinFeeder extends LoggableCommand { + + private final FeederSubsystem subsystem; + private final Timer timer; + private final ControllerSubsystem controllerSubsystem; + + public DefaultSpinFeeder(FeederSubsystem subsystem, ControllerSubsystem controllerSubsystem) { + this.subsystem = subsystem; + this.controllerSubsystem = controllerSubsystem; + timer = new Timer(); + addRequirements(subsystem); + } + + @Override + public void initialize() { + timer.restart(); + } + + @Override + public void execute() { + if (controllerSubsystem.shouldFeederSpin()) { + subsystem.setSpeed(Constants.FEEDER_SPEED); + } else { + subsystem.stopMotors(); + } + } + + @Override + public boolean isFinished() { + return false; + } + + @Override + public void end(boolean interrupted) { + subsystem.stopMotors(); + } + +} diff --git a/src/main/java/frc/robot/commands/feeder/SpinFeeder.java b/src/main/java/frc/robot/commands/feeder/SpinFeeder.java index a7863160..442f5a63 100644 --- a/src/main/java/frc/robot/commands/feeder/SpinFeeder.java +++ b/src/main/java/frc/robot/commands/feeder/SpinFeeder.java @@ -3,11 +3,9 @@ import edu.wpi.first.wpilibj.Timer; import frc.robot.constants.Constants; import frc.robot.subsystems.FeederSubsystem; -import frc.robot.subsystems.IntakeSubsystem; -import frc.robot.utils.logging.TimeoutLogger; import frc.robot.utils.logging.commands.LoggableCommand; -public class SpinFeeder extends LoggableCommand { +public class SpinFeeder extends LoggableCommand { private final FeederSubsystem subsystem; private final Timer timer; diff --git a/src/main/java/frc/robot/commands/hopper/DefaultSpinHopper.java b/src/main/java/frc/robot/commands/hopper/DefaultSpinHopper.java new file mode 100644 index 00000000..1a18e543 --- /dev/null +++ b/src/main/java/frc/robot/commands/hopper/DefaultSpinHopper.java @@ -0,0 +1,46 @@ +package frc.robot.commands.hopper; + +import edu.wpi.first.wpilibj.Timer; +import frc.robot.constants.Constants; +import frc.robot.subsystems.ControllerSubsystem; +import frc.robot.subsystems.HopperSubsystem; +import frc.robot.utils.logging.commands.LoggableCommand; + +public class DefaultSpinHopper extends LoggableCommand { + + private final HopperSubsystem subsystem; + private final Timer timer; + private final ControllerSubsystem controllerSubsystem; + + public DefaultSpinHopper(HopperSubsystem subsystem, ControllerSubsystem controllerSubsystem) { + this.subsystem = subsystem; + this.controllerSubsystem = controllerSubsystem; + timer = new Timer(); + addRequirements(subsystem); + } + + @Override + public void initialize() { + timer.restart(); + } + + @Override + public void execute() { + if (controllerSubsystem.shouldHopperSpin()) { + subsystem.setSpeed(Constants.HOPPER_SPEED); + } else { + subsystem.stopMotors(); + } + } + + @Override + public boolean isFinished() { + return false; + } + + @Override + public void end(boolean interrupted) { + subsystem.stopMotors(); + } + +} diff --git a/src/main/java/frc/robot/commands/hopper/SpinHopper.java b/src/main/java/frc/robot/commands/hopper/SpinHopper.java index 72f8d596..0a697572 100644 --- a/src/main/java/frc/robot/commands/hopper/SpinHopper.java +++ b/src/main/java/frc/robot/commands/hopper/SpinHopper.java @@ -5,11 +5,11 @@ import frc.robot.subsystems.HopperSubsystem; import frc.robot.utils.logging.commands.LoggableCommand; -public class SpinHopper extends LoggableCommand{ +public class SpinHopper extends LoggableCommand { public final HopperSubsystem subsystem; public final Timer timer; - + public SpinHopper(HopperSubsystem subsystem){ timer = new Timer(); @@ -43,5 +43,5 @@ public boolean isFinished() { } - + } diff --git a/src/main/java/frc/robot/commands/shooter/DefaultShooterControl.java b/src/main/java/frc/robot/commands/shooter/DefaultShooterControl.java new file mode 100644 index 00000000..a37176e3 --- /dev/null +++ b/src/main/java/frc/robot/commands/shooter/DefaultShooterControl.java @@ -0,0 +1,32 @@ +package frc.robot.commands.shooter; + +import frc.robot.subsystems.ControllerSubsystem; +import frc.robot.subsystems.ShooterSubsystem; +import frc.robot.utils.logging.commands.LoggableCommand; + +public class DefaultShooterControl extends LoggableCommand { + + private final ShooterSubsystem shooterSubsystem; + private final ControllerSubsystem controllerSubsystem; + + public DefaultShooterControl(ShooterSubsystem shooterSubsystem, ControllerSubsystem controllerSubsystem) { + this.shooterSubsystem = shooterSubsystem; + this.controllerSubsystem = controllerSubsystem; + addRequirements(shooterSubsystem); + } + + @Override + public void execute() { + double targetShooterVelocity = controllerSubsystem.getTargetShooterVelocityRpm(); + if (targetShooterVelocity > 0.0) { + shooterSubsystem.setPidVelocity(targetShooterVelocity); + } else { + shooterSubsystem.stopMotors(); + } + } + + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/turret/DefaultTurretControl.java b/src/main/java/frc/robot/commands/turret/DefaultTurretControl.java new file mode 100644 index 00000000..1fb25cdb --- /dev/null +++ b/src/main/java/frc/robot/commands/turret/DefaultTurretControl.java @@ -0,0 +1,27 @@ +package frc.robot.commands.turret; + +import frc.robot.subsystems.ControllerSubsystem; +import frc.robot.subsystems.TurretSubsystem; +import frc.robot.utils.logging.commands.LoggableCommand; + +public class DefaultTurretControl extends LoggableCommand { + + private final TurretSubsystem turretSubsystem; + private final ControllerSubsystem controllerSubsystem; + + public DefaultTurretControl(TurretSubsystem turretSubsystem, ControllerSubsystem controllerSubsystem) { + this.turretSubsystem = turretSubsystem; + this.controllerSubsystem = controllerSubsystem; + addRequirements(turretSubsystem); + } + + @Override + public void execute() { + turretSubsystem.setAngle(controllerSubsystem.getTargetTurretAngleDegrees()); + } + + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index ac3ac2c3..5e9cf45f 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -102,6 +102,7 @@ public enum Mode { public static final double ANGLER_ANGLE_LOW = 0; //Lowest angle position of Angler public static final double ANGLER_ANGLE_HIGH = 45; //Highest angle position of Angler public static final double ANGLER_FIXED_ROTATIONS = 0.1; //Fixed encoder position of Angler in Fixed ShootState + public static final double ANGLER_FIXED_ANGLE = 10; //Fixed encoder position of Angler in Fixed ShootState public static final double ANGLER_LIMIT_SPEED = 0.2; diff --git a/src/main/java/frc/robot/subsystems/ControllerSubsystem.java b/src/main/java/frc/robot/subsystems/ControllerSubsystem.java new file mode 100644 index 00000000..6f2b0a28 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/ControllerSubsystem.java @@ -0,0 +1,264 @@ +package frc.robot.subsystems; + +import org.littletonrobotics.junction.Logger; + +import frc.robot.RobotContainer; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.constants.Constants; +import frc.robot.constants.enums.ShootingState; +import frc.robot.constants.enums.ShootingState.ShootState; +import frc.robot.subsystems.swervedrive.SwerveSubsystem; + +public class ControllerSubsystem extends SubsystemBase { + + private static final double STOP_DELAY_SECONDS = 0.5; + + // Placeholder target poses until real field target values are finalized + private static final Pose2d HUB_TARGET_POSE = new Pose2d(4.0, 4.0, Rotation2d.kZero); + private static final Pose2d SHUTTLE_TARGET_POSE = new Pose2d(1.0, 7.0, Rotation2d.kZero); + + private static final String MANUAL_POSE_X_KEY = "controller/ManualPoseX"; + private static final String MANUAL_POSE_Y_KEY = "controller/ManualPoseY"; + private static final String USING_MANUAL_POSE_KEY = "controller/UsingManualPose"; + private static final String CURRENT_SHOOT_STATE_KEY = "controller/CurrentShootState"; + private static final String DISTANCE_METERS_KEY = "controller/CalculatedDistanceMeters"; + private static final String TARGET_ANGLER_ANGLE_KEY = "controller/TargetAnglerAngleDegrees"; + private static final String TARGET_SHOOTER_VELOCITY_KEY = "controller/TargetShooterVelocity"; + private static final String TARGET_TURRET_ANGLE_KEY = "controller/TargetTurretAngleDegrees"; + private static final String TARGET_FEEDER_SPEED_KEY = "controller/TargetFeederSpeed"; + private static final String TARGET_HOPPER_SPEED_KEY = "controller/TargetHopperSpeed"; + + // Placeholder fixed-state settings. + private static final ShotTargets STOPPED_TARGETS = + new ShotTargets(Constants.ANGLER_ANGLE_LOW, 0.0, 0.0, 0.0, false, false); + private static final ShotTargets FIXED_TARGETS = + new ShotTargets(10.0, 120.0, 5.0, 0.0, true, true); + private static final ShotTargets FIXED_2_TARGETS = + new ShotTargets(22.0, 180.0, -5.0, 0.0, true, true); + + // Placeholder pose-driven profiles. + private static final PoseControlProfile HUB_PROFILE = + new PoseControlProfile(HUB_TARGET_POSE, 32.0, 230.0, 14.0); + private static final PoseControlProfile SHUTTLE_PROFILE = + new PoseControlProfile(SHUTTLE_TARGET_POSE, 16.0, 90.0, -14.0); + + private final SwerveSubsystem drivebase; + private final RobotContainer robotContainer; + private final Timer stopDelayTimer = new Timer(); + + private ShootState previousState; + private ShotTargets activeTargets; + private boolean driverActivatedShooting = false; + + + public ControllerSubsystem(SwerveSubsystem drivebase, RobotContainer robotContainer) { + this.drivebase = drivebase; + this.robotContainer = robotContainer; + this.previousState = getCurrentShootState(); + this.activeTargets = STOPPED_TARGETS; + + SmartDashboard.putNumber(MANUAL_POSE_X_KEY, 0.0); + SmartDashboard.putNumber(MANUAL_POSE_Y_KEY, 0.0); + } + + @Override + public void periodic() { + Pose2d robotPose = getRobotPose(); + ShootState currentState = getCurrentShootState(); + + updateStopDelayState(currentState); + updateTargets(currentState, robotPose); + + SmartDashboard.putString(CURRENT_SHOOT_STATE_KEY, currentState.toString()); + SmartDashboard.putNumber(DISTANCE_METERS_KEY, activeTargets.distanceMeters); + SmartDashboard.putNumber(TARGET_ANGLER_ANGLE_KEY, activeTargets.anglerAngleDegrees); + SmartDashboard.putNumber(TARGET_SHOOTER_VELOCITY_KEY, activeTargets.shooterVelocityRpm); + SmartDashboard.putNumber(TARGET_TURRET_ANGLE_KEY, activeTargets.turretAngleDegrees); + SmartDashboard.putBoolean(TARGET_FEEDER_SPEED_KEY, activeTargets.feederSpin); + SmartDashboard.putBoolean(TARGET_HOPPER_SPEED_KEY, activeTargets.hopperSpin); + + Logger.recordOutput(CURRENT_SHOOT_STATE_KEY, currentState.toString()); + Logger.recordOutput(DISTANCE_METERS_KEY, activeTargets.distanceMeters); + Logger.recordOutput(TARGET_ANGLER_ANGLE_KEY, activeTargets.anglerAngleDegrees); + Logger.recordOutput(TARGET_SHOOTER_VELOCITY_KEY, activeTargets.shooterVelocityRpm); + Logger.recordOutput(TARGET_TURRET_ANGLE_KEY, activeTargets.turretAngleDegrees); + Logger.recordOutput(TARGET_FEEDER_SPEED_KEY, activeTargets.feederSpin); + Logger.recordOutput(TARGET_HOPPER_SPEED_KEY, activeTargets.hopperSpin); + + previousState = currentState; + } + + private ShootState getCurrentShootState() { + return robotContainer.getShootingState().getShootState(); + } + + private Pose2d getRobotPose() { + SmartDashboard.putBoolean(USING_MANUAL_POSE_KEY, shouldUseManualPose()); + Logger.recordOutput(USING_MANUAL_POSE_KEY, shouldUseManualPose()); + if (shouldUseManualPose()) { + return getManualPose(); + } + return drivebase.getPose(); + } + + private boolean shouldUseManualPose() { + return (Constants.currentMode == Constants.Mode.SIM) || Constants.TESTBED; + } + + private Pose2d getManualPose() { + double x = SmartDashboard.getNumber(MANUAL_POSE_X_KEY, 0.0); + double y = SmartDashboard.getNumber(MANUAL_POSE_Y_KEY, 0.0); + return new Pose2d(x, y, Rotation2d.kZero); + } + + private void updateStopDelayState(ShootState currentState) { + if (currentState == ShootState.STOPPED && previousState != ShootState.STOPPED) { + stopDelayTimer.restart(); + } + } + + private void updateTargets(ShootState state, Pose2d robotPose) { + switch (state) { + case STOPPED -> updateStoppedTargets(); + case FIXED -> useShotTargets(FIXED_TARGETS); + case FIXED_2 -> useShotTargets(FIXED_2_TARGETS); + case SHOOTING_HUB -> useShotTargets(calculateTargetsFromPose(HUB_PROFILE, robotPose)); + case SHUTTLING -> useShotTargets(calculateTargetsFromPose(SHUTTLE_PROFILE, robotPose)); + } + } + + private void updateStoppedTargets() { + //This makes the shooter wait half a second before stopping + double shooterVelocity = stopDelayTimer.hasElapsed(STOP_DELAY_SECONDS) + ? 0.0 + : activeTargets.shooterVelocityRpm; + + activeTargets = new ShotTargets( + STOPPED_TARGETS.anglerAngleDegrees, + shooterVelocity, + STOPPED_TARGETS.turretAngleDegrees, + STOPPED_TARGETS.distanceMeters, + STOPPED_TARGETS.feederSpin, + STOPPED_TARGETS.hopperSpin); + } + + private void useShotTargets(ShotTargets shotTargets) { + boolean driverEnabled = driverActivatedShootingEnabled(); + activeTargets = new ShotTargets( + shotTargets.anglerAngleDegrees, + shotTargets.shooterVelocityRpm, + shotTargets.turretAngleDegrees, + shotTargets.distanceMeters, + driverEnabled, + driverEnabled); + } + + private ShotTargets calculateTargetsFromPose(PoseControlProfile profile, Pose2d robotPose) { + double computedDistanceMeters = calculateDistanceMeters(robotPose, profile.targetPose); + double anglerAngleDegrees = calculateAnglerAngleDegrees(computedDistanceMeters, profile); + double shooterVelocity = calculateShooterVelocity(computedDistanceMeters, profile); + double turretAngleDegrees = calculateTurretAngleDegrees(robotPose, profile); + return new ShotTargets(anglerAngleDegrees, shooterVelocity, turretAngleDegrees, computedDistanceMeters, true, true); + } + + private double calculateDistanceMeters(Pose2d robotPose, Pose2d targetPose) { + return robotPose.getTranslation().getDistance(targetPose.getTranslation()); + } + + private double calculateAnglerAngleDegrees(double computedDistanceMeters, PoseControlProfile profile) { + // TODO: Replace with distance angler angle calculation. + return profile.defaultAnglerAngleDegrees; + } + + private double calculateShooterVelocity(double computedDistanceMeters, PoseControlProfile profile) { + // TODO: Replace with distance shooter velocity calculation. + return profile.defaultShooterVelocityRpm; + } + + private double calculateTurretAngleDegrees(Pose2d robotPose, PoseControlProfile profile) { + // TODO: Replace with distance turret angle calculation. + return profile.defaultTurretAngleDegrees; + } + + //Getters for all the subsystems to set posistion. + public double getTargetAnglerAngleDegrees() { + return activeTargets.anglerAngleDegrees; + } + + public double getTargetShooterVelocityRpm() { + return activeTargets.shooterVelocityRpm; + } + + public double getTargetTurretAngleDegrees() { + return activeTargets.turretAngleDegrees; + } + + public double getDistanceMeters() { + return activeTargets.distanceMeters; + } + + public boolean shouldFeederSpin() { + return activeTargets.feederSpin; + } + + public boolean shouldHopperSpin() { + return activeTargets.hopperSpin; + } + + public void setDriverActivatedShooting(boolean set) { + driverActivatedShooting = set; + } + + public boolean driverActivatedShootingEnabled() { + return driverActivatedShooting; + } + + + //Class to save all the fixed targets + private static final class ShotTargets { + private final double anglerAngleDegrees; + private final double shooterVelocityRpm; + private final double turretAngleDegrees; + private final double distanceMeters; + private final boolean feederSpin; + private final boolean hopperSpin; + + private ShotTargets( + double anglerAngleDegrees, + double shooterVelocityRpm, + double turretAngleDegrees, + double distanceMeters, + boolean feederSpin, + boolean hopperSpin) { + this.anglerAngleDegrees = anglerAngleDegrees; + this.shooterVelocityRpm = shooterVelocityRpm; + this.turretAngleDegrees = turretAngleDegrees; + this.distanceMeters = distanceMeters; + this.feederSpin = feederSpin; + this.hopperSpin = hopperSpin; + } + } + + //Class to save all the target poses and each subsystem position at that point so we can calculate true values later on + private static final class PoseControlProfile { + private final Pose2d targetPose; + private final double defaultAnglerAngleDegrees; + private final double defaultShooterVelocityRpm; + private final double defaultTurretAngleDegrees; + + private PoseControlProfile( + Pose2d targetPose, + double defaultAnglerAngleDegrees, + double defaultShooterVelocityRpm, + double defaultTurretAngleDegrees) { + this.targetPose = targetPose; + this.defaultAnglerAngleDegrees = defaultAnglerAngleDegrees; + this.defaultShooterVelocityRpm = defaultShooterVelocityRpm; + this.defaultTurretAngleDegrees = defaultTurretAngleDegrees; + } + } +}