From 7a08a7b6e00d971d40006101075d5c7216bd31a2 Mon Sep 17 00:00:00 2001 From: Orcasphynx Date: Tue, 6 Aug 2024 15:34:30 -0400 Subject: [PATCH 01/11] //Committed out unused stuff and added testchooser --- src/main/java/frc/robot/RobotContainer.java | 79 ++++++++++--------- .../subsystems/drive/DriveSubsystem.java | 28 ++++++- 2 files changed, 70 insertions(+), 37 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 0999f42..5c291c8 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -52,7 +52,7 @@ import edu.wpi.first.wpilibj2.command.button.JoystickButton; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import frc.robot.subsystems.IntakeSubsystem; +//import frc.robot.subsystems.IntakeSubsystem; import java.util.function.DoubleSupplier; import java.util.function.Supplier; @@ -61,7 +61,7 @@ import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; -import frc.robot.commands.ResetGyro; +//import frc.robot.commands.ResetGyro; import edu.wpi.first.wpilibj2.command.button.POVButton; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.commands.AutoWait; @@ -95,28 +95,28 @@ public class RobotContainer implements Logged { private DoublePreference intakePower = new DoublePreference("intake/intakePower", 0.5); private DoublePreference intakePosition = new DoublePreference("intake/intakePosition", 100); private DoublePreference outtakePower = new DoublePreference("intake/outtakePower", -0.75); - private DoublePreference umbrellaPower = new DoublePreference( "umbrella/Power", 0.25); - private DoublePreference shooterPower = new DoublePreference("shooter/Power", 0.79); - private DoublePreference shooterRPM = new DoublePreference("shooter/RPM", 4000); + //private DoublePreference umbrellaPower = new DoublePreference( "umbrella/Power", 0.25); + //private DoublePreference shooterPower = new DoublePreference("shooter/Power", 0.79); + //private DoublePreference shooterRPM = new DoublePreference("shooter/RPM", 4000); private DoublePreference intakeShooterPosition = new DoublePreference("shooter/Position", Constants.ShooterConstants.TARGET_POSITION_DEGREES); private DoublePreference indexPower = new DoublePreference("shooter/IndexPower", 0.1); private DoublePreference shooterIndexPower = new DoublePreference("shooter/ShootingIndexPower", 0.5); - private DoublePreference shooterOuttakePower = new DoublePreference("shooter/OuttakePower", -0.1); - private DoublePreference shooterPosition = new DoublePreference("shooter/shootingPosition", 65); - private DoublePreference outdexPower = new DoublePreference("shooter/OutdexPower", -0.2); - private DoublePreference outtakeFlywheelPower = new DoublePreference("shooter/outtakeFlywheelPower", -0.3); - private DoublePreference preSpinDistanceM = new DoublePreference("shooter/preSpinDistanceM", 5.842); + //private DoublePreference shooterOuttakePower = new DoublePreference("shooter/OuttakePower", -0.1); + //private DoublePreference shooterPosition = new DoublePreference("shooter/shootingPosition", 65); + //private DoublePreference outdexPower = new DoublePreference("shooter/OutdexPower", -0.2); + //private DoublePreference outtakeFlywheelPower = new DoublePreference("shooter/outtakeFlywheelPower", -0.3); + //private DoublePreference preSpinDistanceM = new DoublePreference("shooter/preSpinDistanceM", 5.842); private DoublePreference shooterHome = new DoublePreference("shooter/homeposition", 0); private DoublePreference climberUpPower = new DoublePreference("climber/UpPower", ClimberConstants.POWER); private DoublePreference climberDownPower = new DoublePreference("climber/DownPower", -ClimberConstants.POWER); - private DoublePreference climberUpPosition = new DoublePreference("climber/UpPosition", ClimberConstants.TOP_POSITION); - private DoublePreference climberDownPosition = new DoublePreference("climber/DownPosition", ClimberConstants.BOTTOM_POSITION); + //private DoublePreference climberUpPosition = new DoublePreference("climber/UpPosition", ClimberConstants.TOP_POSITION); + //private DoublePreference climberDownPosition = new DoublePreference("climber/DownPosition", ClimberConstants.BOTTOM_POSITION); //For tuning - private DoublePreference tuningPower = new DoublePreference("shooter/tuning_rpm", 2500); - private DoublePreference tuningPosition = new DoublePreference("shooter/tuning_Position", 65); + //private DoublePreference tuningPower = new DoublePreference("shooter/tuning_rpm", 2500); + //private DoublePreference tuningPosition = new DoublePreference("shooter/tuning_Position", 65); //Low Angle, Mid Angle, High Angle @@ -146,13 +146,13 @@ public class RobotContainer implements Logged { private DoublePreference closeAutoShotRPM = new DoublePreference("shooter/closeAutoShotRPM", 3200); //High power - private DoublePreference shooterHighPower = new DoublePreference("shooter/highPower", 78); + //private DoublePreference shooterHighPower = new DoublePreference("shooter/highPower", 78); //preferences for slew rates - private DoublePreference m_kDirectionSlewRate = new DoublePreference("Direction Slew Rate", Constants.DriveConstants.kDirectionSlewRate); - private DoublePreference m_kMagnitudeSlewRate = new DoublePreference("Magnitude Slew Rate", Constants.DriveConstants.kMagnitudeSlewRate); - private DoublePreference m_kRotationalSlewRate = new DoublePreference("Rotational Slew Rate", Constants.DriveConstants.kRotationalSlewRate); + // private DoublePreference m_kDirectionSlewRate = new DoublePreference("Direction Slew Rate", Constants.DriveConstants.kDirectionSlewRate); + //private DoublePreference m_kMagnitudeSlewRate = new DoublePreference("Magnitude Slew Rate", Constants.DriveConstants.kMagnitudeSlewRate); + //private DoublePreference m_kRotationalSlewRate = new DoublePreference("Rotational Slew Rate", Constants.DriveConstants.kRotationalSlewRate); //preference for auto wait (in seconds) private DoublePreference m_autoWait = new DoublePreference("Autonomous Wait Command (Secs)", 0); @@ -165,31 +165,31 @@ public class RobotContainer implements Logged { private DoublePreference m_FourthShotImprovRPM = new DoublePreference("Fourth Shot Improved RPM", 3200); private final Command resetGyro = new ResetGyro(m_robotDrive); - private final Command intakeCommand = new RunIntake(m_robotIntake, intakePower, intakePosition); + //private final Command intakeCommand = new RunIntake(m_robotIntake, intakePower, intakePosition); private final Command extakeCommand = new RunIntake(m_robotIntake, outtakePower, intakePosition); private final Command intakePiece = new IntakePiece(m_robotIntake, m_shooter, intakePower, intakePosition, indexPower, intakeShooterPosition); - private final Command outTakePiece = new OuttakePiece(m_robotIntake, m_shooter, outtakePower, intakePosition, outdexPower, intakeShooterPosition, outtakeFlywheelPower); + //private final Command outTakePiece = new OuttakePiece(m_robotIntake, m_shooter, outtakePower, intakePosition, outdexPower, intakeShooterPosition, outtakeFlywheelPower); private final Command stowIntake = new StowIntake(m_robotIntake); private final Command parkCommand = new ParkCommand(m_robotDrive); private final Command stowShooter = new StowShooter(m_shooter, shooterHome); - private final Command raiseShooter = new PositionShooter(m_shooter, intakeShooterPosition); - private final Command spinShooter = new RunShooterPower(m_shooter, shooterPower); - private final Command spinIndex = new IndexPower(m_shooter, outdexPower, outtakePower); + //private final Command raiseShooter = new PositionShooter(m_shooter, intakeShooterPosition); + //private final Command spinShooter = new RunShooterPower(m_shooter, shooterPower); + //private final Command spinIndex = new IndexPower(m_shooter, outdexPower, outtakePower); private final Command shootSubwoofer = new ShootPiece(m_shooter, subShotAngle, subShotRPM, shooterIndexPower, () -> Operator.driver_leftTrigger.getAsBoolean()); private final Command shootPodium = new ShootPiece(m_shooter, podiumShotAngle, podiumShotRPM, shooterIndexPower, () -> Operator.driver_leftTrigger.getAsBoolean()); private final Command shootWing = new ShootPiece(m_shooter, wingShotAngle, wingShotRPM, shooterIndexPower, () -> Operator.driver_leftTrigger.getAsBoolean()); - private final Command spinRPM = new RunShooterRPM(m_shooter, shooterRPM); - private final Command preSpinShooter = new PrepareShooter(m_shooter, preSpinDistanceM); + //private final Command spinRPM = new RunShooterRPM(m_shooter, shooterRPM); + //private final Command preSpinShooter = new PrepareShooter(m_shooter, preSpinDistanceM); private final Command ejectPiece = new EjectPiece(m_shooter); private final Command climberPowerUp = new ClimbPower(m_Climber, climberUpPower); private final Command climberPowerDown = new ClimbPower(m_Climber, climberDownPower); - private final Command climbMMUp = new ClimbMM(m_Climber, climberUpPosition); - private final Command climbMMDown = new ClimbMM(m_Climber, climberDownPosition); + //private final Command climbMMUp = new ClimbMM(m_Climber, climberUpPosition); + //private final Command climbMMDown = new ClimbMM(m_Climber, climberDownPosition); - private final Command testTurnPID = new TurnDegrees(m_robotDrive, 15); + //private final Command testTurnPID = new TurnDegrees(m_robotDrive, 15); - private final Command shooterTune = new ShootPiece(m_shooter, tuningPosition, tuningPower, shooterIndexPower, () -> Operator.driver_leftTrigger.getAsBoolean()); + //private final Command shooterTune = new ShootPiece(m_shooter, tuningPosition, tuningPower, shooterIndexPower, () -> Operator.driver_leftTrigger.getAsBoolean()); private final Command shootInterpolated = new ShootInterpolated(m_shooter, m_photonCameraWrapper, shooterIndexPower, () -> Operator.driver_leftTrigger.getAsBoolean()); private final Command driveToTarget = new DriveToTarget(m_robotDrive, m_photonCameraWrapper, @@ -217,7 +217,9 @@ public class RobotContainer implements Logged { private final SendableChooser autonChooser; - private final double pathSpeed = 2; + private final SendableChooser testChooser; + + //private final double pathSpeed = 2; // The driver's controller @@ -232,22 +234,22 @@ private static class Operator { private static Supplier driver_axisLX = () -> MathUtil.applyDeadband(-driver.getRawAxis(0), Constants.OIConstants.kDriveDeadband); private static Supplier driver_axisLY = () -> MathUtil.applyDeadband(-driver.getRawAxis(1), Constants.OIConstants.kDriveDeadband); - private static Supplier driver_axisRY = () -> MathUtil.applyDeadband(-driver.getRawAxis(5), Constants.OIConstants.kDriveDeadband); + //private static Supplier driver_axisRY = () -> MathUtil.applyDeadband(-driver.getRawAxis(5), Constants.OIConstants.kDriveDeadband); private static JoystickButton driver_x = new JoystickButton(driver, XboxController.Button.kX.value); - private static JoystickButton driver_a = new JoystickButton(driver, XboxController.Button.kA.value); + //private static JoystickButton driver_a = new JoystickButton(driver, XboxController.Button.kA.value); private static JoystickButton driver_b = new JoystickButton(driver, XboxController.Button.kB.value); - private static JoystickButton driver_y = new JoystickButton(driver, XboxController.Button.kY.value); + // private static JoystickButton driver_y = new JoystickButton(driver, XboxController.Button.kY.value); private static JoystickButton driver_start = new JoystickButton(driver, XboxController.Button.kStart.value); private static JoystickButton driver_back = new JoystickButton(driver, XboxController.Button.kBack.value); private static JoystickButton driver_leftBumper = new JoystickButton(driver, XboxController.Button.kLeftBumper.value); private static JoystickButton driver_rightBumper = new JoystickButton(driver, XboxController.Button.kRightBumper.value); - private static AxisButton driver_rightTrigger = new AxisButton(driver, XboxController.Axis.kRightTrigger.value, 0.25); + //private static AxisButton driver_rightTrigger = new AxisButton(driver, XboxController.Axis.kRightTrigger.value, 0.25); private static AxisButton driver_leftTrigger = new AxisButton(driver, XboxController.Axis.kLeftTrigger.value, 0.25); private static POVButton driver_dpad_up = new POVButton(driver, 0); - private static POVButton driver_dpad_right = new POVButton(driver, 90); + //private static POVButton driver_dpad_right = new POVButton(driver, 90); private static POVButton driver_dpad_down= new POVButton(driver, 180); - private static POVButton driver_dpad_left = new POVButton(driver, 270); + //private static POVButton driver_dpad_left = new POVButton(driver, 270); private static JoystickButton manip_a = new JoystickButton(manipulator, XboxController.Button.kA.value); private static JoystickButton manip_b = new JoystickButton(manipulator, XboxController.Button.kB.value); @@ -299,6 +301,11 @@ public RobotContainer(){ autonChooser.addOption("FourRingImproved", AutoBuilder.buildAuto("4RingImproved")); SmartDashboard.putData("Autonomous", autonChooser); + testChooser = AutoBuilder.buildAutoChooser(); + + + + } diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java index 53c83d5..404415a 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java @@ -709,4 +709,30 @@ public Command turnSysIdTestBuilder(double staticTimeout, double dynamicTimeout) .finallyDo(() -> this.setTurnVolts(0)); } -} + //testing garbarge + public void rotateFL360() { + m_frontLeft.setDesiredState(new SwerveModuleState(0, Rotation2d.fromDegrees(360))); + } + + public void rotateFR360() { + m_frontRight.setDesiredState(new SwerveModuleState(0, Rotation2d.fromDegrees(360))); + } + + public void rotateRL360() { + m_rearLeft.setDesiredState(new SwerveModuleState(0, Rotation2d.fromDegrees(360))); + } + + public void rotateRR360(){ + m_rearRight.setDesiredState(new SwerveModuleState(0, Rotation2d.fromDegrees(360))); + } + + public void rotateAll360(){ + m_frontLeft.setDesiredState(new SwerveModuleState(0, Rotation2d.fromDegrees(360))); + m_frontRight.setDesiredState(new SwerveModuleState(0, Rotation2d.fromDegrees(360))); + m_rearLeft.setDesiredState(new SwerveModuleState(0, Rotation2d.fromDegrees(360))); + m_rearRight.setDesiredState(new SwerveModuleState(0, Rotation2d.fromDegrees(360))); + } + } + + + From 0892f462f73bc5a048885a293825ed84017b93e3 Mon Sep 17 00:00:00 2001 From: Orcasphynx Date: Tue, 6 Aug 2024 16:22:06 -0400 Subject: [PATCH 02/11] //test prototype --- .../robot/commands/test/rotateRearLeft.java | 38 +++++++++++++++++++ .../subsystems/drive/DriveSubsystem.java | 4 ++ 2 files changed, 42 insertions(+) create mode 100644 src/main/java/frc/robot/commands/test/rotateRearLeft.java diff --git a/src/main/java/frc/robot/commands/test/rotateRearLeft.java b/src/main/java/frc/robot/commands/test/rotateRearLeft.java new file mode 100644 index 0000000..0ba8676 --- /dev/null +++ b/src/main/java/frc/robot/commands/test/rotateRearLeft.java @@ -0,0 +1,38 @@ +// 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.commands.test; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.drive.DriveSubsystem; + +public class rotateRearLeft extends Command { + DriveSubsystem m_testDrive; + /** Creates a new rotateRearLeft. */ + public rotateRearLeft(DriveSubsystem testDrive) { + m_testDrive = testDrive; + // Use addRequirements() here to declare subsystem dependencies. + addRequirements(m_testDrive); + } + + // Called when the command is initially scheduled. + @Override + public void initialize() { + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + m_testDrive.rotateRL360(); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return m_testDrive.getRLPos() == 360; + } +} diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java index 404415a..c43a8e0 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java @@ -732,6 +732,10 @@ public void rotateAll360(){ m_rearLeft.setDesiredState(new SwerveModuleState(0, Rotation2d.fromDegrees(360))); m_rearRight.setDesiredState(new SwerveModuleState(0, Rotation2d.fromDegrees(360))); } + + public double getRLPos(){ + return m_rearLeft.getPosition().angle.getDegrees(); + } } From 9db011e745f50b09e5102d266357b07787aea38d Mon Sep 17 00:00:00 2001 From: Orcasphynx Date: Tue, 6 Aug 2024 16:29:54 -0400 Subject: [PATCH 03/11] makes a working test mode selector --- src/main/java/frc/robot/Robot.java | 10 ++++++++++ src/main/java/frc/robot/RobotContainer.java | 13 ++++++++++++- 2 files changed, 22 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index f12bded..7f613be 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -34,6 +34,8 @@ public class Robot extends TimedRobot implements Logged { private Command m_autonomousCommand; + private Command m_testCommand; + private RobotContainer m_robotContainer; @Log.NT @@ -165,6 +167,9 @@ public void teleopInit() { if (m_autonomousCommand != null) { m_autonomousCommand.cancel(); } + if (m_testCommand != null){ + m_testCommand.cancel(); + } } /** This function is called periodically during operator control. */ @@ -176,6 +181,11 @@ public void testInit() { getAllianceInfo(); // Cancels all running commands at the start of test mode. CommandScheduler.getInstance().cancelAll(); + m_testCommand = m_robotContainer.getTestCommand(); + + if(m_testCommand != null){ + m_testCommand.schedule(); + } } /** This function is called periodically during test mode. */ diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5c291c8..cc853d5 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -66,6 +66,8 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.commands.AutoWait; +import frc.robot.commands.test.rotateRearLeft; + @@ -213,6 +215,9 @@ public class RobotContainer implements Logged { //Autonomous Wait Command private final Command autoWait = new AutoWait(m_autoWait); + //test mode command + private final Command rotateRL = new rotateRearLeft(m_robotDrive); + private final ParallelCommandGroup speakerShotGroup = new ParallelCommandGroup(shootInterpolated, driveToTarget); private final SendableChooser autonChooser; @@ -302,7 +307,9 @@ public RobotContainer(){ SmartDashboard.putData("Autonomous", autonChooser); testChooser = AutoBuilder.buildAutoChooser(); - + testChooser.addOption("rotateRL", rotateRL); + SmartDashboard.putData("Test Mode", testChooser); + @@ -383,6 +390,10 @@ private void configureDefaultCommands(){ public Command getAutonomousCommand() { return autonChooser.getSelected(); } + + public Command getTestCommand(){ + return testChooser.getSelected(); + } } From 71cb1f7bdcdafe2a69fe2d8924e9790fb1311573 Mon Sep 17 00:00:00 2001 From: Orcasphynx Date: Tue, 6 Aug 2024 20:57:00 -0400 Subject: [PATCH 04/11] new prototype for testing turn and drive for mods --- src/main/java/frc/robot/RobotContainer.java | 28 +++++++++- .../robot/commands/test/rotateRearLeft.java | 5 +- .../subsystems/drive/DriveSubsystem.java | 55 ++++++++++++++++++- 3 files changed, 82 insertions(+), 6 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index cc853d5..dcefe95 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -215,8 +215,18 @@ public class RobotContainer implements Logged { //Autonomous Wait Command private final Command autoWait = new AutoWait(m_autoWait); - //test mode command - private final Command rotateRL = new rotateRearLeft(m_robotDrive); + //test mode commands + private final Command driveFrontLeft = m_robotDrive.driveModuleSysIdTestBuilder(2, 2, 0); + private final Command driveFrontRight = m_robotDrive.driveModuleSysIdTestBuilder(2, 2, 1); + private final Command driveRearLeft = m_robotDrive.driveModuleSysIdTestBuilder(2, 2, 2); + private final Command driveRearRight = m_robotDrive.driveModuleSysIdTestBuilder(2, 2, 3); + private final Command driveAll = m_robotDrive.driveSysIdTestBuilder(2,2); + + private final Command rotateFrontLeft = m_robotDrive.turnModuleSysIdTestBuilder(2,2,0); + private final Command rotateFrontRight = m_robotDrive.turnModuleSysIdTestBuilder(2,2,1); + private final Command rotateRearLeft = m_robotDrive.turnModuleSysIdTestBuilder(2,2,2); + private final Command rotateRearRight = m_robotDrive.turnModuleSysIdTestBuilder(2,2,3); + private final Command rotateAll = m_robotDrive.turnSysIdTestBuilder(2, 2); private final ParallelCommandGroup speakerShotGroup = new ParallelCommandGroup(shootInterpolated, driveToTarget); @@ -307,7 +317,19 @@ public RobotContainer(){ SmartDashboard.putData("Autonomous", autonChooser); testChooser = AutoBuilder.buildAutoChooser(); - testChooser.addOption("rotateRL", rotateRL); + + testChooser.addOption("driveFrontLeft", driveFrontLeft); + testChooser.addOption("driveFrontRight", driveFrontRight); + testChooser.addOption("driveRearLeft", driveRearLeft); + testChooser.addOption("driveRearRight", driveRearRight); + testChooser.addOption("driveAll", driveAll); + + testChooser.addOption("rotateFrontLeft", rotateFrontLeft); + testChooser.addOption("rotateFrontRight", rotateFrontRight); + testChooser.addOption("rotateRearLeft", rotateRearLeft); + testChooser.addOption("rotateRearRight", rotateRearRight); + testChooser.addOption("rotateAll", rotateAll); + SmartDashboard.putData("Test Mode", testChooser); diff --git a/src/main/java/frc/robot/commands/test/rotateRearLeft.java b/src/main/java/frc/robot/commands/test/rotateRearLeft.java index 0ba8676..6f3ef9f 100644 --- a/src/main/java/frc/robot/commands/test/rotateRearLeft.java +++ b/src/main/java/frc/robot/commands/test/rotateRearLeft.java @@ -23,7 +23,7 @@ public void initialize() { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - m_testDrive.rotateRL360(); + //m_testDrive.rotateRL360(); } // Called once the command ends or is interrupted. @@ -33,6 +33,7 @@ public void end(boolean interrupted) {} // Returns true when the command should end. @Override public boolean isFinished() { - return m_testDrive.getRLPos() == 360; + // return m_testDrive.getRLPos() == 360; + return false; } } diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java index c43a8e0..1bb643b 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java @@ -89,9 +89,11 @@ public class DriveSubsystem extends SubsystemBase implements Logged{ private final SwerveDrivePoseEstimator poseEstimator; private final SwerveDrivePoseEstimator autonPoseEstimator; + private MAXSwerveModule[] m_modules = {m_frontLeft, m_frontRight, m_rearLeft, m_rearRight}; private SwerveModuleState[] m_moduleStates = new SwerveModuleState[4]; + // Slew rate filter variables for controlling lateral acceleration @Log.NT @Log.File @@ -241,6 +243,24 @@ public DriveSubsystem(PhotonCameraWrapper photonCameraWrapper) { } + public SysIdRoutine createModuleTurnSysIdRoutine(int module){ + SysIdRoutine newSysIdRoutine = new SysIdRoutine( + new SysIdRoutine.Config(), + new SysIdRoutine.Mechanism( + (Measure volts) -> this.setTurnModuleVolts(module, volts.in(Volts)), null, this) + ); + return newSysIdRoutine; + } + + public SysIdRoutine createModuleDriveSysIdRoutine(int module){ + SysIdRoutine newSysIdRoutine = new SysIdRoutine( + new SysIdRoutine.Config(), + new SysIdRoutine.Mechanism( + (Measure volts) -> this.setDriveModuleVolts(module, volts.in(Volts)), null, this) + ); + return newSysIdRoutine; + } + @Override public void periodic( ) { @@ -505,6 +525,10 @@ public void setDriveVolts(double volts){ m_rearRight.setDriveVolts(volts); } + public void setDriveModuleVolts(int module, double volts){ + m_modules[module].setDriveVolts(volts); + } + /** * Set all trurn motors to voltage * @@ -517,6 +541,10 @@ public void setTurnVolts(double volts){ m_rearRight.setTurnVolts(volts); } + public void setTurnModuleVolts(int module, double volts){ + m_modules[module].setTurnVolts(volts); + } + /** * Sets the wheels into an X formation to prevent movement. */ @@ -701,6 +729,19 @@ public Command driveSysIdTestBuilder(double staticTimeout, double dynamicTimeout .finallyDo(() -> this.setDriveVolts(0)); } + public Command driveModuleSysIdTestBuilder(double staticTimeout, double dynamicTimeout, int module){ + SysIdRoutine newSysIdRoutine = createModuleDriveSysIdRoutine(module); + return + new InstantCommand(() -> this.setModulesZero()) + .andThen(newSysIdRoutine.quasistatic(SysIdRoutine.Direction.kForward).withTimeout(staticTimeout)) + .andThen(newSysIdRoutine.quasistatic(SysIdRoutine.Direction.kReverse).withTimeout(staticTimeout)) + .andThen(new WaitCommand(2)) + .andThen(newSysIdRoutine.dynamic(SysIdRoutine.Direction.kForward).withTimeout(dynamicTimeout)) + .andThen(newSysIdRoutine.dynamic(SysIdRoutine.Direction.kReverse).withTimeout(dynamicTimeout)) + .finallyDo(() -> this.setDriveVolts(0)); + } + + public Command turnSysIdTestBuilder(double staticTimeout, double dynamicTimeout){ return m_TurnSysIdRoutine.quasistatic(SysIdRoutine.Direction.kForward).withTimeout(staticTimeout) .andThen(m_TurnSysIdRoutine.quasistatic(SysIdRoutine.Direction.kReverse).withTimeout(staticTimeout)) @@ -709,7 +750,18 @@ public Command turnSysIdTestBuilder(double staticTimeout, double dynamicTimeout) .finallyDo(() -> this.setTurnVolts(0)); } + public Command turnModuleSysIdTestBuilder(double staticTimeout, double dynamicTimeout, int module){ + SysIdRoutine newSysIdRoutine = createModuleTurnSysIdRoutine(module); + return newSysIdRoutine.quasistatic(SysIdRoutine.Direction.kForward).withTimeout(staticTimeout) + .andThen(newSysIdRoutine.quasistatic(SysIdRoutine.Direction.kReverse).withTimeout(staticTimeout)) + .andThen(newSysIdRoutine.dynamic(SysIdRoutine.Direction.kForward).withTimeout(dynamicTimeout)) + .andThen(newSysIdRoutine.dynamic(SysIdRoutine.Direction.kReverse).withTimeout(dynamicTimeout)) + .finallyDo(() -> this.setTurnVolts(0)); + } + + //testing garbarge + /* public void rotateFL360() { m_frontLeft.setDesiredState(new SwerveModuleState(0, Rotation2d.fromDegrees(360))); } @@ -726,7 +778,7 @@ public void rotateRR360(){ m_rearRight.setDesiredState(new SwerveModuleState(0, Rotation2d.fromDegrees(360))); } - public void rotateAll360(){ + public void rotateAll360(){ m_frontLeft.setDesiredState(new SwerveModuleState(0, Rotation2d.fromDegrees(360))); m_frontRight.setDesiredState(new SwerveModuleState(0, Rotation2d.fromDegrees(360))); m_rearLeft.setDesiredState(new SwerveModuleState(0, Rotation2d.fromDegrees(360))); @@ -736,6 +788,7 @@ public void rotateAll360(){ public double getRLPos(){ return m_rearLeft.getPosition().angle.getDegrees(); } + */ } From a8a9159366c8eda8ab31b25d796e99fafb3bf27f Mon Sep 17 00:00:00 2001 From: Orcasphynx Date: Tue, 27 Aug 2024 19:51:48 -0400 Subject: [PATCH 05/11] Shooter Test in the works --- src/main/java/frc/robot/RobotContainer.java | 4 ++++ .../robot/subsystems/ShooterSubsystem.java | 24 +++++++++++++++++++ 2 files changed, 28 insertions(+) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index dcefe95..ae8568b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -228,6 +228,8 @@ public class RobotContainer implements Logged { private final Command rotateRearRight = m_robotDrive.turnModuleSysIdTestBuilder(2,2,3); private final Command rotateAll = m_robotDrive.turnSysIdTestBuilder(2, 2); + private final Command shooterPositionTest = m_shooter.positionerTestBuilder(2,2); + private final ParallelCommandGroup speakerShotGroup = new ParallelCommandGroup(shootInterpolated, driveToTarget); private final SendableChooser autonChooser; @@ -330,6 +332,8 @@ public RobotContainer(){ testChooser.addOption("rotateRearRight", rotateRearRight); testChooser.addOption("rotateAll", rotateAll); + testChooser.addOption("shooterTest", shooterPositionTest); + SmartDashboard.putData("Test Mode", testChooser); diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index d181f00..827b803 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -46,6 +46,9 @@ import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; import frc.robot.RobotState; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.WaitCommand; @@ -448,6 +451,27 @@ public void periodic() { robotPose2d = m_robotState.getRobotPose(); } + + public void setPositionMotorSpeed(double speed){ + m_shooterPositionMotor.set(speed); + } + + public Command positionerTestBuilder(double staticTimeout, double dynamicTimeout){ + return + new InstantCommand(() -> this.resetPosition()).withTimeout(staticTimeout) + //TODO:Change speed + .andThen(new InstantCommand(() -> this.setPositionMotorSpeed(1))) + .andThen(new InstantCommand(() -> this.setAngleDegrees(90)).withTimeout(staticTimeout)) + .andThen(new InstantCommand(() -> this.resetPosition()).withTimeout(staticTimeout)) + .andThen(new WaitCommand(2)) + //TODO:Change speed + .andThen(new InstantCommand(() -> this.setPositionMotorSpeed(2))) + .andThen(new InstantCommand(() -> this.setAngleDegrees(90)).withTimeout(dynamicTimeout)) + .andThen(new InstantCommand(() -> this.resetPosition()).withTimeout(dynamicTimeout)) + .andThen(new InstantCommand(() -> this.setPositionMotorSpeed(0))); + } + + public void simulationPeriodic(){ m_shooterPositionMotor.setPosition(targetPosition); } From 2d082f60cb7b3165a75346083483f4fab559f1ff Mon Sep 17 00:00:00 2001 From: Orcasphynx Date: Tue, 27 Aug 2024 20:07:41 -0400 Subject: [PATCH 06/11] Intake Speed Test Made --- src/main/java/frc/robot/RobotContainer.java | 6 +++++- .../frc/robot/subsystems/IntakeSubsystem.java | 16 ++++++++++++++++ 2 files changed, 21 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index ae8568b..60d1a20 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -230,6 +230,8 @@ public class RobotContainer implements Logged { private final Command shooterPositionTest = m_shooter.positionerTestBuilder(2,2); + private final Command intakeSpeedTest = m_robotIntake.intakeMotorTestBuilder(2, 2); + private final ParallelCommandGroup speakerShotGroup = new ParallelCommandGroup(shootInterpolated, driveToTarget); private final SendableChooser autonChooser; @@ -332,7 +334,9 @@ public RobotContainer(){ testChooser.addOption("rotateRearRight", rotateRearRight); testChooser.addOption("rotateAll", rotateAll); - testChooser.addOption("shooterTest", shooterPositionTest); + testChooser.addOption("shooterPositionTest", shooterPositionTest); + + testChooser.addOption("intakeSpeedTest", intakeSpeedTest); SmartDashboard.putData("Test Mode", testChooser); diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index 3675c9b..eb31943 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -7,7 +7,9 @@ import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.WaitCommand; import com.revrobotics.CANSparkBase.ControlType; import com.revrobotics.CANSparkBase.IdleMode; @@ -24,6 +26,8 @@ import monologue.Logged; import monologue.Annotations.Log; import edu.wpi.first.wpilibj.Preferences; +import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; +import edu.wpi.first.units.Measure; @@ -209,5 +213,17 @@ public Command spinRollers(){ }, this::stop); } + + public Command intakeMotorTestBuilder(double staticTimeout, double dynamicTimeout){ + return + new InstantCommand(() -> this.setSpeed(0)) + .andThen(new InstantCommand(() -> this.setSpeed(0.5))).withTimeout(staticTimeout) + .andThen(new InstantCommand(() -> this.setSpeed(-0.5)).withTimeout(staticTimeout)) + .andThen(new InstantCommand(() -> this.setSpeed(0))) + .andThen(new WaitCommand(2)) + .andThen(new InstantCommand(() -> this.setSpeed(1)).withTimeout(dynamicTimeout)) + .andThen(new InstantCommand(() -> this.setSpeed(-1)).withTimeout(dynamicTimeout)) + .andThen(new InstantCommand(() -> this.setSpeed(0))); + } } From 6cf440fd9d82ad793839af0a83ca4f7a99bc10bc Mon Sep 17 00:00:00 2001 From: Orcasphynx Date: Tue, 27 Aug 2024 20:17:50 -0400 Subject: [PATCH 07/11] Minor Changes --- .../frc/robot/subsystems/IntakeSubsystem.java | 16 ++++++++++++++++ .../frc/robot/subsystems/ShooterSubsystem.java | 1 + 2 files changed, 17 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index eb31943..bedbaa7 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -215,6 +215,18 @@ public Command spinRollers(){ } public Command intakeMotorTestBuilder(double staticTimeout, double dynamicTimeout){ + return + new InstantCommand(() -> this.stop()) + .andThen(new InstantCommand(() -> this.setSpeed(0.5))).withTimeout(staticTimeout) + .andThen(new InstantCommand(() -> this.setSpeed(-0.5)).withTimeout(staticTimeout)) + .andThen(new InstantCommand(() -> this.stop())) + .andThen(new WaitCommand(2)) + .andThen(new InstantCommand(() -> this.setSpeed(1)).withTimeout(dynamicTimeout)) + .andThen(new InstantCommand(() -> this.setSpeed(-1)).withTimeout(dynamicTimeout)) + .andThen(new InstantCommand(() -> this.stop())); + } +/* + public Command intakePositionTestBuilder(double staticTimeout, double dynamicTimeout){ return new InstantCommand(() -> this.setSpeed(0)) .andThen(new InstantCommand(() -> this.setSpeed(0.5))).withTimeout(staticTimeout) @@ -225,5 +237,9 @@ public Command intakeMotorTestBuilder(double staticTimeout, double dynamicTimeou .andThen(new InstantCommand(() -> this.setSpeed(-1)).withTimeout(dynamicTimeout)) .andThen(new InstantCommand(() -> this.setSpeed(0))); } + +*/ + + } diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 827b803..304cc2b 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -463,6 +463,7 @@ public Command positionerTestBuilder(double staticTimeout, double dynamicTimeout .andThen(new InstantCommand(() -> this.setPositionMotorSpeed(1))) .andThen(new InstantCommand(() -> this.setAngleDegrees(90)).withTimeout(staticTimeout)) .andThen(new InstantCommand(() -> this.resetPosition()).withTimeout(staticTimeout)) + .andThen(new InstantCommand(() -> this.setPositionMotorSpeed(0))) .andThen(new WaitCommand(2)) //TODO:Change speed .andThen(new InstantCommand(() -> this.setPositionMotorSpeed(2))) From 889fa3270ef1a61a9edfb073ce9cae88d0c630cf Mon Sep 17 00:00:00 2001 From: Orcasphynx Date: Tue, 27 Aug 2024 20:56:43 -0400 Subject: [PATCH 08/11] Climber test made, need to fix shooter test --- src/main/java/frc/robot/RobotContainer.java | 4 ++++ src/main/java/frc/robot/subsystems/Climber.java | 16 ++++++++++++++++ 2 files changed, 20 insertions(+) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 60d1a20..9cd60eb 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -232,6 +232,8 @@ public class RobotContainer implements Logged { private final Command intakeSpeedTest = m_robotIntake.intakeMotorTestBuilder(2, 2); + private final Command climberSpeedTest = m_Climber.climberTestBuilder(2, 2); + private final ParallelCommandGroup speakerShotGroup = new ParallelCommandGroup(shootInterpolated, driveToTarget); private final SendableChooser autonChooser; @@ -338,6 +340,8 @@ public RobotContainer(){ testChooser.addOption("intakeSpeedTest", intakeSpeedTest); + testChooser.addOption("climberTest", climberSpeedTest); + SmartDashboard.putData("Test Mode", testChooser); diff --git a/src/main/java/frc/robot/subsystems/Climber.java b/src/main/java/frc/robot/subsystems/Climber.java index e56c83f..5ad9d92 100644 --- a/src/main/java/frc/robot/subsystems/Climber.java +++ b/src/main/java/frc/robot/subsystems/Climber.java @@ -15,7 +15,10 @@ import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.robot.Constants; import frc.robot.Constants.ClimberConstants; import frc.robot.Constants.ShooterConstants; @@ -135,4 +138,17 @@ public void periodic() { current = m_climberMotor.getStatorCurrent().getValueAsDouble(); position = m_climberMotor.getPosition().getValueAsDouble(); } + + public Command climberTestBuilder(double staticTimeout, double dynamicTimeout){ + return + new InstantCommand(() -> this.stop()) + .andThen(new InstantCommand(() -> this.move(0.5)).withTimeout(staticTimeout)) + .andThen(new InstantCommand(() -> this.move(-0.5)).withTimeout(staticTimeout)) + .andThen(new InstantCommand(() -> this.stop())) + .andThen(new WaitCommand(2)) + .andThen(new InstantCommand(() -> this.move(1)).withTimeout(dynamicTimeout)) + .andThen(new InstantCommand(() -> this.move(-1)).withTimeout(dynamicTimeout)) + .andThen(new InstantCommand(() -> this.stop())); + } + } From 97ef15b25daf4a34fc48bbc2f568e4654ac88f76 Mon Sep 17 00:00:00 2001 From: Ignis Date: Sat, 16 Nov 2024 10:35:20 -0500 Subject: [PATCH 09/11] RemoteCancoder to FusedCancoder --- src/main/java/frc/robot/subsystems/ShooterSubsystem.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 6d9a554..5557213 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -235,7 +235,7 @@ private void configurePositionMotor(TalonFX motor, configurator.refresh(mmConfig); baseConfiguration.Feedback.FeedbackRemoteSensorID = m_shooterPositionCancoder.getDeviceID(); - baseConfiguration.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.RemoteCANcoder; + baseConfiguration.Feedback.FeedbackSensorSource = FeedbackSensorSourceValue.FusedCANcoder; configurator.apply(baseConfiguration); From 790cae14bf474e2a8c878a690f080eed05c81d1c Mon Sep 17 00:00:00 2001 From: Ignis Date: Sat, 16 Nov 2024 10:51:13 -0500 Subject: [PATCH 10/11] Co-authored-by: Josh Reddick --- src/main/java/frc/robot/Constants.java | 1 + src/main/java/frc/robot/subsystems/ShooterSubsystem.java | 6 ++++++ 2 files changed, 7 insertions(+) diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 5e8c59e..5636a74 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -140,6 +140,7 @@ public static final class ShooterConstants { public static final double POSITION_kD = 2; public static final double POSITION_kS = 0.24; public static final double POSITION_kV = 0.12; + public static final double POSITION_kG = 0.2; public static final double POSITION_ForwardsLimit = 0.42; public static final double POSITION_ReverseLimit = 0; diff --git a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java index 5557213..8bb6a33 100644 --- a/src/main/java/frc/robot/subsystems/ShooterSubsystem.java +++ b/src/main/java/frc/robot/subsystems/ShooterSubsystem.java @@ -47,6 +47,7 @@ import edu.wpi.first.math.geometry.Transform3d; import frc.robot.RobotState; import edu.wpi.first.wpilibj.Servo; +import com.ctre.phoenix6.signals.GravityTypeValue; @@ -82,6 +83,9 @@ public class ShooterSubsystem extends SubsystemBase implements Logged { private DigitalInput m_indexerBeamBreak = new DigitalInput(0); + //temp preference + private DoublePreference shooterPositionkGPreference = new DoublePreference("shooter/positionkG", Constants.ShooterConstants.POSITION_kG); + /********************* Telemetry Variables *********************/ @@ -248,6 +252,8 @@ private void configurePositionMotor(TalonFX motor, slot0PID.kP = Constants.ShooterConstants.POSITION_kP; slot0PID.kI = Constants.ShooterConstants.POSITION_kI; slot0PID.kD = Constants.ShooterConstants.POSITION_kD; + slot0PID.kG = shooterPositionkGPreference.getValue(); + slot0PID.GravityType = GravityTypeValue.Arm_Cosine; configurator.apply(slot0PID, 0.050); From 05e379fd2efc67a606bb72c36bde39467f3af39c Mon Sep 17 00:00:00 2001 From: y0shi Date: Sat, 16 Nov 2024 12:49:38 -0500 Subject: [PATCH 11/11] =?UTF-8?q?=E0=B2=A0=5F=E0=B2=A0=20William!?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/main/java/frc/robot/RobotContainer.java | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index bc3ebb1..da205af 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -115,8 +115,8 @@ public class RobotContainer implements Logged { private DoublePreference shooterIndexPower = new DoublePreference("shooter/ShootingIndexPower", 0.5); //private DoublePreference shooterOuttakePower = new DoublePreference("shooter/OuttakePower", -0.1); //private DoublePreference shooterPosition = new DoublePreference("shooter/shootingPosition", 65); - //private DoublePreference outdexPower = new DoublePreference("shooter/OutdexPower", -0.2); - //private DoublePreference outtakeFlywheelPower = new DoublePreference("shooter/outtakeFlywheelPower", -0.3); + private DoublePreference outdexPower = new DoublePreference("shooter/OutdexPower", -0.2); + private DoublePreference outtakeFlywheelPower = new DoublePreference("shooter/outtakeFlywheelPower", -0.3); //private DoublePreference preSpinDistanceM = new DoublePreference("shooter/preSpinDistanceM", 5.842); private DoublePreference shooterHome = new DoublePreference("shooter/homeposition", 0); @@ -287,14 +287,14 @@ private static class Operator { //private static Supplier driver_axisRY = () -> MathUtil.applyDeadband(-driver.getRawAxis(5), Constants.OIConstants.kDriveDeadband); private static JoystickButton driver_x = new JoystickButton(driver, XboxController.Button.kX.value); - //private static JoystickButton driver_a = new JoystickButton(driver, XboxController.Button.kA.value); + private static JoystickButton driver_a = new JoystickButton(driver, XboxController.Button.kA.value); private static JoystickButton driver_b = new JoystickButton(driver, XboxController.Button.kB.value); - // private static JoystickButton driver_y = new JoystickButton(driver, XboxController.Button.kY.value); + private static JoystickButton driver_y = new JoystickButton(driver, XboxController.Button.kY.value); private static JoystickButton driver_start = new JoystickButton(driver, XboxController.Button.kStart.value); private static JoystickButton driver_back = new JoystickButton(driver, XboxController.Button.kBack.value); private static JoystickButton driver_leftBumper = new JoystickButton(driver, XboxController.Button.kLeftBumper.value); private static JoystickButton driver_rightBumper = new JoystickButton(driver, XboxController.Button.kRightBumper.value); - //private static AxisButton driver_rightTrigger = new AxisButton(driver, XboxController.Axis.kRightTrigger.value, 0.25); + private static AxisButton driver_rightTrigger = new AxisButton(driver, XboxController.Axis.kRightTrigger.value, 0.25); private static AxisButton driver_leftTrigger = new AxisButton(driver, XboxController.Axis.kLeftTrigger.value, 0.25); private static POVButton driver_dpad_up = new POVButton(driver, 0); //private static POVButton driver_dpad_right = new POVButton(driver, 90);