// 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; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; import au.grapplerobotics.LaserCan; import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.units.measure.Velocity; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Filesystem; import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.XboxController.Button; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.JoystickButton; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Constants.OperatorConstants; import frc.robot.ShuffleboardDisplay; import frc.robot.commands.swervedrive.drivebase.AbsoluteDriveAdv; import frc.robot.Constants.Intake; import frc.robot.commands.IntakeIn; import frc.robot.commands.IntakeOut; import frc.robot.commands.IntakeS; import frc.robot.commands.L1; import frc.robot.commands.L2; import frc.robot.commands.L3; import frc.robot.commands.L4; import frc.robot.subsystems.IntakeSubsystem; import frc.robot.subsystems.swervedrive.SwerveSubsystem; import frc.robot.subsystems.AlgaeSubsystem; import frc.robot.Constants.Elevator; import frc.robot.commands.ElevatorIn; import frc.robot.commands.ElevatorOut; import frc.robot.commands.IntakeAuto; import frc.robot.commands.AlgaeIn; import frc.robot.commands.AlgaeOut; import frc.robot.commands.Align; import frc.robot.Constants.Algae; import frc.robot.subsystems.ElevatorSubsystem; import frc.robot.subsystems.AlgaeArmSubsystem; import frc.robot.commands.AlgaeArmDown; import frc.robot.commands.AlgaeArmUp; import frc.robot.subsystems.swervedrive.SwerveSubsystem; import java.io.File; import java.io.WriteAbortedException; import swervelib.SwerveInputStream; import au.grapplerobotics.LaserCan; import frc.robot.LimelightHelpers; import frc.robot.commands.IntakeAuto; import frc.robot.commands.L1A; import frc.robot.commands.L2A; import frc.robot.commands.L3A; import frc.robot.commands.L4A; import frc.robot.commands.AlgaeA; import frc.robot.commands.AlgaeAU; /** * This class is where the bulk of the robot should be declared. Since Command-based is a "declarative" paradigm, very * little robot logic should actually be handled in the {@link Robot} periodic methods (other than the scheduler calls). * Instead, the structure of the robot (including subsystems, commands, and trigger mappings) should be declared here. */ @SuppressWarnings("unused") public class RobotContainer { // Replace with CommandPS4Controller or CommandJoystick if needed final CommandXboxController driverXbox = new CommandXboxController(0); final CommandXboxController commandXboxControllerXbox = new CommandXboxController(1); // The robot's subsystems and commands are defined here... private final SwerveSubsystem drivebase = new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "swerve/neo")); private SendableChooser autoChooser; private ShuffleboardDisplay shuffleboardInstance; private final IntakeSubsystem IntakeSubsystem = new IntakeSubsystem(); private final ElevatorSubsystem ElevatorSubsystem = new ElevatorSubsystem(); private final AlgaeSubsystem AlgaeSubsystem = new AlgaeSubsystem(); private final AlgaeArmSubsystem AlgaeArmSubsystem = new AlgaeArmSubsystem(); private final LimelightHelpers LimelightHelpers = new LimelightHelpers(); AbsoluteDriveAdv closedAbsoluteDriveAdv = new AbsoluteDriveAdv(drivebase, () -> -MathUtil.applyDeadband(driverXbox.getLeftY(), OperatorConstants.LEFT_Y_DEADBAND), () -> -MathUtil.applyDeadband(driverXbox.getLeftX(), OperatorConstants.DEADBAND), () -> -MathUtil.applyDeadband(driverXbox.getRightX()*-1, OperatorConstants.RIGHT_X_DEADBAND), driverXbox.getHID()::getYButtonPressed, driverXbox.getHID()::getAButtonPressed, driverXbox.getHID()::getXButtonPressed, driverXbox.getHID()::getBButtonPressed, driverXbox.getHID()::getLeftBumperButtonPressed, driverXbox.getHID()::getRightBumperButtonPressed, driverXbox.getHID()::getBackButtonPressed, driverXbox.getHID()::getStartButtonPressed); /** * * * Converts driver input into a field-relative ChassisSpeeds that is controlled by angular velocity. */ SwerveInputStream driveAngularVelocity = SwerveInputStream.of(drivebase.getSwerveDrive(), () -> driverXbox.getLeftY() * -1, () -> driverXbox.getLeftX() * -1) .withControllerRotationAxis(driverXbox::getRightX) .deadband(OperatorConstants.DEADBAND) .scaleTranslation(0.8) .allianceRelativeControl(true); /** * Clone's the angular velocity input stream and converts it to a fieldRelative input stream. */ SwerveInputStream driveDirectAngle = driveAngularVelocity.copy().withControllerHeadingAxis(driverXbox::getRightX, driverXbox::getRightY) .headingWhile(true); /** * Clone's the angular velocity input stream and converts it to a robotRelative input stream. */ SwerveInputStream driveRobotOriented = driveAngularVelocity.copy().robotRelative(true) .allianceRelativeControl(false); SwerveInputStream driveAngularVelocityKeyboard = SwerveInputStream.of(drivebase.getSwerveDrive(), () -> -driverXbox.getLeftY(), () -> -driverXbox.getLeftX()) .withControllerRotationAxis(() -> driverXbox.getRawAxis( 2)) .deadband(OperatorConstants.DEADBAND) .scaleTranslation(0.8) .allianceRelativeControl(true); // Derive the heading axis with math! SwerveInputStream driveDirectAngleKeyboard = driveAngularVelocityKeyboard.copy() .withControllerHeadingAxis(() -> Math.sin( driverXbox.getRawAxis( 2) * Math.PI) * (Math.PI * 2), () -> Math.cos( driverXbox.getRawAxis( 2) * Math.PI) * (Math.PI * 2)) .headingWhile(true); /** * The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { // Configure the trigger bindings configureBindings(); DriverStation.silenceJoystickConnectionWarning(true); } /** * Use this method to define your trigger->command mappings. Triggers can be created via the * {@link Trigger#Trigger(java.util.function.BooleanSupplier)} constructor with an arbitrary predicate, or via the * named factories in {@link edu.wpi.first.wpilibj2.command.button.CommandGenericHID}'s subclasses for * {@link CommandXboxController Xbox}/{@link edu.wpi.first.wpilibj2.command.button.CommandPS4Controller PS4} * controllers or {@link edu.wpi.first.wpilibj2.command.button.CommandJoystick Flight joysticks}. */ private void configureBindings() { Command driveFieldOrientedDirectAngle = drivebase.driveFieldOriented(driveDirectAngle); Command driveFieldOrientedAnglularVelocity = drivebase.driveFieldOriented(driveAngularVelocity); Command driveRobotOrientedAngularVelocity = drivebase.driveFieldOriented(driveRobotOriented); Command driveSetpointGen = drivebase.driveWithSetpointGeneratorFieldRelative( driveDirectAngle); Command driveFieldOrientedDirectAngleKeyboard = drivebase.driveFieldOriented(driveDirectAngleKeyboard); Command driveFieldOrientedAnglularVelocityKeyboard = drivebase.driveFieldOriented(driveAngularVelocityKeyboard); Command driveSetpointGenKeyboard = drivebase.driveWithSetpointGeneratorFieldRelative( driveDirectAngleKeyboard); NamedCommands.registerCommand("Outake", new IntakeIn(IntakeSubsystem).withTimeout(1)); NamedCommands.registerCommand("Intake", new IntakeIn(IntakeSubsystem).withTimeout(2)); NamedCommands.registerCommand("Coral", new IntakeAuto(IntakeSubsystem).withTimeout(5)); NamedCommands.registerCommand("L1", new L1A(ElevatorSubsystem)); NamedCommands.registerCommand("L2", new L2A(ElevatorSubsystem)); NamedCommands.registerCommand("L3", new L3A(ElevatorSubsystem)); NamedCommands.registerCommand("L4", new L4A(ElevatorSubsystem).withTimeout(4)); NamedCommands.registerCommand("AutoAlign", new Align(drivebase, LimelightHelpers).withTimeout(3)); NamedCommands.registerCommand("AlgaeAD",new AlgaeA(this.AlgaeArmSubsystem).withTimeout(3)); NamedCommands.registerCommand("AlgaeAU",new AlgaeAU(this.AlgaeArmSubsystem).withTimeout(3)); NamedCommands.registerCommand("AlgaeIN", new AlgaeIn(this.AlgaeSubsystem).withTimeout(3)); NamedCommands.registerCommand("IntakeSlow", new IntakeS(IntakeSubsystem).withTimeout(.3)); drivebase.setDefaultCommand(closedAbsoluteDriveAdv); autoChooser = AutoBuilder.buildAutoChooser(); SmartDashboard.putData("Auto Chooser", autoChooser); this.setupShuffleboard(); driverXbox.povUp().onTrue((Commands.runOnce(drivebase::zeroGyro))); commandXboxControllerXbox.a().whileTrue(new IntakeIn(this.IntakeSubsystem)); commandXboxControllerXbox.b().onTrue(new IntakeAuto(this.IntakeSubsystem)); commandXboxControllerXbox.x().whileTrue(new ElevatorIn(this.ElevatorSubsystem)); commandXboxControllerXbox.y().whileTrue(new ElevatorOut(this.ElevatorSubsystem)); commandXboxControllerXbox.leftBumper().whileTrue(new AlgaeIn(this.AlgaeSubsystem)); commandXboxControllerXbox.rightBumper().whileTrue(new AlgaeOut(this.AlgaeSubsystem)); commandXboxControllerXbox.leftTrigger().whileTrue(new AlgaeArmUp(this.AlgaeArmSubsystem)); commandXboxControllerXbox.rightTrigger().whileTrue(new AlgaeArmDown(this.AlgaeArmSubsystem)); commandXboxControllerXbox.povUp().onTrue(new L1(this.ElevatorSubsystem)); commandXboxControllerXbox.povRight().onTrue(new L2(this.ElevatorSubsystem)); commandXboxControllerXbox.povDown().onTrue(new L3(this.ElevatorSubsystem)); commandXboxControllerXbox.povLeft().onTrue(new L4(this.ElevatorSubsystem)); commandXboxControllerXbox.back().onTrue(Commands.runOnce(ElevatorSubsystem::zero)); commandXboxControllerXbox.start().whileTrue(new IntakeS(this.IntakeSubsystem)); commandXboxControllerXbox.rightStick().whileTrue(new IntakeOut(this.IntakeSubsystem)); driverXbox.povLeft().whileTrue( Commands.runOnce(() -> frc.robot.LimelightHelpers.setPipelineIndex("limelight", 1)) .andThen(new Align(drivebase, LimelightHelpers)) ); driverXbox.povRight().whileTrue( Commands.runOnce(() -> frc.robot.LimelightHelpers.setPipelineIndex("limelight", 0)) .andThen(new Align(drivebase, LimelightHelpers)) ); } /** * Use this to pass the autonomous command to the main {@link Robot} class. * * @return the command to run in autonomous */ public Command getAutonomousCommand() { // An example command will be run in autonomous return autoChooser.getSelected(); } private void setupShuffleboard() { // Add your Shuffleboard setup code here this.shuffleboardInstance = ShuffleboardDisplay.getInstance(); } public void setMotorBrake(boolean brake) { drivebase.setMotorBrake(brake); } }