Handle balls 3

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

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

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);
+
+			}
+		}
+	}
+}

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()
+		);
+	}
+}

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()
+		);
+	}
+}

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;
+	}
+}

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());
+		}
+
+}

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);

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;
+	}
+
+
+}