diff --git a/.pathplanner/settings.json b/.pathplanner/settings.json index 818b77f..40a7b2a 100644 --- a/.pathplanner/settings.json +++ b/.pathplanner/settings.json @@ -12,7 +12,7 @@ "WIP", "working autos" ], - "defaultMaxVel": 4.0, + "defaultMaxVel": 4.5, "defaultMaxAccel": 4.0, "defaultMaxAngVel": 540.0, "defaultMaxAngAccel": 540.0, diff --git a/src/main/deploy/pathplanner/autos/Center 123.auto b/src/main/deploy/pathplanner/autos/Center 123.auto index 185f2c3..2b06e0b 100644 --- a/src/main/deploy/pathplanner/autos/Center 123.auto +++ b/src/main/deploy/pathplanner/autos/Center 123.auto @@ -66,15 +66,28 @@ "data": { "commands": [ { - "type": "named", + "type": "path", "data": { - "name": "Intake" + "pathName": "1 to 2" } }, { - "type": "path", + "type": "race", "data": { - "pathName": "1 to 2" + "commands": [ + { + "type": "named", + "data": { + "name": "Intake" + } + }, + { + "type": "wait", + "data": { + "waitTime": 3.0 + } + } + ] } } ] @@ -97,15 +110,28 @@ "data": { "commands": [ { - "type": "named", + "type": "path", "data": { - "name": "Intake" + "pathName": "2 to 3" } }, { - "type": "path", + "type": "race", "data": { - "pathName": "2 to 3" + "commands": [ + { + "type": "named", + "data": { + "name": "Intake" + } + }, + { + "type": "wait", + "data": { + "waitTime": 3.0 + } + } + ] } } ] diff --git a/src/main/deploy/pathplanner/autos/Source 54.auto b/src/main/deploy/pathplanner/autos/Source 54.auto index 9db61c8..6e26332 100644 --- a/src/main/deploy/pathplanner/autos/Source 54.auto +++ b/src/main/deploy/pathplanner/autos/Source 54.auto @@ -26,7 +26,7 @@ { "type": "named", "data": { - "name": "Sub Shoot Command" + "name": "Shoot Command" } }, { @@ -38,7 +38,7 @@ { "type": "named", "data": { - "name": "Shoot Command" + "name": "ShootAutoLow" } } ] diff --git a/src/main/deploy/pathplanner/paths/1 to 2.path b/src/main/deploy/pathplanner/paths/1 to 2.path index b8a9477..c87f5de 100644 --- a/src/main/deploy/pathplanner/paths/1 to 2.path +++ b/src/main/deploy/pathplanner/paths/1 to 2.path @@ -3,25 +3,25 @@ "waypoints": [ { "anchor": { - "x": 2.9066934025451485, - "y": 4.438407050851191 + "x": 2.8715608929078047, + "y": 4.344720358484939 }, "prevControl": null, "nextControl": { - "x": 1.5365255266887123, - "y": 4.8248646568619815 + "x": 1.5013930170513685, + "y": 4.73117796449573 }, "isLocked": false, "linkedName": "End Of Center 1" }, { "anchor": { - "x": 3.30486184510172, - "y": 5.9022616190738795 + "x": 3.2931510085559395, + "y": 5.714888234341376 }, "prevControl": { - "x": 1.0446703917658886, - "y": 5.796864090161845 + "x": 1.032959555220108, + "y": 5.609490705429342 }, "nextControl": null, "isLocked": false, @@ -32,7 +32,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.0, + "maxVelocity": 4.5, "maxAcceleration": 4.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 540.0 diff --git a/src/main/deploy/pathplanner/paths/2 to 3.path b/src/main/deploy/pathplanner/paths/2 to 3.path index 302cbe7..f7f25c8 100644 --- a/src/main/deploy/pathplanner/paths/2 to 3.path +++ b/src/main/deploy/pathplanner/paths/2 to 3.path @@ -3,24 +3,24 @@ "waypoints": [ { "anchor": { - "x": 3.30486184510172, - "y": 5.9022616190738795 + "x": 3.2931510085559395, + "y": 5.714888234341376 }, "prevControl": null, "nextControl": { - "x": 1.969826478882628, - "y": 5.995948311440131 + "x": 1.9581156423368475, + "y": 5.808574926707627 }, "isLocked": false, "linkedName": "End Of 1 to 2" }, { "anchor": { - "x": 3.5156569029257874, + "x": 3.4922352298342245, "y": 7.0733452736520315 }, "prevControl": { - "x": 2.0400914981573175, + "x": 2.0166698250657547, "y": 7.096766946743594 }, "nextControl": null, @@ -32,7 +32,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.0, + "maxVelocity": 4.5, "maxAcceleration": 4.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 540.0 diff --git a/src/main/deploy/pathplanner/paths/Center 1.path b/src/main/deploy/pathplanner/paths/Center 1.path index 7984ba9..af9a4cd 100644 --- a/src/main/deploy/pathplanner/paths/Center 1.path +++ b/src/main/deploy/pathplanner/paths/Center 1.path @@ -8,20 +8,20 @@ }, "prevControl": null, "nextControl": { - "x": 2.356284084893418, - "y": 5.562647359246215 + "x": 2.7310308543584263, + "y": 5.539225686154654 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 2.9066934025451485, - "y": 4.438407050851191 + "x": 2.8715608929078047, + "y": 4.344720358484939 }, "prevControl": { - "x": 0.4474177279310322, - "y": 4.450117887396973 + "x": 1.7473205845127804, + "y": 3.677202675375394 }, "nextControl": null, "isLocked": false, @@ -32,7 +32,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.0, + "maxVelocity": 4.5, "maxAcceleration": 4.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 540.0 diff --git a/src/main/deploy/pathplanner/paths/Center 123.path b/src/main/deploy/pathplanner/paths/Center 123.path index 13477b6..6d77cdd 100644 --- a/src/main/deploy/pathplanner/paths/Center 123.path +++ b/src/main/deploy/pathplanner/paths/Center 123.path @@ -198,7 +198,7 @@ } ], "globalConstraints": { - "maxVelocity": 4.0, + "maxVelocity": 4.5, "maxAcceleration": 4.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 540.0 diff --git a/src/main/deploy/pathplanner/paths/Note 3 to Note 8.path b/src/main/deploy/pathplanner/paths/Note 3 to Note 8.path index 0b90d30..241d0bf 100644 --- a/src/main/deploy/pathplanner/paths/Note 3 to Note 8.path +++ b/src/main/deploy/pathplanner/paths/Note 3 to Note 8.path @@ -48,7 +48,7 @@ "constraintZones": [], "eventMarkers": [], "globalConstraints": { - "maxVelocity": 4.0, + "maxVelocity": 4.5, "maxAcceleration": 4.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 540.0 diff --git a/src/main/deploy/pathplanner/paths/Source 4 original.path b/src/main/deploy/pathplanner/paths/Source 4 original.path deleted file mode 100644 index ffc15c7..0000000 --- a/src/main/deploy/pathplanner/paths/Source 4 original.path +++ /dev/null @@ -1,52 +0,0 @@ -{ - "version": 1.0, - "waypoints": [ - { - "anchor": { - "x": 0.8690078435791664, - "y": 4.215901156481343 - }, - "prevControl": null, - "nextControl": { - "x": 2.157199863615132, - "y": 4.133925300660873 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 3.152620970006561, - "y": 1.428722058585343 - }, - "prevControl": { - "x": 1.7824530941501249, - "y": 1.7097821356840972 - }, - "nextControl": null, - "isLocked": false, - "linkedName": null - } - ], - "rotationTargets": [], - "constraintZones": [], - "eventMarkers": [], - "globalConstraints": { - "maxVelocity": 4.0, - "maxAcceleration": 4.0, - "maxAngularVelocity": 540.0, - "maxAngularAcceleration": 540.0 - }, - "goalEndState": { - "velocity": 0, - "rotation": 178.83875809168597, - "rotateFast": false - }, - "reversed": false, - "folder": null, - "previewStartingState": { - "rotation": 125.0958167870262, - "velocity": 0 - }, - "useDefaultConstraints": true -} \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/Source 4.path b/src/main/deploy/pathplanner/paths/Source 4.path index 446ff91..352dd21 100644 --- a/src/main/deploy/pathplanner/paths/Source 4.path +++ b/src/main/deploy/pathplanner/paths/Source 4.path @@ -3,41 +3,41 @@ "waypoints": [ { "anchor": { - "x": 1.1852004303152672, - "y": 4.005106098657276 + "x": 4.101198730214863, + "y": 2.6466490593466205 }, "prevControl": null, "nextControl": { - "x": 2.1852004303152683, - "y": 4.005106098657276 + "x": 5.101198730214864, + "y": 2.6466490593466205 }, "isLocked": false, "linkedName": "End Of Source 5" }, { "anchor": { - "x": 9.441340195091232, - "y": 0.8548910678420494 + "x": 8.563027454157618, + "y": 1.0773969622118975 }, "prevControl": { - "x": 6.010065087177249, - "y": 0.4918551349228223 + "x": 5.131752346243635, + "y": 0.7143610292926704 }, "nextControl": { - "x": 9.674547874759517, - "y": 0.8795649179093423 + "x": 8.796235133825903, + "y": 1.1020708122791905 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 3.3282835181932833, - "y": 2.213348107152705 + "x": 2.976958421819838, + "y": 3.2087692135441324 }, "prevControl": { - "x": 4.593053865137684, - "y": 0.7846260485673611 + "x": 3.1877534796439044, + "y": 1.0305536160287714 }, "nextControl": null, "isLocked": false, @@ -72,18 +72,18 @@ } ], "globalConstraints": { - "maxVelocity": 4.0, + "maxVelocity": 4.5, "maxAcceleration": 4.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 540.0 }, "goalEndState": { "velocity": 0, - "rotation": 134.99999999999991, + "rotation": 137.7263109939063, "rotateFast": false }, "reversed": false, - "folder": "test", + "folder": null, "previewStartingState": { "rotation": 120.39872771416607, "velocity": 0 diff --git a/src/main/deploy/pathplanner/paths/Source 5.path b/src/main/deploy/pathplanner/paths/Source 5.path index 4f4805e..eec066b 100644 --- a/src/main/deploy/pathplanner/paths/Source 5.path +++ b/src/main/deploy/pathplanner/paths/Source 5.path @@ -16,64 +16,48 @@ }, { "anchor": { - "x": 3.69131945111251, - "y": 0.9837102698456457 - }, - "prevControl": { - "x": 2.911001336543219, - "y": 1.3694855174978797 - }, - "nextControl": { - "x": 4.733583903687064, - "y": 0.46843346183125933 - }, - "isLocked": false, - "linkedName": null - }, - { - "anchor": { - "x": 8.773822511981685, - "y": 3.1385041942694443 + "x": 3.984090364757048, + "y": 1.7917579915045703 }, "prevControl": { - "x": 8.75040083889012, - "y": 3.1502150308152257 + "x": 3.402229213978416, + "y": 2.4391809620892446 }, "nextControl": { - "x": 8.803448845414168, - "y": 3.123691027553203 + "x": 4.8155597595075355, + "y": 0.8666019043878308 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 5.7524266831700555, - "y": 1.7917579915045703 + "x": 8.375654069425114, + "y": 2.1547939244237972 }, "prevControl": { - "x": 6.068619269906157, - "y": 1.955709703145511 + "x": 8.35223239633355, + "y": 2.1665047609695787 }, "nextControl": { - "x": 5.468661601093226, - "y": 1.6446205415388075 + "x": 8.405280402857597, + "y": 2.139980757707556 }, "isLocked": false, "linkedName": null }, { "anchor": { - "x": 0.7753211512129142, - "y": 4.344720358484939 + "x": 4.101198730214863, + "y": 2.6466490593466205 }, "prevControl": { - "x": 2.0635131712488803, - "y": -0.36303593291922687 + "x": 5.143463182789417, + "y": 0.9485777602083012 }, "nextControl": null, "isLocked": false, - "linkedName": null + "linkedName": "End Of Source 5" } ], "rotationTargets": [ @@ -104,18 +88,18 @@ } ], "globalConstraints": { - "maxVelocity": 4.0, + "maxVelocity": 4.5, "maxAcceleration": 4.0, "maxAngularVelocity": 540.0, "maxAngularAcceleration": 540.0 }, "goalEndState": { "velocity": 0, - "rotation": 119.35775354279134, + "rotation": 150.2551187030578, "rotateFast": false }, "reversed": false, - "folder": "working", + "folder": null, "previewStartingState": { "rotation": 121.18497465873837, "velocity": 0 diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 521a945..6799124 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -16,10 +16,7 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics; import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj.DriverStation; import frc.robot.subsystems.ShamperSubsystem; -import frc.robot.util.calculator.ShootAngleConfig; -import frc.robot.util.calculator.ShootingAngleCalculator; public final class Constants { @@ -225,7 +222,7 @@ public static class Angle { public static final double MAXIMUM = 124; public static final double CLIMB = 13; public static final double DEFAULT = 30; - public static final double PODIUM = 34; + public static final double PODIUM = 35; public static final double AMP = 122; public static final double SUB = 53; public static final double TRAP = 95; @@ -350,6 +347,10 @@ public static final class AprilTagLocations { public static final double TAG16_ROTATION = 240; } + public static final class AprilTags { + public static final double APRILTAG_TIMEOUT = 1000; + } + public static final class Elevator{ public static final int BELT_MOTOR = 0; diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index beb063a..1040fea 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -84,6 +84,8 @@ public void teleopInit() { if (m_autonomousCommand != null) { m_autonomousCommand.cancel(); } + + RobotState.getInstance().applyNavxOffset(); // m_robotContainer.robotStatusCommunicator.onRobotTeleop(); } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6aa6e33..b34e3af 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -7,17 +7,17 @@ import frc.robot.auto.AutoFactory; import frc.robot.commands.climb.ClimberRetractCommand; import frc.robot.commands.climb.ClimberSlowRetractCommand; -import frc.robot.commands.GyroOffsetCommand; -import frc.robot.commands.auto.drive.AimToSpeakerCommand; -import frc.robot.commands.auto.drive.BasicAuto; -import frc.robot.commands.auto.shoot.ShootCommandAuto; -import frc.robot.commands.auto.shoot.ShootSubCommandAuto; +import frc.robot.commands.auto.commands.drive.AimToSpeakerCommand; +import frc.robot.commands.auto.commands.shoot.ShootAutoLowCommand; +import frc.robot.commands.auto.commands.shoot.ShootCommandAuto; +import frc.robot.commands.auto.commands.shoot.ShootSubCommandAuto; import frc.robot.commands.climb.ClimberExtendCommand; import frc.robot.commands.drive.DriveCommand; import frc.robot.commands.drive.DriveWhileAimingCommand; import frc.robot.commands.indexer.IndexerBackupCommand; import frc.robot.commands.indexer.IndexerIndexCommand; import frc.robot.commands.intake.IntakeCommand; +import frc.robot.commands.intake.IntakeThenBackupCommand; import frc.robot.commands.intake.OuttakeCommand; import frc.robot.commands.shamper.ShamperAmpCommand; import frc.robot.commands.shamper.ShamperAngleCommand; @@ -35,27 +35,17 @@ import frc.robot.subsystems.AprilTagSubsystem; import frc.robot.subsystems.ClimberSubsystem; import frc.robot.subsystems.AdvantageScopeSubsystem; -//import frc.robot.subsystems.ClimberSubsystem; import frc.robot.subsystems.DrivetrainSubsystem; import frc.robot.subsystems.IndexerSubsystem; -//import frc.robot.subsystems.MusicPlayerSubsystem; import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.LedSubsystem; import frc.robot.subsystems.ShamperSubsystem; import frc.robot.subsystems.TrapArmSubsystem; -// import frc.robot.subsystems.Superstructure; -//import frc.robot.subsystems.TrapArmSubsystem; import frc.robot.subsystems.VisionSubsystem; import frc.robot.subsystems.ShamperSubsystem.ShamperSpeed; -// import frc.robot.subsystems.Superstructure.SuperstructureState; -//import frc.robot.util.RobotStatusCommunicator; import frc.robot.util.io.Dashboard; import com.pathplanner.lib.auto.NamedCommands; -import com.pathplanner.lib.commands.PathPlannerAuto; -import edu.wpi.first.networktables.GenericEntry; import edu.wpi.first.wpilibj.Joystick; -import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; -import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; @@ -75,8 +65,6 @@ public class RobotContainer { private final AdvantageScopeSubsystem advantageScope; private final TrapArmSubsystem trapArm; - //private final Superstructure superstructure; - private final AutoFactory autoFactory; private final Joystick translationJoystick; @@ -84,7 +72,7 @@ public class RobotContainer { private final Joystick controlPanel; public static boolean musicOn; - //public RobotStatusCommunicator robotStatusCommunicator; + // public RobotStatusCommunicator robotStatusCommunicator; /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { drivetrain = new DrivetrainSubsystem(); @@ -99,9 +87,11 @@ public RobotContainer() { trapArm = new TrapArmSubsystem(); advantageScope = new AdvantageScopeSubsystem(intake, shamper, climber, drivetrain, indexer); + // robotStatusCommunicator = new RobotStatusCommunicator(musicPlayer); musicOn = true; + ledSubsystem.enableLEDs(); autoFactory = new AutoFactory(() -> Dashboard.getInstance().getAuto()); @@ -132,17 +122,9 @@ public RobotContainer() { NamedCommands.registerCommand("Manual Angle", new ShamperAngleCommand(shamper, Constants.Shamper.Angle.SUB)); NamedCommands.registerCommand("Manual Shoot", new ShamperManualShootCommand(shamper, ShamperSpeed.SPEAKER_IDLE)); NamedCommands.registerCommand("Manual Index", new IndexerIndexCommand(indexer)); - // NamedCommands.registerCommand("Gyro Offset", new GyroOffsetCommand()); + NamedCommands.registerCommand("ShootAutoLow", new ShootAutoLowCommand(shamper, indexer)); NamedCommands.registerCommand("Intake", new IntakeCommand(intake, indexer, shamper)); - // NamedCommands.registerCommand("Outtake", new OuttakeCommand(intake, indexer)); - // NamedCommands.registerCommand("Default Superstructure", new InstantCommand(() ->superstructure.setState(SuperstructureState.DEFAULT))); - // NamedCommands.registerCommand("Speaker Idle Superstructure", new InstantCommand(() ->superstructure.setState(SuperstructureState.SPEAKER_IDLE))); - // NamedCommands.registerCommand("Speaker Score Superstructure", new InstantCommand(() ->superstructure.setState(SuperstructureState.SPEAKER_SCORE))); - // NamedCommands.registerCommand("Amp Idle Superstructure", new InstantCommand(() ->superstructure.setState(SuperstructureState.AMP_IDLE))); - // NamedCommands.registerCommand("Amp Score Superstructure", new InstantCommand(() ->superstructure.setState(SuperstructureState.AMP_SCORE))); - - //advantageScope.startRecording(); configureButtonBindings(); } @@ -154,7 +136,7 @@ private void configureButtonBindings() { */ JoystickButton zeroGyroButton = new JoystickButton(translationJoystick, 9); - zeroGyroButton.onTrue(new InstantCommand(() -> drivetrain.zeroGyro())); + zeroGyroButton.onTrue(new InstantCommand(() -> {drivetrain.zeroGyro(); RobotState.getInstance().clearNavXOffset();})); JoystickButton driveWhileAimingButton = new JoystickButton(rotationJoystick, 2); driveWhileAimingButton.whileTrue(new DriveWhileAimingCommand( @@ -182,7 +164,7 @@ private void configureButtonBindings() { JoystickButton intakeInButton = new JoystickButton(translationJoystick, 1); JoystickButton outtakeButton = new JoystickButton(translationJoystick, 3); - intakeInButton.whileTrue(new IntakeCommand(intake, indexer, shamper)); + intakeInButton.whileTrue(new IntakeThenBackupCommand(intake, indexer, shamper)); outtakeButton.whileTrue(new OuttakeCommand(intake, indexer, shamper)); /* @@ -245,37 +227,6 @@ private void configureButtonBindings() { JoystickButton trapReleaseButton = new JoystickButton(controlPanel, 3); trapReleaseButton.whileTrue(new TrapReleaseCommand(trapArm)); - - /* - * Superstructure Position Button Bindings - */ - - // JoystickButton shamperShootButton = new JoystickButton(rotationJoystick, 1); - // JoystickButton shamperAmpIdleButton = new JoystickButton(rotationJoystick, 4); - // JoystickButton superstructureIntakeButton = new JoystickButton(rotationJoystick, 2); - // // JoystickButton shamperSpeakerIdleAimButon = new JoystickButton(rotationJoystick, 3); - // JoystickButton shamperPodiumIdleButton = new JoystickButton(rotationJoystick, 3); - // JoystickButton shamperDefaultButton = new JoystickButton(rotationJoystick, 5); - - // shamperAmpIdleButton.onTrue(new InstantCommand(() -> superstructure.setState(SuperstructureState.AMP_IDLE))); - // // shamperSpeakerIdleAimButon.onTrue(new InstantCommand(() -> superstructure.setState(SuperstructureState.SPEAKER_IDLE))); - // superstructureIntakeButton.onTrue(new InstantCommand(() -> superstructure.setState(SuperstructureState.INTAKE))); - // shamperPodiumIdleButton.onTrue(new InstantCommand(() -> superstructure.setState(SuperstructureState.PODIUM_IDLE))); - // shamperDefaultButton.onTrue(new InstantCommand(() -> superstructure.setState(SuperstructureState.DEFAULT))); - - // shamperShootButton.onTrue(new InstantCommand(() -> { - // if(superstructure.getState() == SuperstructureState.AMP_IDLE) { - // superstructure.setState(SuperstructureState.AMP_SCORE); - // // } else if (superstructure.getState() == SuperstructureState.SPEAKER_IDLE) { - // // superstructure.setState(SuperstructureState.SPEAKER_SCORE); - // } else if (superstructure.getState() == SuperstructureState.PODIUM_IDLE) { - // superstructure.setState(SuperstructureState.PODIUM_SCORE); - // } else { - // System.out.println("WASN'T IDLING*********"); - // } - // })).onFalse(new InstantCommand(() -> superstructure.setState(SuperstructureState.DEFAULT))); - - // shamperDefaultButton.onTrue(new InstantCommand(() -> superstructure.setState(SuperstructureState.DEFAULT))); /* * Music Player Toggle @@ -303,8 +254,6 @@ public void precompileAuto() { } public Command getAutonomousCommand() { return autoFactory.getCompiledAuto(); - //return new PathPlannerAuto("Amp 3"); - // return new BasicAuto(drivetrain, shamper, indexer); } public void resetGyro(){ diff --git a/src/main/java/frc/robot/RobotState.java b/src/main/java/frc/robot/RobotState.java index 7cd1c91..87319ad 100644 --- a/src/main/java/frc/robot/RobotState.java +++ b/src/main/java/frc/robot/RobotState.java @@ -13,7 +13,6 @@ import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; -// import frc.robot.subsystems.Superstructure.SuperstructureState; import frc.robot.util.AimingCalculator; import frc.robot.util.io.Dashboard; import frc.robot.util.states.DrivetrainState; @@ -33,8 +32,7 @@ public class RobotState { private boolean musicEnabled; private boolean isClimbing; private boolean shamperAtGoalAngle; - - // private SuperstructureState superstructureState; + private double autoOffset; public static RobotState getInstance() { if (INSTANCE == null) { @@ -114,7 +112,8 @@ public boolean getIsShamperAtGoalAngle() { */ public void resetInitialPose(Pose2d initialStartingPose) { navxOffset = new Rotation2d(); - // navxOffset = initialStartingPose.getRotation(); + autoOffset = - initialStartingPose.getRotation().getRadians() + Math.PI; + System.out.println("auto offset of :" + Units.radiansToDegrees(autoOffset)); initialPose = initialStartingPose; } @@ -160,6 +159,15 @@ public Rotation2d getRotation2dRaw() { return robotRotation2d.rotateBy(navxOffset); } + public void applyNavxOffset(){ + System.out.println("NavX Offset Applied"); + this.navxOffset = new Rotation2d(autoOffset); + } + + public void clearNavXOffset() { + this.navxOffset = new Rotation2d(); + } + public void addNavXOffset(double offset){ this.navxOffset = new Rotation2d(Units.degreesToRadians(offset)); } diff --git a/src/main/java/frc/robot/auto/AutoFactory.java b/src/main/java/frc/robot/auto/AutoFactory.java index 1cae36c..189ef39 100644 --- a/src/main/java/frc/robot/auto/AutoFactory.java +++ b/src/main/java/frc/robot/auto/AutoFactory.java @@ -9,7 +9,7 @@ import edu.wpi.first.wpilibj2.command.Command; import frc.robot.Constants; -import frc.robot.commands.auto.drive.BasicAuto; +import frc.robot.commands.auto.backupAuto.BasicAuto; import frc.robot.subsystems.DrivetrainSubsystem; import frc.robot.subsystems.ShamperSubsystem; import frc.robot.util.io.Dashboard; diff --git a/src/main/java/frc/robot/commands/auto/drive/BasicAuto.java b/src/main/java/frc/robot/commands/auto/backupAuto/BasicAuto.java similarity index 70% rename from src/main/java/frc/robot/commands/auto/drive/BasicAuto.java rename to src/main/java/frc/robot/commands/auto/backupAuto/BasicAuto.java index 75e5bf1..fac353a 100644 --- a/src/main/java/frc/robot/commands/auto/drive/BasicAuto.java +++ b/src/main/java/frc/robot/commands/auto/backupAuto/BasicAuto.java @@ -1,11 +1,11 @@ -package frc.robot.commands.auto.drive; +package frc.robot.commands.auto.backupAuto; -import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.robot.Constants; -import frc.robot.commands.GyroOffsetCommand; +import frc.robot.commands.auto.commands.GyroOffsetCommand; +import frc.robot.commands.auto.commands.drive.AutoDriveCommand; import frc.robot.commands.indexer.IndexerIndexCommand; import frc.robot.commands.shamper.ShamperAngleCommand; import frc.robot.commands.shamper.ShamperManualShootCommand; @@ -13,21 +13,10 @@ import frc.robot.subsystems.IndexerSubsystem; import frc.robot.subsystems.ShamperSubsystem; import frc.robot.subsystems.ShamperSubsystem.ShamperSpeed; -import frc.robot.util.calculator.ShootingAngleCalculator; public class BasicAuto extends SequentialCommandGroup { - private String pointOnPodium; - private DrivetrainSubsystem drivetrain; - private ShamperSubsystem shamper; - private IndexerSubsystem indexer; - - public BasicAuto (DrivetrainSubsystem drivetrain, ShamperSubsystem shamper, IndexerSubsystem indexer) { - this.pointOnPodium = pointOnPodium; - this.drivetrain = drivetrain; - this.shamper = shamper; - this.indexer = indexer; addCommands(new ParallelDeadlineGroup(new WaitCommand(1.5), new ShamperAngleCommand(shamper, Constants.Shamper.Angle.SUB)), new ParallelDeadlineGroup(new WaitCommand(2.5), new ShamperManualShootCommand(shamper, ShamperSpeed.SPEAKER_IDLE)), new ParallelDeadlineGroup(new WaitCommand(1), new IndexerIndexCommand(indexer)), @@ -35,12 +24,4 @@ public BasicAuto (DrivetrainSubsystem drivetrain, ShamperSubsystem shamper, Inde new GyroOffsetCommand(180); } - - - - - - - - } diff --git a/src/main/java/frc/robot/commands/GyroOffsetCommand.java b/src/main/java/frc/robot/commands/auto/commands/GyroOffsetCommand.java similarity index 74% rename from src/main/java/frc/robot/commands/GyroOffsetCommand.java rename to src/main/java/frc/robot/commands/auto/commands/GyroOffsetCommand.java index 9d9809e..106fb7b 100644 --- a/src/main/java/frc/robot/commands/GyroOffsetCommand.java +++ b/src/main/java/frc/robot/commands/auto/commands/GyroOffsetCommand.java @@ -2,14 +2,10 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. -package frc.robot.commands; +package frc.robot.commands.auto.commands; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.RobotState; -import frc.robot.util.RobotStateEstimator; public class GyroOffsetCommand extends Command { private double offset; @@ -27,7 +23,6 @@ public void initialize() {} @Override public void execute() { RobotState.getInstance().addNavXOffset(offset); - //RobotStateEstimator.getInstance().resetOdometry(new Pose2d(1.33, 7.026501927468906, new Rotation2d(Units.degreesToRadians(-126.78843142290575)))); } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/commands/auto/drive/AimToSpeakerCommand.java b/src/main/java/frc/robot/commands/auto/commands/drive/AimToSpeakerCommand.java similarity index 97% rename from src/main/java/frc/robot/commands/auto/drive/AimToSpeakerCommand.java rename to src/main/java/frc/robot/commands/auto/commands/drive/AimToSpeakerCommand.java index b0a0dc1..8d36699 100644 --- a/src/main/java/frc/robot/commands/auto/drive/AimToSpeakerCommand.java +++ b/src/main/java/frc/robot/commands/auto/commands/drive/AimToSpeakerCommand.java @@ -2,7 +2,7 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. -package frc.robot.commands.auto.drive; +package frc.robot.commands.auto.commands.drive; import org.littletonrobotics.junction.Logger; import edu.wpi.first.wpilibj2.command.Command; diff --git a/src/main/java/frc/robot/commands/auto/commands/drive/AimToSpeakerUsingOneAprilTagCommand.java b/src/main/java/frc/robot/commands/auto/commands/drive/AimToSpeakerUsingOneAprilTagCommand.java new file mode 100644 index 0000000..bb773e8 --- /dev/null +++ b/src/main/java/frc/robot/commands/auto/commands/drive/AimToSpeakerUsingOneAprilTagCommand.java @@ -0,0 +1,60 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands.auto.commands.drive; + +import org.littletonrobotics.junction.Logger; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.RobotState; +import frc.robot.subsystems.DrivetrainSubsystem; +import frc.robot.util.AimingCalculator; + +public class AimToSpeakerUsingOneAprilTagCommand extends Command { + protected final DrivetrainSubsystem drivetrain; + private double goalAngleDegrees; + private double deltaDegrees; + private boolean isFinished = false; + + public AimToSpeakerUsingOneAprilTagCommand(DrivetrainSubsystem drivetrain) { + this.drivetrain = drivetrain; + isFinished = false; + + addRequirements(drivetrain); + } + + private double getRotation() { + isFinished = false; + goalAngleDegrees = AimingCalculator.calculateRobotAngleOneAprilTag(); + deltaDegrees = RobotState.getInstance().getRotation2d360().getDegrees() - goalAngleDegrees; + Logger.recordOutput("goal angle", goalAngleDegrees); + Logger.recordOutput("measured angle", RobotState.getInstance().getRotation2d360().getDegrees()); + + // return 0; + if (Math.abs(deltaDegrees) > 90) { + return Math.copySign(0.5, -deltaDegrees); + } else if (Math.abs(deltaDegrees) > 45){ + return Math.copySign(0.25, -deltaDegrees); + } else if (Math.abs(deltaDegrees) > 5){ + return Math.copySign(0.1, -deltaDegrees); + } else { + isFinished = true; + return 0; + } + } + + @Override + public void execute() { + drivetrain.drive(0, 0, getRotation(), true); + } + + @Override + public void end(boolean interrupted) { + drivetrain.stop(); + } + + @Override + public boolean isFinished() { + return isFinished; + } +} diff --git a/src/main/java/frc/robot/commands/auto/drive/AutoDriveCommand.java b/src/main/java/frc/robot/commands/auto/commands/drive/AutoDriveCommand.java similarity index 95% rename from src/main/java/frc/robot/commands/auto/drive/AutoDriveCommand.java rename to src/main/java/frc/robot/commands/auto/commands/drive/AutoDriveCommand.java index 5a3ecd0..ef404a2 100644 --- a/src/main/java/frc/robot/commands/auto/drive/AutoDriveCommand.java +++ b/src/main/java/frc/robot/commands/auto/commands/drive/AutoDriveCommand.java @@ -1,4 +1,4 @@ -package frc.robot.commands.auto.drive; +package frc.robot.commands.auto.commands.drive; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; diff --git a/src/main/java/frc/robot/commands/auto/commands/shoot/ShootAutoLowCommand.java b/src/main/java/frc/robot/commands/auto/commands/shoot/ShootAutoLowCommand.java new file mode 100644 index 0000000..b5d5890 --- /dev/null +++ b/src/main/java/frc/robot/commands/auto/commands/shoot/ShootAutoLowCommand.java @@ -0,0 +1,23 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands.auto.commands.shoot; + +import frc.robot.subsystems.IndexerSubsystem; +import frc.robot.subsystems.ShamperSubsystem; +import frc.robot.util.calculator.ShootAngleConfig; + +public class ShootAutoLowCommand extends ShootCommandAuto { + /** Creates a new ShootAutoLowCommand. */ + public ShootAutoLowCommand(ShamperSubsystem shamper, IndexerSubsystem indexer) { + super(shamper, indexer); + // Use addRequirements() here to declare subsystem dependencies. + } + + @Override + protected ShootAngleConfig getTargetAngle(){ + ShootAngleConfig config = super.getTargetAngle(); + return new ShootAngleConfig(config.getDistanceCentimeters(), config.getAngleDegrees() - 3, config.getShooterSpeedVelocityRPS()); + } +} diff --git a/src/main/java/frc/robot/commands/auto/shoot/ShootCommandAuto.java b/src/main/java/frc/robot/commands/auto/commands/shoot/ShootCommandAuto.java similarity index 82% rename from src/main/java/frc/robot/commands/auto/shoot/ShootCommandAuto.java rename to src/main/java/frc/robot/commands/auto/commands/shoot/ShootCommandAuto.java index 0b6783f..a3b5553 100644 --- a/src/main/java/frc/robot/commands/auto/shoot/ShootCommandAuto.java +++ b/src/main/java/frc/robot/commands/auto/commands/shoot/ShootCommandAuto.java @@ -2,16 +2,12 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. -package frc.robot.commands.auto.shoot; +package frc.robot.commands.auto.commands.shoot; import org.littletonrobotics.junction.Logger; -import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import frc.robot.Constants.Shamper; import frc.robot.RobotState; import frc.robot.subsystems.IndexerSubsystem; import frc.robot.subsystems.ShamperSubsystem; @@ -23,6 +19,7 @@ public class ShootCommandAuto extends Command { private final ShamperSubsystem shamper; private final IndexerSubsystem indexer; private double indexStartTime = 0; + private ShootAngleConfig config; public ShootCommandAuto(ShamperSubsystem shamper, IndexerSubsystem indexer) { this.shamper = shamper; @@ -35,9 +32,13 @@ public void initialize() { indexStartTime = 0; } + protected ShootAngleConfig getTargetAngle(){ + return ShootingAngleCalculator.getInstance().getShooterConfig(AimingCalculator.calculateDistanceToSpeaker(RobotState.getInstance().getRobotPose())); + } + @Override public void execute() { - ShootAngleConfig config = ShootingAngleCalculator.getInstance().getShooterConfig(AimingCalculator.calculateDistanceToSpeaker(RobotState.getInstance().getRobotPose())); + config = getTargetAngle(); Logger.recordOutput("shoot speed calculated", config.getShooterSpeedVelocityRPS()); Logger.recordOutput("angle calculated", config.getAngleDegrees()); shamper.setShootSpeed(config.getShooterSpeedVelocityRPS(), config.getShooterSpeedVelocityRPS()); diff --git a/src/main/java/frc/robot/commands/auto/shoot/ShootSubCommandAuto.java b/src/main/java/frc/robot/commands/auto/commands/shoot/ShootSubCommandAuto.java similarity index 76% rename from src/main/java/frc/robot/commands/auto/shoot/ShootSubCommandAuto.java rename to src/main/java/frc/robot/commands/auto/commands/shoot/ShootSubCommandAuto.java index dbb2b2d..4ccc99e 100644 --- a/src/main/java/frc/robot/commands/auto/shoot/ShootSubCommandAuto.java +++ b/src/main/java/frc/robot/commands/auto/commands/shoot/ShootSubCommandAuto.java @@ -2,24 +2,14 @@ // Open Source Software; you can modify and/or share it under the terms of // the WPILib BSD license file in the root directory of this project. -package frc.robot.commands.auto.shoot; +package frc.robot.commands.auto.commands.shoot; -import org.littletonrobotics.junction.Logger; - -import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import frc.robot.Constants.Shamper; import frc.robot.Constants; -import frc.robot.RobotState; import frc.robot.subsystems.IndexerSubsystem; import frc.robot.subsystems.ShamperSubsystem; import frc.robot.subsystems.ShamperSubsystem.ShamperSpeed; -import frc.robot.util.AimingCalculator; -import frc.robot.util.calculator.ShootAngleConfig; -import frc.robot.util.calculator.ShootingAngleCalculator; public class ShootSubCommandAuto extends Command { private final ShamperSubsystem shamper; diff --git a/src/main/java/frc/robot/commands/drive/DriveWhileAimingCommand.java b/src/main/java/frc/robot/commands/drive/DriveWhileAimingCommand.java index e5a6e67..172100f 100644 --- a/src/main/java/frc/robot/commands/drive/DriveWhileAimingCommand.java +++ b/src/main/java/frc/robot/commands/drive/DriveWhileAimingCommand.java @@ -10,7 +10,6 @@ import org.littletonrobotics.junction.Logger; import edu.wpi.first.math.filter.SlewRateLimiter; -import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.wpilibj2.command.Command; import frc.robot.RobotState; import frc.robot.subsystems.DrivetrainSubsystem; diff --git a/src/main/java/frc/robot/commands/indexer/IndexerBackupCommand.java b/src/main/java/frc/robot/commands/indexer/IndexerBackupCommand.java index 5059983..3832c88 100644 --- a/src/main/java/frc/robot/commands/indexer/IndexerBackupCommand.java +++ b/src/main/java/frc/robot/commands/indexer/IndexerBackupCommand.java @@ -32,7 +32,7 @@ public void initialize() { @Override public void execute() { System.out.println(runBackTimer.get()); - if(runBackTimer.get() < 0.1){ + if(runBackTimer.get() < 0.125){ indexer.slideBack(); } else { isFinished = true; diff --git a/src/main/java/frc/robot/commands/intake/IntakeThenBackupCommand.java b/src/main/java/frc/robot/commands/intake/IntakeThenBackupCommand.java index 6549053..37af2fb 100644 --- a/src/main/java/frc/robot/commands/intake/IntakeThenBackupCommand.java +++ b/src/main/java/frc/robot/commands/intake/IntakeThenBackupCommand.java @@ -12,20 +12,9 @@ import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.ShamperSubsystem; -// NOTE: Consider using this command inline, rather than writing a subclass. For more -// information, see: -// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html public class IntakeThenBackupCommand extends SequentialCommandGroup { - private IndexerSubsystem indexer; - private IntakeSubsystem intake; - private ShamperSubsystem shamper; /** Creates a new IntakeThenBackupCommand. */ public IntakeThenBackupCommand(IntakeSubsystem intake, IndexerSubsystem indexer, ShamperSubsystem shamper) { - this.indexer = indexer; - this.intake = intake; - this.shamper = shamper; - - addCommands( new IntakeCommand(intake, indexer, shamper), new ParallelDeadlineGroup( diff --git a/src/main/java/frc/robot/commands/music/PauseMusicPlayerCommand.java b/src/main/java/frc/robot/commands/music/PauseMusicPlayerCommand.java index 1a9adb6..74741c8 100644 --- a/src/main/java/frc/robot/commands/music/PauseMusicPlayerCommand.java +++ b/src/main/java/frc/robot/commands/music/PauseMusicPlayerCommand.java @@ -1,24 +1,24 @@ -// package frc.robot.commands.music; +package frc.robot.commands.music; -// import edu.wpi.first.wpilibj2.command.Command; -// import frc.robot.subsystems.MusicPlayerSubsystem; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.MusicPlayerSubsystem; -// public class PauseMusicPlayerCommand extends Command { -// private final MusicPlayerSubsystem player; +public class PauseMusicPlayerCommand extends Command { + private final MusicPlayerSubsystem player; -// public PauseMusicPlayerCommand(MusicPlayerSubsystem player) { -// this.player = player; -// addRequirements(player); -// } + public PauseMusicPlayerCommand(MusicPlayerSubsystem player) { + this.player = player; + addRequirements(player); + } -// @Override -// public void initialize() { -// player.pauseMusic(); -// } + @Override + public void initialize() { + player.pauseMusic(); + } -// @Override -// public void end(boolean interrupted) { -// player.playLoadedTrack(); -// } + @Override + public void end(boolean interrupted) { + player.playLoadedTrack(); + } -// } +} diff --git a/src/main/java/frc/robot/commands/music/PlayActivationJingleCommand.java b/src/main/java/frc/robot/commands/music/PlayActivationJingleCommand.java index aa3b478..98d98c7 100644 --- a/src/main/java/frc/robot/commands/music/PlayActivationJingleCommand.java +++ b/src/main/java/frc/robot/commands/music/PlayActivationJingleCommand.java @@ -1,26 +1,26 @@ -// package frc.robot.commands.music; +package frc.robot.commands.music; -// import edu.wpi.first.wpilibj2.command.Command; -// import frc.robot.subsystems.MusicPlayerSubsystem; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.MusicPlayerSubsystem; -// public class PlayActivationJingleCommand extends Command{ -// private final MusicPlayerSubsystem player; +public class PlayActivationJingleCommand extends Command{ + private final MusicPlayerSubsystem player; -// public PlayActivationJingleCommand(MusicPlayerSubsystem player) { -// this.player = player; -// addRequirements(player); -// } + public PlayActivationJingleCommand(MusicPlayerSubsystem player) { + this.player = player; + addRequirements(player); + } -// @Override -// public void initialize() { -// player.stopMusic(); -// player.loadMusic("ActivationJingle.chrp"); -// player.playLoadedTrack(); -// } + @Override + public void initialize() { + player.stopMusic(); + player.loadMusic("ActivationJingle.chrp"); + player.playLoadedTrack(); + } -// @Override -// public void end(boolean interrupted) { -// player.stopMusic(); -// } + @Override + public void end(boolean interrupted) { + player.stopMusic(); + } -// } +} diff --git a/src/main/java/frc/robot/commands/music/PlayAnthemCommand.java b/src/main/java/frc/robot/commands/music/PlayAnthemCommand.java index 3c7f567..543143d 100644 --- a/src/main/java/frc/robot/commands/music/PlayAnthemCommand.java +++ b/src/main/java/frc/robot/commands/music/PlayAnthemCommand.java @@ -1,26 +1,26 @@ -// package frc.robot.commands.music; +package frc.robot.commands.music; -// import edu.wpi.first.wpilibj2.command.Command; -// import frc.robot.subsystems.MusicPlayerSubsystem; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.MusicPlayerSubsystem; -// public class PlayAnthemCommand extends Command{ -// private final MusicPlayerSubsystem player; +public class PlayAnthemCommand extends Command{ + private final MusicPlayerSubsystem player; -// public PlayAnthemCommand(MusicPlayerSubsystem player) { -// this.player = player; -// addRequirements(player); -// } + public PlayAnthemCommand(MusicPlayerSubsystem player) { + this.player = player; + addRequirements(player); + } -// @Override -// public void initialize() { -// player.stopMusic(); -// player.loadMusic("NationalAnthem.chrp"); -// player.playLoadedTrack(); -// } + @Override + public void initialize() { + player.stopMusic(); + player.loadMusic("NationalAnthem.chrp"); + player.playLoadedTrack(); + } -// @Override -// public void end(boolean interrupted) { -// player.stopMusic(); -// } + @Override + public void end(boolean interrupted) { + player.stopMusic(); + } -// } +} diff --git a/src/main/java/frc/robot/commands/music/PlayDOTFCommand.java b/src/main/java/frc/robot/commands/music/PlayDOTFCommand.java index 8b3b4f5..840122b 100644 --- a/src/main/java/frc/robot/commands/music/PlayDOTFCommand.java +++ b/src/main/java/frc/robot/commands/music/PlayDOTFCommand.java @@ -1,26 +1,26 @@ -// package frc.robot.commands.music; +package frc.robot.commands.music; -// import edu.wpi.first.wpilibj2.command.Command; -// import frc.robot.subsystems.MusicPlayerSubsystem; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.MusicPlayerSubsystem; -// public class PlayDOTFCommand extends Command{ -// private final MusicPlayerSubsystem player; +public class PlayDOTFCommand extends Command{ + private final MusicPlayerSubsystem player; -// public PlayDOTFCommand(MusicPlayerSubsystem player) { -// this.player = player; -// addRequirements(player); -// } + public PlayDOTFCommand(MusicPlayerSubsystem player) { + this.player = player; + addRequirements(player); + } -// @Override -// public void initialize() { -// player.stopMusic(); -// player.loadMusic("DuelOfTheFates.chrp"); -// player.playLoadedTrack(); -// } + @Override + public void initialize() { + player.stopMusic(); + player.loadMusic("DuelOfTheFates.chrp"); + player.playLoadedTrack(); + } -// @Override -// public void end(boolean interrupted) { -// player.stopMusic(); -// } + @Override + public void end(boolean interrupted) { + player.stopMusic(); + } -// } +} diff --git a/src/main/java/frc/robot/commands/music/PlayFOTBCommand.java b/src/main/java/frc/robot/commands/music/PlayFOTBCommand.java index 3cb53cd..62de3f3 100644 --- a/src/main/java/frc/robot/commands/music/PlayFOTBCommand.java +++ b/src/main/java/frc/robot/commands/music/PlayFOTBCommand.java @@ -1,26 +1,26 @@ -// package frc.robot.commands.music; +package frc.robot.commands.music; -// import edu.wpi.first.wpilibj2.command.Command; -// import frc.robot.subsystems.MusicPlayerSubsystem; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.MusicPlayerSubsystem; -// public class PlayFOTBCommand extends Command{ -// private final MusicPlayerSubsystem player; +public class PlayFOTBCommand extends Command{ + private final MusicPlayerSubsystem player; -// public PlayFOTBCommand(MusicPlayerSubsystem player) { -// this.player = player; -// addRequirements(player); -// } + public PlayFOTBCommand(MusicPlayerSubsystem player) { + this.player = player; + addRequirements(player); + } -// @Override -// public void initialize() { -// player.stopMusic(); -// player.loadMusic("FlightOfTheBumblebees.chrp"); -// player.playLoadedTrack(); -// } + @Override + public void initialize() { + player.stopMusic(); + player.loadMusic("FlightOfTheBumblebees.chrp"); + player.playLoadedTrack(); + } -// @Override -// public void end(boolean interrupted) { -// player.stopMusic(); -// } + @Override + public void end(boolean interrupted) { + player.stopMusic(); + } -// } +} diff --git a/src/main/java/frc/robot/commands/music/PlayTeleopJingleCommand.java b/src/main/java/frc/robot/commands/music/PlayTeleopJingleCommand.java index 0423a74..793cc75 100644 --- a/src/main/java/frc/robot/commands/music/PlayTeleopJingleCommand.java +++ b/src/main/java/frc/robot/commands/music/PlayTeleopJingleCommand.java @@ -1,26 +1,26 @@ -// package frc.robot.commands.music; +package frc.robot.commands.music; -// import edu.wpi.first.wpilibj2.command.Command; -// import frc.robot.subsystems.MusicPlayerSubsystem; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.MusicPlayerSubsystem; -// public class PlayTeleopJingleCommand extends Command{ -// private final MusicPlayerSubsystem player; +public class PlayTeleopJingleCommand extends Command{ + private final MusicPlayerSubsystem player; -// public PlayTeleopJingleCommand(MusicPlayerSubsystem player) { -// this.player = player; -// addRequirements(player); -// } + public PlayTeleopJingleCommand(MusicPlayerSubsystem player) { + this.player = player; + addRequirements(player); + } -// @Override -// public void initialize() { -// player.stopMusic(); -// player.loadMusic("TeleopJingle.chrp"); -// player.playLoadedTrack(); -// } + @Override + public void initialize() { + player.stopMusic(); + player.loadMusic("TeleopJingle.chrp"); + player.playLoadedTrack(); + } -// @Override -// public void end(boolean interrupted) { -// player.stopMusic(); -// } + @Override + public void end(boolean interrupted) { + player.stopMusic(); + } -// } +} diff --git a/src/main/java/frc/robot/commands/music/PlayTwentySecondsLeftCommand.java b/src/main/java/frc/robot/commands/music/PlayTwentySecondsLeftCommand.java index a8c4f1c..7b37a87 100644 --- a/src/main/java/frc/robot/commands/music/PlayTwentySecondsLeftCommand.java +++ b/src/main/java/frc/robot/commands/music/PlayTwentySecondsLeftCommand.java @@ -1,26 +1,26 @@ -// package frc.robot.commands.music; +package frc.robot.commands.music; -// import edu.wpi.first.wpilibj2.command.Command; -// import frc.robot.subsystems.MusicPlayerSubsystem; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.MusicPlayerSubsystem; -// public class PlayTwentySecondsLeftCommand extends Command{ -// private final MusicPlayerSubsystem player; +public class PlayTwentySecondsLeftCommand extends Command{ + private final MusicPlayerSubsystem player; -// public PlayTwentySecondsLeftCommand(MusicPlayerSubsystem player) { -// this.player = player; -// addRequirements(player); -// } + public PlayTwentySecondsLeftCommand(MusicPlayerSubsystem player) { + this.player = player; + addRequirements(player); + } -// @Override -// public void initialize() { -// player.stopMusic(); -// player.loadMusic("TwentySecondsLeft.chrp"); -// player.playLoadedTrack(); -// } + @Override + public void initialize() { + player.stopMusic(); + player.loadMusic("TwentySecondsLeft.chrp"); + player.playLoadedTrack(); + } -// @Override -// public void end(boolean interrupted) { -// player.stopMusic(); -// } + @Override + public void end(boolean interrupted) { + player.stopMusic(); + } -// } +} diff --git a/src/main/java/frc/robot/commands/shamper/ShamperManualShootCommand.java b/src/main/java/frc/robot/commands/shamper/ShamperManualShootCommand.java index 943e550..dadef21 100644 --- a/src/main/java/frc/robot/commands/shamper/ShamperManualShootCommand.java +++ b/src/main/java/frc/robot/commands/shamper/ShamperManualShootCommand.java @@ -1,11 +1,8 @@ package frc.robot.commands.shamper; import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.Constants; -import frc.robot.RobotState; import frc.robot.subsystems.ShamperSubsystem; import frc.robot.subsystems.ShamperSubsystem.ShamperSpeed; -import frc.robot.util.calculator.ShootingAngleCalculator; public class ShamperManualShootCommand extends Command{ private final ShamperSubsystem shamper; @@ -35,13 +32,10 @@ public void initialize() { @Override public void execute(){ - - //shamper.setAngle(ShootingAngleCalculator.getInstance().getShooterConfig(RobotState.getInstance().getRobotPose()).getAngleDegrees()); } @Override public void end(boolean interrupted) { shamper.stopShooter(); - //shamper.setAngle(Constants.Shamper.Angle.DEFAULT); } } diff --git a/src/main/java/frc/robot/subsystems/AdvantageScopeSubsystem.java b/src/main/java/frc/robot/subsystems/AdvantageScopeSubsystem.java index 4d2c4ae..7dc3299 100644 --- a/src/main/java/frc/robot/subsystems/AdvantageScopeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/AdvantageScopeSubsystem.java @@ -13,6 +13,7 @@ public class AdvantageScopeSubsystem extends SubsystemBase { static DrivetrainSubsystem drivetrainSubsystem; // static MusicPlayerSubsystem musicPlayerSubsystem; static IndexerSubsystem indexerSubsystem; + static String folder = "Data_"; public AdvantageScopeSubsystem ( @@ -23,6 +24,7 @@ public AdvantageScopeSubsystem ( // MusicPlayerSubsystem musicPlayerSubsystem, IndexerSubsystem indexerSubsystem ) { + AdvantageScopeSubsystem.intakeSubsystem = intakeSubsystem; AdvantageScopeSubsystem.shamperSubsystem = shamperSubsystem; AdvantageScopeSubsystem.climberSubsystem = climberSubsystem; @@ -41,6 +43,7 @@ public void periodic() { recordMusicPlayerData(); recordIndexerData(); recordTrapArmData(); + } public static void recordIntakeData () { @@ -60,6 +63,7 @@ public static void recordTrapArmData() { // trapArmSubsystem.getPosition()); } + public static void recordShamperData () { String shamperFolder = "Shamper_"; @@ -78,6 +82,7 @@ public static void recordClimberData () { } public static void recordMusicPlayerData() { + // String musicPlayerFolder = "MusicPlayer_"; // Logger.recordOutput(folder+musicPlayerFolder+"Current Track Play Time", @@ -85,6 +90,7 @@ public static void recordMusicPlayerData() { // Logger.recordOutput(folder+musicPlayerFolder+"Is Music Player Playing", // musicPlayerSubsystem.isPlayerPLaying()); + } public static void recordDrivetrainData () { @@ -94,6 +100,7 @@ public static void recordDrivetrainData () { drivetrainSubsystem.frontLeftModule.getState().angle.getDegrees(), drivetrainSubsystem.frontLeftModule.getState().speedMetersPerSecond, + drivetrainSubsystem.backLeftModule.getState().angle.getDegrees(), drivetrainSubsystem.backLeftModule.getState().speedMetersPerSecond, @@ -102,6 +109,7 @@ public static void recordDrivetrainData () { drivetrainSubsystem.frontRightModule.getState().angle.getDegrees(), drivetrainSubsystem.frontRightModule.getState().speedMetersPerSecond, + }; Logger.recordOutput(folder+drivetrainFolder+"Swerve Array", swerveArray); @@ -119,6 +127,7 @@ public static void recordDriverStationData() { Logger.recordOutput(folder+driverstationFolder+"Match Time", DriverStation.getMatchTime()); + Logger.recordOutput(folder+driverstationFolder+"Is Robot Enabled", DriverStation.isEnabled()); } } diff --git a/src/main/java/frc/robot/subsystems/AprilTagSubsystem.java b/src/main/java/frc/robot/subsystems/AprilTagSubsystem.java index c6f69aa..946ad88 100644 --- a/src/main/java/frc/robot/subsystems/AprilTagSubsystem.java +++ b/src/main/java/frc/robot/subsystems/AprilTagSubsystem.java @@ -3,6 +3,9 @@ import java.util.ArrayList; import java.util.List; import java.util.Optional; +import java.util.OptionalDouble; + +import javax.swing.text.html.Option; import org.photonvision.targeting.PhotonTrackedTarget; @@ -11,8 +14,12 @@ import com.team2052.lib.photonvision.PhotonPoseEstimator; import com.team2052.lib.photonvision.PhotonPoseEstimator.PoseStrategy; +import edu.wpi.first.apriltag.AprilTag; import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.apriltag.AprilTagFields; +import edu.wpi.first.units.Time; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; import frc.robot.RobotState; @@ -27,6 +34,12 @@ public class AprilTagSubsystem extends SubsystemBase{ private AprilTagFieldLayout aprilTagFieldLayout = AprilTagFields.k2024Crescendo.loadAprilTagLayoutField(); private PhotonCamera camera0 = new PhotonCamera(Constants.PhotonCamera1.CAMERA_NAME, Constants.PhotonCamera1.ROBOT_TO_CAMERA_METERS); private PhotonPoseEstimator photonPoseEstimator = new PhotonPoseEstimator(aprilTagFieldLayout, PoseStrategy.MULTI_TAG_PNP_ON_COPROCESSOR, camera0, camera0.getRobotToCamera()); + private int speakertag; + private double speakerTagYaw; + private Timer timer; + private double lastUpdatedTime; + private boolean isSeeingTag; + //private PhotonPoseEstimator photonPoseEstimator = new PhotonPoseEstimator(aprilTagFieldLayout, PoseStrategy., camera0, camera0.getRobotToCamera()); //private PhotonCamera camera1 = new PhotonCamera(Constants.PhotonCamera2.CAMERA_NAME, Constants.PhotonCamera2.ROBOT_TO_CAMERA_METERS); @@ -41,6 +54,14 @@ public static AprilTagSubsystem getInstance(){ private AprilTagSubsystem() { cameras.add(camera0); //cameras.add(camera1); + if (!robotState.getInstance().isRedAlliance()){ + speakertag = 8; //blue + } else { + speakertag = 4;//red + } + timer.start(); + isSeeingTag = false; + SmartDashboard.putBoolean("Is Seeing Speaker April Tag", isSeeingTag); } @Override @@ -48,6 +69,8 @@ public void periodic() { for(int i = 0; i < cameras.size(); i++){ PhotonCamera camera = cameras.get(i); var result = camera.getLatestResult(); + var targets = result.getTargets(); + boolean hasTargets = result.hasTargets(); if (hasTargets){ // PhotonTrackedTarget target7 = null; @@ -66,7 +89,25 @@ public void periodic() { estimatedPose = poseUpdate.get(); robotState.addAprilTagVisionUpdate(estimatedPose); } + + for (int j = 0; j < targets.size(); j++) { + if (targets.get(j).getFiducialId() == speakertag) { + speakerTagYaw = targets.get(j).getYaw(); + double lastUpdatedTime = timer.get(); + } + } + } } } + + public Optional getYaw() { + if (timer.get() - lastUpdatedTime <= Constants.AprilTags.APRILTAG_TIMEOUT) { + isSeeingTag = true; + return Optional.of(speakerTagYaw); + } else { + isSeeingTag = false; + return Optional.empty(); + } + } } diff --git a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java index 90bdcaa..5ff54e7 100644 --- a/src/main/java/frc/robot/subsystems/ClimberSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ClimberSubsystem.java @@ -6,7 +6,6 @@ import com.revrobotics.CANSparkBase.IdleMode; import com.revrobotics.CANSparkLowLevel.MotorType; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; import frc.robot.RobotState; diff --git a/src/main/java/frc/robot/subsystems/LedSubsystem.java b/src/main/java/frc/robot/subsystems/LedSubsystem.java index d5d610e..85235ae 100644 --- a/src/main/java/frc/robot/subsystems/LedSubsystem.java +++ b/src/main/java/frc/robot/subsystems/LedSubsystem.java @@ -2,10 +2,8 @@ import edu.wpi.first.wpilibj.DigitalOutput; import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; -import frc.robot.Robot; import frc.robot.RobotState; import frc.robot.auto.AutoFactory.Auto; import frc.robot.util.io.Dashboard; @@ -49,7 +47,7 @@ public static LedSubsystem getInstance() { // Method to allow calling this clas } public static enum LEDStatusMode { - OFF(0), + OFF(5), // have to do 5 because we don't have pin 8 and it reads pin 8 high when sending 0 CONE(1), CUBE(2), DISABLED_RED_PULSE(3), @@ -73,24 +71,21 @@ public int getPositionTicks() { @Override public void periodic() { - int code = 0; + int code = 5; if(!disableLEDs) { - if (RobotState.isDisabled()) { + if (DriverStation.isDisabled()) { // If disabled, finds gets the alliance color from the driver station and pulses that. Only pulses color if connected to station or FMS, else pulses default disabled color (Firefl status mode) Auto selected = Dashboard.getInstance().getAuto(); - if (selected == Auto.NO_AUTO){ + if (selected == Auto.NO_AUTO || selected == null){ currentStatusMode = LEDStatusMode.NO_AUTO; - } - else if (RobotState.getInstance().isRedAlliance()) { + } else if (RobotState.getInstance().isRedAlliance()) { currentStatusMode = LEDStatusMode.LAVA; } else if (!RobotState.getInstance().isRedAlliance()) { currentStatusMode = LEDStatusMode.WATER; } else { currentStatusMode = LEDStatusMode.OFF; // Reaches here if DriverStation.getAlliance returns Invalid, which just means it can't determine our alliance and we do cool default effect } - } - - if (RobotState.isAutonomous()) { + } else if (DriverStation.isAutonomous()) { if(RobotState.getInstance().getNoteDetected()){ currentStatusMode = LEDStatusMode.NOTE_DETECTED; } else if (RobotState.getInstance().getIsShamperAtGoalAngle()){ @@ -104,23 +99,23 @@ else if (RobotState.getInstance().isRedAlliance()) { } } - if (RobotState.isTeleop()) { - if(RobotState.getInstance().getNoteDetected()){ - currentStatusMode = LEDStatusMode.NOTE_DETECTED; - } else if (RobotState.getInstance().getIsShamperAtGoalAngle()){ - currentStatusMode = LEDStatusMode.RAINBOW; - } else { + if (DriverStation.isTeleopEnabled()) { + if(RobotState.getInstance().getNoteDetected() && RobotState.getInstance().getIsShamperAtGoalAngle()){ + currentStatusMode = LEDStatusMode.CUBE; + } else if(RobotState.getInstance().getNoteDetected()){ currentStatusMode = LEDStatusMode.CONE; + } else { + currentStatusMode = LEDStatusMode.NO_AUTO; } + } code = currentStatusMode.code; } else { //LEDs are disabled - code = 0; + code = 5; } - code = 2; // Code for encoding the code to binary on the digitalOutput pins Dashboard.getInstance().putData("Sending LED Code", code); codeChannel1.set((code & 1) > 0); // 2^0 diff --git a/src/main/java/frc/robot/subsystems/MusicPlayerSubsystem.java b/src/main/java/frc/robot/subsystems/MusicPlayerSubsystem.java index e6d8470..2f13c46 100644 --- a/src/main/java/frc/robot/subsystems/MusicPlayerSubsystem.java +++ b/src/main/java/frc/robot/subsystems/MusicPlayerSubsystem.java @@ -1,46 +1,48 @@ -// package frc.robot.subsystems; +package frc.robot.subsystems; -// import com.ctre.phoenix.motorcontrol.can.TalonFX; -// import com.ctre.phoenix.music.Orchestra; -// import edu.wpi.first.wpilibj2.command.SubsystemBase; -// import frc.robot.Constants; -// public class MusicPlayerSubsystem extends SubsystemBase{ +import com.ctre.phoenix6.Orchestra; +import com.ctre.phoenix6.hardware.TalonFX; -// private final Orchestra musicPlayer; -// private final TalonFX[] instrumentList; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; -// public MusicPlayerSubsystem() { -// musicPlayer = new Orchestra(); -// instrumentList = Constants.MusicPlayer.INSTRUMENT_TALONFX_PORT_LIST; +public class MusicPlayerSubsystem extends SubsystemBase{ -// for (int i = 0; i < instrumentList.length; i++) { -// musicPlayer.addInstrument(instrumentList[i]); -// } -// } + private final Orchestra musicPlayer; + private final TalonFX[] instrumentList; -// public void loadMusic(String filepath) { -// musicPlayer.loadMusic(filepath); -// } + public MusicPlayerSubsystem() { + musicPlayer = new Orchestra(); + instrumentList = Constants.MusicPlayer.INSTRUMENT_TALONFX_PORT_LIST; -// public void playLoadedTrack() { -// musicPlayer.play(); -// } + for (int i = 0; i < instrumentList.length; i++) { + musicPlayer.addInstrument(instrumentList[i]); + } + } -// public void pauseMusic() { -// musicPlayer.pause(); -// } + public void loadMusic(String filepath) { + musicPlayer.loadMusic(filepath); + } -// public void stopMusic() { -// musicPlayer.stop(); -// } + public void playLoadedTrack() { + musicPlayer.play(); + } -// public int getCurrentPlayTime() { -// return musicPlayer.getCurrentTime(); -// } + public void pauseMusic() { + musicPlayer.pause(); + } -// public boolean isPlayerPLaying() { -// return musicPlayer.isPlaying(); -// } -// } + public void stopMusic() { + musicPlayer.stop(); + } + + public double getCurrentPlayTime() { + return musicPlayer.getCurrentTime(); + } + + public boolean isPlayerPLaying() { + return musicPlayer.isPlaying(); + } +} diff --git a/src/main/java/frc/robot/subsystems/ShamperSubsystem.java b/src/main/java/frc/robot/subsystems/ShamperSubsystem.java index 70ba9ae..d059cbb 100644 --- a/src/main/java/frc/robot/subsystems/ShamperSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShamperSubsystem.java @@ -8,10 +8,8 @@ import com.ctre.phoenix6.configs.ClosedLoopRampsConfigs; import com.ctre.phoenix6.configs.Slot0Configs; -import com.ctre.phoenix6.configs.Slot1Configs; import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.GravityTypeValue; import com.ctre.phoenix6.signals.NeutralModeValue; import com.revrobotics.CANSparkMax; import com.revrobotics.CANSparkBase.IdleMode; @@ -25,7 +23,6 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.Constants; import frc.robot.RobotState; -import frc.robot.util.AimingCalculator; public class ShamperSubsystem extends SubsystemBase { private static TalonFX lowerMotor; diff --git a/src/main/java/frc/robot/subsystems/Superstructure.java b/src/main/java/frc/robot/subsystems/Superstructure.java deleted file mode 100644 index 5cba0a0..0000000 --- a/src/main/java/frc/robot/subsystems/Superstructure.java +++ /dev/null @@ -1,171 +0,0 @@ -// package frc.robot.subsystems; - -// import edu.wpi.first.wpilibj2.command.Commands; -// import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -// import edu.wpi.first.wpilibj2.command.RunCommand; -// import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -// import edu.wpi.first.wpilibj2.command.SubsystemBase; -// import edu.wpi.first.wpilibj2.command.WaitCommand; -// import frc.robot.Constants; -// import frc.robot.RobotState; -// import frc.robot.commands.climb.ClimberRetractCommand; -// import frc.robot.commands.indexer.IndexerIndexCommand; -// import frc.robot.commands.indexer.IndexerLoadCommand; -// import frc.robot.commands.intake.IntakeCommand; -// import frc.robot.commands.climb.ClimberExtendCommand; -// import frc.robot.commands.shamper.ShamperAngleCommand; -// import frc.robot.commands.shamper.ShamperIdleCommand; -// import frc.robot.commands.shamper.ShamperShootCommand; -// import frc.robot.subsystems.ShamperSubsystem.ShamperSpeed; -// import frc.robot.util.AimingCalculator; -// import frc.robot.util.calculator.ShootingAngleCalculator; - -// public class Superstructure extends SubsystemBase { -// private SuperstructureState state; -// private ShamperSubsystem shamper; -// private IndexerSubsystem indexer; -// private IntakeSubsystem intake; - -// public Superstructure(ShamperSubsystem shamper, IndexerSubsystem indexer, IntakeSubsystem intake) { -// this.shamper = shamper; -// this.indexer = indexer; -// this.intake = intake; - -// this.state = SuperstructureState.DEFAULT; -// } - -// public void setState(SuperstructureState state) { -// this.state = state; -// switch(state){ -// case DEFAULT: -// setDefault(); -// case INTAKE: -// setIntake(); -// case PODIUM_IDLE: -// setPodiumIdle(); -// case PODIUM_SCORE: -// setPodiumScoring(); -// case SPEAKER_IDLE: -// setSpeakerIdle(); -// case SPEAKER_SCORE: -// setSpeakerScoring(); -// case AMP_IDLE: -// setAmpIdle(); -// case AMP_SCORE: -// setAmpScore(); -// case CLIMBING: -// setClimbing(); -// } -// } - -// public SuperstructureState getState() { -// return state; -// } - -// private void setDefault() { -// SequentialCommandGroup setDefaultPositionCommand = new SequentialCommandGroup( -// new ShamperAngleCommand(shamper, Constants.Shamper.Angle.DEFAULT), -// new ShamperShootCommand(shamper, indexer, ShamperSpeed.OFF) -// ); - -// setDefaultPositionCommand.schedule(); -// } - -// private void setIntake(){ -// SequentialCommandGroup setIntakePositionCommand = new SequentialCommandGroup( -// new IntakeCommand(intake), -// new IndexerLoadCommand(indexer) -// ); - -// setIntakePositionCommand.schedule(); -// } - -// private void setPodiumIdle() { -// SequentialCommandGroup setPodiumIdlePositionCommand = new SequentialCommandGroup( -// new ShamperAngleCommand(shamper, Constants.Shamper.Angle.DEFAULT), -// new ShamperIdleCommand(shamper, ShamperSpeed.SPEAKER_IDLE) -// ); - -// setPodiumIdlePositionCommand.schedule(); -// } - -// private void setPodiumScoring() { -// SequentialCommandGroup setPodiumScoringCommand = new SequentialCommandGroup( -// new ShamperAngleCommand(shamper, Constants.Shamper.Angle.DEFAULT), -// new ShamperShootCommand(shamper, indexer, ShamperSpeed.SPEAKER_SCORE) -// ); - -// setPodiumScoringCommand.schedule(); -// } - -// private void setSpeakerIdle() { -// SequentialCommandGroup setSpeakerIdleCommand = new SequentialCommandGroup( -// new ShamperAngleCommand(shamper, ShootingAngleCalculator.getInstance().getShooterConfig(RobotState.getInstance().getRobotPose()).getAngleDegrees()), -// new ShamperIdleCommand(shamper, ShamperSpeed.SPEAKER_IDLE) -// ); - -// setSpeakerIdleCommand.schedule(); -// } - -// private void setSpeakerScoring() { -// SequentialCommandGroup setSpeakerScoringCommand = new SequentialCommandGroup( -// new ShamperAngleCommand(shamper, ShootingAngleCalculator.getInstance().getShooterConfig(RobotState.getInstance().getRobotPose()).getAngleDegrees()), -// new ShamperShootCommand(shamper, indexer, ShamperSpeed.SPEAKER_SCORE, ShamperSpeed.SPEAKER_IDLE) -// ); - -// setSpeakerScoringCommand.schedule(); -// } - -// private void setAmpIdle() { -// SequentialCommandGroup setAmpIdleCommand = new SequentialCommandGroup( -// new ShamperAngleCommand(shamper, Constants.Shamper.Angle.AMP), -// new ShamperIdleCommand(shamper, ShamperSpeed.AMP_IDLE) -// ); - -// setAmpIdleCommand.schedule(); -// } - -// private void setAmpScore() { -// SequentialCommandGroup setAmpScoreCommand = new SequentialCommandGroup( -// new ShamperAngleCommand(shamper, Constants.Shamper.Angle.AMP), -// new ShamperShootCommand(shamper, indexer, ShamperSpeed.AMP_SCORE, ShamperSpeed.SPEAKER_IDLE) -// ); - -// setAmpScoreCommand.schedule(); -// } - -// private void setClimbing() { -// // no -// } - -// @Override -// public void periodic() { -// if(state.needsRefresh()){ -// setState(state); -// } - -// System.out.println(Math.toDegrees(ShootingAngleCalculator.getInstance().getShooterConfig(RobotState.getInstance().getRobotPose()).getAngleDegrees())); -// } - -// public enum SuperstructureState { -// DEFAULT(false), -// INTAKE(false), -// PODIUM_IDLE(false), -// PODIUM_SCORE(false), -// SPEAKER_IDLE(true), -// SPEAKER_SCORE(true), -// AMP_IDLE(false), -// AMP_SCORE(false), -// CLIMBING(false); - -// private boolean refreshRequired; - -// private SuperstructureState(boolean refreshRequired) { -// this.refreshRequired = refreshRequired; -// } - -// public boolean needsRefresh() { -// return refreshRequired; -// } -// } -// } diff --git a/src/main/java/frc/robot/util/AimingCalculator.java b/src/main/java/frc/robot/util/AimingCalculator.java index 5ecb5ae..aaa8745 100644 --- a/src/main/java/frc/robot/util/AimingCalculator.java +++ b/src/main/java/frc/robot/util/AimingCalculator.java @@ -1,5 +1,7 @@ package frc.robot.util; +import java.util.Optional; + import org.littletonrobotics.junction.Logger; import edu.wpi.first.math.MathUtil; @@ -7,78 +9,12 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.util.Units; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.Constants; import frc.robot.RobotState; -import frc.robot.util.states.ShamperAndRotationState; +import frc.robot.subsystems.AprilTagSubsystem; public class AimingCalculator { - -// public static ShamperAndRotationState calculate() { -// double robotAngle; -// double shamperAngle; - -// double targetHeight = (Constants.FieldAndRobot.SPEAKER_TARGET_HEIGHT_OFF_GROUND_IN_METERS -// - Constants.FieldAndRobot.SHAMPER_HEIGHT_IN_METERS); - -// double robotXVelocity = RobotState.getInstance().getChassisSpeeds().vxMetersPerSecond; -// double robotYVelocity = RobotState.getInstance().getChassisSpeeds().vyMetersPerSecond; - -// double xDistanceToSpeaker = ((RobotState.getInstance().isRedAlliance()) -// ? Constants.FieldAndRobot.RED_SPEAKER_LOCATION.getX() -// + Constants.FieldAndRobot.RED_SPEAKER_TARGET_X_OFFSET_IN_METERS -// : Constants.FieldAndRobot.BLUE_SPEAKER_LOCATION.getX() -// + Constants.FieldAndRobot.BLUE_SPEAKER_TARGET_X_OFFSET_IN_METERS) -// // - RobotState.getInstance().getRobotPose().getX(); -// - Units.inchesToMeters(114); - -// double yDistanceToSpeaker = ((RobotState.getInstance().isRedAlliance()) -// ? Constants.FieldAndRobot.RED_SPEAKER_LOCATION.getY() -// + Constants.FieldAndRobot.RED_SPEAKER_TARGET_Y_OFFSET_IN_METERS -// : Constants.FieldAndRobot.BLUE_SPEAKER_LOCATION.getY() -// + Constants.FieldAndRobot.BLUE_SPEAKER_TARGET_Y_OFFSET_IN_METERS) -// // - RobotState.getInstance().getRobotPose().getY(); -// - Units.inchesToMeters(0); - -// double fullDistanceToSpeaker = RobotState.getInstance().getRobotPose().getTranslation() -// .getDistance((RobotState.getInstance().isRedAlliance()) ? Constants.FieldAndRobot.RED_SPEAKER_LOCATION -// : Constants.FieldAndRobot.BLUE_SPEAKER_LOCATION); - -// double robotVelocityAngle; -// if (robotXVelocity == 0) { -// robotVelocityAngle = (robotYVelocity <= 0) ? -// Math.toRadians(-90) : (robotYVelocity >= 0) ? -// Math.toRadians(90) : 0; -// } else { -// robotVelocityAngle = Math.atan(robotYVelocity / robotXVelocity); -// } - -// double angleToSpeaker; - -// if (xDistanceToSpeaker == 0) { -// angleToSpeaker = (yDistanceToSpeaker <= 0) ? Math.toRadians(90) : (yDistanceToSpeaker >= 0) ? Math.toRadians(-90) : 0; -// } else { -// angleToSpeaker = Math.atan(yDistanceToSpeaker / xDistanceToSpeaker); -// } - -// double differenceInAngle = robotVelocityAngle - angleToSpeaker; - -// double robotCombinedVelocity = Math.sqrt(Math.pow(robotXVelocity, 2) + Math.pow(robotYVelocity, 2)); - -// double verticalVelocityNeeded = Math.sqrt(2 * Constants.FieldAndRobot.GRAVITY_IN_METERS_PER_SECOND_SQUARED * targetHeight); -// double timeUntilTrajectoryTargetHeight = (verticalVelocityNeeded / Constants.FieldAndRobot.GRAVITY_IN_METERS_PER_SECOND_SQUARED); - -// double predictedNoteVelocity = 2 * robotCombinedVelocity * Math.cos(differenceInAngle); - -// double horizontalVelocityNeeded = (fullDistanceToSpeaker / timeUntilTrajectoryTargetHeight) - predictedNoteVelocity; - -// shamperAngle = Math.atan(verticalVelocityNeeded / horizontalVelocityNeeded); - -// double noteSpeedAfterLaunch = Constants.FieldAndRobot.NOTE_SPEED_IN_METERS_PER_SECOND * Math.acos(shamperAngle); - -// robotAngle = Math.toDegrees(angleToSpeaker - ((robotCombinedVelocity * differenceInAngle) / noteSpeedAfterLaunch)) + Constants.Drivetrain.ROBOT_AIMING_ROTATION_OFFSET_IN_DEGREES; - -// return new ShamperAndRotationState(shamperAngle, robotAngle); -// } // public static double calculateStill(Pose2d robotPose) { // Rotation2d angleToSpeaker = new Rotation2d(); @@ -184,6 +120,15 @@ public static double calculateAngleToSpeaker(Pose2d robotPose) { // Logger.recordOutput("ANGLE TO SPEAKER FIELD RELATIVE", angleToSpeakerFieldRelativeDegrees); // return MathUtil.inputModulus(Math.copySign(angleToSpeakerFieldRelativeDegrees, robotPose.getRotation().getDegrees()), 0, 360); // } + public static double calculateRobotAngleOneAprilTag() { + Optional yaw = AprilTagSubsystem.getInstance().getYaw(); + + if (!yaw.isEmpty()) { + return yaw.get(); + } else { + return 0; + } + } public static double calculateDistanceToSpeaker(Pose2d robotPose) { if(!RobotState.getInstance().isRedAlliance()) { // blue alliance diff --git a/src/main/java/frc/robot/util/RobotStateEstimator.java b/src/main/java/frc/robot/util/RobotStateEstimator.java index 9a75081..0236621 100644 --- a/src/main/java/frc/robot/util/RobotStateEstimator.java +++ b/src/main/java/frc/robot/util/RobotStateEstimator.java @@ -2,9 +2,6 @@ import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.kinematics.SwerveModulePosition; -import edu.wpi.first.math.kinematics.SwerveModuleState; -import edu.wpi.first.wpilibj.DriverStation; import frc.robot.Constants; import frc.robot.RobotState; diff --git a/src/main/java/frc/robot/util/RobotStatusCommunicator.java b/src/main/java/frc/robot/util/RobotStatusCommunicator.java index 0e48b80..e3cdf17 100644 --- a/src/main/java/frc/robot/util/RobotStatusCommunicator.java +++ b/src/main/java/frc/robot/util/RobotStatusCommunicator.java @@ -1,61 +1,55 @@ -// // package frc.robot.util; - +package frc.robot.util; + +import edu.wpi.first.wpilibj.DriverStation; +import frc.robot.commands.music.PlayActivationJingleCommand; +import frc.robot.commands.music.PlayTeleopJingleCommand; +import frc.robot.commands.music.PlayTwentySecondsLeftCommand; +import frc.robot.RobotState; +import frc.robot.subsystems.MusicPlayerSubsystem; // import edu.wpi.first.wpilibj.DriverStation; -// import frc.robot.Robot; // import frc.robot.RobotContainer; // import frc.robot.commands.music.PlayActivationJingleCommand; // import frc.robot.commands.music.PlayTeleopJingleCommand; // import frc.robot.commands.music.PlayTwentySecondsLeftCommand; -// import frc.robot.RobotState; // import frc.robot.subsystems.MusicPlayerSubsystem; -// // import edu.wpi.first.wpilibj.DriverStation; -// // import frc.robot.RobotContainer; -// // import frc.robot.commands.music.PlayActivationJingleCommand; -// // import frc.robot.commands.music.PlayTeleopJingleCommand; -// // import frc.robot.commands.music.PlayTwentySecondsLeftCommand; -// // import frc.robot.subsystems.MusicPlayerSubsystem; - -// public class RobotStatusCommunicator { -// private RobotState robotState = RobotState.getInstance(); -// private MusicPlayerSubsystem musicPlayer; -// private boolean hasRunTwentySeconds; - -// // public RobotStatusCommunicator(MusicPlayerSubsystem musicPlayer) { -// // this.musicPlayer = musicPlayer; -// // hasRunTwentySeconds = false; -// // } -// public void onRobotInitiation() { -// if (robotState.getMusicEnableStatus()) { -// new PlayActivationJingleCommand(musicPlayer); -// } -// } +public class RobotStatusCommunicator { + private RobotState robotState = RobotState.getInstance(); + private MusicPlayerSubsystem musicPlayer; + private boolean hasRunTwentySeconds; + + public RobotStatusCommunicator(MusicPlayerSubsystem musicPlayer) { + this.musicPlayer = musicPlayer; + hasRunTwentySeconds = false; + } -// // public void onRobotDisable() { -// // //BE VERY CARFUL WHAT IS PUT IN HERE BECAUSE IT WILL BE DONE AFTER THE ROBOT IS DISABLED!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! -// // } + public void onRobotInitiation() { + if (robotState.getMusicEnableStatus()) { + new PlayActivationJingleCommand(musicPlayer); + } + } -// public void onRobotPeriodic() { -// if (DriverStation.getMatchTime() >= 180 && !hasRunTwentySeconds) { -// onTwentySecondsLeft(); -// hasRunTwentySeconds = true; -// } -// } + public void onRobotPeriodic() { + if (DriverStation.getMatchTime() >= 180 && !hasRunTwentySeconds) { + onTwentySecondsLeft(); + hasRunTwentySeconds = true; + } + } -// public void onRobotTeleop() { -// if (robotState.getMusicEnableStatus()) { -// new PlayTeleopJingleCommand(musicPlayer); -// } -// } + public void onRobotTeleop() { + if (robotState.getMusicEnableStatus()) { + new PlayTeleopJingleCommand(musicPlayer); + } + } -// public void onTwentySecondsLeft() { -// if (robotState.getMusicEnableStatus()) { -// new PlayTwentySecondsLeftCommand(musicPlayer); -// } -// } + public void onTwentySecondsLeft() { + if (robotState.getMusicEnableStatus()) { + new PlayTwentySecondsLeftCommand(musicPlayer); + } + } -// // } +} diff --git a/src/main/java/frc/robot/util/calculator/ShootingAngleCalculator.java b/src/main/java/frc/robot/util/calculator/ShootingAngleCalculator.java index c8347bc..3823e9b 100644 --- a/src/main/java/frc/robot/util/calculator/ShootingAngleCalculator.java +++ b/src/main/java/frc/robot/util/calculator/ShootingAngleCalculator.java @@ -25,13 +25,13 @@ public ShootingAngleCalculator(){ public void setupShootAngleLookup(){ shootAngleLookup.add(new ShootAngleConfig(100, 55, 0.7 * Constants.Shamper.SHOOTER_MAX_VELOCITY_RPS)); shootAngleLookup.add(new ShootAngleConfig(150, 53, 0.7 * Constants.Shamper.SHOOTER_MAX_VELOCITY_RPS)); - shootAngleLookup.add(new ShootAngleConfig(200, 47, 0.7 * Constants.Shamper.SHOOTER_MAX_VELOCITY_RPS)); - shootAngleLookup.add(new ShootAngleConfig(250, 41, 0.8 * Constants.Shamper.SHOOTER_MAX_VELOCITY_RPS)); + shootAngleLookup.add(new ShootAngleConfig(200, 47, 0.8 * Constants.Shamper.SHOOTER_MAX_VELOCITY_RPS)); + shootAngleLookup.add(new ShootAngleConfig(250, 41, 0.9 * Constants.Shamper.SHOOTER_MAX_VELOCITY_RPS)); shootAngleLookup.add(new ShootAngleConfig(300, 38, 0.9 * Constants.Shamper.SHOOTER_MAX_VELOCITY_RPS)); - shootAngleLookup.add(new ShootAngleConfig(350, 35, 0.9 * Constants.Shamper.SHOOTER_MAX_VELOCITY_RPS)); - shootAngleLookup.add(new ShootAngleConfig(400, 34, 0.9 * Constants.Shamper.SHOOTER_MAX_VELOCITY_RPS)); - shootAngleLookup.add(new ShootAngleConfig(450, 33, 0.9 * Constants.Shamper.SHOOTER_MAX_VELOCITY_RPS)); - shootAngleLookup.add(new ShootAngleConfig(500, 33, 0.9 * Constants.Shamper.SHOOTER_MAX_VELOCITY_RPS)); + shootAngleLookup.add(new ShootAngleConfig(350, 33, 0.9 * Constants.Shamper.SHOOTER_MAX_VELOCITY_RPS)); + shootAngleLookup.add(new ShootAngleConfig(400, 32, 0.9 * Constants.Shamper.SHOOTER_MAX_VELOCITY_RPS)); + shootAngleLookup.add(new ShootAngleConfig(450, 28, 0.9 * Constants.Shamper.SHOOTER_MAX_VELOCITY_RPS)); + shootAngleLookup.add(new ShootAngleConfig(500, 27, 0.9 * Constants.Shamper.SHOOTER_MAX_VELOCITY_RPS)); shootAngleLookup.add(new ShootAngleConfig(550, 26, 0.9 * Constants.Shamper.SHOOTER_MAX_VELOCITY_RPS)); shootAngleLookup.add(new ShootAngleConfig(600, 25, 0.9 * Constants.Shamper.SHOOTER_MAX_VELOCITY_RPS)); } diff --git a/src/main/java/frc/robot/util/states/ShamperAndRotationState.java b/src/main/java/frc/robot/util/states/ShamperAndRotationState.java deleted file mode 100644 index 662529a..0000000 --- a/src/main/java/frc/robot/util/states/ShamperAndRotationState.java +++ /dev/null @@ -1,19 +0,0 @@ -package frc.robot.util.states; - -public class ShamperAndRotationState { - private double shamperAngle; - private double robotAngle; - - public ShamperAndRotationState(double shamperAngle, double robotAngle) { - this.shamperAngle = shamperAngle; - this.robotAngle = robotAngle; - } - - public double getShamperAngle(){ - return shamperAngle; - } - - public double getRobotAngle(){ - return robotAngle; - } -}