diff --git a/src/main/deploy/pathplanner/autos/4-SB-[F5-F4-F3]-EC.auto b/src/main/deploy/pathplanner/autos/4-SB-[F5-F4-F3]-EC.auto index 47a3de72..d1f33d3f 100644 --- a/src/main/deploy/pathplanner/autos/4-SB-[F5-F4-F3]-EC.auto +++ b/src/main/deploy/pathplanner/autos/4-SB-[F5-F4-F3]-EC.auto @@ -6,15 +6,60 @@ "data": { "commands": [ { - "type": "named", + "type": "sequential", "data": { - "name": "Smart-Shoot" + "commands": [ + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "path", + "data": { + "pathName": "SB-BL" + } + }, + { + "type": "named", + "data": { + "name": "preAim" + } + } + ] + } + }, + { + "type": "deadline", + "data": { + "commands": [ + { + "type": "named", + "data": { + "name": "Point-At-Speaker" + } + }, + { + "type": "named", + "data": { + "name": "preAim" + } + } + ] + } + }, + { + "type": "named", + "data": { + "name": "Smart-Shoot" + } + } + ] } }, { "type": "path", "data": { - "pathName": "SB-F5" + "pathName": "BL-F5" } }, { @@ -206,7 +251,7 @@ { "type": "path", "data": { - "pathName": "F3-BL" + "pathName": "F3-TS" } }, { @@ -265,6 +310,6 @@ ] } }, - "folder": "Commands", + "folder": "Testing", "choreoAuto": false } \ No newline at end of file diff --git a/src/main/deploy/pathplanner/paths/BL-F3.path b/src/main/deploy/pathplanner/paths/BL-F3.path index 9cedda52..d87bd8ac 100644 --- a/src/main/deploy/pathplanner/paths/BL-F3.path +++ b/src/main/deploy/pathplanner/paths/BL-F3.path @@ -8,24 +8,24 @@ }, "prevControl": null, "nextControl": { - "x": 4.303745530567986, - "y": 3.404095269729001 + "x": 4.1360075027842385, + "y": 3.571833297512749 }, "isLocked": false, "linkedName": "BottomLine" }, { "anchor": { - "x": 5.122701783865108, - "y": 3.7593052109181126 + "x": 5.033899298567829, + "y": 3.9073093530802447 }, "prevControl": { - "x": 4.27414470213556, - "y": 3.384361384107383 + "x": 4.19520915964909, + "y": 3.6606357828100275 }, "nextControl": { - "x": 6.073090172099115, - "y": 4.179244266184301 + "x": 6.030710478138978, + "y": 4.2004891117776415 }, "isLocked": false, "linkedName": null @@ -36,8 +36,8 @@ "y": 4.084914323674802 }, "prevControl": { - "x": 5.665383638459587, - "y": 4.055313495242375 + "x": 5.783786952189291, + "y": 4.2921201227017844 }, "nextControl": null, "isLocked": false, @@ -60,7 +60,7 @@ "maxAngularAcceleration": 720.0 }, "goalEndState": { - "velocity": 0, + "velocity": 1.5, "rotation": 0, "rotateFast": true }, diff --git a/src/main/deploy/pathplanner/paths/BL-F4.path b/src/main/deploy/pathplanner/paths/BL-F4.path index 25cc173d..c763f1db 100644 --- a/src/main/deploy/pathplanner/paths/BL-F4.path +++ b/src/main/deploy/pathplanner/paths/BL-F4.path @@ -38,7 +38,7 @@ "maxAngularAcceleration": 720.0 }, "goalEndState": { - "velocity": 0.75, + "velocity": 1.5, "rotation": 14.984877872841416, "rotateFast": false }, diff --git a/src/main/deploy/pathplanner/paths/F3-TL.path b/src/main/deploy/pathplanner/paths/F3-TS.path similarity index 80% rename from src/main/deploy/pathplanner/paths/F3-TL.path rename to src/main/deploy/pathplanner/paths/F3-TS.path index 4ef6f6f7..9cc015c0 100644 --- a/src/main/deploy/pathplanner/paths/F3-TL.path +++ b/src/main/deploy/pathplanner/paths/F3-TS.path @@ -8,24 +8,24 @@ }, "prevControl": null, "nextControl": { - "x": 6.1784646646216395, - "y": 4.953205291025968 + "x": 5.892323323108186, + "y": 4.272386237080167 }, "isLocked": false, "linkedName": "F3" }, { "anchor": { - "x": 5.892323323108186, - "y": 6.5 + "x": 4.096539731541003, + "y": 5.268947460971846 }, "prevControl": { - "x": 7.046755631972804, - "y": 6.028702057404115 + "x": 4.974697641702977, + "y": 4.617729235458472 }, "nextControl": null, "isLocked": false, - "linkedName": "TopLine" + "linkedName": "TS" } ], "rotationTargets": [], @@ -39,7 +39,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 8.0, + "rotation": 0.0, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/SB-BL.path b/src/main/deploy/pathplanner/paths/SB-BL.path index 7b5d89cb..0d7c73ff 100644 --- a/src/main/deploy/pathplanner/paths/SB-BL.path +++ b/src/main/deploy/pathplanner/paths/SB-BL.path @@ -8,8 +8,8 @@ }, "prevControl": null, "nextControl": { - "x": 1.5031243065887911, - "y": 2.313622145075961 + "x": 2.290889197163011, + "y": 3.5027646978370885 }, "isLocked": false, "linkedName": "SubwooferBottom" @@ -20,8 +20,8 @@ "y": 2.565405130810262 }, "prevControl": { - "x": 1.7580742853793425, - "y": 1.9635216193509297 + "x": 2.537562767433229, + "y": 2.851546472323715 }, "nextControl": null, "isLocked": false, @@ -39,7 +39,7 @@ }, "goalEndState": { "velocity": 0, - "rotation": 0, + "rotation": -35.5376777919744, "rotateFast": false }, "reversed": false, diff --git a/src/main/deploy/pathplanner/paths/SB-F5.path b/src/main/deploy/pathplanner/paths/SB-F5.path index 89fb82df..e3eacf14 100644 --- a/src/main/deploy/pathplanner/paths/SB-F5.path +++ b/src/main/deploy/pathplanner/paths/SB-F5.path @@ -29,7 +29,19 @@ } ], "rotationTargets": [], - "constraintZones": [], + "constraintZones": [ + { + "name": "Smart Shoot", + "minWaypointRelativePos": 0.0, + "maxWaypointRelativePos": 0.15, + "constraints": { + "maxVelocity": 2.0, + "maxAcceleration": 5.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + } + } + ], "eventMarkers": [], "globalConstraints": { "maxVelocity": 5.0, @@ -38,7 +50,7 @@ "maxAngularAcceleration": 720.0 }, "goalEndState": { - "velocity": 0.75, + "velocity": 1.5, "rotation": 0.0, "rotateFast": false }, diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index e8c1110c..bf3863c3 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -78,372 +78,377 @@ public class RobotContainer extends LightningContainer { - public static XboxControllerFilter driver; - public static XboxControllerFilter coPilot; - - // Subsystems - public Swerve drivetrain; - private Limelights limelights; - private Collector collector; - private Flywheel flywheel; - private Pivot pivot; - private Indexer indexer; - private Climber climber; - LEDs leds; - Orchestra sing; - - private SendableChooser autoChooser; - SwerveRequest.FieldCentric drive; - SwerveRequest.FieldCentric slow; - SwerveRequest.RobotCentric driveRobotCentric; - SwerveRequest.RobotCentric slowRobotCentric; - SwerveRequest.PointWheelsAt point; - Telemetry logger; - - private Boolean triggerInit; - - @Override - protected void initializeSubsystems() { - boolean setPath = SignalLogger.setPath(Constants.HOOT_PATH).isOK(); - // SignalLogger.enableAutoLogging(true); // TODO Return during COMPS - boolean startedLogs = SignalLogger.start().isOK(); - - if (startedLogs && setPath) { - System.out.println("STARTED HOOT LOG"); - } else { - System.out.println("FAILED TO START HOOT LOG"); - } - - driver = new XboxControllerFilter(ControllerConstants.DriverControllerPort, - Constants.ControllerConstants.DEADBAND, -1, 1, - XboxControllerFilter.filterMode.SQUARED); // Driver controller - coPilot = new XboxControllerFilter(ControllerConstants.CopilotControllerPort, - Constants.ControllerConstants.DEADBAND, -1, 1, - XboxControllerFilter.filterMode.SQUARED); // CoPilot controller - - drivetrain = TunerConstants.getDrivetrain(); - limelights = new Limelights(); - limelights.setApplyVisionUpdate(drivetrain::applyVisionPose); - - collector = new Collector(); - flywheel = new Flywheel(); - pivot = Constants.isMercury() ? new PivotMercury() : new PivotRhapsody(); - indexer = new Indexer(collector); - climber = new Climber(); - leds = new LEDs(); - sing = new Orchestra(); - - triggerInit = false; - - point = new SwerveRequest.PointWheelsAt(); - logger = new Telemetry(DrivetrainConstants.MaxSpeed); - - triggerInit = false; - } - - @Override - protected void initializeNamedCommands() { - NamedCommands.registerCommand("disable-Vision", - new InstantCommand(() -> drivetrain.disableVision())); - NamedCommands.registerCommand("enable-Vision", - new InstantCommand(() -> drivetrain.enableVision())); - NamedCommands.registerCommand("led-Shoot", - leds.enableState(LED_STATES.SHOOTING).withTimeout(0.5)); - - NamedCommands.registerCommand("Cand-Sub", - new PointBlankShotAuton(flywheel, pivot, indexer) - .deadlineWith(leds.enableState(LED_STATES.SHOOTING).withTimeout(1))); - NamedCommands.registerCommand("Cand-C1", new CandC1(flywheel, pivot, indexer)); - NamedCommands.registerCommand("Cand-C2", new CandC2(flywheel, pivot, indexer)); - NamedCommands.registerCommand("Cand-C3", new CandC3(flywheel, pivot, indexer)); - NamedCommands.registerCommand("Cand-Line", new CandLine(flywheel, pivot, indexer)); - NamedCommands.registerCommand("AMP", new AmpShotAuton(flywheel, pivot, indexer)); - NamedCommands.registerCommand("Stow", new Stow(flywheel, pivot)); - NamedCommands.registerCommand("Smart-Shoot", - new SmartShoot(flywheel, pivot, drivetrain, indexer, leds) - .alongWith(leds.enableState(LED_STATES.SHOOTING).withTimeout(0.5))); - NamedCommands.registerCommand("preAim", new preAim(flywheel, pivot, drivetrain)); - NamedCommands.registerCommand("Chase-Pieces", - new ChasePieces(drivetrain, collector, indexer, pivot, flywheel, limelights)); - NamedCommands.registerCommand("Smart-Collect", - new AutonSmartCollect(() -> 0.5, () -> 0.6, collector, indexer) - .deadlineWith(leds.enableState(LED_STATES.COLLECTING).withTimeout(1))); - NamedCommands.registerCommand("Index-Up", new Index(() -> IndexerConstants.INDEXER_DEFAULT_POWER, indexer)); - NamedCommands.registerCommand("PathFind", new PathToPose(PathFindingConstants.TEST_POSE)); - NamedCommands.registerCommand("Collect-And-Go", new CollectAndGo(collector, flywheel, indexer)); - NamedCommands.registerCommand("Point-At-Speaker", - new PointAtPoint(DrivetrainConstants.SPEAKER_POSE, drivetrain, driver)); - NamedCommands.registerCommand("Has-Piece", new HasPieceAuto(indexer)); - NamedCommands.registerCommand("Stop-Drive", new stopDrive(drivetrain)); - NamedCommands.registerCommand("Stop-Flywheel", new FlywheelIN(flywheel)); - - // make sure named commands are initialized before autobuilder! - autoChooser = AutoBuilder.buildAutoChooser(); - LightningShuffleboard.set("Auton", "Auto Chooser", autoChooser); - } - - @Override - protected void configureButtonBindings() { - /* driver */ - // field centric for the robot - new Trigger(() -> driver.getLeftTriggerAxis() > 0.25d) - .onTrue(new InstantCommand(() -> drivetrain.setRobotCentricControl(true))) - .whileTrue(drivetrain.applyPercentRequestRobot( - () -> -driver.getLeftY(), () -> -driver.getLeftX(), () -> -driver.getRightX())) - .onFalse(new InstantCommand(() -> drivetrain.setRobotCentricControl(false))); - - // enables slow mode for driving - new Trigger(() -> driver.getRightTriggerAxis() > 0.25d) - .onTrue(new InstantCommand(() -> drivetrain.setSlowMode(true))) - .onFalse(new InstantCommand(() -> drivetrain.setSlowMode(false))); - - // sets field relative forward to the direction the robot is facing - new Trigger(() -> driver.getStartButton() && driver.getBackButton()) - .onTrue(drivetrain.runOnce(drivetrain::seedFieldRelative) - .andThen(new InstantCommand( - () -> drivetrain.setOperatorPerspectiveForward(new Rotation2d(Math.toRadians(0)))))); - - // makes the robot chase pieces - new Trigger(driver::getRightBumper) - .whileTrue(new ChasePieces(drivetrain, collector, indexer, pivot, flywheel, limelights) - .deadlineWith(leds.enableState(LED_STATES.CHASING))); - - // new Trigger(driver::getRightBumper) - // .whileTrue(new PathFindToAuton(PathPlannerPath.fromPathFile("PathFind-AMP"), - // drivetrain, driver)); - - // parks the robot - new Trigger(driver::getXButton).whileTrue(new RunCommand(() -> drivetrain.brake())); - - // smart shoot for the robot - new Trigger(driver::getAButton) - .whileTrue(new SmartShoot(flywheel, pivot, drivetrain, indexer, leds) - .deadlineWith(leds.enableState(LED_STATES.SHOOTING))); - - // aim at amp and stage tags for the robot - new Trigger(driver::getYButton) - .whileTrue(new PointAtTag(drivetrain, limelights, driver)); // TODO: make work - - new Trigger(driver::getLeftBumper) - .whileTrue(new PointAtPoint(DrivetrainConstants.SPEAKER_POSE, drivetrain, driver)); - - // new Trigger(driver::getYButton) - // .whileTrue(new MoveToPose(AutonomousConstants.TARGET_POSE, drivetrain)); - - new Trigger(() -> driver.getPOV() == 0).toggleOnTrue(leds.enableState(LED_STATES.DISABLED)); - - /* copilot */ - new Trigger(coPilot::getBButton) - .whileTrue(new InstantCommand(() -> flywheel.stop(), flywheel) - .andThen(new SmartCollect(() -> 0.65, () -> 0.9, collector, indexer, pivot, flywheel)) - .deadlineWith(leds.enableState(LED_STATES.COLLECTING))); - - // .andThen(new SmartCollect(() -> 0.65, () -> 0.9, collector, indexer, pivot, flywheel)) - - // cand shots for the robot - new Trigger(coPilot::getAButton) - .whileTrue(new AmpShot(flywheel, pivot).deadlineWith(leds.enableState(LED_STATES.SHOOTING))); - new Trigger(coPilot::getXButton) - .whileTrue(new PointBlankShot(flywheel, pivot).deadlineWith(leds.enableState(LED_STATES.SHOOTING))); - new Trigger(coPilot::getYButton).whileTrue(new PivotUP(pivot)); - // new Trigger(coPilot::getAButton).whileTrue(new Tune(flywheel, pivot) - // .deadlineWith(leds.enableState(LED_STATES.SHOOTING))); - - /* BIAS */ - new Trigger(() -> coPilot.getPOV() == 0) - .onTrue(new InstantCommand(() -> pivot.increaseBias())); // UP - new Trigger(() -> coPilot.getPOV() == 180) - .onTrue(new InstantCommand(() -> pivot.decreaseBias())); // DOWN - - new Trigger(() -> coPilot.getPOV() == 90) - .onTrue(new InstantCommand(() -> flywheel.increaseBias())); // RIGHT - new Trigger(() -> coPilot.getPOV() == 270) - .onTrue(new InstantCommand(() -> flywheel.decreaseBias())); // LEFT - - new Trigger(coPilot::getRightBumper) - .whileTrue(new Index(() -> IndexerConstants.INDEXER_DEFAULT_POWER, indexer)); - new Trigger(coPilot::getLeftBumper) - .whileTrue(new Index(() -> -IndexerConstants.INDEXER_DEFAULT_POWER, indexer) - .deadlineWith(new FlywheelIN(flywheel))); - - /* Other */ - new Trigger( - () -> ((limelights.getStopMe().hasTarget() || limelights.getChamps().hasTarget()) - && DriverStation.isEnabled())) - .whileTrue(leds.enableState(LED_STATES.HAS_VISION)); - new Trigger(() -> indexer.getEntryBeamBreakState() || indexer.getExitBeamBreakState() - || collector.getEntryBeamBreakState()) - .whileTrue(leds.enableState(LED_STATES.HAS_PIECE)) - .onTrue(leds.enableState(LED_STATES.COLLECTED).withTimeout(2)); - new Trigger(() -> drivetrain.isInField() && triggerInit).whileFalse(leds.enableState(LED_STATES.BAD_POSE)); - new Trigger(() -> !drivetrain.isStable() && DriverStation.isDisabled() - && !(limelights.getStopMe().getBlueAlliancePose().getMoreThanOneTarget() - || limelights.getChamps().getBlueAlliancePose().getMoreThanOneTarget())) - .whileTrue(leds.enableState(LED_STATES.BAD_POSE)); - new Trigger(() -> DriverStation.isDisabled() - && !(limelights.getStopMe().getBlueAlliancePose().getMoreThanOneTarget() - || limelights.getChamps().getBlueAlliancePose().getMoreThanOneTarget())) - .whileTrue(leds.enableState(LED_STATES.BAD_POSE)); - new Trigger(() -> !drivetrain.isStable() && DriverStation.isDisabled() - && (limelights.getStopMe().getBlueAlliancePose().getMoreThanOneTarget() - || limelights.getChamps().getBlueAlliancePose().getMoreThanOneTarget())) - .whileTrue(leds.enableState(LED_STATES.GOOD_POSE)); - triggerInit = true; - - new Trigger(() -> collector.getEntryBeamBreakState()) - .whileTrue(leds.enableState(LED_STATES.COLLECTOR_BEAMBREAK)); - new Trigger(() -> indexer.getEntryBeamBreakState()) - .whileTrue(leds.enableState(LED_STATES.INDEXER_ENTER_BEAMBREAK)); - new Trigger(() -> indexer.getExitBeamBreakState()) - .whileTrue(leds.enableState(LED_STATES.INDEXER_EXIT_BEAMBREAK)); - new Trigger(() -> pivot.getForwardLimit()) - .whileTrue(leds.enableState(LED_STATES.PIVOT_BOTTOM_SWITCH)); - new Trigger(() -> pivot.getReverseLimit()) - .whileTrue(leds.enableState(LED_STATES.PIVOT_TOP_SWITCH)); - - new Trigger(() -> DriverStation.isAutonomousEnabled()).whileTrue(new CollisionDetection( - drivetrain, CollisionType.AUTON)); - - new Trigger(() -> LightningShuffleboard.getBool("Swerve", "Swap", false)) - .onTrue(new InstantCommand(() -> drivetrain.swap(driver, coPilot))) - .onFalse(new InstantCommand(() -> drivetrain.swap(driver, coPilot))); - - // BLUE Alliance set - new Trigger(() -> LightningShuffleboard.getBool("Auton", "POSE BLUE A", false)) - .onTrue(new InstantCommand( - () -> drivetrain.setDrivetrainPose(AutonomousConstants.SOURCE_SUB_A_STARTPOSE_BLUE))); - new Trigger(() -> LightningShuffleboard.getBool("Auton", "POSE BLUE B", false)) - .onTrue(new InstantCommand( - () -> drivetrain.setDrivetrainPose(AutonomousConstants.SOURCE_SUB_B_STARTPOSE_BLUE))); - new Trigger(() -> LightningShuffleboard.getBool("Auton", "POSE BLUE C", false)) - .onTrue(new InstantCommand( - () -> drivetrain.setDrivetrainPose(AutonomousConstants.SOURCE_SUB_C_STARTPOSE_BLUE))); - - // BLUE Alliance set - new Trigger(() -> LightningShuffleboard.getBool("Auton", "POSE RED A", false)) - .onTrue(new InstantCommand( - () -> drivetrain.setDrivetrainPose(AutonomousConstants.SOURCE_SUB_A_STARTPOSE_RED))); - new Trigger(() -> LightningShuffleboard.getBool("Auton", "POSE RED B", false)) - .onTrue(new InstantCommand( - () -> drivetrain.setDrivetrainPose(AutonomousConstants.SOURCE_SUB_B_STARTPOSE_RED))); - new Trigger(() -> LightningShuffleboard.getBool("Auton", "POSE RED C", false)) - .onTrue(new InstantCommand( - () -> drivetrain.setDrivetrainPose(AutonomousConstants.SOURCE_SUB_C_STARTPOSE_RED))); - } - - @Override - protected void configureDefaultCommands() { - /* driver */ - drivetrain.registerTelemetry(logger::telemeterize); - - drivetrain.setDefaultCommand(drivetrain.applyPercentRequestField( - () -> -driver.getLeftY(), () -> -driver.getLeftX(), () -> -driver.getRightX())); - // .alongWith(new CollisionDetection(drivetrain, CollisionType.TELEOP))); - - /* copilot */ - collector.setDefaultCommand( - new Collect(() -> MathUtil.applyDeadband( - (coPilot.getRightTriggerAxis() - coPilot.getLeftTriggerAxis()), - ControllerConstants.DEADBAND), collector)); - - // climber.setDefaultCommand( - // new SmartClimb(climber, drivetrain, pivot, leds, () -> -coPilot.getLeftY(), - // () -> -coPilot.getRightY(), - // coPilot::getYButton).deadlineWith(leds.enableState(LED_STATES.CLIMBING))); - - climber.setDefaultCommand(new ManualClimb(() -> -coPilot.getLeftY(), () -> -coPilot.getRightY(), climber)); - } - - protected Command getAutonomousCommand() { - return autoChooser.getSelected(); - } - - @Override - protected void releaseDefaultCommands() { - } - - @Override - protected void initializeDashboardCommands() { - } - - @Override - protected void configureFaultCodes() { - } - - @Override - protected void configureFaultMonitors() { - } - - @Override - protected void configureSystemTests() { - SystemTest.registerTest("Drive Test", new DrivetrainSystemTest(drivetrain, - DrivetrainConstants.SYS_TEST_SPEED_DRIVE)); // to be tested - SystemTest.registerTest("Azimuth Test", new TurnSystemTest(drivetrain, - DrivetrainConstants.SYS_TEST_SPEED_TURN)); - - // SystemTest.registerTest("Single Note Cycle", new CycleSytemTest(collector, - // indexer, pivot, flywheel, () -> 0.5d, () -> 0.6d, () -> 250)); - - SystemTest.registerTest("Collector Test", new CollectorSystemTest(collector, - Constants.CollectorConstants.COLLECTOR_SYSTEST_POWER)); - - // SystemTest.registerTest("Pivot 90 Degrees", new PivotAngleTest(pivot, - // Constants.PivotConstants.PIVOT_SYSTEST_ANGLE)); - - SystemTest.registerTest("Flywheel Test", new FlywheelSystemTest(flywheel, collector, - indexer, pivot, Constants.FlywheelConstants.FLYWHEEL_SYSTEST_RPM)); - - SystemTest.registerTest("Indexer Test", new IndexerSystemTest(indexer, - Constants.IndexerConstants.INDEXER_SYSTEST_POWER)); - - // SystemTest.registerTest("Climb Test", new ClimbSystemTest(climber, - // Constants.ClimbConstants.CLIMB_SYSTEST_POWER)); - - // Sing chooser SendableChooser songChooser = new - // SendableChooser<>(); - // songChooser.setDefaultOption(MusicConstants.BOH_RHAP_FILEPATH, - // new SingSystemTest(drivetrain, MusicConstants.BOH_RHAP_FILEPATH, sing)); - // for (String filepath : MusicConstants.SET_LIST) { - // songChooser.addOption(filepath, new SingSystemTest(drivetrain, filepath, - // sing)); - // } - - // LightningShuffleboard.set("SystemTest", "Songs List", songChooser); - - // songChooser.onChange((SystemTestCommand command) -> { - // if (command != null) { - // command.schedule(); - // } - // }); - - // songChooser.close(); - } - - public static Command hapticDriverCommand() { - if (!DriverStation.isAutonomous()) { - return new StartEndCommand(() -> { - driver.setRumble(GenericHID.RumbleType.kRightRumble, 1d); - driver.setRumble(GenericHID.RumbleType.kLeftRumble, 1d); - }, () -> { - driver.setRumble(GenericHID.RumbleType.kRightRumble, 0); - driver.setRumble(GenericHID.RumbleType.kLeftRumble, 0); - }); - } else { - return new InstantCommand(); - } - } - - public static Command hapticCopilotCommand() { - if (!DriverStation.isAutonomous()) { - return new StartEndCommand(() -> { - coPilot.setRumble(GenericHID.RumbleType.kRightRumble, 1d); - coPilot.setRumble(GenericHID.RumbleType.kLeftRumble, 1d); - }, () -> { - coPilot.setRumble(GenericHID.RumbleType.kRightRumble, 0); - coPilot.setRumble(GenericHID.RumbleType.kLeftRumble, 0); - }); - } else { - return new InstantCommand(); - } - } + public static XboxControllerFilter driver; + public static XboxControllerFilter coPilot; + + // Subsystems + public Swerve drivetrain; + private Limelights limelights; + private Collector collector; + private Flywheel flywheel; + private Pivot pivot; + private Indexer indexer; + private Climber climber; + LEDs leds; + Orchestra sing; + + private SendableChooser autoChooser; + SwerveRequest.FieldCentric drive; + SwerveRequest.FieldCentric slow; + SwerveRequest.RobotCentric driveRobotCentric; + SwerveRequest.RobotCentric slowRobotCentric; + SwerveRequest.PointWheelsAt point; + Telemetry logger; + + private Boolean triggerInit; + + @Override + protected void initializeSubsystems() { + boolean setPath = SignalLogger.setPath(Constants.HOOT_PATH).isOK(); + // SignalLogger.enableAutoLogging(true); // TODO Return during COMPS + boolean startedLogs = SignalLogger.start().isOK(); + + if (startedLogs && setPath) { + System.out.println("STARTED HOOT LOG"); + } else { + System.out.println("FAILED TO START HOOT LOG"); + } + + driver = new XboxControllerFilter(ControllerConstants.DriverControllerPort, + Constants.ControllerConstants.DEADBAND, -1, 1, + XboxControllerFilter.filterMode.SQUARED); // Driver controller + coPilot = new XboxControllerFilter(ControllerConstants.CopilotControllerPort, + Constants.ControllerConstants.DEADBAND, -1, 1, + XboxControllerFilter.filterMode.SQUARED); // CoPilot controller + + drivetrain = TunerConstants.getDrivetrain(); + limelights = new Limelights(); + limelights.setApplyVisionUpdate(drivetrain::applyVisionPose); + + collector = new Collector(); + flywheel = new Flywheel(); + pivot = Constants.isMercury() ? new PivotMercury() : new PivotRhapsody(); + indexer = new Indexer(collector); + if (!Constants.isMercury()) { + climber = new Climber(); + } + leds = new LEDs(); + sing = new Orchestra(); + + triggerInit = false; + + point = new SwerveRequest.PointWheelsAt(); + logger = new Telemetry(DrivetrainConstants.MaxSpeed); + + triggerInit = false; + } + + @Override + protected void initializeNamedCommands() { + NamedCommands.registerCommand("disable-Vision", + new InstantCommand(() -> drivetrain.disableVision())); + NamedCommands.registerCommand("enable-Vision", + new InstantCommand(() -> drivetrain.enableVision())); + NamedCommands.registerCommand("led-Shoot", + leds.enableState(LED_STATES.SHOOTING).withTimeout(0.5)); + + NamedCommands.registerCommand("Cand-Sub", + new PointBlankShotAuton(flywheel, pivot, indexer) + .deadlineWith(leds.enableState(LED_STATES.SHOOTING).withTimeout(1))); + NamedCommands.registerCommand("Cand-C1", new CandC1(flywheel, pivot, indexer)); + NamedCommands.registerCommand("Cand-C2", new CandC2(flywheel, pivot, indexer)); + NamedCommands.registerCommand("Cand-C3", new CandC3(flywheel, pivot, indexer)); + NamedCommands.registerCommand("Cand-Line", new CandLine(flywheel, pivot, indexer)); + NamedCommands.registerCommand("AMP", new AmpShotAuton(flywheel, pivot, indexer)); + NamedCommands.registerCommand("Stow", new Stow(flywheel, pivot)); + NamedCommands.registerCommand("Smart-Shoot", + new SmartShoot(flywheel, pivot, drivetrain, indexer, leds) + .alongWith(leds.enableState(LED_STATES.SHOOTING).withTimeout(0.5))); + NamedCommands.registerCommand("preAim", new preAim(flywheel, pivot, drivetrain)); + NamedCommands.registerCommand("Chase-Pieces", + new ChasePieces(drivetrain, collector, indexer, pivot, flywheel, limelights)); + NamedCommands.registerCommand("Smart-Collect", + new AutonSmartCollect(() -> 0.5, () -> 0.6, collector, indexer) + .deadlineWith(leds.enableState(LED_STATES.COLLECTING).withTimeout(1))); + NamedCommands.registerCommand("Index-Up", new Index(() -> IndexerConstants.INDEXER_DEFAULT_POWER, indexer)); + NamedCommands.registerCommand("PathFind", new PathToPose(PathFindingConstants.TEST_POSE)); + NamedCommands.registerCommand("Collect-And-Go", new CollectAndGo(collector, flywheel, indexer)); + NamedCommands.registerCommand("Point-At-Speaker", + new PointAtPoint(DrivetrainConstants.SPEAKER_POSE, drivetrain, driver)); + NamedCommands.registerCommand("Has-Piece", new HasPieceAuto(indexer)); + NamedCommands.registerCommand("Stop-Drive", new stopDrive(drivetrain)); + NamedCommands.registerCommand("Stop-Flywheel", new FlywheelIN(flywheel)); + + // make sure named commands are initialized before autobuilder! + autoChooser = AutoBuilder.buildAutoChooser(); + LightningShuffleboard.set("Auton", "Auto Chooser", autoChooser); + } + + @Override + protected void configureButtonBindings() { + /* driver */ + // field centric for the robot + new Trigger(() -> driver.getLeftTriggerAxis() > 0.25d) + .onTrue(new InstantCommand(() -> drivetrain.setRobotCentricControl(true))) + .whileTrue(drivetrain.applyPercentRequestRobot( + () -> -driver.getLeftY(), () -> -driver.getLeftX(), () -> -driver.getRightX())) + .onFalse(new InstantCommand(() -> drivetrain.setRobotCentricControl(false))); + + // enables slow mode for driving + new Trigger(() -> driver.getRightTriggerAxis() > 0.25d) + .onTrue(new InstantCommand(() -> drivetrain.setSlowMode(true))) + .onFalse(new InstantCommand(() -> drivetrain.setSlowMode(false))); + + // sets field relative forward to the direction the robot is facing + new Trigger(() -> driver.getStartButton() && driver.getBackButton()) + .onTrue(drivetrain.runOnce(drivetrain::seedFieldRelative) + .andThen(new InstantCommand( + () -> drivetrain.setOperatorPerspectiveForward(new Rotation2d(Math.toRadians(0)))))); + + // makes the robot chase pieces + new Trigger(driver::getRightBumper) + .whileTrue(new ChasePieces(drivetrain, collector, indexer, pivot, flywheel, limelights) + .deadlineWith(leds.enableState(LED_STATES.CHASING))); + + // new Trigger(driver::getRightBumper) + // .whileTrue(new PathFindToAuton(PathPlannerPath.fromPathFile("PathFind-AMP"), + // drivetrain, driver)); + + // parks the robot + new Trigger(driver::getXButton).whileTrue(new RunCommand(() -> drivetrain.brake())); + + // smart shoot for the robot + new Trigger(driver::getAButton) + .whileTrue(new SmartShoot(flywheel, pivot, drivetrain, indexer, leds) + .deadlineWith(leds.enableState(LED_STATES.SHOOTING))); + + // aim at amp and stage tags for the robot + new Trigger(driver::getYButton) + .whileTrue(new PointAtTag(drivetrain, limelights, driver)); // TODO: make work + + new Trigger(driver::getLeftBumper) + .whileTrue(new PointAtPoint(DrivetrainConstants.SPEAKER_POSE, drivetrain, driver)); + + // new Trigger(driver::getYButton) + // .whileTrue(new MoveToPose(AutonomousConstants.TARGET_POSE, drivetrain)); + + new Trigger(() -> driver.getPOV() == 0).toggleOnTrue(leds.enableState(LED_STATES.DISABLED)); + + /* copilot */ + new Trigger(coPilot::getBButton) + // .whileTrue(new InstantCommand(() -> flywheel.stop(), flywheel) + .whileTrue(new AutonSmartCollect(() -> 0.65, () -> 0.9, collector, indexer) + .deadlineWith(leds.enableState(LED_STATES.COLLECTING))); + + // .andThen(new SmartCollect(() -> 0.65, () -> 0.9, collector, indexer, pivot, + // flywheel)) + + // cand shots for the robot + new Trigger(coPilot::getAButton) + .whileTrue(new AmpShot(flywheel, pivot).deadlineWith(leds.enableState(LED_STATES.SHOOTING))); + new Trigger(coPilot::getXButton) + .whileTrue(new PointBlankShot(flywheel, pivot).deadlineWith(leds.enableState(LED_STATES.SHOOTING))); + new Trigger(coPilot::getYButton).whileTrue(new PivotUP(pivot)); + // new Trigger(coPilot::getAButton).whileTrue(new Tune(flywheel, pivot) + // .deadlineWith(leds.enableState(LED_STATES.SHOOTING))); + + /* BIAS */ + new Trigger(() -> coPilot.getPOV() == 0) + .onTrue(new InstantCommand(() -> pivot.increaseBias())); // UP + new Trigger(() -> coPilot.getPOV() == 180) + .onTrue(new InstantCommand(() -> pivot.decreaseBias())); // DOWN + + new Trigger(() -> coPilot.getPOV() == 90) + .onTrue(new InstantCommand(() -> flywheel.increaseBias())); // RIGHT + new Trigger(() -> coPilot.getPOV() == 270) + .onTrue(new InstantCommand(() -> flywheel.decreaseBias())); // LEFT + + new Trigger(coPilot::getRightBumper) + .whileTrue(new Index(() -> IndexerConstants.INDEXER_DEFAULT_POWER, indexer)); + new Trigger(coPilot::getLeftBumper) + .whileTrue(new Index(() -> -IndexerConstants.INDEXER_DEFAULT_POWER, indexer) + .deadlineWith(new FlywheelIN(flywheel))); + + /* Other */ + new Trigger( + () -> ((limelights.getStopMe().hasTarget() || limelights.getChamps().hasTarget()) + && DriverStation.isEnabled())) + .whileTrue(leds.enableState(LED_STATES.HAS_VISION)); + new Trigger(() -> indexer.getEntryBeamBreakState() || indexer.getExitBeamBreakState() + || collector.getEntryBeamBreakState()) + .whileTrue(leds.enableState(LED_STATES.HAS_PIECE)) + .onTrue(leds.enableState(LED_STATES.COLLECTED).withTimeout(2)); + new Trigger(() -> drivetrain.isInField() && triggerInit).whileFalse(leds.enableState(LED_STATES.BAD_POSE)); + new Trigger(() -> !drivetrain.isStable() && DriverStation.isDisabled() + && !(limelights.getStopMe().getBlueAlliancePose().getMoreThanOneTarget() + || limelights.getChamps().getBlueAlliancePose().getMoreThanOneTarget())) + .whileTrue(leds.enableState(LED_STATES.BAD_POSE)); + new Trigger(() -> DriverStation.isDisabled() + && !(limelights.getStopMe().getBlueAlliancePose().getMoreThanOneTarget() + || limelights.getChamps().getBlueAlliancePose().getMoreThanOneTarget())) + .whileTrue(leds.enableState(LED_STATES.BAD_POSE)); + new Trigger(() -> !drivetrain.isStable() && DriverStation.isDisabled() + && (limelights.getStopMe().getBlueAlliancePose().getMoreThanOneTarget() + || limelights.getChamps().getBlueAlliancePose().getMoreThanOneTarget())) + .whileTrue(leds.enableState(LED_STATES.GOOD_POSE)); + triggerInit = true; + + new Trigger(() -> collector.getEntryBeamBreakState()) + .whileTrue(leds.enableState(LED_STATES.COLLECTOR_BEAMBREAK)); + new Trigger(() -> indexer.getEntryBeamBreakState()) + .whileTrue(leds.enableState(LED_STATES.INDEXER_ENTER_BEAMBREAK)); + new Trigger(() -> indexer.getExitBeamBreakState()) + .whileTrue(leds.enableState(LED_STATES.INDEXER_EXIT_BEAMBREAK)); + new Trigger(() -> pivot.getForwardLimit()) + .whileTrue(leds.enableState(LED_STATES.PIVOT_BOTTOM_SWITCH)); + new Trigger(() -> pivot.getReverseLimit()) + .whileTrue(leds.enableState(LED_STATES.PIVOT_TOP_SWITCH)); + + new Trigger(() -> DriverStation.isAutonomousEnabled()).whileTrue(new CollisionDetection( + drivetrain, CollisionType.AUTON)); + + new Trigger(() -> LightningShuffleboard.getBool("Swerve", "Swap", false)) + .onTrue(new InstantCommand(() -> drivetrain.swap(driver, coPilot))) + .onFalse(new InstantCommand(() -> drivetrain.swap(driver, coPilot))); + + // BLUE Alliance set + new Trigger(() -> LightningShuffleboard.getBool("Auton", "POSE BLUE A", false)) + .onTrue(new InstantCommand( + () -> drivetrain.setDrivetrainPose(AutonomousConstants.SOURCE_SUB_A_STARTPOSE_BLUE))); + new Trigger(() -> LightningShuffleboard.getBool("Auton", "POSE BLUE B", false)) + .onTrue(new InstantCommand( + () -> drivetrain.setDrivetrainPose(AutonomousConstants.SOURCE_SUB_B_STARTPOSE_BLUE))); + new Trigger(() -> LightningShuffleboard.getBool("Auton", "POSE BLUE C", false)) + .onTrue(new InstantCommand( + () -> drivetrain.setDrivetrainPose(AutonomousConstants.SOURCE_SUB_C_STARTPOSE_BLUE))); + + // BLUE Alliance set + new Trigger(() -> LightningShuffleboard.getBool("Auton", "POSE RED A", false)) + .onTrue(new InstantCommand( + () -> drivetrain.setDrivetrainPose(AutonomousConstants.SOURCE_SUB_A_STARTPOSE_RED))); + new Trigger(() -> LightningShuffleboard.getBool("Auton", "POSE RED B", false)) + .onTrue(new InstantCommand( + () -> drivetrain.setDrivetrainPose(AutonomousConstants.SOURCE_SUB_B_STARTPOSE_RED))); + new Trigger(() -> LightningShuffleboard.getBool("Auton", "POSE RED C", false)) + .onTrue(new InstantCommand( + () -> drivetrain.setDrivetrainPose(AutonomousConstants.SOURCE_SUB_C_STARTPOSE_RED))); + } + + @Override + protected void configureDefaultCommands() { + /* driver */ + drivetrain.registerTelemetry(logger::telemeterize); + + drivetrain.setDefaultCommand(drivetrain.applyPercentRequestField( + () -> -driver.getLeftY(), () -> -driver.getLeftX(), () -> -driver.getRightX())); + // .alongWith(new CollisionDetection(drivetrain, CollisionType.TELEOP))); + + /* copilot */ + collector.setDefaultCommand( + new Collect(() -> MathUtil.applyDeadband( + (coPilot.getRightTriggerAxis() - coPilot.getLeftTriggerAxis()), + ControllerConstants.DEADBAND), collector)); + + // climber.setDefaultCommand( + // new SmartClimb(climber, drivetrain, pivot, leds, () -> -coPilot.getLeftY(), + // () -> -coPilot.getRightY(), + // coPilot::getYButton).deadlineWith(leds.enableState(LED_STATES.CLIMBING))); + + if (!Constants.isMercury()) { + climber.setDefaultCommand(new ManualClimb(() -> -coPilot.getLeftY(), () -> -coPilot.getRightY(), climber)); + } + } + + protected Command getAutonomousCommand() { + return autoChooser.getSelected(); + } + + @Override + protected void releaseDefaultCommands() { + } + + @Override + protected void initializeDashboardCommands() { + } + + @Override + protected void configureFaultCodes() { + } + + @Override + protected void configureFaultMonitors() { + } + + @Override + protected void configureSystemTests() { + SystemTest.registerTest("Drive Test", new DrivetrainSystemTest(drivetrain, + DrivetrainConstants.SYS_TEST_SPEED_DRIVE)); // to be tested + SystemTest.registerTest("Azimuth Test", new TurnSystemTest(drivetrain, + DrivetrainConstants.SYS_TEST_SPEED_TURN)); + + // SystemTest.registerTest("Single Note Cycle", new CycleSytemTest(collector, + // indexer, pivot, flywheel, () -> 0.5d, () -> 0.6d, () -> 250)); + + SystemTest.registerTest("Collector Test", new CollectorSystemTest(collector, + Constants.CollectorConstants.COLLECTOR_SYSTEST_POWER)); + + // SystemTest.registerTest("Pivot 90 Degrees", new PivotAngleTest(pivot, + // Constants.PivotConstants.PIVOT_SYSTEST_ANGLE)); + + SystemTest.registerTest("Flywheel Test", new FlywheelSystemTest(flywheel, collector, + indexer, pivot, Constants.FlywheelConstants.FLYWHEEL_SYSTEST_RPM)); + + SystemTest.registerTest("Indexer Test", new IndexerSystemTest(indexer, + Constants.IndexerConstants.INDEXER_SYSTEST_POWER)); + + // SystemTest.registerTest("Climb Test", new ClimbSystemTest(climber, + // Constants.ClimbConstants.CLIMB_SYSTEST_POWER)); + + // Sing chooser SendableChooser songChooser = new + // SendableChooser<>(); + // songChooser.setDefaultOption(MusicConstants.BOH_RHAP_FILEPATH, + // new SingSystemTest(drivetrain, MusicConstants.BOH_RHAP_FILEPATH, sing)); + // for (String filepath : MusicConstants.SET_LIST) { + // songChooser.addOption(filepath, new SingSystemTest(drivetrain, filepath, + // sing)); + // } + + // LightningShuffleboard.set("SystemTest", "Songs List", songChooser); + + // songChooser.onChange((SystemTestCommand command) -> { + // if (command != null) { + // command.schedule(); + // } + // }); + + // songChooser.close(); + } + + public static Command hapticDriverCommand() { + if (!DriverStation.isAutonomous()) { + return new StartEndCommand(() -> { + driver.setRumble(GenericHID.RumbleType.kRightRumble, 1d); + driver.setRumble(GenericHID.RumbleType.kLeftRumble, 1d); + }, () -> { + driver.setRumble(GenericHID.RumbleType.kRightRumble, 0); + driver.setRumble(GenericHID.RumbleType.kLeftRumble, 0); + }); + } else { + return new InstantCommand(); + } + } + + public static Command hapticCopilotCommand() { + if (!DriverStation.isAutonomous()) { + return new StartEndCommand(() -> { + coPilot.setRumble(GenericHID.RumbleType.kRightRumble, 1d); + coPilot.setRumble(GenericHID.RumbleType.kLeftRumble, 1d); + }, () -> { + coPilot.setRumble(GenericHID.RumbleType.kRightRumble, 0); + coPilot.setRumble(GenericHID.RumbleType.kLeftRumble, 0); + }); + } else { + return new InstantCommand(); + } + } } diff --git a/src/main/java/frc/robot/command/AutonSmartCollect.java b/src/main/java/frc/robot/command/AutonSmartCollect.java index 7dff0fdc..8dee0f56 100644 --- a/src/main/java/frc/robot/command/AutonSmartCollect.java +++ b/src/main/java/frc/robot/command/AutonSmartCollect.java @@ -25,8 +25,7 @@ public class AutonSmartCollect extends Command { * @param collector subsystem * @param indexer subsystem */ - public AutonSmartCollect(DoubleSupplier collectorPower, DoubleSupplier indexerPower, Collector collector, - Indexer indexer) { + public AutonSmartCollect(DoubleSupplier collectorPower, DoubleSupplier indexerPower, Collector collector, Indexer indexer) { this.collectorPower = collectorPower; this.indexerPower = indexerPower; diff --git a/src/main/java/frc/robot/command/ChasePieces.java b/src/main/java/frc/robot/command/ChasePieces.java index 732b44a9..9631ce6d 100644 --- a/src/main/java/frc/robot/command/ChasePieces.java +++ b/src/main/java/frc/robot/command/ChasePieces.java @@ -236,6 +236,7 @@ public boolean trustValues() { @Override public boolean isFinished() { + // return true; if (DriverStation.isAutonomous()) { if (DriverStation.getAlliance().get() == Alliance.Blue) { if (drivetrain.getPose().getX() > AutonomousConstants.CHASE_BOUNDARY) { diff --git a/src/main/java/frc/robot/subsystems/Collector.java b/src/main/java/frc/robot/subsystems/Collector.java index 61589bef..bba258cf 100644 --- a/src/main/java/frc/robot/subsystems/Collector.java +++ b/src/main/java/frc/robot/subsystems/Collector.java @@ -73,7 +73,7 @@ private void initLogging() { */ public boolean getEntryBeamBreakState() { if (Constants.isMercury()) { - return entryDebouncer.calculate(beamBreak.get()); + return entryDebouncer.calculate(!beamBreak.get()); } return entryDebouncer.calculate(beamBreak.get()); }