diff --git a/src/main/deploy/pathplanner/paths/BlueRightToTopClose.path b/src/main/deploy/pathplanner/paths/BlueRightToTopClose.path new file mode 100644 index 0000000..a907851 --- /dev/null +++ b/src/main/deploy/pathplanner/paths/BlueRightToTopClose.path @@ -0,0 +1,68 @@ +{ + "version": 1.0, + "waypoints": [ + { + "anchor": { + "x": 0.8020771597098753, + "y": 6.684305590833405 + }, + "prevControl": null, + "nextControl": { + "x": 1.045306757084283, + "y": 7.1055916113904 + }, + "isLocked": false, + "linkedName": null + }, + { + "anchor": { + "x": 2.5265994976434403, + "y": 7.0 + }, + "prevControl": { + "x": 1.5265994976434403, + "y": 7.0 + }, + "nextControl": null, + "isLocked": false, + "linkedName": null + } + ], + "rotationTargets": [ + { + "waypointRelativePos": 0, + "rotationDegrees": 0, + "rotateFast": false + }, + { + "waypointRelativePos": 0, + "rotationDegrees": 0, + "rotateFast": false + }, + { + "waypointRelativePos": 0, + "rotationDegrees": 0, + "rotateFast": false + } + ], + "constraintZones": [], + "eventMarkers": [], + "globalConstraints": { + "maxVelocity": 3.0, + "maxAcceleration": 3.0, + "maxAngularVelocity": 540.0, + "maxAngularAcceleration": 720.0 + }, + "goalEndState": { + "velocity": 0, + "rotation": 0, + "rotateFast": false + }, + "reversed": false, + "folder": null, + "previewStartingState": { + "rotation": 57.994616791916435, + "velocity": 0 + }, + "useDefaultConstraints": true +} \ No newline at end of file diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 01122d3..5133c71 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -47,152 +47,152 @@ * (including subsystems, commands, and button mappings) should be declared here. */ public class RobotContainer { - // The robot's subsystems and commands are defined here - - private final XboxController m_driverController = new XboxController(IOConstants.kDriverControllerPort); - private final XboxController m_operatorController = new XboxController(IOConstants.kOperatorControllerPort); - private final DriveSubsystem m_robotDrive = new DriveSubsystem(); - private final ShooterSubsystem m_shooterSubsystem = new ShooterSubsystem(); - private final IntakeSubsystem m_intakeSubsystem = new IntakeSubsystem(); - private final ClimberSubsystem m_climberSubsystem = new ClimberSubsystem(); - private final VisionSubsystem m_visionSubsystem = new VisionSubsystem(); - private final SendableChooser autoChooser; - - /** - * The container for the robot. Contains subsystems, IO devices, and commands. - */ - public RobotContainer() { - // Registering Named Commands for autonpaths - NamedCommands.registerCommand("shoot", new SequentialCommandGroup( - new InstantCommand(() -> m_shooterSubsystem.spin(0.75), m_shooterSubsystem), - new WaitCommand(0.5), - new InstantCommand(() -> m_shooterSubsystem.spin(0), m_shooterSubsystem))); - - NamedCommands.registerCommand("intake", new SequentialCommandGroup( - new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Extended), - new NoteIntakeCommand(m_intakeSubsystem), - new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Retracted), - new WaitCommand(0.5), - new NoteOuttakeCommand(m_intakeSubsystem), - new WaitCommand(0.5))); - - // All paths automatically - - autoChooser = AutoBuilder.buildAutoChooser(); - SmartDashboard.putData("Auto Chooser", autoChooser); - - AutoBuilder.configureHolonomic(m_robotDrive::getPose, m_robotDrive::resetOdometry, - m_robotDrive::getChassisSpeeds, - m_robotDrive::autonDrive, - new HolonomicPathFollowerConfig( - new PIDConstants(5, 0.0, 0.0), // Translation PID constants - new PIDConstants(5, 0.0, 0.0), // Rotation PID constants - DriveConstants.kMaxSpeedMetersPerSecond, // Max module speed, in m/s - Math.hypot(DriveConstants.kTrackWidth, DriveConstants.kWheelBase), // Drive base radius in - // meters. Distance - // from robot center to - // furthest module. - new ReplanningConfig(true, true)), - () -> false, m_robotDrive); - - m_visionSubsystem.addConsumer(m_robotDrive::addVisionMeasurement); - - // Configure the trigger bindings - configureBindings(); - - m_robotDrive.setDefaultCommand( - new RunCommand( - () -> m_robotDrive.drive( - MathUtil.applyDeadband( - -m_driverController.getLeftY(), - IOConstants.kControllerDeadband) - * DriveConstants.kMaxSpeedMetersPerSecond - * (1 - m_driverController - .getLeftTriggerAxis() - * IOConstants.kSlowModeScalar), - // * 0.8, - MathUtil.applyDeadband( - -m_driverController.getLeftX(), - IOConstants.kControllerDeadband) - * DriveConstants.kMaxSpeedMetersPerSecond - * (1 - m_driverController - .getLeftTriggerAxis() - * IOConstants.kSlowModeScalar), - // * 0.8, - MathUtil.applyDeadband( - -m_driverController.getRightX(), - IOConstants.kControllerDeadband) - * DriveConstants.kMaxAngularSpeedRadiansPerSecond - * (1 - m_driverController - .getLeftTriggerAxis() - * IOConstants.kSlowModeScalar) - / 2, - !m_driverController.getRightBumper()), - m_robotDrive)); + // The robot's subsystems and commands are defined here + + private final XboxController m_driverController = new XboxController(IOConstants.kDriverControllerPort); + private final XboxController m_operatorController = new XboxController(IOConstants.kOperatorControllerPort); + private final DriveSubsystem m_robotDrive = new DriveSubsystem(); + private final ShooterSubsystem m_shooterSubsystem = new ShooterSubsystem(); + private final IntakeSubsystem m_intakeSubsystem = new IntakeSubsystem(); + private final ClimberSubsystem m_climberSubsystem = new ClimberSubsystem(); + private final VisionSubsystem m_visionSubsystem = new VisionSubsystem(); + private final SendableChooser autoChooser; + + /** + * The container for the robot. Contains subsystems, IO devices, and commands. + */ + public RobotContainer() { + // Registering Named Commands for autonpaths + NamedCommands.registerCommand("shoot", new SequentialCommandGroup( + new InstantCommand(() -> m_shooterSubsystem.spin(0.75), m_shooterSubsystem), + new WaitCommand(0.5), + new InstantCommand(() -> m_shooterSubsystem.spin(0), m_shooterSubsystem))); + + NamedCommands.registerCommand("intake", new SequentialCommandGroup( + new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Extended), + new NoteIntakeCommand(m_intakeSubsystem), + new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Retracted), + new WaitCommand(0.5), + new NoteOuttakeCommand(m_intakeSubsystem), + new WaitCommand(0.5))); + + // All paths automatically + + autoChooser = AutoBuilder.buildAutoChooser(); + SmartDashboard.putData("Auto Chooser", autoChooser); + + AutoBuilder.configureHolonomic(m_robotDrive::getPose, m_robotDrive::resetOdometry, + m_robotDrive::getChassisSpeeds, + m_robotDrive::autonDrive, + new HolonomicPathFollowerConfig( + new PIDConstants(5, 0.0, 0.0), // Translation PID constants + new PIDConstants(5, 0.0, 0.0), // Rotation PID constants + DriveConstants.kMaxSpeedMetersPerSecond, // Max module speed, in m/s + Math.hypot(DriveConstants.kTrackWidth, DriveConstants.kWheelBase), // Drive base radius in + // meters. Distance + // from robot center to + // furthest module. + new ReplanningConfig(true, true)), + () -> false, m_robotDrive); + + m_visionSubsystem.addConsumer(m_robotDrive::addVisionMeasurement); + + // Configure the trigger bindings + configureBindings(); + + m_robotDrive.setDefaultCommand( + new RunCommand( + () -> m_robotDrive.drive( + MathUtil.applyDeadband( + -m_driverController.getLeftY(), + IOConstants.kControllerDeadband) + * DriveConstants.kMaxSpeedMetersPerSecond + * (1 - m_driverController + .getLeftTriggerAxis() + * IOConstants.kSlowModeScalar), + // * 0.8, + MathUtil.applyDeadband( + -m_driverController.getLeftX(), + IOConstants.kControllerDeadband) + * DriveConstants.kMaxSpeedMetersPerSecond + * (1 - m_driverController + .getLeftTriggerAxis() + * IOConstants.kSlowModeScalar), + // * 0.8, + MathUtil.applyDeadband( + -m_driverController.getRightX(), + IOConstants.kControllerDeadband) + * DriveConstants.kMaxAngularSpeedRadiansPerSecond + * (1 - m_driverController + .getLeftTriggerAxis() + * IOConstants.kSlowModeScalar) + / 2, + !m_driverController.getRightBumper()), + m_robotDrive)); + } + + /** + * Use this method to define your button->command mappings. + */ + private void configureBindings() { + new JoystickButton(m_driverController, Button.kStart.value) + .onTrue(new InstantCommand(() -> m_robotDrive.zeroHeading(), m_robotDrive)); + + // new JoystickButton(m_driverController, Button.kA.value).whileTrue( + // AutoBuilder.pathfindToPose(new Pose2d(2.8, 5.5, new Rotation2d()), new + // PathConstraints( + // DriveConstants.kMaxSpeedMetersPerSecond - 1, 5, + // DriveConstants.kMaxAngularSpeedRadiansPerSecond - 1, 5))); + + new JoystickButton(m_operatorController, Button.kX.value) + .onTrue(new InstantCommand(() -> m_shooterSubsystem.spin(0.75), m_shooterSubsystem)) + .onFalse(new InstantCommand(() -> m_shooterSubsystem.spin(0), m_shooterSubsystem)); + new JoystickButton(m_operatorController, Button.kY.value) + .onTrue(new InstantCommand(() -> m_shooterSubsystem.spin(-0.75), m_shooterSubsystem)) + .onFalse(new InstantCommand(() -> m_shooterSubsystem.spin(0), m_shooterSubsystem)); + + new JoystickButton(m_operatorController, Button.kA.value) + .onTrue(new InstantCommand(() -> m_climberSubsystem.forward(), m_robotDrive)); + new JoystickButton(m_operatorController, Button.kB.value) + .onTrue(new InstantCommand(() -> m_climberSubsystem.reverse(), m_robotDrive)); + + new Trigger(() -> { + return m_driverController.getLeftTriggerAxis() > 0.5; + }).whileTrue( + new SequentialCommandGroup( + new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Extended), + new NoteIntakeCommand(m_intakeSubsystem), + new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Retracted))) + .onFalse(new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Retracted)); + + new Trigger(() -> { + return m_driverController.getRightTriggerAxis() > 0.5; + }).whileTrue( + new SequentialCommandGroup( + new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Amp), + new NoteOuttakeCommand(m_intakeSubsystem))) + .onFalse(new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Retracted)); + } + + // adding auton Path options + + /** + * Use this to pass the autonomous command to the main {@link Robot} class. + * + * @return the command to run in autonomous + */ + public Command getAutonomousCommand() { + Command follow = autoChooser.getSelected(); + PathPlannerPath path = PathPlannerPath.fromPathFile(follow.getName()); + + var alliance = DriverStation.getAlliance(); + PathPlannerPath autonPath = path; + if (alliance.isPresent() && alliance.get() == DriverStation.Alliance.Red) { + autonPath = autonPath.flipPath(); } + m_robotDrive.resetOdometry(autonPath.getPreviewStartingHolonomicPose()); - /** - * Use this method to define your button->command mappings. - */ - private void configureBindings() { - new JoystickButton(m_driverController, Button.kStart.value) - .onTrue(new InstantCommand(() -> m_robotDrive.zeroHeading(), m_robotDrive)); - - // new JoystickButton(m_driverController, Button.kA.value).whileTrue( - // AutoBuilder.pathfindToPose(new Pose2d(2.8, 5.5, new Rotation2d()), new - // PathConstraints( - // DriveConstants.kMaxSpeedMetersPerSecond - 1, 5, - // DriveConstants.kMaxAngularSpeedRadiansPerSecond - 1, 5))); - - new JoystickButton(m_operatorController, Button.kX.value) - .onTrue(new InstantCommand(() -> m_shooterSubsystem.spin(0.75), m_shooterSubsystem)) - .onFalse(new InstantCommand(() -> m_shooterSubsystem.spin(0), m_shooterSubsystem)); - new JoystickButton(m_operatorController, Button.kY.value) - .onTrue(new InstantCommand(() -> m_shooterSubsystem.spin(-0.75), m_shooterSubsystem)) - .onFalse(new InstantCommand(() -> m_shooterSubsystem.spin(0), m_shooterSubsystem)); - - new JoystickButton(m_operatorController, Button.kA.value) - .onTrue(new InstantCommand(() -> m_climberSubsystem.forward(), m_robotDrive)); - new JoystickButton(m_operatorController, Button.kB.value) - .onTrue(new InstantCommand(() -> m_climberSubsystem.reverse(), m_robotDrive)); - - new Trigger(() -> { - return m_driverController.getLeftTriggerAxis() > 0.5; - }).whileTrue( - new SequentialCommandGroup( - new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Extended), - new NoteIntakeCommand(m_intakeSubsystem), - new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Retracted))) - .onFalse(new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Retracted)); - - new Trigger(() -> { - return m_driverController.getRightTriggerAxis() > 0.5; - }).whileTrue( - new SequentialCommandGroup( - new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Amp), - new NoteOuttakeCommand(m_intakeSubsystem))) - .onFalse(new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Retracted)); - } - - // adding auton Path options - - /** - * Use this to pass the autonomous command to the main {@link Robot} class. - * - * @return the command to run in autonomous - */ - public Command getAutonomousCommand() { - Command follow = autoChooser.getSelected(); - PathPlannerPath path = PathPlannerPath.fromPathFile(follow.getName()); - - var alliance = DriverStation.getAlliance(); - PathPlannerPath autonPath = path; - if (alliance.isPresent() && alliance.get() == DriverStation.Alliance.Red) { - autonPath = autonPath.flipPath(); - } - m_robotDrive.resetOdometry(autonPath.getPreviewStartingHolonomicPose()); - - // return AutoBuilder.followPath(autonPath); - return null; - } + // return AutoBuilder.followPath(autonPath); + return null; + } }