diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 315aa66..104d757 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -173,7 +173,7 @@ public class RobotContainer { // runs the storage not //m_robotStorage.setDefaultCommand(new RunCommand(() -> m_robotStorage.runStorage(0), m_robotStorage)); m_robotStorage.setDefaultCommand(new ManageStorage(m_robotStorage)); - //m_robotLime.setDefaultCommand(new RunCommand(() -> m_robotLime.limeOff(), m_robotLime)); + m_robotLime.setDefaultCommand(new RunCommand(() -> m_robotLime.limeOff(), m_robotLime)); } /** @@ -233,7 +233,7 @@ public class RobotContainer { //.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(0.5), m_robotStorage)) + .whenPressed(new RunCommand(() -> m_robotStorage.runStorage(1), m_robotStorage)) .whenReleased(new InterruptSubystem(m_robotStorage)); // extends or retracts the extender @@ -254,7 +254,7 @@ public class RobotContainer { // starts tracking target new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) - .whileHeld(new TrackTarget(m_robotShooterAim)) + .whileHeld(new TrackTarget(m_robotShooterAim, m_robotLime)) .whileHeld(new RunCommand(() -> m_robotShooterHood.runAngleAdjustPID(m_robotShooterHood.addFireAngle()))) .whenReleased(new InstantCommand(() -> m_robotLime.limeOff())); @@ -274,15 +274,15 @@ public class RobotContainer { //Run drum - new JoystickManualButton(getOperatorJoystick(), XboxController.B_BUTTON, false) - .whileHeld(new ShootPrepGroup(m_robotShooter, m_robotShooterAim, m_robotShooterHood, m_robotStorage), false) + new JoystickButton(getOperatorJoystick(), XboxController.B_BUTTON) + .whileHeld(new ShootPrepGroup(m_robotShooter, m_robotShooterAim, m_robotShooterHood, m_robotStorage, m_robotLime), false) //.whenReleased(new ManageStorage(m_robotStorage, StorageMode.RESET)) .whenReleased(new InstantCommand(() -> m_robotLime.limeOff())); //Run drum manual - new JoystickManualButton(getOperatorJoystick(), XboxController.B_BUTTON, true) + /*new JoystickManualButton(getOperatorJoystick(), XboxController.B_BUTTON, true) .whileHeld(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(10000))) - .whenReleased(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(0))); + .whenReleased(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(0)));*/ @@ -356,7 +356,7 @@ public class RobotContainer { }; m_tenBallAutoMiddle = new TenBallAutoMiddle(m_robotShooterHood, m_robotStorage, m_robotIntake, m_robotShooter, - m_robotShooterAim, m_robotDrive,buildPaths(tenBallAutoMiddlePaths)); + m_robotShooterAim, m_robotDrive, m_robotLime, buildPaths(tenBallAutoMiddlePaths)); } /** diff --git a/src/main/java/frc4388/robot/commands/auto/TenBallAutoMiddle.java b/src/main/java/frc4388/robot/commands/auto/TenBallAutoMiddle.java index 28ae59c..b2bbc04 100644 --- a/src/main/java/frc4388/robot/commands/auto/TenBallAutoMiddle.java +++ b/src/main/java/frc4388/robot/commands/auto/TenBallAutoMiddle.java @@ -18,6 +18,7 @@ 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.LimeLight; import frc4388.robot.subsystems.Shooter; import frc4388.robot.subsystems.ShooterAim; import frc4388.robot.subsystems.ShooterHood; @@ -30,7 +31,7 @@ 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) { + public TenBallAutoMiddle(ShooterHood shooterHood, Storage storage, Intake intake, Shooter shooter, ShooterAim shooterAim, Drive drive, LimeLight m_limeLight, RamseteCommand[] paths) { // Add your commands in the super() call, e.g. // super(new FooCommand(), new BarCommand()); addCommands( @@ -46,10 +47,10 @@ public class TenBallAutoMiddle extends SequentialCommandGroup { new Wait(drive, 4, 0), new PrepChecker(shooter, shooterAim), new RunCommand(() -> intake.runExtender(IntakeConstants.EXTENDER_SPEED), intake), - new ShootPrepGroup(shooter, shooterAim, shooterHood, storage) + new ShootPrepGroup(shooter, shooterAim, shooterHood, storage, m_limeLight) ), new ParallelDeadlineGroup( - new ShootPrepGroup(shooter, shooterAim, shooterHood, storage), + new ShootPrepGroup(shooter, shooterAim, shooterHood, storage, m_limeLight), new RunStorage(storage) ) //paths[0], diff --git a/src/main/java/frc4388/robot/commands/shooter/ShootPrepGroup.java b/src/main/java/frc4388/robot/commands/shooter/ShootPrepGroup.java index df92eb4..0f15b7c 100644 --- a/src/main/java/frc4388/robot/commands/shooter/ShootPrepGroup.java +++ b/src/main/java/frc4388/robot/commands/shooter/ShootPrepGroup.java @@ -9,6 +9,7 @@ package frc4388.robot.commands.shooter; import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; import frc4388.robot.commands.storage.StoragePrep; +import frc4388.robot.subsystems.LimeLight; import frc4388.robot.subsystems.Shooter; import frc4388.robot.subsystems.ShooterAim; import frc4388.robot.subsystems.ShooterHood; @@ -24,9 +25,9 @@ public class ShootPrepGroup extends ParallelDeadlineGroup { * @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) { + public ShootPrepGroup(Shooter m_shooter, ShooterAim m_shooterAim, ShooterHood m_shooterHood, Storage m_storage, LimeLight m_limeLight) { super( - new TrackTarget(m_shooterAim), + new TrackTarget(m_shooterAim, m_limeLight), new ShooterVelocityControlPID(m_shooter), new HoodPositionPID(m_shooterHood) ); diff --git a/src/main/java/frc4388/robot/commands/shooter/TrackTarget.java b/src/main/java/frc4388/robot/commands/shooter/TrackTarget.java index 598c317..0fca6a5 100644 --- a/src/main/java/frc4388/robot/commands/shooter/TrackTarget.java +++ b/src/main/java/frc4388/robot/commands/shooter/TrackTarget.java @@ -39,24 +39,30 @@ public class TrackTarget extends CommandBase { public double m_hoodTrim; public double m_turretTrim; + LimeLight m_limeLight; + /** * Uses the Limelight to track the target * @param shooterSubsystem The Shooter subsystem * @param aimSubsystem The ShooterAim subsystem */ - public TrackTarget(ShooterAim aimSubsystem) { + public TrackTarget(ShooterAim aimSubsystem, LimeLight limeLight) { m_shooterAim = aimSubsystem; m_shooter = m_shooterAim.m_shooterSubsystem; m_shooterHood = m_shooter.m_shooterHoodSubsystem; + m_limeLight = limeLight; addRequirements(m_shooterAim); - NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(1); - NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(0); + //NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(1); + //NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(0); + m_limeLight.limeOff(); } // 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("camMode").setNumber(0); NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(3); } diff --git a/src/main/java/frc4388/robot/subsystems/LimeLight.java b/src/main/java/frc4388/robot/subsystems/LimeLight.java index d8e39a4..9ea2c96 100644 --- a/src/main/java/frc4388/robot/subsystems/LimeLight.java +++ b/src/main/java/frc4388/robot/subsystems/LimeLight.java @@ -20,7 +20,7 @@ public class LimeLight extends SubsystemBase { public void limeOff(){ NetworkTableInstance.getDefault().getTable("limelight").getEntry("camMode").setNumber(1); - NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(0); + NetworkTableInstance.getDefault().getTable("limelight").getEntry("ledMode").setNumber(1); } public void limeOn(){