diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 09e625f..85c7735 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -41,7 +41,7 @@ public static final class DriveConstants { // TODO: set motor and encoder constants public static final int kFrontLeftDriveMotorPort = 32; public static final int kRearLeftDriveMotorPort = 29; - public static final int kFrontRightDriveMotorPort = 38; + public static final int kFrontRightDriveMotorPort = 36; public static final int kRearRightDriveMotorPort = 34; public static final int kFrontLeftTurningMotorPort = 28; @@ -101,7 +101,7 @@ public static final class IntakeConstants { public static final int kArmEncoderChannel = 1; // In degrees - public static final double kIntakeLoweredAngle = -193; + public static final double kIntakeLoweredAngle = -188; public static final double kIntakeRaisedAngle = 0; public static final double kIntakeAmpScoringAngle = -71; // 193 - 100 (previous angle) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 52e7e47..5420005 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -72,6 +72,12 @@ public class RobotContainer { * The container for the robot. Contains subsystems, IO devices, and commands. */ public RobotContainer() { + + SmartDashboard.putBoolean("fast", false); + SmartDashboard.putBoolean("shoot fast", false); + + + NamedCommands.registerCommand("Shoot", new SequentialCommandGroup( new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Shooting, ShooterConstants.kShooterOnTime), @@ -151,24 +157,21 @@ public RobotContainer() { scaleJoysticks(-m_driverController.getLeftY(), -m_driverController.getLeftX()), IOConstants.kControllerDeadband) * DriveConstants.kMaxSpeedMetersPerSecond - * (1 - m_driverController - .getRightTriggerAxis() + * (1 - (SmartDashboard.getBoolean("fast", false) ? m_driverController.getRightTriggerAxis() : 1) * IOConstants.kSlowModeScalar), // * 0.8, MathUtil.applyDeadband( scaleJoysticks(-m_driverController.getLeftX(), -m_driverController.getLeftY()), IOConstants.kControllerDeadband) * DriveConstants.kMaxSpeedMetersPerSecond - * (1 - m_driverController - .getRightTriggerAxis() + * (1 - (SmartDashboard.getBoolean("fast", false) ? m_driverController.getRightTriggerAxis() : 1) * IOConstants.kSlowModeScalar), // * 0.8, MathUtil.applyDeadband( -m_driverController.getRightX(), IOConstants.kControllerDeadband) * DriveConstants.kMaxAngularSpeedRadiansPerSecond - * (1 - m_driverController - .getRightTriggerAxis() + * (1 - (SmartDashboard.getBoolean("fast", false) ? m_driverController.getRightTriggerAxis() : 1) * IOConstants.kSlowModeScalar) * 0.7, !m_driverController.getLeftBumper()), @@ -202,7 +205,7 @@ private void configureBindings() { new JoystickButton(m_driverController, Button.kX.value) .onTrue(new SequentialCommandGroup( - new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Shooting, ShooterConstants.kShooterOnTime), + new ShooterSetSpeedCommand(m_shooterSubsystem, SmartDashboard.getBoolean("shoot fast", false) ? ShootSpeed.Shooting : ShootSpeed.Prep, ShooterConstants.kShooterOnTime), new ParallelDeadlineGroup(new WaitCommand(1), new NoteOuttakeCommand(m_intakeSubsystem)))) .onFalse(new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Off, ShooterConstants.kShooterOffTime)); @@ -229,8 +232,8 @@ private void configureBindings() { .onTrue(new InstantCommand(() -> m_intakeSubsystem.resetHard())); // i better run - new JoystickButton(m_driverController, Button.kX.value) - .onTrue(new InstantCommand(() -> m_intakeSubsystem.daveImSorry())); + // new JoystickButton(m_driverController, Button.kX.value) + // .onTrue(new InstantCommand(() -> m_intakeSubsystem.daveImSorry())); new JoystickButton(m_driverController, Button.kBack.value) .onTrue(new InstantCommand(() -> m_intakeSubsystem.heCantKillMeTwice())); @@ -285,7 +288,7 @@ private void configureBindings() { // Spin up Shooter, Operator Controller Left Trigger new Trigger(() -> { return m_operatorController.getLeftTriggerAxis() > 0.5; - }).onTrue(new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Shooting, ShooterConstants.kShooterOnTime)) + }).onTrue(new ShooterSetSpeedCommand(m_shooterSubsystem, SmartDashboard.getBoolean("shoot fast", false) ? ShootSpeed.Shooting : ShootSpeed.SlowShoot, ShooterConstants.kShooterOnTime)) .onFalse(new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Off, ShooterConstants.kShooterOffTime)); // Climber Up, Operator Controller Right Bumper + A Button diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 54ffc08..be02068 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -46,9 +46,12 @@ public void setShootingSpeed(ShootSpeed speed) { m_topSpeed = ShooterConstants.kShooterSpeedTop; m_bottomSpeed = ShooterConstants.kShooterSpeedBottom; break; + case SlowShoot: + // m_topSpeed = case Amp: m_topSpeed = 0.2; m_bottomSpeed = 0.3; + break; case Prep: m_topSpeed = ShooterConstants.kPrepShooterSpeed; m_bottomSpeed = ShooterConstants.kPrepShooterSpeed; @@ -80,6 +83,7 @@ public static enum ShootSpeed { Shooting, Prep, Amp, - Off + Off, + SlowShoot } }