From 3c73a85e96ad3142ed38ff71c461a3280a12b77d Mon Sep 17 00:00:00 2001 From: itamaroryan Date: Wed, 29 Nov 2023 17:49:15 +0200 Subject: [PATCH 1/3] Wrote amazing clean code --- .idea/misc.xml | 1 - src/main/java/edu/greenblitz/pegasus/OI.java | 1 - .../java/edu/greenblitz/pegasus/RobotMap.java | 4 -- .../commands/handleBalls/HandleBalls.java | 68 ------------------ .../EjectEnemyBallFromGripper.java | 14 ---- .../multiSystem/EjectFromShooter.java | 18 ----- .../multiSystem/InsertIntoShooter.java | 46 ------------ .../multiSystem/MoveBallUntilClick.java | 19 ----- .../pegasus/subsystems/Indexing.java | 72 ------------------- 9 files changed, 243 deletions(-) delete mode 100644 src/main/java/edu/greenblitz/pegasus/commands/handleBalls/HandleBalls.java delete mode 100644 src/main/java/edu/greenblitz/pegasus/commands/multiSystem/EjectEnemyBallFromGripper.java delete mode 100644 src/main/java/edu/greenblitz/pegasus/commands/multiSystem/EjectFromShooter.java delete mode 100644 src/main/java/edu/greenblitz/pegasus/commands/multiSystem/InsertIntoShooter.java delete mode 100644 src/main/java/edu/greenblitz/pegasus/commands/multiSystem/MoveBallUntilClick.java delete mode 100644 src/main/java/edu/greenblitz/pegasus/subsystems/Indexing.java diff --git a/.idea/misc.xml b/.idea/misc.xml index 5821b2fa..0303d116 100644 --- a/.idea/misc.xml +++ b/.idea/misc.xml @@ -1,4 +1,3 @@ - diff --git a/src/main/java/edu/greenblitz/pegasus/OI.java b/src/main/java/edu/greenblitz/pegasus/OI.java index e196e350..ad85383b 100644 --- a/src/main/java/edu/greenblitz/pegasus/OI.java +++ b/src/main/java/edu/greenblitz/pegasus/OI.java @@ -81,7 +81,6 @@ private void initRealButtons() { } private void initAmirButtons() { - //Indexing.getInstance().setDefaultCommand(new HandleBalls()); SwerveChassis.getInstance().setDefaultCommand(new CombineJoystickMovement(mainJoystick, false)); mainJoystick.Y.whenPressed(new SwerveCommand() { @Override diff --git a/src/main/java/edu/greenblitz/pegasus/RobotMap.java b/src/main/java/edu/greenblitz/pegasus/RobotMap.java index e86ac47d..58a1ed74 100644 --- a/src/main/java/edu/greenblitz/pegasus/RobotMap.java +++ b/src/main/java/edu/greenblitz/pegasus/RobotMap.java @@ -71,7 +71,6 @@ public static class ShooterMotor { public static class Funnel { public static final double POWER = 0.7; public static final double REVERSE_POWER = -0.7; - public static final int MACRO_SWITCH_PORT = 0; public static class FunnelMotor { public static final int MOTOR_PORT = 5; @@ -90,9 +89,6 @@ public static class PressureSensor { } } - public static class DigitalInputMap { - public static final int MACRO_SWITCH = 0; - } public static class Swerve { public static final double WHEEL_CIRC = 0.0517 * 2 * Math.PI; //very accurate right now diff --git a/src/main/java/edu/greenblitz/pegasus/commands/handleBalls/HandleBalls.java b/src/main/java/edu/greenblitz/pegasus/commands/handleBalls/HandleBalls.java deleted file mode 100644 index 0783dfd3..00000000 --- a/src/main/java/edu/greenblitz/pegasus/commands/handleBalls/HandleBalls.java +++ /dev/null @@ -1,68 +0,0 @@ -package edu.greenblitz.pegasus.commands.handleBalls; - -import edu.greenblitz.pegasus.commands.funnel.ReverseRunFunnel; -import edu.greenblitz.pegasus.commands.funnel.RunFunnel; -import edu.greenblitz.pegasus.commands.intake.IntakeCommand; -import edu.greenblitz.pegasus.commands.intake.roller.ReverseRunRoller; -import edu.greenblitz.pegasus.commands.shooter.ShooterEvacuate; -import edu.greenblitz.pegasus.subsystems.Indexing; -import edu.greenblitz.pegasus.utils.DigitalInputMap; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; -import edu.wpi.first.wpilibj2.command.WaitCommand; - -public class HandleBalls extends IntakeCommand { - - - /** - * this command checks if an enemy ball is trying to enter the Funnel, - * and if so it is rejecting it from entering the funnel system using a - * color sensor to check the color - * - * @see com.revrobotics.ColorSensorV3 colorSensor. - * @see Indexing Subsystem - */ - - private final Indexing index; - private boolean isBallInFunnel; - - public HandleBalls() { - this.index = Indexing.getInstance(); - isBallInFunnel = false; - require(Indexing.getInstance()); - } - - @Override - public void execute() { - SmartDashboard.putBoolean("isBallInFunnel", isBallInFunnel); - SmartDashboard.putBoolean("isEnemyBallUnSensor", index.isEnemyBallInSensor()); - SmartDashboard.putBoolean("isMacroSwitch", DigitalInputMap.getInstance().getValue(0)); - - if (index.isTeamsBallInSensor()) { - //if our team's ball is in front of the sensor activate the boolean - isBallInFunnel = true; - } - if (DigitalInputMap.getInstance().getValue(0)) { - //if the ball got to the macroSwitch then disable the boolean - isBallInFunnel = false; - } - - if (index.isEnemyBallInSensor()) { - if (DigitalInputMap.getInstance().getValue(0) || isBallInFunnel) { - isBallInFunnel = false; - SmartDashboard.putBoolean("isEvacuatingFromShooter", false); - //back direction - new ParallelDeadlineGroup( - new WaitCommand(0.5), - new ReverseRunRoller(), - new ReverseRunFunnel().raceWith(new WaitCommand(0.2)) - ).andThen(new RunFunnel().until(() -> DigitalInputMap.getInstance().getValue(0))).schedule(false); - } else { - //shooter evacuation - SmartDashboard.putBoolean("isEvacuatingFromShooter", true); - new ShooterEvacuate().raceWith(new WaitCommand(5)).schedule(false); - - } - } - } -} diff --git a/src/main/java/edu/greenblitz/pegasus/commands/multiSystem/EjectEnemyBallFromGripper.java b/src/main/java/edu/greenblitz/pegasus/commands/multiSystem/EjectEnemyBallFromGripper.java deleted file mode 100644 index 052147a2..00000000 --- a/src/main/java/edu/greenblitz/pegasus/commands/multiSystem/EjectEnemyBallFromGripper.java +++ /dev/null @@ -1,14 +0,0 @@ -package edu.greenblitz.pegasus.commands.multiSystem; - -import edu.greenblitz.pegasus.commands.intake.roller.ReverseRunRoller; -import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; -import edu.wpi.first.wpilibj2.command.WaitCommand; - -public class EjectEnemyBallFromGripper extends ParallelRaceGroup { //todo can be deadline - public EjectEnemyBallFromGripper() { - addCommands( - new WaitCommand(1.5), - new ReverseRunRoller() - ); - } -} diff --git a/src/main/java/edu/greenblitz/pegasus/commands/multiSystem/EjectFromShooter.java b/src/main/java/edu/greenblitz/pegasus/commands/multiSystem/EjectFromShooter.java deleted file mode 100644 index 024edf15..00000000 --- a/src/main/java/edu/greenblitz/pegasus/commands/multiSystem/EjectFromShooter.java +++ /dev/null @@ -1,18 +0,0 @@ -package edu.greenblitz.pegasus.commands.multiSystem; - -import edu.greenblitz.pegasus.RobotMap; -import edu.greenblitz.pegasus.commands.funnel.ReverseRunFunnel; -import edu.greenblitz.pegasus.commands.intake.roller.ReverseRunRoller; -import edu.greenblitz.pegasus.utils.DigitalInputMap; -import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; -import edu.wpi.first.wpilibj2.command.WaitUntilCommand; - -public class EjectFromShooter extends ParallelRaceGroup {//todo delete - public EjectFromShooter() { - addCommands( - new WaitUntilCommand(() -> DigitalInputMap.getInstance().getValue(RobotMap.Pegasus.Funnel.MACRO_SWITCH_PORT)), - new ReverseRunFunnel(), - new ReverseRunRoller() - ); - } -} diff --git a/src/main/java/edu/greenblitz/pegasus/commands/multiSystem/InsertIntoShooter.java b/src/main/java/edu/greenblitz/pegasus/commands/multiSystem/InsertIntoShooter.java deleted file mode 100644 index 5278c3e4..00000000 --- a/src/main/java/edu/greenblitz/pegasus/commands/multiSystem/InsertIntoShooter.java +++ /dev/null @@ -1,46 +0,0 @@ -package edu.greenblitz.pegasus.commands.multiSystem; - -import edu.greenblitz.GBLib.src.main.java.edu.greenblitz.gblib.subsystems.shooter.Shooter; -import edu.greenblitz.pegasus.RobotMap; -import edu.greenblitz.pegasus.commands.funnel.RunFunnel; -import edu.greenblitz.pegasus.commands.intake.roller.RunRoller; -import edu.greenblitz.pegasus.utils.DigitalInputMap; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.WaitUntilCommand; - -public class InsertIntoShooter extends SequentialCommandGroup { - - private double startTime; - private boolean reported = false; - - // AKA InsertoShooter @tal935 - public InsertIntoShooter() { - addCommands( - new MoveBallUntilClick(), - - //waits until the shooter is ready - new WaitUntilCommand(() -> Shooter.getInstance().isPreparedToShoot()), - - new ParallelDeadlineGroup(//activates both roller and funnel until ball is no longer at macro switch (was probably propelled) - new WaitUntilCommand(() -> !DigitalInputMap.getInstance().getValue(RobotMap.Pegasus.Funnel.MACRO_SWITCH_PORT)), - new RunFunnel(), - new RunRoller() - )); - - } - - @Override - public void initialize() { - super.initialize(); - startTime = System.currentTimeMillis() / 1000.0; - } - - - @Override - public void end(boolean interrupted) { - super.end(interrupted); - reported = false; - } -} diff --git a/src/main/java/edu/greenblitz/pegasus/commands/multiSystem/MoveBallUntilClick.java b/src/main/java/edu/greenblitz/pegasus/commands/multiSystem/MoveBallUntilClick.java deleted file mode 100644 index 0ddf9320..00000000 --- a/src/main/java/edu/greenblitz/pegasus/commands/multiSystem/MoveBallUntilClick.java +++ /dev/null @@ -1,19 +0,0 @@ -package edu.greenblitz.pegasus.commands.multiSystem; - -import edu.greenblitz.GBLib.src.main.java.edu.greenblitz.gblib.base.GBCommand; -import edu.greenblitz.GBLib.src.main.java.edu.greenblitz.gblib.commands.DoUntilCommand; -import edu.greenblitz.pegasus.RobotMap; -import edu.greenblitz.pegasus.commands.funnel.RunFunnel; -import edu.greenblitz.pegasus.commands.intake.roller.RunRoller; -import edu.greenblitz.pegasus.utils.DigitalInputMap; -import edu.greenblitz.pegasus.utils.WaitCommand; -import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; -import edu.wpi.first.wpilibj2.command.WaitUntilCommand; - -public class MoveBallUntilClick extends ParallelDeadlineGroup { - - public MoveBallUntilClick() { - super(new WaitUntilCommand(() -> DigitalInputMap.getInstance().getValue(0)), new RunRoller(),new RunFunnel()); - } - -} \ No newline at end of file diff --git a/src/main/java/edu/greenblitz/pegasus/subsystems/Indexing.java b/src/main/java/edu/greenblitz/pegasus/subsystems/Indexing.java deleted file mode 100644 index a200c2af..00000000 --- a/src/main/java/edu/greenblitz/pegasus/subsystems/Indexing.java +++ /dev/null @@ -1,72 +0,0 @@ -package edu.greenblitz.pegasus.subsystems; - -import com.revrobotics.ColorSensorV3; -import edu.greenblitz.GBLib.src.main.java.edu.greenblitz.gblib.subsystems.GBSubsystem; -import edu.wpi.first.wpilibj.DriverStation; -import edu.wpi.first.wpilibj.I2C; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - -import java.awt.*; - - -public class Indexing extends GBSubsystem { - /** - * This class is in-charge of checking if an enemy ball is trying to get to the funnel (and eventually to the shooter) - * by using color sensor - * - * @see ColorSensorV3 colorSensorV3 - */ - - private static Indexing instance; - private ColorSensorV3 cs; - public DriverStation.Alliance alliance; - - private Indexing() { - cs = new ColorSensorV3(I2C.Port.kOnboard); - alliance = DriverStation.getAlliance(); - } - - public static Indexing getInstance() { - if (instance == null) { - instance = new Indexing(); - } - return instance; - } - - - /** - * @return the color that is in front of the sensor (as a DriverStation.Alliance enum) - */ - public DriverStation.Alliance getBallColor() { - float[] color = Color.RGBtoHSB(cs.getRed(), cs.getGreen(), cs.getBlue(), new float[3]); - if (color[0] >= 0.05 && color[0] <= 0.2) { - return DriverStation.Alliance.Red; - } else if (color[0] >= 0.4 && color[0] < 0.6) { - return DriverStation.Alliance.Blue; - } else { - return DriverStation.Alliance.Invalid; - } - } - - /** - * @return true if enemy ball in front of the sensor - */ - public boolean isEnemyBallInSensor() { - DriverStation.Alliance a = instance.getBallColor(); - SmartDashboard.putString("color", a.toString()); - return a != alliance - && a != DriverStation.Alliance.Invalid; - } - - /** - * @return true is our team's ball is in front of the sensor. - */ - public boolean isTeamsBallInSensor() { - DriverStation.Alliance a = instance.getBallColor(); - SmartDashboard.putString("color", a.toString()); - return a == alliance - && a != DriverStation.Alliance.Invalid; - } - - -} \ No newline at end of file From b178af198eef23b736f48cc8bd028daa308f1ff6 Mon Sep 17 00:00:00 2001 From: itamaroryan Date: Wed, 29 Nov 2023 17:58:55 +0200 Subject: [PATCH 2/3] oops --- src/main/java/edu/greenblitz/pegasus/subsystems/Funnel.java | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/java/edu/greenblitz/pegasus/subsystems/Funnel.java b/src/main/java/edu/greenblitz/pegasus/subsystems/Funnel.java index c5a86511..22b968cf 100644 --- a/src/main/java/edu/greenblitz/pegasus/subsystems/Funnel.java +++ b/src/main/java/edu/greenblitz/pegasus/subsystems/Funnel.java @@ -28,6 +28,7 @@ public void moveMotor(double power) { public void moveMotor(boolean reversed) { moveMotor(reversed ? RobotMap.Pegasus.Funnel.REVERSE_POWER : RobotMap.Pegasus.Funnel.POWER); } + public void moveMotor() { moveMotor(false); From 6503b57fc4998bb69d00645ac5d2b979421a892c Mon Sep 17 00:00:00 2001 From: itamaroryan Date: Wed, 29 Nov 2023 18:11:00 +0200 Subject: [PATCH 3/3] Nitzan - bollz --- src/main/java/edu/greenblitz/pegasus/OI.java | 1 + .../java/edu/greenblitz/pegasus/RobotMap.java | 4 ++ .../commands/handleBalls/HandleBalls.java | 68 ++++++++++++++++++ .../EjectEnemyBallFromGripper.java | 14 ++++ .../multiSystem/EjectFromShooter.java | 18 +++++ .../multiSystem/InsertIntoShooter.java | 46 ++++++++++++ .../multiSystem/MoveBallUntilClick.java | 19 +++++ .../pegasus/subsystems/Indexing.java | 72 +++++++++++++++++++ 8 files changed, 242 insertions(+) create mode 100644 src/main/java/edu/greenblitz/pegasus/commands/handleBalls/HandleBalls.java create mode 100644 src/main/java/edu/greenblitz/pegasus/commands/multiSystem/EjectEnemyBallFromGripper.java create mode 100644 src/main/java/edu/greenblitz/pegasus/commands/multiSystem/EjectFromShooter.java create mode 100644 src/main/java/edu/greenblitz/pegasus/commands/multiSystem/InsertIntoShooter.java create mode 100644 src/main/java/edu/greenblitz/pegasus/commands/multiSystem/MoveBallUntilClick.java create mode 100644 src/main/java/edu/greenblitz/pegasus/subsystems/Indexing.java diff --git a/src/main/java/edu/greenblitz/pegasus/OI.java b/src/main/java/edu/greenblitz/pegasus/OI.java index ad85383b..e196e350 100644 --- a/src/main/java/edu/greenblitz/pegasus/OI.java +++ b/src/main/java/edu/greenblitz/pegasus/OI.java @@ -81,6 +81,7 @@ private void initRealButtons() { } private void initAmirButtons() { + //Indexing.getInstance().setDefaultCommand(new HandleBalls()); SwerveChassis.getInstance().setDefaultCommand(new CombineJoystickMovement(mainJoystick, false)); mainJoystick.Y.whenPressed(new SwerveCommand() { @Override diff --git a/src/main/java/edu/greenblitz/pegasus/RobotMap.java b/src/main/java/edu/greenblitz/pegasus/RobotMap.java index 58a1ed74..e86ac47d 100644 --- a/src/main/java/edu/greenblitz/pegasus/RobotMap.java +++ b/src/main/java/edu/greenblitz/pegasus/RobotMap.java @@ -71,6 +71,7 @@ public static class ShooterMotor { public static class Funnel { public static final double POWER = 0.7; public static final double REVERSE_POWER = -0.7; + public static final int MACRO_SWITCH_PORT = 0; public static class FunnelMotor { public static final int MOTOR_PORT = 5; @@ -89,6 +90,9 @@ public static class PressureSensor { } } + public static class DigitalInputMap { + public static final int MACRO_SWITCH = 0; + } public static class Swerve { public static final double WHEEL_CIRC = 0.0517 * 2 * Math.PI; //very accurate right now diff --git a/src/main/java/edu/greenblitz/pegasus/commands/handleBalls/HandleBalls.java b/src/main/java/edu/greenblitz/pegasus/commands/handleBalls/HandleBalls.java new file mode 100644 index 00000000..0783dfd3 --- /dev/null +++ b/src/main/java/edu/greenblitz/pegasus/commands/handleBalls/HandleBalls.java @@ -0,0 +1,68 @@ +package edu.greenblitz.pegasus.commands.handleBalls; + +import edu.greenblitz.pegasus.commands.funnel.ReverseRunFunnel; +import edu.greenblitz.pegasus.commands.funnel.RunFunnel; +import edu.greenblitz.pegasus.commands.intake.IntakeCommand; +import edu.greenblitz.pegasus.commands.intake.roller.ReverseRunRoller; +import edu.greenblitz.pegasus.commands.shooter.ShooterEvacuate; +import edu.greenblitz.pegasus.subsystems.Indexing; +import edu.greenblitz.pegasus.utils.DigitalInputMap; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; + +public class HandleBalls extends IntakeCommand { + + + /** + * this command checks if an enemy ball is trying to enter the Funnel, + * and if so it is rejecting it from entering the funnel system using a + * color sensor to check the color + * + * @see com.revrobotics.ColorSensorV3 colorSensor. + * @see Indexing Subsystem + */ + + private final Indexing index; + private boolean isBallInFunnel; + + public HandleBalls() { + this.index = Indexing.getInstance(); + isBallInFunnel = false; + require(Indexing.getInstance()); + } + + @Override + public void execute() { + SmartDashboard.putBoolean("isBallInFunnel", isBallInFunnel); + SmartDashboard.putBoolean("isEnemyBallUnSensor", index.isEnemyBallInSensor()); + SmartDashboard.putBoolean("isMacroSwitch", DigitalInputMap.getInstance().getValue(0)); + + if (index.isTeamsBallInSensor()) { + //if our team's ball is in front of the sensor activate the boolean + isBallInFunnel = true; + } + if (DigitalInputMap.getInstance().getValue(0)) { + //if the ball got to the macroSwitch then disable the boolean + isBallInFunnel = false; + } + + if (index.isEnemyBallInSensor()) { + if (DigitalInputMap.getInstance().getValue(0) || isBallInFunnel) { + isBallInFunnel = false; + SmartDashboard.putBoolean("isEvacuatingFromShooter", false); + //back direction + new ParallelDeadlineGroup( + new WaitCommand(0.5), + new ReverseRunRoller(), + new ReverseRunFunnel().raceWith(new WaitCommand(0.2)) + ).andThen(new RunFunnel().until(() -> DigitalInputMap.getInstance().getValue(0))).schedule(false); + } else { + //shooter evacuation + SmartDashboard.putBoolean("isEvacuatingFromShooter", true); + new ShooterEvacuate().raceWith(new WaitCommand(5)).schedule(false); + + } + } + } +} diff --git a/src/main/java/edu/greenblitz/pegasus/commands/multiSystem/EjectEnemyBallFromGripper.java b/src/main/java/edu/greenblitz/pegasus/commands/multiSystem/EjectEnemyBallFromGripper.java new file mode 100644 index 00000000..052147a2 --- /dev/null +++ b/src/main/java/edu/greenblitz/pegasus/commands/multiSystem/EjectEnemyBallFromGripper.java @@ -0,0 +1,14 @@ +package edu.greenblitz.pegasus.commands.multiSystem; + +import edu.greenblitz.pegasus.commands.intake.roller.ReverseRunRoller; +import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; +import edu.wpi.first.wpilibj2.command.WaitCommand; + +public class EjectEnemyBallFromGripper extends ParallelRaceGroup { //todo can be deadline + public EjectEnemyBallFromGripper() { + addCommands( + new WaitCommand(1.5), + new ReverseRunRoller() + ); + } +} diff --git a/src/main/java/edu/greenblitz/pegasus/commands/multiSystem/EjectFromShooter.java b/src/main/java/edu/greenblitz/pegasus/commands/multiSystem/EjectFromShooter.java new file mode 100644 index 00000000..024edf15 --- /dev/null +++ b/src/main/java/edu/greenblitz/pegasus/commands/multiSystem/EjectFromShooter.java @@ -0,0 +1,18 @@ +package edu.greenblitz.pegasus.commands.multiSystem; + +import edu.greenblitz.pegasus.RobotMap; +import edu.greenblitz.pegasus.commands.funnel.ReverseRunFunnel; +import edu.greenblitz.pegasus.commands.intake.roller.ReverseRunRoller; +import edu.greenblitz.pegasus.utils.DigitalInputMap; +import edu.wpi.first.wpilibj2.command.ParallelRaceGroup; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; + +public class EjectFromShooter extends ParallelRaceGroup {//todo delete + public EjectFromShooter() { + addCommands( + new WaitUntilCommand(() -> DigitalInputMap.getInstance().getValue(RobotMap.Pegasus.Funnel.MACRO_SWITCH_PORT)), + new ReverseRunFunnel(), + new ReverseRunRoller() + ); + } +} diff --git a/src/main/java/edu/greenblitz/pegasus/commands/multiSystem/InsertIntoShooter.java b/src/main/java/edu/greenblitz/pegasus/commands/multiSystem/InsertIntoShooter.java new file mode 100644 index 00000000..5278c3e4 --- /dev/null +++ b/src/main/java/edu/greenblitz/pegasus/commands/multiSystem/InsertIntoShooter.java @@ -0,0 +1,46 @@ +package edu.greenblitz.pegasus.commands.multiSystem; + +import edu.greenblitz.GBLib.src.main.java.edu.greenblitz.gblib.subsystems.shooter.Shooter; +import edu.greenblitz.pegasus.RobotMap; +import edu.greenblitz.pegasus.commands.funnel.RunFunnel; +import edu.greenblitz.pegasus.commands.intake.roller.RunRoller; +import edu.greenblitz.pegasus.utils.DigitalInputMap; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; + +public class InsertIntoShooter extends SequentialCommandGroup { + + private double startTime; + private boolean reported = false; + + // AKA InsertoShooter @tal935 + public InsertIntoShooter() { + addCommands( + new MoveBallUntilClick(), + + //waits until the shooter is ready + new WaitUntilCommand(() -> Shooter.getInstance().isPreparedToShoot()), + + new ParallelDeadlineGroup(//activates both roller and funnel until ball is no longer at macro switch (was probably propelled) + new WaitUntilCommand(() -> !DigitalInputMap.getInstance().getValue(RobotMap.Pegasus.Funnel.MACRO_SWITCH_PORT)), + new RunFunnel(), + new RunRoller() + )); + + } + + @Override + public void initialize() { + super.initialize(); + startTime = System.currentTimeMillis() / 1000.0; + } + + + @Override + public void end(boolean interrupted) { + super.end(interrupted); + reported = false; + } +} diff --git a/src/main/java/edu/greenblitz/pegasus/commands/multiSystem/MoveBallUntilClick.java b/src/main/java/edu/greenblitz/pegasus/commands/multiSystem/MoveBallUntilClick.java new file mode 100644 index 00000000..0ddf9320 --- /dev/null +++ b/src/main/java/edu/greenblitz/pegasus/commands/multiSystem/MoveBallUntilClick.java @@ -0,0 +1,19 @@ +package edu.greenblitz.pegasus.commands.multiSystem; + +import edu.greenblitz.GBLib.src.main.java.edu.greenblitz.gblib.base.GBCommand; +import edu.greenblitz.GBLib.src.main.java.edu.greenblitz.gblib.commands.DoUntilCommand; +import edu.greenblitz.pegasus.RobotMap; +import edu.greenblitz.pegasus.commands.funnel.RunFunnel; +import edu.greenblitz.pegasus.commands.intake.roller.RunRoller; +import edu.greenblitz.pegasus.utils.DigitalInputMap; +import edu.greenblitz.pegasus.utils.WaitCommand; +import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup; +import edu.wpi.first.wpilibj2.command.WaitUntilCommand; + +public class MoveBallUntilClick extends ParallelDeadlineGroup { + + public MoveBallUntilClick() { + super(new WaitUntilCommand(() -> DigitalInputMap.getInstance().getValue(0)), new RunRoller(),new RunFunnel()); + } + +} \ No newline at end of file diff --git a/src/main/java/edu/greenblitz/pegasus/subsystems/Indexing.java b/src/main/java/edu/greenblitz/pegasus/subsystems/Indexing.java new file mode 100644 index 00000000..a200c2af --- /dev/null +++ b/src/main/java/edu/greenblitz/pegasus/subsystems/Indexing.java @@ -0,0 +1,72 @@ +package edu.greenblitz.pegasus.subsystems; + +import com.revrobotics.ColorSensorV3; +import edu.greenblitz.GBLib.src.main.java.edu.greenblitz.gblib.subsystems.GBSubsystem; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.I2C; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +import java.awt.*; + + +public class Indexing extends GBSubsystem { + /** + * This class is in-charge of checking if an enemy ball is trying to get to the funnel (and eventually to the shooter) + * by using color sensor + * + * @see ColorSensorV3 colorSensorV3 + */ + + private static Indexing instance; + private ColorSensorV3 cs; + public DriverStation.Alliance alliance; + + private Indexing() { + cs = new ColorSensorV3(I2C.Port.kOnboard); + alliance = DriverStation.getAlliance(); + } + + public static Indexing getInstance() { + if (instance == null) { + instance = new Indexing(); + } + return instance; + } + + + /** + * @return the color that is in front of the sensor (as a DriverStation.Alliance enum) + */ + public DriverStation.Alliance getBallColor() { + float[] color = Color.RGBtoHSB(cs.getRed(), cs.getGreen(), cs.getBlue(), new float[3]); + if (color[0] >= 0.05 && color[0] <= 0.2) { + return DriverStation.Alliance.Red; + } else if (color[0] >= 0.4 && color[0] < 0.6) { + return DriverStation.Alliance.Blue; + } else { + return DriverStation.Alliance.Invalid; + } + } + + /** + * @return true if enemy ball in front of the sensor + */ + public boolean isEnemyBallInSensor() { + DriverStation.Alliance a = instance.getBallColor(); + SmartDashboard.putString("color", a.toString()); + return a != alliance + && a != DriverStation.Alliance.Invalid; + } + + /** + * @return true is our team's ball is in front of the sensor. + */ + public boolean isTeamsBallInSensor() { + DriverStation.Alliance a = instance.getBallColor(); + SmartDashboard.putString("color", a.toString()); + return a == alliance + && a != DriverStation.Alliance.Invalid; + } + + +} \ No newline at end of file