diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 69b284a7..639f7633 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -39,6 +39,7 @@ import frc.robot.commands.hopper.SpinHopper; import frc.robot.commands.intake.SpinIntake; import frc.robot.commands.intake.StopIntake; +import frc.robot.commands.intakeDeployment.RunDeployer; import frc.robot.commands.intakeDeployment.SetDeploymentState; import frc.robot.commands.intakeDeployment.ToggleDeployment; import frc.robot.commands.parallels.RunHopperAndFeeder; @@ -152,7 +153,7 @@ public RobotContainer() { drivebase = !Constants.TESTBED ? new SwerveSubsystem( new File(Filesystem.getDeployDirectory(), "YAGSL/" + Constants.SWERVE_JSON_DIRECTORY), swerveIMU) : null; apriltagSubsystem = !Constants.TESTBED ? new ApriltagSubsystem(ApriltagSubsystem.createRealIo(), drivebase, truster) : null; - controllerSubsystem = !Constants.TESTBED ? new ControllerSubsystem(drivebase, this) : null; + controllerSubsystem = !Constants.TESTBED ? new ControllerSubsystem(drivebase,intakeDeployer, this) : null; lightStripSubsystem = new LightStripSubsystem(drivebase, shootState); } case REPLAY -> { @@ -169,7 +170,7 @@ public RobotContainer() { // create the drive subsystem with null gyro (use default json) drivebase = !Constants.TESTBED ? new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "YAGSL/" + Constants.SWERVE_JSON_DIRECTORY), null) : null; apriltagSubsystem = !Constants.TESTBED ? new ApriltagSubsystem(ApriltagSubsystem.createMockIo(), drivebase, truster) : null; - controllerSubsystem = !Constants.TESTBED ? new ControllerSubsystem(drivebase, this) : null; + controllerSubsystem = !Constants.TESTBED ? new ControllerSubsystem(drivebase, intakeDeployer, this) : null; lightStripSubsystem = new LightStripSubsystem(drivebase, shootState); } @@ -187,7 +188,7 @@ public RobotContainer() { // create the drive subsystem with null gyro (use default json) drivebase = !Constants.TESTBED ? new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "YAGSL/" + Constants.SWERVE_JSON_DIRECTORY), null) : null; - controllerSubsystem = !Constants.TESTBED ? new ControllerSubsystem(drivebase, this) : null; + controllerSubsystem = !Constants.TESTBED ? new ControllerSubsystem(drivebase, intakeDeployer, this) : null; apriltagSubsystem = !Constants.TESTBED ? new ApriltagSubsystem(ApriltagSubsystem.createSimIo(truster,drivebase), drivebase, truster) : null; lightStripSubsystem = new LightStripSubsystem(drivebase, shootState); @@ -282,7 +283,7 @@ private void setUpAutoFactory() { } private void configureBindings() { - controller.a().onTrue(new ToggleDeployment(intakeDeployer)); + controller.a().onTrue(new ToggleDeployment(intakeDeployer, controllerSubsystem)); controller.b().onTrue(new SetDeploymentState(intakeDeployer, DeploymentState.STOPPED)); controller.y().onTrue(new ClimberUp(climberSubsystem)); controller.x().onTrue(new ClimberDown(climberSubsystem)); @@ -293,9 +294,7 @@ private void configureBindings() { controller.leftTrigger().onTrue((new ResetAll(anglerSubsystem, climberSubsystem, intakeDeployer, intakeSubsystem, shooterSubsystem, turretSubsystem, shootState))); 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) @@ -308,6 +307,7 @@ private void configureBindings() { intakeSubsystem.setDefaultCommand(new SpinIntake(intakeSubsystem, intakeDeployer)); if (controllerSubsystem != null) { + intakeDeployer.setDefaultCommand(new RunDeployer(intakeDeployer)); anglerSubsystem.setDefaultCommand(new DefaultAnglerControl(anglerSubsystem, controllerSubsystem)); shooterSubsystem.setDefaultCommand(new DefaultShooterControl(shooterSubsystem, controllerSubsystem)); turretSubsystem.setDefaultCommand(new DefaultTurretControl(turretSubsystem, controllerSubsystem)); @@ -457,7 +457,7 @@ public void putShuffleboardCommands() { SmartDashboard.putData( "intakedeployer/InitlizeDeployer", - new ToggleDeployment(intakeDeployer)); + new ToggleDeployment(intakeDeployer, controllerSubsystem)); SmartDashboard.putData( "Spin Intake", new SpinIntake(intakeSubsystem, intakeDeployer)); diff --git a/src/main/java/frc/robot/commands/intakeDeployment/RunDeployer.java b/src/main/java/frc/robot/commands/intakeDeployment/RunDeployer.java index 34f292d7..853391bb 100644 --- a/src/main/java/frc/robot/commands/intakeDeployment/RunDeployer.java +++ b/src/main/java/frc/robot/commands/intakeDeployment/RunDeployer.java @@ -5,6 +5,7 @@ package frc.robot.commands.intakeDeployment; import frc.robot.constants.Constants; +import frc.robot.subsystems.ControllerSubsystem; import frc.robot.subsystems.IntakeDeployerSubsystem; import frc.robot.utils.logging.commands.LoggableCommand; @@ -22,11 +23,11 @@ public void initialize() { @Override public void execute() { - switch (subsystem.getDeploymentState()) { - case UP -> subsystem.setSpeed(Constants.INTAKE_RETRACTION_SPEED); - case DOWN -> subsystem.setSpeed(Constants.INTAKE_DEPLOYER_SPEED); - case STOPPED -> subsystem.stopMotors(); - default -> subsystem.stopMotors(); + switch (subsystem.getDeploymentState()) { + case UP -> subsystem.setSpeed(Constants.INTAKE_RETRACTION_SPEED); + case DOWN -> subsystem.setSpeed(Constants.INTAKE_DEPLOYER_SPEED); + case STOPPED -> subsystem.stopMotors(); + default -> subsystem.stopMotors(); } } diff --git a/src/main/java/frc/robot/commands/intakeDeployment/ToggleDeployment.java b/src/main/java/frc/robot/commands/intakeDeployment/ToggleDeployment.java index cb2ba59d..60de1e21 100644 --- a/src/main/java/frc/robot/commands/intakeDeployment/ToggleDeployment.java +++ b/src/main/java/frc/robot/commands/intakeDeployment/ToggleDeployment.java @@ -7,23 +7,26 @@ import edu.wpi.first.wpilibj.Timer; import frc.robot.constants.Constants; import frc.robot.constants.enums.DeploymentState; +import frc.robot.subsystems.ControllerSubsystem; import frc.robot.subsystems.IntakeDeployerSubsystem; import frc.robot.utils.logging.commands.LoggableCommand; public class ToggleDeployment extends LoggableCommand { private final IntakeDeployerSubsystem subsystem; private final Timer timer; + private final ControllerSubsystem controller; - public ToggleDeployment(IntakeDeployerSubsystem subsystem) { + public ToggleDeployment(IntakeDeployerSubsystem subsystem, ControllerSubsystem controller) { timer = new Timer(); this.subsystem = subsystem; + this.controller = controller; addRequirements(subsystem); } @Override - public void initialize() { - subsystem.toggleState(); - timer.restart(); + public void initialize() { + subsystem.toggleState(controller.canIntakeDeploy()); + timer.restart(); } @Override @@ -42,7 +45,9 @@ public void end(boolean interrupted) { @Override public boolean isFinished() { - boolean limitSwitch = subsystem.getDeploymentState() == DeploymentState.UP ? subsystem.getFwdLimitSwitchState() : subsystem.getRevLimitSwitchState(); - return timer.hasElapsed(Constants.INTAKE_DEPLOYER_BURNOUT_TIMER) || limitSwitch || subsystem.getDeploymentState() == DeploymentState.STOPPED; + boolean limitSwitch = subsystem.getDeploymentState() == DeploymentState.UP ? subsystem.getFwdLimitSwitchState() + : subsystem.getRevLimitSwitchState(); + return timer.hasElapsed(Constants.INTAKE_DEPLOYER_BURNOUT_TIMER) || limitSwitch + || subsystem.getDeploymentState() == DeploymentState.STOPPED; } } diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java index 89565c75..a005f6e5 100644 --- a/src/main/java/frc/robot/constants/GameConstants.java +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -36,7 +36,7 @@ public enum Mode { public static final boolean ENABLE_LOGGING = true; //Debugs - public static final boolean DEBUG = false; + public static final boolean DEBUG = true; public static final boolean ARM_DEBUG = true; //Joystick @@ -122,8 +122,8 @@ public enum Mode { public static final double TURRET_HOME_ANGLE = 0.0; //Turret facing forward public static final double TURRET_MIN_ANGLE = -92; public static final double TURRET_MAX_ANGLE = 92; - public static final double TURRET_LIMIT_SPEED = 0.2; - public static final double TURRET_PID_DISTANCE_THRESHOLD = 45; /* TODO: Change later + public static final double TURRET_LIMIT_SPEED = 0.1; + public static final double TURRET_PID_DISTANCE_THRESHOLD = 5; /* TODO: Change later Minimum target encoder distance needed to use the longer pid slot*/ diff --git a/src/main/java/frc/robot/subsystems/ControllerSubsystem.java b/src/main/java/frc/robot/subsystems/ControllerSubsystem.java index 074d6163..82ed47f8 100644 --- a/src/main/java/frc/robot/subsystems/ControllerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ControllerSubsystem.java @@ -12,19 +12,23 @@ import org.littletonrobotics.junction.Logger; import frc.robot.RobotContainer; +import frc.robot.commands.intakeDeployment.ToggleDeployment; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Transform2d; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.constants.Constants; +import frc.robot.constants.enums.DeploymentState; 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; + private static final double SHOOT_DELAY_SECONDS = 0.5; // Placeholder target poses until real field target values are finalized private static final Pose2d BLUE_HUB_TARGET_POSE = new Pose2d(Constants.BLUE_HUB_X_POSITION, @@ -47,10 +51,10 @@ public class ControllerSubsystem extends SubsystemBase { // Placeholder fixed-state settings. private static final ShotTargets STOPPED_TARGETS = new ShotTargets(Constants.ANGLER_ANGLE_LOW, 0.0, 0.0, 0.0, false, - false); + false,false); //3.25 meters away - private static final ShotTargets FIXED_TARGETS = new ShotTargets(21.16, -2945.21, 0, 3.25, true, true); - private static final ShotTargets FIXED_2_TARGETS = new ShotTargets(22.0, 180.0, -5.0, 0.0, true, true); + private static final ShotTargets FIXED_TARGETS = new ShotTargets(21.16, -2945.21, 0, 3.25, true, true,true); + private static final ShotTargets FIXED_2_TARGETS = new ShotTargets(22.0, 180.0, -5.0, 0.0, true, true,true); // Placeholder pose-driven profiles. private static final PoseControlProfile BLUE_HUB_PROFILE = new PoseControlProfile(BLUE_HUB_TARGET_POSE, 32.0, 230.0, @@ -63,18 +67,21 @@ public class ControllerSubsystem extends SubsystemBase { -14.0); private final SwerveSubsystem drivebase; + private final IntakeDeployerSubsystem intakeDeployer; private final RobotContainer robotContainer; private final Timer stopDelayTimer = new Timer(); + private final Timer shootDelayTimer = new Timer(); private ShootState previousState; private ShotTargets activeTargets; private boolean driverActivatedShooting = false; - public ControllerSubsystem(SwerveSubsystem drivebase, RobotContainer robotContainer) { + public ControllerSubsystem(SwerveSubsystem drivebase, IntakeDeployerSubsystem intakeDeployer, RobotContainer robotContainer) { this.drivebase = drivebase; this.robotContainer = robotContainer; this.previousState = getCurrentShootState(); this.activeTargets = STOPPED_TARGETS; + this.intakeDeployer = intakeDeployer; SmartDashboard.putNumber(MANUAL_POSE_X_KEY, 0.0); SmartDashboard.putNumber(MANUAL_POSE_Y_KEY, 0.0); @@ -86,6 +93,7 @@ public void periodic() { Pose2d robotPose = getRobotPose(); ShootState currentState = getCurrentShootState(); updateStopDelayState(currentState); + updateShootDelayState(currentState); updateTargets(currentState, robotPose); if (Constants.DEBUG) { SmartDashboard.putString(CURRENT_SHOOT_STATE_KEY, currentState.toString()); @@ -141,7 +149,16 @@ private void updateStopDelayState(ShootState currentState) { } } + private void updateShootDelayState(ShootState currentState) { + if (currentState != ShootState.STOPPED && previousState != currentState) { + shootDelayTimer.restart(); + } + } + private void updateTargets(ShootState state, Pose2d robotPose) { + if(!activeTargets.intakeDeploy && intakeDeployer.getDeploymentState() == DeploymentState.DOWN){ + new ToggleDeployment(intakeDeployer, this).schedule(); + } switch (state) { case STOPPED -> updateStoppedTargets(); case FIXED -> useShotTargets(FIXED_TARGETS); @@ -159,7 +176,6 @@ private void updateTargets(ShootState state, Pose2d robotPose) { useShotTargets(FIXED_TARGETS); } } - case SHUTTLING -> { if (Robot.allianceColor().isEmpty()) { useShotTargets(FIXED_TARGETS); @@ -186,18 +202,37 @@ private void updateStoppedTargets() { STOPPED_TARGETS.turretAngleDegrees, STOPPED_TARGETS.distanceMeters, STOPPED_TARGETS.feederSpin, - STOPPED_TARGETS.hopperSpin); + STOPPED_TARGETS.hopperSpin, + STOPPED_TARGETS.intakeDeploy); } private void useShotTargets(ShotTargets shotTargets) { - boolean driverEnabled = driverActivatedShootingEnabled(); - activeTargets = new ShotTargets( + + // This makes everything wait until after the shooter has run for half a second before starting + if (shootDelayTimer.hasElapsed(SHOOT_DELAY_SECONDS)) { + + activeTargets = new ShotTargets( shotTargets.anglerAngleDegrees, shotTargets.shooterVelocityRpm, shotTargets.turretAngleDegrees, shotTargets.distanceMeters, - driverEnabled, - driverEnabled); + shotTargets.hopperSpin, + shotTargets.feederSpin, + shotTargets.intakeDeploy); + + } else { + + activeTargets = new ShotTargets( + activeTargets.anglerAngleDegrees, + shotTargets.shooterVelocityRpm, // Shooter starts half a second before everything else + activeTargets.turretAngleDegrees, + activeTargets.distanceMeters, + false, + false, + activeTargets.intakeDeploy); + + } + } private ShotTargets calculateTargetsFromPose(ShootState state,PoseControlProfile profile, Pose2d robotPose) { @@ -206,7 +241,7 @@ private ShotTargets calculateTargetsFromPose(ShootState state,PoseControlProfile double shooterVelocity = calculateShooterVelocity(computedDistanceMeters, profile); double turretAngleDegrees = calculateTurretAngleDegrees(robotPose, profile); return new ShotTargets(anglerAngleDegrees, shooterVelocity, turretAngleDegrees, computedDistanceMeters, true, - true); + true, true); } private double calculateDistanceMeters(ShootState state,Pose2d robotPose, Pose2d targetPose) { @@ -296,6 +331,10 @@ public boolean shouldHopperSpin() { return activeTargets.hopperSpin; } + public boolean canIntakeDeploy() { + return activeTargets.intakeDeploy; + } + public void setDriverActivatedShooting(boolean set) { driverActivatedShooting = set; } @@ -312,6 +351,7 @@ private static final class ShotTargets { private final double distanceMeters; private final boolean feederSpin; private final boolean hopperSpin; + private final boolean intakeDeploy; private ShotTargets( double anglerAngleDegrees, @@ -319,13 +359,14 @@ private ShotTargets( double turretAngleDegrees, double distanceMeters, boolean feederSpin, - boolean hopperSpin) { + boolean hopperSpin, boolean intakeDeploy) { this.anglerAngleDegrees = anglerAngleDegrees; this.shooterVelocityRpm = shooterVelocityRpm; this.turretAngleDegrees = turretAngleDegrees; this.distanceMeters = distanceMeters; this.feederSpin = feederSpin; this.hopperSpin = hopperSpin; + this.intakeDeploy = intakeDeploy; } } diff --git a/src/main/java/frc/robot/subsystems/IntakeDeployerSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeDeployerSubsystem.java index fa0151dc..441201d2 100644 --- a/src/main/java/frc/robot/subsystems/IntakeDeployerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeDeployerSubsystem.java @@ -35,7 +35,6 @@ public class IntakeDeployerSubsystem extends SubsystemBase { public IntakeDeployerSubsystem(SparkMaxIo io) { this.io = io; - setDefaultCommand(new RunDeployer(this)); } public void setSpeed(double speed) { @@ -103,8 +102,8 @@ public void setDeploymentState(DeploymentState deploymentState) { this.deploymentState = deploymentState; } - public void toggleState() { - if (deploymentState == DeploymentState.UP) { + public void toggleState(boolean canDeploy) { + if (deploymentState == DeploymentState.UP && canDeploy) { deploymentState = DeploymentState.DOWN; } else { deploymentState = DeploymentState.UP;