diff --git a/src/main/java/competition/electrical_contract/Contract2025.java b/src/main/java/competition/electrical_contract/Contract2025.java index e31b203..c9627f5 100644 --- a/src/main/java/competition/electrical_contract/Contract2025.java +++ b/src/main/java/competition/electrical_contract/Contract2025.java @@ -48,6 +48,23 @@ public boolean areCanCodersReady() { return true; } + @Override + public boolean isElevatorReady() { + return false; //return true when ready + } + + @Override + public CANMotorControllerInfo getElevatorMaster() { + return new CANMotorControllerInfo( //change CANBUS ID's later + "Master Elevator Motor", MotorControllerType.TalonFx, new CANBusId("99"), 99, new CANMotorControllerOutputConfig()); + } + + @Override + public CANMotorControllerInfo getElevatorFollower() { + return new CANMotorControllerInfo( //same as above + "Follower Elevator Motor", MotorControllerType.TalonFx, new CANBusId("98"), 98, new CANMotorControllerOutputConfig()); + } + protected String getDriveControllerName(SwerveInstance swerveInstance) { return "DriveSubsystem/" + swerveInstance.label() + "/Drive"; } diff --git a/src/main/java/competition/electrical_contract/ElectricalContract.java b/src/main/java/competition/electrical_contract/ElectricalContract.java index 41260d0..98153ba 100644 --- a/src/main/java/competition/electrical_contract/ElectricalContract.java +++ b/src/main/java/competition/electrical_contract/ElectricalContract.java @@ -20,6 +20,12 @@ public abstract class ElectricalContract implements XSwerveDriveElectricalContra public abstract XYPair getSwerveModuleOffsetsInInches(SwerveInstance swerveInstance); + public abstract boolean isElevatorReady(); + + public abstract CANMotorControllerInfo getElevatorMaster(); + + public abstract CANMotorControllerInfo getElevatorFollower(); + public abstract boolean isAlgaeCollectionReady(); public abstract CANMotorControllerInfo getAlgaeCollectionMotor(); diff --git a/src/main/java/competition/subsystems/elevator/ElevatorSubsystem.java b/src/main/java/competition/subsystems/elevator/ElevatorSubsystem.java new file mode 100644 index 0000000..9a07009 --- /dev/null +++ b/src/main/java/competition/subsystems/elevator/ElevatorSubsystem.java @@ -0,0 +1,111 @@ +package competition.subsystems.elevator; + +import competition.electrical_contract.ElectricalContract; +import xbot.common.advantage.DataFrameRefreshable; +import xbot.common.command.BaseSetpointSubsystem; +import xbot.common.controls.actuators.XCANMotorController; +import xbot.common.properties.DoubleProperty; +import xbot.common.properties.PropertyFactory; + +import javax.inject.Inject; +import javax.inject.Singleton; + +@Singleton +public class ElevatorSubsystem extends BaseSetpointSubsystem implements DataFrameRefreshable { + + public enum ElevatorGoals{ + ScoreL1, + ScoreL2, + ScoreL3, + ScoreL4, + CoralCollection, + ReturnToBase + } + + final DoubleProperty elevatorPower; + final ElectricalContract contract; + + private boolean isCalibrated; + + final DoubleProperty elevatorTargetHeight; + final DoubleProperty currentHeight; + + //assuming we'll have a master and follower motors + public XCANMotorController masterMotor; + public XCANMotorController followerMotor; + + + @Inject + public ElevatorSubsystem(XCANMotorController.XCANMotorControllerFactory motorFactory, PropertyFactory pf, ElectricalContract contract){ + + this.elevatorPower = pf.createPersistentProperty("Elevator Power", 0.5); + this.contract = contract; + + //was going to make this ephemeral but cant find it + this.elevatorTargetHeight = pf.createPersistentProperty("Elevator Target", 0.0); + this.currentHeight = pf.createPersistentProperty("Current Height", 0.0); + + + if(contract.isElevatorReady()){ + this.masterMotor = motorFactory.create(contract.getElevatorMaster(), this.getPrefix(), "Elevator Motor"); + this.followerMotor = motorFactory.create(contract.getElevatorFollower(), this.getPrefix(), "Elevator Motor"); + } + } + + //will implement logic later + @Override + public void setPower(Double power) { + if (contract.isElevatorReady()) { + masterMotor.setPower(power); + followerMotor.setPower(power); + } + } + + public void rise(){ + setPower(elevatorPower.get()); + } + + public void lower(){ + setPower(-elevatorPower.get()); + } + + public void stop(){ + setPower(0.0); + } + + + @Override + public Double getCurrentValue() { + if (contract.isElevatorReady()){ + return this.currentHeight.get(); + } + return 0.0; + } + + @Override + public void setTargetValue(Double targetHeight) { + this.elevatorTargetHeight.set(targetHeight); + } + + @Override + public Double getTargetValue() { + return this.elevatorTargetHeight.get(); + } + + @Override + public boolean isCalibrated() { + return isCalibrated; + } + + @Override + protected boolean areTwoTargetsEquivalent(Double target1, Double target2) { + return BaseSetpointSubsystem.areTwoDoublesEquivalent(target1, target2, 1); + } + + @Override + public void refreshDataFrame() { + //to be implemented later + } + + +} diff --git a/src/main/java/competition/subsystems/elevator/commands/ElevatorMaintainerCommand.java b/src/main/java/competition/subsystems/elevator/commands/ElevatorMaintainerCommand.java new file mode 100644 index 0000000..77c5033 --- /dev/null +++ b/src/main/java/competition/subsystems/elevator/commands/ElevatorMaintainerCommand.java @@ -0,0 +1,35 @@ +package competition.subsystems.elevator.commands; + +import competition.subsystems.elevator.ElevatorSubsystem; +import xbot.common.command.BaseCommand; +import xbot.common.command.BaseSubsystem; +import xbot.common.properties.PropertyFactory; + +import javax.inject.Inject; + +public class ElevatorMaintainerCommand extends BaseCommand { + + public enum MaintainerMode{ + Calibrating, + GaveUp, + Calibrated, + } + + ElevatorSubsystem elevator; + + @Inject + public ElevatorMaintainerCommand(ElevatorSubsystem elevator, PropertyFactory pf){ + this.elevator = elevator; + } + + @Override + public void initialize() { + log.info("initializing"); + } + + @Override + public void execute(){ + + } + +} diff --git a/src/main/java/competition/subsystems/elevator/commands/LowerElevatorCommand.java b/src/main/java/competition/subsystems/elevator/commands/LowerElevatorCommand.java new file mode 100644 index 0000000..1a76f69 --- /dev/null +++ b/src/main/java/competition/subsystems/elevator/commands/LowerElevatorCommand.java @@ -0,0 +1,29 @@ +package competition.subsystems.elevator.commands; + +import competition.subsystems.elevator.ElevatorSubsystem; +import xbot.common.command.BaseCommand; + +import javax.inject.Inject; + +public class LowerElevatorCommand extends BaseCommand { + + ElevatorSubsystem elevator; + + @Inject + public LowerElevatorCommand(ElevatorSubsystem elevator){ + this.elevator = elevator; + addRequirements(elevator); + } + + @Override + public void initialize() { + log.info("initializing"); + } + + @Override + public void execute(){ + elevator.lower(); + } +} + + diff --git a/src/main/java/competition/subsystems/elevator/commands/RiseElevatorCommand.java b/src/main/java/competition/subsystems/elevator/commands/RiseElevatorCommand.java new file mode 100644 index 0000000..3976305 --- /dev/null +++ b/src/main/java/competition/subsystems/elevator/commands/RiseElevatorCommand.java @@ -0,0 +1,27 @@ +package competition.subsystems.elevator.commands; + +import competition.subsystems.elevator.ElevatorSubsystem; +import xbot.common.command.BaseCommand; + +import javax.inject.Inject; + +public class RiseElevatorCommand extends BaseCommand { + + ElevatorSubsystem elevator; + + @Inject + public RiseElevatorCommand(ElevatorSubsystem elevator){ + this.elevator = elevator; + addRequirements(elevator); + } + + @Override + public void initialize() { + log.info("initializing"); + } + + @Override + public void execute(){ + elevator.rise(); + } +} diff --git a/src/main/java/competition/subsystems/elevator/commands/SetElevatorTargetHeightCommand.java b/src/main/java/competition/subsystems/elevator/commands/SetElevatorTargetHeightCommand.java new file mode 100644 index 0000000..1a492ba --- /dev/null +++ b/src/main/java/competition/subsystems/elevator/commands/SetElevatorTargetHeightCommand.java @@ -0,0 +1,33 @@ +package competition.subsystems.elevator.commands; + +import competition.subsystems.elevator.ElevatorSubsystem; +import xbot.common.command.BaseCommand; + +import javax.inject.Inject; + +public class SetElevatorTargetHeightCommand extends BaseCommand { + + Double height; + ElevatorSubsystem elevator; + + @Inject + public SetElevatorTargetHeightCommand(ElevatorSubsystem elevator){ + this.elevator = elevator; + addRequirements(elevator); + } + + @Override + public void initialize() { + log.info("initializing"); + elevator.setTargetValue(height); + } + + @Override + public void execute() { + elevator.setTargetValue(height); + } + + public void setHeight(double height) { + this.height = height; + } +} diff --git a/src/main/java/competition/subsystems/elevator/commands/StopElevatorCommand.java b/src/main/java/competition/subsystems/elevator/commands/StopElevatorCommand.java new file mode 100644 index 0000000..dd408f5 --- /dev/null +++ b/src/main/java/competition/subsystems/elevator/commands/StopElevatorCommand.java @@ -0,0 +1,29 @@ +package competition.subsystems.elevator.commands; + +import competition.subsystems.elevator.ElevatorSubsystem; +import xbot.common.command.BaseCommand; + +import javax.inject.Inject; +import javax.inject.Singleton; + +@Singleton +public class StopElevatorCommand extends BaseCommand { + + ElevatorSubsystem elevator; + + @Inject + public StopElevatorCommand(ElevatorSubsystem elevator) { + this.elevator = elevator; + addRequirements(elevator); + } + + @Override + public void initialize() { + log.info("initializing"); + } + + @Override + public void execute() { + elevator.stop(); + } +} \ No newline at end of file