diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index b710a59a..1e1ce814 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -65,19 +65,7 @@ public void disabledInit() { public void disabledPeriodic() { } - /** - * This autonomous runs the autonomous command selected by your - * {@link RobotContainer} class. - */ - @Override - public void autonomousInit() { - autonomousCommand = robotContainer.getAutonomousCommand(); - - // schedule the autonomous command (example) - if (autonomousCommand != null) { - autonomousCommand.schedule(); - } - } + /** This function is called periodically during autonomous. */ @Override diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6954181f..ecd2fee2 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -10,13 +10,7 @@ import java.util.EnumSet; -import com.choreo.lib.ChoreoTrajectory; import com.fasterxml.jackson.databind.util.Named; -import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.auto.NamedCommands; -import com.pathplanner.lib.commands.PathPlannerAuto; -import com.pathplanner.lib.path.PathPlannerPath; -import com.pathplanner.lib.util.PathPlannerLogging; import edu.wpi.first.cscore.MjpegServer; import edu.wpi.first.cscore.UsbCamera; @@ -151,8 +145,6 @@ public class RobotContainer { private UsbCamera driverCamera; private MjpegServer driverCameraServer; - ChoreoTrajectory trajectory; - private PIDController xPID; private PIDController yPID; @@ -184,14 +176,6 @@ public RobotContainer() { autonTable = ntInstance.getTable("Auton"); listEntry = autonTable.getEntry("AutonList"); selectedAutonEntry = autonTable.getEntry("Selected"); - // System.out.print("Available Autons: " + AutoBuilder.getAllAutoNames().toArray(new String[0])); - listEntry.setStringArray(AutoBuilder.getAllAutoNames().toArray(new String[0])); - selectedAutonEntry.setString(autonValue); - autonTable.addListener("Auton", EnumSet.of(NetworkTableEvent.Kind.kValueAll), (table, key, event) -> { - this.autonValue = event.valueData.value.getString(); - this.selectedAutonEntry.setString(this.autonValue); - System.out.print("New auton value: " + this.autonValue); - }); fmsSubsystem = new FieldManagementSubsystem(); lightBarSubsystem = new LightBarSubsystem(); superstructureSubsystem = new SuperstructureSubsystem(lightBarSubsystem, fmsSubsystem); @@ -271,46 +255,6 @@ public RobotContainer() { swerveCrauton.add(autonPathChooser); - NamedCommands.registerCommand("Shoot", new ParallelDeadlineGroup( - new SequentialCommandGroup( - new WaitCommand(.05), - new ConditionalWaitCommand(swerveSubsystem::atTargetAngle).withTimeout(1), - new IntakeRollerFeedCommand(intakeRollerSubsystem, 1).until(intakeRollerSubsystem::getRockwellSensorValue), - new IntakeRollerFeedCommand(intakeRollerSubsystem, 1).until(() -> - !intakeRollerSubsystem.getRockwellSensorValue() - && - !intakeRollerSubsystem.getAmpSensor() - && - !intakeRollerSubsystem.getFrontSensorValue()), - new IntakeRollerFeedCommand(intakeRollerSubsystem, 1).withTimeout(.3) - ), - new SwerveAimCommand(swerveSubsystem) - )); - - NamedCommands.registerCommand("StationaryShoot", new ParallelDeadlineGroup( - new SequentialCommandGroup( - new ConditionalWaitCommand(shooterFlywheelSubsystem::atSpeed), - new ConditionalWaitCommand(swerveSubsystem::atTargetAngle), - new IntakeRollerFeedCommand(intakeRollerSubsystem, 1).until(() -> !intakeRollerSubsystem.getRockwellSensorValue()), - new IntakeRollerFeedCommand(intakeRollerSubsystem, 1).withTimeout(.3) - ), - new SwerveAimCommand(swerveSubsystem) - )); - - NamedCommands.registerCommand( - "Intake", - new IntakeRollerAmpIntakeCommand(intakeRollerSubsystem).andThen(new IntakeRollerIntakeCommand(intakeRollerSubsystem, lightBarSubsystem)) - ); - - NamedCommands.registerCommand("AutoIntake", - new NoteAlignCommand(swerveSubsystem, noteDetector, driveController).until(intakeRollerSubsystem::getAmpSensor).alongWith( - new IntakeRollerAmpIntakeCommand(intakeRollerSubsystem) - ) - ); - NamedCommands.registerCommand("Feed", - new IntakeRollerFeedCommand(intakeRollerSubsystem).until(intakeRollerSubsystem::getRockwellSensorValue) - ); - swerveSubsystem.targetSpeaker(); configureBindings(); @@ -349,169 +293,9 @@ private void configureBindings() { swerveSubsystem.resetDriverHeading(); })); - /* SWERVE BINDINGS */ - - /* Shooter Aim -- Holding down the button will change the shooter's pitch to aim it at the speaker. */ - // drive - - /* Automatic Amping -- Pressing and holding the button will cause the robot to automatically path find to the - * amp and deposit its note. Releasing the button will stop the robot (and the path finding). */ - // TODO: Bring back LED integration. - driveController.getAutoAmp().whileTrue( - Commands.deferredProxy(() -> new AutoAmpSequence( - fmsSubsystem, - swerveSubsystem, - elevatorSubsystem, - intakePivotSubsystem, - intakeRollerSubsystem - )) - ); - - // DEPRECATED AMP ALIGN - // driveController.getAmpAlign().onTrue(new InstantCommand( - // () -> lightBarSubsystem.setLightBarStatus(LightBarStatus.AUTO_ALIGN, 1) - // ).andThen(new ParallelRaceGroup( - // AlignCommand.getAmpAlignCommand(swerveSubsystem, fmsSubsystem.isRedAlliance()), - // new ConditionalWaitCommand( - // () -> !driveController.getAmpAlign().getAsBoolean())) - // ).andThen( - // new InstantCommand(() -> lightBarSubsystem.setLightBarStatus(LightBarStatus.DORMANT, 1)) - // ) - // ); - - /* Stage Align -- Pressing and holding the button will cause the robot to automatically pathfind such that its - * climb hooks will end up directly above the center of the nearest chain. */ - driveController.getStageAlignButton().whileTrue( - Commands.deferredProxy(() -> AlignCommand.getStageAlignCommand( - swerveSubsystem, - swerveSubsystem::getRobotPosition, - fmsSubsystem::isRedAlliance - )) - ); - - - /* Note align -- Auto-intakes the nearest visible note, leaving left power control to the driver. */ - driveController.getNoteAlign().onTrue( - new AutoIntakeSequence(intakeRollerSubsystem, intakePivotSubsystem, - swerveSubsystem, noteDetector, - driveController, lightBarSubsystem) - .andThen(new IntakePivotSetPositionCommand(intakePivotSubsystem, 0)) - .onlyWhile(driveController.getNoteAlign()) - ); - /* Swerve Stop -- Pressing the button completely stops the robot's motion. */ driveController.getSwerveStop().onTrue(new SwerveStopCommand(swerveSubsystem)); - driveController.getShooterAimButton().onTrue(Commands.runOnce( - () -> { - swerveSubsystem.setTargetPoint(fmsSubsystem.isRedAlliance() ? SwerveConstants.RED_SPEAKER_POS : SwerveConstants.BLUE_SPEAKER_POS); - swerveSubsystem.setAim(true); - } - )); - - driveController.getShooterAimButton().onFalse(Commands.runOnce( - () -> { - swerveSubsystem.setAim(false); - } - )); - - - - /* SHOOTER PIVOT TEST */ - - // rightBumper.onTrue(new ShooterPivotSetAngleCommand(shooterPivotSubsystem, - // Units.degreesToRadians(18))); - - // leftBumper.onTrue(new ShooterPivotSetAngleCommand(shooterPivotSubsystem, - // Units.degreesToRadians(60))); - - /* SHOOTER PIVOT TUNE */ - - shooterPivotSubsystem.setDefaultCommand(new InstantCommand(() -> { - //TODO: switch to enum for dpad angles - - switch (mechController.getPOV()) { - case 0: - shooterPivotSetPosition += .003; - break; - - case 180: - shooterPivotSetPosition -= .003; - break; - - case 90: - shooterSpeed += .001; - break; - - case 270: - shooterSpeed -= .001; - break; - - default: - break; - } - - // System.out.println(intakePosition); - - - - // System.out.print(" Speed: " + GRTUtil.twoDecimals(shooterSpeed) - // ); - - // shooterPivotSubsystem.getAutoAimAngle(); - // shooterPivotSubsystem.setAngle(shooterPivotSetPosition); - }, shooterPivotSubsystem)); - - /* ElEVATOR TEST */ - - dPadUp.onTrue(new ElevatorToTrapCommand(elevatorSubsystem)); - dPadUp.onTrue(new InstantCommand(() -> intakePivotSubsystem.enablePowerLimit(false))); - dPadDown.onTrue(new ElevatorToZeroCommand(elevatorSubsystem)); - dPadDown.onTrue(new InstantCommand(() -> intakePivotSubsystem.enablePowerLimit(true))); - - - dPadLeft.onTrue(new ElevatorToMidCommand(elevatorSubsystem)); - dPadLeft.onFalse(new ElevatorToZeroCommand(elevatorSubsystem)); - // elevatorSubsystem.setManual(); - - // elevatorSubsystem.setDefaultCommand(new InstantCommand(() -> { - // if (mechController.getPOV() == 0) { - // elevatorSubsystem.setTargetState(ElevatorState.TRAP); - // // elevatorSubsystem.setMotorPower(0.1); - // } else if (mechController.getPOV() == 180) { - // elevatorSubsystem.setTargetState(ElevatorState.ZERO); - // // elevatorSubsystem.setMotorPower(-0.1); - // } - // else{ - // // elevatorSubsystem.setMotorPower(0); - // } - // }, elevatorSubsystem)); - - elevatorToZero.onTrue(new ElevatorToLimitSwitchCommand(elevatorSubsystem)); - /* INTAKE TEST */ - - // xButton.onTrue(new InstantCommand(() -> intakePivotSubsystem.setPosition(.3), - // intakePivotSubsystem)); - - // rightBumper.onTrue(new InstantCommand(() -> - // intakePivotSubsystem.setPosition(0), intakePivotSubsystem)); - - // leftBumper.onTrue(new InstantCommand(() -> - // intakePivotSubsystem.setPosition(.85), intakePivotSubsystem)); - - /* MECHANISM BINDINGS */ - - /* Climb Controls -- Left and right joystick up/down controls left and right arm up/down, respectively. Pressing - * down on either the left or right joystick toggles automatic lowering (on by default), which brings down both - * arms automatically when there is no joystick input. */ - climbSubsystem.setDefaultCommand(Commands.run( - () -> climbSubsystem.setSpeeds(-mechController.getLeftY(), -mechController.getRightY()), climbSubsystem - )); - - leftStickButton.onTrue(Commands.runOnce(() -> climbSubsystem.toggleAutomaticLowering(), climbSubsystem)); - rightStickButton.onTrue(Commands.runOnce(() -> climbSubsystem.toggleAutomaticLowering(), climbSubsystem)); - - // rightBumper toggles the amp sequence // if the elevator is up, lower it and stow the intake // if the elevator is down, run the amp sequence @@ -536,50 +320,6 @@ private void configureBindings() { ) ); - // leftBumper toggles the trap position for the elevator - leftBumper.onTrue( - new ConditionalCommand( - new ElevatorToZeroCommand(elevatorSubsystem).alongWith(new InstantCommand(// lower the elevator - () -> { - intakePivotSubsystem.enablePowerLimit(true); - intakePivotSubsystem.setPosition(0); - }, intakePivotSubsystem)), // stow intake - new ConditionalCommand( - new ElevatorToTrapCommand(elevatorSubsystem).andThen( - new InstantCommand(() -> { - intakePivotSubsystem.enablePowerLimit(false); - intakePivotSubsystem.setPosition(.45); - }) - ), - new IntakePivotSetPositionCommand(intakePivotSubsystem, 1).andThen(// extend pivot - new ConditionalWaitCommand(intakeRollerSubsystem::getFrontSensorReached), - new InstantCommand(() -> intakePivotSubsystem.setPosition(0)) - ), - intakeRollerSubsystem::getAmpSensor - ), // raise the elevator - () -> (elevatorSubsystem.getTargetState() == ElevatorState.AMP // check if targeting a high pos - || elevatorSubsystem.getTargetState() == ElevatorState.TRAP) && elevatorSubsystem.getExtensionPercent() > .5) - ); - - - // roll behaviors - leftBumper.onTrue( - new ConditionalCommand( - new InstantCommand(), - new ConditionalCommand( - new InstantCommand(), - new WaitCommand(.05).andThen( - new ConditionalWaitCommand(intakePivotSubsystem::atPosition), // extend pivot - new IntakeRollerOuttakeCommand(intakeRollerSubsystem, .17, .75) // run rollers to front sensor - .until(intakeRollerSubsystem::getFrontSensorReached) - ), - intakeRollerSubsystem::getAmpSensor - ), // raise the elevator - () -> elevatorSubsystem.getTargetState() == ElevatorState.AMP // check if targeting a high pos - || elevatorSubsystem.getTargetState() == ElevatorState.TRAP) - ); - - // aButton runs the intake sequence aButton.onTrue( @@ -719,34 +459,5 @@ private void configureBindings() { intakeRollerSubsystem.setRollSpeeds(power, power); }, intakeRollerSubsystem)); - // Offset buttons to correct the shooter if needed - offsetUpButton.onTrue(new InstantCommand( - () -> shooterPivotSubsystem.setAngleOffset(Units.degreesToRadians(3))) - ); - offsetUpButton.onFalse(new InstantCommand( - () -> shooterPivotSubsystem.setAngleOffset(Units.degreesToRadians(0))) - ); - - offsetDownButton.onTrue(new InstantCommand( - () -> shooterPivotSubsystem.setAngleOffset(Units.degreesToRadians(-3))) - ); - offsetDownButton.onFalse(new InstantCommand( - () -> shooterPivotSubsystem.setAngleOffset(Units.degreesToRadians(0))) - ); - - } - - /** - * Returns the autonomous command. - * - * @return The selected autonomous command. - */ - public Command getAutonomousCommand() { - return AutoBuilder.buildAuto(autonValue).alongWith( - new ShooterFlywheelReadyCommand(shooterFlywheelSubsystem, lightBarSubsystem), - new InstantCommand(() -> shooterPivotSubsystem.setAutoAimBoolean(true)), - new IntakePivotSetPositionCommand(intakePivotSubsystem, 1), - new ClimbLowerCommand(climbSubsystem) - ); } } \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/auton/AutoAmpSequence.java b/src/main/java/frc/robot/commands/auton/AutoAmpSequence.java deleted file mode 100644 index 08c2bb17..00000000 --- a/src/main/java/frc/robot/commands/auton/AutoAmpSequence.java +++ /dev/null @@ -1,46 +0,0 @@ -package frc.robot.commands.auton; - -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import frc.robot.commands.elevator.ElevatorToZeroCommand; -import frc.robot.commands.intake.pivot.IntakePivotSetPositionCommand; -import frc.robot.commands.intake.roller.IntakeRollerOuttakeCommand; -import frc.robot.commands.sequences.PrepareAmpSequence; -import frc.robot.commands.swerve.AlignCommand; -import frc.robot.subsystems.FieldManagementSubsystem; -import frc.robot.subsystems.elevator.ElevatorSubsystem; -import frc.robot.subsystems.intake.IntakePivotSubsystem; -import frc.robot.subsystems.intake.IntakeRollerSubsystem; -import frc.robot.subsystems.swerve.SwerveSubsystem; - -/** - * Automatically runs an amp cycle including preparing the note, going to and aligning with the amp, and depositing the - * note (given that the robot already has a note). - */ -public class AutoAmpSequence extends SequentialCommandGroup { - - /** Constructs a new {@link AutoAmpSequence}. */ - public AutoAmpSequence(FieldManagementSubsystem fms, - SwerveSubsystem swerveSubsystem, - ElevatorSubsystem elevatorSubsystem, - IntakePivotSubsystem intakePivotSubsystem, - IntakeRollerSubsystem intakeRollerSubsystem) { - - addCommands( - /* Prepares the mechanisms for amping while path-finding the robot to its amping position. */ - Commands.parallel( - new PrepareAmpSequence(elevatorSubsystem, intakePivotSubsystem, intakeRollerSubsystem), - AlignCommand.getAmpAlignCommand(swerveSubsystem, fms::isRedAlliance) - ), - - /* Deposits the note into the amp. */ - new IntakeRollerOuttakeCommand(intakeRollerSubsystem).withTimeout(1), - - /* Retracts the intake and lowers the elevator, resetting the robot to its stowed state.*/ - Commands.parallel( - new IntakePivotSetPositionCommand(intakePivotSubsystem, 0), - new ElevatorToZeroCommand(elevatorSubsystem) - ) - ); - } -} diff --git a/src/main/java/frc/robot/commands/auton/AutonBuilder.java b/src/main/java/frc/robot/commands/auton/AutonBuilder.java deleted file mode 100644 index 39164ae8..00000000 --- a/src/main/java/frc/robot/commands/auton/AutonBuilder.java +++ /dev/null @@ -1,472 +0,0 @@ -package frc.robot.commands.auton; - -import com.choreo.lib.Choreo; -import com.choreo.lib.ChoreoTrajectory; -import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.path.PathConstraints; -import edu.wpi.first.math.controller.PIDController; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Transform2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.math.kinematics.ChassisSpeeds; -import edu.wpi.first.math.util.Units; -import edu.wpi.first.wpilibj2.command.Command; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.WaitCommand; -import frc.robot.Constants.AutonConstants; -import frc.robot.commands.elevator.ElevatorToEncoderZeroCommand; -import frc.robot.commands.intake.pivot.IntakePivotSetPositionCommand; -import frc.robot.commands.intake.roller.IntakeRollerFeedCommand; -import frc.robot.commands.intake.roller.IntakeRollerIntakeCommand; -import frc.robot.commands.shooter.flywheel.ShooterFlywheelReadyCommand; -import frc.robot.commands.shooter.flywheel.ShooterFlywheelStopCommand; -import frc.robot.commands.shooter.pivot.ShooterPivotAimCommand; -import frc.robot.subsystems.FieldManagementSubsystem; -import frc.robot.subsystems.climb.ClimbSubsystem; -import frc.robot.subsystems.elevator.ElevatorSubsystem; -import frc.robot.subsystems.intake.IntakePivotSubsystem; -import frc.robot.subsystems.intake.IntakeRollerSubsystem; -import frc.robot.subsystems.leds.LightBarSubsystem; -import frc.robot.subsystems.shooter.ShooterFlywheelSubsystem; -import frc.robot.subsystems.shooter.ShooterPivotSubsystem; -import frc.robot.subsystems.swerve.SwerveSubsystem; -import frc.robot.util.GRTUtil; - - -/** - * The base autonomous sequence that other autons extend. This class provides functions that abstract shared tasks - * between autons. - */ -public class AutonBuilder { - - private final IntakePivotSubsystem intakePivotSubsystem; - private final IntakeRollerSubsystem intakeRollerSubsystem; - private final ShooterPivotSubsystem shooterPivotSubsystem; - private final ShooterFlywheelSubsystem shooterFlywheelSubsystem; - private final ElevatorSubsystem elevatorSubsystem; - private final ClimbSubsystem climbSubsystem; - private final SwerveSubsystem swerveSubsystem; - private final LightBarSubsystem lightBarSubsystem; - private final FieldManagementSubsystem fmsSubsystem; - private final PIDController thetaController; - private final PIDController xPID; - private final PIDController yPID; - - /** Constructs a {@link AutonBuilder} with auton-abstracted functions. */ - public AutonBuilder(IntakePivotSubsystem intakePivotSubsystem, - IntakeRollerSubsystem intakeRollersSubsystem, - ShooterFlywheelSubsystem shooterFlywheelSubsystem, - ShooterPivotSubsystem shooterPivotSubsystem, - ElevatorSubsystem elevatorSubsystem, - ClimbSubsystem climbSubsystem, - SwerveSubsystem swerveSubsystem, - LightBarSubsystem lightBarSubsystem, - FieldManagementSubsystem fmsSubsystem) { - this.intakePivotSubsystem = intakePivotSubsystem; - this.intakeRollerSubsystem = intakeRollersSubsystem; - this.shooterFlywheelSubsystem = shooterFlywheelSubsystem; - this.shooterPivotSubsystem = shooterPivotSubsystem; - this.elevatorSubsystem = elevatorSubsystem; - this.climbSubsystem = climbSubsystem; - this.swerveSubsystem = swerveSubsystem; - this.lightBarSubsystem = lightBarSubsystem; - this.fmsSubsystem = fmsSubsystem; - - xPID = new PIDController(4, 0, 0); - yPID = new PIDController(4, 0, 0); - thetaController = new PIDController(3.5, 0, 0); - } - - /** - * Follows trajectory. - * - * @param trajectory ChoreoTrajectory - * @return followPath command - */ - public Command followPath(ChoreoTrajectory trajectory) { - return Choreo.choreoSwerveCommand( - trajectory, - swerveSubsystem::getRobotPosition, - xPID, - yPID, - thetaController, - ((ChassisSpeeds speeds) -> { - swerveSubsystem.setChassisSpeeds( - speeds.vxMetersPerSecond, - speeds.vyMetersPerSecond, - speeds.omegaRadiansPerSecond - ); - }), - fmsSubsystem::isRedAlliance, - swerveSubsystem - ); - } - - /** - * Follows trajectory to intake. - * - * @param intakeTrajectory ChoreoTrajectory - * @return goIntake Command - */ - public Command goIntake(ChoreoTrajectory intakeTrajectory) { - return followPath(intakeTrajectory).alongWith( - //new IntakePivotSetPositionCommand(intakePivotSubsystem, 1) - ).andThen( - new IntakeRollerIntakeCommand(intakeRollerSubsystem, lightBarSubsystem) - .alongWith(new DriveForwardCommand(swerveSubsystem).until(intakeRollerSubsystem::getFrontSensorValue).until(intakeRollerSubsystem::getRockwellSensorValue).withTimeout(1)) - ); - } - - public Command goAndIntake(ChoreoTrajectory intakeTrajectory) { - return followPath(intakeTrajectory).alongWith( - new IntakeRollerIntakeCommand(intakeRollerSubsystem, lightBarSubsystem) - ); - } - - /** - * Shoots at calculated robot angle and shooter angle. - * - * @return shoot command - */ - public Command shoot() { - return new ShooterPivotAimCommand(shooterPivotSubsystem) - .alongWith(new SwerveAimCommand(swerveSubsystem)).withTimeout(1) - .andThen(new IntakeRollerFeedCommand(intakeRollerSubsystem).until( - () -> !intakeRollerSubsystem.getRockwellSensorValue()) - .andThen(new IntakeRollerFeedCommand(intakeRollerSubsystem)).withTimeout(.5) - ); - } - - /** - * Follows trajectory and then shoots at calculated robot angle and shooter angle. - * - * @param shootTrajectory ChoreoTrajectory - * @return goShoot command - */ - public SequentialCommandGroup goShoot(ChoreoTrajectory shootTrajectory) { - return followPath(shootTrajectory) - .andThen(shoot()); - } - - /** - * Resets the swerve pose estimation to a newPose. - * - * @param newPose The pose to reset pose estimation to. - * @return The command to reset the pose. - */ - public InstantCommand resetSwerve(Pose2d newPose) { - return new InstantCommand(() -> swerveSubsystem.resetPose(newPose), swerveSubsystem); - } - - /** - * Wraps auton commands with common auton commands. - * - * @param initPose The initial pose to reset swerve to. - * @param commands The commands to run during auton. - * @return The full auton sequence. - */ - public SequentialCommandGroup buildAuton(Pose2d initPose, Command... commands) { - SequentialCommandGroup autonSequence = new SequentialCommandGroup(); - - autonSequence.addCommands( - resetSwerve(GRTUtil.mirrorAcrossField(initPose, fmsSubsystem::isRedAlliance)), - new ShooterFlywheelReadyCommand(shooterFlywheelSubsystem, lightBarSubsystem).alongWith( - new SwerveAimCommand(swerveSubsystem), - new ShooterPivotAimCommand(shooterPivotSubsystem), - new IntakePivotSetPositionCommand(intakePivotSubsystem, 1) - ).withTimeout(2) - ); - autonSequence.addCommands(commands); - autonSequence.addCommands( - new WaitCommand(2), - new ShooterFlywheelStopCommand(shooterFlywheelSubsystem) - ); - - return autonSequence; - } - - // starts furthest away from amp in SPECIAL START POSITION, sweeps center notes starting furthest away from amp - public SequentialCommandGroup getBottomBottomCenterDistruptor() { - ChoreoTrajectory trajectory = Choreo.getTrajectory("BottomBottomCenterDisruptor"); - return new SequentialCommandGroup( - resetSwerve(GRTUtil.mirrorAcrossField(trajectory.getInitialPose(), fmsSubsystem::isRedAlliance)), - followPath(trajectory)); - } - - // starts furthest away from amp in SPECIAL START POSITION, sweeps center notes starting closest to amp - public SequentialCommandGroup getBottomTopCenterDistruptor() { - ChoreoTrajectory trajectory = Choreo.getTrajectory("BottomTopCenterDistruptor"); - return new SequentialCommandGroup( - resetSwerve(GRTUtil.mirrorAcrossField(trajectory.getInitialPose(), fmsSubsystem::isRedAlliance)), - new InstantCommand(() -> climbSubsystem.setSpeeds(-0.2, -0.2)), - followPath(trajectory)); - } - - /** Starts amp side and shoots the preloaded note. */ - public SequentialCommandGroup getTopPreloaded() { - return buildAuton(AutonConstants.TOP_START_POSE, shoot()); - } - - /** Starts in front of subwoofer and shoots preloaded note. */ - public SequentialCommandGroup getMiddlePreloaded() { - return buildAuton(AutonConstants.MIDDLE_START_POSE, shoot()); - } - - /** Starts source side and shoots preloaded note. */ - public SequentialCommandGroup getBottomPreloaded() { - return buildAuton(AutonConstants.BOTTOM_START_POSE, shoot()); - } - - /** Starts amp side. Shoots preloaded note, intakes top note, shoots note. */ - public SequentialCommandGroup getTopTwoPiece() { - - ChoreoTrajectory startToPiece1 = Choreo.getTrajectory("T1-OffsetAmpStartToAmpNote"); - - return buildAuton( - new Pose2d(startToPiece1.getInitialPose().getTranslation(), new Rotation2d()), - shoot(), - goIntake(startToPiece1), - shoot() - ); - } - - public SequentialCommandGroup getTopThreePiece() { - - ChoreoTrajectory startToPiece1 = Choreo.getTrajectory("T1-OffsetAmpStartToAmpNote"); - ChoreoTrajectory piece1ToPiece2 = Choreo.getTrajectory("T2-AmpNoteToSpeakerNote"); - - return buildAuton( - new Pose2d(startToPiece1.getInitialPose().getTranslation(), new Rotation2d()), - shoot(), - goIntake(startToPiece1), - shoot(), - goIntake(piece1ToPiece2), - shoot() - ); - } - - public SequentialCommandGroup getTopFourPiece() { - - ChoreoTrajectory startToPiece1 = Choreo.getTrajectory("T1-OffsetAmpStartToAmpNote"); - ChoreoTrajectory piece1ToPiece2 = Choreo.getTrajectory("T2-AmpNoteToSpeakerNote"); - ChoreoTrajectory piece2ToPiece3 = Choreo.getTrajectory("T3-SpeakerNoteToBottomNote"); - - return buildAuton( - new Pose2d(startToPiece1.getInitialPose().getTranslation(), new Rotation2d()), - shoot(), - goIntake(startToPiece1), - shoot(), - goIntake(piece1ToPiece2), - shoot(), - goIntake(piece2ToPiece3), - shoot() - ); - - } - - public SequentialCommandGroup getTopTwoPieceThenCenter2() { - - ChoreoTrajectory startToPiece1 = Choreo.getTrajectory("T1-OffsetAmpStartToAmpNote"); - ChoreoTrajectory piece1ToPiece2 = Choreo.getTrajectory("T2-AmpNoteToSpeakerNote"); - ChoreoTrajectory piece2ToPiece3 = Choreo.getTrajectory("Z1M2-SpeakerNoteToCenter1"); - ChoreoTrajectory piece3ToWing = Choreo.getTrajectory("Z2-Center1ToWing"); - ChoreoTrajectory wingToPiece4 = Choreo.getTrajectory("Z3-WingToCenter2"); - ChoreoTrajectory piece4ToWing = Choreo.getTrajectory("Z4-Center2ToWing"); - - return buildAuton( - new Pose2d(startToPiece1.getInitialPose().getTranslation(), new Rotation2d()), - shoot(), - goIntake(startToPiece1), - shoot(), - goIntake(piece1ToPiece2), - shoot(), - goIntake(piece2ToPiece3), - goShoot(piece3ToWing), - goIntake(wingToPiece4), - goShoot(piece4ToWing) - ); - - } - - public SequentialCommandGroup getTopTwoPieceThenCenter1() { - - ChoreoTrajectory startToPiece1 = Choreo.getTrajectory("T1-OffsetAmpStartToAmpNote"); - ChoreoTrajectory piece1ToPiece2 = Choreo.getTrajectory("T2-AmpNoteToSpeakerNote"); - ChoreoTrajectory piece2ToPiece3 = Choreo.getTrajectory("Z1M2-SpeakerNoteToCenter1"); - ChoreoTrajectory piece3ToWing = Choreo.getTrajectory("Z2-Center1ToWing"); - - return buildAuton( - new Pose2d(startToPiece1.getInitialPose().getTranslation(), new Rotation2d()), - shoot(), - goIntake(startToPiece1), - shoot(), - goIntake(piece1ToPiece2), - shoot(), - goIntake(piece2ToPiece3), - goShoot(piece3ToWing) - ); - - } - - public SequentialCommandGroup getTopCenterTwoPiece(){ - - ChoreoTrajectory startToPiece1 = Choreo.getTrajectory("Z1-OffsetTopToCenter1"); - ChoreoTrajectory piece1ToWing = Choreo.getTrajectory("Z2-Center1ToWing"); - ChoreoTrajectory wingToPiece2 = Choreo.getTrajectory("Z3-WingToCenter2"); - ChoreoTrajectory piece3ToWing = Choreo.getTrajectory("Z4-Center2ToWing"); - - return buildAuton( - new Pose2d(startToPiece1.getInitialPose().getTranslation(), new Rotation2d()), - shoot(), - goIntake(startToPiece1), - goShoot(piece1ToWing), - goIntake(wingToPiece2), - goShoot(piece3ToWing) - ); - - } - - public SequentialCommandGroup getMiddleCenterTwoPiece(){ - // - ChoreoTrajectory startToPiece1 = Choreo.getTrajectory("Z1-MiddleToCenter1"); - ChoreoTrajectory piece1ToWing = Choreo.getTrajectory("Z2-Center1ToWing"); - ChoreoTrajectory wingToPiece2 = Choreo.getTrajectory("Z3-WingToCenter2"); - ChoreoTrajectory piece3ToWing = Choreo.getTrajectory("Z4-Center2ToWing"); - - return buildAuton( - new Pose2d(startToPiece1.getInitialPose().getTranslation(), new Rotation2d()), - shoot(), - goIntake(startToPiece1), - goShoot(piece1ToWing), - goIntake(wingToPiece2), - goShoot(piece3ToWing) - ); - - } - - /** Starts at subwoofer. Shoots preloaded note, intakes middle note, shoots note. */ - public SequentialCommandGroup getMiddleTwoPiece() { - - ChoreoTrajectory startToPiece1 = Choreo.getTrajectory("M1-SpeakerStartToSpeakerNote"); - - return buildAuton( - new Pose2d(startToPiece1.getInitialPose().getTranslation(), new Rotation2d()), - shoot(), - goIntake(startToPiece1), - shoot() - ); - } - - /** Starts source side. Shoots preloaded note, intakes bottom note, shoots note. */ - public SequentialCommandGroup getBottomTwoPiece() { - - ChoreoTrajectory startToPiece1 = Choreo.getTrajectory("B1-OffsetBottomNoteStartToBottomNote"); - - return buildAuton( - new Pose2d(startToPiece1.getInitialPose().getTranslation(), new Rotation2d()), - shoot(), - goIntake(startToPiece1), - shoot() - ); - } - - /** - * Starts: right in front of subwoofer. Shoots preloaded note, intakes middle note, shoots, - * intakes top note, shoots. - */ - public SequentialCommandGroup getMiddleThreePiece() { - - ChoreoTrajectory startToPiece1 = Choreo.getTrajectory("M1-SpeakerStartToSpeakerNote"); - ChoreoTrajectory piece1ToPiece2 = Choreo.getTrajectory("M2-SpeakerNoteToAmpNote"); - - return buildAuton( - new Pose2d(startToPiece1.getInitialPose().getTranslation(), new Rotation2d()), - shoot(), - goIntake(startToPiece1), - shoot(), - goIntake(piece1ToPiece2), - shoot() - ); - } - - /** - * Starts: right in front of subwoofer. Shoots preloaded note, intakes middle note, shoots, - * intakes top note, shoots. - */ - public SequentialCommandGroup getMiddleFourPiece() { - - ChoreoTrajectory startToPiece1 = Choreo.getTrajectory("M1-SpeakerStartToSpeakerNote"); - ChoreoTrajectory piece1ToPiece2 = Choreo.getTrajectory("M2-SpeakerNoteToAmpNote"); - ChoreoTrajectory piece2ToPiece3 = Choreo.getTrajectory("M3-AmpToBottomNote"); - - return buildAuton( - new Pose2d(startToPiece1.getInitialPose().getTranslation(), new Rotation2d()), - shoot(), - goIntake(startToPiece1), - shoot(), - goIntake(piece1ToPiece2), - shoot(), - goIntake(piece2ToPiece3), - shoot() - ); - } - - public SequentialCommandGroup getMiddleTwoPieceThen1TopCenter(){ - - ChoreoTrajectory startToPiece1 = Choreo.getTrajectory("M1-SpeakerStartToSpeakerNote"); - ChoreoTrajectory piece1ToPiece2 = Choreo.getTrajectory("M2-SpeakerNoteToAmpNote"); - ChoreoTrajectory piece2ToPiece3 = Choreo.getTrajectory("Z1M3-AmpToCenter1"); - ChoreoTrajectory piece3ToWing = Choreo.getTrajectory("Z2-Center1ToWing"); - - return buildAuton( - new Pose2d(startToPiece1.getInitialPose().getTranslation(), new Rotation2d()), - shoot(), - goIntake(startToPiece1), - shoot(), - goIntake(piece1ToPiece2), - shoot(), - goIntake(piece2ToPiece3), - goShoot(piece3ToWing) - ); - } - - public SequentialCommandGroup getMiddleTwoPieceThen2TopCenter() { - - ChoreoTrajectory startToPiece1 = Choreo.getTrajectory("M1-SpeakerStartToSpeakerNote"); - ChoreoTrajectory piece1ToPiece2 = Choreo.getTrajectory("M2-SpeakerNoteToAmpNote"); - ChoreoTrajectory piece2ToPiece3 = Choreo.getTrajectory("Z1M3-AmpToCenter1"); - ChoreoTrajectory piece3ToWing = Choreo.getTrajectory("Z2-Center1ToWing"); - ChoreoTrajectory wingToPiece4 = Choreo.getTrajectory("Z3-WingToCenter2"); - ChoreoTrajectory piece4ToWing = Choreo.getTrajectory("Z4-Center2ToWing"); - - return buildAuton( - new Pose2d(startToPiece1.getInitialPose().getTranslation(), new Rotation2d()), - shoot(), - goIntake(startToPiece1), - shoot(), - goIntake(piece1ToPiece2), - shoot(), - goIntake(piece2ToPiece3), - goShoot(piece3ToWing), - goIntake(wingToPiece4), - goShoot(piece4ToWing) - ); - } - - /** Drives 2 meters away from the alliance wall. */ - public Command getTaxiTwoMeters() { - return AutoBuilder.pathfindToPose( - swerveSubsystem.getRobotPosition().plus( - new Transform2d(new Translation2d(fmsSubsystem.isRedAlliance() ? -2 : 2, 0), new Rotation2d()) - ), - new PathConstraints( - 2.0, 2.0, - Units.degreesToRadians(720), Units.degreesToRadians(1080) - ), - 0, - 0.0 - ); - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/auton/DriveForwardCommand.java b/src/main/java/frc/robot/commands/auton/DriveForwardCommand.java deleted file mode 100644 index cf422b3f..00000000 --- a/src/main/java/frc/robot/commands/auton/DriveForwardCommand.java +++ /dev/null @@ -1,27 +0,0 @@ -package frc.robot.commands.auton; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.Constants.AutonConstants; -import frc.robot.subsystems.swerve.SwerveSubsystem; - -/** DriveForward Command. */ -public class DriveForwardCommand extends Command { - private final SwerveSubsystem swerve; - private double xPower = AutonConstants.INTAKE_SWERVE_SPEED; - - /** Sets robot relative powers to drive forward until timed out. */ - public DriveForwardCommand(SwerveSubsystem swerve) { - this.swerve = swerve; - addRequirements(swerve); - } - - @Override - public void initialize() { - swerve.setRobotRelativeDrivePowers(xPower, 0, 0); - } - - @Override - public void end(boolean interrupted) { - swerve.setRobotRelativeDrivePowers(0, 0, 0); - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/auton/StationarySwerveAimCommand.java b/src/main/java/frc/robot/commands/auton/StationarySwerveAimCommand.java deleted file mode 100644 index 6d8e2a71..00000000 --- a/src/main/java/frc/robot/commands/auton/StationarySwerveAimCommand.java +++ /dev/null @@ -1,18 +0,0 @@ -package frc.robot.commands.auton; - -import frc.robot.subsystems.swerve.SwerveSubsystem; - -public class StationarySwerveAimCommand extends SwerveAimCommand{ - - SwerveSubsystem swerveSubsystem; - - public StationarySwerveAimCommand(SwerveSubsystem swerveSubsystem){ - super(swerveSubsystem); - this.swerveSubsystem = swerveSubsystem; - } - - @Override - public void execute() { - swerveSubsystem.setChassisSpeeds(0, 0, 0); - } -} diff --git a/src/main/java/frc/robot/commands/auton/SwerveAimCommand.java b/src/main/java/frc/robot/commands/auton/SwerveAimCommand.java deleted file mode 100644 index 33265018..00000000 --- a/src/main/java/frc/robot/commands/auton/SwerveAimCommand.java +++ /dev/null @@ -1,25 +0,0 @@ -package frc.robot.commands.auton; - -import edu.wpi.first.wpilibj2.command.Command; -import frc.robot.subsystems.swerve.SwerveSubsystem; - -/** SetCalculatedAngleCommand. */ -public class SwerveAimCommand extends Command { - private final SwerveSubsystem swerve; - - /** Sets the angle of the robot to aim at the speaker. */ - public SwerveAimCommand(SwerveSubsystem swerve) { - this.swerve = swerve; - } - - @Override - public void initialize() { - swerve.targetSpeaker(); - swerve.setAim(true); - } - - @Override - public void end(boolean interrupted) { - swerve.setAim(false); - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/swerve/AlignCommand.java b/src/main/java/frc/robot/commands/swerve/AlignCommand.java index cb9f9e84..9f0a99cc 100644 --- a/src/main/java/frc/robot/commands/swerve/AlignCommand.java +++ b/src/main/java/frc/robot/commands/swerve/AlignCommand.java @@ -7,8 +7,6 @@ import static frc.robot.Constants.AutoAlignConstants.RED_STAGE_LEFT_POSE; import static frc.robot.Constants.AutoAlignConstants.RED_STAGE_RIGHT_POSE; -import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.path.PathConstraints; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.util.Units; import edu.wpi.first.wpilibj2.command.Command; diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java index 2e2ed208..29101c25 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveSubsystem.java @@ -26,10 +26,6 @@ import static frc.robot.Constants.VisionConstants.FRONT_RIGHT_CAMERA_POSE; import com.kauailabs.navx.frc.AHRS; -import com.pathplanner.lib.auto.AutoBuilder; -import com.pathplanner.lib.util.HolonomicPathFollowerConfig; -import com.pathplanner.lib.util.PIDConstants; -import com.pathplanner.lib.util.ReplanningConfig; import edu.wpi.first.math.MatBuilder; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.Nat; @@ -187,22 +183,6 @@ public SwerveSubsystem(BooleanSupplier redSupplier) { ); // Configure AutoBuilder - AutoBuilder.configureHolonomic( - this::getRobotPosition, - this::resetPose, - this::getRobotRelativeChassisSpeeds, - this::setRobotRelativeDrivePowers, - new HolonomicPathFollowerConfig( - new PIDConstants(3.0, 0.0, 0.0), // Translation PID constants - new PIDConstants(3.0, 0.0, 0.0), // Rotation PID constants - 4.5, // Max module speed, in m/s - FL_POS.getNorm(), // Drive base radius in meters. Distance from robot center to furthest module. - new ReplanningConfig(true, true) - ), - redSupplier, - this - ); - velocityTimer.start(); } diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json deleted file mode 100644 index ff15fab4..00000000 --- a/vendordeps/PathplannerLib.json +++ /dev/null @@ -1,38 +0,0 @@ -{ - "fileName": "PathplannerLib.json", - "name": "PathplannerLib", - "version": "2024.2.3", - "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", - "frcYear": "2024", - "mavenUrls": [ - "https://3015rangerrobotics.github.io/pathplannerlib/repo" - ], - "jsonUrl": "https://3015rangerrobotics.github.io/pathplannerlib/PathplannerLib.json", - "javaDependencies": [ - { - "groupId": "com.pathplanner.lib", - "artifactId": "PathplannerLib-java", - "version": "2024.2.3" - } - ], - "jniDependencies": [], - "cppDependencies": [ - { - "groupId": "com.pathplanner.lib", - "artifactId": "PathplannerLib-cpp", - "version": "2024.2.3", - "libName": "PathplannerLib", - "headerClassifier": "headers", - "sharedLibrary": false, - "skipInvalidPlatforms": true, - "binaryPlatforms": [ - "windowsx86-64", - "linuxx86-64", - "osxuniversal", - "linuxathena", - "linuxarm32", - "linuxarm64" - ] - } - ] -} \ No newline at end of file