diff --git a/src/main/java/competition/subsystems/collector_intake/CollectorSubsystem.java b/src/main/java/competition/subsystems/collector_intake/CollectorSubsystem.java index d0733d92..3016e8f5 100644 --- a/src/main/java/competition/subsystems/collector_intake/CollectorSubsystem.java +++ b/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