From b178af198eef23b736f48cc8bd028daa308f1ff6 Mon Sep 17 00:00:00 2001 From: itamaroryan Date: Wed, 29 Nov 2023 17:58:55 +0200 Subject: [PATCH 1/2] 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 2/2] 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