From 22b0cd679fa76063c5009866b36dd20e631ccacb Mon Sep 17 00:00:00 2001
From: sVampified <126119689+sVampified@users.noreply.github.com>
Date: Tue, 6 Feb 2024 19:51:06 -0800
Subject: [PATCH] Started ShooterFireCommand (#70)

* Started ShooterFireCommand

* Fixed if statement

* Removed unnecessary log statement

* End of day commit 2/3/2024

Fixed CollectorSubsystem and ShooterFireCommand, not done.

* Done with FireWhenReadyCommand and cleaned up CollectorSub
---
 .../collector/CollectorSubsystem.java         | 44 ++++++++++++++-----
 .../commands/FireWhenReadyCommand.java        | 38 ++++++++++++++++
 2 files changed, 71 insertions(+), 11 deletions(-)
 create mode 100644 src/main/java/competition/subsystems/shooter/commands/FireWhenReadyCommand.java

diff --git a/src/main/java/competition/subsystems/collector/CollectorSubsystem.java b/src/main/java/competition/subsystems/collector/CollectorSubsystem.java
index c320500c..5cc54cb5 100644
--- a/src/main/java/competition/subsystems/collector/CollectorSubsystem.java
+++ b/src/main/java/competition/subsystems/collector/CollectorSubsystem.java
@@ -38,7 +38,12 @@ public enum IntakeState {
     public CollectorSubsystem(PropertyFactory pf, XCANSparkMax.XCANSparkMaxFactory sparkMaxFactory,
                               ElectricalContract electricalContract, XDigitalInput.XDigitalInputFactory xDigitalInputFactory) {
         this.contract = electricalContract;
-        this.collectorMotor = sparkMaxFactory.createWithoutProperties(contract.getCollectorMotor(), getPrefix(), "CollectorMotor");
+        if (contract.isCollectorReady()) {
+            this.collectorMotor = sparkMaxFactory.createWithoutProperties(contract.getCollectorMotor(), getPrefix(), "CollectorMotor");
+        } else {
+            this.collectorMotor = null;
+        }
+
         this.inControlNoteSensor = xDigitalInputFactory.create(contract.getInControlNoteSensorDio());
         this.readyToFireNoteSensor = xDigitalInputFactory.create(contract.getReadyToFireNoteSensorDio());
 
@@ -58,38 +63,55 @@ public void intake(){
         if (getGamePieceInControl()) {
             power *= intakePowerInControlMultiplier.get();
         }
-        collectorMotor.set(power);
+        setPower(power);
         intakeState = IntakeState.INTAKING;
     }
     public void eject(){
-        collectorMotor.set(ejectPower.get());
+        setPower(ejectPower.get());
         intakeState = IntakeState.EJECTING;
     }
     public void stop(){
-        collectorMotor.set(0);
+        setPower(0);
         intakeState = IntakeState.STOPPED;
     }
     public void fire(){
-        collectorMotor.set(firePower.get());
+        setPower(firePower.get());
         intakeState = IntakeState.FIRING;
     }
+
+    public void setPower(double power) {
+        if (contract.isCollectorReady()) {
+            collectorMotor.set(power);
+        }
+    }
+
     public boolean getGamePieceInControl() {
-        return inControlNoteSensor.get();
+        if (contract.isCollectorReady()) {
+            return inControlNoteSensor.get();
+        }
+        return false;
     }
 
     public boolean getGamePieceReady() {
-        return readyToFireNoteSensor.get();
+        if (contract.isCollectorReady()) {
+            return readyToFireNoteSensor.get();
+        }
+        return false;
     }
 
     @Override
     public void periodic() {
-        aKitLog.record("HasGamePiece", getGamePieceReady());
+        if (contract.isCollectorReady()) {
+            aKitLog.record("HasGamePiece", getGamePieceReady());
+        }
     }
 
     @Override
     public void refreshDataFrame() {
-        collectorMotor.refreshDataFrame();
-        inControlNoteSensor.refreshDataFrame();
-        readyToFireNoteSensor.refreshDataFrame();
+        if (contract.isCollectorReady()) {
+            collectorMotor.refreshDataFrame();
+            inControlNoteSensor.refreshDataFrame();
+            readyToFireNoteSensor.refreshDataFrame();
+        }
     }
 }
\ No newline at end of file
diff --git a/src/main/java/competition/subsystems/shooter/commands/FireWhenReadyCommand.java b/src/main/java/competition/subsystems/shooter/commands/FireWhenReadyCommand.java
new file mode 100644
index 00000000..1c82e935
--- /dev/null
+++ b/src/main/java/competition/subsystems/shooter/commands/FireWhenReadyCommand.java
@@ -0,0 +1,38 @@
+package competition.subsystems.shooter.commands;
+
+import competition.subsystems.arm.ArmSubsystem;
+import competition.subsystems.collector.CollectorSubsystem;
+import competition.subsystems.shooter.ShooterWheelSubsystem;
+import xbot.common.command.BaseCommand;
+
+import javax.inject.Inject;
+
+public class FireWhenReadyCommand extends BaseCommand {
+    final ShooterWheelSubsystem wheel;
+    final ArmSubsystem arm;
+    final CollectorSubsystem collector;
+
+    @Inject
+    public FireWhenReadyCommand(ShooterWheelSubsystem wheel, ArmSubsystem arm, CollectorSubsystem collector) {
+        this.wheel = wheel;
+        this.arm = arm;
+        this.collector = collector;
+    }
+
+    @Override
+    public void initialize() {
+        log.info("Initializing...");
+    }
+
+    @Override
+    public void execute() {
+        /* WE CANNOT CHECK IF CURRENT VALUE OF WHEEL AND TARGET VALUE ARE EXACTLY THE SAME, THAT IS WHY WE
+        HAVE THE TOLERANCE PROPERTIES IN THE FIRST PLACE
+
+        RUNS 50 TIMES A SECOND
+        */
+        if (wheel.isMaintainerAtGoal() && arm.isMaintainerAtGoal()) {
+            collector.fire();
+        }
+    }
+}