From 3d1bcc05056522482e9dc18eace80f9144c220d8 Mon Sep 17 00:00:00 2001 From: im-a-robo Date: Tue, 29 Mar 2022 21:27:20 -0400 Subject: [PATCH 1/2] PROG-234 working 5 ball better this time --- .../auto/paths/FiveBallTerminalStopping.java | 5 +- .../commands/indexer/AutoIndexCargo.java | 88 ++++++++++++++----- .../voidrobot/constants/Constants.java | 4 +- .../voidrobot/subsystems/HubTargeting.java | 2 - 4 files changed, 69 insertions(+), 30 deletions(-) diff --git a/src/main/java/com/lightningrobotics/voidrobot/commands/auto/paths/FiveBallTerminalStopping.java b/src/main/java/com/lightningrobotics/voidrobot/commands/auto/paths/FiveBallTerminalStopping.java index 9502e5c..976e453 100644 --- a/src/main/java/com/lightningrobotics/voidrobot/commands/auto/paths/FiveBallTerminalStopping.java +++ b/src/main/java/com/lightningrobotics/voidrobot/commands/auto/paths/FiveBallTerminalStopping.java @@ -33,7 +33,7 @@ public FiveBallTerminalStopping(Drivetrain drivetrain, Indexer indexer, Intake i new InstantCommand(() -> targeting.setState(0)), new AimTurret(turret, targeting), new AutonIntake(intake), - // new TimedCommand(new AutonDeployIntake(intake), 0.75d), + new TimedCommand(new AutonDeployIntake(intake), 0.75d), new SequentialCommandGroup( new InstantCommand(indexer::initializeBallsHeld), @@ -45,7 +45,8 @@ public FiveBallTerminalStopping(Drivetrain drivetrain, Indexer indexer, Intake i new SequentialCommandGroup( new AutonVisionShooting(shooter, hood, indexer, targeting, 3d, 0.2d, 200d), new InstantCommand(() -> targeting.setState(1)), - new TimedCommand(new AutonIndexeCargo(indexer, 2), start5Ball.getDuration(drivetrain)) + new TimedCommand(new AutonIndexeCargo(indexer, 1), start5Ball.getDuration(drivetrain) - (start5Ball.getDuration(drivetrain) / 2)), + new TimedCommand(new AutonIndexeCargo(indexer, 1), start5Ball.getDuration(drivetrain) - (start5Ball.getDuration(drivetrain) / 2)) ) ), new InstantCommand(drivetrain::stop), diff --git a/src/main/java/com/lightningrobotics/voidrobot/commands/indexer/AutoIndexCargo.java b/src/main/java/com/lightningrobotics/voidrobot/commands/indexer/AutoIndexCargo.java index d018a34..e5b2d6a 100644 --- a/src/main/java/com/lightningrobotics/voidrobot/commands/indexer/AutoIndexCargo.java +++ b/src/main/java/com/lightningrobotics/voidrobot/commands/indexer/AutoIndexCargo.java @@ -1,22 +1,29 @@ package com.lightningrobotics.voidrobot.commands.indexer; +import javax.swing.plaf.basic.BasicTreeUI.TreeCancelEditingAction; + +import com.lightningrobotics.voidrobot.constants.Constants; import com.lightningrobotics.voidrobot.subsystems.Indexer; -import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.math.filter.Debouncer; import edu.wpi.first.wpilibj2.command.CommandBase; public class AutoIndexCargo extends CommandBase { // Creates our indexer subsystem private final Indexer indexer; + private boolean colorProximity = false; - private double indexTimeBall1 = 0.35d; // 0.185d; // The time we want the indexer to index in seconds - private double indexTimeBall2 = 0.275d; // The time we want the indexer to index in seconds - private double startIndexTime = 0d; // Setting a default start time of 0 - - private double power = 1; // the power we want the indexer to run at + public enum State{ + WAITING0, + COLLECT1, + WAITING1, + WAITING2, + COLLECT2PART1, + COLLECT2PART2, + } - private boolean isStopped = false; + State state = State.WAITING1; public AutoIndexCargo(Indexer indexer) { this.indexer = indexer; @@ -25,28 +32,61 @@ public AutoIndexCargo(Indexer indexer) { } @Override - public void initialize() {} + public void initialize() { + state = indexer.getBallCount() == 0 ? State.WAITING0 : (indexer.getBallCount() == 1 ? State.WAITING1 : State.WAITING2); + } @Override public void execute() { + colorProximity = indexer.getColorSensor().getProximity() > 350; - if (indexer.getCollectedBall()) { // && indexer.getAutoIndex() - startIndexTime = Timer.getFPGATimestamp(); - } + switch (state) { + case WAITING0: + indexer.stop(); + if (indexer.getCollectedBall()) { + state = State.COLLECT1; + } + break; - if(indexer.getBallCount() == 1 && Timer.getFPGATimestamp() - startIndexTime < indexTimeBall1) { - indexer.setPower(power); - isStopped = false; - } - else if(indexer.getBallCount() == 2 && Timer.getFPGATimestamp() - startIndexTime < indexTimeBall2 && indexer.getBallCount() != 2) { // Checks to see if we have reached the amount of time we want to index, then stops - indexer.setPower(power); - isStopped = false; - } - else if(!isStopped){ - System.out.println("AUTO INDEX STOPPED_____________________________"); - indexer.setPower(0); - isStopped = true; - } + case COLLECT1: + indexer.setPower(Constants.DEFAULT_INDEXER_POWER); + if (colorProximity) { + state = State.WAITING1; + } + break; + + case COLLECT2PART1: + indexer.setPower(Constants.DEFAULT_INDEXER_POWER); + if (!colorProximity) { + state = State.COLLECT2PART2; + } + break; + + case COLLECT2PART2: + indexer.setPower(Constants.DEFAULT_INDEXER_POWER); + if (colorProximity) { + state = State.WAITING2; + } + break; + + case WAITING1: + indexer.stop(); + if (indexer.getCollectedBall()) { + state = State.COLLECT2PART1; + } else if (indexer.getBallCount() == 0) { + state = State.WAITING0; + } + break; + + case WAITING2: + indexer.stop(); + if (indexer.getBallCount() == 1) { + state = State.WAITING1; + } else if (indexer.getBallCount() == 0) { + state = State.WAITING0; + } + break; + } } @Override diff --git a/src/main/java/com/lightningrobotics/voidrobot/constants/Constants.java b/src/main/java/com/lightningrobotics/voidrobot/constants/Constants.java index fee40a9..5028fe7 100644 --- a/src/main/java/com/lightningrobotics/voidrobot/constants/Constants.java +++ b/src/main/java/com/lightningrobotics/voidrobot/constants/Constants.java @@ -117,10 +117,10 @@ public final class Constants { { put(-135d, 150d); put(-90d, 150d); - put(-45d, 250d); + put(-45d, 200d); put(-15d, 0d); put(15d, 0d); - put(45d, 250d); + put(45d, 200d); put(90d, 150d); put(135d, 150d); } diff --git a/src/main/java/com/lightningrobotics/voidrobot/subsystems/HubTargeting.java b/src/main/java/com/lightningrobotics/voidrobot/subsystems/HubTargeting.java index 72dc2f0..69904fe 100644 --- a/src/main/java/com/lightningrobotics/voidrobot/subsystems/HubTargeting.java +++ b/src/main/java/com/lightningrobotics/voidrobot/subsystems/HubTargeting.java @@ -239,9 +239,7 @@ private void snapshot() { if (Timer.getFPGATimestamp() - lastVisionSnapshot > Constants.SNAPSHOT_DELAY) { lastVisionSnapshot = Timer.getFPGATimestamp(); visionSnapshotEntry.setNumber(1); - System.out.println("Logging Snapshot Taken"); visionSnapshotEntry.setNumber(0); - } } } From aaccedbd358147dc26279437867a5b09dc06ace3 Mon Sep 17 00:00:00 2001 From: im-a-robo Date: Tue, 29 Mar 2022 21:46:14 -0400 Subject: [PATCH 2/2] PROG-234 cleaned and added 1 and 2 ball --- .../voidrobot/RobotContainer.java | 10 +-- .../auto/commands/AutonShootCargo.java | 73 ------------------- .../auto/commands/AutonShootCargoVision.java | 61 ---------------- .../auto/commands/AutonVisionShooting.java | 1 - ...nalStopping.java => FiveBallTerminal.java} | 9 +-- .../auto/paths/FiveBallTerminalVision.java | 59 --------------- .../commands/auto/paths/OneBall.java | 28 +++---- .../auto/paths/ThreeBallTerminal.java | 66 ----------------- .../auto/paths/ThreeBallTerminalVision.java | 42 ----------- .../commands/auto/paths/TwoBall.java | 35 ++++----- 10 files changed, 31 insertions(+), 353 deletions(-) delete mode 100644 src/main/java/com/lightningrobotics/voidrobot/commands/auto/commands/AutonShootCargo.java delete mode 100644 src/main/java/com/lightningrobotics/voidrobot/commands/auto/commands/AutonShootCargoVision.java rename src/main/java/com/lightningrobotics/voidrobot/commands/auto/paths/{FiveBallTerminalStopping.java => FiveBallTerminal.java} (87%) delete mode 100644 src/main/java/com/lightningrobotics/voidrobot/commands/auto/paths/FiveBallTerminalVision.java delete mode 100644 src/main/java/com/lightningrobotics/voidrobot/commands/auto/paths/ThreeBallTerminal.java delete mode 100644 src/main/java/com/lightningrobotics/voidrobot/commands/auto/paths/ThreeBallTerminalVision.java diff --git a/src/main/java/com/lightningrobotics/voidrobot/RobotContainer.java b/src/main/java/com/lightningrobotics/voidrobot/RobotContainer.java index 96a279f..a3fb1c6 100644 --- a/src/main/java/com/lightningrobotics/voidrobot/RobotContainer.java +++ b/src/main/java/com/lightningrobotics/voidrobot/RobotContainer.java @@ -8,10 +8,8 @@ import com.lightningrobotics.common.util.filter.JoystickFilter; import com.lightningrobotics.common.util.filter.JoystickFilter.Mode; import com.lightningrobotics.voidrobot.commands.ZeroTurretHood; -import com.lightningrobotics.voidrobot.commands.auto.paths.FiveBallTerminalStopping; +import com.lightningrobotics.voidrobot.commands.auto.paths.FiveBallTerminal; import com.lightningrobotics.voidrobot.commands.auto.paths.OneBall; -import com.lightningrobotics.voidrobot.commands.auto.paths.ThreeBallTerminal; -import com.lightningrobotics.voidrobot.commands.auto.paths.ThreeBallTerminalVision; import com.lightningrobotics.voidrobot.commands.auto.paths.TwoBall; import com.lightningrobotics.voidrobot.commands.climber.runClimb; import com.lightningrobotics.voidrobot.commands.hood.ResetHood; @@ -68,10 +66,8 @@ protected void configureAutonomousCommands() { try { Autonomous.register("Taxi", new Path("1-2Ball.path", false).getCommand(drivetrain)); Autonomous.register("2 Ball", new TwoBall(drivetrain, shooter, hood, turret, indexer, intake, targeting)); - Autonomous.register("1 Ball", new OneBall(drivetrain, shooter, hood, turret, indexer, intake, targeting)); - Autonomous.register("3 Ball Terminal Vision", new ThreeBallTerminalVision(drivetrain, indexer, intake, shooter, hood, turret, targeting)); - // Autonomous.register("3 Ball Terminal", new ThreeBallTerminal(drivetrain, indexer, intake, shooter, hood, turret, targeting)); - Autonomous.register("5 Ball Terminal Vision", new FiveBallTerminalStopping(drivetrain, indexer, intake, shooter, hood, turret, targeting)); + Autonomous.register("1 Ball", new OneBall(drivetrain, shooter, hood, turret, indexer, intake, targeting)); + Autonomous.register("5 Ball Terminal", new FiveBallTerminal(drivetrain, indexer, intake, shooter, hood, turret, targeting)); } catch (Exception e) { System.err.println("I did an oopsie."); e.printStackTrace(); diff --git a/src/main/java/com/lightningrobotics/voidrobot/commands/auto/commands/AutonShootCargo.java b/src/main/java/com/lightningrobotics/voidrobot/commands/auto/commands/AutonShootCargo.java deleted file mode 100644 index 63b7c83..0000000 --- a/src/main/java/com/lightningrobotics/voidrobot/commands/auto/commands/AutonShootCargo.java +++ /dev/null @@ -1,73 +0,0 @@ -package com.lightningrobotics.voidrobot.commands.auto.commands; - -import com.lightningrobotics.voidrobot.constants.Constants; -import com.lightningrobotics.voidrobot.subsystems.Hood; -import com.lightningrobotics.voidrobot.subsystems.HubTargeting; -import com.lightningrobotics.voidrobot.subsystems.Indexer; -import com.lightningrobotics.voidrobot.subsystems.Shooter; -import com.lightningrobotics.voidrobot.subsystems.Turret; - -import edu.wpi.first.wpilibj2.command.CommandBase; - -public class AutonShootCargo extends CommandBase { - - private Shooter shooter; - private Hood hood; - private Indexer indexer; - private Turret turret; - private HubTargeting targeting; - - private double rpm; - private double hoodAngle; - private double turretAngle; - - public AutonShootCargo(Shooter shooter, Hood hood, Indexer indexer, Turret turret, HubTargeting targeting, double rpm, double hoodAngle, double turretAngle) { - this.shooter = shooter; - this.hood = hood; - this.indexer = indexer; - this.turret = turret; - this.targeting = targeting; - this.rpm = rpm; - this.hoodAngle = hoodAngle; - this.turretAngle = turretAngle; - - addRequirements(shooter, hood, indexer, turret); // not adding vision or turret as it is read only - - } - - @Override - public void execute() { - - shooter.setRPM(rpm); - hood.setAngle(hoodAngle); - turret.setAngle(turretAngle); - - if (targeting.onTarget(rpm, turretAngle, hoodAngle)) { - indexer.setPower(Constants.DEFAULT_INDEXER_POWER); - System.out.println("on target -------------------------------------------"); - - } else { - System.out.println("not on target -------------------------------------------"); - System.out.println("target RPM: " + rpm); - System.out.println("current RPM: " + shooter.getCurrentRPM()); - System.out.println("target Hood: " + hoodAngle); - System.out.println("Current hood: " + hood.getAngle()); - System.out.println("target Turret: " + turretAngle); - System.out.println("curent turret: " + turret.getCurrentAngle().getDegrees()); - System.out.println("hood limit switch" + hood.getLimitSwitch()); - - } - } - - @Override - public void end(boolean interrupted) { - // indexer.stop(); - // shooter.coast(); - } - - @Override - public boolean isFinished() { - return indexer.getEjectedBall(); - } - -} \ No newline at end of file diff --git a/src/main/java/com/lightningrobotics/voidrobot/commands/auto/commands/AutonShootCargoVision.java b/src/main/java/com/lightningrobotics/voidrobot/commands/auto/commands/AutonShootCargoVision.java deleted file mode 100644 index acc53a0..0000000 --- a/src/main/java/com/lightningrobotics/voidrobot/commands/auto/commands/AutonShootCargoVision.java +++ /dev/null @@ -1,61 +0,0 @@ -package com.lightningrobotics.voidrobot.commands.auto.commands; - -import com.lightningrobotics.voidrobot.constants.Constants; -import com.lightningrobotics.voidrobot.subsystems.Hood; -import com.lightningrobotics.voidrobot.subsystems.HubTargeting; -import com.lightningrobotics.voidrobot.subsystems.Indexer; -import com.lightningrobotics.voidrobot.subsystems.Shooter; - -import edu.wpi.first.wpilibj2.command.CommandBase; - -public class AutonShootCargoVision extends CommandBase { - - private Shooter shooter; - private Hood hood; - private Indexer indexer; - private HubTargeting targeting; - - public AutonShootCargoVision(Shooter shooter, Hood hood, Indexer indexer, HubTargeting targeting) { - this.shooter = shooter; - this.indexer = indexer; - this.hood = hood; - this.targeting = targeting; - - addRequirements(shooter, hood, indexer); - - } - - @Override - public void initialize() {} - - @Override - public void execute() { - - var rpm = targeting.getTargetFlywheelRPM(); - var hoodAngle = targeting.getTargetHoodAngle(); - - System.out.println("wanted RPM " + rpm); - System.out.println("current RPM " + shooter.getCurrentRPM()); - - shooter.setRPM(rpm); - hood.setAngle(hoodAngle); - - if(targeting.onTarget()) { - indexer.setPower(Constants.DEFAULT_INDEXER_POWER); - } - - } - - @Override - public void end(boolean interrupted) { - shooter.coast(); - indexer.stop(); - hood.stop(); - } - - @Override - public boolean isFinished() { - return indexer.getEjectedBall(); - } - -} \ No newline at end of file diff --git a/src/main/java/com/lightningrobotics/voidrobot/commands/auto/commands/AutonVisionShooting.java b/src/main/java/com/lightningrobotics/voidrobot/commands/auto/commands/AutonVisionShooting.java index f5c7500..ef7401e 100644 --- a/src/main/java/com/lightningrobotics/voidrobot/commands/auto/commands/AutonVisionShooting.java +++ b/src/main/java/com/lightningrobotics/voidrobot/commands/auto/commands/AutonVisionShooting.java @@ -5,7 +5,6 @@ import com.lightningrobotics.voidrobot.subsystems.HubTargeting; import com.lightningrobotics.voidrobot.subsystems.Indexer; import com.lightningrobotics.voidrobot.subsystems.Shooter; -import com.lightningrobotics.voidrobot.subsystems.Turret; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.CommandBase; diff --git a/src/main/java/com/lightningrobotics/voidrobot/commands/auto/paths/FiveBallTerminalStopping.java b/src/main/java/com/lightningrobotics/voidrobot/commands/auto/paths/FiveBallTerminal.java similarity index 87% rename from src/main/java/com/lightningrobotics/voidrobot/commands/auto/paths/FiveBallTerminalStopping.java rename to src/main/java/com/lightningrobotics/voidrobot/commands/auto/paths/FiveBallTerminal.java index 976e453..70220f2 100644 --- a/src/main/java/com/lightningrobotics/voidrobot/commands/auto/paths/FiveBallTerminalStopping.java +++ b/src/main/java/com/lightningrobotics/voidrobot/commands/auto/paths/FiveBallTerminal.java @@ -1,7 +1,5 @@ package com.lightningrobotics.voidrobot.commands.auto.paths; -import javax.sound.midi.MidiEvent; - import com.lightningrobotics.common.auto.Path; import com.lightningrobotics.common.command.core.TimedCommand; import com.lightningrobotics.voidrobot.commands.auto.commands.AutonDeployIntake; @@ -9,17 +7,14 @@ import com.lightningrobotics.voidrobot.commands.auto.commands.AutonIntake; import com.lightningrobotics.voidrobot.commands.auto.commands.AutonVisionShooting; import com.lightningrobotics.voidrobot.commands.turret.AimTurret; -import com.lightningrobotics.voidrobot.commands.auto.commands.AutonShootCargo; import com.lightningrobotics.voidrobot.subsystems.*; -import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; -import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -public class FiveBallTerminalStopping extends ParallelCommandGroup { +public class FiveBallTerminal extends ParallelCommandGroup { private static Path start5Ball = new Path("Start5Ball.path", false); @@ -28,7 +23,7 @@ public class FiveBallTerminalStopping extends ParallelCommandGroup { // private static Path oneMeter = new Path("1Meter.path", false); - public FiveBallTerminalStopping(Drivetrain drivetrain, Indexer indexer, Intake intake, Shooter shooter, Hood hood, Turret turret, HubTargeting targeting) throws Exception { + public FiveBallTerminal(Drivetrain drivetrain, Indexer indexer, Intake intake, Shooter shooter, Hood hood, Turret turret, HubTargeting targeting) throws Exception { super( new InstantCommand(() -> targeting.setState(0)), new AimTurret(turret, targeting), diff --git a/src/main/java/com/lightningrobotics/voidrobot/commands/auto/paths/FiveBallTerminalVision.java b/src/main/java/com/lightningrobotics/voidrobot/commands/auto/paths/FiveBallTerminalVision.java deleted file mode 100644 index 59ca786..0000000 --- a/src/main/java/com/lightningrobotics/voidrobot/commands/auto/paths/FiveBallTerminalVision.java +++ /dev/null @@ -1,59 +0,0 @@ -package com.lightningrobotics.voidrobot.commands.auto.paths; - -import com.lightningrobotics.common.auto.Path; -import com.lightningrobotics.common.command.core.TimedCommand; -import com.lightningrobotics.voidrobot.commands.auto.commands.AutonIndexeCargo; -import com.lightningrobotics.voidrobot.commands.auto.commands.AutonIntake; -import com.lightningrobotics.voidrobot.commands.auto.commands.AutonVisionShooting; -import com.lightningrobotics.voidrobot.commands.turret.AimTurret; -import com.lightningrobotics.voidrobot.commands.auto.commands.AutonShootCargo; -import com.lightningrobotics.voidrobot.subsystems.*; - -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; - -public class FiveBallTerminalVision extends ParallelCommandGroup { - - // private static Path terminal3Ball = new Path("3BallTerminal.path", false); - private static Path terminal5Ball = new Path("4-5BallTerminal.path", false); - private static Path endTerminal5Ball = new Path("End5Ball.path", true); - - public FiveBallTerminalVision(Drivetrain drivetrain, Indexer indexer, Intake intake, Shooter shooter, Hood hood, Turret turret, HubTargeting targeting) throws Exception { - super( - - new AimTurret(turret, targeting), - - new SequentialCommandGroup( - - new ParallelDeadlineGroup( - terminal5Ball.getCommand(drivetrain), - new AutonIntake(intake), - // new TimedCommand(new AutonDeployIntake(intake), 0.75d), - new InstantCommand(indexer::initializeBallsHeld), - new InstantCommand(hood::zero), - - new SequentialCommandGroup( - new AutonVisionShooting(shooter, hood, indexer, targeting, 0d, 0.2d, 200d), - - new AutonVisionShooting(shooter, hood, indexer, targeting, 10d, 1d, 0d), - new AutonVisionShooting(shooter, hood, indexer, targeting, 10d, 3d, 0d) - - // new AutonIndexeCargo(indexer) - ) - ), - - endTerminal5Ball.getCommand(drivetrain), - - new AutonVisionShooting(shooter, hood, indexer, targeting, 0d, 0.23d, 0d), - - new InstantCommand(indexer::stop), - new InstantCommand(shooter::coast), - new InstantCommand(hood::stop), - new InstantCommand(turret::stop) - - ) - ); - } -} \ No newline at end of file diff --git a/src/main/java/com/lightningrobotics/voidrobot/commands/auto/paths/OneBall.java b/src/main/java/com/lightningrobotics/voidrobot/commands/auto/paths/OneBall.java index ec456fe..64456b1 100644 --- a/src/main/java/com/lightningrobotics/voidrobot/commands/auto/paths/OneBall.java +++ b/src/main/java/com/lightningrobotics/voidrobot/commands/auto/paths/OneBall.java @@ -3,7 +3,8 @@ import com.lightningrobotics.common.auto.Path; import com.lightningrobotics.common.command.core.TimedCommand; import com.lightningrobotics.voidrobot.commands.auto.commands.AutonDeployIntake; -import com.lightningrobotics.voidrobot.commands.auto.commands.AutonShootCargoVision; +import com.lightningrobotics.voidrobot.commands.auto.commands.AutonIntake; +import com.lightningrobotics.voidrobot.commands.auto.commands.AutonVisionShooting; import com.lightningrobotics.voidrobot.subsystems.*; import edu.wpi.first.wpilibj2.command.InstantCommand; @@ -12,26 +13,21 @@ public class OneBall extends ParallelCommandGroup { - private static Path path = new Path("1-2Ball.path", false); + private static Path twoBallPath = new Path("1-2Ball.path", false); public OneBall(Drivetrain drivetrain, Shooter shooter, Hood hood, Turret turret, Indexer indexer, Intake intake, HubTargeting targeting) throws Exception { super( - new SequentialCommandGroup( - - new InstantCommand(() -> shooter.setPower(0.4)), - - // Set Initial Balls Held To 1 - new InstantCommand(indexer::initializeBallsHeld, indexer), - - // Deploy Intake + new AutonIntake(intake), new TimedCommand(new AutonDeployIntake(intake), 0.75d), - path.getCommand(drivetrain), - - // Shoot 2 (Preload & Collected) - new AutonShootCargoVision(shooter, hood, indexer, targeting) - - )); + new SequentialCommandGroup( + new InstantCommand(indexer::initializeBallsHeld), + + twoBallPath.getCommand(drivetrain), + + new AutonVisionShooting(shooter, hood, indexer, targeting, 0d, 0d, 0d) + ) + ); } } diff --git a/src/main/java/com/lightningrobotics/voidrobot/commands/auto/paths/ThreeBallTerminal.java b/src/main/java/com/lightningrobotics/voidrobot/commands/auto/paths/ThreeBallTerminal.java deleted file mode 100644 index 960a2c5..0000000 --- a/src/main/java/com/lightningrobotics/voidrobot/commands/auto/paths/ThreeBallTerminal.java +++ /dev/null @@ -1,66 +0,0 @@ -package com.lightningrobotics.voidrobot.commands.auto.paths; - -import com.lightningrobotics.common.auto.Path; -import com.lightningrobotics.common.command.core.TimedCommand; -import com.lightningrobotics.voidrobot.commands.auto.commands.AutonDeployIntake; -import com.lightningrobotics.voidrobot.commands.auto.commands.AutonIndexeCargo; -import com.lightningrobotics.voidrobot.commands.auto.commands.AutonIntake; -import com.lightningrobotics.voidrobot.commands.auto.commands.AutonShootCargoVision; -import com.lightningrobotics.voidrobot.commands.turret.AimTurret; -import com.lightningrobotics.voidrobot.commands.auto.commands.AutonShootCargo; -import com.lightningrobotics.voidrobot.subsystems.*; - -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; - -public class ThreeBallTerminal extends ParallelCommandGroup { - - private static Path start3Ball = new Path("Start3Ball.path", false); - private static Path end3Ball = new Path("End3Ball.path", false); - - public ThreeBallTerminal(Drivetrain drivetrain, Indexer indexer, Intake intake, Shooter shooter, Hood hood, Turret turret, HubTargeting targeting) throws Exception { - super( - - new AutonIntake(intake), - new TimedCommand(new AutonDeployIntake(intake), 0.75d), - - new SequentialCommandGroup( - - // shoots the first ball - new ParallelDeadlineGroup( - start3Ball.getCommand(drivetrain), - new SequentialCommandGroup( - new AutonShootCargo(shooter, hood, indexer, turret, targeting, 4000d, 0d, 20d), - new ParallelDeadlineGroup( - // new AutonIndexeCargo(indexer), - new AimTurret(turret, targeting) - ) - ) - ), - - new ParallelCommandGroup( - new AimTurret(turret, targeting), - - new SequentialCommandGroup( - new InstantCommand(() -> System.out.println("about to shoot ball two ----------------------------------------")), - - new TimedCommand(new AutonShootCargoVision(shooter, hood, indexer, targeting), 2), - - new ParallelDeadlineGroup( - end3Ball.getCommand(drivetrain) - // new AutonIndexeCargo(indexer) - ), - - new InstantCommand(() -> System.out.println("about to shoot ball three ----------------------------------------")), - - new TimedCommand(new AutonShootCargoVision(shooter, hood, indexer, targeting), 2), - - new InstantCommand(() -> System.out.println("we have ended ----------------------------------------------------")) // This line was written by Enoch - ) - ) - ) - ); - } -} \ No newline at end of file diff --git a/src/main/java/com/lightningrobotics/voidrobot/commands/auto/paths/ThreeBallTerminalVision.java b/src/main/java/com/lightningrobotics/voidrobot/commands/auto/paths/ThreeBallTerminalVision.java deleted file mode 100644 index ad80255..0000000 --- a/src/main/java/com/lightningrobotics/voidrobot/commands/auto/paths/ThreeBallTerminalVision.java +++ /dev/null @@ -1,42 +0,0 @@ -package com.lightningrobotics.voidrobot.commands.auto.paths; - -import com.lightningrobotics.common.auto.Path; -import com.lightningrobotics.voidrobot.commands.auto.commands.AutonIntake; -import com.lightningrobotics.voidrobot.commands.auto.commands.AutonVisionShooting; -import com.lightningrobotics.voidrobot.commands.auto.commands.AutonShootCargo; -import com.lightningrobotics.voidrobot.subsystems.*; - -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; - -public class ThreeBallTerminalVision extends ParallelCommandGroup { - - private static Path terminal3Ball = new Path("3BallTerminal.path", false); - - public ThreeBallTerminalVision(Drivetrain drivetrain, Indexer indexer, Intake intake, Shooter shooter, Hood hood, Turret turret, HubTargeting targeting) throws Exception { - super( - - new AutonIntake(intake), - // new TimedCommand(new AutonDeployIntake(intake), 0.75d), - new InstantCommand(indexer::initializeBallsHeld), - new InstantCommand(hood::zero), - - terminal3Ball.getCommand(drivetrain), - - new SequentialCommandGroup( - new AutonShootCargo(shooter, hood, indexer, turret, targeting, 3800d, 0d, 8d), - - // new AutonVisionShooting(shooter, hood, indexer, turret, targeting, 20d, 0.8d), - // new AutonVisionShooting(shooter, hood, indexer, turret, targeting, 10d, 1.2d), - - new SequentialCommandGroup( - new InstantCommand(indexer::stop), - new InstantCommand(shooter::coast), - new InstantCommand(hood::stop), - new InstantCommand(turret::stop) - ) - ) - ); - } -} \ No newline at end of file diff --git a/src/main/java/com/lightningrobotics/voidrobot/commands/auto/paths/TwoBall.java b/src/main/java/com/lightningrobotics/voidrobot/commands/auto/paths/TwoBall.java index 3c4f3ce..89f0cb3 100644 --- a/src/main/java/com/lightningrobotics/voidrobot/commands/auto/paths/TwoBall.java +++ b/src/main/java/com/lightningrobotics/voidrobot/commands/auto/paths/TwoBall.java @@ -3,41 +3,34 @@ import com.lightningrobotics.common.auto.Path; import com.lightningrobotics.common.command.core.TimedCommand; import com.lightningrobotics.voidrobot.commands.auto.commands.AutonDeployIntake; +import com.lightningrobotics.voidrobot.commands.auto.commands.AutonIndexeCargo; import com.lightningrobotics.voidrobot.commands.auto.commands.AutonIntake; -import com.lightningrobotics.voidrobot.commands.auto.commands.AutonShootCargoVision; +import com.lightningrobotics.voidrobot.commands.auto.commands.AutonVisionShooting; import com.lightningrobotics.voidrobot.subsystems.*; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.ParallelCommandGroup; +import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; public class TwoBall extends ParallelCommandGroup { - private static Path path = new Path("1-2Ball.path", false); + private static Path twoBallPath = new Path("1-2Ball.path", false); public TwoBall(Drivetrain drivetrain, Shooter shooter, Hood hood, Turret turret, Indexer indexer, Intake intake, HubTargeting targeting) throws Exception { super( - - new SequentialCommandGroup( - - new InstantCommand(() -> shooter.setPower(0.4)), - - // Set Initial Balls Held To 1 - new InstantCommand(indexer::initializeBallsHeld, indexer), - - // Deploy Intake + new AutonIntake(intake), new TimedCommand(new AutonDeployIntake(intake), 0.75d), - // Run Path & Collect 1 - new ParallelCommandGroup( - new TimedCommand(new AutonIntake(intake), path.getDuration(drivetrain)+1), - path.getCommand(drivetrain) - ), - - // Shoot 2 (Preload & Collected) - new AutonShootCargoVision(shooter, hood, indexer, targeting) - - )); + new SequentialCommandGroup( + new InstantCommand(indexer::initializeBallsHeld), + new ParallelDeadlineGroup( + twoBallPath.getCommand(drivetrain), + new AutonIndexeCargo(indexer, 2) + ), + new AutonVisionShooting(shooter, hood, indexer, targeting, 0d, 0d, 0d) + ) + ); } }