From cb1e06b72f0df353e55c0ae0c4655c96a4ee3855 Mon Sep 17 00:00:00 2001 From: aarav18 Date: Fri, 18 Feb 2022 19:16:59 -0700 Subject: [PATCH] delete all the things --- .OutlineViewer/outlineviewer-window.json | 35 ++ .OutlineViewer/outlineviewer.json | 14 + src/main/java/frc4388/robot/Robot.java | 10 +- .../java/frc4388/robot/RobotContainer.java | 319 +++++++++--------- .../robot/commands/InterruptSubsystem.java | 42 --- .../commands/auto/AutoPath1FromCenter.java | 46 --- .../commands/auto/AutoPath2FromRight.java | 54 --- .../frc4388/robot/commands/auto/Barrel.java | 28 -- .../robot/commands/auto/BarrelMany.java | 27 -- .../robot/commands/auto/BarrelStart.java | 34 -- .../frc4388/robot/commands/auto/Bounce.java | 35 -- .../commands/auto/DriveOffLineBackward.java | 31 -- .../commands/auto/DriveOffLineForward.java | 34 -- .../commands/auto/EightBallAutoMiddle.java | 29 -- .../robot/commands/auto/ExecuteCommand.java | 65 ---- .../commands/auto/FiveBallAutoMiddle.java | 28 -- .../robot/commands/auto/GalacticSearch.java | 51 --- .../robot/commands/auto/IdentifyPath.java | 163 --------- .../frc4388/robot/commands/auto/RunPath.java | 27 -- .../robot/commands/auto/SequentialTest.java | 33 -- .../commands/auto/SixBallAutoMiddle.java | 35 -- .../frc4388/robot/commands/auto/Slalom.java | 28 -- .../commands/auto/TankDriveVelocity.java | 67 ---- .../commands/auto/TenBallAutoMiddle.java | 59 ---- .../frc4388/robot/commands/auto/Wait.java | 71 ---- .../commands/climber/DisengageRatchet.java | 48 --- .../climber/RunClimberWithTriggers.java | 65 ---- .../climber/RunLevelerWithJoystick.java | 53 --- .../drive/GotoCoordinatesRobotRelative.java | 87 ----- .../robot/commands/drive/PlaySongDrive.java | 5 +- .../robot/commands/drive/SetShooterToOdo.java | 36 -- .../robot/commands/drive/SkipSong.java | 56 --- .../robot/commands/drive/TurnDegrees.java | 74 ---- .../commands/drive/VisionUpdateOdometry.java | 19 +- .../commands/intake/RunExtenderOutIn.java | 77 ----- .../robot/commands/intake/RunIntake.java | 46 --- .../intake/RunIntakeWithTriggers.java | 63 ---- .../commands/shooter/CalibrateShooter.java | 69 ---- .../commands/shooter/HoodPositionPID.java | 59 ---- .../robot/commands/shooter/PrepChecker.java | 52 --- .../commands/shooter/RunHoodWithJoystick.java | 54 --- .../commands/shooter/ShootPrepGroup.java | 34 -- .../commands/shooter/ShooterGoalPosition.java | 52 --- .../robot/commands/shooter/ShooterManual.java | 45 --- .../shooter/ShooterTrenchPosition.java | 52 --- .../shooter/ShooterVelocityControlPID.java | 61 ---- .../robot/commands/shooter/TrackTarget.java | 138 -------- .../robot/commands/shooter/TrimShooter.java | 65 ---- .../robot/commands/storage/ManageStorage.java | 147 -------- .../commands/storage/ManageStoragePID.java | 143 -------- .../robot/commands/storage/RunStorage.java | 46 --- .../robot/commands/storage/StoragePrep.java | 54 --- .../frc4388/robot/subsystems/Climber.java | 93 ----- .../java/frc4388/robot/subsystems/Drive.java | 8 +- .../java/frc4388/robot/subsystems/Intake.java | 86 ----- .../frc4388/robot/subsystems/Leveler.java | 57 ---- .../frc4388/robot/subsystems/LimeLight.java | 157 --------- .../frc4388/robot/subsystems/Shooter.java | 200 ----------- .../frc4388/robot/subsystems/ShooterAim.java | 177 ---------- .../frc4388/robot/subsystems/ShooterHood.java | 107 ------ .../frc4388/robot/subsystems/Storage.java | 144 -------- .../java/frc4388/robot/subsystems/Vision.java | 34 +- 62 files changed, 240 insertions(+), 3888 deletions(-) create mode 100644 .OutlineViewer/outlineviewer-window.json create mode 100644 .OutlineViewer/outlineviewer.json delete mode 100644 src/main/java/frc4388/robot/commands/InterruptSubsystem.java delete mode 100644 src/main/java/frc4388/robot/commands/auto/AutoPath1FromCenter.java delete mode 100644 src/main/java/frc4388/robot/commands/auto/AutoPath2FromRight.java delete mode 100644 src/main/java/frc4388/robot/commands/auto/Barrel.java delete mode 100644 src/main/java/frc4388/robot/commands/auto/BarrelMany.java delete mode 100644 src/main/java/frc4388/robot/commands/auto/BarrelStart.java delete mode 100644 src/main/java/frc4388/robot/commands/auto/Bounce.java delete mode 100644 src/main/java/frc4388/robot/commands/auto/DriveOffLineBackward.java delete mode 100644 src/main/java/frc4388/robot/commands/auto/DriveOffLineForward.java delete mode 100644 src/main/java/frc4388/robot/commands/auto/EightBallAutoMiddle.java delete mode 100644 src/main/java/frc4388/robot/commands/auto/ExecuteCommand.java delete mode 100644 src/main/java/frc4388/robot/commands/auto/FiveBallAutoMiddle.java delete mode 100644 src/main/java/frc4388/robot/commands/auto/GalacticSearch.java delete mode 100644 src/main/java/frc4388/robot/commands/auto/IdentifyPath.java delete mode 100644 src/main/java/frc4388/robot/commands/auto/RunPath.java delete mode 100644 src/main/java/frc4388/robot/commands/auto/SequentialTest.java delete mode 100644 src/main/java/frc4388/robot/commands/auto/SixBallAutoMiddle.java delete mode 100644 src/main/java/frc4388/robot/commands/auto/Slalom.java delete mode 100644 src/main/java/frc4388/robot/commands/auto/TankDriveVelocity.java delete mode 100644 src/main/java/frc4388/robot/commands/auto/TenBallAutoMiddle.java delete mode 100644 src/main/java/frc4388/robot/commands/auto/Wait.java delete mode 100644 src/main/java/frc4388/robot/commands/climber/DisengageRatchet.java delete mode 100644 src/main/java/frc4388/robot/commands/climber/RunClimberWithTriggers.java delete mode 100644 src/main/java/frc4388/robot/commands/climber/RunLevelerWithJoystick.java delete mode 100644 src/main/java/frc4388/robot/commands/drive/GotoCoordinatesRobotRelative.java delete mode 100644 src/main/java/frc4388/robot/commands/drive/SetShooterToOdo.java delete mode 100644 src/main/java/frc4388/robot/commands/drive/SkipSong.java delete mode 100644 src/main/java/frc4388/robot/commands/drive/TurnDegrees.java delete mode 100644 src/main/java/frc4388/robot/commands/intake/RunExtenderOutIn.java delete mode 100644 src/main/java/frc4388/robot/commands/intake/RunIntake.java delete mode 100644 src/main/java/frc4388/robot/commands/intake/RunIntakeWithTriggers.java delete mode 100644 src/main/java/frc4388/robot/commands/shooter/CalibrateShooter.java delete mode 100644 src/main/java/frc4388/robot/commands/shooter/HoodPositionPID.java delete mode 100644 src/main/java/frc4388/robot/commands/shooter/PrepChecker.java delete mode 100644 src/main/java/frc4388/robot/commands/shooter/RunHoodWithJoystick.java delete mode 100644 src/main/java/frc4388/robot/commands/shooter/ShootPrepGroup.java delete mode 100644 src/main/java/frc4388/robot/commands/shooter/ShooterGoalPosition.java delete mode 100644 src/main/java/frc4388/robot/commands/shooter/ShooterManual.java delete mode 100644 src/main/java/frc4388/robot/commands/shooter/ShooterTrenchPosition.java delete mode 100644 src/main/java/frc4388/robot/commands/shooter/ShooterVelocityControlPID.java delete mode 100644 src/main/java/frc4388/robot/commands/shooter/TrackTarget.java delete mode 100644 src/main/java/frc4388/robot/commands/shooter/TrimShooter.java delete mode 100644 src/main/java/frc4388/robot/commands/storage/ManageStorage.java delete mode 100644 src/main/java/frc4388/robot/commands/storage/ManageStoragePID.java delete mode 100644 src/main/java/frc4388/robot/commands/storage/RunStorage.java delete mode 100644 src/main/java/frc4388/robot/commands/storage/StoragePrep.java delete mode 100644 src/main/java/frc4388/robot/subsystems/Climber.java delete mode 100644 src/main/java/frc4388/robot/subsystems/Intake.java delete mode 100644 src/main/java/frc4388/robot/subsystems/Leveler.java delete mode 100644 src/main/java/frc4388/robot/subsystems/LimeLight.java delete mode 100644 src/main/java/frc4388/robot/subsystems/Shooter.java delete mode 100644 src/main/java/frc4388/robot/subsystems/ShooterAim.java delete mode 100644 src/main/java/frc4388/robot/subsystems/ShooterHood.java delete mode 100644 src/main/java/frc4388/robot/subsystems/Storage.java diff --git a/.OutlineViewer/outlineviewer-window.json b/.OutlineViewer/outlineviewer-window.json new file mode 100644 index 0000000..30eae86 --- /dev/null +++ b/.OutlineViewer/outlineviewer-window.json @@ -0,0 +1,35 @@ +{ + "MainWindow": { + "GLOBAL": { + "height": "0", + "maximized": "0", + "style": "0", + "userScale": "2", + "width": "0", + "xpos": "-32000", + "ypos": "-32000" + } + }, + "Window": { + "About": { + "Collapsed": "0", + "Pos": "13,143", + "Size": "574,113" + }, + "Debug##Default": { + "Collapsed": "0", + "Pos": "60,60", + "Size": "400,400" + }, + "Entries": { + "Collapsed": "0", + "Pos": "0,0", + "Size": "46,32" + }, + "Settings": { + "Collapsed": "0", + "Pos": "173,161", + "Size": "272,123" + } + } +} diff --git a/.OutlineViewer/outlineviewer.json b/.OutlineViewer/outlineviewer.json new file mode 100644 index 0000000..b9a7aec --- /dev/null +++ b/.OutlineViewer/outlineviewer.json @@ -0,0 +1,14 @@ +{ + "CameraPublisher": { + "limelight": { + "open": true + }, + "open": true + }, + "NetworkTables Settings": { + "mode": "Client" + }, + "limelight": { + "open": true + } +} diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 2822027..11d171a 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -77,13 +77,13 @@ public class Robot extends TimedRobot { /* Builds Autos */ m_robotContainer.buildAutos(); SmartDashboard.putString("Is Auto Start?", "NAH"); - m_robotContainer.m_robotLime.limeOff(); + // m_robotContainer.m_robotLime.limeOff(); } @Override public void disabledPeriodic() { m_robotContainer.resetOdometry(new Pose2d()); - m_robotContainer.idenPath(); + // m_robotContainer.idenPath(); if (m_modeChooser.getSelected() != Mode.get()) Mode.set(m_modeChooser.getSelected()); } @@ -97,7 +97,7 @@ public class Robot extends TimedRobot { m_autonomousCommand = m_robotContainer.getAutonomousCommand(); m_robotContainer.setDriveNeutralMode(NeutralMode.Coast); - m_robotContainer.setDriveGearState(true); + // m_robotContainer.setDriveGearState(true); m_initialTime = System.currentTimeMillis(); @@ -135,9 +135,9 @@ public class Robot extends TimedRobot { @Override public void teleopInit() { m_robotContainer.setDriveNeutralMode(NeutralMode.Brake); - m_robotContainer.setDriveGearState(true); + // m_robotContainer.setDriveGearState(true); - m_robotContainer.shiftClimberRatchet(false); + // m_robotContainer.shiftClimberRatchet(false); //m_robotContainer.configDriveTrainSensors(FeedbackDevice.IntegratedSensor); // This makes sure that the autonomous stops running when diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 6033a5d..2fec160 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -30,50 +30,50 @@ import edu.wpi.first.wpilibj2.command.button.JoystickButton; import frc4388.robot.Constants.DriveConstants; import frc4388.robot.Constants.Mode; import frc4388.robot.Constants.OIConstants; -import frc4388.robot.commands.InterruptSubsystem; -import frc4388.robot.commands.auto.Barrel; -import frc4388.robot.commands.auto.BarrelMany; -import frc4388.robot.commands.auto.BarrelStart; -import frc4388.robot.commands.auto.Bounce; -import frc4388.robot.commands.auto.DriveOffLineBackward; -import frc4388.robot.commands.auto.DriveOffLineForward; -import frc4388.robot.commands.auto.EightBallAutoMiddle; -import frc4388.robot.commands.auto.FiveBallAutoMiddle; -import frc4388.robot.commands.auto.GalacticSearch; -import frc4388.robot.commands.auto.SequentialTest; -import frc4388.robot.commands.auto.SixBallAutoMiddle; -import frc4388.robot.commands.auto.Slalom; -import frc4388.robot.commands.auto.TenBallAutoMiddle; -import frc4388.robot.commands.climber.DisengageRatchet; -import frc4388.robot.commands.climber.RunClimberWithTriggers; -import frc4388.robot.commands.climber.RunLevelerWithJoystick; +// import frc4388.robot.commands.InterruptSubsystem; +// import frc4388.robot.commands.auto.Barrel; +// import frc4388.robot.commands.auto.BarrelMany; +// import frc4388.robot.commands.auto.BarrelStart; +// import frc4388.robot.commands.auto.Bounce; +// import frc4388.robot.commands.auto.DriveOffLineBackward; +// import frc4388.robot.commands.auto.DriveOffLineForward; +// import frc4388.robot.commands.auto.EightBallAutoMiddle; +// import frc4388.robot.commands.auto.FiveBallAutoMiddle; +// import frc4388.robot.commands.auto.GalacticSearch; +// import frc4388.robot.commands.auto.SequentialTest; +// import frc4388.robot.commands.auto.SixBallAutoMiddle; +// import frc4388.robot.commands.auto.Slalom; +// import frc4388.robot.commands.auto.TenBallAutoMiddle; +// import frc4388.robot.commands.climber.DisengageRatchet; +// import frc4388.robot.commands.climber.RunClimberWithTriggers; +// import frc4388.robot.commands.climber.RunLevelerWithJoystick; import frc4388.robot.commands.drive.DriveWithJoystick; import frc4388.robot.commands.drive.PlaySongDrive; -import frc4388.robot.commands.drive.SetShooterToOdo; +// import frc4388.robot.commands.drive.SetShooterToOdo; import frc4388.robot.commands.drive.VisionUpdateOdometry; -import frc4388.robot.commands.intake.RunIntakeWithTriggers; -import frc4388.robot.commands.shooter.CalibrateShooter; -import frc4388.robot.commands.shooter.RunHoodWithJoystick; -import frc4388.robot.commands.shooter.ShootPrepGroup; -import frc4388.robot.commands.shooter.ShooterManual; -import frc4388.robot.commands.shooter.ShooterTrenchPosition; -import frc4388.robot.commands.shooter.TrackTarget; -import frc4388.robot.commands.shooter.TrimShooter; -import frc4388.robot.commands.storage.ManageStorage; +// import frc4388.robot.commands.intake.RunIntakeWithTriggers; +// import frc4388.robot.commands.shooter.CalibrateShooter; +// import frc4388.robot.commands.shooter.RunHoodWithJoystick; +// import frc4388.robot.commands.shooter.ShootPrepGroup; +// import frc4388.robot.commands.shooter.ShooterManual; +// import frc4388.robot.commands.shooter.ShooterTrenchPosition; +// import frc4388.robot.commands.shooter.TrackTarget; +// import frc4388.robot.commands.shooter.TrimShooter; +// import frc4388.robot.commands.storage.ManageStorage; import frc4388.robot.subsystems.Camera; -import frc4388.robot.subsystems.Climber; +// import frc4388.robot.subsystems.Climber; import frc4388.robot.subsystems.Drive; -import frc4388.robot.subsystems.Intake; +// import frc4388.robot.subsystems.Intake; import frc4388.robot.subsystems.LED; -import frc4388.robot.subsystems.Leveler; -import frc4388.robot.subsystems.LimeLight; +// import frc4388.robot.subsystems.Leveler; +// import frc4388.robot.subsystems.LimeLight; import frc4388.robot.subsystems.Pneumatics; -import frc4388.robot.subsystems.Shooter; -import frc4388.robot.subsystems.ShooterAim; -import frc4388.robot.subsystems.ShooterHood; -import frc4388.robot.subsystems.Storage; +// import frc4388.robot.subsystems.Shooter_1; +// import frc4388.robot.subsystems.ShooterAim_1; +// import frc4388.robot.subsystems.ShooterHood_1; +// import frc4388.robot.subsystems.Storage; import frc4388.robot.subsystems.Vision; -import frc4388.robot.subsystems.Storage.StorageMode; +// import frc4388.robot.subsystems.Storage.StorageMode; import frc4388.utility.controller.ButtonFox; import frc4388.utility.controller.IHandController; import frc4388.utility.controller.XboxController; @@ -87,20 +87,20 @@ import frc4388.utility.controller.XboxController; public class RobotContainer { /* Subsystems */ private final Drive m_robotDrive = new Drive(); - private final Pneumatics m_robotPneumatics = new Pneumatics(); + // private final Pneumatics m_robotPneumatics = new Pneumatics(); private final LED m_robotLED = new LED(); - private final Intake m_robotIntake = new Intake(); - private final Shooter m_robotShooter = new Shooter(); - private final ShooterAim m_robotShooterAim = new ShooterAim(); - private final ShooterHood m_robotShooterHood = new ShooterHood(); - private final Climber m_robotClimber = new Climber(); - private final Leveler m_robotLeveler = new Leveler(); - private final Storage m_robotStorage = new Storage(); + // private final Intake m_robotIntake = new Intake(); + // private final Shooter_1 m_robotShooter = new Shooter_1(); + // private final ShooterAim_1 m_robotShooterAim = new ShooterAim_1(); + // private final ShooterHood_1 m_robotShooterHood = new ShooterHood_1(); + // private final Climber m_robotClimber = new Climber(); + // private final Leveler m_robotLeveler = new Leveler(); + // private final Storage m_robotStorage = new Storage(); /* Cameras */ private final Camera m_robotCameraFront = new Camera("front", 0, 160, 120, 40); private final Camera m_robotCameraBack = new Camera("back", 1, 160, 120, 40); - public final LimeLight m_robotLime = new LimeLight(); + // public final LimeLight m_robotLime = new LimeLight(); public final Vision m_robotVision = new Vision(); /* Controllers */ @@ -111,22 +111,22 @@ public class RobotContainer { private static XboxController m_manualXbox = new XboxController(3); /* Autos */ - double m_totalTimeAuto; - SixBallAutoMiddle m_sixBallAutoMiddle; - SixBallAutoMiddle m_sixBallAutoMiddle0; - SixBallAutoMiddle m_sixBallAutoMiddle1; - EightBallAutoMiddle m_eightBallAutoMiddle; - DriveOffLineForward m_driveOffLineForward; - DriveOffLineBackward m_driveOffLineBackward; - FiveBallAutoMiddle m_fiveBallAutoMiddle; - TenBallAutoMiddle m_tenBallAutoMiddle; - Slalom m_slalom; - Barrel m_barrel; - BarrelStart m_barrelStart; - BarrelMany m_barrelMany; - Bounce m_bounce; - SequentialTest m_sequentialTest; - GalacticSearch m_galacticSearch; + // double m_totalTimeAuto; + // SixBallAutoMiddle m_sixBallAutoMiddle; + // SixBallAutoMiddle m_sixBallAutoMiddle0; + // SixBallAutoMiddle m_sixBallAutoMiddle1; + // EightBallAutoMiddle m_eightBallAutoMiddle; + // DriveOffLineForward m_driveOffLineForward; + // DriveOffLineBackward m_driveOffLineBackward; + // FiveBallAutoMiddle m_fiveBallAutoMiddle; + // TenBallAutoMiddle m_tenBallAutoMiddle; + // Slalom m_slalom; + // Barrel m_barrel; + // BarrelStart m_barrelStart; + // BarrelMany m_barrelMany; + // Bounce m_bounce; + // SequentialTest m_sequentialTest; + // // GalacticSearch m_galacticSearch; public static boolean m_isShooterManual = false; @@ -140,12 +140,12 @@ public class RobotContainer { public void setControls(Mode mode) { CommandScheduler.getInstance().clearButtons(); /* Passing Drive and Pneumatics Subsystems */ - m_robotPneumatics.passRequiredSubsystem(m_robotDrive); - m_robotDrive.passRequiredSubsystem(m_robotPneumatics, m_robotShooter); - m_robotShooter.passRequiredSubsystem(m_robotShooterHood, m_robotShooterAim); - m_robotShooterHood.passRequiredSubsystem(m_robotShooter); - m_robotShooterAim.passRequiredSubsystem(m_robotShooter, m_robotDrive); - m_robotLeveler.passRequiredSubsystem(m_robotClimber); + // m_robotPneumatics.passRequiredSubsystem(m_robotDrive); + // m_robotDrive.passRequiredSubsystem(m_robotPneumatics, m_robotShooter); + // m_robotShooter.passRequiredSubsystem(m_robotShooterHood, m_robotShooterAim); + // m_robotShooterHood.passRequiredSubsystem(m_robotShooter); + // m_robotShooterAim.passRequiredSubsystem(m_robotShooter, m_robotDrive); + // m_robotLeveler.passRequiredSubsystem(m_robotClimber); final double drumShooterTargetPIDVelocity; @@ -156,9 +156,9 @@ public class RobotContainer { // buildAutos(); /* Default Commands */ // drives climber with input from triggers on the opperator controller - m_robotClimber.setDefaultCommand(new RunClimberWithTriggers(m_robotClimber, getDriverController())); + // m_robotClimber.setDefaultCommand(new RunClimberWithTriggers(m_robotClimber, getDriverController())); // drives the leveler with an axis input from the driver controller - m_robotLeveler.setDefaultCommand(new RunLevelerWithJoystick(m_robotLeveler, getOperatorController())); + // m_robotLeveler.setDefaultCommand(new RunLevelerWithJoystick(m_robotLeveler, getOperatorController())); } else { drumShooterTargetPIDVelocity = 0; configureButtonBindingsCasual(); @@ -166,21 +166,21 @@ public class RobotContainer { /* Default Commands */ // drives the robot with a two-axis input from the driver controller - m_robotDrive.setDefaultCommand(new DriveWithJoystick(m_robotDrive, m_robotPneumatics, getDriverController())); + // m_robotDrive.setDefaultCommand(new DriveWithJoystick(m_robotDrive, m_robotPneumatics, getDriverController())); // m_robotDrive.setDefaultCommand(new DriveWithJoystickUsingDeadAssistPID(m_robotDrive, m_robotPneumatics, getDriverController())); // drives intake with input from triggers on the opperator controller - m_robotIntake.setDefaultCommand(new RunIntakeWithTriggers(m_robotIntake, getOperatorController())); + // m_robotIntake.setDefaultCommand(new RunIntakeWithTriggers(m_robotIntake, getOperatorController())); // runs the turret with joystick - m_robotShooterAim.setDefaultCommand(new RunCommand(() -> m_robotShooterAim.runShooterWithInput(-m_operatorXbox.getLeftXAxis()), m_robotShooterAim)); + // m_robotShooterAim.setDefaultCommand(new RunCommand(() -> m_robotShooterAim.runShooterWithInput(-m_operatorXbox.getLeftXAxis()), m_robotShooterAim)); // runs the hood with joystick - m_robotShooterHood.setDefaultCommand(new RunHoodWithJoystick(m_robotShooterHood, getOperatorController())); + // m_robotShooterHood.setDefaultCommand(new RunHoodWithJoystick(m_robotShooterHood, getOperatorController())); // moves the drum not - m_robotShooter.setDefaultCommand(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(drumShooterTargetPIDVelocity), m_robotShooter)); + // m_robotShooter.setDefaultCommand(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(drumShooterTargetPIDVelocity), m_robotShooter)); // continually sends updates to the Blinkin LED controller to keep the lights on m_robotLED.setDefaultCommand(new RunCommand(m_robotLED::updateLED, m_robotLED)); // runs the storage not - m_robotStorage.setDefaultCommand(new ManageStorage(m_robotStorage)); - m_robotLime.setDefaultCommand(new RunCommand(m_robotLime::limeOff, m_robotLime)); + // m_robotStorage.setDefaultCommand(new ManageStorage(m_robotStorage)); + //m_robotLime.setDefaultCommand(new RunCommand(m_robotLime::limeOff, m_robotLime)); } /** @@ -192,7 +192,7 @@ public class RobotContainer { private void configureButtonBindingsCompetitive() { /* Test Buttons */ // A driver test button - new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON).whileHeld(new ShooterTrenchPosition(m_robotShooter, m_robotShooterHood, m_robotShooterAim)).whenReleased(new InterruptSubsystem(m_robotShooter)).whenReleased(new InterruptSubsystem(m_robotShooterHood)); + // new JoystickButton(getDriverJoystick(), XboxController.A_BUTTON).whileHeld(new ShooterTrenchPosition(m_robotShooter, m_robotShooterHood, m_robotShooterAim)).whenReleased(new InterruptSubsystem(m_robotShooter)).whenReleased(new InterruptSubsystem(m_robotShooterHood)); // B driver test button new JoystickButton(getDriverJoystick(), XboxController.B_BUTTON).whileHeld(new RunCommand(() -> m_robotDrive.runDriveVelocityPID(2000, 45), m_robotDrive)); // Y driver test button @@ -201,37 +201,37 @@ public class RobotContainer { new JoystickButton(getDriverJoystick(), XboxController.X_BUTTON).whenPressed(new InstantCommand()); /* Driver Buttons */ // sets solenoids into high gear - new JoystickButton(getDriverJoystick(), XboxController.RIGHT_BUMPER_BUTTON).whenPressed(new InstantCommand(() -> m_robotPneumatics.setShiftState(true), m_robotDrive)); + // new JoystickButton(getDriverJoystick(), XboxController.RIGHT_BUMPER_BUTTON).whenPressed(new InstantCommand(() -> m_robotPneumatics.setShiftState(true), m_robotDrive)); // sets solenoids into low gear - new JoystickButton(getDriverJoystick(), XboxController.LEFT_BUMPER_BUTTON).whenPressed(new InstantCommand(() -> m_robotPneumatics.setShiftState(false), m_robotDrive)); + // new JoystickButton(getDriverJoystick(), XboxController.LEFT_BUMPER_BUTTON).whenPressed(new InstantCommand(() -> m_robotPneumatics.setShiftState(false), m_robotDrive)); // Disengages the ratchet to allow for a climb - new JoystickButton(getDriverJoystick(), XboxController.BACK_BUTTON).whileHeld(new DisengageRatchet(m_robotClimber)); + // new JoystickButton(getDriverJoystick(), XboxController.BACK_BUTTON).whileHeld(new DisengageRatchet(m_robotClimber)); /* Operator Buttons */ // shoots until released - new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_BUMPER_BUTTON) + // new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_BUMPER_BUTTON) // .whileHeld(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotShooterHood, // m_robotStorage), false); // .whenReleased(new ManageStorage(m_robotStorage, StorageMode.RESET)); // .whenReleased(new RunCommand(() -> m_robotLime.limeOff())); - .whenPressed(new RunCommand(() -> m_robotStorage.runStorage(-1), m_robotStorage)).whenReleased(new InterruptSubsystem(m_robotStorage)); + // .whenPressed(new RunCommand(() -> m_robotStorage.runStorage(-1), m_robotStorage)).whenReleased(new InterruptSubsystem(m_robotStorage)); // shoots one ball - new JoystickButton(getOperatorJoystick(), XboxController.LEFT_BUMPER_BUTTON) + // new JoystickButton(getOperatorJoystick(), XboxController.LEFT_BUMPER_BUTTON) // .whenPressed(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotShooterHood, // m_robotStorage), false); // .whenReleased(new ManageStorage(m_robotStorage, StorageMode.RESET)); // .whenReleased(new RunCommand(() -> m_robotLime.limeOff())); - .whenPressed(new RunCommand(() -> m_robotStorage.runStorage(1.0), m_robotStorage)).whenReleased(new InterruptSubsystem(m_robotStorage)); + // .whenPressed(new RunCommand(() -> m_robotStorage.runStorage(1.0), m_robotStorage)).whenReleased(new InterruptSubsystem(m_robotStorage)); // extends or retracts the extender - new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON).whileHeld(new RunCommand(() -> m_robotIntake.runExtender(0.5))).whenReleased(new InstantCommand(() -> m_robotIntake.runExtender(0))); + // new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON).whileHeld(new RunCommand(() -> m_robotIntake.runExtender(0.5))).whenReleased(new InstantCommand(() -> m_robotIntake.runExtender(0))); // .whileHeld(new RunCommand(() -> m_robotShooterHood.runHood(0.2), m_robotShooterHood)); // VOP system testing - new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON) - .whenPressed(new SetShooterToOdo(m_robotShooterAim, m_robotDrive)); + // new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON) + // .whenPressed(new SetShooterToOdo(m_robotShooterAim, m_robotDrive)); - new JoystickButton(getOperatorJoystick(), XboxController.BACK_BUTTON) - .whenPressed(new VisionUpdateOdometry(m_robotVision, m_robotShooterAim, m_robotDrive)); + // new JoystickButton(getOperatorJoystick(), XboxController.BACK_BUTTON) + // .whenPressed(new VisionUpdateOdometry(m_robotVision, m_robotShooterAim, m_robotDrive)); // new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON).whileHeld(new RunCommand(() -> m_robotIntake.runExtender(-0.5))).whenReleased(new InstantCommand(() -> m_robotIntake.runExtender(0))); // // .whileHeld(new RunCommand(() -> m_robotShooterHood.runHood(-0.2), m_robotShooterHood)); @@ -240,115 +240,115 @@ public class RobotContainer { // starts tracking target - new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON).whileHeld(new TrackTarget(m_robotShooterAim)).whileHeld(new RunCommand(() -> m_robotShooterHood.runAngleAdjustPID(m_robotShooterHood.addFireAngle()))).whenReleased(new InstantCommand(m_robotLime::limeOff)); + // new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON).whileHeld(new TrackTarget(m_robotShooterAim)).whileHeld(new RunCommand(() -> m_robotShooterHood.runAngleAdjustPID(m_robotShooterHood.addFireAngle()))).whenReleased(new InstantCommand(m_robotLime::limeOff)); // .whileHeld(new RunCommand(() -> m_robotShooterAim.runshooterRotatePID())); // .whileHeld(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(11000))); // .whileHeld(new HoldTarget(m_robotShooter, m_robotShooterAim)) // .whileHeld(new RunCommand(() -> m_robotShooter.runAngleAdjustPID(30))); // Trims shooter - new JoystickButton(getOperatorJoystick(), XboxController.TOP_BOTTOM_DPAD_AXIS).whenPressed(new TrimShooter(m_robotShooter)); + // new JoystickButton(getOperatorJoystick(), XboxController.TOP_BOTTOM_DPAD_AXIS).whenPressed(new TrimShooter(m_robotShooter)); // Calibrates turret and hood - new JoystickButton(getOperatorJoystick(), XboxController.START_BUTTON).whileHeld(new CalibrateShooter(m_robotShooter, m_robotShooterAim, m_robotShooterHood)); + // new JoystickButton(getOperatorJoystick(), XboxController.START_BUTTON).whileHeld(new CalibrateShooter(m_robotShooter, m_robotShooterAim, m_robotShooterHood)); // Run drum /* Button Box */ // Storage Manual - new JoystickButton(getButtonFox(), ButtonFox.LEFT_SWITCH).whenPressed(new InstantCommand(() -> m_robotStorage.changeStorageMode(StorageMode.MANUAL))).whenReleased(new InstantCommand(() -> m_robotStorage.changeStorageMode(StorageMode.RESET))); + // new JoystickButton(getButtonFox(), ButtonFox.LEFT_SWITCH).whenPressed(new InstantCommand(() -> m_robotStorage.changeStorageMode(StorageMode.MANUAL))).whenReleased(new InstantCommand(() -> m_robotStorage.changeStorageMode(StorageMode.RESET))); // Meg - new JoystickButton(getButtonFox(), ButtonFox.MIDDLE_SWITCH).whileHeld(new PlaySongDrive(m_robotDrive, m_robotShooter)).whenReleased(new InterruptSubsystem(m_robotDrive)); + // new JoystickButton(getButtonFox(), ButtonFox.MIDDLE_SWITCH).whileHeld(new PlaySongDrive(m_robotDrive, m_robotShooter)).whenReleased(new InterruptSubsystem(m_robotDrive)); // Shooter Manual - new JoystickButton(getButtonFox(), ButtonFox.RIGHT_SWITCH).whileHeld(new ShooterManual(true)).whenReleased(new ShooterManual(false)); + // new JoystickButton(getButtonFox(), ButtonFox.RIGHT_SWITCH).whileHeld(new ShooterManual(true)).whenReleased(new ShooterManual(false)); // Goal Shooter Position - new JoystickButton(getButtonFox(), ButtonFox.RIGHT_BUTTON).whileHeld(new PlaySongDrive(m_robotDrive, m_robotShooter)).whenReleased(new InterruptSubsystem(m_robotDrive)); + // new JoystickButton(getButtonFox(), ButtonFox.RIGHT_BUTTON).whileHeld(new PlaySongDrive(m_robotDrive, m_robotShooter)).whenReleased(new InterruptSubsystem(m_robotDrive)); // Trench Shooter Position - new JoystickButton(getButtonFox(), ButtonFox.LEFT_BUTTON).whileHeld(new ShooterTrenchPosition(m_robotShooter, m_robotShooterHood, m_robotShooterAim)).whenReleased(new InterruptSubsystem(m_robotShooter)).whenReleased(new InterruptSubsystem(m_robotShooterHood)).whenReleased(new InterruptSubsystem(m_robotShooterAim)); + // new JoystickButton(getButtonFox(), ButtonFox.LEFT_BUTTON).whileHeld(new ShooterTrenchPosition(m_robotShooter, m_robotShooterHood, m_robotShooterAim)).whenReleased(new InterruptSubsystem(m_robotShooter)).whenReleased(new InterruptSubsystem(m_robotShooterHood)).whenReleased(new InterruptSubsystem(m_robotShooterAim)); // .whenPressed(new SkipSong(m_robotDrive, 1)); } private void configureButtonBindingsCasual() { /* Driver Buttons */ // sets solenoids into high gear - new JoystickButton(getDriverJoystick(), XboxController.RIGHT_BUMPER_BUTTON).whenPressed(new InstantCommand(() -> m_robotPneumatics.setShiftState(true), m_robotDrive)); + // new JoystickButton(getDriverJoystick(), XboxController.RIGHT_BUMPER_BUTTON).whenPressed(new InstantCommand(() -> m_robotPneumatics.setShiftState(true), m_robotDrive)); // sets solenoids into low gear - new JoystickButton(getDriverJoystick(), XboxController.LEFT_BUMPER_BUTTON).whenPressed(new InstantCommand(() -> m_robotPneumatics.setShiftState(false), m_robotDrive)); + // new JoystickButton(getDriverJoystick(), XboxController.LEFT_BUMPER_BUTTON).whenPressed(new InstantCommand(() -> m_robotPneumatics.setShiftState(false), m_robotDrive)); /* Operator Buttons */ // storage - new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_BUMPER_BUTTON) - .whenPressed(new RunCommand(() -> m_robotStorage.runStorage(-1), m_robotStorage)) - .whenReleased(new InterruptSubsystem(m_robotStorage)); + // new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_BUMPER_BUTTON) + // .whenPressed(new RunCommand(() -> m_robotStorage.runStorage(-1), m_robotStorage)) + // .whenReleased(new InterruptSubsystem(m_robotStorage)); - new JoystickButton(getOperatorJoystick(), XboxController.LEFT_BUMPER_BUTTON) - .whenPressed(new RunCommand(() -> m_robotStorage.runStorage(1.0), m_robotStorage)) - .whenReleased(new InterruptSubsystem(m_robotStorage)); + // new JoystickButton(getOperatorJoystick(), XboxController.LEFT_BUMPER_BUTTON) + // .whenPressed(new RunCommand(() -> m_robotStorage.runStorage(1.0), m_robotStorage)) + // .whenReleased(new InterruptSubsystem(m_robotStorage)); // extends or retracts the extender - new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON) - .whileHeld(new RunCommand(() -> m_robotIntake.runExtender(0.5))) - .whenReleased(new InstantCommand(() -> m_robotIntake.runExtender(0))); + // new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON) + // .whileHeld(new RunCommand(() -> m_robotIntake.runExtender(0.5))) + // .whenReleased(new InstantCommand(() -> m_robotIntake.runExtender(0))); - new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON) - .whileHeld(new RunCommand(() -> m_robotIntake.runExtender(-0.5))) - .whenReleased(new InstantCommand(() -> m_robotIntake.runExtender(0))); + // new JoystickButton(getOperatorJoystick(), XboxController.Y_BUTTON) + // .whileHeld(new RunCommand(() -> m_robotIntake.runExtender(-0.5))) + // .whenReleased(new InstantCommand(() -> m_robotIntake.runExtender(0))); - // Calibrates turret and hood - new JoystickButton(getOperatorJoystick(), XboxController.START_BUTTON) - .whileHeld(new CalibrateShooter(m_robotShooter, m_robotShooterAim, m_robotShooterHood)); + // // Calibrates turret and hood + // new JoystickButton(getOperatorJoystick(), XboxController.START_BUTTON) + // .whileHeld(new CalibrateShooter(m_robotShooter, m_robotShooterAim, m_robotShooterHood)); - // Run drum manual - new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) - .whileHeld(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(6000))) - .whenReleased(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(0))); + // // Run drum manual + // new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) + // .whileHeld(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(6000))) + // .whenReleased(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(0))); } public void buildAutos() { // resetOdometry(new Pose2d(0, 0, new Rotation2d(180))); - String[] sixBallAutoMiddlePaths = new String[] { "SixBallMidComplete" }; - m_sixBallAutoMiddle = new SixBallAutoMiddle(m_robotDrive, buildPaths(sixBallAutoMiddlePaths)); + // String[] sixBallAutoMiddlePaths = new String[] { "SixBallMidComplete" }; + // m_sixBallAutoMiddle = new SixBallAutoMiddle(m_robotDrive, buildPaths(sixBallAutoMiddlePaths)); - String[] sixBallAutoMiddle0Paths = new String[] { "SixBallMid0" }; - m_sixBallAutoMiddle0 = new SixBallAutoMiddle(m_robotDrive, buildPaths(sixBallAutoMiddle0Paths)); + // String[] sixBallAutoMiddle0Paths = new String[] { "SixBallMid0" }; + // m_sixBallAutoMiddle0 = new SixBallAutoMiddle(m_robotDrive, buildPaths(sixBallAutoMiddle0Paths)); - String[] sixBallAutoMiddle1Paths = new String[] { "SixBallMid1" }; - m_sixBallAutoMiddle1 = new SixBallAutoMiddle(m_robotDrive, buildPaths(sixBallAutoMiddle1Paths)); + // String[] sixBallAutoMiddle1Paths = new String[] { "SixBallMid1" }; + // m_sixBallAutoMiddle1 = new SixBallAutoMiddle(m_robotDrive, buildPaths(sixBallAutoMiddle1Paths)); - String[] slalom = new String[] { "Slalom" }; - m_slalom = new Slalom(m_robotDrive, buildPaths(slalom)); + // String[] slalom = new String[] { "Slalom" }; + // m_slalom = new Slalom(m_robotDrive, buildPaths(slalom)); - String[] barrel = new String[] { "BarrelStart" }; - m_barrel = new Barrel(m_robotDrive, buildPaths(barrel)); + // String[] barrel = new String[] { "BarrelStart" }; + // m_barrel = new Barrel(m_robotDrive, buildPaths(barrel)); - String[] barrelStart = new String[] { "Barrel" }; - m_barrelStart = new BarrelStart(m_robotDrive, buildPaths(barrelStart)); + // String[] barrelStart = new String[] { "Barrel" }; + // m_barrelStart = new BarrelStart(m_robotDrive, buildPaths(barrelStart)); - String[] bounce = new String[] { "Bounce1", "Bounce2", "Bounce3", "Bounce4" }; - m_bounce = new Bounce(m_robotDrive, this, buildPaths(bounce)); + // String[] bounce = new String[] { "Bounce1", "Bounce2", "Bounce3", "Bounce4" }; + // m_bounce = new Bounce(m_robotDrive, this, buildPaths(bounce)); - String[] barrelMany = new String[] { "BarrelManyWaypoints" }; - m_barrelMany = new BarrelMany(m_robotDrive, buildPaths(barrelMany)); + // String[] barrelMany = new String[] { "BarrelManyWaypoints" }; + // m_barrelMany = new BarrelMany(m_robotDrive, buildPaths(barrelMany)); - String[] eightBallAutoMiddlePaths = new String[] { "EightBallMidComplete" }; - m_eightBallAutoMiddle = new EightBallAutoMiddle(m_robotDrive, buildPaths(eightBallAutoMiddlePaths)); + // String[] eightBallAutoMiddlePaths = new String[] { "EightBallMidComplete" }; + // m_eightBallAutoMiddle = new EightBallAutoMiddle(m_robotDrive, buildPaths(eightBallAutoMiddlePaths)); - String[] driveOffLineForwardPaths = new String[] { "DriveOffLineForward" }; - m_driveOffLineForward = new DriveOffLineForward(m_robotDrive, this, buildPaths(driveOffLineForwardPaths)); + // String[] driveOffLineForwardPaths = new String[] { "DriveOffLineForward" }; + // m_driveOffLineForward = new DriveOffLineForward(m_robotDrive, this, buildPaths(driveOffLineForwardPaths)); - String[] driveOffLineBackwardPaths = new String[] { "DriveOffLineBackward" }; - m_driveOffLineBackward = new DriveOffLineBackward(m_robotDrive, buildPaths(driveOffLineBackwardPaths)); + // String[] driveOffLineBackwardPaths = new String[] { "DriveOffLineBackward" }; + // m_driveOffLineBackward = new DriveOffLineBackward(m_robotDrive, buildPaths(driveOffLineBackwardPaths)); - String[] fiveBallAutoMiddlePaths = new String[] { "FiveBallMidComplete" }; - m_fiveBallAutoMiddle = new FiveBallAutoMiddle(m_robotDrive, buildPaths(fiveBallAutoMiddlePaths)); + // String[] fiveBallAutoMiddlePaths = new String[] { "FiveBallMidComplete" }; + // m_fiveBallAutoMiddle = new FiveBallAutoMiddle(m_robotDrive, buildPaths(fiveBallAutoMiddlePaths)); String[] tenBallAutoMiddlePaths = new String[] { "SixBallMidComplete", "TenBallMidComplete" }; - m_tenBallAutoMiddle = new TenBallAutoMiddle(m_robotShooterHood, m_robotStorage, m_robotIntake, m_robotShooter, m_robotShooterAim, m_robotDrive, buildPaths(tenBallAutoMiddlePaths)); + // m_tenBallAutoMiddle = new TenBallAutoMiddle(m_robotShooterHood, m_robotStorage, m_robotIntake, m_robotShooter, m_robotShooterAim, m_robotDrive, buildPaths(tenBallAutoMiddlePaths)); String[] galacticSearchPaths = new String[] { "GSC_ARED", "GSC_ABLUE", "GSC_BRED", "GSC_BBLUE" }; - m_galacticSearch = new GalacticSearch(m_robotLime, m_robotIntake, buildPaths(galacticSearchPaths)); + // m_galacticSearch = new GalacticSearch(m_robotLime, m_robotIntake, buildPaths(galacticSearchPaths)); } - public void idenPath() { - m_robotLime.identifyPath(); - } + // public void idenPath() { + // m_robotLime.identifyPath(); + // } /** * Use this to pass the autonomous command to the main {@link Robot} class. @@ -362,7 +362,7 @@ public class RobotContainer { // RamseteCommand ramseteCommand = getRamseteCommand(trajectory); // Run path following command, then stop at the end. try { - SmartDashboard.putNumber("Trajectory Total Time", m_totalTimeAuto); + // SmartDashboard.putNumber("Trajectory Total Time", m_totalTimeAuto); // return m_sixBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); // return m_sixBallAutoMiddle1.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); // return m_eightBallAutoMiddle.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); @@ -373,13 +373,14 @@ public class RobotContainer { // return m_barrel.andThen(()-> m_robotDrive.tankDriveVelocity(0, 0)); // return m_barrelStart.andThen(()-> m_robotDrive.tankDriveVelocity(0, 0)); // return m_sequentialTest.andThen(() -> m_robotDrive.tankDriveVelocity(0,0)); - return m_galacticSearch.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); + // return m_galacticSearch.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0)); } catch (Exception e) { System.err.println("ERROR"); } return new InstantCommand(); } + //TODO TrajectoryConfig getTrajectoryConfig() { return new TrajectoryConfig(DriveConstants.MAX_SPEED_METERS_PER_SECOND, DriveConstants.MAX_ACCELERATION_METERS_PER_SECOND_SQUARED) // Add kinematics to ensure max speed is actually obeyed @@ -394,7 +395,7 @@ public class RobotContainer { RamseteCommand[] ramseteCommands = new RamseteCommand[paths.length]; double[] times = new double[paths.length]; Trajectory initialTrajectory; - m_totalTimeAuto = 0; + // m_totalTimeAuto = 0; try { if (true) { @@ -424,9 +425,9 @@ public class RobotContainer { DriverStation.reportError("Unable to open trajectory", e.getStackTrace()); } - for (int i = 0; i < times.length; i++) { - m_totalTimeAuto += times[i]; - } + // for (int i = 0; i < times.length; i++) { + // m_totalTimeAuto += times[i]; + // } return ramseteCommands; } @@ -442,13 +443,13 @@ public class RobotContainer { * Sets the gear of the drivetrain * @param state the gearing of the gearbox (true is high, false is low) */ - public void setDriveGearState(boolean state) { - m_robotPneumatics.setShiftState(state); - } + // public void setDriveGearState(boolean state) { + // m_robotPneumatics.setShiftState(state); + // } - public void shiftClimberRatchet(boolean state) { - m_robotClimber.shiftServo(state); - } + // public void shiftClimberRatchet(boolean state) { + // m_robotClimber.shiftServo(state); + // } public void resetOdometry(Pose2d pose) { m_robotDrive.setOdometry(pose); diff --git a/src/main/java/frc4388/robot/commands/InterruptSubsystem.java b/src/main/java/frc4388/robot/commands/InterruptSubsystem.java deleted file mode 100644 index 11722f3..0000000 --- a/src/main/java/frc4388/robot/commands/InterruptSubsystem.java +++ /dev/null @@ -1,42 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc4388.robot.commands; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -public class InterruptSubsystem extends CommandBase { - /** - * Creates a new InterruptSubsystem. - */ - public InterruptSubsystem(SubsystemBase subsystem) { - // Use addRequirements() here to declare subsystem dependencies. - addRequirements(subsystem); - } - - // 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() { - } - - // 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 true; - } -} diff --git a/src/main/java/frc4388/robot/commands/auto/AutoPath1FromCenter.java b/src/main/java/frc4388/robot/commands/auto/AutoPath1FromCenter.java deleted file mode 100644 index b7871b1..0000000 --- a/src/main/java/frc4388/robot/commands/auto/AutoPath1FromCenter.java +++ /dev/null @@ -1,46 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc4388.robot.commands.auto; - -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import frc4388.robot.commands.drive.GotoCoordinatesRobotRelative; -import frc4388.robot.subsystems.Drive; -import frc4388.robot.subsystems.Pneumatics; - -// NOTE: Consider using this command inline, rather than writing a subclass. For more -// information, see: -// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html -public class AutoPath1FromCenter extends SequentialCommandGroup { - Drive m_drive; - Pneumatics m_pneumatics; - - /** - * Creates a new AutoPath1FromCenter. - */ - public AutoPath1FromCenter(Drive subsystem, Pneumatics subsystem2) { - // Add your commands in the super() call, e.g. - // super(new FooCommand(), new BarCommand()); - m_drive = subsystem; - m_pneumatics = subsystem2; - - addCommands( new Wait(m_drive, 0, 1), - //shoot pre-loaded 3 balls - new GotoCoordinatesRobotRelative(m_drive, m_pneumatics, 75, 44, -90), - //Start Intake Ball 1 - new GotoCoordinatesRobotRelative(m_drive, m_pneumatics, 0, 12), - new GotoCoordinatesRobotRelative(m_drive, m_pneumatics, 0, 28), - //Start Intake Ball 2 - new GotoCoordinatesRobotRelative(m_drive, m_pneumatics, 0, 8), - new GotoCoordinatesRobotRelative(m_drive, m_pneumatics, 0, 28), - //Start Intake Ball 3 - new GotoCoordinatesRobotRelative(m_drive, m_pneumatics, 0, 8), - new Wait(m_drive, 0, 2) - //Shoot 3 Balls - ); - } -} diff --git a/src/main/java/frc4388/robot/commands/auto/AutoPath2FromRight.java b/src/main/java/frc4388/robot/commands/auto/AutoPath2FromRight.java deleted file mode 100644 index 5da03fa..0000000 --- a/src/main/java/frc4388/robot/commands/auto/AutoPath2FromRight.java +++ /dev/null @@ -1,54 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc4388.robot.commands.auto; - -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import frc4388.robot.commands.drive.GotoCoordinatesRobotRelative; -import frc4388.robot.subsystems.Drive; -import frc4388.robot.subsystems.Pneumatics; - -// NOTE: Consider using this command inline, rather than writing a subclass. For more -// information, see: -// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html -public class AutoPath2FromRight extends SequentialCommandGroup { - Drive m_drive; - Pneumatics m_pneumatics; - - /** - * Creates a new AutoPath2FromRight. - */ - public AutoPath2FromRight(Drive subsystem, Pneumatics subsystem2) { - // Add your commands in the super() call, e.g. - // super(new FooCommand(), new BarCommand()); - m_drive = subsystem; - m_pneumatics = subsystem2; - - addCommands( new Wait(m_drive, 0, 1), - new GotoCoordinatesRobotRelative(m_drive, m_pneumatics, 0, 77), - //Start Intake Ball 1 - new GotoCoordinatesRobotRelative(m_drive, m_pneumatics, 0, 8), - new GotoCoordinatesRobotRelative(m_drive, m_pneumatics, 0, 28), - //Start Intake Ball 2 - new GotoCoordinatesRobotRelative(m_drive, m_pneumatics, 0, 8), - //Shoot 5 Balls - new GotoCoordinatesRobotRelative(m_drive, m_pneumatics, 0, 28), - //Start Intake Ball 6 (Ball 1 second round) - new GotoCoordinatesRobotRelative(m_drive, m_pneumatics, 0, 8), - //Move to 7th Ball - new GotoCoordinatesRobotRelative(m_drive, m_pneumatics, 86.7, -64.11, -180), - //Move to 8th Ball - new GotoCoordinatesRobotRelative(m_drive, m_pneumatics, -6.34, 15.31, 90), - //Move to 9th Ball - new GotoCoordinatesRobotRelative(m_drive, m_pneumatics, 7.11, 24.41, 0), - //Move to 10th Ball - new GotoCoordinatesRobotRelative(m_drive, m_pneumatics, -6.34, 13.30), - //Shoot 5 more Balls (Total 10 Ball Autonomous Path) - new Wait(m_drive, 0, 2) - ); - } -} diff --git a/src/main/java/frc4388/robot/commands/auto/Barrel.java b/src/main/java/frc4388/robot/commands/auto/Barrel.java deleted file mode 100644 index 54908bf..0000000 --- a/src/main/java/frc4388/robot/commands/auto/Barrel.java +++ /dev/null @@ -1,28 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc4388.robot.commands.auto; - -import edu.wpi.first.wpilibj2.command.RamseteCommand; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import frc4388.robot.subsystems.Drive; - -// NOTE: Consider using this command inline, rather than writing a subclass. For more -// information, see: -// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html -public class Barrel extends SequentialCommandGroup { - /** - * Creates a new Barrel. - */ - public Barrel(Drive drive, RamseteCommand[] paths) { - // Add your commands in the super() call, e.g. - // super(new FooCommand(), new BarCommand()); - addCommands( - paths[0] - ); - } -} diff --git a/src/main/java/frc4388/robot/commands/auto/BarrelMany.java b/src/main/java/frc4388/robot/commands/auto/BarrelMany.java deleted file mode 100644 index 70fcf3c..0000000 --- a/src/main/java/frc4388/robot/commands/auto/BarrelMany.java +++ /dev/null @@ -1,27 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc4388.robot.commands.auto; - -import edu.wpi.first.wpilibj2.command.RamseteCommand; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import frc4388.robot.subsystems.Drive; - -// NOTE: Consider using this command inline, rather than writing a subclass. For more -// information, see: -// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html -public class BarrelMany extends SequentialCommandGroup { - /** - * Creates a new BarrelMany. - */ - public BarrelMany(Drive drive, RamseteCommand[] paths) { - // Add your commands in the super() call, e.g. - addCommands( - paths[0] - ); - } -} diff --git a/src/main/java/frc4388/robot/commands/auto/BarrelStart.java b/src/main/java/frc4388/robot/commands/auto/BarrelStart.java deleted file mode 100644 index 506309f..0000000 --- a/src/main/java/frc4388/robot/commands/auto/BarrelStart.java +++ /dev/null @@ -1,34 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc4388.robot.commands.auto; - -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.RamseteCommand; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc4388.robot.RobotContainer; -import frc4388.robot.subsystems.Drive; - -// NOTE: Consider using this command inline, rather than writing a subclass. For more -// information, see: -// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html -public class BarrelStart extends SequentialCommandGroup { - /** - * Creates a new BarrelStart. - */ - public BarrelStart(Drive drive, RamseteCommand[] paths) { - // Add your commands in the super() call, e.g. - // super(new FooCommand(), new BarCommand()); - addCommands( - paths[0], - //new Wait(drive, 0.01, 1), - new TankDriveVelocity(drive, 5, 5, 1.2) //my life be like oooooo aaaaaa ooooo aaaa - ); - - } -} diff --git a/src/main/java/frc4388/robot/commands/auto/Bounce.java b/src/main/java/frc4388/robot/commands/auto/Bounce.java deleted file mode 100644 index 8a11888..0000000 --- a/src/main/java/frc4388/robot/commands/auto/Bounce.java +++ /dev/null @@ -1,35 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc4388.robot.commands.auto; - -import edu.wpi.first.wpilibj.geometry.Pose2d; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.RamseteCommand; -import edu.wpi.first.wpilibj2.command.RunCommand; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import frc4388.robot.RobotContainer; -import frc4388.robot.subsystems.Drive; - -// NOTE: Consider using this command inline, rather than writing a subclass. For more -// information, see: -// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html -public class Bounce extends SequentialCommandGroup { - /** - * Creates a new Bounce. - */ - public Bounce(Drive drive, RobotContainer robotContainer, RamseteCommand[] paths) { - // Add your commands in the super() call, e.g. - // super(new FooCommand(), new BarCommand()); - addCommands( - paths[0], - paths[1], - paths[2], - paths[3] - ); - } -} diff --git a/src/main/java/frc4388/robot/commands/auto/DriveOffLineBackward.java b/src/main/java/frc4388/robot/commands/auto/DriveOffLineBackward.java deleted file mode 100644 index fed1212..0000000 --- a/src/main/java/frc4388/robot/commands/auto/DriveOffLineBackward.java +++ /dev/null @@ -1,31 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc4388.robot.commands.auto; - -import edu.wpi.first.wpilibj2.command.RamseteCommand; -import edu.wpi.first.wpilibj2.command.RunCommand; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import frc4388.robot.commands.drive.TurnDegrees; -import frc4388.robot.subsystems.Drive; - -// NOTE: Consider using this command inline, rather than writing a subclass. For more -// information, see: -// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html -public class DriveOffLineBackward extends SequentialCommandGroup { - /** - * Creates a new DriveOffLineBackward. - */ - public DriveOffLineBackward(Drive drive, RamseteCommand[] paths) { - // Add your commands in the super() call, e.g. - // super(new FooCommand(), new BarCommand()); - - addCommands( - new TankDriveVelocity(drive, -1, -1, 2) - ); - } -} diff --git a/src/main/java/frc4388/robot/commands/auto/DriveOffLineForward.java b/src/main/java/frc4388/robot/commands/auto/DriveOffLineForward.java deleted file mode 100644 index 05b2533..0000000 --- a/src/main/java/frc4388/robot/commands/auto/DriveOffLineForward.java +++ /dev/null @@ -1,34 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc4388.robot.commands.auto; - -import edu.wpi.first.wpilibj.geometry.Pose2d; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.RamseteCommand; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import frc4388.robot.RobotContainer; -import frc4388.robot.subsystems.Drive; - -// NOTE: Consider using this command inline, rather than writing a subclass. For more -// information, see: -// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html -public class DriveOffLineForward extends SequentialCommandGroup { - /** - * Creates a new DriveOffLineForward. - */ - public DriveOffLineForward(Drive drive, RobotContainer robotContainer, RamseteCommand[] paths) { - // Add your commands in the super() call, e.g. - // super(new FooCommand(), new BarCommand()); - - addCommands( - new InstantCommand(() -> drive.switchReversed(true)), - new InstantCommand(() -> robotContainer.resetOdometry(new Pose2d())), - paths[0] - ); - } -} diff --git a/src/main/java/frc4388/robot/commands/auto/EightBallAutoMiddle.java b/src/main/java/frc4388/robot/commands/auto/EightBallAutoMiddle.java deleted file mode 100644 index 1c0f863..0000000 --- a/src/main/java/frc4388/robot/commands/auto/EightBallAutoMiddle.java +++ /dev/null @@ -1,29 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc4388.robot.commands.auto; - -import edu.wpi.first.wpilibj2.command.RamseteCommand; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import frc4388.robot.subsystems.Drive; - -// NOTE: Consider using this command inline, rather than writing a subclass. For more -// information, see: -// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html -public class EightBallAutoMiddle extends SequentialCommandGroup { - /** - * Creates a new EightBallAutoMiddle. - */ - public EightBallAutoMiddle(Drive drive, RamseteCommand[] paths) { - // Add your commands in the super() call, e.g. - // super(new FooCommand(), new BarCommand()); - - addCommands( - paths[0] - ); - } -} diff --git a/src/main/java/frc4388/robot/commands/auto/ExecuteCommand.java b/src/main/java/frc4388/robot/commands/auto/ExecuteCommand.java deleted file mode 100644 index 03f8fa1..0000000 --- a/src/main/java/frc4388/robot/commands/auto/ExecuteCommand.java +++ /dev/null @@ -1,65 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc4388.robot.commands.auto; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import edu.wpi.first.wpilibj2.command.RamseteCommand; -import frc4388.robot.subsystems.LimeLight; - -public class ExecuteCommand extends CommandBase { - /** - * Creates a new ExecuteCommand. - */ - RamseteCommand[] m_paths; - LimeLight m_limeLight; - public ExecuteCommand(LimeLight limeLight, RamseteCommand[] paths) { - // Use addRequirements() here to declare subsystem dependencies. - m_limeLight = limeLight; - m_paths = paths; - } - - // 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() { - String gsPath = m_limeLight.galacticSearchPath; - switch (gsPath) - { - case "A_RED": - new RunPath(m_paths[0]); - break; - case "A_BLUE": - new RunPath(m_paths[1]); - break; - case "B_RED": - new RunPath(m_paths[2]); - break; - case "B_BLUE": - new RunPath(m_paths[3]); - break; - case "test": - new RunPath(m_paths[0]); - break; - } - } - - // 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 false; - } -} diff --git a/src/main/java/frc4388/robot/commands/auto/FiveBallAutoMiddle.java b/src/main/java/frc4388/robot/commands/auto/FiveBallAutoMiddle.java deleted file mode 100644 index 9a655de..0000000 --- a/src/main/java/frc4388/robot/commands/auto/FiveBallAutoMiddle.java +++ /dev/null @@ -1,28 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc4388.robot.commands.auto; - -import edu.wpi.first.wpilibj2.command.RamseteCommand; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import frc4388.robot.subsystems.Drive; - -// NOTE: Consider using this command inline, rather than writing a subclass. For more -// information, see: -// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html -public class FiveBallAutoMiddle extends SequentialCommandGroup { - /** - * Creates a new FiveBallAutoMiddle. - */ - public FiveBallAutoMiddle(Drive drive, RamseteCommand[] paths) { - // Use addRequirements() here to declare subsystem dependencies. - addCommands( - paths[0], - new TankDriveVelocity(drive, -3.2, -0.2, 0.8) - ); - } -} diff --git a/src/main/java/frc4388/robot/commands/auto/GalacticSearch.java b/src/main/java/frc4388/robot/commands/auto/GalacticSearch.java deleted file mode 100644 index 40df0f5..0000000 --- a/src/main/java/frc4388/robot/commands/auto/GalacticSearch.java +++ /dev/null @@ -1,51 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc4388.robot.commands.auto; - -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.RamseteCommand; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import frc4388.robot.commands.intake.RunIntake; -import frc4388.robot.subsystems.Intake; -import frc4388.robot.subsystems.LimeLight; - -// NOTE: Consider using this command inline, rather than writing a subclass. For more -// information, see: -// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html -public class GalacticSearch extends SequentialCommandGroup { - /** - * Creates a new GalacticSearch. - */ - public GalacticSearch(LimeLight m_limeLight, Intake m_intake, RamseteCommand[] paths) { - // addCommands( - // new IdentifyPath(m_limeLight), - // new ExecuteCommand(m_limeLight, paths) - // ); - if (m_limeLight.galacticSearchPath == "A_RED") - { - addCommands(new ParallelCommandGroup(paths[0], new RunIntake(m_intake))); - } - else if (m_limeLight.galacticSearchPath == "A_BLUE") - { - addCommands(new ParallelCommandGroup(paths[1], new RunIntake(m_intake))); - } - else if (m_limeLight.galacticSearchPath == "B_RED") - { - addCommands(new ParallelCommandGroup(paths[2], new RunIntake(m_intake))); - } - else if (m_limeLight.galacticSearchPath == "B_BLUE") - { - addCommands(new ParallelCommandGroup(paths[3], new RunIntake(m_intake))); - } - else if (m_limeLight.galacticSearchPath == "test") - { - addCommands(new ParallelCommandGroup(paths[0], new RunIntake(m_intake))); - } - } -} diff --git a/src/main/java/frc4388/robot/commands/auto/IdentifyPath.java b/src/main/java/frc4388/robot/commands/auto/IdentifyPath.java deleted file mode 100644 index f98b888..0000000 --- a/src/main/java/frc4388/robot/commands/auto/IdentifyPath.java +++ /dev/null @@ -1,163 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc4388.robot.commands.auto; - -import javax.lang.model.util.ElementScanner6; - -import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.CommandBase; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import frc4388.robot.Constants.VisionConstants; -import frc4388.robot.subsystems.LimeLight; - -public class IdentifyPath extends CommandBase { - - LimeLight m_limeLight; - double xAngle; - double yAngle; - double target; - public String path; - boolean closeVisible; - boolean finished; - boolean pathFound; - - public IdentifyPath(LimeLight limeLight) { - m_limeLight = limeLight; - addRequirements(m_limeLight); - m_limeLight.limeOff(); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - path = ""; - closeVisible = false; - pathFound = false; - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - target = m_limeLight.getV(); - xAngle = m_limeLight.getX(); - yAngle = m_limeLight.getY(); - //m_limeLight.limeOn(); - // //Identify which of four paths - // m_limeLight.changePipeline(1);//Dual Targetting Lowest - // if (withinError(yAngle, VisionConstants.bothCloseVisibleY) && !closeVisible) //BLUE PATHS - // { - // closeVisible = true; - // } - // else if (!withinError(yAngle, VisionConstants.bothCloseVisibleY) && !closeVisible) // RED PATHS - // { - // closeVisible = false; - // } - - // if (closeVisible) - // { - // m_limeLight.changePipeline(2); //Dual Targetting Highest - // if(withinError(xAngle, VisionConstants.farLeftVisibleX)) //A PATH - // { - // path = "A_BLUE"; - // } - // if(withinError(xAngle, VisionConstants.farRightVisibleX)) //B PATH - // { - // path = "B_BLUE"; - // } - // } - - // else{ - // m_limeLight.changePipeline(1); //Dual Targetting Lowest - // if(withinError(yAngle, VisionConstants.closeLeftVisibleY)) //A PATH - // { - // path = "A_RED"; - // } - // else if(withinError(yAngle, VisionConstants.closeRightVisibleY)) //B PATH - // { - // path = "B_RED"; - // } - // } - - SmartDashboard.putBoolean("PathFound", pathFound); - // System.out.println("If you see this message a bunch of times in a row, IdentifyPath.java is stuck trying to find the path for GalacticSearch"); - // System.out.println(path); - - - m_limeLight.changePipeline(1); - if (withinCoords(VisionConstants.aBlue)) - { - pathFound = true; - path = "A_BLUE"; - } - - m_limeLight.changePipeline(2); - if (withinCoords(VisionConstants.bBlue)) - { - pathFound = true; - path = "B_BLUE"; - } - - m_limeLight.changePipeline(1); - if (withinCoords(VisionConstants.aRed)) - { - pathFound = true; - path = "A_RED"; - } - - m_limeLight.changePipeline(1); - if (withinCoords(VisionConstants.bRed)) - { - pathFound = true; - path = "B_RED"; - } - - path = "test"; - } - - public boolean withinError(double angle, double goal) - { - if(goal > (angle - VisionConstants.searchError) && goal < (angle + VisionConstants.searchError)) - { - return true; - } - else - { - return false; - } - } - - public boolean withinCoords(double[] coords) - { - if (withinError(xAngle, coords[0]) && withinError(yAngle, coords[1])) - { - return true; - } - else - { - return false; - } - } - // 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() { - if (path != "") - { - SmartDashboard.putString("GalacticSearchPath", path); - m_limeLight.galacticSearchPath = path; - //m_limeLight.limeOff(); - return true; - } - return false; - } -} diff --git a/src/main/java/frc4388/robot/commands/auto/RunPath.java b/src/main/java/frc4388/robot/commands/auto/RunPath.java deleted file mode 100644 index fd7523e..0000000 --- a/src/main/java/frc4388/robot/commands/auto/RunPath.java +++ /dev/null @@ -1,27 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc4388.robot.commands.auto; - -import edu.wpi.first.wpilibj2.command.RamseteCommand; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; - -// NOTE: Consider using this command inline, rather than writing a subclass. For more -// information, see: -// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html -public class RunPath extends SequentialCommandGroup { - /** - * Creates a new RunPath. - */ - public RunPath(RamseteCommand path) { - // Add your commands in the super() call, e.g. - // super(new FooCommand(), new BarCommand()); - addCommands( - path - ); - } -} diff --git a/src/main/java/frc4388/robot/commands/auto/SequentialTest.java b/src/main/java/frc4388/robot/commands/auto/SequentialTest.java deleted file mode 100644 index 1fdddb2..0000000 --- a/src/main/java/frc4388/robot/commands/auto/SequentialTest.java +++ /dev/null @@ -1,33 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc4388.robot.commands.auto; - -import edu.wpi.first.wpilibj.geometry.Pose2d; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.RamseteCommand; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import frc4388.robot.RobotContainer; -import frc4388.robot.subsystems.Drive; - -// NOTE: Consider using this command inline, rather than writing a subclass. For more -// information, see: -// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html -public class SequentialTest extends SequentialCommandGroup { - /** - * Creates a new SequentialTest. - */ - public SequentialTest(RobotContainer m_robotContainer, RamseteCommand[] paths) { - // Add your commands in the super() call, e.g. - // super(new FooCommand(), new BarCommand()); - addCommands( - paths[0], - new InstantCommand(() -> m_robotContainer.resetOdometry(new Pose2d())), - paths[1] - ); - } -} diff --git a/src/main/java/frc4388/robot/commands/auto/SixBallAutoMiddle.java b/src/main/java/frc4388/robot/commands/auto/SixBallAutoMiddle.java deleted file mode 100644 index 9900c65..0000000 --- a/src/main/java/frc4388/robot/commands/auto/SixBallAutoMiddle.java +++ /dev/null @@ -1,35 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc4388.robot.commands.auto; - -import java.nio.file.Path; - -import edu.wpi.first.wpilibj.Filesystem; -import edu.wpi.first.wpilibj.trajectory.Trajectory; -import edu.wpi.first.wpilibj.trajectory.TrajectoryUtil; -import edu.wpi.first.wpilibj2.command.RamseteCommand; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import frc4388.robot.RobotContainer; -import frc4388.robot.subsystems.Drive; - -// NOTE: Consider using this command inline, rather than writing a subclass. For more -// information, see: -// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html -public class SixBallAutoMiddle extends SequentialCommandGroup { - /** - * Creates a new SixBallAutoMiddle. - */ - public SixBallAutoMiddle(Drive drive, RamseteCommand[] paths) { - // Add your commands in the super() call, e.g. - // super(new FooCommand(), new BarCommand()); - - addCommands( - paths[0] - ); - } -} diff --git a/src/main/java/frc4388/robot/commands/auto/Slalom.java b/src/main/java/frc4388/robot/commands/auto/Slalom.java deleted file mode 100644 index 40159d1..0000000 --- a/src/main/java/frc4388/robot/commands/auto/Slalom.java +++ /dev/null @@ -1,28 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc4388.robot.commands.auto; - -import edu.wpi.first.wpilibj2.command.RamseteCommand; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import frc4388.robot.subsystems.Drive; - -// NOTE: Consider using this command inline, rather than writing a subclass. For more -// information, see: -// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html -public class Slalom extends SequentialCommandGroup { - /** - * Creates a new Slalom. - */ - public Slalom(Drive drive, RamseteCommand[] paths) { - // Add your commands in the super() call, e.g. - // super(new FooCommand(), new BarCommand()); - addCommands( - paths[0] - ); - } -} diff --git a/src/main/java/frc4388/robot/commands/auto/TankDriveVelocity.java b/src/main/java/frc4388/robot/commands/auto/TankDriveVelocity.java deleted file mode 100644 index 945a77f..0000000 --- a/src/main/java/frc4388/robot/commands/auto/TankDriveVelocity.java +++ /dev/null @@ -1,67 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc4388.robot.commands.auto; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc4388.robot.subsystems.Drive; - -public class TankDriveVelocity extends CommandBase { - Drive m_drive; - double m_leftTargetVel; - double m_rightTargetVel; - - long m_targetTime; - long m_firstTime; - long m_currentTime; - long m_diffTime; - - /** - * Creates a new TankDriveVelocity. - */ - public TankDriveVelocity(Drive subsystem, double leftTargetVel, double rightTargetVel, double targetTime) { - // Use addRequirements() here to declare subsystem dependencies. - m_drive = subsystem; - m_leftTargetVel = leftTargetVel; - m_rightTargetVel = rightTargetVel; - m_targetTime = (long) (targetTime * 1000); - addRequirements(subsystem); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - m_firstTime = System.currentTimeMillis(); - m_diffTime = 0; - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - m_currentTime = System.currentTimeMillis(); - m_diffTime = m_currentTime - m_firstTime; - - if (m_diffTime < m_targetTime) { - m_drive.tankDriveVelocity(m_leftTargetVel, m_rightTargetVel); - } - - } - - // 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() { - if (m_diffTime >= m_targetTime) { - return true; - } - return false; - } -} diff --git a/src/main/java/frc4388/robot/commands/auto/TenBallAutoMiddle.java b/src/main/java/frc4388/robot/commands/auto/TenBallAutoMiddle.java deleted file mode 100644 index 28ae59c..0000000 --- a/src/main/java/frc4388/robot/commands/auto/TenBallAutoMiddle.java +++ /dev/null @@ -1,59 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc4388.robot.commands.auto; - -import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; -import edu.wpi.first.wpilibj2.command.RamseteCommand; -import edu.wpi.first.wpilibj2.command.RunCommand; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import frc4388.robot.Constants.IntakeConstants; -import frc4388.robot.commands.shooter.CalibrateShooter; -import frc4388.robot.commands.shooter.PrepChecker; -import frc4388.robot.commands.shooter.ShootPrepGroup; -import frc4388.robot.commands.storage.RunStorage; -import frc4388.robot.subsystems.Drive; -import frc4388.robot.subsystems.Intake; -import frc4388.robot.subsystems.Shooter; -import frc4388.robot.subsystems.ShooterAim; -import frc4388.robot.subsystems.ShooterHood; -import frc4388.robot.subsystems.Storage; - -// NOTE: Consider using this command inline, rather than writing a subclass. For more -// information, see: -// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html -public class TenBallAutoMiddle extends SequentialCommandGroup { - /** - * Creates a new TenBallAutoMiddle. - */ - public TenBallAutoMiddle(ShooterHood shooterHood, Storage storage, Intake intake, Shooter shooter, ShooterAim shooterAim, Drive drive, RamseteCommand[] paths) { - // Add your commands in the super() call, e.g. - // super(new FooCommand(), new BarCommand()); - addCommands( - new ParallelDeadlineGroup( - new Wait(drive, 0.1, 0), - new CalibrateShooter(shooter, shooterAim, shooterHood) - ), - new ParallelDeadlineGroup( - new Wait(drive, 1, 0), - new RunCommand(() -> shooterAim.runShooterWithInput(-0.75), shooterAim) - ), - new ParallelDeadlineGroup( - new Wait(drive, 4, 0), - new PrepChecker(shooter, shooterAim), - new RunCommand(() -> intake.runExtender(IntakeConstants.EXTENDER_SPEED), intake), - new ShootPrepGroup(shooter, shooterAim, shooterHood, storage) - ), - new ParallelDeadlineGroup( - new ShootPrepGroup(shooter, shooterAim, shooterHood, storage), - new RunStorage(storage) - ) - //paths[0], - //paths[1] - ); - } -} diff --git a/src/main/java/frc4388/robot/commands/auto/Wait.java b/src/main/java/frc4388/robot/commands/auto/Wait.java deleted file mode 100644 index 6bbedf3..0000000 --- a/src/main/java/frc4388/robot/commands/auto/Wait.java +++ /dev/null @@ -1,71 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc4388.robot.commands.auto; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import edu.wpi.first.wpilibj2.command.SubsystemBase; - -public class Wait extends CommandBase { - - long m_startTime; - long m_waitTime; - long m_currentTime; - SubsystemBase m_subsystem; - int m_waitNum; - - int counter = 0; - - /** - * Creates a new WaitCommand. - */ - public Wait(SubsystemBase subsystem, double seconds, int waitNum) { - // Use addRequirements() here to declare subsystem dependencies. - - m_waitTime = (long) (seconds * 1000); - m_subsystem = subsystem; - m_waitNum = waitNum; - - addRequirements(m_subsystem); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - m_currentTime = System.currentTimeMillis(); - m_startTime = m_currentTime; - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - - if (counter == 0) { - //SmartDashboard.putNumber("Wait Coordinates" + m_waitNum, m_currentTime); - } - - m_currentTime = System.currentTimeMillis(); - //SmartDashboard.putNumber("Time Difference for Wait", (m_currentTime - m_startTime)); - - counter ++; - } - - // 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() { - if ((m_currentTime - m_startTime) >= m_waitTime) { - return true; - } else { - return false; - } - } -} diff --git a/src/main/java/frc4388/robot/commands/climber/DisengageRatchet.java b/src/main/java/frc4388/robot/commands/climber/DisengageRatchet.java deleted file mode 100644 index 89296b4..0000000 --- a/src/main/java/frc4388/robot/commands/climber/DisengageRatchet.java +++ /dev/null @@ -1,48 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc4388.robot.commands.climber; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc4388.robot.subsystems.Climber; - -public class DisengageRatchet extends CommandBase { - Climber m_climber; - - /** - * Creates a new DisengageRatchet command. - */ - public DisengageRatchet(Climber subsystem) { - // Use addRequirements() here to declare subsystem dependencies. - m_climber = subsystem; - } - - // 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() { - if (m_climber.m_climberSafety) { - m_climber.shiftServo(false); - System.err.println("Disengage Rachet"); - } - } - - // 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 true; - } -} diff --git a/src/main/java/frc4388/robot/commands/climber/RunClimberWithTriggers.java b/src/main/java/frc4388/robot/commands/climber/RunClimberWithTriggers.java deleted file mode 100644 index de3acd9..0000000 --- a/src/main/java/frc4388/robot/commands/climber/RunClimberWithTriggers.java +++ /dev/null @@ -1,65 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc4388.robot.commands.climber; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc4388.robot.subsystems.Climber; -import frc4388.utility.controller.IHandController; - -public class RunClimberWithTriggers extends CommandBase { - private Climber m_climber; - private IHandController m_controller; - - /** - * Uses input from opperator triggers to control climber motor - * @param subsystem the climber subsystem - * @param controller the driver controller - */ - public RunClimberWithTriggers(Climber subsystem, IHandController controller) { - m_climber = subsystem; - m_controller = controller; - addRequirements(m_climber); - } - - // 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() { - double rightTrigger = m_controller.getRightTriggerAxis(); - double leftTrigger = m_controller.getLeftTriggerAxis(); - double output = 0; - if (rightTrigger < .5) { - if(rightTrigger > leftTrigger) { - output = rightTrigger; - } - if (leftTrigger > rightTrigger) { - output = -leftTrigger; - m_climber.shiftServo(true); - System.err.println("Engage Rachet"); - } - } else { - output = rightTrigger; - } - m_climber.runClimber(output); - } - - // 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 false; - } -} diff --git a/src/main/java/frc4388/robot/commands/climber/RunLevelerWithJoystick.java b/src/main/java/frc4388/robot/commands/climber/RunLevelerWithJoystick.java deleted file mode 100644 index 2052ef4..0000000 --- a/src/main/java/frc4388/robot/commands/climber/RunLevelerWithJoystick.java +++ /dev/null @@ -1,53 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc4388.robot.commands.climber; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc4388.robot.subsystems.Leveler; -import frc4388.utility.controller.IHandController; - -public class RunLevelerWithJoystick extends CommandBase { - private Leveler m_leveler; - private IHandController m_controller; - - /** - * Creates a new RunLevelerWithJoystick to control the leveler with an Xbox controller. - * @param subsystem pass the Drive subsystem from {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer} - * @param controller pass the Driver {@link frc4388.utility.controller.IHandController#getClass() IHandController} using the - * {@link frc4388.robot.RobotContainer#getDriverJoystick() getDriverJoystick()} method in - * {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer} - */ - public RunLevelerWithJoystick(Leveler subsystem, IHandController controller) { - m_leveler = subsystem; - m_controller = controller; - addRequirements(m_leveler); - } - - // 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() { - double input = m_controller.getRightXAxis(); - m_leveler.runLeveler(input); - } - - // 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 false; - } -} diff --git a/src/main/java/frc4388/robot/commands/drive/GotoCoordinatesRobotRelative.java b/src/main/java/frc4388/robot/commands/drive/GotoCoordinatesRobotRelative.java deleted file mode 100644 index df68e4d..0000000 --- a/src/main/java/frc4388/robot/commands/drive/GotoCoordinatesRobotRelative.java +++ /dev/null @@ -1,87 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc4388.robot.commands.drive; - -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import frc4388.robot.commands.auto.Wait; -import frc4388.robot.subsystems.Drive; -import frc4388.robot.subsystems.Pneumatics; - -// NOTE: Consider using this command inline, rather than writing a subclass. For more -// information, see: -// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html -public class GotoCoordinatesRobotRelative extends SequentialCommandGroup { - Drive m_drive; - Pneumatics m_pneumatics; - - double m_xTarget; - double m_yTarget; - double m_currentAngle; - double m_hypotDist; - double m_endAngle; - - /** - * Creates a new GotoPosition. - */ - public GotoCoordinatesRobotRelative(Drive subsystem, Pneumatics subsystem2, double xTarget, double yTarget, double endAngle) { - // Add your commands in the super() call, e.g. - // super(new FooCommand(), new BarCommand()); - m_drive = subsystem; - m_pneumatics = subsystem2; - - m_xTarget = xTarget; - m_yTarget = yTarget; - - m_hypotDist = Math.sqrt((m_xTarget*m_xTarget) + (m_yTarget*m_yTarget)); - - m_currentAngle = calcAngle(); - - m_endAngle = endAngle; - - addCommands( new TurnDegrees(m_drive, m_currentAngle), - new Wait(m_drive, 0, 0), - new DriveStraightToPositionPID(m_drive, m_pneumatics, m_hypotDist), - new TurnDegrees(m_drive, (m_endAngle - m_currentAngle) % 360)); - } - - public GotoCoordinatesRobotRelative(Drive subsystem, Pneumatics subsystem2, double xTarget, double yTarget) { - m_drive = subsystem; - m_pneumatics = subsystem2; - - m_xTarget = xTarget; - m_yTarget = yTarget; - - m_hypotDist = Math.sqrt((m_xTarget*m_xTarget) + (m_yTarget*m_yTarget)); - - m_currentAngle = calcAngle(); - - m_endAngle = m_currentAngle; - - addCommands( new TurnDegrees(m_drive, m_currentAngle), - new Wait(m_drive, 0, 0), - new DriveStraightToPositionPID(m_drive, m_pneumatics, m_hypotDist), - new TurnDegrees(m_drive, m_endAngle - m_currentAngle)); - } - - public boolean isQuadrantThree() { - if ((m_xTarget < 0) && (m_yTarget < 0)) { - return true; - } else { - return false; - } - } - - public double calcAngle() { - if (isQuadrantThree()) { - return 360 + (Math.atan2(m_yTarget, m_xTarget) * (180 / Math.PI)) - 90; - } else { - return (Math.atan2(m_yTarget, m_xTarget) * (180 / Math.PI)) - 90; - } - } - -} diff --git a/src/main/java/frc4388/robot/commands/drive/PlaySongDrive.java b/src/main/java/frc4388/robot/commands/drive/PlaySongDrive.java index 10a99ee..eaa0f07 100644 --- a/src/main/java/frc4388/robot/commands/drive/PlaySongDrive.java +++ b/src/main/java/frc4388/robot/commands/drive/PlaySongDrive.java @@ -9,7 +9,6 @@ package frc4388.robot.commands.drive; import edu.wpi.first.wpilibj2.command.CommandBase; import frc4388.robot.subsystems.Drive; -import frc4388.robot.subsystems.Shooter; public class PlaySongDrive extends CommandBase { private Drive m_drive; @@ -17,10 +16,10 @@ public class PlaySongDrive extends CommandBase { /** * Creates a new PlaySongDrive. */ - public PlaySongDrive(Drive subsystem, Shooter shooter) { + public PlaySongDrive(Drive subsystem) { // Use addRequirements() here to declare subsystem dependencies. m_drive = subsystem; - addRequirements(m_drive, shooter); + addRequirements(m_drive); } // Called when the command is initially scheduled. diff --git a/src/main/java/frc4388/robot/commands/drive/SetShooterToOdo.java b/src/main/java/frc4388/robot/commands/drive/SetShooterToOdo.java deleted file mode 100644 index 8404441..0000000 --- a/src/main/java/frc4388/robot/commands/drive/SetShooterToOdo.java +++ /dev/null @@ -1,36 +0,0 @@ -package frc4388.robot.commands.drive; - -import org.opencv.core.Mat; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc4388.robot.subsystems.Drive; -import frc4388.robot.subsystems.ShooterAim; - -public class SetShooterToOdo extends CommandBase { - private ShooterAim m_shooterAim; - private Drive m_driveTrain; - - private double targetAngle; - - public SetShooterToOdo(ShooterAim shooterAim, Drive driveTrain) { - m_shooterAim = shooterAim; - m_driveTrain = driveTrain; - } - - @Override - public void initialize() { - double xPos = m_driveTrain.getPose().getX(); - double yPos = m_driveTrain.getPose().getY(); - targetAngle = Math.atan(yPos / xPos) - m_driveTrain.getPose().getRotation().getDegrees(); - } - - @Override - public void execute() { - m_shooterAim.runshooterRotatePID(targetAngle); - } - - @Override - public boolean isFinished() { - return Math.abs(m_shooterAim.getShooterRotatePosition() - targetAngle) < 5; - } -} diff --git a/src/main/java/frc4388/robot/commands/drive/SkipSong.java b/src/main/java/frc4388/robot/commands/drive/SkipSong.java deleted file mode 100644 index 0d020a1..0000000 --- a/src/main/java/frc4388/robot/commands/drive/SkipSong.java +++ /dev/null @@ -1,56 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc4388.robot.commands.drive; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc4388.robot.subsystems.Drive; - -public class SkipSong extends CommandBase { - Drive m_drive; - int m_index; - - /** - * Creates a new SkipSong. - */ - public SkipSong(Drive m_robotDrive, int index) { - // Use addRequirements() here to declare subsystem dependencies. - m_drive = m_robotDrive; - m_index = index; - } - - // 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() { - String[] songs = m_drive.songsStrings; - String song = m_drive.m_currentSong; - - for (int i = 0; i < songs.length; i++) { - if (songs[i] == song) { - m_drive.selectSong(songs[i + m_index]); - break; - } - } - } - - // 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 true; - } -} diff --git a/src/main/java/frc4388/robot/commands/drive/TurnDegrees.java b/src/main/java/frc4388/robot/commands/drive/TurnDegrees.java deleted file mode 100644 index 8b5d199..0000000 --- a/src/main/java/frc4388/robot/commands/drive/TurnDegrees.java +++ /dev/null @@ -1,74 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc4388.robot.commands.drive; - -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc4388.robot.Constants.DriveConstants; -import frc4388.robot.subsystems.Drive; - -public class TurnDegrees extends CommandBase { - - double m_targetAngle; - Drive m_drive; - double m_currentYawInTicks; - double m_targetAngleTicksIn; - double m_targetAngleTicksOut; - int i; - - /** - * Creates a new TurnDeg. - */ - public TurnDegrees(Drive subsystem, double targetAngle) { - // Use addRequirements() here to declare subsystem dependencies. - - m_targetAngle = targetAngle; - m_drive = subsystem; - - addRequirements(m_drive); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - m_targetAngleTicksIn = (m_targetAngle / 360) * DriveConstants.TICKS_PER_GYRO_REV; - m_currentYawInTicks = (m_drive.getGyroYaw() / 360) * DriveConstants.TICKS_PER_GYRO_REV; - SmartDashboard.putNumber("Current Yaw Ticks", m_currentYawInTicks); - m_targetAngleTicksOut = m_targetAngleTicksIn + m_currentYawInTicks; - - i = 0; - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - m_currentYawInTicks = (m_drive.getGyroYaw() / 360) * DriveConstants.TICKS_PER_GYRO_REV; - - m_drive.runTurningPID(m_targetAngleTicksIn); - - SmartDashboard.putNumber("Turning Error", Math.abs(m_currentYawInTicks - m_targetAngleTicksOut)); - SmartDashboard.putNumber("Turning Target", m_targetAngleTicksOut); - - i++; - } - - // 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() { - if ((Math.abs(m_drive.getTurnRate()) < 1) && (Math.abs(m_currentYawInTicks - m_targetAngleTicksOut) < 70)) { - return true; - } - return false; - } -} - diff --git a/src/main/java/frc4388/robot/commands/drive/VisionUpdateOdometry.java b/src/main/java/frc4388/robot/commands/drive/VisionUpdateOdometry.java index 82dbd83..cee917a 100644 --- a/src/main/java/frc4388/robot/commands/drive/VisionUpdateOdometry.java +++ b/src/main/java/frc4388/robot/commands/drive/VisionUpdateOdometry.java @@ -13,6 +13,8 @@ import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj.geometry.Pose2d; import edu.wpi.first.wpilibj.geometry.Rotation2d; import edu.wpi.first.wpilibj.geometry.Translation2d; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; import edu.wpi.first.wpiutil.math.Nat; import edu.wpi.first.wpiutil.math.Num; @@ -22,13 +24,12 @@ import edu.wpi.first.wpiutil.math.numbers.N6; import frc4388.robot.Constants.VOPConstants; import frc4388.robot.Constants.VisionConstants; import frc4388.robot.subsystems.Drive; -import frc4388.robot.subsystems.LimeLight; -import frc4388.robot.subsystems.ShooterAim; +// import frc4388.robot.subsystems.ShooterAim_1; import frc4388.robot.subsystems.Vision; public class VisionUpdateOdometry extends CommandBase { private Vision m_limeLight; - private ShooterAim m_shooterAim; + // private ShooterAim_1 m_shooterAim; private Drive m_driveTrain; private double xPos; @@ -43,11 +44,11 @@ public class VisionUpdateOdometry extends CommandBase { * @param shooterAim replace with Turret subsystem for integration with 2022 * @param driveTrain replace with Swerve subsystem for integration with 2022 */ - public VisionUpdateOdometry(Vision limeLight, ShooterAim shooterAim, Drive driveTrain) { + public VisionUpdateOdometry(Vision limeLight, Drive driveTrain) { m_limeLight = limeLight; - m_shooterAim = shooterAim; + // m_shooterAim = shooterAim; m_driveTrain = driveTrain; - addRequirements(m_limeLight, m_shooterAim, m_driveTrain); + addRequirements(m_limeLight, m_driveTrain); // // Turn camera on but leave LEDs off // NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(1); @@ -86,10 +87,11 @@ public class VisionUpdateOdometry extends CommandBase { double distance = 1.d / Math.tan(viewAngle); distance *= VOPConstants.TARGET_HEIGHT; // replace with VisionConstants for 2022 + System.out.println("Never gonna give you up: " + distance); double[] ypr = new double[3]; Drive.m_pigeon.getYawPitchRoll(ypr); // Replace static reference to pigeon with SwerveDrive object for 2022 - double relativeAngle = Math.toDegrees(m_shooterAim.getShooterAngleDegrees() - ypr[0]); + double relativeAngle = Math.toDegrees(/*m_shooterAim.getShooterAngleDegrees()*/ - ypr[0]); rotation = new Rotation2d(ypr[0]); xPos = Math.cos(relativeAngle) * distance; @@ -98,6 +100,9 @@ public class VisionUpdateOdometry extends CommandBase { Pose2d pose = new Pose2d(translate, rotation); m_driveTrain.setOdometry(pose); // Replace with adding new pose to Kalman filter + SmartDashboard.putNumber("x:", pose.getX()); + SmartDashboard.putNumber("y:", pose.getY()); + m_limeLight.setLEDs(false); } // http://www.lee-mac.com/5pointellipse.html diff --git a/src/main/java/frc4388/robot/commands/intake/RunExtenderOutIn.java b/src/main/java/frc4388/robot/commands/intake/RunExtenderOutIn.java deleted file mode 100644 index c222ad6..0000000 --- a/src/main/java/frc4388/robot/commands/intake/RunExtenderOutIn.java +++ /dev/null @@ -1,77 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc4388.robot.commands.intake; - -import com.revrobotics.CANDigitalInput; -import com.revrobotics.CANSparkMax; -import com.revrobotics.CANSparkMaxLowLevel.MotorType; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc4388.robot.Constants.IntakeConstants; -import frc4388.robot.subsystems.Intake; - -public class RunExtenderOutIn extends CommandBase { - private Intake m_intake; - - CANSparkMax m_intakeMotor = new CANSparkMax(IntakeConstants.INTAKE_SPARK_ID, MotorType.kBrushless); - CANSparkMax m_extenderMotor = new CANSparkMax(IntakeConstants.EXTENDER_SPARK_ID, MotorType.kBrushless); - CANDigitalInput m_extenderForwardLimit; - CANDigitalInput m_extenderReverseLimit; - - - - /** - * Uses input from opperator to run the extender motor. - * The left bumper will run the extender in and out. - * @param subsystem pass the Intake subsystem from {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer} - */ - public RunExtenderOutIn(Intake subsystem) { - // Use addRequirements() here to declare subsystem dependencies. - m_intake = subsystem; - addRequirements(m_intake); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - m_intake.isExtended = !m_intake.isExtended; - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - if (m_intake.isExtended){ - m_intake.runExtender(0.3); - } else { - m_intake.runExtender(-0.3); - } - - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - m_intake.runExtender(0.0); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - if (m_intake.isExtended && m_extenderForwardLimit.get() == true){ - return true; - } - - else if(m_intake.isExtended && m_extenderReverseLimit.get() == true){ - return true; - } - - else{ - return false; - } - } -} diff --git a/src/main/java/frc4388/robot/commands/intake/RunIntake.java b/src/main/java/frc4388/robot/commands/intake/RunIntake.java deleted file mode 100644 index c88b1f7..0000000 --- a/src/main/java/frc4388/robot/commands/intake/RunIntake.java +++ /dev/null @@ -1,46 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc4388.robot.commands.intake; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc4388.robot.Constants.IntakeConstants; -import frc4388.robot.subsystems.Intake; - -public class RunIntake extends CommandBase { - Intake m_intake; - /** - * Creates a new RunIntake. - */ - public RunIntake(Intake subsystem) { - // Use addRequirements() here to declare subsystem dependencies. - m_intake = subsystem; - addRequirements(m_intake); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - m_intake.runIntake(IntakeConstants.INTAKE_SPEED); - } - - // 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 false; - } -} diff --git a/src/main/java/frc4388/robot/commands/intake/RunIntakeWithTriggers.java b/src/main/java/frc4388/robot/commands/intake/RunIntakeWithTriggers.java deleted file mode 100644 index 7f90f63..0000000 --- a/src/main/java/frc4388/robot/commands/intake/RunIntakeWithTriggers.java +++ /dev/null @@ -1,63 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc4388.robot.commands.intake; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc4388.robot.subsystems.Intake; -import frc4388.utility.controller.IHandController; - -public class RunIntakeWithTriggers extends CommandBase { - private Intake m_intake; - private IHandController m_controller; - - /** - * Uses input from opperator triggers to control intake motor. - * The right trigger will run the intake in and the left trigger will run it out. - * @param subsystem pass the Intake subsystem from {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer} - * @param controller pass the Operator {@link frc4388.utility.controller.IHandController#getClass() IHandController} using the - * {@link frc4388.robot.RobotContainer#getOperatorJoystick() getOperatorJoystick()} method in - * {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer} - */ - public RunIntakeWithTriggers(Intake subsystem, IHandController controller) { - // Use addRequirements() here to declare subsystem dependencies. - m_intake = subsystem; - m_controller = controller; - addRequirements(m_intake); - } - - // 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() { - double rightTrigger = m_controller.getRightTriggerAxis(); - double leftTrigger = m_controller.getLeftTriggerAxis(); - double output = 0; - if(leftTrigger >= rightTrigger) { - output = leftTrigger; - } - if (rightTrigger > leftTrigger) { - output = -rightTrigger; - } - m_intake.runIntake(output); - } - - // 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 false; - } -} diff --git a/src/main/java/frc4388/robot/commands/shooter/CalibrateShooter.java b/src/main/java/frc4388/robot/commands/shooter/CalibrateShooter.java deleted file mode 100644 index 14e19ce..0000000 --- a/src/main/java/frc4388/robot/commands/shooter/CalibrateShooter.java +++ /dev/null @@ -1,69 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc4388.robot.commands.shooter; - -import com.revrobotics.CANDigitalInput.LimitSwitchPolarity; -import com.revrobotics.CANSparkMax.SoftLimitDirection; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc4388.robot.Constants.ShooterConstants; -import frc4388.robot.subsystems.Shooter; -import frc4388.robot.subsystems.ShooterAim; -import frc4388.robot.subsystems.ShooterHood; - -public class CalibrateShooter extends CommandBase { - Shooter m_shooter; - ShooterAim m_shooterAim; - ShooterHood m_shooterHood; - - /** - * Calibrates the turret by moving the hood all the way down and moving the turret all the way right, then reseting the encoders - * @param shootSub The Shooter subsystem - * @param aimSub The ShooterAim subsystem - */ - public CalibrateShooter(Shooter shootSub, ShooterAim aimSub, ShooterHood hoodSub) { - m_shooter = shootSub; - m_shooterAim = aimSub; - m_shooterHood = hoodSub; - addRequirements(m_shooter, m_shooterHood, m_shooterAim); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - m_shooterHood.m_angleEncoder.setPosition(0); - m_shooterHood.m_angleAdjustMotor.set(-ShooterConstants.HOOD_CALIBRATE_SPEED); - - m_shooterAim.m_shooterRotateMotor.enableSoftLimit(SoftLimitDirection.kForward, false); - m_shooterAim.m_shooterRotateMotor.enableSoftLimit(SoftLimitDirection.kReverse, false); - m_shooterAim.m_shooterRotateEncoder.setPosition(0); - m_shooterAim.m_shooterRotateMotor.set(ShooterConstants.TURRET_CALIBRATE_SPEED); - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - m_shooterAim.m_shooterRotateMotor.enableSoftLimit(SoftLimitDirection.kForward, true); - m_shooterAim.m_shooterRotateMotor.enableSoftLimit(SoftLimitDirection.kReverse, true); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - if (m_shooterAim.m_shooterRotateMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyOpen).get() && - m_shooterHood.m_hoodDownLimit.get()) { - return true; - } - return false; - } -} diff --git a/src/main/java/frc4388/robot/commands/shooter/HoodPositionPID.java b/src/main/java/frc4388/robot/commands/shooter/HoodPositionPID.java deleted file mode 100644 index 2f5a14a..0000000 --- a/src/main/java/frc4388/robot/commands/shooter/HoodPositionPID.java +++ /dev/null @@ -1,59 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc4388.robot.commands.shooter; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc4388.robot.Constants.ShooterConstants; -import frc4388.robot.subsystems.ShooterHood; - -public class HoodPositionPID extends CommandBase { - ShooterHood m_shooterHood; - double firingAngle; - - /** - * Creates a new HoodPositionPID. - */ - public HoodPositionPID(ShooterHood subSystem) { - m_shooterHood = subSystem; - addRequirements(m_shooterHood); - } - - // 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() { - //double slope = ShooterConstants.HOOD_CONVERT_SLOPE; - //double b = ShooterConstants.HOOD_CONVERT_B; - //firingAngle = (-slope*m_shooterHood.addFireAngle())+b; - firingAngle = m_shooterHood.addFireAngle(); - //SmartDashboard.putNumber("Shoot Angle From Equation", m_shooter.addFireAngle()); - //SmartDashboard.putNumber("Fire Angle", firingAngle); - m_shooterHood.runAngleAdjustPID(firingAngle); - } - - // 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() { - double encoderPos = m_shooterHood.m_angleAdjustMotor.getEncoder().getPosition(); - if(encoderPos < firingAngle + 1 || encoderPos < firingAngle - 1){ - m_shooterHood.m_isHoodReady = true; - } else { - m_shooterHood.m_isHoodReady = false; - } - return false; - } -} diff --git a/src/main/java/frc4388/robot/commands/shooter/PrepChecker.java b/src/main/java/frc4388/robot/commands/shooter/PrepChecker.java deleted file mode 100644 index cc978fd..0000000 --- a/src/main/java/frc4388/robot/commands/shooter/PrepChecker.java +++ /dev/null @@ -1,52 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc4388.robot.commands.shooter; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc4388.robot.subsystems.Shooter; -import frc4388.robot.subsystems.ShooterAim; - -public class PrepChecker extends CommandBase { - Shooter m_shooter; - ShooterAim m_shooterAim; - - /** - * Creates a new PrepChecker. - */ - public PrepChecker(Shooter shooter, ShooterAim shooterAim) { - // Use addRequirements() here to declare subsystem dependencies. - m_shooter = shooter; - m_shooterAim = shooterAim; - } - - // 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() { - - } - - // 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() { - if (m_shooterAim.m_isAimReady && m_shooter.m_isDrumReady) { - return true; - } - - return false; - } -} diff --git a/src/main/java/frc4388/robot/commands/shooter/RunHoodWithJoystick.java b/src/main/java/frc4388/robot/commands/shooter/RunHoodWithJoystick.java deleted file mode 100644 index f406e04..0000000 --- a/src/main/java/frc4388/robot/commands/shooter/RunHoodWithJoystick.java +++ /dev/null @@ -1,54 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc4388.robot.commands.shooter; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc4388.robot.subsystems.Leveler; -import frc4388.robot.subsystems.ShooterHood; -import frc4388.utility.controller.IHandController; - -public class RunHoodWithJoystick extends CommandBase { - private ShooterHood m_hood; - private IHandController m_controller; - - /** - * Creates a new RunLevelerWithJoystick to control the leveler with an Xbox controller. - * @param subsystem pass the Hood subsystem from {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer} - * @param controller pass the Operator {@link frc4388.utility.controller.IHandController#getClass() IHandController} using the - * {@link frc4388.robot.RobotContainer#getOperatorJoystick() getOperatorJoystick()} method in - * {@link frc4388.robot.RobotContainer#RobotContainer() RobotContainer} - */ - public RunHoodWithJoystick(ShooterHood subsystem, IHandController controller) { - m_hood = subsystem; - m_controller = controller; - addRequirements(m_hood); - } - - // 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() { - double input = m_controller.getRightYAxis(); - m_hood.runHood(input); - } - - // 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 false; - } -} diff --git a/src/main/java/frc4388/robot/commands/shooter/ShootPrepGroup.java b/src/main/java/frc4388/robot/commands/shooter/ShootPrepGroup.java deleted file mode 100644 index df92eb4..0000000 --- a/src/main/java/frc4388/robot/commands/shooter/ShootPrepGroup.java +++ /dev/null @@ -1,34 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc4388.robot.commands.shooter; - -import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; -import frc4388.robot.commands.storage.StoragePrep; -import frc4388.robot.subsystems.Shooter; -import frc4388.robot.subsystems.ShooterAim; -import frc4388.robot.subsystems.ShooterHood; -import frc4388.robot.subsystems.Storage; - -// NOTE: Consider using this command inline, rather than writing a subclass. For more -// information, see: -// https://docs.wpilib.org/en/latest/docs/software/commandbased/convenience-features.html -public class ShootPrepGroup extends ParallelDeadlineGroup { - /** - * Prepares the Shooter to be fired - * @param m_shooter The Shooter subsytem - * @param m_shooterAim The ShooterAim subsystem - * @param m_storage The Storage subsytem - */ - public ShootPrepGroup(Shooter m_shooter, ShooterAim m_shooterAim, ShooterHood m_shooterHood, Storage m_storage) { - super( - new TrackTarget(m_shooterAim), - new ShooterVelocityControlPID(m_shooter), - new HoodPositionPID(m_shooterHood) - ); - } -} diff --git a/src/main/java/frc4388/robot/commands/shooter/ShooterGoalPosition.java b/src/main/java/frc4388/robot/commands/shooter/ShooterGoalPosition.java deleted file mode 100644 index 100e077..0000000 --- a/src/main/java/frc4388/robot/commands/shooter/ShooterGoalPosition.java +++ /dev/null @@ -1,52 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc4388.robot.commands.shooter; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc4388.robot.subsystems.Shooter; -import frc4388.robot.subsystems.ShooterAim; -import frc4388.robot.subsystems.ShooterHood; - -public class ShooterGoalPosition extends CommandBase { - Shooter m_shooter; - ShooterHood m_hood; - ShooterAim m_aim; - /** - * Creates a new ShooterGoalPosition. - */ - public ShooterGoalPosition(Shooter shooterSub, ShooterHood hoodSub, ShooterAim aimSub) { - m_shooter = shooterSub; - m_hood = hoodSub; - m_aim = aimSub; - addRequirements(m_shooter,m_hood,m_aim); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - m_shooter.runDrumShooterVelocityPID(5000); - m_hood.runAngleAdjustPID(4); - m_aim.runshooterRotatePID(-26.5); - } - - // 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 false; - } -} diff --git a/src/main/java/frc4388/robot/commands/shooter/ShooterManual.java b/src/main/java/frc4388/robot/commands/shooter/ShooterManual.java deleted file mode 100644 index 2017ff4..0000000 --- a/src/main/java/frc4388/robot/commands/shooter/ShooterManual.java +++ /dev/null @@ -1,45 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc4388.robot.commands.shooter; - -import edu.wpi.first.wpilibj.XboxController; -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc4388.robot.RobotContainer; - -public class ShooterManual extends CommandBase { - public boolean isManual = false; - /** - * Creates a new ShooterManual. - */ - public ShooterManual(boolean man) { - isManual = man; - } - - // 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() { - RobotContainer.m_isShooterManual = isManual; - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - RobotContainer.m_isShooterManual = isManual; - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} diff --git a/src/main/java/frc4388/robot/commands/shooter/ShooterTrenchPosition.java b/src/main/java/frc4388/robot/commands/shooter/ShooterTrenchPosition.java deleted file mode 100644 index 793f245..0000000 --- a/src/main/java/frc4388/robot/commands/shooter/ShooterTrenchPosition.java +++ /dev/null @@ -1,52 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc4388.robot.commands.shooter; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc4388.robot.subsystems.Shooter; -import frc4388.robot.subsystems.ShooterAim; -import frc4388.robot.subsystems.ShooterHood; - -public class ShooterTrenchPosition extends CommandBase { - Shooter m_shooter; - ShooterHood m_hood; - ShooterAim m_aim; - /** - * Creates a new ShooterTrenchPosition. - */ - public ShooterTrenchPosition(Shooter shooterSub, ShooterHood hoodSub, ShooterAim aimSub) { - m_shooter = shooterSub; - m_hood = hoodSub; - m_aim = aimSub; - addRequirements(m_shooter,m_hood,m_aim); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - m_shooter.runDrumShooterVelocityPID(5500); - m_hood.runAngleAdjustPID(11); - //m_aim.runshooterRotatePID(-28); - } - - // 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 false; - } -} diff --git a/src/main/java/frc4388/robot/commands/shooter/ShooterVelocityControlPID.java b/src/main/java/frc4388/robot/commands/shooter/ShooterVelocityControlPID.java deleted file mode 100644 index 11ccd56..0000000 --- a/src/main/java/frc4388/robot/commands/shooter/ShooterVelocityControlPID.java +++ /dev/null @@ -1,61 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc4388.robot.commands.shooter; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc4388.robot.Constants.ShooterConstants; -import frc4388.robot.subsystems.Shooter; - -public class ShooterVelocityControlPID extends CommandBase { - Shooter m_shooter; - double m_targetVel; - double m_actualVel; - - /** - * Runs the drum at a velocity - * @param subsystem The Shooter subsytem - */ - public ShooterVelocityControlPID(Shooter subsystem) { - m_shooter = subsystem; - addRequirements(m_shooter); - } - - // 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() { - //Tells whether the target velocity has been reached - m_actualVel = m_shooter.m_shooterFalconLeft.getSelectedSensorPosition(); - m_targetVel = m_shooter.addFireVel(); - double error = m_actualVel - m_targetVel; - if (Math.abs(error) < ShooterConstants.DRUM_VELOCITY_BOUND){ - m_shooter.m_isDrumReady = true; - m_shooter.runDrumShooterVelocityPID(m_targetVel); - } - else{ - m_shooter.m_isDrumReady = false; - m_shooter.runDrumShooterVelocityPID(m_targetVel); - } - } - - // 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 false; - } -} diff --git a/src/main/java/frc4388/robot/commands/shooter/TrackTarget.java b/src/main/java/frc4388/robot/commands/shooter/TrackTarget.java deleted file mode 100644 index 047b060..0000000 --- a/src/main/java/frc4388/robot/commands/shooter/TrackTarget.java +++ /dev/null @@ -1,138 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc4388.robot.commands.shooter; - -import edu.wpi.first.networktables.NetworkTableEntry; -import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc4388.robot.Constants.VisionConstants; -import frc4388.robot.subsystems.LimeLight; -import frc4388.robot.subsystems.Shooter; -import frc4388.robot.subsystems.ShooterAim; -import frc4388.robot.subsystems.ShooterHood; -import frc4388.utility.controller.IHandController; - -public class TrackTarget extends CommandBase { - // Setup - ShooterAim m_shooterAim; - Shooter m_shooter; - ShooterHood m_shooterHood; - - NetworkTableEntry xEntry; - IHandController m_driverController; - // Aiming - double turnAmount = 0; - double xAngle = 0; - double yAngle = 0; - double target = 0; - public double distance; - public double realDistance; - public static double fireVel; - public static double fireAngle; - - public double m_hoodTrim; - public double m_turretTrim; - - /** - * Uses the Limelight to track the target - * @param shooterSubsystem The Shooter subsystem - * @param aimSubsystem The ShooterAim subsystem - */ - public TrackTarget(ShooterAim aimSubsystem) { - m_shooterAim = aimSubsystem; - m_shooter = m_shooterAim.m_shooterSubsystem; - m_shooterHood = m_shooter.m_shooterHoodSubsystem; - addRequirements(m_shooterAim); - NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(1); - NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(0); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - // Vision Processing Mode - NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(0); - NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(3); - NetworkTableInstance.getDefault().getTable("limelight").getEntry("pipeline").setNumber(0); - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - target = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tv").getDouble(0); - xAngle = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tx").getDouble(0); - yAngle = NetworkTableInstance.getDefault().getTable("limelight").getEntry("ty").getDouble(0); - - // Finding Distance - distance = (VisionConstants.TARGET_HEIGHT) / Math.tan((VisionConstants.LIME_ANGLE + yAngle) * (Math.PI / 180)); - realDistance = (1.09 * distance) - 12.8; - - - if (target == 1.0) { // If target in view - // Aiming Left/Right - xAngle = xAngle + m_shooter.getCenterDisplacement(realDistance); - turnAmount = ((xAngle / VisionConstants.FOV) * VisionConstants.TURN_P_VALUE); - if (Math.abs(xAngle) < VisionConstants.X_ANGLE_ERROR) { - turnAmount = 0; - } // Angle Error Zone - // Deadzones - else if (turnAmount > 0 && turnAmount < 0.1){// VisionConstants.MOTOR_DEAD_ZONE) { - turnAmount = 0.1;//VisionConstants.MOTOR_DEAD_ZONE; - } else if (turnAmount < 0 && turnAmount > -0.1){//-VisionConstants.MOTOR_DEAD_ZONE) { - turnAmount = -0.1;//-VisionConstants.MOTOR_DEAD_ZONE; - } - - m_shooterAim.runShooterWithInput(-turnAmount);// - m_shooter.shooterTrims.m_turretTrim); - //m_shooterAim.runshooterRotatePID(targetAngle); - - SmartDashboard.putNumber("Distance to Target", realDistance); - SmartDashboard.putNumber("Center Displacement", m_shooter.getCenterDisplacement(realDistance)); - //START Equation Code - double yVel = Math.sqrt(2 * VisionConstants.GRAV * VisionConstants.TARGET_HEIGHT); - double xVel = (distance * VisionConstants.GRAV) / (yVel); - - //fireVel = Math.sqrt((Math.pow(xVel, 2))+(Math.pow(yVel,2))); - //fireAngle = Math.atan(yVel/xVel) * (180/Math.PI); - //END Equation Code - - //START CSV Code - fireVel = m_shooter.getVelocity(realDistance); - fireAngle = m_shooter.getHood(realDistance); //Note: Ensure to follow because units are different - //fireAngle = 33; - //END CSV Code - - //fireVel = SmartDashboard.getNumber("Velocity Target", 0); - //fireAngle = SmartDashboard.getNumber("Angle Target", 3); - - - m_shooter.m_fireVel = fireVel; - m_shooterHood.m_fireAngle = fireAngle;// + m_shooter.shooterTrims.m_hoodTrim; - m_shooterAim.m_targetDistance = distance; - } - } - - // 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() { - if (xAngle < 0.5 && xAngle > -0.5 && target == 1) - { - m_shooterAim.m_isAimReady = true; - } else { - m_shooterAim.m_isAimReady = false; - } - - return false; - } -} diff --git a/src/main/java/frc4388/robot/commands/shooter/TrimShooter.java b/src/main/java/frc4388/robot/commands/shooter/TrimShooter.java deleted file mode 100644 index 2c673b8..0000000 --- a/src/main/java/frc4388/robot/commands/shooter/TrimShooter.java +++ /dev/null @@ -1,65 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc4388.robot.commands.shooter; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc4388.robot.Constants.OIConstants; -import frc4388.robot.subsystems.Shooter; -import frc4388.utility.controller.XboxController; - -public class TrimShooter extends CommandBase { - private final XboxController m_operatorXbox = new XboxController(OIConstants.XBOX_OPERATOR_ID); - public double turretTrim = 0; - public double hoodTrim = 0; - - public Shooter m_shooter; - /** - * Trims the shooter based on the D-Pad inputs - * @param shootSub The Shooter subsytems - */ - public TrimShooter(Shooter shootSub) { - m_shooter = shootSub; - } - - // 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() { - - if(m_operatorXbox.getDPadTop()){ - hoodTrim += 1; - } - else if(m_operatorXbox.getDPadBottom()){ - hoodTrim -= 1; - } - else if(m_operatorXbox.getDPadRight()){ - turretTrim += 1; - } - else if(m_operatorXbox.getDPadLeft()){ - turretTrim -= 1; - } - - m_shooter.shooterTrims.m_turretTrim = turretTrim; - m_shooter.shooterTrims.m_hoodTrim = hoodTrim; - } - - // 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 false; - } -} diff --git a/src/main/java/frc4388/robot/commands/storage/ManageStorage.java b/src/main/java/frc4388/robot/commands/storage/ManageStorage.java deleted file mode 100644 index 8b55380..0000000 --- a/src/main/java/frc4388/robot/commands/storage/ManageStorage.java +++ /dev/null @@ -1,147 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc4388.robot.commands.storage; - -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc4388.robot.Constants.StorageConstants; -import frc4388.robot.subsystems.Storage; -import frc4388.robot.subsystems.Storage.StorageMode; - -public class ManageStorage extends CommandBase { - Storage m_storage; - - /* Timer */ - long m_resetStartTime; - - /* Keeps track of which beam breaks are pressed */ - boolean m_isBallInIntake = false; - boolean m_isBallInStorage = false; - boolean m_isBallInUseless = false; - boolean m_isBallInShooter = false; - - /* Used for intaking a ball. Keeps track off when the 2nd ball in storage has moved */ - boolean m_isStorageEmpty = true; - - /** - * Creates a new ManageStorage. - */ - public ManageStorage(Storage m_robotStorage) { - // Use addRequirements() here to declare subsystem dependencies. - m_storage = m_robotStorage; - addRequirements(m_storage); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - m_isBallInIntake = !m_storage.getBeamIntake(); - m_isBallInStorage = !m_storage.getBeamStorage(); - m_isBallInUseless = !m_storage.getBeamUseless(); - m_isBallInShooter = !m_storage.getBeamShooter(); - - m_isStorageEmpty = !m_isBallInStorage; - - if (m_storage.m_storageMode == StorageMode.RESET) { - m_resetStartTime = System.currentTimeMillis(); - } - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - m_isBallInIntake = !m_storage.getBeamIntake(); - m_isBallInStorage = !m_storage.getBeamStorage(); - m_isBallInUseless = !m_storage.getBeamUseless(); - m_isBallInShooter = !m_storage.getBeamShooter(); - - /// TODO: Delete/Comment these when done - SmartDashboard.putBoolean("!Ball in Intake!", m_isBallInIntake); - SmartDashboard.putBoolean("!Ball Storage!", m_isBallInStorage); - SmartDashboard.putBoolean("!Ball Shooter!", m_isBallInShooter); - - if (m_storage.m_storageMode == StorageMode.IDLE) { - runIdle(); - } else if (m_storage.m_storageMode == StorageMode.INTAKE) { - runIntake(); - } else if (m_storage.m_storageMode == StorageMode.RESET) { - runReset(); - } else if (m_storage.m_storageMode == StorageMode.MANUAL) { - runManual(); - } - } - - /** - * Intakes a ball. - * Runs until the storage ball has moved past the - * storage sensor and the intake ball has taken its place. - */ - private void runIntake() { - if (!m_isBallInShooter) { // Intake balls as long as there is not a ball at the shooter - m_storage.runStorage(StorageConstants.STORAGE_SPEED); - - if (!m_isStorageEmpty && !m_isBallInStorage) { // If ball moves out of storage, set storage to empty - m_isStorageEmpty = true; - } - if (m_isStorageEmpty && m_isBallInStorage) { // If Ball moves into storage, set storage to full and swtich to idle mode - m_isStorageEmpty = false; - m_storage.changeStorageMode(StorageMode.IDLE); - } - } else { - m_storage.changeStorageMode(StorageMode.IDLE); - } - } - - /** - * Idles until a ball is in the intake. - * Also updates the status of the storage position - */ - private void runIdle() { - m_storage.runStorage(0); - - if (m_isBallInIntake) { - m_storage.changeStorageMode(StorageMode.INTAKE); - } - m_isStorageEmpty = !m_isBallInStorage; - } - - /** - * Runs Storage out until the Intake Sensor is tripped. - * Then switches into intake mode. This effectively - * resets the position of the balls back to the bottom of the storage. - */ - private void runReset() { - m_storage.runStorage(-StorageConstants.STORAGE_SPEED); - - if (m_isBallInIntake) { - m_storage.changeStorageMode(StorageMode.INTAKE); - } else if (m_resetStartTime + StorageConstants.STORAGE_TIMEOUT < System.currentTimeMillis()) { - m_storage.changeStorageMode(StorageMode.IDLE); - } - m_isStorageEmpty = !m_isBallInStorage; - } - - /** - * Switches Storage to Manual only - */ - private void runManual() { - m_storage.runStorage(0); - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - m_storage.changeStorageMode(StorageMode.RESET); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} diff --git a/src/main/java/frc4388/robot/commands/storage/ManageStoragePID.java b/src/main/java/frc4388/robot/commands/storage/ManageStoragePID.java deleted file mode 100644 index 97ff356..0000000 --- a/src/main/java/frc4388/robot/commands/storage/ManageStoragePID.java +++ /dev/null @@ -1,143 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc4388.robot.commands.storage; - -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc4388.robot.Constants.StorageConstants; -import frc4388.robot.subsystems.Storage; -import frc4388.robot.subsystems.Storage.StorageMode; - -public class ManageStoragePID extends CommandBase { - Storage m_storage; - - /* Timer */ - long m_resetStartTime; - - /* Start Positions */ - double m_intakeStartPos; - - /* Keeps track of which beam breaks are pressed */ - boolean m_isBallInIntake = false; - boolean m_isBallInStorage = false; - boolean m_isBallInUseless = false; - boolean m_isBallInShooter = false; - - /* Used for intaking a ball. Keeps track off when the 2nd ball in storage has moved */ - boolean m_isStorageEmpty = true; - - - /** - * Creates a new ManageStorage. - */ - public ManageStoragePID(Storage m_robotStorage) { - // Use addRequirements() here to declare subsystem dependencies. - m_storage = m_robotStorage; - addRequirements(m_storage); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - m_isBallInIntake = !m_storage.getBeamIntake(); - m_isBallInStorage = !m_storage.getBeamStorage(); - m_isBallInUseless = !m_storage.getBeamUseless(); - m_isBallInShooter = !m_storage.getBeamShooter(); - - m_isStorageEmpty = !m_isBallInStorage; - - m_intakeStartPos = m_storage.getEncoderPosInches(); - - if (m_storage.m_storageMode == StorageMode.RESET) { - m_resetStartTime = System.currentTimeMillis(); - } - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - m_isBallInIntake = !m_storage.getBeamIntake(); - m_isBallInStorage = !m_storage.getBeamStorage(); - m_isBallInUseless = !m_storage.getBeamUseless(); - m_isBallInShooter = !m_storage.getBeamShooter(); - - /// TODO: Delete/Comment these when done - SmartDashboard.putBoolean("!Ball in Intake!", m_isBallInIntake); - SmartDashboard.putBoolean("!Ball Storage!", m_isBallInStorage); - SmartDashboard.putBoolean("!Ball Shooter!", m_isBallInShooter); - - if (m_storage.m_storageMode == StorageMode.IDLE) { - runIdle(); - } else if (m_storage.m_storageMode == StorageMode.INTAKE) { - runIntake(); - } else if (m_storage.m_storageMode == StorageMode.RESET) { - runReset(); - } - } - - /** - * Intakes a ball. - * Runs until the storage ball has moved past the - * storage sensor and the intake ball has taken its place. - */ - private void runIntake() { - if (!m_isBallInShooter) { // Intake balls as long as there is not a ball at the shooter - m_storage.runStoragePositionPID(m_intakeStartPos + StorageConstants.STORAGE_FULL_BALL); - - double error = (m_intakeStartPos + StorageConstants.STORAGE_FULL_BALL) - m_storage.getEncoderPosInches(); - if (m_storage.getEncoderVel() == 0 && Math.abs(error) < 0.5) { - m_storage.changeStorageMode(StorageMode.IDLE); - } - } else { - m_storage.changeStorageMode(StorageMode.IDLE); - } - } - - /** - * Idles until a ball is in the intake. - * Also updates the status of the storage position - */ - private void runIdle() { - m_storage.runStorage(0); - - if (m_isBallInIntake) { - m_storage.changeStorageMode(StorageMode.INTAKE); - m_intakeStartPos = m_storage.getEncoderPosInches(); - } - m_isStorageEmpty = !m_isBallInStorage; - } - - /** - * Runs Storage out until the Intake Sensor is tripped. - * Then switches into intake mode. This effectively - * resets the position of the balls back to the bottom of the storage. - */ - private void runReset() { - m_storage.runStorage(-StorageConstants.STORAGE_SPEED); - - if (m_isBallInIntake) { - m_storage.changeStorageMode(StorageMode.INTAKE); - m_intakeStartPos = m_storage.getEncoderPosInches(); - } else if (m_resetStartTime + StorageConstants.STORAGE_TIMEOUT < System.currentTimeMillis()) { - m_storage.changeStorageMode(StorageMode.IDLE); - } - m_isStorageEmpty = !m_isBallInStorage; - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) { - m_storage.changeStorageMode(StorageMode.RESET); - } - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/commands/storage/RunStorage.java b/src/main/java/frc4388/robot/commands/storage/RunStorage.java deleted file mode 100644 index 3a259d1..0000000 --- a/src/main/java/frc4388/robot/commands/storage/RunStorage.java +++ /dev/null @@ -1,46 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc4388.robot.commands.storage; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc4388.robot.Constants.StorageConstants; -import frc4388.robot.subsystems.Storage; - -public class RunStorage extends CommandBase { - Storage m_storage; - /** - * Creates a new RunStorage. - */ - public RunStorage(Storage subsystem) { - // Use addRequirements() here to declare subsystem dependencies. - m_storage = subsystem; - addRequirements(m_storage); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - m_storage.runStorage(StorageConstants.STORAGE_SPEED); - } - - // 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 false; - } -} diff --git a/src/main/java/frc4388/robot/commands/storage/StoragePrep.java b/src/main/java/frc4388/robot/commands/storage/StoragePrep.java deleted file mode 100644 index 9fcee06..0000000 --- a/src/main/java/frc4388/robot/commands/storage/StoragePrep.java +++ /dev/null @@ -1,54 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc4388.robot.commands.storage; - -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc4388.robot.Constants.StorageConstants; -import frc4388.robot.subsystems.Storage; - -public class StoragePrep extends CommandBase { - Storage m_storage; - double m_startTime; - /** - * Prepares the Storage for aiming - * @param storeSub The Storage subsystem - */ - public StoragePrep(Storage storeSub) { - m_storage = storeSub; - addRequirements(m_storage); - } - - // Called when the command is initially scheduled. - @Override - public void initialize() { - m_startTime = System.currentTimeMillis(); - } - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - if ((m_storage.getBeamUseless() && m_storage.getBeamShooter()) || (m_startTime + StorageConstants.STORAGE_TIMEOUT) < System.currentTimeMillis()) { - m_storage.runStorage(StorageConstants.STORAGE_SPEED); - m_storage.m_isStorageReadyToFire = false; - } else { - m_storage.runStorage(0); - m_storage.m_isStorageReadyToFire = true; - } - } - - // 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 false; - } -} diff --git a/src/main/java/frc4388/robot/subsystems/Climber.java b/src/main/java/frc4388/robot/subsystems/Climber.java deleted file mode 100644 index 687ede2..0000000 --- a/src/main/java/frc4388/robot/subsystems/Climber.java +++ /dev/null @@ -1,93 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc4388.robot.subsystems; - -import com.revrobotics.CANDigitalInput; -import com.revrobotics.CANSparkMax; -import com.revrobotics.CANDigitalInput.LimitSwitchPolarity; -import com.revrobotics.CANSparkMax.IdleMode; -import com.revrobotics.CANSparkMaxLowLevel.MotorType; - -import edu.wpi.first.wpilibj.Servo; -import edu.wpi.first.wpilibj.Spark; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc4388.robot.Constants.ClimberConstants; - -public class Climber extends SubsystemBase { - CANSparkMax m_climberMotor = new CANSparkMax(ClimberConstants.CLIMBER_SPARK_ID, MotorType.kBrushless); - CANDigitalInput m_climberForwardLimit, m_climberReverseLimit; - - Servo m_servo; - //Spark m_spark = new Spark(4); - - public boolean m_climberSafety = false; - public boolean m_isRatchetEngaged = true; - - /** - * Creates a new Climber. - */ - public Climber() { - m_servo = new Servo(4); - - m_climberMotor.restoreFactoryDefaults(); - - m_climberMotor.setIdleMode(IdleMode.kBrake); - m_climberMotor.setInverted(true); - - m_climberForwardLimit = m_climberMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyOpen); - m_climberReverseLimit = m_climberMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyOpen); - m_climberForwardLimit.enableLimitSwitch(true); - m_climberReverseLimit.enableLimitSwitch(true); - } - - @Override - public void periodic() { - // This method will be called once per scheduler run - SmartDashboard.putBoolean("Climber Safety", m_climberSafety); - SmartDashboard.putBoolean("Rachet", m_isRatchetEngaged); - } - - /** - * Runs climber motor - * @param input the voltage to run motor at - */ - public void runClimber(double input) { - if(m_climberSafety){ - input *= 1.0; - m_climberMotor.set(input); - } - else{ - m_climberMotor.set(0); - } - } - - /* Safety Button for Climber */ - public void setSafetyPressed() - { - m_climberSafety = true; - } - - /* Safety Button for Climber set back to false */ - public void setSafetyNotPressed() - { - m_climberSafety = false; - } - - /** - * @param shift true to engage ratchet, false to disengage - */ - public void shiftServo(boolean shift) { - if (shift) { - m_servo.setPosition(0.48); - } else { - m_servo.setPosition(0.56); - } - m_isRatchetEngaged = shift; - } -} diff --git a/src/main/java/frc4388/robot/subsystems/Drive.java b/src/main/java/frc4388/robot/subsystems/Drive.java index 3033530..945b2b0 100644 --- a/src/main/java/frc4388/robot/subsystems/Drive.java +++ b/src/main/java/frc4388/robot/subsystems/Drive.java @@ -55,7 +55,7 @@ public class Drive extends SubsystemBase { /* Pneumatics Subsystem */ public Pneumatics m_pneumaticsSubsystem; - Shooter m_shooter; + // Shooter_1 m_shooter; /* Low Gear Gains */ public static Gains m_gainsDistanceLow = DriveConstants.DRIVE_DISTANCE_GAINS_LOW; @@ -310,10 +310,10 @@ public class Drive extends SubsystemBase { * * @param subsystem Subsystem needed. */ - public void passRequiredSubsystem(Pneumatics subsystem, Shooter shooter) { + public void passRequiredSubsystem(Pneumatics subsystem) { m_pneumaticsSubsystem = subsystem; - m_shooter = shooter; - m_orchestra.addInstrument(m_shooter.m_shooterFalconLeft); + // m_shooter = shooter; + // m_orchestra.addInstrument(m_shooter.m_shooterFalconLeft); } public void updateTime() { diff --git a/src/main/java/frc4388/robot/subsystems/Intake.java b/src/main/java/frc4388/robot/subsystems/Intake.java deleted file mode 100644 index 109b412..0000000 --- a/src/main/java/frc4388/robot/subsystems/Intake.java +++ /dev/null @@ -1,86 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc4388.robot.subsystems; - -import com.revrobotics.CANDigitalInput; -import com.revrobotics.CANDigitalInput.LimitSwitchPolarity; -import com.revrobotics.CANSparkMax; -import com.revrobotics.CANSparkMax.IdleMode; -import com.revrobotics.CANSparkMaxLowLevel.MotorType; - -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc4388.robot.Constants.IntakeConstants; - - - -public class Intake extends SubsystemBase { - CANSparkMax m_intakeMotor = new CANSparkMax(IntakeConstants.INTAKE_SPARK_ID, MotorType.kBrushless); - CANSparkMax m_extenderMotor = new CANSparkMax(IntakeConstants.EXTENDER_SPARK_ID, MotorType.kBrushless); - CANDigitalInput m_extenderForwardLimit; - CANDigitalInput m_extenderReverseLimit; - public boolean isExtended = false; - - /** - * Creates a new Intake. - */ - public Intake() { - - - m_intakeMotor.restoreFactoryDefaults(); - m_extenderMotor.restoreFactoryDefaults(); - - m_intakeMotor.setIdleMode(IdleMode.kCoast); - m_extenderMotor.setIdleMode(IdleMode.kBrake); - m_intakeMotor.setInverted(false); - m_extenderMotor.setInverted(true); - - m_extenderForwardLimit = m_extenderMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyOpen); - m_extenderReverseLimit = m_extenderMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyClosed); - m_extenderForwardLimit.enableLimitSwitch(true); - m_extenderReverseLimit.enableLimitSwitch(true); - } - - @Override - public void periodic() { - // This method will be called once per scheduler run - } - - /** - * Runs intake motor - * @param input the percent output to run motor at - */ - public void runIntake(double input) { - m_intakeMotor.set(input); - } - - public void runExtender(double input){ - m_extenderMotor.set(input); - } - /** - * Runs extender motor - * @param input the percent output to run motor at - */ - /*public void runExtender(double input) { - if (m_extenderForwardLimit.get()) { - isExtended = true; - } - else if (m_extenderReverseLimit.get()) { - isExtended = false; - } - else{ - m_extenderMotor.set(-input); - } - - if (isExtended == false) { - m_extenderMotor.set(input); - } - if (isExtended == true) { - m_extenderMotor.set(-input); - } - }*/ -} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/Leveler.java b/src/main/java/frc4388/robot/subsystems/Leveler.java deleted file mode 100644 index c2a1aa5..0000000 --- a/src/main/java/frc4388/robot/subsystems/Leveler.java +++ /dev/null @@ -1,57 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc4388.robot.subsystems; - -import com.revrobotics.CANSparkMax; -import com.revrobotics.CANSparkMax.IdleMode; -import com.revrobotics.CANSparkMaxLowLevel.MotorType; - -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc4388.robot.Constants.LevelerConstants; - -public class Leveler extends SubsystemBase { - CANSparkMax m_levelerMotor = new CANSparkMax(LevelerConstants.LEVELER_CAN_ID, MotorType.kBrushless); - - Climber m_climberSubsystem; - - /** - * Creates a new Leveler. - */ - public Leveler() { - m_levelerMotor.restoreFactoryDefaults(); - - m_levelerMotor.setIdleMode(IdleMode.kBrake); - m_levelerMotor.setInverted(false); - } - - @Override - public void periodic() { - // This method will be called once per scheduler run - } - - /** - * Runs intake motor - * @param input the percent output to run motor at - */ - public void runLeveler(double input) { - if(m_climberSubsystem.m_climberSafety){ - m_levelerMotor.set(input); - } - else{ - m_levelerMotor.set(0); - } - } - - /** - * Passes subsystem needed. - * @param subsystem Subsystem needed. - */ - public void passRequiredSubsystem(Climber subsystem) { - m_climberSubsystem = subsystem; - } -} diff --git a/src/main/java/frc4388/robot/subsystems/LimeLight.java b/src/main/java/frc4388/robot/subsystems/LimeLight.java deleted file mode 100644 index a382e2b..0000000 --- a/src/main/java/frc4388/robot/subsystems/LimeLight.java +++ /dev/null @@ -1,157 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc4388.robot.subsystems; - -import edu.wpi.first.networktables.NetworkTableInstance; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc4388.robot.Constants.VisionConstants; - -public class LimeLight extends SubsystemBase { - /** - * Creates a new LimeLight. - */ - public String galacticSearchPath; - - public LimeLight() { - - } - - public void limeOff(){ - NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(1); - NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(1); - } - - public void limeOn(){ - NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(0); - NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(3); - } - - public void changePipeline(int pipelineId) - { - NetworkTableInstance.getDefault().getTable("limelight").getEntry("pipeline").setNumber(pipelineId); - } - - public double getV() - { - return NetworkTableInstance.getDefault().getTable("limelight").getEntry("tv").getDouble(0); - } - - public double getX() - { - return NetworkTableInstance.getDefault().getTable("limelight").getEntry("tx").getDouble(0); - } - - public double getY() - { - return NetworkTableInstance.getDefault().getTable("limelight").getEntry("ty").getDouble(0); - } - - // for VOP - public double[] getVArray() - { - double[] defaultValue = new double[0]; - return NetworkTableInstance.getDefault().getTable("limelight").getEntry("tv").getDoubleArray(defaultValue); - } - - public double[] getXArray() - { - double[] defaultValue = new double[0]; - return NetworkTableInstance.getDefault().getTable("limelight").getEntry("tx").getDoubleArray(defaultValue); - } - - public double[] getYArray() - { - double[] defaultValue = new double[0]; - return NetworkTableInstance.getDefault().getTable("limelight").getEntry("ty").getDoubleArray(defaultValue); - } - // end VOP - - int i = 0; - boolean onceThrough = false; - boolean pathFound = false; - public void identifyPath(){ - - if (!onceThrough) - { - changePipeline(2); - if (withinCoords(VisionConstants.aBlue)) - { - pathFound = true; - galacticSearchPath = "A_BLUE"; - } - - changePipeline(4); - if (withinCoords(VisionConstants.bBlue)) - { - pathFound = true; - galacticSearchPath = "B_BLUE"; - } - - changePipeline(1); - if (withinCoords(VisionConstants.aRed)) - { - pathFound = true; - galacticSearchPath = "A_RED"; - } - - changePipeline(3); - if (withinCoords(VisionConstants.bRed)) - { - pathFound = true; - galacticSearchPath = "B_RED"; - } - - if (pathFound == false) - { - galacticSearchPath = "PathNotFound"; - } - SmartDashboard.putString("GalacticSearchPath", galacticSearchPath); - onceThrough = true; - } - else{ - i++; - SmartDashboard.putNumber("Counter", i); - } - if (i >= 50) - { - i=0; - pathFound = false; - onceThrough = false; - } - } - -public boolean withinError(double angle, double goal) -{ - if(goal > (angle - VisionConstants.searchError) && goal < (angle + VisionConstants.searchError)) - { - return true; - } - else - { - return false; - } -} - -public boolean withinCoords(double[] coords) -{ - if (withinError(getX(), coords[0]) && withinError(getY(), coords[1])) - { - return true; - } - else - { - return false; - } -} - - @Override - public void periodic() { - // This method will be called once per scheduler run - } -} diff --git a/src/main/java/frc4388/robot/subsystems/Shooter.java b/src/main/java/frc4388/robot/subsystems/Shooter.java deleted file mode 100644 index cc2d7b0..0000000 --- a/src/main/java/frc4388/robot/subsystems/Shooter.java +++ /dev/null @@ -1,200 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc4388.robot.subsystems; - -import java.io.File; -import java.io.IOException; -import java.util.Comparator; -import java.util.Map; -import java.util.Optional; -import java.util.function.Function; -import java.util.regex.Pattern; -import java.util.stream.IntStream; - -import com.ctre.phoenix.motorcontrol.NeutralMode; -import com.ctre.phoenix.motorcontrol.TalonFXControlMode; -import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX; - -import edu.wpi.first.wpilibj.Filesystem; -import edu.wpi.first.wpilibj.RobotBase; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc4388.robot.Constants; -import frc4388.robot.Robot; -import frc4388.robot.Constants.ShooterConstants; -import frc4388.utility.CSV; -import frc4388.utility.Trims; - -public class Shooter extends SubsystemBase { - - public final WPI_TalonFX m_shooterFalconLeft = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_BALLER_ID); - public final WPI_TalonFX m_shooterFalconRight = new WPI_TalonFX(ShooterConstants.SHOOTER_FALCON_BALLER_FOLLOWER_ID); - - public static class ShooterTableEntry { - public Double distance, hoodExt, drumVelocity, centerDisplacement; - } - - private ShooterTableEntry[][] m_shooterTables; - - public ShooterTableEntry[] getActiveShooterTable() { - return m_shooterTables[Constants.Mode.get().ordinal()]; - } - - public boolean m_isDrumReady = false; - public double m_fireVel; - - public Trims shooterTrims; - - public ShooterHood m_shooterHoodSubsystem; - public ShooterAim m_shooterAimSubsystem; - - /* - * Creates a new Shooter subsystem, with the drum shooter and the angle adjuster. - */ - public Shooter() { - // Testing purposes reseting gyros - // resetGyroAngleAdj(); - shooterTrims = new Trims(0, 0); - // SmartDashboard.putNumber("Velocity Target", 10000); - // SmartDashboard.putNumber("Angle Target", 3); - - // LEFT FALCON - m_shooterFalconLeft.configFactoryDefault(); - m_shooterFalconLeft.setNeutralMode(NeutralMode.Coast); - m_shooterFalconLeft.setInverted(true); - m_shooterFalconLeft.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS); - m_shooterFalconLeft.configClosedloopRamp(0.75, ShooterConstants.SHOOTER_TIMEOUT_MS); - - // RIGHT FALCON - m_shooterFalconRight.configFactoryDefault(); - m_shooterFalconRight.setNeutralMode(NeutralMode.Coast); - m_shooterFalconRight.setInverted(false); - m_shooterFalconRight.configOpenloopRamp(1, ShooterConstants.SHOOTER_TIMEOUT_MS); - m_shooterFalconRight.configClosedloopRamp(0.75, ShooterConstants.SHOOTER_TIMEOUT_MS); - setShooterGains(); - - final int closedLoopTimeMs = 1; - // LEFT FALCON - m_shooterFalconLeft.configPeakOutputReverse(0, ShooterConstants.SHOOTER_TIMEOUT_MS); - m_shooterFalconLeft.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX, ShooterConstants.SHOOTER_TIMEOUT_MS); - m_shooterFalconLeft.configClosedLoopPeriod(0, closedLoopTimeMs, ShooterConstants.SHOOTER_TIMEOUT_MS); - m_shooterFalconLeft.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG, ShooterConstants.SHOOTER_TIMEOUT_MS); - - // RIGHT FALCON - // m_shooterFalconRight.configPeakOutputForward(0, ShooterConstants.SHOOTER_TIMEOUT_MS); - m_shooterFalconRight.setSelectedSensorPosition(0, ShooterConstants.SHOOTER_PID_LOOP_IDX, ShooterConstants.SHOOTER_TIMEOUT_MS); - m_shooterFalconRight.configClosedLoopPeriod(0, closedLoopTimeMs, ShooterConstants.SHOOTER_TIMEOUT_MS); - m_shooterFalconRight.configSupplyCurrentLimit(ShooterConstants.SUPPLY_CURRENT_LIMIT_CONFIG, ShooterConstants.SHOOTER_TIMEOUT_MS); - - try { - CSV csv = new CSV<>(ShooterTableEntry::new) { - private final Pattern parentheses = Pattern.compile("\\([^\\)]*+\\)"); - - @Override - protected String headerSanitizer(final String header) { - return super.headerSanitizer(parentheses.matcher(header).replaceAll("")); - } - }; - m_shooterTables = new ShooterTableEntry[][] { - csv.read(new File(Filesystem.getDeployDirectory(), "Robot Data - Distances.csv").toPath()), - csv.read(new File(Filesystem.getDeployDirectory(), "Robot Data - Distances Casual.csv").toPath()) - }; - new Thread(() -> System.out.println(CSV.ReflectionTable.create(getActiveShooterTable(), RobotBase.isSimulation()))).start(); - } catch (final IOException e) { - throw new RuntimeException(e); - } - } - - @Override - public void periodic() { - // This method will be called once per scheduler run - try { - SmartDashboard.putNumber("Drum Velocity", m_shooterFalconLeft.getSelectedSensorVelocity()); - SmartDashboard.putNumber("Drum Velocity CSV", m_fireVel); - SmartDashboard.putNumber("Shooter Temp C", m_shooterFalconLeft.getTemperature()); - SmartDashboard.putNumber("Shooter Current", m_shooterFalconLeft.getSupplyCurrent()); - SmartDashboard.putBoolean("Drum Ready", m_isDrumReady); - } catch (final Exception e) { - - } - } - - /** - * Passes subsystem needed. - * @param subsystem Subsystem needed. - */ - public void passRequiredSubsystem(ShooterHood subsystem0, ShooterAim subsystem1) { - m_shooterHoodSubsystem = subsystem0; - m_shooterAimSubsystem = subsystem1; - } - - public double addFireVel() { - return m_fireVel; - } - - /** - * Runs drum shooter motor. - * @param speed Speed to set the motor at - */ - public void runDrumShooter(double speed) { - m_shooterFalconLeft.set(TalonFXControlMode.PercentOutput, speed); - m_shooterFalconRight.follow(m_shooterFalconLeft); - // m_shooterFalconRight.set(TalonFXControlMode.PercentOutput, speed); - } - - /** - * Configures gains for shooter PID. - */ - public void setShooterGains() { - m_shooterFalconLeft.selectProfileSlot(ShooterConstants.SHOOTER_SLOT_IDX, ShooterConstants.SHOOTER_PID_LOOP_IDX); - m_shooterFalconLeft.config_kF(ShooterConstants.SHOOTER_SLOT_IDX, ShooterConstants.DRUM_SHOOTER_GAINS.m_kF, ShooterConstants.SHOOTER_TIMEOUT_MS); - m_shooterFalconLeft.config_kP(ShooterConstants.SHOOTER_SLOT_IDX, ShooterConstants.DRUM_SHOOTER_GAINS.m_kP, ShooterConstants.SHOOTER_TIMEOUT_MS); - m_shooterFalconLeft.config_kI(ShooterConstants.SHOOTER_SLOT_IDX, ShooterConstants.DRUM_SHOOTER_GAINS.m_kI, ShooterConstants.SHOOTER_TIMEOUT_MS); - m_shooterFalconLeft.config_kD(ShooterConstants.SHOOTER_SLOT_IDX, ShooterConstants.DRUM_SHOOTER_GAINS.m_kD, ShooterConstants.SHOOTER_TIMEOUT_MS); - } - - /** - * Runs drum shooter velocity PID. - * @param targetVel Target velocity to run motor at - */ - public void runDrumShooterVelocityPID(double targetVel) { - // System.out.println("Target Velocity" + targetVel); - m_shooterFalconLeft.set(TalonFXControlMode.Velocity, targetVel); // Init PID - m_shooterFalconRight.follow(m_shooterFalconLeft); - } - - public Double getCenterDisplacement(final Double distance) { - return linearInterpolate(getActiveShooterTable(), distance, e -> e.distance, e -> e.centerDisplacement).doubleValue(); - } - - public Double getVelocity(final Double distance) { - return linearInterpolate(getActiveShooterTable(), distance, e -> e.distance, e -> e.drumVelocity).doubleValue(); - } - - public Double getHood(final Double distance) { - return linearInterpolate(getActiveShooterTable(), distance, e -> e.distance, e -> e.hoodExt).doubleValue(); - } - - private static Number linearInterpolate(final E[] table, final Number lookupValue, final Function lookupGetter, final Function targetGetter) { - final Map.Entry closestEntry = lookup(table, lookupValue.doubleValue(), lookupGetter, false).orElse(Map.entry(table.length - 1, table[table.length - 1])); - final E closestRecord = closestEntry.getValue(); - final int closestRecordIndex = closestEntry.getKey(); - final E neighborRecord = table[lookupValue.doubleValue() <= lookupGetter.apply(closestRecord).doubleValue() ? Math.max(closestRecordIndex == 0 ? 1 : 0, closestRecordIndex - 1) : Math.min(closestRecordIndex + 1, table.length - (closestRecordIndex == table.length - 1 ? 2 : 1))]; - return lerp2(lookupValue, lookupGetter.apply(closestRecord), targetGetter.apply(closestRecord), lookupGetter.apply(neighborRecord), targetGetter.apply(neighborRecord)); - } - - private static Optional> lookup(final E[] table, final Number value, final Function valueGetter, final boolean exactMatch) { - final Optional> match = IntStream.range(0, table.length).mapToObj(i -> Map.entry(i, table[i])).min(Comparator.comparingDouble(e -> Math.abs(valueGetter.apply(e.getValue()).doubleValue() - value.doubleValue()))); - return !exactMatch || match.map(e -> valueGetter.apply(e.getValue()).equals(value)).orElse(false) ? match : Optional.empty(); - } - - private static Number lerp2(final Number x, final Number x0, final Number y0, final Number x1, final Number y1) { - final Number f = (x.doubleValue() - x0.doubleValue()) / (x1.doubleValue() - x0.doubleValue()); - return (1.0 - f.doubleValue()) * y0.doubleValue() + f.doubleValue() * y1.doubleValue(); - } -} \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/ShooterAim.java b/src/main/java/frc4388/robot/subsystems/ShooterAim.java deleted file mode 100644 index db9d231..0000000 --- a/src/main/java/frc4388/robot/subsystems/ShooterAim.java +++ /dev/null @@ -1,177 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc4388.robot.subsystems; - -import com.revrobotics.CANDigitalInput; -import com.revrobotics.CANDigitalInput.LimitSwitchPolarity; -import com.revrobotics.CANEncoder; -import com.revrobotics.CANPIDController; -import com.revrobotics.CANSparkMax; -import com.revrobotics.CANSparkMax.IdleMode; -import com.revrobotics.CANSparkMax.SoftLimitDirection; -import com.revrobotics.CANSparkMaxLowLevel.MotorType; -import com.revrobotics.ControlType; - -import edu.wpi.first.wpilibj.GyroBase; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc4388.robot.Constants.ShooterConstants; -import frc4388.utility.Gains; - -public class ShooterAim extends SubsystemBase { - public Shooter m_shooterSubsystem; - public Drive m_driveSubsystem; - - public CANSparkMax m_shooterRotateMotor = new CANSparkMax(ShooterConstants.SHOOTER_ROTATE_ID, MotorType.kBrushless); - public static Gains m_shooterTurretGains = ShooterConstants.SHOOTER_TURRET_GAINS; - CANDigitalInput m_shooterRightLimit, m_shooterLeftLimit; - public GyroBase m_turretGyro; - - public double m_targetDistance = 0; - - public boolean m_isAimReady = false; - - // Configure PID Controllers - CANPIDController m_shooterRotatePIDController = m_shooterRotateMotor.getPIDController(); - public CANEncoder m_shooterRotateEncoder = m_shooterRotateMotor.getEncoder(); - - /** - * Creates a subsytem for the turret aiming - */ - public ShooterAim() { - //resetGyroShooterRotate(); - m_shooterRotateMotor.setIdleMode(IdleMode.kBrake); - - m_turretGyro = getGyroInterface(); - - m_shooterLeftLimit = m_shooterRotateMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyOpen); - m_shooterRightLimit = m_shooterRotateMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyOpen); - m_shooterRightLimit.enableLimitSwitch(true); - m_shooterLeftLimit.enableLimitSwitch(true); - - m_shooterRotateMotor.enableSoftLimit(SoftLimitDirection.kForward, false); - m_shooterRotateMotor.enableSoftLimit(SoftLimitDirection.kReverse, true); - m_shooterRotateMotor.setSoftLimit(SoftLimitDirection.kForward, ShooterConstants.TURRET_RIGHT_SOFT_LIMIT); - m_shooterRotateMotor.setSoftLimit(SoftLimitDirection.kReverse, ShooterConstants.TURRET_LEFT_SOFT_LIMIT); - - m_shooterRotateMotor.setInverted(false); - } - - @Override - public void periodic() { - // This method will be called once per scheduler run - SmartDashboard.putNumber("Turret Angle Raw", getShooterRotatePosition()); - - SmartDashboard.putData("Turret Angle", m_turretGyro); - - SmartDashboard.putBoolean("Turret Aimed" , m_isAimReady); - } - - /** - * Passes subsystem needed. - * @param subsystem Subsystem needed. - */ - public void passRequiredSubsystem(Shooter subsystem0, Drive subsystem1) { - m_shooterSubsystem = subsystem0; - m_driveSubsystem = subsystem1; - } - - public void runShooterWithInput(double input) { - m_shooterRotateMotor.set(input*ShooterConstants.TURRET_SPEED_MULTIPLIER); - } - - - /* Rotate Shooter PID Control */ - public void runshooterRotatePID(double targetAngle) - { - // Set PID Coefficients - m_shooterRotatePIDController.setP(m_shooterTurretGains.m_kP); - m_shooterRotatePIDController.setI(m_shooterTurretGains.m_kI); - m_shooterRotatePIDController.setD(m_shooterTurretGains.m_kD); - m_shooterRotatePIDController.setFF(m_shooterTurretGains.m_kF); - m_shooterRotatePIDController.setIZone(m_shooterTurretGains.m_kIzone); - m_shooterRotatePIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_shooterTurretGains.m_kPeakOutput); - - // Convert input angle in degrees to rotations of the motor - targetAngle = targetAngle/ShooterConstants.DEGREES_PER_ROT; - - m_shooterRotatePIDController.setReference(targetAngle, ControlType.kPosition); - } - - public void resetGyroShooterRotate() - { - m_shooterRotateEncoder.setPosition(0); - } - - public double getShooterRotatePosition() - { - return m_shooterRotateEncoder.getPosition(); - } - - public double getShooterAngleDegrees() { - return (m_shooterRotateEncoder.getPosition() - ShooterConstants.TURRET_MOTOR_POS_AT_ZERO_ROT) * 360/ShooterConstants.TURRET_MOTOR_ROTS_PER_ROT; - } - - /** - * Gets the angle of the Shooter relative to the target. - */ - public double getTargetAngleDegrees() { - return m_driveSubsystem.getHeading() + getShooterAngleDegrees(); - } - - public double getTargetXDisplacement() { - return m_targetDistance * Math.cos(getTargetAngleDegrees()); - } - - public double getTargetYDisplacement() { - return m_targetDistance * Math.sin(getTargetAngleDegrees()); - } - - /** - * Gets the angle of the Shooter relative to the inner port. - * Use for tuning the Shooter Displacement - */ - public double getInnerPortAngleDegrees() { - return Math.atan( getTargetYDisplacement() / (getTargetXDisplacement() + 29.25) ); - } - - public GyroBase getGyroInterface() { - return new GyroBase(){ - - @Override - public void close() throws Exception { - // TODO Auto-generated method stub - - } - - @Override - public void reset() { - // TODO Auto-generated method stub - - } - - @Override - public double getRate() { - // TODO Auto-generated method stub - return 0; - } - - @Override - public double getAngle() { - // TODO Auto-generated method stub - return getShooterAngleDegrees(); - } - - @Override - public void calibrate() { - // TODO Auto-generated method stub - - } - }; - } -} diff --git a/src/main/java/frc4388/robot/subsystems/ShooterHood.java b/src/main/java/frc4388/robot/subsystems/ShooterHood.java deleted file mode 100644 index 94b37f0..0000000 --- a/src/main/java/frc4388/robot/subsystems/ShooterHood.java +++ /dev/null @@ -1,107 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc4388.robot.subsystems; - -import com.revrobotics.CANDigitalInput; -import com.revrobotics.CANEncoder; -import com.revrobotics.CANPIDController; -import com.revrobotics.CANSparkMax; -import com.revrobotics.ControlType; -import com.revrobotics.CANDigitalInput.LimitSwitchPolarity; -import com.revrobotics.CANSparkMax.IdleMode; -import com.revrobotics.CANSparkMax.SoftLimitDirection; -import com.revrobotics.CANSparkMaxLowLevel.MotorType; - -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc4388.robot.Constants.ShooterConstants; -import frc4388.utility.Gains; - -public class ShooterHood extends SubsystemBase { - public Shooter m_shooterSubsystem; - - public CANSparkMax m_angleAdjustMotor = new CANSparkMax(ShooterConstants.SHOOTER_ANGLE_ADJUST_ID, MotorType.kBrushless); - - public static Gains m_angleAdjustGains = ShooterConstants.SHOOTER_ANGLE_GAINS; - - public CANPIDController m_angleAdjustPIDController = m_angleAdjustMotor.getPIDController(); - public CANEncoder m_angleEncoder = m_angleAdjustMotor.getEncoder(); - - public boolean m_isHoodReady = false; - - public double m_fireAngle; - public CANDigitalInput m_hoodUpLimit, m_hoodDownLimit; - - /** - * Creates a new ShooterHood. - */ - public ShooterHood() { - m_angleAdjustMotor.setIdleMode(IdleMode.kBrake); - - m_hoodUpLimit = m_angleAdjustMotor.getForwardLimitSwitch(LimitSwitchPolarity.kNormallyOpen); - m_hoodDownLimit = m_angleAdjustMotor.getReverseLimitSwitch(LimitSwitchPolarity.kNormallyOpen); - m_hoodUpLimit.enableLimitSwitch(true); - m_hoodDownLimit.enableLimitSwitch(true); - - //m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kForward, true); - //m_angleAdjustMotor.enableSoftLimit(SoftLimitDirection.kReverse, true); - //m_angleAdjustMotor.setSoftLimit(SoftLimitDirection.kForward, ShooterConstants.HOOD_UP_SOFT_LIMIT); - //m_angleAdjustMotor.setSoftLimit(SoftLimitDirection.kReverse, ShooterConstants.HOOD_DOWN_SOFT_LIMIT); - } - - @Override - public void periodic() { - // This method will be called once per scheduler run - SmartDashboard.putNumber("Fire Angle CSV", m_fireAngle); - - SmartDashboard.putNumber("Hood Angle Raw", getAnglePositionDegrees()); - } - - /** - * Passes subsystem needed. - * @param subsystem Subsystem needed. - */ - public void passRequiredSubsystem(Shooter subsystem) { - m_shooterSubsystem = subsystem; - } - - public double addFireAngle() { - return m_fireAngle; - } - - /* Angle Adjustment PID Control */ - public void runAngleAdjustPID(double targetAngle) - { - // Set PID Coefficients - m_angleAdjustPIDController.setP(m_angleAdjustGains.m_kP); - m_angleAdjustPIDController.setI(m_angleAdjustGains.m_kI); - m_angleAdjustPIDController.setD(m_angleAdjustGains.m_kD); - m_angleAdjustPIDController.setIZone(m_angleAdjustGains.m_kIzone); - m_angleAdjustPIDController.setFF(m_angleAdjustGains.m_kF); - m_angleAdjustPIDController.setOutputRange(ShooterConstants.SHOOTER_TURRET_MIN, m_angleAdjustGains.m_kPeakOutput); - - m_angleAdjustPIDController.setReference(targetAngle, ControlType.kPosition); - } - - public void runHood(double input) - { - m_angleAdjustMotor.set(input); - } - - public void resetGyroAngleAdj(){ - m_angleEncoder.setPosition(0); -} - - public double getAnglePosition(){ - return m_angleEncoder.getPosition(); - } - - public double getAnglePositionDegrees(){ - return ((m_angleEncoder.getPosition() - ShooterConstants.HOOD_MOTOR_POS_AT_ZERO_ROT) * 360/ShooterConstants.HOOD_MOTOR_ROTS_PER_ROT) - 90; - } -} diff --git a/src/main/java/frc4388/robot/subsystems/Storage.java b/src/main/java/frc4388/robot/subsystems/Storage.java deleted file mode 100644 index 98bc3b3..0000000 --- a/src/main/java/frc4388/robot/subsystems/Storage.java +++ /dev/null @@ -1,144 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2019 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - -package frc4388.robot.subsystems; - -import com.revrobotics.CANEncoder; -import com.revrobotics.CANPIDController; -import com.revrobotics.CANSparkMax; -import com.revrobotics.CANSparkMaxLowLevel.MotorType; -import com.revrobotics.ControlType; - -import edu.wpi.first.wpilibj.DigitalInput; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc4388.robot.Constants.StorageConstants; -import frc4388.utility.Gains; - -public class Storage extends SubsystemBase { - public CANSparkMax m_storageMotor = new CANSparkMax(StorageConstants.STORAGE_CAN_ID, MotorType.kBrushless); - private DigitalInput m_beamShooter = new DigitalInput(StorageConstants.BEAM_SENSOR_SHOOTER); - private DigitalInput m_beamUseless = new DigitalInput(StorageConstants.BEAM_SENSOR_USELESS); - private DigitalInput m_beamStorage = new DigitalInput(StorageConstants.BEAM_SENSOR_STORAGE); - private DigitalInput m_beamIntake = new DigitalInput(StorageConstants.BEAM_SENSOR_INTAKE); - - CANPIDController m_storagePIDController = m_storageMotor.getPIDController(); - - CANEncoder m_encoder = m_storageMotor.getEncoder(); - - Gains storageGains = StorageConstants.STORAGE_GAINS; - - Intake m_intake; - - public boolean m_isStorageReadyToFire = false; - - public enum StorageMode{IDLE, INTAKE, RESET, MANUAL}; - public StorageMode m_storageMode = StorageMode.IDLE; - - /** - * Creates a new Storage. - */ - public Storage() { - resetEncoder(); - - // Set PID Coefficients - m_storagePIDController.setP(storageGains.m_kP); - m_storagePIDController.setI(storageGains.m_kI); - m_storagePIDController.setD(storageGains.m_kD); - m_storagePIDController.setIZone(storageGains.m_kIzone); - m_storagePIDController.setFF(storageGains.m_kF); - m_storagePIDController.setOutputRange(storageGains.m_kminOutput, storageGains.m_kmaxOutput); - } - - @Override - public void periodic() { - SmartDashboard.putBoolean("Intake Beam", getBeamIntake()); - SmartDashboard.putBoolean("Storage Beam", getBeamStorage()); - SmartDashboard.putBoolean("Upper Beam", getBeamUseless()); - SmartDashboard.putBoolean("Shooter Beam", getBeamShooter()); - } - - /** - * Runs storage motor - * - * @param input the percent output to run motor at - */ - public void runStorage(double input) { - m_storageMotor.set(input); - } - - public void resetEncoder(){ - m_encoder.setPosition(0); - } - - /** - * Runs Storage to a particular position - * @param targetPos in inches - */ - public void runStoragePositionPID(double targetPos){ - //SmartDashboard.putNumber("Storage Position PID Target", targetPos); - //SmartDashboard.putNumber("Storage Position Pos", getEncoderPos()); - targetPos = InchesToMotorRots(targetPos); - m_storagePIDController.setReference(targetPos, ControlType.kPosition); - } - - /** - * Runs Storage to a particular position - * @param position in motor rotations - */ - public void setStoragePID(double position){ - m_storagePIDController.setReference(position, ControlType.kPosition); - } - - public double getEncoderPos(){ - return m_encoder.getPosition(); - } - - public double getEncoderPosInches(){ - return motorRotsToInches(getEncoderPos()); - } - - public double getEncoderVel(){ - return m_encoder.getVelocity(); - } - - /** - * @param motorRots - * @return inches - */ - public double motorRotsToInches(double motorRots) { - return motorRots * (1/StorageConstants.MOTOR_ROTS_PER_STORAGE_ROT) * (StorageConstants.INCHES_PER_STORAGE_ROT); - } - - /** - * @param inches - * @return motorRots - */ - public double InchesToMotorRots(double inches) { - return inches * (1/StorageConstants.INCHES_PER_STORAGE_ROT) * (StorageConstants.MOTOR_ROTS_PER_STORAGE_ROT); - } - - public boolean getBeamShooter(){ - return m_beamShooter.get(); - } - - public boolean getBeamUseless(){ - return m_beamUseless.get(); - } - - public boolean getBeamStorage(){ - return m_beamStorage.get(); - } - - public boolean getBeamIntake(){ - return m_beamIntake.get(); - } - - public void changeStorageMode(StorageMode storageMode){ - m_storageMode = storageMode; - } -} diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java index 19448bc..cd4f63e 100644 --- a/src/main/java/frc4388/robot/subsystems/Vision.java +++ b/src/main/java/frc4388/robot/subsystems/Vision.java @@ -7,52 +7,34 @@ package frc4388.robot.subsystems; import java.util.ArrayList; -import java.util.List; import org.opencv.core.Point; -import org.photonvision.PhotonCamera; -import org.photonvision.common.hardware.VisionLEDMode; -import org.photonvision.targeting.PhotonPipelineResult; -import org.photonvision.targeting.PhotonTrackedTarget; -import org.photonvision.targeting.TargetCorner; +import edu.wpi.first.networktables.NetworkTableInstance; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc4388.robot.Constants.VisionConstants; public class Vision extends SubsystemBase { - private PhotonCamera m_camera; public Vision() { - m_camera = new PhotonCamera(VisionConstants.NAME); + // TODO } public ArrayList getTargetPoints() { - PhotonPipelineResult result = m_camera.getLatestResult(); - - if(!result.hasTargets()) + if(NetworkTableInstance.getDefault().getTable("limelight").getEntry("tv").getDouble(0) != 1) return null; ArrayList points = new ArrayList<>(); - for(PhotonTrackedTarget target : result.getTargets()) { - List corners = target.getCorners(); - - double centerY = 0; - for(TargetCorner corner : corners) { - centerY += corner.y; - } - centerY /= corners.size(); - - for(TargetCorner corner : corners) { - if(corner.y <= centerY) - points.add(new Point(corner.x, corner.y)); - } + double[] corners = NetworkTableInstance.getDefault().getTable("limelight").getEntry("tcornxy").getDoubleArray(new double[0]); + for(int i = 0; i < corners.length; i += 2*2) { + points.add(new Point(corners[i], corners[i+1])); } return points; } public void setLEDs(boolean on) { - m_camera.setLED(on ? VisionLEDMode.kOn : VisionLEDMode.kOff); + NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(on ? 0 : 1); + NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(on ? 3 : 1); } } \ No newline at end of file