From: Taran Nathan Date: Wed, 13 May 2026 18:54:08 +0000 (-0700) Subject: I think mostly finished cleaning out old 2026 code, builds now X-Git-Url: https://git.taranathan.com/?a=commitdiff_plain;h=7769931c3ac473fb79a98799b86510a3720edba3;p=FRC2027.git I think mostly finished cleaning out old 2026 code, builds now --- diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 5559db2..3d057a3 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -76,7 +76,7 @@ public class Robot extends LoggedRobot { // Uncomment the next line, set the desired RobotId, deploy, and then comment the line out // RobotId.setRobotId(RobotId.SwerveCompetition); - RobotController.setBrownoutVoltage(6.0); + RobotController.setBrownoutVoltage(4.6); //TODO might break on systemcores https://www.chiefdelphi.com/t/frc-1678-citrus-circuits-systemcore-alpha-testing-thread/506842 // obtain this robot's identity RobotId robotId = RobotId.getRobotId(); diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 2d5d474..a646227 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -6,7 +6,6 @@ import org.littletonrobotics.junction.Logger; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.AutoBuilderException; -import com.pathplanner.lib.auto.NamedCommands; import com.pathplanner.lib.commands.PathPlannerAuto; import choreo.auto.AutoChooser; @@ -20,33 +19,20 @@ import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import frc.robot.commands.DoNothing; import frc.robot.commands.LogCommand; import frc.robot.commands.auto_comm.ChoreoPathCommandBuilder; import frc.robot.commands.auto_comm.DynamicAutoBuilder; -import frc.robot.commands.drive_comm.DefaultDriveCommand; import frc.robot.commands.drive_comm.SysIDDriveCommand; -import frc.robot.commands.gpm.IntakeMovementCommand; -import frc.robot.commands.gpm.LockedShoot; -import frc.robot.commands.gpm.RunSpindexer; -import frc.robot.commands.gpm.Superstructure; -import frc.robot.commands.vision.ShutdownAllPis; import frc.robot.constants.AutoConstants; import frc.robot.constants.Constants; import frc.robot.constants.VisionConstants; import frc.robot.controls.BaseDriverConfig; import frc.robot.controls.Operator; import frc.robot.controls.PS5ControllerDriverConfig; -import frc.robot.subsystems.Intake.Intake; import frc.robot.subsystems.PowerControl.EMABreaker; import frc.robot.subsystems.drivetrain.Drivetrain; import frc.robot.subsystems.drivetrain.GyroIOPigeon2; -import frc.robot.subsystems.hood.Hood; -import frc.robot.subsystems.shooter.Shooter; -import frc.robot.subsystems.spindexer.Spindexer; -import frc.robot.subsystems.turret.Turret; import frc.robot.util.PathGroupLoader; import frc.robot.util.Vision.DetectedObject; import frc.robot.util.Vision.Vision; @@ -64,11 +50,6 @@ public class RobotContainer { // The robot's subsystems are defined here... private Drivetrain drive = null; private Vision vision = null; - private Turret turret = null; - private Shooter shooter = null; - private Hood hood = null; - private Spindexer spindexer = null; - private Intake intake = null; // private LED led = null; // Controllers are defined here @@ -113,15 +94,8 @@ public class RobotContainer { case PrimeJr: // AKA Valence - spindexer = new Spindexer(); - intake = new Intake(); - // led = new LED(); - breaker = new EMABreaker(); case WaffleHouse: // AKA Betabot - turret = new Turret(); - shooter = new Shooter(); - hood = new Hood(); case SwerveCompetition: // AKA "Vantage" @@ -135,7 +109,7 @@ public class RobotContainer { case Vertigo: // AKA "French Toast" drive = new Drivetrain(vision, new GyroIOPigeon2()); - driver = new PS5ControllerDriverConfig(drive, shooter, turret, hood, intake, spindexer); + driver = new PS5ControllerDriverConfig(drive); operator = new Operator(drive); initChoreo(); @@ -153,13 +127,6 @@ public class RobotContainer { initializeAutoBuilder(); autoChooserInit(); - if (turret != null && drive != null && hood != null && shooter != null) { - SmartDashboard.putData("Lock Shooting", new LockedShoot(turret, drive, hood, shooter)); - } - - if (turret != null) { - turret.setDefaultCommand(new Superstructure(turret, drive, hood, shooter, spindexer)); - } if (drive != null && driver != null) { // drive.setDefaultCommand(new DefaultDriveCommand(drive, driver)); @@ -169,7 +136,6 @@ public class RobotContainer { } - if (intake != null && hood != null && turret != null) // CommandScheduler.getInstance().schedule(new HardstopWarning(hood, intake, turret)); (no more crt for this) // This is really annoying so it's disabled DriverStation.silenceJoystickConnectionWarning(true); @@ -180,10 +146,6 @@ public class RobotContainer { // LiveWindow is causing periodic loop overruns LiveWindow.disableAllTelemetry(); LiveWindow.setEnabled(false); - - if (!Constants.DISABLE_SMART_DASHBOARD) { - SmartDashboard.putData("Shutdown Orange Pis", new ShutdownAllPis()); - } } private void initChoreo() { @@ -200,9 +162,6 @@ public class RobotContainer { Logger.recordOutput("Autos/StartingOrFinishing", startOrFinish); }); - autoFactory.bind("hoodUp", new InstantCommand(() -> hood.forceHoodDown(false))); - autoFactory.bind("hoodDown", new InstantCommand(() -> hood.forceHoodDown(true))); - // warmup command for choreo, prevents lag on auto startup CommandScheduler.getInstance().schedule(autoFactory.warmupCmd().ignoringDisable(true)); } @@ -234,50 +193,9 @@ public class RobotContainer { drive); } - private boolean seizing; public void registerCommands() { - if (intake != null) { - - NamedCommands.registerCommand("Extend Intake", new InstantCommand(() -> { - intake.extend(); - })); - NamedCommands.registerCommand("Retract Intake", new InstantCommand(() -> intake.retract())); - NamedCommands.registerCommand("Intermediate Extend", new InstantCommand(() -> intake.intermediateExtend())); - NamedCommands.registerCommand("Spin Intake Rollers", new InstantCommand(() -> intake.spinStart())); - NamedCommands.registerCommand("Stop Intake Rollers", new InstantCommand(() -> intake.spinStop())); - - NamedCommands.registerCommand("Start Intake Seizure", new InstantCommand(() -> { - seizing = true; - CommandScheduler.getInstance().schedule(new IntakeMovementCommand(intake).until(() -> !seizing)); - })); - NamedCommands.registerCommand("Stop Intake Seizure", new InstantCommand(() -> { - seizing = false; - })); - } - - if (turret != null && drive != null && hood != null && shooter != null && spindexer != null && intake != null) { - Command runSpindexer = new RunSpindexer(spindexer, turret, hood, intake); - NamedCommands.registerCommand("Start Spindexer", - new InstantCommand(() -> CommandScheduler.getInstance().schedule(runSpindexer))); - NamedCommands.registerCommand("Stop Spindexer", new InstantCommand(() -> runSpindexer.cancel())); - } - - if (hood != null) { - - NamedCommands.registerCommand("Hood Down", new InstantCommand(() -> { - hood.forceHoodDown(true); - })); - NamedCommands.registerCommand("Stop Hood Down", new InstantCommand(() -> { - hood.forceHoodDown(false); - })); - } - - NamedCommands.registerCommand("After Depot", new InstantCommand()); - NamedCommands.registerCommand("Constraints Zone", new InstantCommand()); - NamedCommands.registerCommand("Depot", new InstantCommand()); - NamedCommands.registerCommand("Reset Spindexer", new InstantCommand()); } public void addAuto(String name) { @@ -311,41 +229,10 @@ public class RobotContainer { */ public void autoChooserInit() { // add the options to the Chooser - String leftSideAuto = "Left Week V1"; - String rightSideAuto = "Right Week V1"; - String shootOnlyAuto = "Shoot Only Left Week V1"; - String leftLiberalSwipe = "LeftLiberalDoubleSwipe"; - String rightLiberalSwipe = "RightLiberalDoubleSwipe"; - String leftLiberalSwipeTranslation = "LeftLiberalDoubleSwipeTranslation"; - String leftConservativeSwipe = "LeftConservativeDoubleSwipe"; - String leftDoNothing = "Left Do Nothing"; - String rightDoNothing = "Right Do Nothing"; - String centerDoNothing = "Center Do Nothing"; - String leftShallowDoubleSwipe = "LeftShallowDoubleSwipe"; - String rightShallowDoubleSwipe = "RightShallowDoubleSwipe"; - String leftBumpDepotCenter = "LeftBumpDepotCenter"; - String leftTrenchDepotCenter = "LeftTrenchDepotCenter"; - String depotCenterPath = "DepotCenterPath"; autoChooser.setDefaultOption("Default", getDefaultAuto()); - addAuto(leftSideAuto); - addAuto(rightSideAuto); - addAuto(shootOnlyAuto); - addAuto(leftConservativeSwipe); - addAuto(leftLiberalSwipe); - addAuto(rightLiberalSwipe); - addAuto(leftLiberalSwipeTranslation); - addAuto(leftDoNothing); - addAuto(rightDoNothing); - addAuto(centerDoNothing); - addAuto(leftShallowDoubleSwipe); - addAuto(rightShallowDoubleSwipe); - addAuto(leftBumpDepotCenter); - addAuto(leftTrenchDepotCenter); - addAuto(depotCenterPath); - - - DynamicAutoBuilder dynamicAutoBuilder = new DynamicAutoBuilder(spindexer, turret, hood, intake); + + DynamicAutoBuilder dynamicAutoBuilder = new DynamicAutoBuilder(); // names String leftDynamicLiberalDoubleSwipe = "LeftDynamicDoubleLiberalSwipe"; @@ -361,15 +248,9 @@ public class RobotContainer { addAuto(leftDynamicConservativeDoubleSwipe, dynamicAutoBuilder.getDynamicDoubleConservativeSwipe(true)); addAuto(rightDynamicConservativeDoubleSwipe, dynamicAutoBuilder.getDynamicDoubleConservativeSwipe(false)); - ChoreoPathCommandBuilder choreo = new ChoreoPathCommandBuilder(intake, spindexer, turret, hood); + ChoreoPathCommandBuilder choreo = new ChoreoPathCommandBuilder(); - addAuto("testChoreo", ChoreoPathCommandBuilder.basicTrajectoryAuto("test.traj", true, autoFactory)); - addChoreoAuto("choreoLiberalLeft", choreo.leftLiberal(autoFactory)); - addChoreoAuto("choreoLiberalRight", choreo.rightLiberal(autoFactory)); - addChoreoAuto("choreoConservativeLeft", choreo.leftConservative(autoFactory)); - addChoreoAuto("choreoConservativeRight", choreo.rightConservative(autoFactory)); - addChoreoAuto("choreoShallowLeft", choreo.leftShallow(autoFactory)); - addChoreoAuto("choreoShallowRight", choreo.rightShallow(autoFactory)); + // addAuto("testChoreo", ChoreoPathCommandBuilder.basicTrajectoryAuto("test.traj", true, autoFactory)); // put the Chooser on the SmartDashboard SmartDashboard.putData("Auto chooser", autoChooser); @@ -400,13 +281,7 @@ public class RobotContainer { } public Command getDefaultAuto() { - if (spindexer == null || turret == null || hood == null || intake == null) { return new DoNothing(); - } - ParallelCommandGroup defaultShoot = new ParallelCommandGroup( - new RunSpindexer(spindexer, turret, hood, intake) - ); - return defaultShoot; } public Command getAutoCommand() { diff --git a/src/main/java/frc/robot/commands/LogCommand.java b/src/main/java/frc/robot/commands/LogCommand.java index 3340971..ca565d0 100644 --- a/src/main/java/frc/robot/commands/LogCommand.java +++ b/src/main/java/frc/robot/commands/LogCommand.java @@ -5,10 +5,10 @@ import org.littletonrobotics.junction.Logger; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.constants.Constants; import frc.robot.util.Elastic; -import frc.robot.util.HubActive; import frc.robot.util.Elastic.Notification; import frc.robot.util.Elastic.NotificationLevel; +/// Command for logging stuff public class LogCommand extends Command { private boolean hubActive = false; @@ -22,16 +22,6 @@ public class LogCommand extends Command { return; } - boolean current = HubActive.isHubActive(); - Logger.recordOutput("HubActive", current); - - if (current && !hubActive) { - Elastic.sendNotification(new Notification(NotificationLevel.INFO, "HUB ACTIVE", "")); - } else if (!current && hubActive) { - Elastic.sendNotification(new Notification(NotificationLevel.INFO, "HUB DEACTIVATED", "")); - } - - hubActive = current; } @Override diff --git a/src/main/java/frc/robot/commands/auto_comm/ChoreoPathCommandBuilder.java b/src/main/java/frc/robot/commands/auto_comm/ChoreoPathCommandBuilder.java index 3e8aafe..96a2e30 100644 --- a/src/main/java/frc/robot/commands/auto_comm/ChoreoPathCommandBuilder.java +++ b/src/main/java/frc/robot/commands/auto_comm/ChoreoPathCommandBuilder.java @@ -6,27 +6,12 @@ import choreo.auto.AutoTrajectory; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.robot.commands.DoNothing; -import frc.robot.commands.gpm.IntakeMovementCommand; -import frc.robot.commands.gpm.RunSpindexer; -import frc.robot.commands.gpm.RunSpindexerWithStop; -import frc.robot.subsystems.Intake.Intake; -import frc.robot.subsystems.hood.Hood; -import frc.robot.subsystems.spindexer.Spindexer; -import frc.robot.subsystems.turret.Turret; public class ChoreoPathCommandBuilder { - private Intake intake; - private Spindexer spindexer; - private Turret turret; - private Hood hood; - public ChoreoPathCommandBuilder(Intake intake, Spindexer spindexer, Turret turret, Hood hood) { - this.intake = intake; - this.spindexer = spindexer; - this.turret = turret; + public ChoreoPathCommandBuilder() { } @@ -37,297 +22,5 @@ public class ChoreoPathCommandBuilder { resetOdemetry ? new InstantCommand(() -> factory.resetOdometry(pathName)) : new DoNothing(), command); } - - public AutoRoutine leftLiberal(AutoFactory factory) { - AutoRoutine routine = factory.newRoutine("leftLiberal"); - - AutoTrajectory liberalSwipe = routine.trajectory("liberal", 0); - AutoTrajectory shallowSwipe = routine.trajectory("liberal", 1); - - routine.active().onTrue( - Commands.sequence( - liberalSwipe.resetOdometry(), - new InstantCommand(() -> { - intake.extend(); - intake.spinStart(); - hood.forceHoodDown(true); - }), - liberalSwipe.cmd())); - - liberalSwipe.done() - .onTrue(Commands.sequence( - new InstantCommand(() -> { - hood.forceHoodDown(false); - }), - new RunSpindexerWithStop(spindexer, turret, hood, intake).raceWith(new IntakeMovementCommand(intake)), - new InstantCommand(() -> { - intake.extend(); - intake.spinStart(); - hood.forceHoodDown(true); - }), - shallowSwipe.cmd())); - - shallowSwipe.done() - .onTrue(Commands.sequence( - new InstantCommand(() -> { - hood.forceHoodDown(false); - }), - new RunSpindexerWithStop(spindexer, turret, hood, intake).raceWith(new IntakeMovementCommand(intake)), - new InstantCommand(() -> { - intake.extend(); - intake.spinStart(); - hood.forceHoodDown(true); - }), - shallowSwipe.cmd())); - - return routine; - - } - - public AutoRoutine rightLiberal(AutoFactory factory) { - AutoRoutine routine = factory.newRoutine("rightLiberal"); - - AutoTrajectory liberalSwipe = routine.trajectory("liberal", 0).mirrorY(); - AutoTrajectory shallowSwipe = routine.trajectory("liberal", 1).mirrorY(); - - routine.active().onTrue( - Commands.sequence( - liberalSwipe.resetOdometry(), - new InstantCommand(() -> { - intake.extend(); - intake.spinStart(); - }), - liberalSwipe.cmd())); - - liberalSwipe.done() - .onTrue(Commands.sequence( - new RunSpindexerWithStop(spindexer, turret, hood, intake).raceWith(new IntakeMovementCommand(intake)), - shallowSwipe.cmd())); - - shallowSwipe.done() - .onTrue(Commands.sequence( - new RunSpindexerWithStop(spindexer, turret, hood, intake).raceWith(new IntakeMovementCommand(intake)), - shallowSwipe.cmd())); - - return routine; - - } - - public AutoRoutine leftConservative(AutoFactory factory) { - AutoRoutine routine = factory.newRoutine("leftConservative"); - - AutoTrajectory liberalSwipe = routine.trajectory("conservative", 0); - AutoTrajectory shallowSwipe = routine.trajectory("conservative", 1); - - routine.active().onTrue( - Commands.sequence( - liberalSwipe.resetOdometry(), - new InstantCommand(() -> { - intake.extend(); - intake.spinStart(); - hood.forceHoodDown(true); - }), - liberalSwipe.cmd())); - - liberalSwipe.done() - .onTrue(Commands.sequence( - new InstantCommand(() -> { - hood.forceHoodDown(false); - }), - new RunSpindexerWithStop(spindexer, turret, hood, intake).raceWith(new IntakeMovementCommand(intake)), - new InstantCommand(() -> { - intake.extend(); - intake.spinStart(); - hood.forceHoodDown(true); - }), - shallowSwipe.cmd())); - - shallowSwipe.done() - .onTrue(Commands.sequence( - new InstantCommand(() -> { - hood.forceHoodDown(false); - }), - new RunSpindexerWithStop(spindexer, turret, hood, intake).raceWith(new IntakeMovementCommand(intake)), - new InstantCommand(() -> { - intake.extend(); - intake.spinStart(); - hood.forceHoodDown(true); - }), - shallowSwipe.cmd())); - - return routine; - - } - - public AutoRoutine rightConservative(AutoFactory factory) { - AutoRoutine routine = factory.newRoutine("rightConservative"); - - AutoTrajectory liberalSwipe = routine.trajectory("conservative", 0).mirrorY(); - AutoTrajectory shallowSwipe = routine.trajectory("conservative", 1).mirrorY(); - - routine.active().onTrue( - Commands.sequence( - liberalSwipe.resetOdometry(), - new InstantCommand(() -> { - intake.extend(); - intake.spinStart(); - hood.forceHoodDown(true); - }), - liberalSwipe.cmd())); - - liberalSwipe.done() - .onTrue(Commands.sequence( - new InstantCommand(() -> { - hood.forceHoodDown(false); - }), - new RunSpindexerWithStop(spindexer, turret, hood, intake).raceWith(new IntakeMovementCommand(intake)), - new InstantCommand(() -> { - intake.extend(); - intake.spinStart(); - hood.forceHoodDown(true); - }), - shallowSwipe.cmd())); - - shallowSwipe.done() - .onTrue(Commands.sequence( - new InstantCommand(() -> { - hood.forceHoodDown(false); - }), - new RunSpindexerWithStop(spindexer, turret, hood, intake).raceWith(new IntakeMovementCommand(intake)), - new InstantCommand(() -> { - intake.extend(); - intake.spinStart(); - hood.forceHoodDown(true); - }), - shallowSwipe.cmd())); - - return routine; - - } - - public AutoRoutine leftShallow(AutoFactory factory) { - AutoRoutine routine = factory.newRoutine("leftShallow"); - - AutoTrajectory shallowSwipe = routine.trajectory("shallowSwipe"); - - routine.active().onTrue( - Commands.sequence( - shallowSwipe.resetOdometry(), - new InstantCommand(() -> { - intake.extend(); - intake.spinStart(); - hood.forceHoodDown(true); - }), - shallowSwipe.cmd())); - - shallowSwipe.done() - .onTrue(Commands.sequence( - new InstantCommand(() -> { - hood.forceHoodDown(false); - }), - new RunSpindexerWithStop(spindexer, turret, hood, intake).raceWith(new IntakeMovementCommand(intake)), - new InstantCommand(() -> { - intake.extend(); - intake.spinStart(); - hood.forceHoodDown(true); - }), - shallowSwipe.cmd())); - - return routine; - - } - - public AutoRoutine rightShallow(AutoFactory factory) { - AutoRoutine routine = factory.newRoutine("rightShallow"); - - AutoTrajectory shallowSwipe = routine.trajectory("shallowSwipe").mirrorY(); - - routine.active().onTrue( - Commands.sequence( - shallowSwipe.resetOdometry(), - new InstantCommand(() -> { - intake.extend(); - intake.spinStart(); - hood.forceHoodDown(true); - }), - shallowSwipe.cmd())); - - shallowSwipe.done() - .onTrue(Commands.sequence( - new InstantCommand(() -> { - hood.forceHoodDown(false); - }), - new RunSpindexerWithStop(spindexer, turret, hood, intake).raceWith(new IntakeMovementCommand(intake)), - new InstantCommand(() -> { - intake.extend(); - intake.spinStart(); - hood.forceHoodDown(true); - }), - shallowSwipe.cmd())); - - return routine; - - } - - public AutoRoutine leftSuperShuttling(AutoFactory factory) { - AutoRoutine routine = factory.newRoutine("leftSuperShuttling"); - - AutoTrajectory shuttlingTrajectory = routine.trajectory("superShuttling"); - - routine.active().onTrue(Commands.sequence( - shuttlingTrajectory.resetOdometry(), - new InstantCommand(() -> { - intake.extend(); - intake.spinStart(); - }), - shuttlingTrajectory.cmd())); - - routine.active().whileTrue(new RunSpindexer(spindexer, turret, hood, intake)); - - shuttlingTrajectory.done().onTrue(Commands.sequence( - new InstantCommand(() -> { - hood.forceHoodDown(false); - }), - new WaitCommand(2.0).raceWith(new IntakeMovementCommand(intake)), - new InstantCommand(() -> { - intake.extend(); - intake.spinStart(); - hood.forceHoodDown(true); - }), - shuttlingTrajectory.cmd())); - - return routine; - - } - - public AutoRoutine rightSuperShuttling(AutoFactory factory) { - AutoRoutine routine = factory.newRoutine("rightSuperShuttling"); - - AutoTrajectory shuttlingTrajectory = routine.trajectory("superShuttling"); - - routine.active().onTrue(Commands.sequence( - shuttlingTrajectory.resetOdometry(), - new InstantCommand(() -> { - intake.extend(); - intake.spinStart(); - }), - shuttlingTrajectory.cmd())); - - routine.active().whileTrue(new RunSpindexer(spindexer, turret, hood, intake)); - - shuttlingTrajectory.done().onTrue(Commands.sequence( - new InstantCommand(() -> { - hood.forceHoodDown(false); - }), - new WaitCommand(2.0).raceWith(new IntakeMovementCommand(intake)), - new InstantCommand(() -> { - intake.extend(); - intake.spinStart(); - hood.forceHoodDown(true); - }), - shuttlingTrajectory.cmd())); - - return routine; - - } + } diff --git a/src/main/java/frc/robot/commands/auto_comm/DynamicAutoBuilder.java b/src/main/java/frc/robot/commands/auto_comm/DynamicAutoBuilder.java index 0404c4b..57acb50 100644 --- a/src/main/java/frc/robot/commands/auto_comm/DynamicAutoBuilder.java +++ b/src/main/java/frc/robot/commands/auto_comm/DynamicAutoBuilder.java @@ -1,26 +1,14 @@ package frc.robot.commands.auto_comm; import edu.wpi.first.wpilibj2.command.*; -import frc.robot.commands.gpm.RunSpindexerWithStop; -import frc.robot.subsystems.Intake.Intake; -import frc.robot.subsystems.hood.Hood; -import frc.robot.subsystems.spindexer.Spindexer; -import frc.robot.subsystems.turret.Turret; +import frc.robot.commands.DoNothing; import com.pathplanner.lib.commands.PathPlannerAuto; public class DynamicAutoBuilder { - private final Spindexer spindexer; - private final Turret turret; - private final Hood hood; - private final Intake intake; - public DynamicAutoBuilder(Spindexer spindexer, Turret turret, Hood hood, Intake intake) { - this.spindexer = spindexer; - this.turret = turret; - this.hood = hood; - this.intake = intake; + public DynamicAutoBuilder() { } /* @@ -100,19 +88,17 @@ public class DynamicAutoBuilder { private Command departCommand() { return new InstantCommand(() -> { - hood.forceHoodDown(true); - intake.extend(); - intake.spinStart(); - spindexer.stopSpindexer(); }); } private Command runSpindexerWithAbort() { // has an isFinnished so works - return new RunSpindexerWithStop(spindexer, turret, hood, intake); + // return new RunSpindexerWithStop(spindexer, turret, hood, intake); + return new DoNothing(); } private Command startShootingCommand() { - return new InstantCommand(() -> hood.forceHoodDown(false)); + // return new InstantCommand(() -> hood.forceHoodDown(false)); + return new DoNothing(); } } diff --git a/src/main/java/frc/robot/commands/drive_comm/DefaultDriveCommand.java b/src/main/java/frc/robot/commands/drive_comm/DefaultDriveCommand.java index 8164d35..3e05476 100644 --- a/src/main/java/frc/robot/commands/drive_comm/DefaultDriveCommand.java +++ b/src/main/java/frc/robot/commands/drive_comm/DefaultDriveCommand.java @@ -12,8 +12,6 @@ import frc.robot.constants.Constants; import frc.robot.constants.swerve.DriveConstants; import frc.robot.controls.BaseDriverConfig; import frc.robot.subsystems.drivetrain.Drivetrain; -import frc.robot.util.TrenchAssist.TrenchAssist; -import frc.robot.util.TrenchAssist.TrenchAssistConstants; import frc.robot.util.Vision.DriverAssist; /** @@ -64,45 +62,6 @@ public class DefaultDriveCommand extends Command { ChassisSpeeds driverInput = new ChassisSpeeds(forwardTranslation, sideTranslation, rotation); ChassisSpeeds corrected = DriverAssist.calculate(swerve, driverInput, swerve.getDesiredPose(), true); - if (!Constants.DISABLE_LOGGING) { - Logger.recordOutput("TrenchAlign", swerve.getTrenchAlign()); - Logger.recordOutput("AlignZones", TrenchAssistConstants.ALIGN_ZONES); - } - // if (swerve.getTrenchAlign()) { - // boolean inZone = false; - // for (Rectangle2d rectangle : TrenchAssistConstants.ALIGN_ZONES) { - // if (rectangle.contains(swerve.getPose().getTranslation())) { - // inZone = true; - // } - // } - - // if (inZone) { - - // double yawDegrees = swerve.getYaw().getDegrees(); - // // double snappedDeg = Math.round(yawDegrees / 90.0) * 90.0; - // if (Math.abs(yawDegrees) <= 90) { - // swerve.setAlignAngle(Units.degreesToRadians(0.0)); - // } else { - // swerve.setAlignAngle(Units.degreesToRadians(180.0)); - // } - // // swerve.setAlignAngle(snappedDeg); - // // Logger.recordOutput("snappy", snappedDeg); - // } else { - // swerve.setIsAlign(false); - // } - // } else { - // swerve.setIsAlign(false); - // } - // if (!Constants.DISABLE_LOGGING) { - // Logger.recordOutput("TrenchAssist", swerve.getTrenchAssist()); - // } - - if (swerve.getTrenchAssist()) { - drive(TrenchAssist.calculate(swerve, corrected, trenchAssistPid)); - } else { - trenchAssistPid.reset(); - drive(corrected); - } } /** diff --git a/src/main/java/frc/robot/commands/gpm/HardstopWarning.java b/src/main/java/frc/robot/commands/gpm/HardstopWarning.java deleted file mode 100644 index 901e51f..0000000 --- a/src/main/java/frc/robot/commands/gpm/HardstopWarning.java +++ /dev/null @@ -1,38 +0,0 @@ -package frc.robot.commands.gpm; - -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.constants.Constants; -import frc.robot.subsystems.Intake.Intake; -import frc.robot.subsystems.Intake.IntakeConstants; -import frc.robot.subsystems.hood.Hood; -import frc.robot.subsystems.hood.HoodConstants; - -public class HardstopWarning extends Command { - private Hood hood; - private Intake intake; - - public HardstopWarning(Hood hood, Intake intake) { - this.hood = hood; - this.intake = intake; - } - - @Override - public boolean runsWhenDisabled() { - return true; - } - - @Override - public void execute() { - double epsilon = 0.05; - if (!Constants.DISABLE_SMART_DASHBOARD) { - SmartDashboard.putBoolean("Hood OK", hood.getPositionDeg() >= HoodConstants.MIN_ANGLE - epsilon); - SmartDashboard.putBoolean("Intake OK", intake.getPosition() >= IntakeConstants.STARTING_POINT - epsilon); - } - } - - @Override - public boolean isFinished() { - return false; - } -} diff --git a/src/main/java/frc/robot/commands/gpm/IntakeCommand.java b/src/main/java/frc/robot/commands/gpm/IntakeCommand.java deleted file mode 100644 index 7723ed8..0000000 --- a/src/main/java/frc/robot/commands/gpm/IntakeCommand.java +++ /dev/null @@ -1,26 +0,0 @@ -package frc.robot.commands.gpm; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.Intake.Intake; - -public class IntakeCommand extends Command{ - private Intake intake; - - public IntakeCommand(Intake intake){ - this.intake = intake; - addRequirements(intake); - } - - @Override - public void initialize() { - intake.extend(); - intake.spinStart(); - } - - @Override - public void end(boolean interrupted) { - intake.intermediateExtend(); - intake.spinStop(); - } - -} diff --git a/src/main/java/frc/robot/commands/gpm/IntakeMovementCommand.java b/src/main/java/frc/robot/commands/gpm/IntakeMovementCommand.java deleted file mode 100644 index a623e61..0000000 --- a/src/main/java/frc/robot/commands/gpm/IntakeMovementCommand.java +++ /dev/null @@ -1,30 +0,0 @@ -package frc.robot.commands.gpm; - -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.Intake.Intake; - -public class IntakeMovementCommand extends Command { - private final Intake intake; - private final double interval = 0.6; // Change this to make it faster/slower (seconds) - - public IntakeMovementCommand(Intake intake) { - this.intake = intake; - addRequirements(intake); - } - - @Override - public void execute() { - intake.spinStart(); - if ((int) (Timer.getFPGATimestamp() / interval) % 2 == 0) { - intake.extend(); - } else { - intake.intermediateExtend(); - } - } - - @Override - public void end(boolean interrupted) { - intake.extend(); - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/gpm/LockedShoot.java b/src/main/java/frc/robot/commands/gpm/LockedShoot.java deleted file mode 100644 index d08f30c..0000000 --- a/src/main/java/frc/robot/commands/gpm/LockedShoot.java +++ /dev/null @@ -1,220 +0,0 @@ -package frc.robot.commands.gpm; - -import edu.wpi.first.math.filter.LinearFilter; -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.math.geometry.Translation2d; -import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.math.geometry.Twist2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.constants.Constants; -import frc.robot.constants.FieldConstants; -import frc.robot.constants.ShotInterpolation; -import frc.robot.constants.ShuttleInterpolation; -import frc.robot.subsystems.drivetrain.Drivetrain; -import frc.robot.subsystems.hood.Hood; -import frc.robot.subsystems.hood.HoodConstants; -import frc.robot.subsystems.shooter.Shooter; -import frc.robot.subsystems.turret.Turret; -import frc.robot.subsystems.turret.TurretConstants; -import frc.robot.util.PhaseManager; -import frc.robot.util.ShooterPhysics; -import frc.robot.util.ShooterPhysics.TurretState; - -public class LockedShoot extends Command { - private Turret turret; - private Drivetrain drive; - private Hood hood; - private Shooter shooter; - - private Pose2d drivepose; - - private final LinearFilter hoodAngleFilter = LinearFilter.movingAverage((int) (0.1 / Constants.LOOP_TIME)); - - private Rotation2d targetAngle; - - private double lastHoodAngle; - private double hoodAngle; - private double hoodVelocity; - - private TurretState goalState; - - private double phaseDelay = 0.03; // Extrapolation delay due to latency - - private Translation2d target = FieldConstants.HUB_BLUE.toTranslation2d(); - - private PhaseManager phaseManager = new PhaseManager(); - - private double distanceFromTarget = 0.0; - - private double TOFAdjustment = 0.85; - - public LockedShoot(Turret turret, Drivetrain drivetrain, Hood hood, Shooter shooter) { - this.turret = turret; - this.drive = drivetrain; - this.hood = hood; - this.shooter = shooter; - drivepose = drivetrain.getPose(); - - goalState = ShooterPhysics.getShotParams( - Translation2d.kZero, - FieldConstants.getHubTranslation().minus(new Translation3d(drivepose.getTranslation())), - 8.0); // Random initial goalState to prevent it being null - - addRequirements(turret, shooter); - } - - public void updateSetpoints(Pose2d drivepose) { - Pose2d turretPosition = drivepose.transformBy( - new Transform2d(TurretConstants.DISTANCE_FROM_ROBOT_CENTER.toTranslation2d(), Rotation2d.kZero)); - - // If the robot is moving, adjust the target position based on velocity - ChassisSpeeds robotRelVel = drive.getChassisSpeeds(); - ChassisSpeeds fieldRelVel = ChassisSpeeds.fromRobotRelativeSpeeds(robotRelVel, drive.getYaw()); - - // Rotational adjustment is not being used, since turret is in center of robot - double turretVelocityX = - fieldRelVel.vxMetersPerSecond; - double turretVelocityY = - fieldRelVel.vyMetersPerSecond; - - double timeOfFlight = 0; - Pose2d lookaheadPose = turretPosition; - - /* - * Loop (max 20) until lookaheadPose converges BECAUSE --> - * If you're 8m away (t = 1.0s) and moving at 2m/s towards target, you calculate for 6m (t = 0.8s) - * At 6m, we run assuming t = 0.8 but then the 6m isn't correct since it was derived using t = 1.0s - * So we make a bunch of guesses until it converges - * Early exit when change < 1mm to avoid unnecessary iterations - */ - for (int i = 0; i < 20; i++) { - Translation3d lookahead3d = new Translation3d(lookaheadPose.getX(), lookaheadPose.getY(), TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getZ()); - - Translation3d target3d = new Translation3d(target.getX(), target.getY(), - target.equals(FieldConstants.getHubTranslation().toTranslation2d()) ? - FieldConstants.getHubTranslation().getZ() : 0.0); // Height of 0 if it's not the hub - - goalState = ShooterPhysics.getShotParams( - Translation2d.kZero, - target3d.minus(lookahead3d), - 2.0); - - timeOfFlight = goalState.timeOfFlight() * TOFAdjustment; - double offsetX = turretVelocityX * timeOfFlight; - double offsetY = turretVelocityY * timeOfFlight; - Pose2d newLookaheadPose = - new Pose2d( - turretPosition.getTranslation().plus(new Translation2d(offsetX, offsetY)), - turretPosition.getRotation()); - - // early exit if converged (change < 1mm) - if (i > 0 && lookaheadPose.getTranslation().getDistance(newLookaheadPose.getTranslation()) < 0.001) { - lookaheadPose = newLookaheadPose; - break; - } - lookaheadPose = newLookaheadPose; - } - - // Get the field angle relative to the target pose - targetAngle = target.minus(lookaheadPose.getTranslation()).getAngle(); - - targetAngle = target.minus(lookaheadPose.getTranslation()).getAngle().minus(new Rotation2d(turret.getPositionRad())); - - - // Pitch is in radians - hoodAngle = goalState.pitch(); - hoodVelocity = hoodAngleFilter.calculate((hoodAngle - lastHoodAngle) / Constants.LOOP_TIME); - lastHoodAngle = hoodAngle; - - distanceFromTarget = target.getDistance(lookaheadPose.getTranslation()); - } - - public void updateDrivePose(){ - Pose2d currentPose = drive.getPose(); - - drivepose = new Pose2d( - currentPose.getTranslation(), - // Uncomment this if robot is backwards - currentPose.getRotation()//.plus(new Rotation2d(Math.PI)) - ); - ChassisSpeeds robotRelVel = drive.getChassisSpeeds(); - - // Add a phase delay extrapolation component for latency delay - drivepose = drivepose.exp( - new Twist2d( - robotRelVel.vxMetersPerSecond * phaseDelay, - robotRelVel.vyMetersPerSecond * phaseDelay, - robotRelVel.omegaRadiansPerSecond * phaseDelay)); - } - - /** - * Stops and stows all subsystems involved in the command - */ - public void stowEverything(){ - hood.setFieldRelativeTarget(Rotation2d.fromDegrees(HoodConstants.MAX_ANGLE), 0.0); - shooter.setShooter(0.0); - //spindexer.stopSpindexer(); - } - - @Override - public void execute() { - updateDrivePose(); - - // Phase manager stuff - phaseManager.update(drivepose, shooter, turret); - target = phaseManager.getTarget(drivepose); - - updateSetpoints(drivepose); - - if (phaseManager.isIdle()) { - stowEverything(); - } else { - drive.setAlignAngle(targetAngle.getRadians()); - - boolean shuttling = !target.equals(FieldConstants.getHubTranslation().toTranslation2d()); // if we're aiming at the hub, we're not shuttling - - // shuttling will move the hood so its angles very close to max (less arch) - if (shuttling) { - hood.setFieldRelativeTarget(Rotation2d.fromDegrees(ShuttleInterpolation.newHoodMap.get(distanceFromTarget)), hoodVelocity); - } else { - hood.setFieldRelativeTarget(Rotation2d.fromDegrees(ShotInterpolation.newHoodMap.get(distanceFromTarget)), hoodVelocity); - } - - // if (FieldConstants.underTrench(x, y)) { - // System.out.println("Hood forced down"); - // } else { - // hood.forceHoodDown(false); - // } - - // different maps for shuttling vs shooting. Less powerful when shuttling. - if (shuttling) { - shooter.setShooter(-ShuttleInterpolation.shooterVelocityMap.get(distanceFromTarget)); - } else { - shooter.setShooter(-ShotInterpolation.shooterVelocityMap.get(distanceFromTarget)); - } - - } - - - } - - @Override - public void end(boolean interrupted) { - stowEverything(); - - drive.setIsAlign(false); - - turret.locked = false; - - } - - @Override - public void initialize() { - drive.setIsAlign(true); - turret.locked = true; - } - -} diff --git a/src/main/java/frc/robot/commands/gpm/PhysicsAutoShoot.java b/src/main/java/frc/robot/commands/gpm/PhysicsAutoShoot.java deleted file mode 100644 index 5693ade..0000000 --- a/src/main/java/frc/robot/commands/gpm/PhysicsAutoShoot.java +++ /dev/null @@ -1,84 +0,0 @@ -package frc.robot.commands.gpm; - -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.constants.Constants; -import frc.robot.constants.FieldConstants; -import frc.robot.subsystems.drivetrain.Drivetrain; -import frc.robot.subsystems.hood.Hood; -import frc.robot.subsystems.hood.HoodConstants; -import frc.robot.subsystems.shooter.Shooter; -import frc.robot.subsystems.shooter.ShooterConstants; -import frc.robot.subsystems.turret.Turret; -import frc.robot.util.ShooterPhysics; - -public class PhysicsAutoShoot extends Command { - private Turret turret; - private Hood hood; - private Shooter shooter; - private Drivetrain drivetrain; - private ShooterPhysics.Constraints constraints; - - public PhysicsAutoShoot(Turret turret, Hood hood, Shooter shooter, Drivetrain drivetrain) { - this.turret = turret; - this.hood = hood; - this.shooter = shooter; - this.drivetrain = drivetrain; - - this.constraints = new ShooterPhysics.Constraints(2.2, ShooterConstants.SHOOTER_VELOCITY, - Units.degreesToRadians(HoodConstants.MIN_ANGLE), Units.degreesToRadians(HoodConstants.MAX_ANGLE)); - - addRequirements(turret, hood, shooter); - } - - @Override - public void execute() { - ChassisSpeeds chassisSpeeds = drivetrain.getChassisSpeeds(); - Translation2d robotVelocity = new Translation2d(chassisSpeeds.vxMetersPerSecond, - chassisSpeeds.vyMetersPerSecond); - Translation3d robotToTarget = FieldConstants.getHubTranslation() - .minus(new Translation3d(drivetrain.getPose().getTranslation())); - - var stateOpt = ShooterPhysics.getConstrainedParams( - robotVelocity, - robotToTarget, - this.constraints); - - // in one periodic - var futureStateOpt = ShooterPhysics.getConstrainedParams( - robotVelocity, - robotToTarget.plus(new Translation3d(robotVelocity.times(Constants.LOOP_TIME))), - this.constraints); - - if (stateOpt.isPresent() && futureStateOpt.isPresent()) { - ShooterPhysics.TurretState state = stateOpt.get(); - ShooterPhysics.TurretState futureState = futureStateOpt.get(); - - double yawSlope = (futureState.yaw().getRadians() - state.yaw().getRadians()) / Constants.LOOP_TIME; - double hoodSlope = (futureState.pitch() - state.pitch()) / Constants.LOOP_TIME; - - turret.setFieldRelativeTarget(state.yaw(), yawSlope); - hood.setFieldRelativeTarget(new Rotation2d(state.pitch()), hoodSlope); - shooter.setShooter(state.exitVel()); - - } else if (stateOpt.isPresent() && futureStateOpt.isEmpty()) { - ShooterPhysics.TurretState state = stateOpt.get(); - - turret.setFieldRelativeTarget(state.yaw(), 0.0); - hood.setFieldRelativeTarget(new Rotation2d(state.pitch()), 0.0); - shooter.setShooter(state.exitVel()); - } - } - - @Override - public void end(boolean interrupted) { - // stop the turret where it is - turret.setFieldRelativeTarget(new Rotation2d(turret.getPositionRad()), 0.0); - hood.setFieldRelativeTarget(new Rotation2d(0), 0.0); - shooter.setShooter(0); - } -} diff --git a/src/main/java/frc/robot/commands/gpm/ReverseMotors.java b/src/main/java/frc/robot/commands/gpm/ReverseMotors.java deleted file mode 100644 index 2ed506d..0000000 --- a/src/main/java/frc/robot/commands/gpm/ReverseMotors.java +++ /dev/null @@ -1,35 +0,0 @@ -package frc.robot.commands.gpm; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.Intake.Intake; - -public class ReverseMotors extends Command { - private Intake intake; - - - public ReverseMotors(Intake intake){ - this.intake = intake; - - addRequirements(intake); - } - - @Override - public void initialize(){ - - } - - @Override - public void execute(){ - intake.extend(); - intake.spinReverse(); - //spindexer.reverseSpindexer(); - } - - @Override - public void end(boolean interrupted){ - intake.extend(); - intake.spinStart(); - //spindexer.maxSpindexer(); - } - -} diff --git a/src/main/java/frc/robot/commands/gpm/RunSpindexer.java b/src/main/java/frc/robot/commands/gpm/RunSpindexer.java deleted file mode 100644 index f2ba4ba..0000000 --- a/src/main/java/frc/robot/commands/gpm/RunSpindexer.java +++ /dev/null @@ -1,107 +0,0 @@ -package frc.robot.commands.gpm; - -import org.littletonrobotics.junction.Logger; - -import edu.wpi.first.math.filter.Debouncer; -import edu.wpi.first.math.filter.Debouncer.DebounceType; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.constants.Constants; -import frc.robot.subsystems.Intake.Intake; -import frc.robot.subsystems.Intake.IntakeConstants; -import frc.robot.subsystems.hood.Hood; -import frc.robot.subsystems.spindexer.Spindexer; -import frc.robot.subsystems.spindexer.SpindexerConstants; -import frc.robot.subsystems.turret.Turret; - -public class RunSpindexer extends Command { - private Spindexer spindexer; - private Turret turret; - private Hood hood; - private Intake intake; - - private Debouncer jam_debouncer = new Debouncer(SpindexerConstants.JAM_DEBOUNCE_TIME, DebounceType.kRising); // if there is jam I would think this is 0 -> 1 - - private boolean reversing = false; - private boolean wasHoodForcedDown = false; - - private Timer reverseTimer = new Timer(); - - private double storedIntakeSpeed = 0.0; - - public RunSpindexer(Spindexer spindexer, Turret turret, Hood hood, Intake intake) { - this.spindexer = spindexer; - this.turret = turret; - this.hood = hood; - this.intake = intake; - - addRequirements(spindexer); - } - - // public RunSpindexer(Spindexer spindexer) { - // this.spindexer = spindexer; - // addRequirements(spindexer); - // } - - @Override - public void initialize() { - wasHoodForcedDown = hood.getHoodForcedDown(); - } - - @Override - public void execute() { - boolean hoodForcedDown = hood.getHoodForcedDown(); - - if (wasHoodForcedDown && !hoodForcedDown) { - spindexer.maxSpindexer(); - } - wasHoodForcedDown = hoodForcedDown; - - if (!turret.atSetpoint() || hoodForcedDown || spindexer.noIndexing) { - spindexer.stopSpindexer(); - reversing = false; - return; // this is so the balls don't fly out when unaligned - } - boolean jammed = spindexer.getMotorOneVelocity() < SpindexerConstants.JAM_CURRENT_THRESHOLD && spindexer.getMotorOneStatorCurrent() > SpindexerConstants.JAM_CURRENT_THRESHOLD; - Logger.recordOutput("SpindexerJammed", jammed); - if (jam_debouncer.calculate(jammed)) { - Logger.recordOutput("SpindexerJammedDebounced", jammed); - - reversing = true; - reverseTimer.reset(); - reverseTimer.start(); - storedIntakeSpeed = intake.getSpeed(); - } - if (!reversing) { - spindexer.maxSpindexer(); - } else { - spindexer.reverseSpindexer(); - - if (intake.getPosition() > IntakeConstants.INTERMEDIATE_EXTENSION + 1.0) { - intake.spinReverse(); - } else { - intake.extend(); - } - - if (reverseTimer.hasElapsed(SpindexerConstants.REVERSE_DEBOUNCE_TIME)) { - reversing = false; - intake.spin(storedIntakeSpeed); - } - } - if (!Constants.DISABLE_SMART_DASHBOARD) { - SmartDashboard.putBoolean("Spindexer Jamming", reversing); - } - } - - @Override - public void end(boolean interrupted) { - spindexer.stopSpindexer(); - reversing = false; - } - - // @Override - // public boolean isFinished() { - // return false; // never ends on its own - // } -} diff --git a/src/main/java/frc/robot/commands/gpm/RunSpindexerWithStop.java b/src/main/java/frc/robot/commands/gpm/RunSpindexerWithStop.java deleted file mode 100644 index 742a0fb..0000000 --- a/src/main/java/frc/robot/commands/gpm/RunSpindexerWithStop.java +++ /dev/null @@ -1,121 +0,0 @@ -package frc.robot.commands.gpm; - -import org.littletonrobotics.junction.Logger; - -import edu.wpi.first.math.filter.Debouncer; -import edu.wpi.first.math.filter.Debouncer.DebounceType; -import edu.wpi.first.wpilibj.Timer; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.constants.Constants; -import frc.robot.subsystems.Intake.Intake; -import frc.robot.subsystems.Intake.IntakeConstants; -import frc.robot.subsystems.hood.Hood; -import frc.robot.subsystems.spindexer.Spindexer; -import frc.robot.subsystems.spindexer.SpindexerConstants; -import frc.robot.subsystems.turret.Turret; - -public class RunSpindexerWithStop extends Command { - private Spindexer spindexer; - private Turret turret; - private Hood hood; - private Intake intake; - - // if there is jam I would think this is 0 -> 1 - private Debouncer jam_debouncer = new Debouncer(SpindexerConstants.JAM_DEBOUNCE_TIME, DebounceType.kRising); - - private boolean reversing = false; - private boolean wasHoodForcedDown = false; - - private Timer reverseTimer = new Timer(); - - private double storedIntakeSpeed = 0.0; - - private Timer runTimer = new Timer(); - - private Debouncer debouncer = new Debouncer(0.3, DebounceType.kRising); - - private final double interval = 0.6; // Change this to make it faster/slower (seconds) - - public RunSpindexerWithStop(Spindexer spindexer, Turret turret, Hood hood, Intake intake) { - this.spindexer = spindexer; - this.turret = turret; - this.hood = hood; - this.intake = intake; - - addRequirements(spindexer); - } - - @Override - public void initialize() { - wasHoodForcedDown = hood.getHoodForcedDown(); - runTimer.reset(); - runTimer.start(); - } - - @Override - public void execute() { - boolean hoodForcedDown = hood.getHoodForcedDown(); - - if (wasHoodForcedDown && !hoodForcedDown) { - spindexer.maxSpindexer(); - } - wasHoodForcedDown = hoodForcedDown; - - if (!turret.atSetpoint() || hoodForcedDown || spindexer.noIndexing) { - spindexer.stopSpindexer(); - reversing = false; - return; // this is so the balls don't fly out when unaligned - } - boolean jammed = spindexer.getSubsystemStatorCurrent() / 2 > SpindexerConstants.JAM_CURRENT_THRESHOLD; - Logger.recordOutput("SpindexerJammed", jammed); - if (jam_debouncer.calculate(jammed)) { - Logger.recordOutput("SpindexerJammedDebounced", jammed); - - reversing = true; - reverseTimer.reset(); - reverseTimer.start(); - storedIntakeSpeed = intake.getSpeed(); - } - if (!reversing) { - spindexer.maxSpindexer(); - } else { - spindexer.reverseSpindexer(); - - if (intake.getPosition() > IntakeConstants.INTERMEDIATE_EXTENSION + 1.0) { - intake.spinReverse(); - } else { - intake.extend(); - } - - if (reverseTimer.hasElapsed(SpindexerConstants.REVERSE_DEBOUNCE_TIME)) { - reversing = false; - intake.spin(storedIntakeSpeed); - } - } - - if (!Constants.DISABLE_SMART_DASHBOARD) { - SmartDashboard.putBoolean("Spindexer Jamming", reversing); - } - - if ((int) (Timer.getFPGATimestamp() / interval) % 2 == 0) { - intake.extend(); - } else { - intake.intermediateExtend(); - } - - } - - @Override - public void end(boolean interrupted) { - spindexer.stopSpindexer(); - reversing = false; - - intake.extend(); - } - - @Override - public boolean isFinished() { - return (runTimer.hasElapsed(1.0) && debouncer.calculate(spindexer.spinningAir())) || runTimer.hasElapsed(4.0); - } -} diff --git a/src/main/java/frc/robot/commands/gpm/SimpleAutoShoot.java b/src/main/java/frc/robot/commands/gpm/SimpleAutoShoot.java deleted file mode 100644 index 902ec7c..0000000 --- a/src/main/java/frc/robot/commands/gpm/SimpleAutoShoot.java +++ /dev/null @@ -1,95 +0,0 @@ -package frc.robot.commands.gpm; - - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.constants.FieldConstants; -import frc.robot.subsystems.drivetrain.Drivetrain; -import frc.robot.subsystems.shooter.Shooter; -import frc.robot.subsystems.shooter.ShooterConstants; -import frc.robot.subsystems.turret.Turret; - -public class SimpleAutoShoot extends Command { - private Turret turret; - private Drivetrain drivetrain; - private Shooter shooter; - - private double fieldAngleRad; - private double turretSetpoint; - - private boolean SOTM = true; - private Translation2d drivepose; - - public SimpleAutoShoot(Turret turret, Drivetrain drivetrain, Shooter shooter) { - this.turret = turret; - this.drivetrain = drivetrain; - drivepose = drivetrain.getPose().getTranslation(); - - addRequirements(turret); - } - - public void updateTurretSetpoint(Translation2d drivepose) { - - //FieldZone currentZone = getZone(drivepose); - Translation2d target = FieldConstants.getHubTranslation().toTranslation2d(); - - double D_y; - double D_x; - double timeToGoal = 0.0; - - // If the robot is moving, adjust the target position based on velocity - if (SOTM) { - ChassisSpeeds robotRelVel = drivetrain.getChassisSpeeds(); - ChassisSpeeds fieldRelVel = ChassisSpeeds.fromRobotRelativeSpeeds(robotRelVel, drivetrain.getYaw()); - double xVel = fieldRelVel.vxMetersPerSecond; - double yVel = fieldRelVel.vyMetersPerSecond; - - D_y = target.getY() - drivepose.getY() - timeToGoal * yVel; - D_x = target.getX() - drivepose.getX() - timeToGoal * xVel; - } else { - D_y = target.getY() - drivepose.getY(); - D_x = target.getX() - drivepose.getX(); - } - - // Calculate the field-relative angle - fieldAngleRad = Math.atan2(D_y, D_x); - - // Calculate robot heading and adjust for reverse drive - double robotHeading = MathUtil.angleModulus((drivetrain.getYaw().getRadians() + Math.PI)); // Reverse drive adjustment - - // Calculate turret setpoint (angle relative to robot heading) - turretSetpoint = MathUtil.inputModulus(Units.radiansToDegrees(fieldAngleRad - robotHeading), -180.0, 180.0); - - } - - @Override - public void initialize() { - // Initialize setpoint calculation and set the initial goal for the turret - updateTurretSetpoint(drivepose); - turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(turretSetpoint)), 0); - } - - @Override - public void execute() { - // Continuously update setpoints and adjust based on vision if available - drivepose = drivetrain.getPose().getTranslation(); - updateTurretSetpoint(drivepose); - - turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(turretSetpoint)), -drivetrain.getAngularRate(2)); - shooter.setShooter(ShooterConstants.SHOOTER_VELOCITY); - - } - - @Override - public void end(boolean interrupted) { - // Set the turret to a safe position when the command ends - turret.setFieldRelativeTarget(new Rotation2d(0.0), 0.0); - shooter.setShooter(0.0); - } - -} - diff --git a/src/main/java/frc/robot/commands/gpm/Superstructure.java b/src/main/java/frc/robot/commands/gpm/Superstructure.java deleted file mode 100644 index 8173949..0000000 --- a/src/main/java/frc/robot/commands/gpm/Superstructure.java +++ /dev/null @@ -1,307 +0,0 @@ -package frc.robot.commands.gpm; - -import org.littletonrobotics.junction.Logger; -import org.littletonrobotics.junction.networktables.LoggedNetworkNumber; - -import edu.wpi.first.math.MathUtil; -import edu.wpi.first.math.filter.LinearFilter; -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.math.geometry.Translation2d; -import edu.wpi.first.math.geometry.Translation3d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.constants.Constants; -import frc.robot.constants.FieldConstants; -import frc.robot.constants.ShotInterpolation; -import frc.robot.constants.ShuttleInterpolation; -import frc.robot.subsystems.drivetrain.Drivetrain; -import frc.robot.subsystems.hood.Hood; -import frc.robot.subsystems.hood.HoodConstants; -import frc.robot.subsystems.shooter.Shooter; -import frc.robot.subsystems.spindexer.Spindexer; -import frc.robot.subsystems.turret.Turret; -import frc.robot.subsystems.turret.TurretConstants; -import frc.robot.util.PhaseManager; -import frc.robot.util.ShooterPhysics; -import frc.robot.util.ShooterPhysics.TurretState; - -public class Superstructure extends Command { - private Turret turret; - private Drivetrain drivetrain; - private Hood hood; - private Shooter shooter; - private Spindexer spindexer; - - private double turretSetpoint; - private double hoodSetpoint; - - private Pose2d drivepose; - - private final LinearFilter turretAngleFilter = LinearFilter.movingAverage((int) (0.1 / Constants.LOOP_TIME)); - private final LinearFilter hoodAngleFilter = LinearFilter.movingAverage((int) (0.1 / Constants.LOOP_TIME)); - - private Rotation2d lastTurretAngle; - private Rotation2d turretAngle; - private double turretVelocity; - - private double lastHoodAngle; - private double hoodAngle; - private double hoodVelocity; - - private TurretState goalState; - - private LoggedNetworkNumber phaseDelay = new LoggedNetworkNumber("/Tuning/OPERATOR/Phase Delay", 0.03); //Extrapolation delay due to latency - - private Translation2d target = FieldConstants.HUB_BLUE.toTranslation2d(); - - private PhaseManager phaseManager = new PhaseManager(); - - private LoggedNetworkNumber hoodOffset = new LoggedNetworkNumber("/Tuning/OPERATOR/Hood Offset", 0.0); - - private LoggedNetworkNumber turretOffset = new LoggedNetworkNumber("/Tuning/OPERATOR/Turret Offet",0.0); - - private double distanceFromTarget = 0.0; - private LoggedNetworkNumber TOFAdjustment = new LoggedNetworkNumber("/Tuning/OPERATOR/TOF Adjustment", 1.1); - - public Superstructure(Turret turret, Drivetrain drivetrain, Hood hood, Shooter shooter, Spindexer spindexer) { - this.turret = turret; - this.drivetrain = drivetrain; - this.hood = hood; - this.shooter = shooter; - this.spindexer = spindexer; - drivepose = drivetrain.getPose(); - - goalState = ShooterPhysics.getShotParams( - Translation2d.kZero, - FieldConstants.getHubTranslation().minus(new Translation3d(drivepose.getTranslation())), - 8.0); // Random initial goalState to prevent it being null - - addRequirements(turret, shooter); - } - - public void updateSetpoints(Pose2d drivepose) { - Pose2d turretPosition = drivepose.transformBy( - new Transform2d(TurretConstants.DISTANCE_FROM_ROBOT_CENTER.toTranslation2d(), Rotation2d.kZero)); - - // If the robot is moving, adjust the target position based on velocity - ChassisSpeeds robotRelVel = drivetrain.getChassisSpeeds(); - ChassisSpeeds fieldRelVel = ChassisSpeeds.fromRobotRelativeSpeeds(robotRelVel, drivetrain.getYaw()); - - // Rotational adjustment is not being used, since turret is in center of robot - double turretVelocityX = - fieldRelVel.vxMetersPerSecond; - double turretVelocityY = - fieldRelVel.vyMetersPerSecond; - - double timeOfFlight = 0; - Pose2d lookaheadPose = turretPosition; - - /* - * Loop (max 20) until lookaheadPose converges BECAUSE --> - * If you're 8m away (t = 1.0s) and moving at 2m/s towards target, you calculate for 6m (t = 0.8s) - * At 6m, we run assuming t = 0.8 but then the 6m isn't correct since it was derived using t = 1.0s - * So we make a bunch of guesses until it converges - * Early exit when change < 1mm to avoid unnecessary iterations - */ - for (int i = 0; i < 20; i++) { - Translation3d lookahead3d = new Translation3d(lookaheadPose.getX(), lookaheadPose.getY(), TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getZ()); - - Translation3d target3d = new Translation3d(target.getX(), target.getY(), - target.equals(FieldConstants.getHubTranslation().toTranslation2d()) ? - FieldConstants.getHubTranslation().getZ() : 0.0); // Height of 0 if it's not the hub - - goalState = ShooterPhysics.getShotParams( - Translation2d.kZero, - target3d.minus(lookahead3d), - 2.0); - - timeOfFlight = goalState.timeOfFlight() * TOFAdjustment.get(); - double offsetX = turretVelocityX * timeOfFlight; - double offsetY = turretVelocityY * timeOfFlight; - Pose2d newLookaheadPose = - new Pose2d( - turretPosition.getTranslation().plus(new Translation2d(offsetX, offsetY)), - turretPosition.getRotation()); - - // early exit if converged (change < 1mm) - if (i > 0 && lookaheadPose.getTranslation().getDistance(newLookaheadPose.getTranslation()) < 0.001) { - lookaheadPose = newLookaheadPose; - break; - } - lookaheadPose = newLookaheadPose; - } - - // Get the field angle relative to the target pose - turretAngle = target.minus(lookaheadPose.getTranslation()).getAngle(); - if (lastTurretAngle == null) { - lastTurretAngle = turretAngle; - } - - // Take the filtered average as the turret's velocity when robot is moving translationally - turretVelocity = - turretAngleFilter.calculate(turretAngle.minus(lastTurretAngle).getRadians() / Constants.LOOP_TIME); - - lastTurretAngle = turretAngle; - - if (!Constants.DISABLE_LOGGING) { - Logger.recordOutput("Turret/Target Pose", target); - Logger.recordOutput("Lookahead Pose", lookaheadPose); - } - if (!Constants.DISABLE_SMART_DASHBOARD) { - SmartDashboard.putNumber("Time of flight", timeOfFlight); - SmartDashboard.putNumber("Turret X-Velocity", turretVelocityX); - SmartDashboard.putNumber("Turret Y-Velocity", turretVelocityY); - } - - // Subtract the rotational angle of the robot from the setpoint - double adjustedTurretSetpoint = MathUtil.angleModulus(turretAngle.getRadians() - drivepose.getRotation().getRadians()); - - // Shortest path - double error = MathUtil.inputModulus(Units.radiansToDegrees(adjustedTurretSetpoint) - Units.radiansToDegrees(turret.getPositionRad()), -180, 180); - double potentialSetpoint = Units.radiansToDegrees(turret.getPositionRad()) + error + turretOffset.get(); - - // Stay within physical limits -- if shortest path is past max angle, we go long way around - if (potentialSetpoint > TurretConstants.MAX_ANGLE) { - potentialSetpoint -= 360; - } else if (potentialSetpoint < TurretConstants.MIN_ANGLE) { - potentialSetpoint += 360; - } - - turretSetpoint = potentialSetpoint; - - // Pitch is in radians - hoodAngle = goalState.pitch(); - hoodSetpoint = MathUtil.clamp(Units.radiansToDegrees(hoodAngle), HoodConstants.MIN_ANGLE, HoodConstants.MAX_ANGLE); - hoodVelocity = hoodAngleFilter.calculate((hoodAngle - lastHoodAngle) / Constants.LOOP_TIME); - lastHoodAngle = hoodAngle; - - distanceFromTarget = target.getDistance(lookaheadPose.getTranslation()); - Logger.recordOutput("Shooting/distanceToTarget", distanceFromTarget); - } - - public void updateDrivePose(){ - Pose2d currentPose = drivetrain.getPose(); - - drivepose = new Pose2d( - currentPose.getTranslation(), - // Uncomment this if robot is backwards - currentPose.getRotation()//.plus(new Rotation2d(Math.PI)) - ); - ChassisSpeeds robotRelVel = drivetrain.getChassisSpeeds(); - - // Add a phase delay extrapolation component for latency delay - // drivepose = drivepose.exp( - // new Twist2d( - // robotRelVel.vxMetersPerSecond * phaseDelay.get(), - // robotRelVel.vyMetersPerSecond * phaseDelay.get(), - // robotRelVel.omegaRadiansPerSecond * phaseDelay.get())); - } - - /** - * Stops and stows all subsystems involved in the command - */ - public void stowEverything(){ - turret.setFieldRelativeTarget(new Rotation2d(0.0), 0.0); - hood.setFieldRelativeTarget(Rotation2d.fromDegrees(HoodConstants.MAX_ANGLE), 0.0); - shooter.setShooter(0.0); - spindexer.noIndexing = true; - } - - public void underLadder(){ - spindexer.noIndexing = true; - } - - // shoot higher - public void bumpUpHoodOffset() { - hoodOffset.set(hoodOffset.get() + 1.0); //1 deg - } - - // shoot lower - public void bumpDownHoodOffset() { - hoodOffset.set(hoodOffset.get() - 1.0); //1 deg - } - - // aim more left - public void bumpUpTurretOffset() { - turretOffset.set(turretOffset.get() + 2.5); //2.5 deg - } - - // aim more right - public void bumpDownTurretOffset() { - turretOffset.set(turretOffset.get() - 2.5); //2.5 deg - } - - @Override - public void execute() { - // Phase manager stuff - phaseManager.update(drivepose, shooter, turret); - target = phaseManager.getTarget(drivepose); - - updateDrivePose(); - updateSetpoints(drivepose); - - if (phaseManager.isIdle()) { - underLadder(); - } else { - if (spindexer.noIndexing) { - spindexer.noIndexing = false; - } - turret.setFieldRelativeTarget(Rotation2d.fromDegrees(turretSetpoint), turretVelocity - drivetrain.getAngularRate(2)); - - boolean shuttling = !target.equals(FieldConstants.getHubTranslation().toTranslation2d()); // if we're aiming at the hub, we're not shuttling - - // shuttling will move the hood so its angles very close to max (less arch) - if (shuttling) { - hood.setFieldRelativeTarget(Rotation2d.fromDegrees(ShuttleInterpolation.newHoodMap.get(distanceFromTarget)), hoodVelocity); - } else { - hood.setFieldRelativeTarget(Rotation2d.fromDegrees(ShotInterpolation.newHoodMap.get(distanceFromTarget)), hoodVelocity); - } - - // if (FieldConstants.underTrench(x, y)) { - // System.out.println("Hood forced down"); - // } else { - // hood.forceHoodDown(false); - // } - - // different maps for shuttling vs shooting. Less powerful when shuttling. - if (shuttling) { - shooter.setShooter(-ShuttleInterpolation.shooterVelocityMap.get(distanceFromTarget)); - } else { - shooter.setShooter(-ShotInterpolation.shooterVelocityMap.get(distanceFromTarget)); - } - - if (!Constants.DISABLE_LOGGING) { - // record when shuttling - Logger.recordOutput("Shuttling", shuttling); - // record distance for tuning if needed - Logger.recordOutput("Distance From Target", distanceFromTarget); - } - } - - if (!Constants.DISABLE_LOGGING) { - Logger.recordOutput("Turret Calculated Setpoint", turretSetpoint); - Logger.recordOutput("Hood Calculate Setpoint", hoodSetpoint); - Logger.recordOutput("Shooter Calculate Velocity", goalState.exitVel()); - - Logger.recordOutput("DistanceToTarget", target.getDistance(drivepose.getTranslation())); - } - - // for operator - if (!Constants.DISABLE_SMART_DASHBOARD) { - SmartDashboard.putString("Phase Manager State", phaseManager.getCurrentState().toString()); - - } else { - phaseDelay.set(0.03); - } - } - - @Override - public void end(boolean interrupted) { - stowEverything(); - } - -} diff --git a/src/main/java/frc/robot/constants/FieldConstants.java b/src/main/java/frc/robot/constants/FieldConstants.java index 5cd3590..a500a8c 100644 --- a/src/main/java/frc/robot/constants/FieldConstants.java +++ b/src/main/java/frc/robot/constants/FieldConstants.java @@ -7,7 +7,6 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.util.Units; -import frc.robot.util.Zone; import edu.wpi.first.wpilibj.DriverStation.Alliance; import frc.robot.Robot; import frc.robot.constants.swerve.DriveConstants; @@ -22,200 +21,6 @@ public class FieldConstants { /** Height of the field [meters] */ public static final double FIELD_WIDTH = field.getFieldWidth(); - public static final double RED_BORDER = FIELD_LENGTH / 2 + Units.inchesToMeters(155); - public static final double BLUE_BORDER = FIELD_LENGTH / 2 - Units.inchesToMeters(155); - public static final double LEFT_SIDE_TARGET = FIELD_WIDTH * 0.225; - public static final double RIGHT_SIDE_TARGET = FIELD_WIDTH * 0.775; - - /** Location of hub target */ - public static final Translation3d HUB_BLUE = new Translation3d(Units.inchesToMeters(182.11), FIELD_WIDTH / 2, - Units.inchesToMeters(72)); - - public static final Translation3d HUB_RED = new Translation3d(FIELD_LENGTH - Units.inchesToMeters(182.11), - FIELD_WIDTH / 2, Units.inchesToMeters(72)); - - // shuttle safety constants - public static final double LADDER_ZONE_WIDTH = 1.251; // meters - public static final double LADDER_ZONE_DEPTH = 1.143; // meters - - // tower positions (center of the zone) - public static final double LADDER_Y_OFFSET = 1.7526 * 2; // two driver stations - public static final Pose2d BLUE_CLIMB_LOCATION = new Pose2d(LADDER_ZONE_DEPTH / 2, - FIELD_WIDTH - LADDER_Y_OFFSET - LADDER_ZONE_WIDTH / 2, new Rotation2d()); - public static final Pose2d RED_CLIMB_LOCATION = new Pose2d(FIELD_LENGTH - LADDER_ZONE_DEPTH / 2, - LADDER_Y_OFFSET + LADDER_ZONE_WIDTH / 2, new Rotation2d()); - - public static final Pose2d getClimbLocation() { - if (Robot.getAlliance() == Alliance.Blue) { - return BLUE_CLIMB_LOCATION; - } else { - return RED_CLIMB_LOCATION; - } - } - - public static final Translation3d NEUTRAL_LEFT = new Translation3d(FIELD_LENGTH / 2, LEFT_SIDE_TARGET, 0); - - public static final Translation3d NEUTRAL_RIGHT = new Translation3d(FIELD_LENGTH / 2, RIGHT_SIDE_TARGET, 0); - - // previous hub + a few feet further back - public static final Translation3d ALLIANCE_LEFT_BLUE = new Translation3d(BLUE_BORDER - 3.2, LEFT_SIDE_TARGET, 0); - - public static final Translation3d ALLIANCE_RIGHT_BLUE = new Translation3d(BLUE_BORDER - 2.2, RIGHT_SIDE_TARGET, 0); - // previous hub + a few feet further back - - public static final Translation3d ALLIANCE_LEFT_RED = new Translation3d(RED_BORDER + 2.2, LEFT_SIDE_TARGET, 0); - - public static final Translation3d ALLIANCE_RIGHT_RED = new Translation3d(RED_BORDER + 3.2, RIGHT_SIDE_TARGET, 0); - - public static final Translation3d ALLIANCE_CENTER_BLUE = new Translation3d(BLUE_BORDER - 2, FIELD_WIDTH / 2, 0); - - public static final Translation3d ALLIANCE_CENTER_RED = new Translation3d(RED_BORDER + 2, FIELD_WIDTH / 2, 0); - - public static final double BLUE_ALLIANCE_LINE = BLUE_BORDER; // That's the distance from one side to the blue bump - public static final double RED_ALLIANCE_LINE = RED_BORDER; // That's the distance from one side to the red bump - - // my zones - public static final double leftNeutralLine = FIELD_LENGTH * 0.25; - public static final double rightNeutralLine = FIELD_LENGTH * 0.75; - public static final double centerLengthLine = FIELD_LENGTH * 0.5; - public static final double centerWidthLine = FIELD_WIDTH * 0.5; - public static final double redLine = 179.111250; - public static final double blueLine = FIELD_LENGTH - 179.111250; - - public static final double hubWidthLeft = FIELD_WIDTH / 2 - (47.0 / 2); - public static final double hubWidthRight = FIELD_WIDTH / 2 + (47.0 / 2); - public static final double hubBackRed = FIELD_LENGTH + 120.0; - public static final double hubBackBlue = FIELD_LENGTH - 120.0; - - public static final double ladderRedLeft = FIELD_WIDTH + 13.0; - public static final double ladderBlueLeft = FIELD_WIDTH - 12.375; - public static final double ladderRedRight = FIELD_WIDTH - 35.75; - public static final double ladderBlueRight = FIELD_WIDTH + 35.75; - - public static final double TRENCH_CENTER_CHANNEL_WIDTH_INCHES = 50.0; - public static final double TRENCH_X_MIN_INCHES = 152.5; - public static final double TRENCH_X_MAX_INCHES = 187.5; - - public static final Zone neutralStrip = new Zone(centerLengthLine, centerWidthLine, - rightNeutralLine - leftNeutralLine, redLine - blueLine); - public static final Zone neutralLeft = new Zone(centerLengthLine, centerWidthLine, rightNeutralLine - leftNeutralLine, - redLine - blueLine); - public static final Zone neutralRight = new Zone(centerLengthLine, centerWidthLine, - rightNeutralLine - leftNeutralLine, redLine - blueLine); - public static final Zone blueHubOut = new Zone(centerLengthLine, centerWidthLine, rightNeutralLine - leftNeutralLine, - redLine - blueLine); - public static final Zone redHubOut = new Zone(centerLengthLine, centerWidthLine, rightNeutralLine - leftNeutralLine, - redLine - blueLine); - - public enum ShootingTarget { - HUB, - NEUTRAL, - ALLIANCE, - OPPOSITION, // not sure why you'd ever do this :) - } - - public enum FieldZone { - ALLIANCE, - NEUTRAL, - OPPOSITION, - TRENCH_BUMP, - UNDER_LADDER, - UNDER_MY_HUB - } - - /** checks if robot is under climb structure */ - public static boolean underLadder(Translation2d drivepose) { - Pose2d ladderPos = getClimbLocation(); - double dx = Math.abs(drivepose.getX() - ladderPos.getX()); - double dy = Math.abs(drivepose.getY() - ladderPos.getY()); - return (dx < LADDER_ZONE_DEPTH / 2.0) && (dy < LADDER_ZONE_WIDTH / 2.0); - } - - public static Translation3d getHubTranslation() { - if (Robot.getAlliance() == Alliance.Blue) { - return HUB_BLUE; - } else { - return HUB_RED; - } - } - - public static Translation3d getNeutralTranslation(boolean sideLeft) { - if (sideLeft) { - return NEUTRAL_LEFT; - } else { - return NEUTRAL_RIGHT; - } - } - - public static Translation3d getAllianceSideTranslation(boolean sideLeft) { - if (sideLeft) { - if (Robot.getAlliance() == Alliance.Blue) { - return ALLIANCE_LEFT_BLUE; - } else { - return ALLIANCE_LEFT_RED; - } - } else { - if (Robot.getAlliance() == Alliance.Blue) { - return ALLIANCE_RIGHT_BLUE; - } else { - return ALLIANCE_RIGHT_RED; - } - } - } - - public static Translation3d getAllianceCenterTranslation() { - if (Robot.getAlliance() == Alliance.Blue) { - return ALLIANCE_CENTER_BLUE; - } else { - return ALLIANCE_CENTER_RED; - } - } - - public static FieldZone getZone(Translation2d drivepose) { - double x = drivepose.getX(); - - if ((x < FIELD_LENGTH / 2 - Units.inchesToMeters(120.0) - && x > (BLUE_ALLIANCE_LINE + (DriveConstants.ROBOT_WIDTH_WITH_BUMPERS) / 2)) // blue alliance line - || x > FIELD_LENGTH / 2 + Units.inchesToMeters(120.0) - && x < (RED_ALLIANCE_LINE - (DriveConstants.ROBOT_WIDTH_WITH_BUMPERS) / 2)) { - return FieldZone.TRENCH_BUMP; - } - - // if (underLadder(drivepose)) { - // return FieldZone.UNDER_LADDER; - // } - - if (x > FieldConstants.RED_ALLIANCE_LINE - (DriveConstants.ROBOT_WIDTH_WITH_BUMPERS) / 2) { - return (Robot.getAlliance() == Alliance.Red) ? FieldZone.ALLIANCE : FieldZone.OPPOSITION; - } else if (x < FieldConstants.BLUE_ALLIANCE_LINE + (DriveConstants.ROBOT_WIDTH_WITH_BUMPERS) / 2) { - return (Robot.getAlliance() == Alliance.Blue) ? FieldZone.ALLIANCE : FieldZone.OPPOSITION; - } else { - return FieldZone.NEUTRAL; - } - } - - /** - * returns true if robot is currently underneath or touching the climbing ladder - */ - public static boolean isUnderLadder(Translation2d drivepose) { - return getZone(drivepose) == FieldZone.UNDER_LADDER; - } - - public static boolean underTrench(double x, double y) { - // ensures we aren't in center channel - if (y > Units.inchesToMeters(TRENCH_CENTER_CHANNEL_WIDTH_INCHES) - && y < FIELD_WIDTH - Units.inchesToMeters(TRENCH_CENTER_CHANNEL_WIDTH_INCHES)) { - return false; - } - // if our location is to far away from right underneath trench in terms of x - // in between blue alliance trench - if (!(x > Units.inchesToMeters(TRENCH_X_MIN_INCHES) && x < Units.inchesToMeters(TRENCH_X_MAX_INCHES)) - && !(x < FIELD_LENGTH - Units.inchesToMeters(TRENCH_X_MIN_INCHES) - && x > FIELD_LENGTH - Units.inchesToMeters(TRENCH_X_MAX_INCHES))) { - return false; - } - return true; - } /** * @@ -226,21 +31,4 @@ public class FieldConstants { return drivepose.getY() > FIELD_WIDTH / 2; } - public static Translation3d getOppositionTranslation(boolean sideLeft) { - if (sideLeft) { - if (Robot.getAlliance() == Alliance.Blue) { - return ALLIANCE_LEFT_RED; - } else { - // Reversed it so we shoot same side, but probably need to change this - return ALLIANCE_LEFT_BLUE; - } - } else { - if (Robot.getAlliance() == Alliance.Blue) { - return ALLIANCE_RIGHT_RED; - } else { - return ALLIANCE_RIGHT_BLUE; - } - } - } - } diff --git a/src/main/java/frc/robot/constants/ShotInterpolation.java b/src/main/java/frc/robot/constants/ShotInterpolation.java deleted file mode 100644 index c7301b7..0000000 --- a/src/main/java/frc/robot/constants/ShotInterpolation.java +++ /dev/null @@ -1,67 +0,0 @@ -package frc.robot.constants; - -import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; - -public class ShotInterpolation { - public static final InterpolatingDoubleTreeMap timeOfFlightMap = new InterpolatingDoubleTreeMap(); - public static final InterpolatingDoubleTreeMap shooterPowerMap = new InterpolatingDoubleTreeMap(); - public static final InterpolatingDoubleTreeMap hoodAngleMap = new InterpolatingDoubleTreeMap(); - - public static final InterpolatingDoubleTreeMap exitVelocityMap = new InterpolatingDoubleTreeMap(); - - public static final InterpolatingDoubleTreeMap shooterVelocityMap = new InterpolatingDoubleTreeMap(); - - public static final InterpolatingDoubleTreeMap newHoodMap = new InterpolatingDoubleTreeMap(); - - static{ - timeOfFlightMap.put(0.0, 0.67); - timeOfFlightMap.put(1.0, 0.67); - - shooterPowerMap.put(0.0, 1.0); - shooterPowerMap.put(1.0, 1.0); - - //hoodAngleMap.put(HoodConstants.MAX_ANGLE, HoodConstants.MAX_ANGLE); - hoodAngleMap.put(81.3, 70.25); - hoodAngleMap.put(79.0, 65.9); - hoodAngleMap.put(58.5, 48.5); - //hoodAngleMap.put(1.0, Units.degreesToRadians(90)); - - exitVelocityMap.put(0.0, 0.0); - exitVelocityMap.put(1.0, 2.2); - exitVelocityMap.put(2.0, 4.4); - exitVelocityMap.put(7.0, 12.0); - exitVelocityMap.put(7.78, 16.8); - exitVelocityMap.put(7.8, 15.2); - exitVelocityMap.put(7.9, 17.1); - exitVelocityMap.put(8.0, 17.9); - exitVelocityMap.put(8.08, 19.0); - exitVelocityMap.put( 21.0, 19.0); - - exitVelocityMap.put(9.90, 14.0); - exitVelocityMap.put(9.95, 16.5); - exitVelocityMap.put(10.0, 19.2); - exitVelocityMap.put(11.0, 26.0); - exitVelocityMap.put(25.0, 25.0* 3.2); - - // currently regresses to y = 1.34959x + 9.79618 - shooterVelocityMap.put(0.0, 9.55 * 1.05); - shooterVelocityMap.put(1.00, 11.5 * 1.05); - shooterVelocityMap.put(2.00, 12.3 * 1.1); - shooterVelocityMap.put(3.00, 14.0 * 1.075); - shooterVelocityMap.put(4.00, 15.5 * 1.075); - shooterVelocityMap.put(5.00, 17.0 * 1.05); - shooterVelocityMap.put(5.60, 18.0 * 1.05); - shooterVelocityMap.put(25.0, 43.44 * 1.05); - - - newHoodMap.put(0.0, 75.9); - newHoodMap.put(1.00, 78.0); - newHoodMap.put(1.49, 72.0 - 2.0 - 1.0); - newHoodMap.put(2.09, 70.0 - 4.0 - 2.0); - newHoodMap.put(2.95, 68.0 - 4.0 - 2.0); - newHoodMap.put(4.07, 65.0 - 3.0 - 2.0); - newHoodMap.put(5.05, 60.0 - 1.0 - 1.0); - newHoodMap.put(5.79, 59.0 - 2.0); - newHoodMap.put(27.99, 0.0); - } -} diff --git a/src/main/java/frc/robot/constants/ShuttleInterpolation.java b/src/main/java/frc/robot/constants/ShuttleInterpolation.java deleted file mode 100644 index 7cf9b4a..0000000 --- a/src/main/java/frc/robot/constants/ShuttleInterpolation.java +++ /dev/null @@ -1,30 +0,0 @@ -package frc.robot.constants; - -import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap; - -public class ShuttleInterpolation { - public static final InterpolatingDoubleTreeMap newHoodMap = new InterpolatingDoubleTreeMap(); - public static final InterpolatingDoubleTreeMap shooterVelocityMap = new InterpolatingDoubleTreeMap(); - /* - * guide to tuning: - * modify the left values in the parenthesis to change the distance of the shot. - * setup the robot at that distance and shoot. Then modifty the right value to - * be more or less until it hits target perfectly. - * Repeat - * OR - * Run robot, estimate distance, and tweak values when nobodies looking until it - * works. - */ - static { - // we can be less aggressive: y = 0.65 * (1.34959x + 9.79618) - // will likely be this that requires tuning. - shooterVelocityMap.put(0.0, 9.0); - shooterVelocityMap.put(4.0, 12.0 * 1.3); // tuned by wesley - shooterVelocityMap.put(8.0, 22.0 * 1.075); // tuned by wesley - shooterVelocityMap.put(16.0, 100.0); // tuned by taren - - // always shoot at low angle to ground. - newHoodMap.put(0.0, 55.0); // min angle (w/ 0.5 deg buffer) - newHoodMap.put(27.99, 55.0); // min angle (w/ 0.5 deg buffer) - } -} diff --git a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java index 37e1d23..829c55f 100644 --- a/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java +++ b/src/main/java/frc/robot/controls/PS5ControllerDriverConfig.java @@ -9,17 +9,8 @@ import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.InstantCommand; import frc.robot.Robot; import frc.robot.commands.drive_comm.SysIDDriveCommand; -import frc.robot.commands.gpm.IntakeMovementCommand; -import frc.robot.commands.gpm.ReverseMotors; -import frc.robot.commands.gpm.RunSpindexer; -import frc.robot.commands.gpm.Superstructure; import frc.robot.constants.Constants; -import frc.robot.subsystems.Intake.Intake; import frc.robot.subsystems.drivetrain.Drivetrain; -import frc.robot.subsystems.hood.Hood; -import frc.robot.subsystems.shooter.Shooter; -import frc.robot.subsystems.spindexer.Spindexer; -import frc.robot.subsystems.turret.Turret; import lib.controllers.PS5Controller; import lib.controllers.PS5Controller.DPad; import lib.controllers.PS5Controller.PS5Axis; @@ -29,156 +20,58 @@ import lib.controllers.PS5Controller.PS5Button; * Driver controls for the PS5 controller */ public class PS5ControllerDriverConfig extends BaseDriverConfig { - private final PS5Controller controller = new PS5Controller(Constants.DRIVER_JOY); - private final BooleanSupplier slowModeSupplier = () -> false; - private boolean intakeBoolean = true; - private Command autoShoot = null; - private Shooter shooter; - private Turret turret; - private Hood hood; - private Intake intake; - private Spindexer spindexer; - - public PS5ControllerDriverConfig( - Drivetrain drive, - Shooter shooter, - Turret turret, - Hood hood, - Intake intake, - Spindexer spindexer) { - super(drive); - this.shooter = shooter; - this.turret = turret; - this.hood = hood; - this.intake = intake; - this.spindexer = spindexer; - } - - public void configureControls() { - // Reset the yaw. Mainly useful for testing/driver practice - controller.get(PS5Button.CREATE).onTrue(new InstantCommand(() -> getDrivetrain().setYaw( - new Rotation2d(Robot.getAlliance() == Alliance.Blue ? 0 : Math.PI)))); - - // Cancel commands - controller.get(PS5Button.RB).onTrue(new InstantCommand(() -> { - getDrivetrain().setIsAlign(false); - getDrivetrain().setDesiredPose(() -> null); - CommandScheduler.getInstance().cancelAll(); - })); - - // Reverse motors - if (intake != null && spindexer != null) { - controller.get(PS5Button.LB).whileTrue(new ReverseMotors(intake)); - } - - // Intake - if (intake != null) { - // Toggle intake - controller.get(PS5Button.RIGHT_TRIGGER).onTrue(new InstantCommand(() -> { - if (intakeBoolean) { - intake.extend(); - intake.spinStart(); - intakeBoolean = false; - } else { - intake.intermediateExtend(); - intake.spinStop(); - intakeBoolean = true; - } - }, intake)); - - // Retract if hold for 2 seconds - controller.get(PS5Button.RIGHT_TRIGGER).debounce(2.0).onTrue(new InstantCommand(() -> { - intake.retract(); - intakeBoolean = true; - intake.spinStop(); - }, intake)); - - // Make the intake go in and out while shooting - controller.get(DPad.UP).whileTrue(new IntakeMovementCommand(intake) - .alongWith(new InstantCommand(()-> intakeBoolean = true))); - - // Calibration: you can now calibrate easily using this button - if (hood != null && intake != null) { - controller.get(PS5Button.PS).onTrue(new InstantCommand(() -> { - intake.calibrate(); - hood.calibrate(); - }, intake, hood)).onFalse(new InstantCommand(() -> { - intake.stopCalibrating(); - hood.stopCalibrating(); - }, intake, hood)); - } - - // Stop intake roller - controller.get(DPad.DOWN).onTrue(new InstantCommand(()->{ - if(intakeBoolean){ - intake.spinStart(); - intakeBoolean = false; - } else{ - intake.spinStop(); - intakeBoolean = true; - } - })); - } - - // Spindexer - if (spindexer != null && turret != null && hood != null && intake != null) { - - // Toggle spindexer - controller.get(PS5Button.LEFT_TRIGGER).toggleOnTrue( - new RunSpindexer(spindexer, turret, hood, intake) - ); - } - - // Auto shoot - if (turret != null && hood != null && shooter != null && spindexer != null) { - autoShoot = new Superstructure(turret, getDrivetrain(), hood, shooter, spindexer); - controller.get(PS5Button.SQUARE).toggleOnTrue(autoShoot); - } - - - // Hood - if (hood != null) { - // Set the hood down -- for safety measures under trench - controller.get(DPad.LEFT).onTrue(new InstantCommand(()->{ - hood.forceHoodDown(true); - }, hood)).onFalse(new InstantCommand(()->{ - hood.forceHoodDown(false); - })); - } - } - - @Override - public double getRawSideTranslation() { - return controller.get(PS5Axis.LEFT_X); - } - - @Override - public double getRawForwardTranslation() { - return controller.get(PS5Axis.LEFT_Y); - } - - @Override - public double getRawRotation() { - return controller.get(PS5Axis.RIGHT_X); - } - - @Override - public double getRawHeadingAngle() { - return Math.atan2(controller.get(PS5Axis.RIGHT_X), -controller.get(PS5Axis.RIGHT_Y)) - Math.PI / 2; - } - - @Override - public double getRawHeadingMagnitude() { - return Math.hypot(controller.get(PS5Axis.RIGHT_X), controller.get(PS5Axis.RIGHT_Y)); - } - - @Override - public boolean getIsSlowMode() { - return slowModeSupplier.getAsBoolean(); - } - - @Override - public boolean getIsAlign() { - return false; - } + private final PS5Controller controller = new PS5Controller(Constants.DRIVER_JOY); + private final BooleanSupplier slowModeSupplier = () -> false; + + public PS5ControllerDriverConfig(Drivetrain drive) { + super(drive); + } + + public void configureControls() { + // Reset the yaw. Mainly useful for testing/driver practice + controller.get(PS5Button.CREATE).onTrue(new InstantCommand(() -> getDrivetrain().setYaw( + new Rotation2d(Robot.getAlliance() == Alliance.Blue ? 0 : Math.PI)))); + + // Cancel commands + controller.get(PS5Button.RB).onTrue(new InstantCommand(() -> { + getDrivetrain().setIsAlign(false); + getDrivetrain().setDesiredPose(() -> null); + CommandScheduler.getInstance().cancelAll(); + })); + } + + @Override + public double getRawSideTranslation() { + return controller.get(PS5Axis.LEFT_X); + } + + @Override + public double getRawForwardTranslation() { + return controller.get(PS5Axis.LEFT_Y); + } + + @Override + public double getRawRotation() { + return controller.get(PS5Axis.RIGHT_X); + } + + @Override + public double getRawHeadingAngle() { + return Math.atan2(controller.get(PS5Axis.RIGHT_X), -controller.get(PS5Axis.RIGHT_Y)) - Math.PI / 2; + } + + @Override + public double getRawHeadingMagnitude() { + return Math.hypot(controller.get(PS5Axis.RIGHT_X), controller.get(PS5Axis.RIGHT_Y)); + } + + @Override + public boolean getIsSlowMode() { + return slowModeSupplier.getAsBoolean(); + } + + @Override + public boolean getIsAlign() { + return false; + } } diff --git a/src/main/java/frc/robot/controls/PS5XboxModeDriverConfig.java b/src/main/java/frc/robot/controls/PS5XboxModeDriverConfig.java index b5bfdb0..20dd5f3 100644 --- a/src/main/java/frc/robot/controls/PS5XboxModeDriverConfig.java +++ b/src/main/java/frc/robot/controls/PS5XboxModeDriverConfig.java @@ -11,15 +11,8 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.robot.Robot; -import frc.robot.commands.gpm.ReverseMotors; -import frc.robot.commands.gpm.Superstructure; import frc.robot.constants.Constants; import frc.robot.subsystems.drivetrain.Drivetrain; -import frc.robot.subsystems.hood.Hood; -import frc.robot.subsystems.Intake.Intake; -import frc.robot.subsystems.shooter.Shooter; -import frc.robot.subsystems.spindexer.Spindexer; -import frc.robot.subsystems.turret.Turret; import lib.controllers.GameController; import lib.controllers.GameController.Axis; import lib.controllers.GameController.Button; @@ -38,195 +31,106 @@ import lib.controllers.GameController.DPad; */ public class PS5XboxModeDriverConfig extends BaseDriverConfig { - private final GameController controller = new GameController(Constants.DRIVER_JOY); - private final BooleanSupplier slowModeSupplier = () -> false; - private boolean intakeBoolean = true; - private Command autoShoot = null; - private Command reverseMotors = null; - private Shooter shooter; - private Turret turret; - private Hood hood; - private Intake intake; - private Spindexer spindexer; - - // PS5 button aliases - // private final Button CROSS = Button.A; - private final Button CIRCLE = Button.B; - private final Button SQUARE = Button.X; - // private final Button TRIANGLE = Button.Y; - // private final Button LB = Button.LB; - private final Button RB = Button.RB; - private final Button CREATE = Button.BACK; - // private final Button OPTIONS = Button.START; - private final Button LEFT_JOY = Button.LEFT_JOY; - private final Button RIGHT_JOY = Button.RIGHT_JOY; - - // PS5 trigger buttons - private final BooleanSupplier LEFT_TRIGGER_BUTTON = controller.LEFT_TRIGGER_BUTTON; - private final BooleanSupplier RIGHT_TRIGGER_BUTTON = controller.RIGHT_TRIGGER_BUTTON; - - // PS5 axis aliases - private final Axis LEFT_X = Axis.LEFT_X; - private final Axis LEFT_Y = Axis.LEFT_Y; - private final Axis RIGHT_X = Axis.RIGHT_X; - private final Axis RIGHT_Y = Axis.RIGHT_Y; - // private final Axis LEFT_TRIGGER = Axis.LEFT_TRIGGER; - // private final Axis RIGHT_TRIGGER = Axis.RIGHT_TRIGGER; - - public PS5XboxModeDriverConfig( - Drivetrain drive, - Shooter shooter, - Turret turret, - Hood hood, - Intake intake, - Spindexer spindexer) { - super(drive); - this.shooter = shooter; - this.turret = turret; - this.hood = hood; - this.intake = intake; - this.spindexer = spindexer; - } - - public void configureControls() { - // Reset the yaw. Mainly useful for testing/driver practice - controller.get(CREATE).onTrue(new InstantCommand(() -> getDrivetrain().setYaw( - new Rotation2d(Robot.getAlliance() == Alliance.Blue ? 0 : Math.PI)))); - - // Cancel commands - controller.get(RB).onTrue(new InstantCommand(() -> { - getDrivetrain().setIsAlign(false); - getDrivetrain().setDesiredPose(() -> null); - CommandScheduler.getInstance().cancelAll(); - })); - - // Align wheels - controller.get(DPad.RIGHT).onTrue(new FunctionalCommand( - () -> getDrivetrain().setStateDeadband(false), - getDrivetrain()::alignWheels, - interrupted -> getDrivetrain().setStateDeadband(true), - () -> false, getDrivetrain()).withTimeout(2)); - - // Trench align - controller.get(DPad.LEFT).onTrue(new InstantCommand(() -> { - getDrivetrain().setTrenchAssist(true); - getDrivetrain().setTrenchAlign(true); - })) - .onFalse(new InstantCommand(() -> { - getDrivetrain().setTrenchAssist(false); - getDrivetrain().setTrenchAlign(false); - })); - - // Reverse motors - if (intake != null && spindexer != null && shooter != null) { - controller.get(CIRCLE).onTrue(new InstantCommand(() -> { - reverseMotors = new ReverseMotors(intake); - CommandScheduler.getInstance().schedule(reverseMotors); - })).onFalse(new InstantCommand(() -> { - if (reverseMotors != null) { - reverseMotors.cancel(); - } - })); - } - - // Intake - if (intake != null) { - // Toggle intake - controller.get(RIGHT_TRIGGER_BUTTON).onTrue(new InstantCommand(() -> { - if (intakeBoolean) { - intake.extend(); - intake.spinStart(); - intakeBoolean = false; - } else { - intake.intermediateExtend(); - intake.spinStop(); - intakeBoolean = true; - } - })); - - // Retract if hold for 3 seconds - controller.get(RIGHT_TRIGGER_BUTTON).debounce(3.0).onTrue(new InstantCommand(() -> { - intake.retract(); - intakeBoolean = true; - })); - - // Calibration - controller.get(LEFT_JOY).onTrue(new InstantCommand(() -> { - intake.calibrate(); - })).onFalse(new InstantCommand(() -> { - intake.stopCalibrating(); - })); - } - - // Spindexer - if (spindexer != null) { - // Will only run if we are not calling default shoot command - controller.get(LEFT_TRIGGER_BUTTON).onTrue(new InstantCommand(() -> spindexer.maxSpindexer())) - .onFalse(new InstantCommand(() -> spindexer.stopSpindexer())); - } - - // Auto shoot - if (turret != null && hood != null && shooter != null && spindexer != null) { - autoShoot = new Superstructure(turret, getDrivetrain(), hood, shooter, spindexer); - controller.get(SQUARE).toggleOnTrue(autoShoot); - } - - // Hood - if (hood != null) { - controller.get(LEFT_JOY).onTrue(new InstantCommand(() -> { - hood.calibrate(); - })).onFalse(new InstantCommand(() -> { - hood.stopCalibrating(); - })); - } - - // Rumble test - controller.get(RIGHT_JOY).onTrue(new SequentialCommandGroup( - new InstantCommand(() -> controller.setRumble(GameController.RumbleStatus.RUMBLE_ON)), - new WaitCommand(0.5), - new InstantCommand(() -> controller.setRumble(GameController.RumbleStatus.RUMBLE_OFF)))); - } - - @Override - public double getRawSideTranslation() { - return controller.get(LEFT_X); - } - - @Override - public double getRawForwardTranslation() { - return controller.get(LEFT_Y); - } - - @Override - public double getRawRotation() { - return controller.get(RIGHT_X); - } - - @Override - public double getRawHeadingAngle() { - return Math.atan2(controller.get(RIGHT_X), -controller.get(RIGHT_Y)) - Math.PI / 2; - } - - @Override - public double getRawHeadingMagnitude() { - return Math.hypot(controller.get(RIGHT_X), controller.get(RIGHT_Y)); - } - - @Override - public boolean getIsSlowMode() { - return slowModeSupplier.getAsBoolean(); - } - - @Override - public boolean getIsAlign() { - return false; - } - - public void startRumble() { - controller.setRumble(GameController.RumbleStatus.RUMBLE_ON); - } - - public void endRumble() { - controller.setRumble(GameController.RumbleStatus.RUMBLE_OFF); - } + private final GameController controller = new GameController(Constants.DRIVER_JOY); + private final BooleanSupplier slowModeSupplier = () -> false; + private boolean intakeBoolean = true; + private Command autoShoot = null; + private Command reverseMotors = null; + + // PS5 button aliases + // private final Button CROSS = Button.A; + private final Button CIRCLE = Button.B; + private final Button SQUARE = Button.X; + // private final Button TRIANGLE = Button.Y; + // private final Button LB = Button.LB; + private final Button RB = Button.RB; + private final Button CREATE = Button.BACK; + // private final Button OPTIONS = Button.START; + private final Button LEFT_JOY = Button.LEFT_JOY; + private final Button RIGHT_JOY = Button.RIGHT_JOY; + + // PS5 trigger buttons + private final BooleanSupplier LEFT_TRIGGER_BUTTON = controller.LEFT_TRIGGER_BUTTON; + private final BooleanSupplier RIGHT_TRIGGER_BUTTON = controller.RIGHT_TRIGGER_BUTTON; + + // PS5 axis aliases + private final Axis LEFT_X = Axis.LEFT_X; + private final Axis LEFT_Y = Axis.LEFT_Y; + private final Axis RIGHT_X = Axis.RIGHT_X; + private final Axis RIGHT_Y = Axis.RIGHT_Y; + // private final Axis LEFT_TRIGGER = Axis.LEFT_TRIGGER; + // private final Axis RIGHT_TRIGGER = Axis.RIGHT_TRIGGER; + + public PS5XboxModeDriverConfig(Drivetrain drive) { + super(drive); + } + + public void configureControls() { + // Reset the yaw. Mainly useful for testing/driver practice + controller.get(CREATE).onTrue(new InstantCommand(() -> getDrivetrain().setYaw( + new Rotation2d(Robot.getAlliance() == Alliance.Blue ? 0 : Math.PI)))); + + // Cancel commands + controller.get(RB).onTrue(new InstantCommand(() -> { + getDrivetrain().setIsAlign(false); + getDrivetrain().setDesiredPose(() -> null); + CommandScheduler.getInstance().cancelAll(); + })); + + // Align wheels + controller.get(DPad.RIGHT).onTrue(new FunctionalCommand( + () -> getDrivetrain().setStateDeadband(false), + getDrivetrain()::alignWheels, + interrupted -> getDrivetrain().setStateDeadband(true), + () -> false, getDrivetrain()).withTimeout(2)); + + // Rumble test + controller.get(RIGHT_JOY).onTrue(new SequentialCommandGroup( + new InstantCommand(() -> controller.setRumble(GameController.RumbleStatus.RUMBLE_ON)), + new WaitCommand(0.5), + new InstantCommand(() -> controller.setRumble(GameController.RumbleStatus.RUMBLE_OFF)))); + } + + @Override + public double getRawSideTranslation() { + return controller.get(LEFT_X); + } + + @Override + public double getRawForwardTranslation() { + return controller.get(LEFT_Y); + } + + @Override + public double getRawRotation() { + return controller.get(RIGHT_X); + } + + @Override + public double getRawHeadingAngle() { + return Math.atan2(controller.get(RIGHT_X), -controller.get(RIGHT_Y)) - Math.PI / 2; + } + + @Override + public double getRawHeadingMagnitude() { + return Math.hypot(controller.get(RIGHT_X), controller.get(RIGHT_Y)); + } + + @Override + public boolean getIsSlowMode() { + return slowModeSupplier.getAsBoolean(); + } + + @Override + public boolean getIsAlign() { + return false; + } + + public void startRumble() { + controller.setRumble(GameController.RumbleStatus.RUMBLE_ON); + } + + public void endRumble() { + controller.setRumble(GameController.RumbleStatus.RUMBLE_OFF); + } } diff --git a/src/main/java/frc/robot/subsystems/LED/LED.java b/src/main/java/frc/robot/subsystems/LED/LED.java index 4e49e16..d502d49 100644 --- a/src/main/java/frc/robot/subsystems/LED/LED.java +++ b/src/main/java/frc/robot/subsystems/LED/LED.java @@ -28,7 +28,6 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.constants.Constants; import frc.robot.constants.IdConstants; -import frc.robot.util.HubActive; public class LED extends SubsystemBase { @@ -163,8 +162,10 @@ public class LED extends SubsystemBase { private boolean underSecsToFlip(double secs) { - Optional timeToActive = HubActive.timeToActive(); - Optional timeToInactive = HubActive.timeToInactive(); + // Optional timeToActive = HubActive.timeToActive(); + // Optional timeToInactive = HubActive.timeToInactive(); + Optional timeToActive = Optional.empty(); + Optional timeToInactive = Optional.empty(); if (timeToActive.isEmpty() && timeToInactive.isEmpty()) { return false; diff --git a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java index 2c45f57..660eb1d 100644 --- a/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java +++ b/src/main/java/frc/robot/subsystems/drivetrain/Drivetrain.java @@ -429,27 +429,6 @@ public class Drivetrain extends SubsystemBase { Arrays.stream(modules).forEach(Module::stop); } - // GETTERS AND SETTERS - - private boolean trenchAssist = false; - private boolean trenchAlign = false; - - public boolean getTrenchAssist() { - return trenchAssist; - } - - public boolean getTrenchAlign() { - return trenchAlign; - } - - public void setTrenchAssist(boolean target) { - trenchAssist = target; - } - - public void setTrenchAlign(boolean target) { - trenchAlign = target; - } - // for current limit setting (brownout protection) public void applyNewModuleCurrents( double steerCurrentStator, double steerCurrentSupply,