diff --git a/src/main/java/xbot/common/subsystems/arm/BaseArmSubsystem.java b/src/main/java/xbot/common/subsystems/arm/BaseArmSubsystem.java new file mode 100644 index 00000000..22a32098 --- /dev/null +++ b/src/main/java/xbot/common/subsystems/arm/BaseArmSubsystem.java @@ -0,0 +1,55 @@ +package xbot.common.subsystems.arm; + +import javax.inject.Singleton; + +import org.wpilib.command2.Command; +import org.wpilib.command2.Commands; +import org.wpilib.units.measure.Angle; + +import xbot.common.command.BaseSetpointSubsystem; +import xbot.common.controls.actuators.XCANMotorController; +import xbot.common.properties.TunableFactory; + +@Singleton +public abstract class BaseArmSubsystem extends BaseSetpointSubsystem { + + boolean isCalibrated = false; + boolean hasAbsoluteEncoder; + + public BaseArmSubsystem(XCANMotorController.XCANMotorControllerFactory xcanMotorControllerFactory, TunableFactory tunableFactory) { + super(); + tunableFactory.setPrefix(this); + + } + + public abstract Angle getTargetValue(); + + public abstract boolean hasAbsoluteEncoder(); + + public abstract void setTargetValue(Angle desiredAngle); + + public abstract void setPower(Double power); + + public abstract boolean isCalibrated(); + + protected abstract boolean areTwoTargetsEquivalent(Angle target1, Angle target2); + + @Override + public void periodic() { + super.periodic(); + } + + public abstract Angle getCurrentValue(); + + public Command getHasAbsoluteEncoder() { + return Commands.runOnce(() -> { + hasAbsoluteEncoder = hasAbsoluteEncoder(); + }); + } + + public Command getCalibrateCommand() { + return Commands.runOnce(() -> { + isCalibrated = isCalibrated(); + }); + } +} diff --git a/src/main/java/xbot/common/subsystems/arm/commands/BaseArmMaintainer.java b/src/main/java/xbot/common/subsystems/arm/commands/BaseArmMaintainer.java new file mode 100644 index 00000000..4ffe8107 --- /dev/null +++ b/src/main/java/xbot/common/subsystems/arm/commands/BaseArmMaintainer.java @@ -0,0 +1,31 @@ +package xbot.common.subsystems.arm.commands; + +import org.wpilib.units.measure.Angle; + +import xbot.common.command.BaseMaintainerCommand; +import xbot.common.logic.HumanVsMachineDecider; +import xbot.common.properties.TunableFactory; +import xbot.common.subsystems.arm.BaseArmSubsystem; + +public abstract class BaseArmMaintainer extends BaseMaintainerCommand { + + final BaseArmSubsystem baseArm; + + public BaseArmMaintainer(TunableFactory tunableFactory, HumanVsMachineDecider.HumanVsMachineDeciderFactory hvmFactory, BaseArmSubsystem baseArm) { + super(baseArm, tunableFactory, hvmFactory, 1, 1); + + this.baseArm = baseArm; + } + + public abstract void initialize(); + + protected abstract void coastAction(); + + protected abstract void calibratedMachineControlAction(); + + protected abstract double getErrorMagnitude(); + + protected abstract Double getHumanInput(); + + protected abstract double getHumanInputMagnitude(); +}