diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index b653677..63bccea 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -40,28 +40,28 @@ public final class Constants { public static final double TURBO_SPEED = 4.0; public static final class DefaultSwerveRotOffsets { - public static final double FRONT_LEFT_ROT_OFFSET = 216.869140625; - public static final double FRONT_RIGHT_ROT_OFFSET = 229.4826875;//-212.695 + 90; //77.783 + 45 + 90 ;//-202.588; - public static final double BACK_LEFT_ROT_OFFSET = -277.646484375; - public static final double BACK_RIGHT_ROT_OFFSET = 140.009765625; + public static final double FRONT_LEFT_ROT_OFFSET = 216.869140625; + public static final double FRONT_RIGHT_ROT_OFFSET = 229.4826875;//-212.695 + 90; //77.783 + 45 + 90 ;//-202.588; + public static final double BACK_LEFT_ROT_OFFSET = -277.646484375; + public static final double BACK_RIGHT_ROT_OFFSET = 140.009765625; } public static final class IDs { - public static final int RIGHT_FRONT_WHEEL_ID = 2; - public static final int RIGHT_FRONT_STEER_ID = 3; - public static final int RIGHT_FRONT_ENCODER_ID = 10; + public static final int RIGHT_FRONT_WHEEL_ID = 2; + public static final int RIGHT_FRONT_STEER_ID = 3; + public static final int RIGHT_FRONT_ENCODER_ID = 10; - public static final int LEFT_FRONT_WHEEL_ID = 4; - public static final int LEFT_FRONT_STEER_ID = 5; - public static final int LEFT_FRONT_ENCODER_ID = 11; + public static final int LEFT_FRONT_WHEEL_ID = 4; + public static final int LEFT_FRONT_STEER_ID = 5; + public static final int LEFT_FRONT_ENCODER_ID = 11; - public static final int LEFT_BACK_WHEEL_ID = 6; - public static final int LEFT_BACK_STEER_ID = 7; - public static final int LEFT_BACK_ENCODER_ID = 12; + public static final int LEFT_BACK_WHEEL_ID = 6; + public static final int LEFT_BACK_STEER_ID = 7; + public static final int LEFT_BACK_ENCODER_ID = 12; - public static final int RIGHT_BACK_WHEEL_ID = 8; - public static final int RIGHT_BACK_STEER_ID = 9; - public static final int RIGHT_BACK_ENCODER_ID = 13; + public static final int RIGHT_BACK_WHEEL_ID = 8; + public static final int RIGHT_BACK_STEER_ID = 9; + public static final int RIGHT_BACK_ENCODER_ID = 13; } public static final class PIDConstants { @@ -120,7 +120,7 @@ public final class Constants { // misc public static final int TIMEOUT_MS = 30; public static final int SMARTDASHBOARD_UPDATE_FRAME = 2; - } + } public static final class VisionConstants { // public static final String NAME = "photonCamera"; diff --git a/src/main/java/frc4388/robot/Main.java b/src/main/java/frc4388/robot/Main.java index ad2d494..76d430a 100644 --- a/src/main/java/frc4388/robot/Main.java +++ b/src/main/java/frc4388/robot/Main.java @@ -15,15 +15,15 @@ import edu.wpi.first.wpilibj.RobotBase; * change the parameter class to the startRobot call. */ public final class Main { - private Main() { - } + private Main() { + } - /** - * Main initialization function. Do not perform any initialization here. - * - *

If you change your main robot class, change the parameter type. - */ - public static void main(String... args) { - RobotBase.startRobot(Robot::new); - } + /** + * Main initialization function. Do not perform any initialization here. + * + *

If you change your main robot class, change the parameter type. + */ + public static void main(String... args) { + RobotBase.startRobot(Robot::new); + } } diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 83d63a9..0c68e58 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -23,114 +23,114 @@ import frc4388.utility.RobotTime; * project. */ public class Robot extends TimedRobot { - Command m_autonomousCommand; + Command m_autonomousCommand; - private RobotTime m_robotTime = RobotTime.getInstance(); - private RobotContainer m_robotContainer; - //private LED mled = new LED(); - /** - * This function is run when the robot is first started up and should be - * used for any initialization code. - */ - @Override - public void robotInit() { - - // Instantiate our RobotContainer. This will perform all our button bindings, and put our - // autonomous chooser on the dashboard. - m_robotContainer = new RobotContainer(); - } - - /** - * This function is called every robot packet, no matter the mode. Use - * this for items like diagnostics that you want ran during disabled, - * autonomous, teleoperated and test.doubl - * - *

This runs after the mode specific periodic functions, but before - * LiveWindow and SmartDashboard integrated updating. - */ - @Override - public void robotPeriodic() { - m_robotTime.updateTimes(); - //System.out.println(m_robotContainer.limelight.isNearSpeaker()); - //mled.updateLED(); - // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled - // commands, running already-scheduled commands, removing finished or interrupted commands, - // and running subsystem periodic() methods. This must be called from the robot's periodic - // block in order for anything in the Command-based framework to work. - CommandScheduler.getInstance().run(); - } - - /** - * This function is called once each time the robot enters Disabled mode. - * You can use it to reset any subsystem information you want to clear when - * the robot is disabled. - */ - @Override - public void disabledInit() { - m_robotTime.endMatchTime(); - } - - @Override - public void disabledPeriodic() { - } - - @Override - public void disabledExit() { - DeferredBlock.execute(); - } - - /** - * This autonomous runs the autonomous command selected by your {@link RobotContainer} class. - */ - @Override - public void autonomousInit() { - m_autonomousCommand = m_robotContainer.getAutonomousCommand(); - - /* - * String autoSelected = SmartDashboard.getString("Auto Selector", - * "Default"); switch(autoSelected) { case "My Auto": autonomousCommand - * = new MyAutoCommand(); break; case "Default Auto": default: - * autonomousCommand = new ExampleCommand(); break; } + private RobotTime m_robotTime = RobotTime.getInstance(); + private RobotContainer m_robotContainer; + //private LED mled = new LED(); + /** + * This function is run when the robot is first started up and should be + * used for any initialization code. */ + @Override + public void robotInit() { - // schedule the autonomous command (example) - if (m_autonomousCommand != null) { - m_autonomousCommand.schedule(); + // Instantiate our RobotContainer. This will perform all our button bindings, and put our + // autonomous chooser on the dashboard. + m_robotContainer = new RobotContainer(); } - m_robotTime.startMatchTime(); - } - /** - * This function is called periodically during autonomous. - */ - @Override - public void autonomousPeriodic() { - } - - @Override - public void teleopInit() { - // This makes sure that the autonomous stops running when - // teleop starts running. If you want the autonomous to - // continue until interrupted by another command, remove - // this line or comment it out. - if (m_autonomousCommand != null) { - m_autonomousCommand.cancel(); + /** + * This function is called every robot packet, no matter the mode. Use + * this for items like diagnostics that you want ran during disabled, + * autonomous, teleoperated and test.doubl + * + *

This runs after the mode specific periodic functions, but before + * LiveWindow and SmartDashboard integrated updating. + */ + @Override + public void robotPeriodic() { + m_robotTime.updateTimes(); + //System.out.println(m_robotContainer.limelight.isNearSpeaker()); + //mled.updateLED(); + // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled + // commands, running already-scheduled commands, removing finished or interrupted commands, + // and running subsystem periodic() methods. This must be called from the robot's periodic + // block in order for anything in the Command-based framework to work. + CommandScheduler.getInstance().run(); } - m_robotTime.startMatchTime(); - } - /** - * This function is called periodically during operator control. - */ - @Override - public void teleopPeriodic() { + /** + * This function is called once each time the robot enters Disabled mode. + * You can use it to reset any subsystem information you want to clear when + * the robot is disabled. + */ + @Override + public void disabledInit() { + m_robotTime.endMatchTime(); + } + + @Override + public void disabledPeriodic() { + } + + @Override + public void disabledExit() { + DeferredBlock.execute(); + } + + /** + * This autonomous runs the autonomous command selected by your {@link RobotContainer} class. + */ + @Override + public void autonomousInit() { + m_autonomousCommand = m_robotContainer.getAutonomousCommand(); + + /* + * String autoSelected = SmartDashboard.getString("Auto Selector", + * "Default"); switch(autoSelected) { case "My Auto": autonomousCommand + * = new MyAutoCommand(); break; case "Default Auto": default: + * autonomousCommand = new ExampleCommand(); break; } + */ + + // schedule the autonomous command (example) + if (m_autonomousCommand != null) { + m_autonomousCommand.schedule(); + } + m_robotTime.startMatchTime(); + } + + /** + * This function is called periodically during autonomous. + */ + @Override + public void autonomousPeriodic() { + } + + @Override + public void teleopInit() { + // This makes sure that the autonomous stops running when + // teleop starts running. If you want the autonomous to + // continue until interrupted by another command, remove + // this line or comment it out. + if (m_autonomousCommand != null) { + m_autonomousCommand.cancel(); + } + m_robotTime.startMatchTime(); + } + + /** + * This function is called periodically during operator control. + */ + @Override + public void teleopPeriodic() { - } + } - /** - * This function is called periodically during test mode. - */ - @Override - public void testPeriodic() { - } + /** + * This function is called periodically during test mode. + */ + @Override + public void testPeriodic() { + } } diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 30d5cac..c46937c 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -86,11 +86,11 @@ public class RobotContainer { private Command interrupt = new InstantCommand(() -> {}, m_robotIntake, m_robotShooter); private ParallelCommandGroup intakeToShoot = new ParallelCommandGroup( - new InstantCommand(() -> m_robotIntake.talonPIDIn()), - new InstantCommand(() -> m_robotShooter.idle()), - new InstantCommand(() -> m_driverXbox.setRumble(null, 1.0)).andThen(new WaitCommand(1)).andThen(new InstantCommand(() -> m_driverXbox.setRumble(null, 0.0))) - //new InstantCommand(() -> m_robotShooter.spin()) - ); + new InstantCommand(() -> m_robotIntake.talonPIDIn()), + new InstantCommand(() -> m_robotShooter.idle()), + new InstantCommand(() -> m_driverXbox.setRumble(null, 1.0)).andThen(new WaitCommand(1)).andThen(new InstantCommand(() -> m_driverXbox.setRumble(null, 0.0))) + //new InstantCommand(() -> m_robotShooter.spin()) + ); // private SequentialCommandGroup outtakeToShootFull = new SequentialCommandGroup( // new InstantCommand(() -> m_robotShooter.spin()), @@ -109,44 +109,44 @@ public class RobotContainer { private AutoAlign autoAlign = new AutoAlign(m_robotSwerveDrive, limelight); private SequentialCommandGroup autoShoot = new SequentialCommandGroup( - // MoveToSpeaker, - autoAlign, - new InstantCommand(() -> m_robotShooter.spin()), - new WaitCommand(3.0), - new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake), - new WaitCommand(3.0), - new InstantCommand(() -> m_robotShooter.idle()), - new InstantCommand(() -> autoAlign.reverse()), - autoAlign.asProxy() - ); + // MoveToSpeaker, + autoAlign, + new InstantCommand(() -> m_robotShooter.spin()), + new WaitCommand(3.0), + new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake), + new WaitCommand(3.0), + new InstantCommand(() -> m_robotShooter.idle()), + new InstantCommand(() -> autoAlign.reverse()), + autoAlign.asProxy() + ); private SequentialCommandGroup i = new SequentialCommandGroup( - intakeToShootStuff, intakeToShoot, - new InstantCommand(() -> m_robotShooter.idle()) - ); + intakeToShootStuff, intakeToShoot, + new InstantCommand(() -> m_robotShooter.idle()) + ); private SequentialCommandGroup ejectToShoot = new SequentialCommandGroup( - new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter), - new WaitCommand(0.75), - new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake) - ); + new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter), + new WaitCommand(0.75), + new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake) + ); private SequentialCommandGroup turnOffShoot = new SequentialCommandGroup( - new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter), - new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake) - ); + new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter), + new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake) + ); private SequentialCommandGroup emergencyRetract = new SequentialCommandGroup( - interrupt, - new InstantCommand(() -> m_robotIntake.talonPIDIn(), m_robotIntake), - new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake) - ); + interrupt, + new InstantCommand(() -> m_robotIntake.talonPIDIn(), m_robotIntake), + new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake) + ); private SequentialCommandGroup ampShoot = new SequentialCommandGroup( - new InstantCommand(() -> m_robotIntake.ampPosition()), - new InstantCommand(() -> m_robotIntake.ampShoot(0.1)) //TODO: Find Actual Speed - ); + new InstantCommand(() -> m_robotIntake.ampPosition()), + new InstantCommand(() -> m_robotIntake.ampShoot(0.1)) //TODO: Find Actual Speed + ); // ! /* Autos */ private Command taxi = new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt"); //new InstantCommand(); @@ -167,114 +167,114 @@ public class RobotContainer { // ); private SequentialCommandGroup oneNoteStartingSpeaker = new SequentialCommandGroup ( - new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter), - new WaitCommand(1).asProxy(), - new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake), - new WaitCommand(1).asProxy(), - new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter), - new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake), - new WaitCommand(1).asProxy(), - new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt") - ); + new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter), + new WaitCommand(1).asProxy(), + new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake), + new WaitCommand(1).asProxy(), + new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter), + new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake), + new WaitCommand(1).asProxy(), + new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt") + ); private SequentialCommandGroup oneNoteStartingSpeakerStationary = new SequentialCommandGroup ( - new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter), - new WaitCommand(1).asProxy(), - new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake), - new WaitCommand(1).asProxy(), - new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter), - new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake) - ); + new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter), + new WaitCommand(1).asProxy(), + new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake), + new WaitCommand(1).asProxy(), + new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter), + new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake) + ); private SequentialCommandGroup oneNoteStartingFromLeft = new SequentialCommandGroup( - startLeftMoveRight.asProxy(), - ejectToShoot.asProxy(), - taxi.asProxy() - ); + startLeftMoveRight.asProxy(), + ejectToShoot.asProxy(), + taxi.asProxy() + ); private SequentialCommandGroup oneNoteStartingFromRight = new SequentialCommandGroup( - startRightMoveLeft.asProxy(), - ejectToShoot.asProxy(), - taxi.asProxy() - ); + startRightMoveLeft.asProxy(), + ejectToShoot.asProxy(), + taxi.asProxy() + ); private SequentialCommandGroup twoNoteStartingFromSpeaker = new SequentialCommandGroup( - new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter), - new WaitCommand(1).asProxy(), - new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake), - new WaitCommand(1).asProxy(), - new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter), - new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake), - new ArmIntakeInAuto(m_robotIntake, m_robotShooter, m_robotSwerveDrive), - new InstantCommand(() -> m_robotIntake.talonPIDIn()), - new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter), - new WaitCommand(1.4).asProxy(), - new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake), - new WaitCommand(0.5), - new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter), - new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake), - new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt") - // new WaitCommand(1).asProxy(), - // new JoystickPlayback(m_robotSwerveDrive, "TwoNotePrt2.txt"), - // new WaitCommand(0.5).asProxy(), - // new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter), - // new WaitCommand(1).asProxy(), - // new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake), - // new WaitCommand(1).asProxy(), - // new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter), - // new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake) - ); + new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter), + new WaitCommand(1).asProxy(), + new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake), + new WaitCommand(1).asProxy(), + new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter), + new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake), + new ArmIntakeInAuto(m_robotIntake, m_robotShooter, m_robotSwerveDrive), + new InstantCommand(() -> m_robotIntake.talonPIDIn()), + new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter), + new WaitCommand(1.4).asProxy(), + new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake), + new WaitCommand(0.5), + new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter), + new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake), + new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt") + // new WaitCommand(1).asProxy(), + // new JoystickPlayback(m_robotSwerveDrive, "TwoNotePrt2.txt"), + // new WaitCommand(0.5).asProxy(), + // new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter), + // new WaitCommand(1).asProxy(), + // new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake), + // new WaitCommand(1).asProxy(), + // new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter), + // new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake) + ); private SequentialCommandGroup stayTwoNoteStartingFromSpeaker = new SequentialCommandGroup( - new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter), - new WaitCommand(1).asProxy(), - new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake), - new WaitCommand(1).asProxy(), - new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter), - new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake), - new ArmIntakeInAuto(m_robotIntake, m_robotShooter, m_robotSwerveDrive), - new InstantCommand(() -> m_robotIntake.talonPIDIn()), - new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter), - new WaitCommand(1.4).asProxy(), - new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake), - new WaitCommand(0.5), - new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter), - new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake) - ); + new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter), + new WaitCommand(1).asProxy(), + new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake), + new WaitCommand(1).asProxy(), + new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter), + new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake), + new ArmIntakeInAuto(m_robotIntake, m_robotShooter, m_robotSwerveDrive), + new InstantCommand(() -> m_robotIntake.talonPIDIn()), + new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter), + new WaitCommand(1.4).asProxy(), + new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake), + new WaitCommand(0.5), + new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter), + new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake) + ); private SequentialCommandGroup threeNoteStartingFromSpeaker = new SequentialCommandGroup( - new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter), - new WaitCommand(1).asProxy(), - new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake), - new WaitCommand(1).asProxy(), - new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter), - new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake), - new ArmIntakeInAuto(m_robotIntake, m_robotShooter, m_robotSwerveDrive), - new InstantCommand(() -> m_robotIntake.talonPIDIn()), - new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter), - new WaitCommand(1.4).asProxy(), - new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake), - new WaitCommand(0.5), - new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter), - new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake), - //? Create Another Parallel Command Group :( - new InstantCommand(() -> m_robotIntake.talonPIDIn()), - new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter), - new WaitCommand(1.4).asProxy(), - new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake), - new WaitCommand(0.5), - new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter), - new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake), - new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt") - ); + new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter), + new WaitCommand(1).asProxy(), + new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake), + new WaitCommand(1).asProxy(), + new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter), + new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake), + new ArmIntakeInAuto(m_robotIntake, m_robotShooter, m_robotSwerveDrive), + new InstantCommand(() -> m_robotIntake.talonPIDIn()), + new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter), + new WaitCommand(1.4).asProxy(), + new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake), + new WaitCommand(0.5), + new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter), + new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake), + //? Create Another Parallel Command Group :( + new InstantCommand(() -> m_robotIntake.talonPIDIn()), + new InstantCommand(() -> m_robotShooter.spin(), m_robotShooter), + new WaitCommand(1.4).asProxy(), + new InstantCommand(() -> m_robotIntake.talonHandoff(), m_robotIntake), + new WaitCommand(0.5), + new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter), + new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake), + new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt") + ); private PlaybackChooser playbackChooser = new PlaybackChooser(m_robotSwerveDrive) - .addOption("Taxi Auto", taxi.asProxy()) - .addOption("One Note Auto Starting in Front of Speaker", oneNoteStartingSpeaker.asProxy()) - .addOption("Stay One Note Auto Starting in Front of Speaker", oneNoteStartingSpeakerStationary.asProxy()) - // .addOption("One Note Auto Starting from Left Position", oneNoteStartingFromLeft.asProxy()) - // .addOption("One Note Auto Starting from Right Position", oneNoteStartingFromRight.asProxy()) - .addOption("Two Note Starting in Front of Speaker", twoNoteStartingFromSpeaker.asProxy()) - .addOption("Stay Two Note Starting in Front of Speaker", stayTwoNoteStartingFromSpeaker.asProxy()) - .buildDisplay(); + .addOption("Taxi Auto", taxi.asProxy()) + .addOption("One Note Auto Starting in Front of Speaker", oneNoteStartingSpeaker.asProxy()) + .addOption("Stay One Note Auto Starting in Front of Speaker", oneNoteStartingSpeakerStationary.asProxy()) + // .addOption("One Note Auto Starting from Left Position", oneNoteStartingFromLeft.asProxy()) + // .addOption("One Note Auto Starting from Right Position", oneNoteStartingFromRight.asProxy()) + .addOption("Two Note Starting in Front of Speaker", twoNoteStartingFromSpeaker.asProxy()) + .addOption("Stay Two Note Starting in Front of Speaker", stayTwoNoteStartingFromSpeaker.asProxy()) + .buildDisplay(); @@ -287,19 +287,19 @@ public class RobotContainer { new DeferredBlock(() -> m_robotSwerveDrive.resetGyro()); - DriverStation.silenceJoystickConnectionWarning(true); - CameraServer.startAutomaticCapture(); + DriverStation.silenceJoystickConnectionWarning(true); + CameraServer.startAutomaticCapture(); - /* Default Commands */ - // drives the robot with a two-axis input from the driver controller - // ! Swerve Drive Default Command (Regular Rotation) - m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> { - m_robotSwerveDrive.driveWithInput(getDeadbandedDriverController().getLeft(), - getDeadbandedDriverController().getRight(), - true); - }, m_robotSwerveDrive) + /* Default Commands */ + // drives the robot with a two-axis input from the driver controller + // ! Swerve Drive Default Command (Regular Rotation) + m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> { + m_robotSwerveDrive.driveWithInput(getDeadbandedDriverController().getLeft(), + getDeadbandedDriverController().getRight(), + true); + }, m_robotSwerveDrive) .withName("SwerveDrive DefaultCommand")); - m_robotSwerveDrive.setToSlow(); + m_robotSwerveDrive.setToSlow(); // ! Swerve Drive Default Command (Orientation Rotation) // m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> { @@ -356,25 +356,25 @@ public class RobotContainer { // .onFalse(new InstantCommand()); new JoystickButton(m_autoRecorderXbox, XboxController.LEFT_BUMPER_BUTTON) - .whileTrue(new neoJoystickRecorder(m_robotSwerveDrive, - new DeadbandedXboxController[]{getDeadbandedDriverController(), getDeadbandedOperatorController()}, - () -> autoplaybackName.get())) - .onFalse(new InstantCommand()); + .whileTrue(new neoJoystickRecorder(m_robotSwerveDrive, + new DeadbandedXboxController[]{getDeadbandedDriverController(), getDeadbandedOperatorController()}, + () -> autoplaybackName.get())) + .onFalse(new InstantCommand()); new JoystickButton(m_autoRecorderXbox, XboxController.RIGHT_BUMPER_BUTTON) - .onTrue(new neoJoystickPlayback(m_robotSwerveDrive, - () -> autoplaybackName.get(), - new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()}, - true, false)) - .onFalse(new InstantCommand()); + .onTrue(new neoJoystickPlayback(m_robotSwerveDrive, + () -> autoplaybackName.get(), + new VirtualController[]{getVirtualDriverController(), getVirtualOperatorController()}, + true, false)) + .onFalse(new InstantCommand()); // ! /* Speed */ - // new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final - // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp())); - // // .onFalse(new InstantCommand(() -> m_robotSwerveDrive.setToFast())); + // new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final + // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp())); + // // .onFalse(new InstantCommand(() -> m_robotSwerveDrive.setToFast())); - // new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) // final - // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown())); + // new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) // final + // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown())); // new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) // .whileTrue(new InstantCommand(() -> @@ -383,7 +383,7 @@ public class RobotContainer { // true), m_robotSwerveDrive)); - //? /* Operator Buttons */ + //? /* Operator Buttons */ new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON) .onTrue(new InstantCommand(() -> m_robotIntake.talonPIDIn())) @@ -398,7 +398,7 @@ public class RobotContainer { .onFalse(new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors())); new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON) - .onTrue(emergencyRetract); + .onTrue(emergencyRetract); // Override Intake Position encoder: out @@ -443,7 +443,7 @@ public class RobotContainer { } - private void configureVirtualButtonBindings() { + private void configureVirtualButtonBindings() { /* Driver Buttons */ // new JoystickButton(getVirtualDriverController(), XboxController.A_BUTTON) // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive)); @@ -462,7 +462,7 @@ public class RobotContainer { // /* Operator Buttons */ - new JoystickButton(getVirtualOperatorController(), XboxController.Y_BUTTON) + new JoystickButton(getVirtualOperatorController(), XboxController.Y_BUTTON) .onTrue(new InstantCommand(() -> m_robotIntake.talonPIDIn())) .onFalse(new InstantCommand(() -> m_robotIntake.talonStopArmMotor())); @@ -475,7 +475,7 @@ public class RobotContainer { .onFalse(new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors())); new JoystickButton(getVirtualOperatorController(), XboxController.B_BUTTON) - .onTrue(emergencyRetract.asProxy()); + .onTrue(emergencyRetract.asProxy()); // Override Intake Position encoder: out diff --git a/src/main/java/frc4388/robot/commands/Autos/ArmIntakeInAuto.java b/src/main/java/frc4388/robot/commands/Autos/ArmIntakeInAuto.java index cb6ac90..a971029 100644 --- a/src/main/java/frc4388/robot/commands/Autos/ArmIntakeInAuto.java +++ b/src/main/java/frc4388/robot/commands/Autos/ArmIntakeInAuto.java @@ -16,16 +16,16 @@ import frc4388.robot.subsystems.SwerveDrive; // information, see: // https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html public class ArmIntakeInAuto extends ParallelCommandGroup { - private final Intake intake; - private final Shooter shooter; - private final SwerveDrive swerve; - /** Creates a new ArmIntakeInAuto. */ - public ArmIntakeInAuto(Intake intake, Shooter shooter, SwerveDrive swerve) { - // Add your commands in the addCommands() call, e.g. - // addCommands(new FooCommand(), new BarCommand()); - this.intake = intake; - this.shooter = shooter; - this.swerve = swerve; - addCommands((new ArmIntakeInTimeout(intake, shooter).withTimeout(3)), new WaitCommand(0.5).andThen(new JoystickPlayback(swerve, "TwoNotePrt1.txt"))); - } + private final Intake intake; + private final Shooter shooter; + private final SwerveDrive swerve; + /** Creates a new ArmIntakeInAuto. */ + public ArmIntakeInAuto(Intake intake, Shooter shooter, SwerveDrive swerve) { + // Add your commands in the addCommands() call, e.g. + // addCommands(new FooCommand(), new BarCommand()); + this.intake = intake; + this.shooter = shooter; + this.swerve = swerve; + addCommands((new ArmIntakeInTimeout(intake, shooter).withTimeout(3)), new WaitCommand(0.5).andThen(new JoystickPlayback(swerve, "TwoNotePrt1.txt"))); + } } diff --git a/src/main/java/frc4388/robot/commands/Autos/ArmIntakeInTimeout.java b/src/main/java/frc4388/robot/commands/Autos/ArmIntakeInTimeout.java index 1e1e523..d07a40e 100644 --- a/src/main/java/frc4388/robot/commands/Autos/ArmIntakeInTimeout.java +++ b/src/main/java/frc4388/robot/commands/Autos/ArmIntakeInTimeout.java @@ -10,49 +10,49 @@ import frc4388.robot.subsystems.Shooter; import frc4388.robot.subsystems.SwerveDrive; public class ArmIntakeInTimeout extends Command { - /** Creates a new ArmIntakeIn. */ - private Intake robotIntake; - private Shooter robotShooter; + /** Creates a new ArmIntakeIn. */ + private Intake robotIntake; + private Shooter robotShooter; - public ArmIntakeInTimeout(Intake robotIntake, Shooter robotShooter) { - // Use addRequirements() here to declare subsystem dependencies. - this.robotIntake = robotIntake; - this.robotShooter = robotShooter; + public ArmIntakeInTimeout(Intake robotIntake, Shooter robotShooter) { + // Use addRequirements() here to declare subsystem dependencies. + this.robotIntake = robotIntake; + this.robotShooter = robotShooter; - addRequirements(this.robotIntake, this.robotShooter); - } - - // 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() { - robotIntake.talonPIDOut(); - robotIntake.talonSpinIntakeMotor(); - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - if(interrupted) { - robotIntake.talonPIDIn(); - robotIntake.talonStopIntakeMotors(); + addRequirements(this.robotIntake, this.robotShooter); } - } - // Returns true when the command should end. - @Override - public boolean isFinished() { - return robotIntake.getTalonIntakeLimitSwitchState(); - // if(!(!robotIntake.getTalonIntakeLimitSwitchState() != !false) && ((-1.0 / 0.0) == (-2.0 / 0.0))) - // { - // return !true==true; - // } - // else - // { - // return !false==!(!(true)); - // } - } + // 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() { + robotIntake.talonPIDOut(); + robotIntake.talonSpinIntakeMotor(); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + if(interrupted) { + robotIntake.talonPIDIn(); + robotIntake.talonStopIntakeMotors(); + } + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return robotIntake.getTalonIntakeLimitSwitchState(); + // if(!(!robotIntake.getTalonIntakeLimitSwitchState() != !false) && ((-1.0 / 0.0) == (-2.0 / 0.0))) + // { + // return !true==true; + // } + // else + // { + // return !false==!(!(true)); + // } + } } diff --git a/src/main/java/frc4388/robot/commands/Autos/AutoAlign.java b/src/main/java/frc4388/robot/commands/Autos/AutoAlign.java index ce8b37c..0d59707 100644 --- a/src/main/java/frc4388/robot/commands/Autos/AutoAlign.java +++ b/src/main/java/frc4388/robot/commands/Autos/AutoAlign.java @@ -118,18 +118,18 @@ public class AutoAlign extends Command { private Translation2d calcRotStick(){ double error = pose.getRotation().getDegrees() - targetRot.getDegrees(); - cumError += error * .02; // 20 ms - double delta = error - prevError; + cumError += error * .02; // 20 ms + double delta = error - prevError; final double kP = 4; final double kI = 4; final double kD = 4; final double kF = 4; - double output = error * kP; - output += cumError * kI; - output += delta * kD; - output += kF; + double output = error * kP; + output += cumError * kI; + output += delta * kD; + output += kF; prevError = error; return clamp(output, 0); @@ -140,8 +140,8 @@ public class AutoAlign extends Command { } // Called when the command is initially scheduled. - @Override - public final void initialize() { + @Override + public final void initialize() { isRed = alliance.get() == DriverStation.Alliance.Red; if(reverseAfterFinish){ // isReverseFinished = false; @@ -150,10 +150,10 @@ public class AutoAlign extends Command { moveStickReplayArr = new Translation2d[]{}; rotStickReplayArr = new Translation2d[]{}; } - } + } // Called every time the scheduler runs while the command is scheduled. - @Override + @Override public void execute() { // Update limelight pos pose = limelight.getPose(); @@ -179,7 +179,7 @@ public class AutoAlign extends Command { // } isFinished = limelight.isNearSpeaker(); - // If reverseAfterFinish, then loop back over and replay in reverse + // If reverseAfterFinish, then loop back over and replay in reverse }else if(reverseAfterFinish && !isReverseFinished){ // Get reverse direction moveStick = moveStickReplayArr[replayIndex-moveStickReplayArr.length-1]; @@ -201,8 +201,8 @@ public class AutoAlign extends Command { } // Returns true when the command should end. - @Override - public final boolean isFinished() { + @Override + public final boolean isFinished() { return isFinished && (isReverseFinished || !reverseAfterFinish); - } -} \ No newline at end of file + } +} diff --git a/src/main/java/frc4388/robot/commands/Autos/AutoBalance.java b/src/main/java/frc4388/robot/commands/Autos/AutoBalance.java index 312325e..3752091 100644 --- a/src/main/java/frc4388/robot/commands/Autos/AutoBalance.java +++ b/src/main/java/frc4388/robot/commands/Autos/AutoBalance.java @@ -17,33 +17,33 @@ import frc4388.utility.RobotGyro; // information, see: // https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html public class AutoBalance extends PID { - RobotGyro gyro; - Intake intake; - /** Creates a new AutoBalance. */ - public AutoBalance(RobotGyro gyro, Intake intake) { - super(0.6, 0, 0, 0, 0); + RobotGyro gyro; + Intake intake; + /** Creates a new AutoBalance. */ + public AutoBalance(RobotGyro gyro, Intake intake) { + super(0.6, 0, 0, 0, 0); - this.gyro = gyro; - this.intake = intake; + this.gyro = gyro; + this.intake = intake; - // Use addRequirements() here to declare subsystem dependencies. - // Configure additional PID options by calling `getController` here. - addRequirements(intake); - } + // Use addRequirements() here to declare subsystem dependencies. + // Configure additional PID options by calling `getController` here. + addRequirements(intake); + } - // Returns true when the command should end. + // Returns true when the command should end. - public double getError() { - var pitch = gyro.getRoll(); - SmartDashboard.putNumber("pitch", pitch); - return pitch; - } + public double getError() { + var pitch = gyro.getRoll(); + SmartDashboard.putNumber("pitch", pitch); + return pitch; + } - @Override - public void runWithOutput(double output) { - double out2 = MathUtil.clamp(output / 40, -59, 0); - if (Math.abs(getError()) < 3) out2 = 0; - intake.talonPIDPosition(out2); - } + @Override + public void runWithOutput(double output) { + double out2 = MathUtil.clamp(output / 40, -59, 0); + if (Math.abs(getError()) < 3) out2 = 0; + intake.talonPIDPosition(out2); + } } diff --git a/src/main/java/frc4388/robot/commands/Intake/ArmIntakeIn.java b/src/main/java/frc4388/robot/commands/Intake/ArmIntakeIn.java index 78c558b..acc8b34 100644 --- a/src/main/java/frc4388/robot/commands/Intake/ArmIntakeIn.java +++ b/src/main/java/frc4388/robot/commands/Intake/ArmIntakeIn.java @@ -10,44 +10,44 @@ import frc4388.robot.subsystems.Shooter; import frc4388.robot.subsystems.SwerveDrive; public class ArmIntakeIn extends Command { - /** Creates a new ArmIntakeIn. */ - private Intake robotIntake; - private Shooter robotShooter; + /** Creates a new ArmIntakeIn. */ + private Intake robotIntake; + private Shooter robotShooter; - public ArmIntakeIn(Intake robotIntake, Shooter robotShooter) { - // Use addRequirements() here to declare subsystem dependencies. - this.robotIntake = robotIntake; - this.robotShooter = robotShooter; + public ArmIntakeIn(Intake robotIntake, Shooter robotShooter) { + // Use addRequirements() here to declare subsystem dependencies. + this.robotIntake = robotIntake; + this.robotShooter = robotShooter; - addRequirements(this.robotIntake, this.robotShooter); - } + addRequirements(this.robotIntake, this.robotShooter); + } - // Called when the command is initially scheduled. - @Override - public void initialize() {} + // 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() { - robotIntake.talonPIDOut(); - robotIntake.talonSpinIntakeMotor(); - } + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + robotIntake.talonPIDOut(); + robotIntake.talonSpinIntakeMotor(); + } - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) {} + // 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 robotIntake.getTalonIntakeLimitSwitchState(); - // if(!(!robotIntake.getTalonIntakeLimitSwitchState() != !false) && ((-1.0 / 0.0) == (-2.0 / 0.0))) - // { - // return !true==true; - // } - // else - // { - // return !false==!(!(true)); - // } - } + // Returns true when the command should end. + @Override + public boolean isFinished() { + return robotIntake.getTalonIntakeLimitSwitchState(); + // if(!(!robotIntake.getTalonIntakeLimitSwitchState() != !false) && ((-1.0 / 0.0) == (-2.0 / 0.0))) + // { + // return !true==true; + // } + // else + // { + // return !false==!(!(true)); + // } + } } diff --git a/src/main/java/frc4388/robot/commands/PID.java b/src/main/java/frc4388/robot/commands/PID.java index 97233f8..1d317c5 100644 --- a/src/main/java/frc4388/robot/commands/PID.java +++ b/src/main/java/frc4388/robot/commands/PID.java @@ -8,53 +8,53 @@ import edu.wpi.first.wpilibj2.command.Command; import frc4388.utility.Gains; public abstract class PID extends Command { - protected Gains gains; - private double output = 0; - private double tolerance = 0; + protected Gains gains; + private double output = 0; + private double tolerance = 0; - /** Creates a new PelvicInflammatoryDisease. */ - public PID(double kp, double ki, double kd, double kf, double tolerance) { - gains = new Gains(kp, ki, kd, kf, 0); - this.tolerance = tolerance; - } + /** Creates a new PelvicInflammatoryDisease. */ + public PID(double kp, double ki, double kd, double kf, double tolerance) { + gains = new Gains(kp, ki, kd, kf, 0); + this.tolerance = tolerance; + } - public PID(Gains gains, double tolerance) { - this.gains = gains; - this.tolerance = tolerance; - } + public PID(Gains gains, double tolerance) { + this.gains = gains; + this.tolerance = tolerance; + } - /** produces the error from the setpoint */ - public abstract double getError(); + /** produces the error from the setpoint */ + public abstract double getError(); - /** todo: javadoc */ - public abstract void runWithOutput(double output); + /** todo: javadoc */ + public abstract void runWithOutput(double output); - // Called when the command is initially scheduled. - @Override - public final void initialize() { - output = 0; - } + // Called when the command is initially scheduled. + @Override + public final void initialize() { + output = 0; + } - private double prevError, cumError = 0; + private double prevError, cumError = 0; - // Called every time the scheduler runs while the command is scheduled. - @Override - public final void execute() { - double error = getError(); - cumError += error * .02; // 20 ms - double delta = error - prevError; + // Called every time the scheduler runs while the command is scheduled. + @Override + public final void execute() { + double error = getError(); + cumError += error * .02; // 20 ms + double delta = error - prevError; - output = error * gains.kP; - output += cumError * gains.kI; - output += delta * gains.kD; - output += gains.kF; + output = error * gains.kP; + output += cumError * gains.kI; + output += delta * gains.kD; + output += gains.kF; - runWithOutput(output); - } + runWithOutput(output); + } - // Returns true when the command should end. - @Override - public final boolean isFinished() { - return Math.abs(getError()) < tolerance; - } + // Returns true when the command should end. + @Override + public final boolean isFinished() { + return Math.abs(getError()) < tolerance; + } } diff --git a/src/main/java/frc4388/robot/commands/Swerve/JoystickPlayback.java b/src/main/java/frc4388/robot/commands/Swerve/JoystickPlayback.java index e92b487..4050518 100644 --- a/src/main/java/frc4388/robot/commands/Swerve/JoystickPlayback.java +++ b/src/main/java/frc4388/robot/commands/Swerve/JoystickPlayback.java @@ -14,132 +14,132 @@ import frc4388.robot.subsystems.SwerveDrive; import frc4388.utility.UtilityStructs.TimedOutput; public class JoystickPlayback extends CommandBase { - private final SwerveDrive swerve; - private String filename; - private int mult = 1; - private Scanner input; - private final ArrayList outputs = new ArrayList<>(); - private int counter = 0; - private long startTime = 0; - private long playbackTime = 0; - private int lastIndex; - private boolean m_finished = false; // ! find a better way + private final SwerveDrive swerve; + private String filename; + private int mult = 1; + private Scanner input; + private final ArrayList outputs = new ArrayList<>(); + private int counter = 0; + private long startTime = 0; + private long playbackTime = 0; + private int lastIndex; + private boolean m_finished = false; // ! find a better way - /** Creates a new JoystickPlayback. */ - public JoystickPlayback(SwerveDrive swerve, String filename, int mult) { - this.swerve = swerve; - this.filename = filename; - this.mult = mult; + /** Creates a new JoystickPlayback. */ + public JoystickPlayback(SwerveDrive swerve, String filename, int mult) { + this.swerve = swerve; + this.filename = filename; + this.mult = mult; - addRequirements(this.swerve); - } + addRequirements(this.swerve); + } - /** Creates a new JoystickPlayback. */ - public JoystickPlayback(SwerveDrive swerve, String filename) { - this(swerve, filename, 1); - } + /** Creates a new JoystickPlayback. */ + public JoystickPlayback(SwerveDrive swerve, String filename) { + this(swerve, filename, 1); + } - // Called when the command is initially scheduled. - @Override - public void initialize() { - outputs.clear(); - m_finished = false; + // Called when the command is initially scheduled. + @Override + public void initialize() { + outputs.clear(); + m_finished = false; - startTime = System.currentTimeMillis(); - playbackTime = 0; - lastIndex = 0; - try { - input = new Scanner(new File("/home/lvuser/autos/" + filename)); + startTime = System.currentTimeMillis(); + playbackTime = 0; + lastIndex = 0; + try { + input = new Scanner(new File("/home/lvuser/autos/" + filename)); - String line = ""; - while (input.hasNextLine()) { - line = input.nextLine(); + String line = ""; + while (input.hasNextLine()) { + line = input.nextLine(); - if (line.isEmpty() || line.isBlank() || line.equals("\n")) { - continue; - } + if (line.isEmpty() || line.isBlank() || line.equals("\n")) { + continue; + } - String[] values = line.split(","); - System.out.println("values: " + values[0] + " " + values[1] + " " + values[2] + " " + values[3]); + String[] values = line.split(","); + System.out.println("values: " + values[0] + " " + values[1] + " " + values[2] + " " + values[3]); - var out = new TimedOutput(); - out.leftX = Double.parseDouble(values[0]) * mult; - out.leftY = Double.parseDouble(values[1]); - out.rightX = Double.parseDouble(values[2]); - out.rightY = Double.parseDouble(values[3]); + var out = new TimedOutput(); + out.leftX = Double.parseDouble(values[0]) * mult; + out.leftY = Double.parseDouble(values[1]); + out.rightX = Double.parseDouble(values[2]); + out.rightY = Double.parseDouble(values[3]); - out.timedOffset = Long.parseLong(values[4]); + out.timedOffset = Long.parseLong(values[4]); - outputs.add(out); - } + outputs.add(out); + } - input.close(); - } catch (FileNotFoundException e) { - e.printStackTrace(); - } - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - if (counter == 0) { - startTime = System.currentTimeMillis(); - playbackTime = 0; - } else { - playbackTime = System.currentTimeMillis() - startTime; + input.close(); + } catch (FileNotFoundException e) { + e.printStackTrace(); + } } - // skip to reasonable time frame - // too tired to write comment: ask daniel thomas; it goes to the thing until it's bigger than the other thing - { - int i = lastIndex == 0 ? 1 : lastIndex; - while (i < outputs.size() && outputs.get(i).timedOffset < playbackTime) { - i++; - } + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + if (counter == 0) { + startTime = System.currentTimeMillis(); + playbackTime = 0; + } else { + playbackTime = System.currentTimeMillis() - startTime; + } - if (i >= outputs.size()) { - m_finished = true; // ! kind of a hack - return; - } - lastIndex = i; - } + // skip to reasonable time frame + // too tired to write comment: ask daniel thomas; it goes to the thing until it's bigger than the other thing + { + int i = lastIndex == 0 ? 1 : lastIndex; + while (i < outputs.size() && outputs.get(i).timedOffset < playbackTime) { + i++; + } - TimedOutput lastOut = outputs.get(lastIndex - 1); - TimedOutput out = outputs.get(lastIndex); + if (i >= outputs.size()) { + m_finished = true; // ! kind of a hack + return; + } + lastIndex = i; + } - double deltaTime = out.timedOffset - lastOut.timedOffset; - double playbackDelta = playbackTime - lastOut.timedOffset; + TimedOutput lastOut = outputs.get(lastIndex - 1); + TimedOutput out = outputs.get(lastIndex); - double lerpLX = lastOut.leftX + (out.leftX - lastOut.leftX) * (playbackDelta / deltaTime); - double lerpLY = lastOut.leftY + (out.leftY - lastOut.leftY) * (playbackDelta / deltaTime); - double lerpRX = lastOut.rightX + (out.rightX - lastOut.rightX) * (playbackDelta / deltaTime); - double lerpRY = lastOut.rightY + (out.rightY - lastOut.rightY) * (playbackDelta / deltaTime); + double deltaTime = out.timedOffset - lastOut.timedOffset; + double playbackDelta = playbackTime - lastOut.timedOffset; - // this.swerve.driveWithInput(new Translation2d(out.leftX, out.leftY), - // new Translation2d(out.rightX, out.rightY), - // true); + double lerpLX = lastOut.leftX + (out.leftX - lastOut.leftX) * (playbackDelta / deltaTime); + double lerpLY = lastOut.leftY + (out.leftY - lastOut.leftY) * (playbackDelta / deltaTime); + double lerpRX = lastOut.rightX + (out.rightX - lastOut.rightX) * (playbackDelta / deltaTime); + double lerpRY = lastOut.rightY + (out.rightY - lastOut.rightY) * (playbackDelta / deltaTime); + + // this.swerve.driveWithInput(new Translation2d(out.leftX, out.leftY), + // new Translation2d(out.rightX, out.rightY), + // true); - // this.swerve.driveWithInput( new Translation2d(lerpLX, lerpLY), - // new Translation2d(lerpRX, lerpRY), - // true); + // this.swerve.driveWithInput( new Translation2d(lerpLX, lerpLY), + // new Translation2d(lerpRX, lerpRY), + // true); - this.swerve.playbackDriveWithInput( new Translation2d(lerpLX, lerpLY), - new Translation2d(lerpRX, lerpRY), - true); + this.swerve.playbackDriveWithInput( new Translation2d(lerpLX, lerpLY), + new Translation2d(lerpRX, lerpRY), + true); - counter++; - } + counter++; + } - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - input.close(); - swerve.stopModules(); - } + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + input.close(); + swerve.stopModules(); + } - // Returns true when the command should end. - @Override - public boolean isFinished() { - return m_finished; - } -} \ No newline at end of file + // Returns true when the command should end. + @Override + public boolean isFinished() { + return m_finished; + } +} diff --git a/src/main/java/frc4388/robot/commands/Swerve/JoystickRecorder.java b/src/main/java/frc4388/robot/commands/Swerve/JoystickRecorder.java index b4907c5..9dd154a 100644 --- a/src/main/java/frc4388/robot/commands/Swerve/JoystickRecorder.java +++ b/src/main/java/frc4388/robot/commands/Swerve/JoystickRecorder.java @@ -16,82 +16,82 @@ import frc4388.robot.subsystems.SwerveDrive; import frc4388.utility.UtilityStructs.TimedOutput; public class JoystickRecorder extends CommandBase { - public final SwerveDrive swerve; + public final SwerveDrive swerve; - public final Supplier leftX; - public final Supplier leftY; - public final Supplier rightX; - public final Supplier rightY; - private String filename; - public final ArrayList outputs = new ArrayList<>(); - private long startTime = -1; + public final Supplier leftX; + public final Supplier leftY; + public final Supplier rightX; + public final Supplier rightY; + private String filename; + public final ArrayList outputs = new ArrayList<>(); + private long startTime = -1; - /** Creates a new JoystickRecorder. */ - public JoystickRecorder(SwerveDrive swerve, Supplier leftX, Supplier leftY, - Supplier rightX, Supplier rightY, - String filename) - { - this.swerve = swerve; - this.leftX = leftX; - this.leftY = leftY; - this.rightX = rightX; - this.rightY = rightY; - this.filename = filename; + /** Creates a new JoystickRecorder. */ + public JoystickRecorder(SwerveDrive swerve, Supplier leftX, Supplier leftY, + Supplier rightX, Supplier rightY, + String filename) + { + this.swerve = swerve; + this.leftX = leftX; + this.leftY = leftY; + this.rightX = rightX; + this.rightY = rightY; + this.filename = filename; - addRequirements(this.swerve); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - outputs.clear(); - - this.startTime = System.currentTimeMillis(); - - outputs.add(new TimedOutput()); - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - var inputs = new TimedOutput(); - inputs.leftX = leftX.get(); - inputs.leftY = leftY.get(); - inputs.rightX = rightX.get(); - inputs.rightY = rightY.get(); - inputs.timedOffset = System.currentTimeMillis() - startTime; - - outputs.add(inputs); - - swerve.playbackDriveWithInput(new Translation2d(inputs.leftX, inputs.leftY), - new Translation2d(inputs.rightX, inputs.rightY), - true); - - System.out.println("RECORDING"); - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - File output = new File("/home/lvuser/autos/" + filename); - - try (PrintWriter writer = new PrintWriter(output)) { - for (var input : outputs) { - writer.println( input.leftX + "," + input.leftY + "," + - input.rightX + "," + input.rightY + "," + - input.timedOffset); - } - - writer.close(); - } catch (IOException e) { - e.printStackTrace(); + addRequirements(this.swerve); } - } - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} \ No newline at end of file + // Called when the command is initially scheduled. + @Override + public void initialize() { + outputs.clear(); + + this.startTime = System.currentTimeMillis(); + + outputs.add(new TimedOutput()); + } + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + var inputs = new TimedOutput(); + inputs.leftX = leftX.get(); + inputs.leftY = leftY.get(); + inputs.rightX = rightX.get(); + inputs.rightY = rightY.get(); + inputs.timedOffset = System.currentTimeMillis() - startTime; + + outputs.add(inputs); + + swerve.playbackDriveWithInput(new Translation2d(inputs.leftX, inputs.leftY), + new Translation2d(inputs.rightX, inputs.rightY), + true); + + System.out.println("RECORDING"); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) { + File output = new File("/home/lvuser/autos/" + filename); + + try (PrintWriter writer = new PrintWriter(output)) { + for (var input : outputs) { + writer.println( input.leftX + "," + input.leftY + "," + + input.rightX + "," + input.rightY + "," + + input.timedOffset); + } + + writer.close(); + } catch (IOException e) { + e.printStackTrace(); + } + } + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc4388/robot/commands/Swerve/RotateToAngle.java b/src/main/java/frc4388/robot/commands/Swerve/RotateToAngle.java index a2945c0..8816150 100644 --- a/src/main/java/frc4388/robot/commands/Swerve/RotateToAngle.java +++ b/src/main/java/frc4388/robot/commands/Swerve/RotateToAngle.java @@ -10,26 +10,26 @@ import frc4388.robot.subsystems.SwerveDrive; public class RotateToAngle extends PID { - SwerveDrive drive; - double targetAngle; + SwerveDrive drive; + double targetAngle; - /** Creates a new RotateToAngle. */ - public RotateToAngle(SwerveDrive drive, double targetAngle) { - super(0.3, 0.0, 0.0, 0.0, 1); + /** Creates a new RotateToAngle. */ + public RotateToAngle(SwerveDrive drive, double targetAngle) { + super(0.3, 0.0, 0.0, 0.0, 1); - this.drive = drive; - this.targetAngle = targetAngle; + this.drive = drive; + this.targetAngle = targetAngle; - addRequirements(drive); - } + addRequirements(drive); + } - @Override - public double getError() { - return targetAngle - drive.getGyroAngle(); - } + @Override + public double getError() { + return targetAngle - drive.getGyroAngle(); + } - @Override - public void runWithOutput(double output) { - drive.driveWithInput(new Translation2d(0.0, 0.0), new Translation2d(output / Math.abs(getError()), 0.0), true); - } + @Override + public void runWithOutput(double output) { + drive.driveWithInput(new Translation2d(0.0, 0.0), new Translation2d(output / Math.abs(getError()), 0.0), true); + } } diff --git a/src/main/java/frc4388/robot/commands/Swerve/neoJoystickPlayback.java b/src/main/java/frc4388/robot/commands/Swerve/neoJoystickPlayback.java index a5ab006..2c3e1e2 100644 --- a/src/main/java/frc4388/robot/commands/Swerve/neoJoystickPlayback.java +++ b/src/main/java/frc4388/robot/commands/Swerve/neoJoystickPlayback.java @@ -65,7 +65,7 @@ public class neoJoystickPlayback extends Command { if (m_numControllers > controllers.length) { System.out.println("AUTOPLAYBACK: The auto file `" + filename + "` wants " + m_numControllers - + " virtual controllers but only " + controllers.length + " were given"); + + " virtual controllers but only " + controllers.length + " were given"); return false; } @@ -133,9 +133,9 @@ public class neoJoystickPlayback extends Command { controllers[i].setFrame(controllerFrame.axes, controllerFrame.button, controllerFrame.POV); if (i == 0) { this.swerve.driveWithInput( - new Translation2d(controllers[i].getRawAxis(0), controllers[i].getRawAxis(1)), - new Translation2d(controllers[i].getRawAxis(4), controllers[i].getRawAxis(5)), - true); + new Translation2d(controllers[i].getRawAxis(0), controllers[i].getRawAxis(1)), + new Translation2d(controllers[i].getRawAxis(4), controllers[i].getRawAxis(5)), + true); } } frame_index++; diff --git a/src/main/java/frc4388/robot/commands/Swerve/neoJoystickRecorder.java b/src/main/java/frc4388/robot/commands/Swerve/neoJoystickRecorder.java index d1870ec..25f6ffa 100644 --- a/src/main/java/frc4388/robot/commands/Swerve/neoJoystickRecorder.java +++ b/src/main/java/frc4388/robot/commands/Swerve/neoJoystickRecorder.java @@ -55,8 +55,8 @@ public class neoJoystickRecorder extends Command { XboxController controller = controllers[i]; AutoRecordingControllerFrame controllerFrame = new AutoRecordingControllerFrame(); double[] axes = {controller.getLeftX(), controller.getLeftY(), - controller.getLeftTriggerAxis(), controller.getRightTriggerAxis(), - controller.getRightX(), controller.getRightY()}; + controller.getLeftTriggerAxis(), controller.getRightTriggerAxis(), + controller.getRightX(), controller.getRightY()}; short button = 0; for (int j = 0; j < 10; j++) if (controller.getRawButton(j+1)) @@ -71,8 +71,8 @@ public class neoJoystickRecorder extends Command { frames.add(frame); swerve.driveWithInput(new Translation2d(frame.controllerFrames[0].axes[0], frame.controllerFrames[0].axes[1]), - new Translation2d(frame.controllerFrames[0].axes[4], frame.controllerFrames[0].axes[5]), - true); // Really jank way of doing this. + new Translation2d(frame.controllerFrames[0].axes[4], frame.controllerFrames[0].axes[5]), + true); // Really jank way of doing this. } @Override @@ -105,7 +105,7 @@ public class neoJoystickRecorder extends Command { } stream.write(DataUtils.intToByteArray(frame.timeStamp)); } - System.out.println("AUTORECORD: Wrote auto `" + filename + "` that is " + frames.size() + " frames long."); + System.out.println("AUTORECORD: Wrote auto `" + filename + "` that is " + frames.size() + " frames long."); } catch (Exception e) { e.printStackTrace(); } diff --git a/src/main/java/frc4388/robot/subsystems/Climber.java b/src/main/java/frc4388/robot/subsystems/Climber.java index 92d682b..6cb9a2a 100644 --- a/src/main/java/frc4388/robot/subsystems/Climber.java +++ b/src/main/java/frc4388/robot/subsystems/Climber.java @@ -15,38 +15,38 @@ import frc4388.robot.Constants.ClimbConstants; //! 6.5C Scoring Criteria for Stage public class Climber extends SubsystemBase { - /** Creates a new Climber. */ - TalonFX climbMotor; + /** Creates a new Climber. */ + TalonFX climbMotor; - public Climber(TalonFX climbMotor) { - this.climbMotor = climbMotor; - this.climbMotor.setInverted(true); - var slot0Configs = new Slot0Configs(); - slot0Configs.kP = 0.7; // An error of 0.5 rotations results in 12 V output - slot0Configs.kI = 0.0; // no output for integrated error - slot0Configs.kD = 0.06; // A velocity of 1 rps results in 0.1 V output + public Climber(TalonFX climbMotor) { + this.climbMotor = climbMotor; + this.climbMotor.setInverted(true); + var slot0Configs = new Slot0Configs(); + slot0Configs.kP = 0.7; // An error of 0.5 rotations results in 12 V output + slot0Configs.kI = 0.0; // no output for integrated error + slot0Configs.kD = 0.06; // A velocity of 1 rps results in 0.1 V output - climbMotor.getConfigurator().apply(slot0Configs); - } + climbMotor.getConfigurator().apply(slot0Configs); + } - public void climbOut() { - PositionVoltage request = new PositionVoltage(0); - climbMotor.setControl(request.withPosition(-520)); - //climbMotor.set(Constants.ClimbConstants.CLIMB_IN_SPEED); - } + public void climbOut() { + PositionVoltage request = new PositionVoltage(0); + climbMotor.setControl(request.withPosition(-520)); + //climbMotor.set(Constants.ClimbConstants.CLIMB_IN_SPEED); + } - public void climbIn() { - PositionVoltage request = new PositionVoltage(-520); - climbMotor.setControl(request.withPosition(0)); - // climbMotor.set(Constants.ClimbConstants.CLIMB_IN_SPEED); - } + public void climbIn() { + PositionVoltage request = new PositionVoltage(-520); + climbMotor.setControl(request.withPosition(0)); + // climbMotor.set(Constants.ClimbConstants.CLIMB_IN_SPEED); + } - public void stopClimb() { - climbMotor.set(0.d); - } + public void stopClimb() { + climbMotor.set(0.d); + } - @Override - public void periodic() { - // This method will be called once per scheduler run - } + @Override + public void periodic() { + // This method will be called once per scheduler run + } } diff --git a/src/main/java/frc4388/robot/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java index 986ed14..9dbc4f4 100644 --- a/src/main/java/frc4388/robot/subsystems/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/Intake.java @@ -38,303 +38,303 @@ import frc4388.utility.configurable.ConfigurableDouble; public class Intake extends SubsystemBase { - //NEO - private CANSparkMax intakeMotor; - private CANSparkMax pivot; - private SparkPIDController m_spedController; - private SparkLimitSwitch forwardLimit; - private SparkLimitSwitch reverseLimit; - private SparkLimitSwitch intakeforwardLimit; - private SparkLimitSwitch intakereverseLimit; + //NEO + private CANSparkMax intakeMotor; + private CANSparkMax pivot; + private SparkPIDController m_spedController; + private SparkLimitSwitch forwardLimit; + private SparkLimitSwitch reverseLimit; + private SparkLimitSwitch intakeforwardLimit; + private SparkLimitSwitch intakereverseLimit; - //Talon - private TalonFX talonIntake; - private TalonFX talonPivot; - private CANcoder encoder; + //Talon + private TalonFX talonIntake; + private TalonFX talonPivot; + private CANcoder encoder; - private boolean r; + private boolean r; - private HardwareLimitSwitchConfigs l; + private HardwareLimitSwitchConfigs l; - TalonFXConfiguration doodooController = new TalonFXConfiguration(); + TalonFXConfiguration doodooController = new TalonFXConfiguration(); - public static Gains armGains = IntakeConstants.ArmPID.INTAKE_GAINS; - private ConfigurableDouble outtakeSpeed = new ConfigurableDouble("Outtake Speed", IntakeConstants.INTAKE_OUT_SPEED_UNPRESSED); - private BooleanSupplier sup = () -> true; - private BooleanSupplier dup = () -> false; + public static Gains armGains = IntakeConstants.ArmPID.INTAKE_GAINS; + private ConfigurableDouble outtakeSpeed = new ConfigurableDouble("Outtake Speed", IntakeConstants.INTAKE_OUT_SPEED_UNPRESSED); + private BooleanSupplier sup = () -> true; + private BooleanSupplier dup = () -> false; - private double smartDashboardOuttakeValue; + private double smartDashboardOuttakeValue; - /** Creates a new Intake. */ - //For NEO - // public Intake(CANSparkMax intakeMotor, CANSparkMax pivot) { - // this.intakeMotor = intakeMotor; - // this.pivot = pivot; + /** Creates a new Intake. */ + //For NEO + // public Intake(CANSparkMax intakeMotor, CANSparkMax pivot) { + // this.intakeMotor = intakeMotor; + // this.pivot = pivot; - // pivot.restoreFactoryDefaults(); - // //pivot.setInverted(true); + // pivot.restoreFactoryDefaults(); + // //pivot.setInverted(true); - // forwardLimit = pivot.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen); - // reverseLimit = pivot.getReverseLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen); - // forwardLimit.enableLimitSwitch(true); - // reverseLimit.enableLimitSwitch(true); + // forwardLimit = pivot.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen); + // reverseLimit = pivot.getReverseLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen); + // forwardLimit.enableLimitSwitch(true); + // reverseLimit.enableLimitSwitch(true); - // intakeMotor.restoreFactoryDefaults(); + // intakeMotor.restoreFactoryDefaults(); - // intakeforwardLimit = intakeMotor.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen); - // intakereverseLimit = intakeMotor.getReverseLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen); - // intakeforwardLimit.enableLimitSwitch(true); - // intakereverseLimit.enableLimitSwitch(false); + // intakeforwardLimit = intakeMotor.getForwardLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen); + // intakereverseLimit = intakeMotor.getReverseLimitSwitch(SparkLimitSwitch.Type.kNormallyOpen); + // intakeforwardLimit.enableLimitSwitch(true); + // intakereverseLimit.enableLimitSwitch(false); - // //Arm PID - // m_spedController = pivot.getPIDController(); - // m_spedController.setP(armGains.kP); - // m_spedController.setI(armGains.kI); - // m_spedController.setD(armGains.kD); + // //Arm PID + // m_spedController = pivot.getPIDController(); + // m_spedController.setP(armGains.kP); + // m_spedController.setI(armGains.kI); + // m_spedController.setD(armGains.kD); - // SmartDashboard.putNumber("Outtake Speed", IntakeConstants.INTAKE_OUT_SPEED_UNPRESSED); - // } + // SmartDashboard.putNumber("Outtake Speed", IntakeConstants.INTAKE_OUT_SPEED_UNPRESSED); + // } - //For Talon - public Intake(TalonFX talonIntake, TalonFX talonPivot) { - this.talonIntake = talonIntake; - this.talonPivot = talonPivot; + //For Talon + public Intake(TalonFX talonIntake, TalonFX talonPivot) { + this.talonIntake = talonIntake; + this.talonPivot = talonPivot; - talonIntake.getConfigurator().apply(new TalonFXConfiguration()); - talonPivot.getConfigurator().apply(new TalonFXConfiguration()); + talonIntake.getConfigurator().apply(new TalonFXConfiguration()); + talonPivot.getConfigurator().apply(new TalonFXConfiguration()); - talonIntake.setNeutralMode(NeutralModeValue.Brake); - talonPivot.setNeutralMode(NeutralModeValue.Brake); + talonIntake.setNeutralMode(NeutralModeValue.Brake); + talonPivot.setNeutralMode(NeutralModeValue.Brake); - // talonPivot.getConfigurator().apply(new HardwareLimitSwitchConfigs()); - // talonIntake.getConfigurator().apply(new HardwareLimitSwitchConfigs()); + // talonPivot.getConfigurator().apply(new HardwareLimitSwitchConfigs()); + // talonIntake.getConfigurator().apply(new HardwareLimitSwitchConfigs()); - // doodooController.Slot0.kP = armGains.kP; - // doodooController.Slot1.kI = armGains.kI; - // doodooController.Slot2.kD = armGains.kD; + // doodooController.Slot0.kP = armGains.kP; + // doodooController.Slot1.kI = armGains.kI; + // doodooController.Slot2.kD = armGains.kD; - // in init function, set slot 0 gains - var slot0Configs = new Slot0Configs(); - slot0Configs.kP = 0.7; // An error of 0.5 rotations results in 12 V output - slot0Configs.kI = 0.0; // no output for integrated error - slot0Configs.kD = 0.06; // A velocity of 1 rps results in 0.1 V output + // in init function, set slot 0 gains + var slot0Configs = new Slot0Configs(); + slot0Configs.kP = 0.7; // An error of 0.5 rotations results in 12 V output + slot0Configs.kI = 0.0; // no output for integrated error + slot0Configs.kD = 0.06; // A velocity of 1 rps results in 0.1 V output - talonPivot.getConfigurator().apply(slot0Configs); + talonPivot.getConfigurator().apply(slot0Configs); - } - - // ! Talon Methods - public void talonPIDIn() { - PositionVoltage request = new PositionVoltage(-59); - talonPivot.setControl(request.withPosition(0)); - } - - public void talonPIDOut() { - PositionVoltage request = new PositionVoltage(0); - talonPivot.setControl(request.withPosition(-59)); - } - - public void talonPIDPosition(double out2) { - PositionVoltage request = new PositionVoltage(out2); - talonPivot.setControl(request); - } - - public void talonHandoff() { - talonIntake.set(-outtakeSpeed.get()); - } - - public void talonSpinIntakeMotor() { - talonIntake.set(IntakeConstants.INTAKE_SPEED); - } - - public void talonSpinIntakeMotor(double speed) { - talonIntake.set(speed); - } - - public boolean getTalonIntakeLimitSwitchState() { - if(r = talonIntake.getForwardLimit().getValue().value == 0) { - return true; } - return false; - } - public void talonSetPivotEncoderPosition(int val) { - talonPivot.setPosition(val); - } - - public void talonStopIntakeMotors() { - talonIntake.set(0); - } - - public void talonStopArmMotor() { - talonPivot.set(0); - } - - public double getArmPos() { - return talonPivot.getPosition().getValue(); - } - - public void resetArmPosition() { - if(getTalonIntakeLimitSwitchState()){ - // talonPivot.setPosition(0); + // ! Talon Methods + public void talonPIDIn() { + PositionVoltage request = new PositionVoltage(-59); + talonPivot.setControl(request.withPosition(0)); } - } - public void ampPosition() { - PositionVoltage request = new PositionVoltage(-0); - talonPivot.setControl(request.withPosition(-59)); //TODO: Find actual value - } + public void talonPIDOut() { + PositionVoltage request = new PositionVoltage(0); + talonPivot.setControl(request.withPosition(-59)); + } - public void ampShoot(double speed) { - talonSpinIntakeMotor(speed); - } + public void talonPIDPosition(double out2) { + PositionVoltage request = new PositionVoltage(out2); + talonPivot.setControl(request); + } + public void talonHandoff() { + talonIntake.set(-outtakeSpeed.get()); + } - // ! NEO Methods - //hanoff - // public void spinIntakeMotor() { - // intakeMotor.set(IntakeConstants.INTAKE_SPEED); - // } + public void talonSpinIntakeMotor() { + talonIntake.set(IntakeConstants.INTAKE_SPEED); + } - // //Rotate robot in for handoff - // public void rotateArmIn() { - // pivot.set(IntakeConstants.PIVOT_SPEED); - // } + public void talonSpinIntakeMotor(double speed) { + talonIntake.set(speed); + } - // //Rotates robot out for intake - // public void rotateArmOut() { - // pivot.set(-IntakeConstants.PIVOT_SPEED); + public boolean getTalonIntakeLimitSwitchState() { + if(r = talonIntake.getForwardLimit().getValue().value == 0) { + return true; + } + return false; + } - // } - - // public void pidIn() { - // m_spedController.setReference(2.5, CANSparkMax.ControlType.kPosition); - // //SmartDashboard.putNumber("Velocity Output", pivot.getEncoder().getVelocity()); - // } + public void talonSetPivotEncoderPosition(int val) { + talonPivot.setPosition(val); + } - // public void pidOut() { - // m_spedController.setReference(-53, CANSparkMax.ControlType.kPosition); - // } + public void talonStopIntakeMotors() { + talonIntake.set(0); + } - // public void limitNote() { - // if (intakeforwardLimit.isPressed()) { - // rotateArmIn2(); - // } else { - // spinIntakeMotor(); - // } - // } + public void talonStopArmMotor() { + talonPivot.set(0); + } - // public void rotateArmOut2() { - // if(reverseLimit.isPressed()){ - // stopArmMotor(); - // } else { - // pidOut(); - // } - // } + public double getArmPos() { + return talonPivot.getPosition().getValue(); + } - // public void rotateArmIn2() { - // if(forwardLimit.isPressed()){ - // stopArmMotor(); - // } else { - // pidIn(); - // } - // } + public void resetArmPosition() { + if(getTalonIntakeLimitSwitchState()){ + // talonPivot.setPosition(0); + } + } + + public void ampPosition() { + PositionVoltage request = new PositionVoltage(-0); + talonPivot.setControl(request.withPosition(-59)); //TODO: Find actual value + } + + public void ampShoot(double speed) { + talonSpinIntakeMotor(speed); + } + + + // ! NEO Methods + //hanoff + // public void spinIntakeMotor() { + // intakeMotor.set(IntakeConstants.INTAKE_SPEED); + // } + + // //Rotate robot in for handoff + // public void rotateArmIn() { + // pivot.set(IntakeConstants.PIVOT_SPEED); + // } + + // //Rotates robot out for intake + // public void rotateArmOut() { + // pivot.set(-IntakeConstants.PIVOT_SPEED); + + // } + + // public void pidIn() { + // m_spedController.setReference(2.5, CANSparkMax.ControlType.kPosition); + // //SmartDashboard.putNumber("Velocity Output", pivot.getEncoder().getVelocity()); + // } - // public void handoff() { - // intakeMotor.set(-IntakeConstants.INTAKE_OUT_SPEED_UNPRESSED); - // } + // public void pidOut() { + // m_spedController.setReference(-53, CANSparkMax.ControlType.kPosition); + // } - // public void handoff2() { - // if(intakeforwardLimit.isPressed()) { - // intakeMotor.set(-smartDashboardOuttakeValue); - // } else { - // intakeMotor.set(-smartDashboardOuttakeValue); - // } - // } + // public void limitNote() { + // if (intakeforwardLimit.isPressed()) { + // rotateArmIn2(); + // } else { + // spinIntakeMotor(); + // } + // } - // public void stopIntakeMotors() { - // intakeMotor.set(0); - // } + // public void rotateArmOut2() { + // if(reverseLimit.isPressed()){ + // stopArmMotor(); + // } else { + // pidOut(); + // } + // } - // public void stopArmMotor() { - // pivot.set(0); - // } + // public void rotateArmIn2() { + // if(forwardLimit.isPressed()){ + // stopArmMotor(); + // } else { + // pidIn(); + // } + // } + + // public void handoff() { + // intakeMotor.set(-IntakeConstants.INTAKE_OUT_SPEED_UNPRESSED); + // } - // public RelativeEncoder getEncoder() { - // return pivot.getEncoder(); - // } + // public void handoff2() { + // if(intakeforwardLimit.isPressed()) { + // intakeMotor.set(-smartDashboardOuttakeValue); + // } else { + // intakeMotor.set(-smartDashboardOuttakeValue); + // } + // } - // public boolean getForwardLimitSwitchState() { - // return forwardLimit.isPressed(); - // } + // public void stopIntakeMotors() { + // intakeMotor.set(0); + // } - // public boolean getReverseLimitSwitchState() { - // return reverseLimit.isPressed(); - // } + // public void stopArmMotor() { + // pivot.set(0); + // } - // public boolean getIntakeLimitSwtichState() { - // return intakeforwardLimit.isPressed(); - // } + // public RelativeEncoder getEncoder() { + // return pivot.getEncoder(); + // } - // public void setVoltage(double voltage) { - // pivot.setVoltage(voltage); - // } + // public boolean getForwardLimitSwitchState() { + // return forwardLimit.isPressed(); + // } - // public double getVelocity() { - // return pivot.getEncoder().getVelocity(); - // } + // public boolean getReverseLimitSwitchState() { + // return reverseLimit.isPressed(); + // } - // public void setPivotEncoderPosition(int val) { - // pivot.getEncoder().setPosition(val); - // } + // public boolean getIntakeLimitSwtichState() { + // return intakeforwardLimit.isPressed(); + // } - // public void resetPosition() { - // if(forwardLimit.isPressed()) { - // setPivotEncoderPosition(0); - // } - // } + // public void setVoltage(double voltage) { + // pivot.setVoltage(voltage); + // } - // public double getPos() { - // return pivot.getEncoder().getPosition(); - // } + // public double getVelocity() { + // return pivot.getEncoder().getVelocity(); + // } - // public double getIntakeVelocity() { - // return intakeMotor.getEncoder().getVelocity(); - // } + // public void setPivotEncoderPosition(int val) { + // pivot.getEncoder().setPosition(val); + // } - // public void rotateArm() { + // public void resetPosition() { + // if(forwardLimit.isPressed()) { + // setPivotEncoderPosition(0); + // } + // } - // } + // public double getPos() { + // return pivot.getEncoder().getPosition(); + // } - // public BooleanSupplier getArmFowardLimitState() { - // if(forwardLimit.isPressed()) { - // return sup; - // } else { - // return dup; - // } - // } + // public double getIntakeVelocity() { + // return intakeMotor.getEncoder().getVelocity(); + // } - // public void changeIntakeNeutralState() { - // if(forwardLimit.isPressed()) { - // intakeMotor.setIdleMode(CANSparkBase.IdleMode.kCoast); - // } - // } + // public void rotateArm() { - @Override - public void periodic() { - // This method will be called once per scheduler run - // SmartDashboard.putNumber("Vel Output", getVelocity()); - // SmartDashboard.putNumber("Position", getPos()); - // resetPosition(); - // changeIntakeNeutralState(); + // } - resetArmPosition(); + // public BooleanSupplier getArmFowardLimitState() { + // if(forwardLimit.isPressed()) { + // return sup; + // } else { + // return dup; + // } + // } - SmartDashboard.putNumber("Pivot Position", getArmPos()); + // public void changeIntakeNeutralState() { + // if(forwardLimit.isPressed()) { + // intakeMotor.setIdleMode(CANSparkBase.IdleMode.kCoast); + // } + // } - smartDashboardOuttakeValue = SmartDashboard.getNumber("Outtake Speed", IntakeConstants.INTAKE_OUT_SPEED_UNPRESSED); - } + @Override + public void periodic() { + // This method will be called once per scheduler run + // SmartDashboard.putNumber("Vel Output", getVelocity()); + // SmartDashboard.putNumber("Position", getPos()); + // resetPosition(); + // changeIntakeNeutralState(); + + resetArmPosition(); + + SmartDashboard.putNumber("Pivot Position", getArmPos()); + + smartDashboardOuttakeValue = SmartDashboard.getNumber("Outtake Speed", IntakeConstants.INTAKE_OUT_SPEED_UNPRESSED); + } } diff --git a/src/main/java/frc4388/robot/subsystems/LED.java b/src/main/java/frc4388/robot/subsystems/LED.java index e9e070c..52518b7 100644 --- a/src/main/java/frc4388/robot/subsystems/LED.java +++ b/src/main/java/frc4388/robot/subsystems/LED.java @@ -22,69 +22,69 @@ import frc4388.utility.LEDPatterns; */ public class LED extends SubsystemBase { - static AddressableLED m_led; - static AddressableLEDBuffer m_ledBuffer; - static LED m_self; - /** - * Add your docs here. - */ + static AddressableLED m_led; + static AddressableLEDBuffer m_ledBuffer; + static LED m_self; + /** + * Add your docs here. + */ - public LED(){ - if(m_self != null) - return; - m_led = new AddressableLED(9); - m_ledBuffer = new AddressableLEDBuffer(10); - m_led.setLength(m_ledBuffer.getLength()); - m_led.setData(m_ledBuffer); - m_led.start(); - System.err.println("In the Beginning, there was Joe.\nAnd he said, 'Let there be LEDs.'\nAnd it was good."); - } - public static LED getInstance() { - if(m_self == null) - m_self = new LED(); - return m_self; - } - @Override - public void periodic(){ - //gamermode(); - //SmartDashboard.putNumber("LED", m_currentPattern.getValue()); - return; - } - static int firstcolor = 0; - static void gamermode() { - for(int i = 0; i < m_ledBuffer.getLength(); i++) { - final int hue = (firstcolor + (i * 180 / m_ledBuffer.getLength())) % 180; - setLEDHSV(i, hue, 255, 128); - } - firstcolor +=3; - firstcolor %= 180; - } - /** - * Add your docs here. - */ - public static void updateLED (){ - gamermode(); - // m_LEDController.set(m_currentPattern.getValue()); - } + public LED(){ + if(m_self != null) + return; + m_led = new AddressableLED(9); + m_ledBuffer = new AddressableLEDBuffer(10); + m_led.setLength(m_ledBuffer.getLength()); + m_led.setData(m_ledBuffer); + m_led.start(); + System.err.println("In the Beginning, there was Joe.\nAnd he said, 'Let there be LEDs.'\nAnd it was good."); + } + public static LED getInstance() { + if(m_self == null) + m_self = new LED(); + return m_self; + } + @Override + public void periodic(){ + //gamermode(); + //SmartDashboard.putNumber("LED", m_currentPattern.getValue()); + return; + } + static int firstcolor = 0; + static void gamermode() { + for(int i = 0; i < m_ledBuffer.getLength(); i++) { + final int hue = (firstcolor + (i * 180 / m_ledBuffer.getLength())) % 180; + setLEDHSV(i, hue, 255, 128); + } + firstcolor +=3; + firstcolor %= 180; + } + /** + * Add your docs here. + */ + public static void updateLED (){ + gamermode(); + // m_LEDController.set(m_currentPattern.getValue()); + } - /** - * Add your docs here. - */ - public static void setLEDRGB(int lednum, int r, int g, int b){ - m_ledBuffer.setRGB(lednum, r, g, b); - //m_currentPattern = pattern; - // m_LEDController.set(m_currentPattern.getValue()); - } - public static void setLEDHSV(int lednum, int hue, int sat, int val){ - m_ledBuffer.setRGB(lednum, hue, sat, val); - //m_currentPattern = pattern; - // m_LEDController.set(m_currentPattern.getValue()); - } - /** - * Add your docs here. - * @return - */ - public AddressableLEDBuffer getBuffer() { - return m_ledBuffer; - } -} \ No newline at end of file + /** + * Add your docs here. + */ + public static void setLEDRGB(int lednum, int r, int g, int b){ + m_ledBuffer.setRGB(lednum, r, g, b); + //m_currentPattern = pattern; + // m_LEDController.set(m_currentPattern.getValue()); + } + public static void setLEDHSV(int lednum, int hue, int sat, int val){ + m_ledBuffer.setRGB(lednum, hue, sat, val); + //m_currentPattern = pattern; + // m_LEDController.set(m_currentPattern.getValue()); + } + /** + * Add your docs here. + * @return + */ + public AddressableLEDBuffer getBuffer() { + return m_ledBuffer; + } +} diff --git a/src/main/java/frc4388/robot/subsystems/Limelight.java b/src/main/java/frc4388/robot/subsystems/Limelight.java index dc5e0e7..fa9010d 100644 --- a/src/main/java/frc4388/robot/subsystems/Limelight.java +++ b/src/main/java/frc4388/robot/subsystems/Limelight.java @@ -18,65 +18,65 @@ import frc4388.robot.Constants.VisionConstants; public class Limelight extends SubsystemBase { - // [X, Y, Z, Roll, Pitch, Yaw] - private double[] cameraPose; - private boolean isTag; + // [X, Y, Z, Roll, Pitch, Yaw] + private double[] cameraPose; + private boolean isTag; - private Pose2d pose; - private boolean isNearSpeaker; + private Pose2d pose; + private boolean isNearSpeaker; - public boolean getIsTag() { - return isTag; - } - - private void update() { - SmartDashboard.putBoolean("Apriltag", isTag); - if(!isTag){ - return; + public boolean getIsTag() { + return isTag; } - double x = cameraPose[0]; - double y = cameraPose[1]; - double yaw = cameraPose[5]; + private void update() { + SmartDashboard.putBoolean("Apriltag", isTag); + if(!isTag){ + return; + } - Rotation2d rot = Rotation2d.fromDegrees(yaw); + double x = cameraPose[0]; + double y = cameraPose[1]; + double yaw = cameraPose[5]; - pose = new Pose2d(x, y, rot); + Rotation2d rot = Rotation2d.fromDegrees(yaw); - boolean isRed = DriverStation.getAlliance().get() == Alliance.Red; + pose = new Pose2d(x, y, rot); + + boolean isRed = DriverStation.getAlliance().get() == Alliance.Red; - double distance; + double distance; - if(isRed){ - distance = pose.getTranslation().getDistance(VisionConstants.RedSpeakerCenter); - }else{ - distance = pose.getTranslation().getDistance(VisionConstants.BlueSpeakerCenter); - } + if(isRed){ + distance = pose.getTranslation().getDistance(VisionConstants.RedSpeakerCenter); + }else{ + distance = pose.getTranslation().getDistance(VisionConstants.BlueSpeakerCenter); + } - isNearSpeaker = distance <= VisionConstants.SpeakerBubbleDistance; + isNearSpeaker = distance <= VisionConstants.SpeakerBubbleDistance; - SmartDashboard.putBoolean("nearSpeaker", isNearSpeaker); - SmartDashboard.putNumber("speakerDistance", distance); - } - - public Pose2d getPose() { - return pose; - } - - public boolean isNearSpeaker() { - return isNearSpeaker; - } - - @Override - public void periodic() { - // This method will be called once per scheduler run - - isTag = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tv").getDouble(0.0) == 1.0; - double[] newPose = NetworkTableInstance.getDefault().getTable("limelight").getEntry("botpose").getDoubleArray(new double[6]); - - if(newPose != cameraPose){ - cameraPose = newPose; - update(); + SmartDashboard.putBoolean("nearSpeaker", isNearSpeaker); + SmartDashboard.putNumber("speakerDistance", distance); } - } -} \ No newline at end of file + + public Pose2d getPose() { + return pose; + } + + public boolean isNearSpeaker() { + return isNearSpeaker; + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + + isTag = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tv").getDouble(0.0) == 1.0; + double[] newPose = NetworkTableInstance.getDefault().getTable("limelight").getEntry("botpose").getDoubleArray(new double[6]); + + if(newPose != cameraPose){ + cameraPose = newPose; + update(); + } + } +} diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java index 026e213..46afcc6 100644 --- a/src/main/java/frc4388/robot/subsystems/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/Shooter.java @@ -21,94 +21,94 @@ import com.ctre.phoenix.motorcontrol.NeutralMode; public class Shooter extends SubsystemBase { - private TalonFX leftShooter; - private TalonFX rightShooter; + private TalonFX leftShooter; + private TalonFX rightShooter; - private Limelight limelight; + private Limelight limelight; - private int spinMode = 0; - // 0 = Stop / Coast - // 1 = Idle - // 2 = Idle Near Speaker - // 3 = Spin - // 4 = SingleSpin + private int spinMode = 0; + // 0 = Stop / Coast + // 1 = Idle + // 2 = Idle Near Speaker + // 3 = Spin + // 4 = SingleSpin - private double smartDashboardShooterSpeed; + private double smartDashboardShooterSpeed; - /** Creates a new Shooter. */ - public Shooter(TalonFX leftTalonFX, TalonFX rightTalonFX, Limelight tmplimelight) { - leftShooter = leftTalonFX; - rightShooter = rightTalonFX; + /** Creates a new Shooter. */ + public Shooter(TalonFX leftTalonFX, TalonFX rightTalonFX, Limelight tmplimelight) { + leftShooter = leftTalonFX; + rightShooter = rightTalonFX; - limelight = tmplimelight; + limelight = tmplimelight; - leftShooter.setNeutralMode(NeutralModeValue.Coast); - rightShooter.setNeutralMode(NeutralModeValue.Coast); - SmartDashboard.putNumber("Shooter Speed", ShooterConstants.SHOOTER_SPEED); + leftShooter.setNeutralMode(NeutralModeValue.Coast); + rightShooter.setNeutralMode(NeutralModeValue.Coast); + SmartDashboard.putNumber("Shooter Speed", ShooterConstants.SHOOTER_SPEED); - } - - public Shooter(TalonFX leftShooter) { - this.leftShooter = leftShooter; - leftShooter.setNeutralMode(NeutralModeValue.Coast); - } - - public void singleSpin() { - leftShooter.set(1.0); - spinMode = 4; - } - - public void singleSpin(double speed) { - leftShooter.set(speed); - spinMode = 4; - } - - public void spin() { - spin(smartDashboardShooterSpeed); - spinMode = 3; - } - - public void spin(double speed) { - leftShooter.set(-speed); - rightShooter.set(-speed); - spinMode = 3; - } - - public void spin(double leftSpeed, double rightSpeed) { - leftShooter.set(leftSpeed); - rightShooter.set(-rightSpeed); - spinMode = 3; - } - - public void stop() { - spin(0.d); - spinMode = 0; - } - - public void idle() { - spin(ShooterConstants.SHOOTER_IDLE); - spinMode = 1; - } - - public int getMode(){ - return spinMode; - } - - @Override - public void periodic() { - // This method will be called once per scheduler run - // SmartDashboard.putNumber("Left Shooter RPM", leftShooter.getRotorVelocity().getValue()); - //SmartDashboard.putNumber("Right Shooter RPM", rightShooter.getRotorVelocity().getValue()); - smartDashboardShooterSpeed = SmartDashboard.getNumber("Shooter Speed", ShooterConstants.SHOOTER_SPEED); - - // If the robot is near the speaker, and is stopped, or idled, set to limelight idle speed. - // Else if the robot is not near the speaker, then set the speed back to idle. - if(limelight.isNearSpeaker() && (spinMode == 0 || spinMode == 1)){ - leftShooter.set(-ShooterConstants.SHOOTER_IDLE_LIMELIGHT); - rightShooter.set(-ShooterConstants.SHOOTER_IDLE_LIMELIGHT); - spinMode = 2; - }else if(!limelight.isNearSpeaker() && spinMode == 2){ - idle(); } - } + + public Shooter(TalonFX leftShooter) { + this.leftShooter = leftShooter; + leftShooter.setNeutralMode(NeutralModeValue.Coast); + } + + public void singleSpin() { + leftShooter.set(1.0); + spinMode = 4; + } + + public void singleSpin(double speed) { + leftShooter.set(speed); + spinMode = 4; + } + + public void spin() { + spin(smartDashboardShooterSpeed); + spinMode = 3; + } + + public void spin(double speed) { + leftShooter.set(-speed); + rightShooter.set(-speed); + spinMode = 3; + } + + public void spin(double leftSpeed, double rightSpeed) { + leftShooter.set(leftSpeed); + rightShooter.set(-rightSpeed); + spinMode = 3; + } + + public void stop() { + spin(0.d); + spinMode = 0; + } + + public void idle() { + spin(ShooterConstants.SHOOTER_IDLE); + spinMode = 1; + } + + public int getMode(){ + return spinMode; + } + + @Override + public void periodic() { + // This method will be called once per scheduler run + // SmartDashboard.putNumber("Left Shooter RPM", leftShooter.getRotorVelocity().getValue()); + //SmartDashboard.putNumber("Right Shooter RPM", rightShooter.getRotorVelocity().getValue()); + smartDashboardShooterSpeed = SmartDashboard.getNumber("Shooter Speed", ShooterConstants.SHOOTER_SPEED); + + // If the robot is near the speaker, and is stopped, or idled, set to limelight idle speed. + // Else if the robot is not near the speaker, then set the speed back to idle. + if(limelight.isNearSpeaker() && (spinMode == 0 || spinMode == 1)){ + leftShooter.set(-ShooterConstants.SHOOTER_IDLE_LIMELIGHT); + rightShooter.set(-ShooterConstants.SHOOTER_IDLE_LIMELIGHT); + spinMode = 2; + }else if(!limelight.isNearSpeaker() && spinMode == 2){ + idle(); + } + } } diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index e2f3da9..8e41578 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -17,267 +17,267 @@ import frc4388.utility.RobotGyro; public class SwerveDrive extends SubsystemBase { - private SwerveModule leftFront; - private SwerveModule rightFront; - private SwerveModule leftBack; - private SwerveModule rightBack; + private SwerveModule leftFront; + private SwerveModule rightFront; + private SwerveModule leftBack; + private SwerveModule rightBack; - private SwerveModule[] modules; + private SwerveModule[] modules; - private Translation2d leftFrontLocation = new Translation2d(Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH)); - private Translation2d rightFrontLocation = new Translation2d(-Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH)); - private Translation2d leftBackLocation = new Translation2d(Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), -Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH)); - private Translation2d rightBackLocation = new Translation2d(-Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), -Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH)); + private Translation2d leftFrontLocation = new Translation2d(Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH)); + private Translation2d rightFrontLocation = new Translation2d(-Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH)); + private Translation2d leftBackLocation = new Translation2d(Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), -Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH)); + private Translation2d rightBackLocation = new Translation2d(-Units.inchesToMeters(SwerveDriveConstants.HALF_HEIGHT), -Units.inchesToMeters(SwerveDriveConstants.HALF_WIDTH)); - private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(leftFrontLocation, rightFrontLocation, leftBackLocation, rightBackLocation); + private SwerveDriveKinematics kinematics = new SwerveDriveKinematics(leftFrontLocation, rightFrontLocation, leftBackLocation, rightBackLocation); - private RobotGyro gyro; + private RobotGyro gyro; - public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST; // * slow by default - public double autoSpeedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; + public double speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST; // * slow by default + public double autoSpeedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; - public double rotTarget = 0.0; - public Rotation2d orientRotTarget = new Rotation2d(); - public ChassisSpeeds chassisSpeeds = new ChassisSpeeds(); + public double rotTarget = 0.0; + public Rotation2d orientRotTarget = new Rotation2d(); + public ChassisSpeeds chassisSpeeds = new ChassisSpeeds(); - /** Creates a new SwerveDrive. */ - public SwerveDrive(SwerveModule leftFront, SwerveModule rightFront, SwerveModule leftBack, SwerveModule rightBack, RobotGyro gyro) { - this.leftFront = leftFront; - this.rightFront = rightFront; - this.leftBack = leftBack; - this.rightBack = rightBack; + /** Creates a new SwerveDrive. */ + public SwerveDrive(SwerveModule leftFront, SwerveModule rightFront, SwerveModule leftBack, SwerveModule rightBack, RobotGyro gyro) { + this.leftFront = leftFront; + this.rightFront = rightFront; + this.leftBack = leftBack; + this.rightBack = rightBack; - this.gyro = gyro; + this.gyro = gyro; - this.modules = new SwerveModule[] {this.leftFront, this.rightFront, this.leftBack, this.rightBack}; - } - - boolean stopped = false; - public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) { - if (fieldRelative) { - - double rot = 0; - - // ! drift correction - if (rightStick.getNorm() > 0.05) { - rotTarget = gyro.getAngle(); - rot = rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED; - // SmartDashboard.putBoolean("drift correction", false); - stopped = false; - } else if(leftStick.getNorm() > 0.05) { - if (!stopped) { - stopModules(); - stopped = true; - } - - // SmartDashboard.putBoolean("drift correction", true); - - rot = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED; - - } - - // Use the left joystick to set speed. Apply a cubic curve and the set max speed. - Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust); - // Translation2d cubedSpeed = new Translation2d(Math.pow(speed.getX(), 3.00), Math.pow(speed.getY(), 3.00)); - - // Convert field-relative speeds to robot-relative speeds. - chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED, gyro.getRotation2d());//.times(-1)); - } else { // Create robot-relative speeds. - chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), -1 * leftStick.getY(), -1 * rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED); + this.modules = new SwerveModule[] {this.leftFront, this.rightFront, this.leftBack, this.rightBack}; } - setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds)); - } - public void playbackDriveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) { - if (fieldRelative) { + boolean stopped = false; + public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) { + if (fieldRelative) { - double rot = 0; + double rot = 0; - // ! drift correction - if (rightStick.getNorm() > 0.05) { - rotTarget = gyro.getAngle(); - rot = rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED; - // SmartDashboard.putBoolean("drift correction", false); - stopped = false; - } else if(leftStick.getNorm() > 0.05) { - if (!stopped) { - stopModules(); - stopped = true; - } + // ! drift correction + if (rightStick.getNorm() > 0.05) { + rotTarget = gyro.getAngle(); + rot = rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED; + // SmartDashboard.putBoolean("drift correction", false); + stopped = false; + } else if(leftStick.getNorm() > 0.05) { + if (!stopped) { + stopModules(); + stopped = true; + } - // SmartDashboard.putBoolean("drift correction", true); + // SmartDashboard.putBoolean("drift correction", true); - rot = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED; + rot = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED; - } + } - // Use the left joystick to set speed. Apply a cubic curve and the set max speed. - Translation2d speed = leftStick.times(leftStick.getNorm() * autoSpeedAdjust); - // Translation2d cubedSpeed = new Translation2d(Math.pow(speed.getX(), 3.00), Math.pow(speed.getY(), 3.00)); + // Use the left joystick to set speed. Apply a cubic curve and the set max speed. + Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust); + // Translation2d cubedSpeed = new Translation2d(Math.pow(speed.getX(), 3.00), Math.pow(speed.getY(), 3.00)); - // Convert field-relative speeds to robot-relative speeds. - chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), rightStick.getX() * SwerveDriveConstants.PLAYBACK_ROTATION_SPEED, gyro.getRotation2d());//.times(-1)); - } else { // Create robot-relative speeds. - chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), -1 * leftStick.getY(), -1 * rightStick.getX() * SwerveDriveConstants.PLAYBACK_ROTATION_SPEED); + // Convert field-relative speeds to robot-relative speeds. + chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED, gyro.getRotation2d());//.times(-1)); + } else { // Create robot-relative speeds. + chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), -1 * leftStick.getY(), -1 * rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED); + } + setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds)); } - setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds)); - } - public void driveWithInputOrientation(Translation2d leftStick, double rightX, double rightY, boolean fieldRelative) { + public void playbackDriveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) { + if (fieldRelative) { - Translation2d rightStick = new Translation2d(-rightX, rightY); + double rot = 0; + + // ! drift correction + if (rightStick.getNorm() > 0.05) { + rotTarget = gyro.getAngle(); + rot = rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED; + // SmartDashboard.putBoolean("drift correction", false); + stopped = false; + } else if(leftStick.getNorm() > 0.05) { + if (!stopped) { + stopModules(); + stopped = true; + } - if(fieldRelative) { - double rot = 0; - if(rightStick.getNorm() > 0.5) { - orientRotTarget = new Rotation2d(rightX, -rightY).minus(new Rotation2d(0,1)); + // SmartDashboard.putBoolean("drift correction", true); - Rotation2d tmp = orientRotTarget.minus(gyro.getRotation2d().minus(new Rotation2d(Math.PI)).interpolate(orientRotTarget, 0.5)); - double min = tmp.getDegrees(); - min = Math.max(Math.abs(min), 2); - if(tmp.getDegrees() < 0) - min*=-1; - tmp = new Rotation2d(min * Math.PI / 180); - rot = tmp.getRadians(); // x x - y/x - } + rot = ((rotTarget - gyro.getAngle()) / 360) * SwerveDriveConstants.ROT_CORRECTION_SPEED; + + } + + // Use the left joystick to set speed. Apply a cubic curve and the set max speed. + Translation2d speed = leftStick.times(leftStick.getNorm() * autoSpeedAdjust); + // Translation2d cubedSpeed = new Translation2d(Math.pow(speed.getX(), 3.00), Math.pow(speed.getY(), 3.00)); + + // Convert field-relative speeds to robot-relative speeds. + chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), rightStick.getX() * SwerveDriveConstants.PLAYBACK_ROTATION_SPEED, gyro.getRotation2d());//.times(-1)); + } else { // Create robot-relative speeds. + chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), -1 * leftStick.getY(), -1 * rightStick.getX() * SwerveDriveConstants.PLAYBACK_ROTATION_SPEED); + } + setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds)); + } + + public void driveWithInputOrientation(Translation2d leftStick, double rightX, double rightY, boolean fieldRelative) { + + Translation2d rightStick = new Translation2d(-rightX, rightY); + + if(fieldRelative) { + double rot = 0; + if(rightStick.getNorm() > 0.5) { + orientRotTarget = new Rotation2d(rightX, -rightY).minus(new Rotation2d(0,1)); + + Rotation2d tmp = orientRotTarget.minus(gyro.getRotation2d().minus(new Rotation2d(Math.PI)).interpolate(orientRotTarget, 0.5)); + double min = tmp.getDegrees(); + min = Math.max(Math.abs(min), 2); + if(tmp.getDegrees() < 0) + min*=-1; + tmp = new Rotation2d(min * Math.PI / 180); + rot = tmp.getRadians(); // x x - y/x + } - Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust); + Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust); - chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), -1 * rot * SwerveDriveConstants.ROTATION_SPEED, gyro.getRotation2d()).times(1); - } else { // Create robot-relative speeds. - chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), -1 * leftStick.getY(), -1 * rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED); - } - setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds)); + chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), -1 * rot * SwerveDriveConstants.ROTATION_SPEED, gyro.getRotation2d()).times(1); + } else { // Create robot-relative speeds. + chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), -1 * leftStick.getY(), -1 * rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED); + } + setModuleStates(kinematics.toSwerveModuleStates(chassisSpeeds)); } - /** - * Set each module of the swerve drive to the corresponding desired state. - * @param desiredStates Array of module states to set. - */ - public void setModuleStates(SwerveModuleState[] desiredStates) { - SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND)); - for (int i = 0; i < desiredStates.length; i++) { - SwerveModule module = modules[i]; - SwerveModuleState state = desiredStates[i]; - module.setDesiredState(state); - } - } - - public boolean rotateToTarget(double angle) { - double currentAngle = getGyroAngle(); - double error = angle - currentAngle; - - driveWithInput(new Translation2d(0, 0), new Translation2d(error / Math.abs(error) * 0.3, 0), true); - - if (Math.abs(angle - getGyroAngle()) < 5.0) { - return true; + /** + * Set each module of the swerve drive to the corresponding desired state. + * @param desiredStates Array of module states to set. + */ + public void setModuleStates(SwerveModuleState[] desiredStates) { + SwerveDriveKinematics.desaturateWheelSpeeds(desiredStates, Units.feetToMeters(SwerveDriveConstants.MAX_SPEED_FEET_PER_SECOND)); + for (int i = 0; i < desiredStates.length; i++) { + SwerveModule module = modules[i]; + SwerveModuleState state = desiredStates[i]; + module.setDesiredState(state); + } } - return false; - } + public boolean rotateToTarget(double angle) { + double currentAngle = getGyroAngle(); + double error = angle - currentAngle; - public double getGyroAngle() { - return gyro.getAngle(); - } + driveWithInput(new Translation2d(0, 0), new Translation2d(error / Math.abs(error) * 0.3, 0), true); - public void resetGyro() { - gyro.reset(); - rotTarget = 0.0; - } + if (Math.abs(angle - getGyroAngle()) < 5.0) { + return true; + } - public void resetGyroFlip() { - gyro.resetFlip(); - rotTarget = 0.0; - } + return false; + } - public void resetGyroRightBlue() { - gyro.resetRightSideBlue(); - rotTarget = 0.0; - } + public double getGyroAngle() { + return gyro.getAngle(); + } - public void resetGyroRightAmp() { - gyro.resetAmpSide(); - rotTarget = 0.0; - } + public void resetGyro() { + gyro.reset(); + rotTarget = 0.0; + } + + public void resetGyroFlip() { + gyro.resetFlip(); + rotTarget = 0.0; + } + + public void resetGyroRightBlue() { + gyro.resetRightSideBlue(); + rotTarget = 0.0; + } + + public void resetGyroRightAmp() { + gyro.resetAmpSide(); + rotTarget = 0.0; + } - public void stopModules() { - for (SwerveModule module : this.modules) { - module.stop(); + public void stopModules() { + for (SwerveModule module : this.modules) { + module.stop(); + } } - } - public SwerveDriveKinematics getKinematics() { - return this.kinematics; - } + public SwerveDriveKinematics getKinematics() { + return this.kinematics; + } - public boolean getSpeedState() { + public boolean getSpeedState() { - return false; - } + return false; + } - @Override - public void periodic() { - // This method will be called once per scheduler run\ - SmartDashboard.putNumber("Gyro", getGyroAngle()); - } + @Override + public void periodic() { + // This method will be called once per scheduler run\ + SmartDashboard.putNumber("Gyro", getGyroAngle()); + } - public void shiftDown() { - if (Math.abs(this.speedAdjust - SwerveDriveConstants.SLOW_SPEED) < .01) { + public void shiftDown() { + if (Math.abs(this.speedAdjust - SwerveDriveConstants.SLOW_SPEED) < .01) { - } else if (Math.abs(this.speedAdjust - SwerveDriveConstants.FAST_SPEED) < .01) { - this.speedAdjust = SwerveDriveConstants.SLOW_SPEED; - } else { - this.speedAdjust = SwerveDriveConstants.FAST_SPEED; + } else if (Math.abs(this.speedAdjust - SwerveDriveConstants.FAST_SPEED) < .01) { + this.speedAdjust = SwerveDriveConstants.SLOW_SPEED; + } else { + this.speedAdjust = SwerveDriveConstants.FAST_SPEED; + } } - } - public void setToSlow() { - this.speedAdjust = SwerveDriveConstants.SLOW_SPEED; - System.out.println("SLOW"); - System.out.println("SLOW"); - System.out.println("SLOW"); - System.out.println("SLOW"); - System.out.println("SLOW"); - } + public void setToSlow() { + this.speedAdjust = SwerveDriveConstants.SLOW_SPEED; + System.out.println("SLOW"); + System.out.println("SLOW"); + System.out.println("SLOW"); + System.out.println("SLOW"); + System.out.println("SLOW"); + } - public void setToFast() { - this.speedAdjust = SwerveDriveConstants.FAST_SPEED; - System.out.println("FAST"); - System.out.println("FAST"); - System.out.println("FAST"); - System.out.println("FAST"); - System.out.println("FAST"); - } + public void setToFast() { + this.speedAdjust = SwerveDriveConstants.FAST_SPEED; + System.out.println("FAST"); + System.out.println("FAST"); + System.out.println("FAST"); + System.out.println("FAST"); + System.out.println("FAST"); + } - public void setToTurbo() { - this.speedAdjust = SwerveDriveConstants.TURBO_SPEED; - System.out.println("TURBO"); - System.out.println("TURBO"); - System.out.println("TURBO"); - System.out.println("TURBO"); - System.out.println("TURBO"); - } + public void setToTurbo() { + this.speedAdjust = SwerveDriveConstants.TURBO_SPEED; + System.out.println("TURBO"); + System.out.println("TURBO"); + System.out.println("TURBO"); + System.out.println("TURBO"); + System.out.println("TURBO"); + } - public void shiftUp() { - if (Math.abs(this.speedAdjust - SwerveDriveConstants.SLOW_SPEED) < .01) { - this.speedAdjust = SwerveDriveConstants.FAST_SPEED; - } else if (Math.abs(this.speedAdjust - SwerveDriveConstants.FAST_SPEED) < .01) { - this.speedAdjust = SwerveDriveConstants.TURBO_SPEED; - } else { + public void shiftUp() { + if (Math.abs(this.speedAdjust - SwerveDriveConstants.SLOW_SPEED) < .01) { + this.speedAdjust = SwerveDriveConstants.FAST_SPEED; + } else if (Math.abs(this.speedAdjust - SwerveDriveConstants.FAST_SPEED) < .01) { + this.speedAdjust = SwerveDriveConstants.TURBO_SPEED; + } else { + } } - } - public void toggleGear(double angle) { - if (Math.abs(this.speedAdjust - SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW) < .01 && Math.abs(angle) < 10) { - this.speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST; - SwerveDriveConstants.ROT_CORRECTION_SPEED = SwerveDriveConstants.CORRECTION_MIN; - } else { - this.speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; - SwerveDriveConstants.ROT_CORRECTION_SPEED = SwerveDriveConstants.CORRECTION_MIN; + public void toggleGear(double angle) { + if (Math.abs(this.speedAdjust - SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW) < .01 && Math.abs(angle) < 10) { + this.speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_FAST; + SwerveDriveConstants.ROT_CORRECTION_SPEED = SwerveDriveConstants.CORRECTION_MIN; + } else { + this.speedAdjust = SwerveDriveConstants.Conversions.JOYSTICK_TO_METERS_PER_SECOND_SLOW; + SwerveDriveConstants.ROT_CORRECTION_SPEED = SwerveDriveConstants.CORRECTION_MIN; + } } - } diff --git a/src/main/java/frc4388/robot/subsystems/SwerveModule.java b/src/main/java/frc4388/robot/subsystems/SwerveModule.java index 1ddab51..6f1be6c 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveModule.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveModule.java @@ -109,9 +109,9 @@ public class SwerveModule extends SubsystemBase { */ public SwerveModuleState getState() { return new SwerveModuleState( - Units.inchesToMeters(driveMotor.getSelectedSensorVelocity() * SwerveDriveConstants.Conversions.INCHES_PER_TICK) * SwerveDriveConstants.Conversions.TICK_TIME_TO_SECONDS, - getAngle() - ); + Units.inchesToMeters(driveMotor.getSelectedSensorVelocity() * SwerveDriveConstants.Conversions.INCHES_PER_TICK) * SwerveDriveConstants.Conversions.TICK_TIME_TO_SECONDS, + getAngle() + ); } /** diff --git a/src/main/java/frc4388/utility/LEDPatterns.java b/src/main/java/frc4388/utility/LEDPatterns.java index 6df032c..0939fcb 100644 --- a/src/main/java/frc4388/utility/LEDPatterns.java +++ b/src/main/java/frc4388/utility/LEDPatterns.java @@ -4,8 +4,8 @@ package frc4388.utility; * Add your docs here. */ public enum LEDPatterns { - /* PALLETTE PATTERNS */ - RAINBOW_RAINBOW(-0.99f), PARTY_RAINBOW(-0.97f), OCEAN_RAINBOW(-0.95f), LAVA_RAINBOW(-0.93f), FOREST_RAINBOW(-0.91f), + /* PALLETTE PATTERNS */ + RAINBOW_RAINBOW(-0.99f), PARTY_RAINBOW(-0.97f), OCEAN_RAINBOW(-0.95f), LAVA_RAINBOW(-0.93f), FOREST_RAINBOW(-0.91f), RAINBOW_GLITTER(-0.89f), CONFETTI(-0.87f), RED_SHOT(-0.85f), BLUE_SHOT(-0.83f), WHITE_SHOT(-0.81f), RAINBOW_SINELON(-0.79f), PARTY_SINELON(-0.77f), OCEAN_SINELON(-0.75f), LAVA_SINELON(-0.73f), FOREST_SINELON(-0.71f), RAINBOW_BPM(-0.69f), PARTY_BPM(-0.67f), OCEAN_BPM(-0.65f), LAVA_BPM(-0.63f), FOREST_BPM(-0.61f), FIRE_MEDIUM(-0.59f), FIRE_LARGE(-0.57f), @@ -15,31 +15,31 @@ public enum LEDPatterns { BLUE_HEARTBEAT(-0.23f), WHITE_HEARTBEAT(-0.21f), GRAY_HEARBEAT(-0.19f), RED_BREATH(-0.17f), BLUE_BREATH(-0.15f), GRAY_BREATH(-0.13f), RED_STROBE(-0.11f), BLUE_STROBE(-0.09f), GOLD_STROBE(-0.07f), WHITE_STROBE(-0.05f), - /* COLOR 1 PATTERNS */ - C1_END_TO_END(-0.03f), C1_SCANNER(-0.01f), C1_CHASE(0.01f), C1_HEARTBEAT_SLOW(0.03f), C1_HEARTBEAT_MEDIUM(0.05f), + /* COLOR 1 PATTERNS */ + C1_END_TO_END(-0.03f), C1_SCANNER(-0.01f), C1_CHASE(0.01f), C1_HEARTBEAT_SLOW(0.03f), C1_HEARTBEAT_MEDIUM(0.05f), C1_HEARTBEAT_FAST(0.07f), C1_BREATH_SLOW(0.09f), C1_BREATH_FAST(0.11f), C1_SHOT(0.13f), C1_STROBE(0.15f), - /* COLOR 2 PATTERNS */ - C2_END_TO_END(0.17f), C2_SCANNER(0.19f), C2_CHASE(0.21f), C2_HEARTBEAT_SLOW(0.23f), C2_HEARTBEAT_MEDIUM(0.25f), + /* COLOR 2 PATTERNS */ + C2_END_TO_END(0.17f), C2_SCANNER(0.19f), C2_CHASE(0.21f), C2_HEARTBEAT_SLOW(0.23f), C2_HEARTBEAT_MEDIUM(0.25f), C2_HEARTBEAT_FAST(0.27f), C2_BREATH_SLOW(0.29f), C2_BREATH_FAST(0.31f), C2_SHOT(0.33f), C2_STROBE(0.35f), - /* COLOR 1 AND 2 PATTERNS */ - C1C2_SPARKLE(0.37f), C2C1_SPARKLE(0.39f), C1C2_GRADIENT(0.41f), C1C2_BPM(0.43f), C1C2_BLEND(0.45f), C1C2_TWINKLES(0.51f), + /* COLOR 1 AND 2 PATTERNS */ + C1C2_SPARKLE(0.37f), C2C1_SPARKLE(0.39f), C1C2_GRADIENT(0.41f), C1C2_BPM(0.43f), C1C2_BLEND(0.45f), C1C2_TWINKLES(0.51f), C1C2_WAVES(0.53f), C1C2_SINELON(0.55f), - /* SOLID COLORS */ - SOLID_PINK_HOT(0.57f), SOLID_RED_DARK(0.59f), SOLID_RED(0.61f), SOLID_RED_ORANGE(0.63f), SOLID_ORANGE(0.65f), + /* SOLID COLORS */ + SOLID_PINK_HOT(0.57f), SOLID_RED_DARK(0.59f), SOLID_RED(0.61f), SOLID_RED_ORANGE(0.63f), SOLID_ORANGE(0.65f), SOLID_GOLD(0.67f), SOLID_YELLOW(0.69f), SOLID_GREEN_LAWN(0.71f), SOLID_GREEN_LIME(0.73f), SOLID_GREEN_DARK(0.75f), SOLID_GREEN(0.77f), SOLID_BLUE_GREEN(0.79f), SOLID_BLUE_AQUA(0.81f), SOLID_BLUE_SKY(0.83f), SOLID_BLUE_DARK(0.85f), SOLID_BLUE(0.87f), SOLID_BLUE_VIOLET(0.89f), SOLID_VIOLET(0.91f), SOLID_WHITE(0.93f), SOLID_GRAY(0.95f), SOLID_GRAY_DARK(0.97f), SOLID_BLACK(0.99f); - /* GETTERS/SETTERS */ - private final float id; - LEDPatterns(float id) { - this.id = id; - } - public float getValue() { - return id; - } -} \ No newline at end of file + /* GETTERS/SETTERS */ + private final float id; + LEDPatterns(float id) { + this.id = id; + } + public float getValue() { + return id; + } +} diff --git a/src/main/java/frc4388/utility/RobotGyro.java b/src/main/java/frc4388/utility/RobotGyro.java index dd90c53..11fd1dd 100644 --- a/src/main/java/frc4388/utility/RobotGyro.java +++ b/src/main/java/frc4388/utility/RobotGyro.java @@ -162,12 +162,12 @@ public class RobotGyro implements Gyro { } /** - * Get Yaw, Pitch, and Roll data. - * - * @return ypr_deg Array with yaw[0], pitch[1], and roll[2] data. - * Yaw is within [-368,640, +368,640] degrees. - * Pitch is within [-90,+90] degrees. - * Roll is within [-90,+90] degrees. + * Get Yaw, Pitch, and Roll data. + * + * @return ypr_deg Array with yaw[0], pitch[1], and roll[2] data. + * Yaw is within [-368,640, +368,640] degrees. + * Pitch is within [-90,+90] degrees. + * Roll is within [-90,+90] degrees. */ private double[] getPigeonAngles() { double[] ypr = new double[3]; @@ -186,7 +186,7 @@ public class RobotGyro implements Gyro { } public double getYaw() { - return this.getAngle(); + return this.getAngle(); } /** diff --git a/src/main/java/frc4388/utility/RobotUnits.java b/src/main/java/frc4388/utility/RobotUnits.java index 9e07312..6d9a12e 100644 --- a/src/main/java/frc4388/utility/RobotUnits.java +++ b/src/main/java/frc4388/utility/RobotUnits.java @@ -8,20 +8,20 @@ package frc4388.utility; * @author Aarav Shah */ public class RobotUnits { - // constants + // constants - // angle conversions - public static double degreesToRadians(final double degrees) {return degrees * Math.PI / 180;} + // angle conversions + public static double degreesToRadians(final double degrees) {return degrees * Math.PI / 180;} - public static double radiansToDegrees(final double radians) {return radians / Math.PI * 180;} + public static double radiansToDegrees(final double radians) {return radians / Math.PI * 180;} - // falcon conversions - public static double falconTicksToRotations(final double ticks) {return ticks / 2048;} + // falcon conversions + public static double falconTicksToRotations(final double ticks) {return ticks / 2048;} - public static double falconRotationsToTicks(final double rotations) {return rotations * 2048;} + public static double falconRotationsToTicks(final double rotations) {return rotations * 2048;} - // distance conversions - public static double metersToFeet(final double meters) {return meters * 3.28084;} + // distance conversions + public static double metersToFeet(final double meters) {return meters * 3.28084;} - public static double feetToMeters(final double feet) {return feet / 3.28084;} -} \ No newline at end of file + public static double feetToMeters(final double feet) {return feet / 3.28084;} +} diff --git a/src/main/java/frc4388/utility/controller/DeadbandedXboxController.java b/src/main/java/frc4388/utility/controller/DeadbandedXboxController.java index 4577a2e..52bcb91 100644 --- a/src/main/java/frc4388/utility/controller/DeadbandedXboxController.java +++ b/src/main/java/frc4388/utility/controller/DeadbandedXboxController.java @@ -6,22 +6,22 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.wpilibj.XboxController; public class DeadbandedXboxController extends XboxController { - public DeadbandedXboxController(int port) { super(port); } + public DeadbandedXboxController(int port) { super(port); } - @Override public double getLeftX() { return getLeft().getX(); } - @Override public double getLeftY() { return getLeft().getY(); } - @Override public double getRightX() { return getRight().getX(); } - @Override public double getRightY() { return getRight().getY(); } + @Override public double getLeftX() { return getLeft().getX(); } + @Override public double getLeftY() { return getLeft().getY(); } + @Override public double getRightX() { return getRight().getX(); } + @Override public double getRightY() { return getRight().getY(); } - public Translation2d getLeft() { return skewToDeadzonedCircle(super.getLeftX(), super.getLeftY()); } - public Translation2d getRight() { return skewToDeadzonedCircle(-super.getRightX(), super.getRightY()); } + public Translation2d getLeft() { return skewToDeadzonedCircle(super.getLeftX(), super.getLeftY()); } + public Translation2d getRight() { return skewToDeadzonedCircle(-super.getRightX(), super.getRightY()); } - public static Translation2d skewToDeadzonedCircle(double x, double y) { - Translation2d translation2d = new Translation2d(x, y); - double magnitude = translation2d.getNorm(); + public static Translation2d skewToDeadzonedCircle(double x, double y) { + Translation2d translation2d = new Translation2d(x, y); + double magnitude = translation2d.getNorm(); - if (magnitude < LEFT_AXIS_DEADBAND) return new Translation2d(0,0); + if (magnitude < LEFT_AXIS_DEADBAND) return new Translation2d(0,0); - return translation2d; - } -} \ No newline at end of file + return translation2d; + } +} diff --git a/src/main/java/frc4388/utility/controller/IHandController.java b/src/main/java/frc4388/utility/controller/IHandController.java index 13aa763..0ab3efd 100644 --- a/src/main/java/frc4388/utility/controller/IHandController.java +++ b/src/main/java/frc4388/utility/controller/IHandController.java @@ -5,17 +5,17 @@ package frc4388.utility.controller; */ public interface IHandController { - public double getLeftXAxis(); + public double getLeftXAxis(); - public double getLeftYAxis(); + public double getLeftYAxis(); - public double getRightXAxis(); + public double getRightXAxis(); - public double getRightYAxis(); + public double getRightYAxis(); - public double getLeftTriggerAxis(); + public double getLeftTriggerAxis(); - public double getRightTriggerAxis(); + public double getRightTriggerAxis(); - public int getDpadAngle(); + public int getDpadAngle(); } diff --git a/src/main/java/frc4388/utility/controller/VirtualController.java b/src/main/java/frc4388/utility/controller/VirtualController.java index 12e98cf..ad83443 100644 --- a/src/main/java/frc4388/utility/controller/VirtualController.java +++ b/src/main/java/frc4388/utility/controller/VirtualController.java @@ -87,7 +87,7 @@ public class VirtualController extends GenericHID { @Override public int getAxisType(int axis) { return 1; /* ! Warning, does not return accurate data. - Hopefully this isn't a problem */ + Hopefully this isn't a problem */ } @Override @@ -107,4 +107,4 @@ public class VirtualController extends GenericHID { public void setRumble(RumbleType type, double value) { System.out.println("Why are you Setting rumble on an virtual controller?"); } -} \ No newline at end of file +} diff --git a/src/main/java/frc4388/utility/controller/XboxController.java b/src/main/java/frc4388/utility/controller/XboxController.java index 8b8f0f8..8ff3f97 100644 --- a/src/main/java/frc4388/utility/controller/XboxController.java +++ b/src/main/java/frc4388/utility/controller/XboxController.java @@ -9,210 +9,210 @@ import edu.wpi.first.wpilibj.Joystick; */ public class XboxController implements IHandController { - public static final int LEFT_X_AXIS = 0; - public static final int LEFT_Y_AXIS = 1; - public static final int LEFT_TRIGGER_AXIS = 2; - public static final int RIGHT_TRIGGER_AXIS = 3; - public static final int RIGHT_X_AXIS = 4; - public static final int RIGHT_Y_AXIS = 5; - public static final int LEFT_RIGHT_DPAD_AXIS = 6; - public static final int TOP_BOTTOM_DPAD_AXIS = 6; + public static final int LEFT_X_AXIS = 0; + public static final int LEFT_Y_AXIS = 1; + public static final int LEFT_TRIGGER_AXIS = 2; + public static final int RIGHT_TRIGGER_AXIS = 3; + public static final int RIGHT_X_AXIS = 4; + public static final int RIGHT_Y_AXIS = 5; + public static final int LEFT_RIGHT_DPAD_AXIS = 6; + public static final int TOP_BOTTOM_DPAD_AXIS = 6; - public static final int A_BUTTON = 1; - public static final int B_BUTTON = 2; - public static final int X_BUTTON = 3; - public static final int Y_BUTTON = 4; - public static final int LEFT_BUMPER_BUTTON = 5; - public static final int RIGHT_BUMPER_BUTTON = 6; - public static final int BACK_BUTTON = 7; - public static final int START_BUTTON = 8; + public static final int A_BUTTON = 1; + public static final int B_BUTTON = 2; + public static final int X_BUTTON = 3; + public static final int Y_BUTTON = 4; + public static final int LEFT_BUMPER_BUTTON = 5; + public static final int RIGHT_BUMPER_BUTTON = 6; + public static final int BACK_BUTTON = 7; + public static final int START_BUTTON = 8; - public static final int LEFT_JOYSTICK_BUTTON = 9; - public static final int RIGHT_JOYSTICK_BUTTON = 10; + public static final int LEFT_JOYSTICK_BUTTON = 9; + public static final int RIGHT_JOYSTICK_BUTTON = 10; - private static final double LEFT_DPAD_TOLERANCE = -0.9; - private static final double RIGHT_DPAD_TOLERANCE = 0.9; - private static final double BOTTOM_DPAD_TOLERANCE = -0.9; - private static final double TOP_DPAD_TOLERANCE = 0.9; + private static final double LEFT_DPAD_TOLERANCE = -0.9; + private static final double RIGHT_DPAD_TOLERANCE = 0.9; + private static final double BOTTOM_DPAD_TOLERANCE = -0.9; + private static final double TOP_DPAD_TOLERANCE = 0.9; - private static final double LEFT_TRIGGER_TOLERANCE = 0.5; - private static final double RIGHT_TRIGGER_TOLERANCE = 0.5; + private static final double LEFT_TRIGGER_TOLERANCE = 0.5; + private static final double RIGHT_TRIGGER_TOLERANCE = 0.5; - private static final double RIGHT_AXIS_UP_TOLERANCE = -0.9; - private static final double RIGHT_AXIS_DOWN_TOLERANCE = 0.9; - private static final double RIGHT_AXIS_RIGHT_TOLERANCE = 0.9; - private static final double RIGHT_AXIS_LEFT_TOLERANCE = -0.9; + private static final double RIGHT_AXIS_UP_TOLERANCE = -0.9; + private static final double RIGHT_AXIS_DOWN_TOLERANCE = 0.9; + private static final double RIGHT_AXIS_RIGHT_TOLERANCE = 0.9; + private static final double RIGHT_AXIS_LEFT_TOLERANCE = -0.9; - private static final double LEFT_AXIS_UP_TOLERANCE = -0.9; - private static final double LEFT_AXIS_DOWN_TOLERANCE = 0.9; - private static final double LEFT_AXIS_RIGHT_TOLERANCE = 0.9; - private static final double LEFT_AXIS_LEFT_TOLERANCE = -0.9; + private static final double LEFT_AXIS_UP_TOLERANCE = -0.9; + private static final double LEFT_AXIS_DOWN_TOLERANCE = 0.9; + private static final double LEFT_AXIS_RIGHT_TOLERANCE = 0.9; + private static final double LEFT_AXIS_LEFT_TOLERANCE = -0.9; - private static final double DEADZONE = 0.1; + private static final double DEADZONE = 0.1; - private Joystick m_stick; + private Joystick m_stick; - /** + /** * Add your docs here. */ - public XboxController(int portNumber){ - m_stick = new Joystick(portNumber); - } + public XboxController(int portNumber){ + m_stick = new Joystick(portNumber); + } - /** + /** * Add your docs here. */ - public Joystick getJoyStick() { - return m_stick; - } + public Joystick getJoyStick() { + return m_stick; + } - /** - * Checks if the input falls within the deadzone. - * @param input from an axis on the controller - * @return true if input falls in deadzone, false if input falls outside deadzone - */ - private boolean inDeadZone(double input){ - return (Math.abs(input) < DEADZONE); + /** + * Checks if the input falls within the deadzone. + * @param input from an axis on the controller + * @return true if input falls in deadzone, false if input falls outside deadzone + */ + private boolean inDeadZone(double input){ + return (Math.abs(input) < DEADZONE); + } + + /** + * Updates an input to have a deadzone around the 0 position + * @param input from an axis on the controller + * @return updated input + */ + private double getAxisWithDeadZoneCheck(double input){ + if(inDeadZone(input)){ + return 0.0; + } else { + return input; } + } - /** - * Updates an input to have a deadzone around the 0 position - * @param input from an axis on the controller - * @return updated input - */ - private double getAxisWithDeadZoneCheck(double input){ - if(inDeadZone(input)){ - return 0.0; - } else { - return input; - } - } + public boolean getAButton(){ + return m_stick.getRawButton(A_BUTTON); + } - public boolean getAButton(){ - return m_stick.getRawButton(A_BUTTON); - } + public boolean getXButton(){ + return m_stick.getRawButton(X_BUTTON); + } - public boolean getXButton(){ - return m_stick.getRawButton(X_BUTTON); - } + public boolean getBButton(){ + return m_stick.getRawButton(B_BUTTON); + } - public boolean getBButton(){ - return m_stick.getRawButton(B_BUTTON); - } + public boolean getYButton(){ + return m_stick.getRawButton(Y_BUTTON); + } - public boolean getYButton(){ - return m_stick.getRawButton(Y_BUTTON); - } + public boolean getBackButton(){ + return m_stick.getRawButton(BACK_BUTTON); + } - public boolean getBackButton(){ - return m_stick.getRawButton(BACK_BUTTON); - } + public boolean getStartButton(){ + return m_stick.getRawButton(START_BUTTON); + } - public boolean getStartButton(){ - return m_stick.getRawButton(START_BUTTON); - } + public boolean getLeftBumperButton(){ + return m_stick.getRawButton(LEFT_BUMPER_BUTTON); + } - public boolean getLeftBumperButton(){ - return m_stick.getRawButton(LEFT_BUMPER_BUTTON); - } + public boolean getRightBumperButton(){ + return m_stick.getRawButton(RIGHT_BUMPER_BUTTON); + } - public boolean getRightBumperButton(){ - return m_stick.getRawButton(RIGHT_BUMPER_BUTTON); - } + public boolean getLeftJoystickButton(){ + return m_stick.getRawButton(LEFT_JOYSTICK_BUTTON); + } - public boolean getLeftJoystickButton(){ - return m_stick.getRawButton(LEFT_JOYSTICK_BUTTON); - } + public boolean getRightJoystickButton(){ + return m_stick.getRawButton(RIGHT_JOYSTICK_BUTTON); + } - public boolean getRightJoystickButton(){ - return m_stick.getRawButton(RIGHT_JOYSTICK_BUTTON); - } + public double getLeftXAxis(){ + return getAxisWithDeadZoneCheck(m_stick.getRawAxis(LEFT_X_AXIS)); + } - public double getLeftXAxis(){ - return getAxisWithDeadZoneCheck(m_stick.getRawAxis(LEFT_X_AXIS)); - } + public double getLeftYAxis(){ + return getAxisWithDeadZoneCheck(m_stick.getRawAxis(LEFT_Y_AXIS)); + } - public double getLeftYAxis(){ - return getAxisWithDeadZoneCheck(m_stick.getRawAxis(LEFT_Y_AXIS)); - } + public double getRightXAxis(){ + return getAxisWithDeadZoneCheck(m_stick.getRawAxis(RIGHT_X_AXIS)); + } - public double getRightXAxis(){ - return getAxisWithDeadZoneCheck(m_stick.getRawAxis(RIGHT_X_AXIS)); - } + public double getRightYAxis(){ + return getAxisWithDeadZoneCheck(m_stick.getRawAxis(RIGHT_Y_AXIS)); + } - public double getRightYAxis(){ - return getAxisWithDeadZoneCheck(m_stick.getRawAxis(RIGHT_Y_AXIS)); - } + public double getLeftTriggerAxis(){ + return getAxisWithDeadZoneCheck(m_stick.getRawAxis(LEFT_TRIGGER_AXIS)); + } - public double getLeftTriggerAxis(){ - return getAxisWithDeadZoneCheck(m_stick.getRawAxis(LEFT_TRIGGER_AXIS)); - } + public double getRightTriggerAxis(){ + return getAxisWithDeadZoneCheck(m_stick.getRawAxis(RIGHT_TRIGGER_AXIS)); + } - public double getRightTriggerAxis(){ - return getAxisWithDeadZoneCheck(m_stick.getRawAxis(RIGHT_TRIGGER_AXIS)); - } + /** + * Get the angle input from the dpad. + * @return -1 if nothing is pressed, or the angle of the button pressed. 0 = up, 90 = right, etc. + */ + public int getDpadAngle() { + return m_stick.getPOV(0); + } - /** - * Get the angle input from the dpad. - * @return -1 if nothing is pressed, or the angle of the button pressed. 0 = up, 90 = right, etc. - */ - public int getDpadAngle() { - return m_stick.getPOV(0); - } + public boolean getDPadLeft(){ + return (m_stick.getRawAxis(LEFT_RIGHT_DPAD_AXIS) < LEFT_DPAD_TOLERANCE); + } - public boolean getDPadLeft(){ - return (m_stick.getRawAxis(LEFT_RIGHT_DPAD_AXIS) < LEFT_DPAD_TOLERANCE); - } + public boolean getDPadRight(){ + return (m_stick.getRawAxis(LEFT_RIGHT_DPAD_AXIS) > RIGHT_DPAD_TOLERANCE); + } - public boolean getDPadRight(){ - return (m_stick.getRawAxis(LEFT_RIGHT_DPAD_AXIS) > RIGHT_DPAD_TOLERANCE); - } + public boolean getDPadTop(){ + return (m_stick.getRawAxis(TOP_BOTTOM_DPAD_AXIS) < TOP_DPAD_TOLERANCE); + } - public boolean getDPadTop(){ - return (m_stick.getRawAxis(TOP_BOTTOM_DPAD_AXIS) < TOP_DPAD_TOLERANCE); - } + public boolean getDPadBottom(){ + return (m_stick.getRawAxis(TOP_BOTTOM_DPAD_AXIS) > BOTTOM_DPAD_TOLERANCE); + } - public boolean getDPadBottom(){ - return (m_stick.getRawAxis(TOP_BOTTOM_DPAD_AXIS) > BOTTOM_DPAD_TOLERANCE); - } + public boolean getLeftTrigger(){ + return (getLeftTriggerAxis() > LEFT_TRIGGER_TOLERANCE); + } - public boolean getLeftTrigger(){ - return (getLeftTriggerAxis() > LEFT_TRIGGER_TOLERANCE); - } + public boolean getRightTrigger(){ + return (getRightTriggerAxis() > RIGHT_TRIGGER_TOLERANCE); + } - public boolean getRightTrigger(){ - return (getRightTriggerAxis() > RIGHT_TRIGGER_TOLERANCE); - } + public boolean getRightAxisUpTrigger(){ + return (getRightYAxis() < RIGHT_AXIS_UP_TOLERANCE); + } - public boolean getRightAxisUpTrigger(){ - return (getRightYAxis() < RIGHT_AXIS_UP_TOLERANCE); - } + public boolean getRightAxisDownTrigger(){ + return (getRightYAxis() > RIGHT_AXIS_DOWN_TOLERANCE); + } - public boolean getRightAxisDownTrigger(){ - return (getRightYAxis() > RIGHT_AXIS_DOWN_TOLERANCE); - } + public boolean getRightAxisLeftTrigger(){ + return (getRightXAxis() > RIGHT_AXIS_LEFT_TOLERANCE); + } - public boolean getRightAxisLeftTrigger(){ - return (getRightXAxis() > RIGHT_AXIS_LEFT_TOLERANCE); - } + public boolean getRightAxisRightTrigger(){ + return (getRightXAxis() > RIGHT_AXIS_RIGHT_TOLERANCE); + } - public boolean getRightAxisRightTrigger(){ - return (getRightXAxis() > RIGHT_AXIS_RIGHT_TOLERANCE); - } + public boolean getLeftAxisUpTrigger(){ + return (getLeftYAxis() < LEFT_AXIS_UP_TOLERANCE); + } - public boolean getLeftAxisUpTrigger(){ - return (getLeftYAxis() < LEFT_AXIS_UP_TOLERANCE); - } + public boolean getLeftAxisDownTrigger(){ + return (getLeftYAxis() > LEFT_AXIS_DOWN_TOLERANCE); + } - public boolean getLeftAxisDownTrigger(){ - return (getLeftYAxis() > LEFT_AXIS_DOWN_TOLERANCE); - } + public boolean getLeftAxisLeftTrigger(){ + return (getLeftXAxis() > LEFT_AXIS_LEFT_TOLERANCE); + } - public boolean getLeftAxisLeftTrigger(){ - return (getLeftXAxis() > LEFT_AXIS_LEFT_TOLERANCE); - } - - public boolean getLeftAxisRightTrigger(){ - return (getLeftXAxis() > LEFT_AXIS_RIGHT_TOLERANCE); - } -} \ No newline at end of file + public boolean getLeftAxisRightTrigger(){ + return (getLeftXAxis() > LEFT_AXIS_RIGHT_TOLERANCE); + } +} diff --git a/src/main/java/frc4388/utility/controller/XboxTriggerButton.java b/src/main/java/frc4388/utility/controller/XboxTriggerButton.java index 0a56841..0ba4384 100644 --- a/src/main/java/frc4388/utility/controller/XboxTriggerButton.java +++ b/src/main/java/frc4388/utility/controller/XboxTriggerButton.java @@ -8,61 +8,61 @@ package frc4388.utility.controller; * exceeds a tolerance defined in {@link XboxController}. */ public class XboxTriggerButton {//extends Trigger { - public static final int RIGHT_TRIGGER = 0; - public static final int LEFT_TRIGGER = 1; - public static final int RIGHT_AXIS_UP_TRIGGER = 2; - public static final int RIGHT_AXIS_DOWN_TRIGGER = 3; - public static final int RIGHT_AXIS_RIGHT_TRIGGER = 4; - public static final int RIGHT_AXIS_LEFT_TRIGGER = 5; - public static final int LEFT_AXIS_UP_TRIGGER = 6; - public static final int LEFT_AXIS_DOWN_TRIGGER = 7; - public static final int LEFT_AXIS_RIGHT_TRIGGER = 8; - public static final int LEFT_AXIS_LEFT_TRIGGER = 9; + public static final int RIGHT_TRIGGER = 0; + public static final int LEFT_TRIGGER = 1; + public static final int RIGHT_AXIS_UP_TRIGGER = 2; + public static final int RIGHT_AXIS_DOWN_TRIGGER = 3; + public static final int RIGHT_AXIS_RIGHT_TRIGGER = 4; + public static final int RIGHT_AXIS_LEFT_TRIGGER = 5; + public static final int LEFT_AXIS_UP_TRIGGER = 6; + public static final int LEFT_AXIS_DOWN_TRIGGER = 7; + public static final int LEFT_AXIS_RIGHT_TRIGGER = 8; + public static final int LEFT_AXIS_LEFT_TRIGGER = 9; - private XboxController m_controller; - private int m_trigger; + private XboxController m_controller; + private int m_trigger; - /** - * Creates a Trigger-Button mapped to a specific Xbox controller and trigger - */ - public XboxTriggerButton(XboxController controller, int trigger) { - m_controller = controller; - m_trigger = trigger; + /** + * Creates a Trigger-Button mapped to a specific Xbox controller and trigger + */ + public XboxTriggerButton(XboxController controller, int trigger) { + m_controller = controller; + m_trigger = trigger; + } + + /** {@inheritDoc} */ + // @Override + public boolean get() { + if (m_trigger == RIGHT_TRIGGER) { + return m_controller.getRightTrigger(); } - - /** {@inheritDoc} */ - // @Override - public boolean get() { - if (m_trigger == RIGHT_TRIGGER) { - return m_controller.getRightTrigger(); - } - else if (m_trigger == LEFT_TRIGGER) { - return m_controller.getLeftTrigger(); - } - else if (m_trigger == RIGHT_AXIS_UP_TRIGGER) { - return m_controller.getRightAxisUpTrigger(); - } - else if (m_trigger == RIGHT_AXIS_DOWN_TRIGGER) { - return m_controller.getRightAxisDownTrigger(); - } - else if (m_trigger == RIGHT_AXIS_RIGHT_TRIGGER) { - return m_controller.getRightAxisRightTrigger(); - } - else if (m_trigger == RIGHT_AXIS_LEFT_TRIGGER) { - return m_controller.getRightAxisLeftTrigger(); - } - else if (m_trigger == LEFT_AXIS_UP_TRIGGER) { - return m_controller.getLeftAxisUpTrigger(); - } - else if (m_trigger == LEFT_AXIS_DOWN_TRIGGER) { - return m_controller.getLeftAxisDownTrigger(); - } - else if (m_trigger == LEFT_AXIS_RIGHT_TRIGGER) { - return m_controller.getLeftAxisRightTrigger(); - } - else if (m_trigger == LEFT_AXIS_LEFT_TRIGGER) { - return m_controller.getLeftAxisLeftTrigger(); - } - return false; + else if (m_trigger == LEFT_TRIGGER) { + return m_controller.getLeftTrigger(); } + else if (m_trigger == RIGHT_AXIS_UP_TRIGGER) { + return m_controller.getRightAxisUpTrigger(); + } + else if (m_trigger == RIGHT_AXIS_DOWN_TRIGGER) { + return m_controller.getRightAxisDownTrigger(); + } + else if (m_trigger == RIGHT_AXIS_RIGHT_TRIGGER) { + return m_controller.getRightAxisRightTrigger(); + } + else if (m_trigger == RIGHT_AXIS_LEFT_TRIGGER) { + return m_controller.getRightAxisLeftTrigger(); + } + else if (m_trigger == LEFT_AXIS_UP_TRIGGER) { + return m_controller.getLeftAxisUpTrigger(); + } + else if (m_trigger == LEFT_AXIS_DOWN_TRIGGER) { + return m_controller.getLeftAxisDownTrigger(); + } + else if (m_trigger == LEFT_AXIS_RIGHT_TRIGGER) { + return m_controller.getLeftAxisRightTrigger(); + } + else if (m_trigger == LEFT_AXIS_LEFT_TRIGGER) { + return m_controller.getLeftAxisLeftTrigger(); + } + return false; + } }