Collector Safety

src/main/java/competition/subsystems/collector_intake/CollectorSubsystem.java

@@ -1,28 +1,36 @@
 package competition.subsystems.collector_intake;
 
 import competition.electrical_contract.ElectricalContract;
+import competition.subsystems.intake_deploy.IntakeDeploySubsystem;
 import xbot.common.command.BaseSubsystem;
 import xbot.common.controls.actuators.XCANMotorController;
+import xbot.common.properties.AngleProperty;
 import xbot.common.properties.DoubleProperty;
 import xbot.common.properties.PropertyFactory;
 
 import javax.inject.Inject;
 import javax.inject.Singleton;
 
+import static edu.wpi.first.units.Units.Degrees;
+
 @Singleton
 public class CollectorSubsystem extends BaseSubsystem {
 
+    public final IntakeDeploySubsystem intakeDeploySubsystem;
     public final ElectricalContract electricalContract;
     public final XCANMotorController collectorMotor;
     final DoubleProperty intakePower;
     final DoubleProperty ejectPower;
+    final AngleProperty collectorAngle;
+
 
     @Inject
     public CollectorSubsystem(ElectricalContract electricalContract,
                               XCANMotorController.XCANMotorControllerFactory motorFactory,
-                              PropertyFactory pf) {
+                              PropertyFactory pf, IntakeDeploySubsystem intakeDeploySubsystem) {
 
         pf.setPrefix(this);
+        this.intakeDeploySubsystem = intakeDeploySubsystem;
         this.electricalContract = electricalContract;
         if (electricalContract.isFuelIntakeMotorReady()) {
             this.collectorMotor = motorFactory.create(
@@ -37,27 +45,34 @@ public CollectorSubsystem(ElectricalContract electricalContract,
 
         intakePower = pf.createPersistentProperty("FuelIntakePower", 1);
         ejectPower = pf.createPersistentProperty("FuelEjectPower", -1);
+        collectorAngle = pf.createPersistentProperty("CollectorAngle", Degrees.of(-90));
+        // collector angle should be tuned to what a safe angle for the collector will be, -90 is just a safe bet.
     }
 
     public void intake() {
-        if (collectorMotor == null) {
-            return;
-        }
-        collectorMotor.setPower(intakePower.get());
+        this.setPower(intakePower.get());
     }
 
     public void eject() {
-        if (collectorMotor == null) {
-            return;
-        }
-        collectorMotor.setPower(ejectPower.get());
+        this.setPower(ejectPower.get());
     }
 
     public void stop() {
+        this.setPower(0.0);
+    }
+
+    public void setPower(double power) {
         if (collectorMotor == null) {
             return;
         }
-        collectorMotor.setPower(0);
+
+
+        if (intakeDeploySubsystem.isCalibrated && intakeDeploySubsystem.getCurrentValue().gt(collectorAngle.get())) {
+            collectorMotor.setPower(0.0);
+
+        } else  {
+            collectorMotor.setPower(power);
+        }
     }
 
     @Override