diff --git a/src/main/java/competition/subsystems/arm/ArmSubsystem.java b/src/main/java/competition/subsystems/arm/ArmSubsystem.java index 9b811b3f..2a00e597 100644 --- a/src/main/java/competition/subsystems/arm/ArmSubsystem.java +++ b/src/main/java/competition/subsystems/arm/ArmSubsystem.java @@ -41,6 +41,7 @@ public class ArmSubsystem extends BaseSetpointSubsystem implements DataF public double extendPower; public double retractPower; + public double hangPower; public final DoubleProperty powerMax; public final DoubleProperty powerMin; @@ -85,6 +86,7 @@ public class ArmSubsystem extends BaseSetpointSubsystem implements DataF public enum ArmState { EXTENDING, RETRACTING, + HANGING, STOPPED } @@ -125,6 +127,7 @@ public ArmSubsystem(PropertyFactory pf, XCANSparkMax.XCANSparkMaxFactory sparkMa setBrakeEnabled(false); extendPower = 0.1; retractPower = -0.1; + hangPower = -0.2; powerMax = pf.createPersistentProperty("PowerMax", 0.45); powerMin = pf.createPersistentProperty("PowerMin", -0.25); @@ -416,6 +419,11 @@ public void retract() { armState = ArmState.RETRACTING; } + public void hang() { + setPower(hangPower); + armState = ArmState.HANGING; + } + public void stop() { setPower(0.0); armState = ArmState.STOPPED; diff --git a/src/main/java/competition/subsystems/arm/commands/HangingCommand.java b/src/main/java/competition/subsystems/arm/commands/HangingCommand.java new file mode 100644 index 00000000..e4ec5e23 --- /dev/null +++ b/src/main/java/competition/subsystems/arm/commands/HangingCommand.java @@ -0,0 +1,28 @@ +package competition.subsystems.arm.commands; + +import competition.electrical_contract.ElectricalContract; +import competition.subsystems.arm.ArmSubsystem; +import xbot.common.command.BaseCommand; + +import javax.inject.Inject; + +public class HangingCommand extends BaseCommand { + ArmSubsystem armSubsystem; + + @Inject + public HangingCommand(ArmSubsystem armSubsystem) { + addRequirements(armSubsystem); + this.armSubsystem = armSubsystem; + } + + @Override + public void initialize() { + log.info("HangingCommand initializing"); + } + + @Override + public void execute() { + armSubsystem.hang(); + } + +}