diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index b65ee53..8d04785 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -40,7 +40,7 @@ import frc4388.robot.commands.RunClimberWithTriggers; import frc4388.robot.commands.RunExtenderOutIn; import frc4388.robot.commands.RunIntakeWithTriggers; import frc4388.robot.commands.ShooterVelocityControlPID; -import frc4388.robot.commands.StorageIntakeGroup; +import frc4388.robot.commands.StorageIntake; import frc4388.robot.subsystems.Drive; import frc4388.robot.subsystems.Intake; import frc4388.robot.subsystems.LED; @@ -57,6 +57,7 @@ import frc4388.robot.subsystems.LED; import frc4388.robot.commands.TrackTarget; import frc4388.robot.commands.StorageOutake; import frc4388.robot.commands.StoragePrepAim; +import frc4388.robot.commands.StoragePrepIntake; import frc4388.robot.subsystems.Camera; import frc4388.robot.subsystems.Leveler; import frc4388.robot.subsystems.LimeLight; @@ -113,8 +114,6 @@ public class RobotContainer { m_robotLeveler.setDefaultCommand(new RunLevelerWithJoystick(m_robotLeveler, getDriverController())); // moves the drum not m_robotShooter.setDefaultCommand(new RunCommand(() -> m_robotShooter.runDrumShooter(0), m_robotShooter)); - //turns limelight off - //m_robotLime.setDefaultCommand(new RunCommand(() -> m_robotLime.limeOff(), m_robotLime)); } @@ -145,10 +144,7 @@ public class RobotContainer { // shoots one ball new JoystickButton(getOperatorJoystick(), XboxController.LEFT_BUMPER_BUTTON) .whileHeld(new ShootFullGroup(m_robotShooter, m_robotShooterAim, m_robotStorage), false); - //.whenReleased(new RunCommand(() -> m_robotLime.limeOff())); - //.whenReleased(new RunCommand(() -> m_robotShooter.runDrumShooterVelocityPID(0, m_robotShooter.m_shooterFalcon.getSelectedSensorVelocity()))) - //.whenReleased(new RunCommand(() -> m_robotShooterAim.limeOff())) - //.whenReleased(new RunCommand(() -> m_robotShooterAim.runShooterWithInput(0))); + // extends or retracts the extender new JoystickButton(getOperatorJoystick(), XboxController.X_BUTTON) .whenPressed(new RunExtenderOutIn(m_robotIntake)); @@ -160,12 +156,12 @@ public class RobotContainer { // starts tracking target new JoystickButton(getOperatorJoystick(), XboxController.A_BUTTON) - .whileHeld(new HoldTarget(m_robotShooter, m_robotShooterAim)); - //.whenPressed(new StoragePrepAim(m_robotStorage)); + .whileHeld(new ShootPrepGroup(m_robotShooter, m_robotShooterAim, m_robotStorage)) + .whenReleased(new StoragePrepIntake(m_robotIntake, m_robotStorage)); //Prepares storage for intaking new JoystickButton(getOperatorJoystick(), XboxController.LEFT_TRIGGER_AXIS) - .whileHeld(new StorageIntakeGroup(m_robotIntake, m_robotStorage)); + .whileHeld(new StorageIntake(m_robotIntake, m_robotStorage)); //Runs storage to outtake new JoystickButton(getOperatorJoystick(), XboxController.RIGHT_TRIGGER_AXIS) diff --git a/src/main/java/frc4388/robot/commands/ShootFireGroup.java b/src/main/java/frc4388/robot/commands/ShootFireGroup.java index 855fc48..7d23c65 100644 --- a/src/main/java/frc4388/robot/commands/ShootFireGroup.java +++ b/src/main/java/frc4388/robot/commands/ShootFireGroup.java @@ -25,10 +25,10 @@ public class ShootFireGroup extends ParallelRaceGroup { */ public ShootFireGroup(Shooter m_shooter, ShooterAim m_shooterAim, Storage m_storage) { addCommands( - new RunCommand(() -> m_shooter.runDrumShooterVelocityPID(7000, m_shooter.m_shooterFalcon.getSelectedSensorVelocity())), + new RunCommand(() -> m_shooter.runDrumShooterVelocityPID(m_shooter.addFireVel(), m_shooter.m_shooterFalcon.getSelectedSensorVelocity())), new RunCommand(() -> m_shooter.runAngleAdjustPID(m_shooter.addFireAngle())), - new HoldTarget(m_shooter, m_shooterAim) - //new StorageRun(m_storage) + new HoldTarget(m_shooter, m_shooterAim), + new StorageRun(m_storage) ); } } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/commands/ShootPrepGroup.java b/src/main/java/frc4388/robot/commands/ShootPrepGroup.java index 5d60abb..7407365 100644 --- a/src/main/java/frc4388/robot/commands/ShootPrepGroup.java +++ b/src/main/java/frc4388/robot/commands/ShootPrepGroup.java @@ -26,8 +26,8 @@ public class ShootPrepGroup extends ParallelCommandGroup { public ShootPrepGroup(Shooter m_shooter, ShooterAim m_shooterAim, Storage m_storage) { addCommands( new TrackTarget(m_shooter, m_shooterAim), - new ShooterVelocityControlPID(m_shooter) - //new StoragePrepAim(m_storage) + new ShooterVelocityControlPID(m_shooter), + new StoragePrepAim(m_storage) ); } } diff --git a/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java b/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java index a2a1b77..e84813c 100644 --- a/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java +++ b/src/main/java/frc4388/robot/commands/ShooterVelocityControlPID.java @@ -33,7 +33,7 @@ public class ShooterVelocityControlPID extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - m_shooter.runDrumShooterVelocityPID(7000, m_shooter.m_shooterFalcon.getSelectedSensorVelocity()); + m_shooter.runDrumShooterVelocityPID(m_shooter.addFireVel(), m_shooter.m_shooterFalcon.getSelectedSensorVelocity()); m_shooter.runAngleAdjustPID(m_shooter.addFireAngle()); } diff --git a/src/main/java/frc4388/robot/commands/StorageIntake.java b/src/main/java/frc4388/robot/commands/StorageIntake.java index 30d73ce..b382a40 100644 --- a/src/main/java/frc4388/robot/commands/StorageIntake.java +++ b/src/main/java/frc4388/robot/commands/StorageIntake.java @@ -38,12 +38,11 @@ public class StorageIntake extends CommandBase { public void execute() { if (m_storage.getBeam(0)){ - m_storage.setStoragePID(m_storage.getEncoderPos() + StorageConstants.STORAGE_PARTIAL_BALL); - m_intake.runExtender(-IntakeConstants.EXTENDER_SPEED); + m_storage.runStorage(StorageConstants.STORAGE_SPEED); intook = true; } else{ - m_intake.runExtender(IntakeConstants.EXTENDER_SPEED); + m_storage.runStorage(0); } } @@ -55,7 +54,7 @@ public class StorageIntake extends CommandBase { // Returns true when the command should end. @Override public boolean isFinished() { - if (!m_storage.getBeam(0) && m_storage.getBeam(1) && intook){ + if (!m_storage.getBeam(0) && intook){ return true; } return false; diff --git a/src/main/java/frc4388/robot/commands/StorageIntakeFinal.java b/src/main/java/frc4388/robot/commands/StorageIntakeFinal.java deleted file mode 100644 index 7ac5f46..0000000 --- a/src/main/java/frc4388/robot/commands/StorageIntakeFinal.java +++ /dev/null @@ -1,47 +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 frc4388.robot.Constants.StorageConstants; -import frc4388.robot.subsystems.Storage; - -public class StorageIntakeFinal extends CommandBase { - Storage m_storage; - /** - * Creates a new StorageIntakeFinal. - */ - public StorageIntakeFinal(Storage subsystem) { - 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() { - if (m_storage.getBeam(1)){ - m_storage.setStoragePID(m_storage.getEncoderPos() + StorageConstants.STORAGE_FULL_BALL); - } - } - - // 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/StorageIntakeGroup.java b/src/main/java/frc4388/robot/commands/StorageIntakeGroup.java deleted file mode 100644 index 4636edc..0000000 --- a/src/main/java/frc4388/robot/commands/StorageIntakeGroup.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; - -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import frc4388.robot.subsystems.Intake; -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 StorageIntakeGroup extends SequentialCommandGroup { - /** - * Creates a new StorageIntakeGroup. - */ - public StorageIntakeGroup(Intake m_intake, Storage m_storage) { - addCommands( - new StoragePrepIntake(m_intake, m_storage), - new StorageIntake(m_intake, m_storage), - new StorageIntakeFinal(m_storage) - ); - } -} diff --git a/src/main/java/frc4388/robot/commands/StoragePrepAim.java b/src/main/java/frc4388/robot/commands/StoragePrepAim.java index 376a92b..7af0252 100644 --- a/src/main/java/frc4388/robot/commands/StoragePrepAim.java +++ b/src/main/java/frc4388/robot/commands/StoragePrepAim.java @@ -29,7 +29,7 @@ public class StoragePrepAim extends CommandBase { // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if (m_storage.getBeam(2) == false){ + if (m_storage.getBeam(1) == false){ m_storage.runStorage(StorageConstants.STORAGE_SPEED); } else{ @@ -45,7 +45,7 @@ public class StoragePrepAim extends CommandBase { // Returns true when the command should end. @Override public boolean isFinished() { - if (m_storage.getBeam(2)){ + if (m_storage.getBeam(1)){ return true; } return false; diff --git a/src/main/java/frc4388/robot/commands/StoragePrepIntake.java b/src/main/java/frc4388/robot/commands/StoragePrepIntake.java index 3321027..3a94353 100644 --- a/src/main/java/frc4388/robot/commands/StoragePrepIntake.java +++ b/src/main/java/frc4388/robot/commands/StoragePrepIntake.java @@ -15,6 +15,7 @@ import frc4388.robot.subsystems.Storage; public class StoragePrepIntake extends CommandBase { public Intake m_intake; public Storage m_storage; + public double startTime; /** * Creates a new StoragePrepIntake. @@ -29,12 +30,13 @@ public class StoragePrepIntake extends CommandBase { // Called when the command is initially scheduled. @Override public void initialize() { + startTime = System.currentTimeMillis(); } // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - if (m_storage.getBeam(1) == false){ + if (!m_storage.getBeam(0) && System.currentTimeMillis() < startTime + 3000){ //If the bottom beam is broken, or 3 seconds have passed m_storage.runStorage(-StorageConstants.STORAGE_SPEED); } else{ @@ -50,7 +52,7 @@ public class StoragePrepIntake extends CommandBase { // Returns true when the command should end. @Override public boolean isFinished() { - if (m_storage.getBeam(1)){ + if (m_storage.getBeam(0)){ return true; } return false; diff --git a/src/main/java/frc4388/robot/commands/TrackTarget.java b/src/main/java/frc4388/robot/commands/TrackTarget.java index 647341b..e7c3c08 100644 --- a/src/main/java/frc4388/robot/commands/TrackTarget.java +++ b/src/main/java/frc4388/robot/commands/TrackTarget.java @@ -73,7 +73,7 @@ public class TrackTarget extends CommandBase { } else if (turnAmount < 0 && turnAmount > -VisionConstants.MOTOR_DEAD_ZONE) { turnAmount = -VisionConstants.MOTOR_DEAD_ZONE; } - m_shooterAim.runShooterWithInput(turnAmount / 3); + m_shooterAim.runShooterWithInput(turnAmount); // Finding Distance distance = VisionConstants.TARGET_HEIGHT / Math.tan((VisionConstants.LIME_ANGLE + yAngle) * (Math.PI / 180)); diff --git a/src/main/java/frc4388/robot/subsystems/Storage.java b/src/main/java/frc4388/robot/subsystems/Storage.java index 5dbcc44..74b2484 100644 --- a/src/main/java/frc4388/robot/subsystems/Storage.java +++ b/src/main/java/frc4388/robot/subsystems/Storage.java @@ -98,10 +98,10 @@ public class Storage extends SubsystemBase { /* If pressing aim + run unti; hitting top beam + else Run until hitting bottom beam dont run intake if balls not at bottom - else - run unti; hitting top beam 2 beamms total */