diff --git a/src/main/java/competition/electrical_contract/Contract2023.java b/src/main/java/competition/electrical_contract/Contract2023.java index 3771dd4c..4592c968 100644 --- a/src/main/java/competition/electrical_contract/Contract2023.java +++ b/src/main/java/competition/electrical_contract/Contract2023.java @@ -5,10 +5,11 @@ import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.units.Units; +import edu.wpi.first.units.measure.Distance; import xbot.common.controls.sensors.XGyro; import xbot.common.injection.electrical_contract.CANBusId; import xbot.common.injection.electrical_contract.CANMotorControllerInfo; -import xbot.common.injection.electrical_contract.CANMotorControllerOutputConfig; import xbot.common.injection.electrical_contract.CameraInfo; import xbot.common.injection.electrical_contract.DeviceInfo; import xbot.common.injection.electrical_contract.IMUInfo; @@ -24,13 +25,15 @@ import static edu.wpi.first.units.Units.Inches; import java.util.EnumSet; +import java.util.Map; +import java.util.Set; -public class Contract2023 extends Contract2026 { - - protected final double simulationScalingValue = 256.0 * PoseSubsystem.INCHES_IN_A_METER; +public class Contract2023 extends GeneralContract { @Inject - public Contract2023() {} + public Contract2023() { + super(Set.of()); + } @Override public boolean isDriveReady() { @@ -42,18 +45,6 @@ public boolean areCanCodersReady() { return true; } - protected String getDriveControllerName(SwerveInstance swerveInstance) { - return "DriveSubsystem/" + swerveInstance.label() + "/Drive"; - } - - protected String getSteeringControllerName(SwerveInstance swerveInstance) { - return "DriveSubsystem/" + swerveInstance.label() + "/Steering"; - } - - protected String getSteeringEncoderControllerName(SwerveInstance swerveInstance) { - return "DriveSubsystem/" + swerveInstance.label() + "/SteeringEncoder"; - } - @Override public CANMotorControllerInfo getDriveMotor(SwerveInstance swerveInstance) { return switch (swerveInstance.label()) { @@ -95,8 +86,6 @@ public CANMotorControllerInfo getDriveMotor(SwerveInstance swerveInstance) { @Override public CANMotorControllerInfo getSteeringMotor(SwerveInstance swerveInstance) { - double simulationScalingValue = 1.0; - return switch (swerveInstance.label()) { case "FrontLeftDrive" -> new CANMotorControllerInfo( @@ -216,63 +205,14 @@ public CameraInfo[] getCameraInfo() { }; } - @Override + @Override public IMUInfo getIMUInfo() { return new IMUInfo(XGyro.InterfaceType.spi, PowerSource.RIO); } @Override - public boolean isLightsReady() { - return false; - } - - @Override - public boolean isHopperRollerReady() { - return false; - } - - @Override - public boolean isShooterFeederReady() { - return false; - } - - @Override - public boolean isIntakeDeployReady() { - return false; + public Distance getRadiusOfRobot() { + return Units.Inches.of(18); } - @Override - public boolean isLeftShooterReady() { - return false; - } - - @Override - public boolean isMiddleShooterReady() { - return false; - } - - @Override - public boolean isRightShooterReady() { - return false; - } - - @Override - public boolean isHoodServoLeftReady() { - return false; - } - - @Override - public boolean isHoodServoRightReady() { - return false; - } - - @Override - public boolean isClimberLeftReady() { - return false; - } - - @Override - public boolean isClimberRightReady() { - return false; - } } \ No newline at end of file diff --git a/src/main/java/competition/electrical_contract/Contract2025.java b/src/main/java/competition/electrical_contract/Contract2025.java index 616ea252..19850deb 100644 --- a/src/main/java/competition/electrical_contract/Contract2025.java +++ b/src/main/java/competition/electrical_contract/Contract2025.java @@ -5,6 +5,8 @@ import edu.wpi.first.math.geometry.Transform3d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Translation3d; +import edu.wpi.first.units.Units; +import edu.wpi.first.units.measure.Distance; import xbot.common.controls.sensors.XGyro; import xbot.common.injection.electrical_contract.CANBusId; import xbot.common.injection.electrical_contract.CANMotorControllerInfo; @@ -21,16 +23,16 @@ import javax.inject.Inject; import java.util.EnumSet; +import java.util.Set; import static edu.wpi.first.units.Units.Amps; import static edu.wpi.first.units.Units.Inches; -public class Contract2025 extends Contract2026 { - - protected final double simulationScalingValue = 256.0 * PoseSubsystem.INCHES_IN_A_METER; +public class Contract2025 extends GeneralContract { @Inject public Contract2025() { + super(Set.of()); } @Override @@ -39,80 +41,8 @@ public IMUInfo getIMUInfo() { } @Override - public boolean isDriveReady() { - return true; - } - - @Override - public boolean areCanCodersReady() { - return true; - } - - @Override - public boolean isLightsReady() { - return false; - } - - @Override - public boolean isHopperRollerReady() { - return false; - } - - @Override - public boolean isShooterFeederReady() { - return false; - } - - @Override - public boolean isIntakeDeployReady() { - return false; - } - - @Override - public boolean isLeftShooterReady() { - return false; - } - - @Override - public boolean isMiddleShooterReady() { - return false; - } - - @Override - public boolean isRightShooterReady() { - return false; - } - - @Override - public boolean isHoodServoLeftReady() { - return false; - } - - @Override - public boolean isHoodServoRightReady() { - return false; - } - - @Override - public boolean isClimberLeftReady() { - return false; - } - - @Override - public boolean isClimberRightReady() { - return false; - } - - protected String getDriveControllerName(SwerveInstance swerveInstance) { - return "DriveSubsystem/" + swerveInstance.label() + "/Drive"; - } - - protected String getSteeringControllerName(SwerveInstance swerveInstance) { - return "DriveSubsystem/" + swerveInstance.label() + "/Steering"; - } - - protected String getSteeringEncoderControllerName(SwerveInstance swerveInstance) { - return "DriveSubsystem/" + swerveInstance.label() + "/SteeringEncoder"; + public Distance getRadiusOfRobot() { + return Units.Inches.of(18); } TalonFxMotorControllerOutputConfig regularDriveMotorConfig = @@ -127,6 +57,16 @@ protected String getSteeringEncoderControllerName(SwerveInstance swerveInstance) .withStatorCurrentLimit(Amps.of(80)) .withNeutralMode(CANMotorControllerOutputConfig.NeutralMode.Brake); + @Override + public boolean isDriveReady() { + return false; + } + + @Override + public boolean areCanCodersReady() { + return false; + } + @Override public CANMotorControllerInfo getDriveMotor(SwerveInstance swerveInstance) { return switch (swerveInstance.label()) { diff --git a/src/main/java/competition/electrical_contract/Contract2026.java b/src/main/java/competition/electrical_contract/Contract2026.java index a5886588..e2195d90 100644 --- a/src/main/java/competition/electrical_contract/Contract2026.java +++ b/src/main/java/competition/electrical_contract/Contract2026.java @@ -31,70 +31,40 @@ import java.util.HashMap; import java.util.List; import java.util.Map; +import java.util.Set; import static edu.wpi.first.units.Units.Amps; import static edu.wpi.first.units.Units.Inches; import static edu.wpi.first.units.Units.Seconds; -public class Contract2026 extends ElectricalContract { - - protected final double simulationScalingValue = 256.0 * PoseSubsystem.INCHES_IN_A_METER; +public class Contract2026 extends GeneralContract { @Inject - public Contract2026() {} - - @Override - public boolean isDriveReady() { return true; } - - @Override - public boolean areCanCodersReady() { return true; } - - @Override - public boolean isClimberLeftReady() { return false; } - - @Override - public CANMotorControllerInfo getClimberMotorLeft() { - return new CANMotorControllerInfo("ClimberMotorLeft", - MotorControllerType.TalonFx, - CANBusId.Canivore, - 25, - PDHPort.PDH05, - new TalonFxMotorControllerOutputConfig() - .withStatorCurrentLimit(Amps.of(80)) - .withSupplyCurrentLimit(Amps.of(40), Amps.of(60), Seconds.of(1))); + public Contract2026() { + super(Set.of( + Hardware.LeftShooter, + Hardware.MiddleShooter, + Hardware.RightShooter, + Hardware.ShooterFeeder, + Hardware.IntakeDeploy, + Hardware.IntakeDeployEncoder, + Hardware.FuelIntake, + Hardware.HopperRoller, + Hardware.HoodServoLeft, + Hardware.HoodServoRight, + Hardware.Lights + )); } - @Override - public boolean isClimberRightReady() { return false; } - - @Override - public CANMotorControllerInfo getClimberMotorRight() { - return new CANMotorControllerInfo("ClimberMotorRight", - MotorControllerType.TalonFx, - CANBusId.Canivore, - 26, - PDHPort.PDH06, - new TalonFxMotorControllerOutputConfig() - .withInversionType(CANMotorControllerOutputConfig.InversionType.Inverted) - .withStatorCurrentLimit(Amps.of(80)) - .withSupplyCurrentLimit(Amps.of(40), Amps.of(60), Seconds.of(1))); + protected Contract2026(Set readinessSet) { + super(readinessSet); } @Override - public boolean isClimberAbsoluteEncoderReady() { return false; } - - @Override - public DeviceInfo getClimberAbsoluteEncoder() { - return new DeviceInfo("ClimberAbsoluteEncoderReady", CANBusId.Canivore, 59); - } - - @Override - public boolean isClimberSensorReady() { return false; } + public boolean isDriveReady() { return true; } @Override - public DeviceInfo getClimberSensor() { - return new DeviceInfo("ClimberSensor", 0, PowerSource.RIO); - } + public boolean areCanCodersReady() { return true; } @Override public IMUInfo getIMUInfo() { @@ -109,183 +79,6 @@ public DeviceInfo candle() { return new DeviceInfo("CANdle",CANBusId.Canivore, 57); } - @Override - public boolean isShooterFeederReady() { return true; } - - public CANMotorControllerInfo getShooterFeederMotor() { - return new CANMotorControllerInfo("ShooterFeederMotor", - MotorControllerType.TalonFx, - CANBusId.Canivore, - 37, - PDHPort.PDH17, - new TalonFxMotorControllerOutputConfig() - .withInversionType(CANMotorControllerOutputConfig.InversionType.Inverted) - .withStatorCurrentLimit(Amps.of(35))); - } - - @Override - public boolean isLeftShooterReady() { return true; } - - @Override - public boolean isMiddleShooterReady() { return true; } - - @Override - public boolean isRightShooterReady() { return true; } - - @Override - public CANMotorControllerInfo getLeftShooterMotor() { - return new CANMotorControllerInfo("ShooterLeftMotor", - MotorControllerType.TalonFx, - CANBusId.Canivore, - 22, - PDHPort.PDH02, - new TalonFxMotorControllerOutputConfig() - .withStatorCurrentLimit(Amps.of(50)) - .withSupplyCurrentLimit( - Amps.of(40), - Amps.of(60), - Seconds.of(1))); - } - - @Override - public CANMotorControllerInfo getMiddleShooterMotor() { - return new CANMotorControllerInfo("ShooterMiddleMotor", - MotorControllerType.TalonFx, - CANBusId.Canivore, - 23, - PDHPort.PDH03, - new TalonFxMotorControllerOutputConfig() - .withInversionType(CANMotorControllerOutputConfig.InversionType.Normal) - .withStatorCurrentLimit(Amps.of(50)) - .withSupplyCurrentLimit( - Amps.of(40), - Amps.of(60), - Seconds.of(1))); - } - - @Override - public CANMotorControllerInfo getRightShooterMotor() { - return new CANMotorControllerInfo("ShooterRightMotor", - MotorControllerType.TalonFx, - CANBusId.Canivore, - 24, - PDHPort.PDH04, - new TalonFxMotorControllerOutputConfig() - .withInversionType(CANMotorControllerOutputConfig.InversionType.Inverted) - .withStatorCurrentLimit(Amps.of(50)) - .withSupplyCurrentLimit( - Amps.of(40), - Amps.of(60), - Seconds.of(1))); - } - - @Override - public boolean isIntakeDeployReady() { return true; } - - @Override - public CANMotorControllerInfo getIntakeDeployMotor() { - return new CANMotorControllerInfo("IntakeDeployMotor", - MotorControllerType.TalonFx, - CANBusId.Canivore, - 34, - PDHPort.PDH14, - new TalonFxMotorControllerOutputConfig() - .withSupplyCurrentLimit(Amps.of(15), Amps.of(30), Seconds.of(1)) - .withStatorCurrentLimit(Amps.of(50)) - .withRemoteCanCoderFeedback(getIntakeDeployAbsoluteEncoder().channel)); - } - - @Override - public boolean isIntakeDeployAbsoluteEncoderReady() { return true; } - - @Override - public DeviceInfo getIntakeDeployAbsoluteEncoder() { - return new DeviceInfo("IntakeDeployAbsoluteEncoderReady", CANBusId.Canivore, 58); - } - - @Override - public boolean isHoodServoLeftReady() { return true; } - - @Override - public DeviceInfo getHoodServoLeft() { - return new DeviceInfo("HoodServoLeft", 7); - } - - @Override - public boolean isHoodServoRightReady() { return true;} - - @Override - public DeviceInfo getHoodServoRight() { - return new DeviceInfo("HoodServoRight", 9); - } - - // OrangePis - powered via buck converters (see getAdditionalPowerBranches) - public DeviceInfo getFrontOrangePi() { - return new DeviceInfo("FrontOrangePi", -1, PowerSource.NONE); - } - - public DeviceInfo getBackOrangePi() { - return new DeviceInfo("BackOrangePi", -1, PowerSource.NONE); - } - - // Orin Nano - powered via FrontBuckBoost_Pwr (see getAdditionalPowerBranches) - public DeviceInfo getOrinNano() { - return new DeviceInfo("Orin_Nano", -1, PowerSource.NONE); - } - - // Ethernet switch - powered via BackBuckBoost_Pwr (see getAdditionalPowerBranches) - public DeviceInfo getEthernetSwitch() { - return new DeviceInfo("EthernetSwitch", -1, PowerSource.NONE); - } - - // VRM1 12V/2A outputs - public DeviceInfo getVrm1_12v_2a() { - return new DeviceInfo("FrontBuckBoost_Pwr", -1, PowerSource.VRM1_12V_2A); - } - - public DeviceInfo getVrm1_12v_2b() { - return new DeviceInfo("BackBuckBoost_Pwr", -1, PowerSource.VRM1_12V_2B); - } - - // VRM1 12V/500mA outputs - public DeviceInfo getVrm1_12v_500ma() { - return new DeviceInfo("FrontBuckBoost_Fan", -1, PowerSource.VRM1_12V_500MA); - } - - public DeviceInfo getVrm1_12v_500mb() { - return new DeviceInfo("BackBuckBoost_Fan", -1, PowerSource.VRM1_12V_500MB); - } - - // VRM1 5V/2A outputs - unused - public DeviceInfo getVrm1_5v_2a() { - return new DeviceInfo("No_Connect", -1, PowerSource.VRM1_5V_2A); - } - - public DeviceInfo getVrm1_5v_2b() { - return new DeviceInfo("No_Connect", -1, PowerSource.VRM1_5V_2B); - } - - // VRM1 5V/500mA outputs - public DeviceInfo getVrm1_5v_500ma() { - return new DeviceInfo("CANdle", -1, PowerSource.VRM1_5V_500MA); - } - - public DeviceInfo getVrm1_5v_500mb() { - return new DeviceInfo("No_Connect", -1, PowerSource.VRM1_5V_500MB); - } - - protected String getDriveControllerName(SwerveInstance swerveInstance) { - return "DriveSubsystem/" + swerveInstance.label() + "/Drive"; - } - - protected String getSteeringControllerName(SwerveInstance swerveInstance) { - return "DriveSubsystem/" + swerveInstance.label() + "/Steering"; - } - - protected String getSteeringEncoderControllerName(SwerveInstance swerveInstance) { - return "DriveSubsystem/" + swerveInstance.label() + "/SteeringEncoder"; - } - @Override public CANMotorControllerInfo getDriveMotor(SwerveInstance swerveInstance) { var motorConfig = new TalonFxMotorControllerOutputConfig() @@ -330,7 +123,6 @@ public CANMotorControllerInfo getDriveMotor(SwerveInstance swerveInstance) { @Override public CANMotorControllerInfo getSteeringMotor(SwerveInstance swerveInstance) { - double simulationScalingValue = 1.0; var motorConfig = new TalonFxMotorControllerOutputConfig() .withInversionType(CANMotorControllerOutputConfig.InversionType.Inverted) .withStatorCurrentLimit(Amps.of(40)); @@ -372,47 +164,6 @@ public CANMotorControllerInfo getSteeringMotor(SwerveInstance swerveInstance) { }; } - @Override - public boolean isFuelIntakeMotorReady() { return true; } - - @Override - public CANMotorControllerInfo getFuelIntakeMotor() { - return new CANMotorControllerInfo("FuelIntakeMotor", - MotorControllerType.TalonFx, - CANBusId.Canivore, - 32, - PDHPort.PDH12, - new TalonFxMotorControllerOutputConfig() - .withInversionType(CANMotorControllerOutputConfig.InversionType.Inverted) - .withStatorCurrentLimit(Amps.of(70))); - } - - @Override - public boolean isLightsReady() { return true; } - - @Override - public CANLightControllerInfo getLightControllerInfo() { - return new CANLightControllerInfo("Lights", - LightControllerType.Candle, CANBusId.Canivore, - 57, new CANLightControllerOutputConfig(LEDStripType.GRB, - 0.15, new int[] {8, 30, 30})); - - } - - @Override - public boolean isHopperRollerReady() { return true; } - - @Override - public CANMotorControllerInfo getHopperRollerMotor() { - return new CANMotorControllerInfo("HopperRoller", - MotorControllerType.TalonFx, - CANBusId.Canivore, - 33, - PDHPort.PDH13, - new TalonFxMotorControllerOutputConfig() - .withStatorCurrentLimit(Amps.of(40))); - } - @Override public DeviceInfo getSteeringEncoder(SwerveInstance swerveInstance) { double simulationScalingValue = 1.0; @@ -441,8 +192,6 @@ public Translation2d getSwerveModuleOffsets(SwerveInstance swerveInstance) { }; } - - @Override public double getSteeringGearRatio() { return 12.1; // Documented value for WCP x2i. diff --git a/src/main/java/competition/electrical_contract/ElectricalContract.java b/src/main/java/competition/electrical_contract/ElectricalContract.java index c3668c3b..72871d9c 100644 --- a/src/main/java/competition/electrical_contract/ElectricalContract.java +++ b/src/main/java/competition/electrical_contract/ElectricalContract.java @@ -14,80 +14,49 @@ import java.util.HashMap; import java.util.List; import java.util.Map; +import java.util.Set; public abstract class ElectricalContract implements XSwerveDriveElectricalContract, XCameraElectricalContract { - public abstract boolean isDriveReady(); + private final Set readinessSet; + + protected ElectricalContract(Set readinessSet) { + this.readinessSet = readinessSet; + } + + public boolean isReady(Hardware hardware) { + return readinessSet.contains(hardware); + } + // TODO: Remove these later + public abstract boolean isDriveReady(); public abstract boolean areCanCodersReady(); public abstract CANMotorControllerInfo getDriveMotor(SwerveInstance swerveInstance); - public abstract CANMotorControllerInfo getSteeringMotor(SwerveInstance swerveInstance); - public abstract DeviceInfo getSteeringEncoder(SwerveInstance swerveInstance); - public abstract Translation2d getSwerveModuleOffsets(SwerveInstance swerveInstance); - public abstract IMUInfo getIMUInfo(); - public abstract boolean isLeftShooterReady(); - public abstract CANMotorControllerInfo getLeftShooterMotor(); - - public abstract boolean isMiddleShooterReady(); - public abstract CANMotorControllerInfo getMiddleShooterMotor(); - - public abstract boolean isRightShooterReady(); - public abstract CANMotorControllerInfo getRightShooterMotor(); - public abstract boolean isHoodServoLeftReady(); - public abstract DeviceInfo getHoodServoLeft(); - - public abstract boolean isHoodServoRightReady(); - public abstract DeviceInfo getHoodServoRight(); - public abstract boolean isIntakeDeployReady(); - public abstract CANMotorControllerInfo getIntakeDeployMotor(); - - public abstract boolean isIntakeDeployAbsoluteEncoderReady(); - public abstract DeviceInfo getIntakeDeployAbsoluteEncoder(); - public abstract boolean isClimberLeftReady(); - public abstract CANMotorControllerInfo getClimberMotorLeft(); - - public abstract boolean isClimberRightReady(); - public abstract CANMotorControllerInfo getClimberMotorRight(); - - public abstract boolean isClimberAbsoluteEncoderReady(); - - public abstract DeviceInfo getClimberAbsoluteEncoder(); - - public abstract boolean isClimberSensorReady(); - public abstract DeviceInfo getClimberSensor(); - public abstract boolean isShooterFeederReady(); - public abstract CANMotorControllerInfo getShooterFeederMotor(); - public abstract boolean isFuelIntakeMotorReady(); - public abstract CANMotorControllerInfo getFuelIntakeMotor(); - public abstract boolean isLightsReady(); - public abstract CANLightControllerInfo getLightControllerInfo(); - public abstract boolean isHopperRollerReady(); - public abstract CANMotorControllerInfo getHopperRollerMotor(); /** @@ -109,4 +78,16 @@ public Map> getAdditionalPowerBranches() { } public abstract Distance getRadiusOfRobot(); + + protected String getDriveControllerName(SwerveInstance swerveInstance) { + return "DriveSubsystem/" + swerveInstance.label() + "/Drive"; + } + + protected String getSteeringControllerName(SwerveInstance swerveInstance) { + return "DriveSubsystem/" + swerveInstance.label() + "/Steering"; + } + + protected String getSteeringEncoderControllerName(SwerveInstance swerveInstance) { + return "DriveSubsystem/" + swerveInstance.label() + "/SteeringEncoder"; + } } diff --git a/src/main/java/competition/electrical_contract/GeneralContract.java b/src/main/java/competition/electrical_contract/GeneralContract.java new file mode 100644 index 00000000..07bf65ab --- /dev/null +++ b/src/main/java/competition/electrical_contract/GeneralContract.java @@ -0,0 +1,230 @@ +package competition.electrical_contract; + +import xbot.common.injection.electrical_contract.CANBusId; +import xbot.common.injection.electrical_contract.CANLightControllerInfo; +import xbot.common.injection.electrical_contract.CANLightControllerOutputConfig; +import xbot.common.injection.electrical_contract.CANMotorControllerInfo; +import xbot.common.injection.electrical_contract.CANMotorControllerOutputConfig; +import xbot.common.injection.electrical_contract.DeviceInfo; +import xbot.common.injection.electrical_contract.LEDStripType; +import xbot.common.injection.electrical_contract.LightControllerType; +import xbot.common.injection.electrical_contract.MotorControllerType; +import xbot.common.injection.electrical_contract.PDHPort; +import xbot.common.injection.electrical_contract.PowerSource; +import xbot.common.injection.electrical_contract.TalonFxMotorControllerOutputConfig; + +import java.util.Set; + +import static edu.wpi.first.units.Units.Amps; +import static edu.wpi.first.units.Units.Seconds; + +public abstract class GeneralContract extends ElectricalContract { + + protected GeneralContract(Set readinessSet) { + super(readinessSet); + } + + @Override + public DeviceInfo getHoodServoLeft() { + return new DeviceInfo("HoodServoLeft", 7); + } + + @Override + public DeviceInfo getHoodServoRight() { + return new DeviceInfo("HoodServoRight", 9); + } + + @Override + public CANMotorControllerInfo getShooterFeederMotor() { + return new CANMotorControllerInfo("ShooterFeederMotor", + MotorControllerType.TalonFx, + CANBusId.Canivore, + 37, + PDHPort.PDH17, + new TalonFxMotorControllerOutputConfig() + .withInversionType(CANMotorControllerOutputConfig.InversionType.Inverted) + .withStatorCurrentLimit(Amps.of(35))); + } + + @Override + public CANMotorControllerInfo getLeftShooterMotor() { + return new CANMotorControllerInfo("ShooterLeftMotor", + MotorControllerType.TalonFx, + CANBusId.Canivore, + 22, + PDHPort.PDH02, + new TalonFxMotorControllerOutputConfig() + .withStatorCurrentLimit(Amps.of(50)) + .withSupplyCurrentLimit( + Amps.of(40), + Amps.of(60), + Seconds.of(1))); + } + + @Override + public CANMotorControllerInfo getMiddleShooterMotor() { + return new CANMotorControllerInfo("ShooterMiddleMotor", + MotorControllerType.TalonFx, + CANBusId.Canivore, + 23, + PDHPort.PDH03, + new TalonFxMotorControllerOutputConfig() + .withInversionType(CANMotorControllerOutputConfig.InversionType.Normal) + .withStatorCurrentLimit(Amps.of(50)) + .withSupplyCurrentLimit( + Amps.of(40), + Amps.of(60), + Seconds.of(1))); + } + + @Override + public CANMotorControllerInfo getRightShooterMotor() { + return new CANMotorControllerInfo("ShooterRightMotor", + MotorControllerType.TalonFx, + CANBusId.Canivore, + 24, + PDHPort.PDH04, + new TalonFxMotorControllerOutputConfig() + .withInversionType(CANMotorControllerOutputConfig.InversionType.Inverted) + .withStatorCurrentLimit(Amps.of(50)) + .withSupplyCurrentLimit( + Amps.of(40), + Amps.of(60), + Seconds.of(1))); + } + + @Override + public CANMotorControllerInfo getIntakeDeployMotor() { + return new CANMotorControllerInfo("IntakeDeployMotor", + MotorControllerType.TalonFx, + CANBusId.Canivore, + 34, + PDHPort.PDH14, + new TalonFxMotorControllerOutputConfig() + .withSupplyCurrentLimit(Amps.of(15), Amps.of(30), Seconds.of(1)) + .withStatorCurrentLimit(Amps.of(50)) + .withRemoteCanCoderFeedback(getIntakeDeployAbsoluteEncoder().channel)); + } + + @Override + public DeviceInfo getIntakeDeployAbsoluteEncoder() { + return new DeviceInfo("IntakeDeployAbsoluteEncoderReady", CANBusId.Canivore, 58); + } + + @Override + public CANMotorControllerInfo getFuelIntakeMotor() { + return new CANMotorControllerInfo("FuelIntakeMotor", + MotorControllerType.TalonFx, + CANBusId.Canivore, + 32, + PDHPort.PDH12, + new TalonFxMotorControllerOutputConfig() + .withInversionType(CANMotorControllerOutputConfig.InversionType.Inverted) + .withStatorCurrentLimit(Amps.of(70))); + } + + @Override + public CANLightControllerInfo getLightControllerInfo() { + return new CANLightControllerInfo("Lights", + LightControllerType.Candle, CANBusId.Canivore, + 57, new CANLightControllerOutputConfig(LEDStripType.GRB, + 0.15, new int[] {8, 30, 30})); + + } + + @Override + public CANMotorControllerInfo getHopperRollerMotor() { + return new CANMotorControllerInfo("HopperRoller", + MotorControllerType.TalonFx, + CANBusId.Canivore, + 33, + PDHPort.PDH13, + new TalonFxMotorControllerOutputConfig() + .withStatorCurrentLimit(Amps.of(40))); + } + + @Override + public CANMotorControllerInfo getClimberMotorLeft() { + return new CANMotorControllerInfo("ClimberMotorLeft", + MotorControllerType.TalonFx, + CANBusId.Canivore, + 25, + PDHPort.PDH05, + new TalonFxMotorControllerOutputConfig() + .withStatorCurrentLimit(Amps.of(80)) + .withSupplyCurrentLimit(Amps.of(40), Amps.of(60), Seconds.of(1))); + } + + @Override + public CANMotorControllerInfo getClimberMotorRight() { + return new CANMotorControllerInfo("ClimberMotorRight", + MotorControllerType.TalonFx, + CANBusId.Canivore, + 26, + PDHPort.PDH06, + new TalonFxMotorControllerOutputConfig() + .withInversionType(CANMotorControllerOutputConfig.InversionType.Inverted) + .withStatorCurrentLimit(Amps.of(80)) + .withSupplyCurrentLimit(Amps.of(40), Amps.of(60), Seconds.of(1))); + } + + @Override + public DeviceInfo getClimberSensor() { + return new DeviceInfo("ClimberSensor", 0, PowerSource.RIO); + } + + // OrangePis - powered via buck converters (see getAdditionalPowerBranches) + public DeviceInfo getFrontOrangePi() { + return new DeviceInfo("FrontOrangePi", -1, PowerSource.NONE); + } + + public DeviceInfo getBackOrangePi() { + return new DeviceInfo("BackOrangePi", -1, PowerSource.NONE); + } + + // Orin Nano - powered via FrontBuckBoost_Pwr (see getAdditionalPowerBranches) + public DeviceInfo getOrinNano() { + return new DeviceInfo("Orin_Nano", -1, PowerSource.NONE); + } + + // Ethernet switch - powered via BackBuckBoost_Pwr (see getAdditionalPowerBranches) + public DeviceInfo getEthernetSwitch() { + return new DeviceInfo("EthernetSwitch", -1, PowerSource.NONE); + } + + // VRM1 12V/2A outputs + public DeviceInfo getVrm1_12v_2a() { + return new DeviceInfo("FrontBuckBoost_Pwr", -1, PowerSource.VRM1_12V_2A); + } + + public DeviceInfo getVrm1_12v_2b() { + return new DeviceInfo("BackBuckBoost_Pwr", -1, PowerSource.VRM1_12V_2B); + } + + // VRM1 12V/500mA outputs + public DeviceInfo getVrm1_12v_500ma() { + return new DeviceInfo("FrontBuckBoost_Fan", -1, PowerSource.VRM1_12V_500MA); + } + + public DeviceInfo getVrm1_12v_500mb() { + return new DeviceInfo("BackBuckBoost_Fan", -1, PowerSource.VRM1_12V_500MB); + } + + // VRM1 5V/2A outputs - unused + public DeviceInfo getVrm1_5v_2a() { + return new DeviceInfo("No_Connect", -1, PowerSource.VRM1_5V_2A); + } + + public DeviceInfo getVrm1_5v_2b() { + return new DeviceInfo("No_Connect", -1, PowerSource.VRM1_5V_2B); + } + + // VRM1 5V/500mA outputs + public DeviceInfo getVrm1_5v_500ma() { + return new DeviceInfo("CANdle", -1, PowerSource.VRM1_5V_500MA); + } + + public DeviceInfo getVrm1_5v_500mb() { + return new DeviceInfo("No_Connect", -1, PowerSource.VRM1_5V_500MB); + } +} diff --git a/src/main/java/competition/electrical_contract/Hardware.java b/src/main/java/competition/electrical_contract/Hardware.java new file mode 100644 index 00000000..ef4d0292 --- /dev/null +++ b/src/main/java/competition/electrical_contract/Hardware.java @@ -0,0 +1,25 @@ +package competition.electrical_contract; + +public enum Hardware { + LeftShooter, + MiddleShooter, + RightShooter, + + HoodServoLeft, + HoodServoRight, + + IntakeDeploy, + IntakeDeployEncoder, + + ClimberLeft, + ClimberRight, + ClimberSensor, + + ShooterFeeder, + + FuelIntake, + + Lights, + + HopperRoller +} diff --git a/src/main/java/competition/electrical_contract/PracticeContract.java b/src/main/java/competition/electrical_contract/PracticeContract.java deleted file mode 100644 index 27d5620b..00000000 --- a/src/main/java/competition/electrical_contract/PracticeContract.java +++ /dev/null @@ -1,8 +0,0 @@ -package competition.electrical_contract; - -import javax.inject.Inject; - -public class PracticeContract extends Contract2026 { - @Inject - public PracticeContract() {} -} diff --git a/src/main/java/competition/electrical_contract/RoboxContract.java b/src/main/java/competition/electrical_contract/RoboxContract.java index 0df30742..7a9c146a 100644 --- a/src/main/java/competition/electrical_contract/RoboxContract.java +++ b/src/main/java/competition/electrical_contract/RoboxContract.java @@ -22,9 +22,6 @@ public RoboxContract() {} @Override public boolean areCanCodersReady() { return false; } - @Override - public boolean isLeftShooterReady() { return true; } - @Override public CANMotorControllerInfo getLeftShooterMotor() { return new CANMotorControllerInfo("ShooterMotor", diff --git a/src/main/java/competition/electrical_contract/UnitTestCompetitionContract.java b/src/main/java/competition/electrical_contract/UnitTestCompetitionContract.java index 77676e10..0bf5bc3a 100644 --- a/src/main/java/competition/electrical_contract/UnitTestCompetitionContract.java +++ b/src/main/java/competition/electrical_contract/UnitTestCompetitionContract.java @@ -1,73 +1,12 @@ package competition.electrical_contract; import javax.inject.Inject; +import java.util.EnumSet; public class UnitTestCompetitionContract extends Contract2026 { @Inject - public UnitTestCompetitionContract() {} - - @Override - public boolean isShooterFeederReady() { - return true; - } - - @Override - public boolean isLeftShooterReady() { - return true; - } - - @Override - public boolean isMiddleShooterReady() { - return true; - } - - @Override - public boolean isRightShooterReady() { - return true; - } - - @Override - public boolean isFuelIntakeMotorReady() { - return true; - } - - @Override - public boolean isIntakeDeployReady() { - return true; - } - - @Override - public boolean isLightsReady() { - return true; - } - - @Override - public boolean isClimberRightReady() { - return true; - } - - @Override - public boolean isClimberLeftReady() { - return true; - } - - @Override - public boolean isIntakeDeployAbsoluteEncoderReady() { - return true; - } - - @Override - public boolean isClimberSensorReady() {return true;} - - @Override - public boolean isHopperRollerReady() { - return true; + public UnitTestCompetitionContract() { + super(EnumSet.allOf(Hardware.class)); } - - @Override - public boolean isHoodServoLeftReady() {return true;} - - @Override - public boolean isHoodServoRightReady() {return true;} } diff --git a/src/main/java/competition/subsystems/climber/ClimberSubsystem.java b/src/main/java/competition/subsystems/climber/ClimberSubsystem.java index a4ddccb8..ed717da6 100644 --- a/src/main/java/competition/subsystems/climber/ClimberSubsystem.java +++ b/src/main/java/competition/subsystems/climber/ClimberSubsystem.java @@ -1,6 +1,7 @@ package competition.subsystems.climber; import competition.electrical_contract.ElectricalContract; +import competition.electrical_contract.Hardware; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.wpilibj2.command.Command; import xbot.common.command.BaseSetpointSubsystem; @@ -66,7 +67,7 @@ public ClimberSubsystem(XCANMotorController.XCANMotorControllerFactory motorFact .withMaxPowerOutput(1.0) .build(); - if (electricalContract.isClimberLeftReady() && electricalContract.isClimberRightReady()) { + if (electricalContract.isReady(Hardware.ClimberLeft) && electricalContract.isReady(Hardware.ClimberRight)) { this.climberMotorLeft = motorFactory.create(electricalContract.getClimberMotorLeft(), getPrefix(), "ClimberMotorPID", defaultPIDProperties); @@ -82,7 +83,7 @@ public ClimberSubsystem(XCANMotorController.XCANMotorControllerFactory motorFact this.climberMotorRight = null; } - if (electricalContract.isClimberSensorReady()) { + if (electricalContract.isReady(Hardware.ClimberSensor)) { this.climberSensor = xDigitalInputFactory.create( electricalContract.getClimberSensor(), this.getPrefix()); diff --git a/src/main/java/competition/subsystems/collector_intake/CollectorSubsystem.java b/src/main/java/competition/subsystems/collector_intake/CollectorSubsystem.java index 4d6b3278..05fee6fa 100644 --- a/src/main/java/competition/subsystems/collector_intake/CollectorSubsystem.java +++ b/src/main/java/competition/subsystems/collector_intake/CollectorSubsystem.java @@ -1,6 +1,7 @@ package competition.subsystems.collector_intake; import competition.electrical_contract.ElectricalContract; +import competition.electrical_contract.Hardware; import xbot.common.command.BaseSubsystem; import xbot.common.controls.actuators.XCANMotorController; import xbot.common.controls.actuators.XCANMotorControllerPIDProperties; @@ -41,7 +42,7 @@ public CollectorSubsystem(ElectricalContract electricalContract, .withMaxPowerOutput(1.0) .build(); - if (electricalContract.isFuelIntakeMotorReady()) { + if (electricalContract.isReady(Hardware.FuelIntake)) { this.collectorMotor = motorFactory.create( electricalContract.getFuelIntakeMotor(), getPrefix(), @@ -97,7 +98,7 @@ public void stop() { @Override public void periodic() { - if (electricalContract.isFuelIntakeMotorReady()) { + if (collectorMotor != null) { collectorMotor.periodic(); } } diff --git a/src/main/java/competition/subsystems/hood/HoodSubsystem.java b/src/main/java/competition/subsystems/hood/HoodSubsystem.java index 1934e458..f1ae86ec 100644 --- a/src/main/java/competition/subsystems/hood/HoodSubsystem.java +++ b/src/main/java/competition/subsystems/hood/HoodSubsystem.java @@ -2,6 +2,7 @@ import competition.electrical_contract.ElectricalContract; +import competition.electrical_contract.Hardware; import edu.wpi.first.units.measure.Time; import edu.wpi.first.wpilibj2.command.Command; import xbot.common.command.BaseSetpointSubsystem; @@ -56,7 +57,7 @@ public HoodSubsystem(XServo.XServoFactory servoFactory, propertyFactory.setPrefix(this); this.electricalContract = electricalContract; - if (electricalContract.isHoodServoLeftReady()) { + if (electricalContract.isReady(Hardware.HoodServoLeft)) { var servoLeft = servoFactory.create( electricalContract.getHoodServoLeft().channel, getName() + "/Servo"); registerDataFrameRefreshable(servoLeft); @@ -69,7 +70,7 @@ public HoodSubsystem(XServo.XServoFactory servoFactory, hoodServoLeft = null; } - if (electricalContract.isHoodServoRightReady()) { + if (electricalContract.isReady(Hardware.HoodServoRight)) { var servoRight = servoFactory.create( electricalContract.getHoodServoRight().channel, getName() + "/Servo"); registerDataFrameRefreshable(servoRight); diff --git a/src/main/java/competition/subsystems/hopper_roller/HopperRollerSubsystem.java b/src/main/java/competition/subsystems/hopper_roller/HopperRollerSubsystem.java index a991234a..40d9604d 100644 --- a/src/main/java/competition/subsystems/hopper_roller/HopperRollerSubsystem.java +++ b/src/main/java/competition/subsystems/hopper_roller/HopperRollerSubsystem.java @@ -1,6 +1,7 @@ package competition.subsystems.hopper_roller; import competition.electrical_contract.ElectricalContract; +import competition.electrical_contract.Hardware; import edu.wpi.first.wpilibj2.command.Command; import xbot.common.command.BaseSubsystem; import xbot.common.command.NamedRunCommand; @@ -39,7 +40,7 @@ public HopperRollerSubsystem(ElectricalContract electricalContract, pf.setPrefix(this); this.electricalContract = electricalContract; this.voltageRampTime = pf.createPersistentProperty("VoltageRampTime", 0.1); - if (electricalContract.isHopperRollerReady()) { + if (electricalContract.isReady(Hardware.HopperRoller)) { this.hopperRollerMotor = motorFactory.create( electricalContract.getHopperRollerMotor(), getPrefix(), diff --git a/src/main/java/competition/subsystems/intake_deploy/IntakeDeploySubsystem.java b/src/main/java/competition/subsystems/intake_deploy/IntakeDeploySubsystem.java index 1b6e7e06..d99ac8ad 100644 --- a/src/main/java/competition/subsystems/intake_deploy/IntakeDeploySubsystem.java +++ b/src/main/java/competition/subsystems/intake_deploy/IntakeDeploySubsystem.java @@ -1,6 +1,7 @@ package competition.subsystems.intake_deploy; import competition.electrical_contract.ElectricalContract; +import competition.electrical_contract.Hardware; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.Current; import edu.wpi.first.wpilibj2.command.Command; @@ -56,7 +57,7 @@ public IntakeDeploySubsystem(XCANMotorController.XCANMotorControllerFactory xcan this.voltageRampTime = propertyFactory.createPersistentProperty("VoltageRampTime", 0.1); - if (electricalContract.isIntakeDeployReady()) { + if (electricalContract.isReady(Hardware.IntakeDeploy)) { this.intakeDeployMotor = xcanMotorControllerFactory.create(electricalContract.getIntakeDeployMotor(), getPrefix(), "IntakeDeployPID", defaultPIDProperties); this.intakeDeployMotor.setOpenLoopRampRates( @@ -70,7 +71,7 @@ public IntakeDeploySubsystem(XCANMotorController.XCANMotorControllerFactory xcan this.intakeDeployMotor = null; } - if (electricalContract.isIntakeDeployAbsoluteEncoderReady()) { + if (electricalContract.isReady(Hardware.IntakeDeployEncoder)) { this.intakeDeployEncoder = xAbsoluteEncoderFactory.create( electricalContract.getIntakeDeployAbsoluteEncoder(), this.getPrefix() diff --git a/src/main/java/competition/subsystems/lights/LightsSubsystem.java b/src/main/java/competition/subsystems/lights/LightsSubsystem.java index 93700bfd..63567780 100644 --- a/src/main/java/competition/subsystems/lights/LightsSubsystem.java +++ b/src/main/java/competition/subsystems/lights/LightsSubsystem.java @@ -2,6 +2,7 @@ import com.ctre.phoenix6.signals.LarsonBounceValue; import competition.electrical_contract.ElectricalContract; +import competition.electrical_contract.Hardware; import competition.subsystems.hood.HoodSubsystem; import competition.subsystems.intake_deploy.IntakeDeploySubsystem; import competition.subsystems.vision.AprilTagVisionSubsystemExtended; @@ -41,7 +42,7 @@ public LightsSubsystem(XCANLightController.XCANLightControllerFactory lightsFact this.voltageMonitor = voltageMonitor; this.assertionManager = assertionManager; this.vision = vision; - if (electricalContract.isLightsReady()) { + if (electricalContract.isReady(Hardware.Lights)) { this.lights = lightsFactory.create( electricalContract.getLightControllerInfo() ); diff --git a/src/main/java/competition/subsystems/shooter/ShooterSubsystem.java b/src/main/java/competition/subsystems/shooter/ShooterSubsystem.java index fc24d98f..eb3d36d8 100644 --- a/src/main/java/competition/subsystems/shooter/ShooterSubsystem.java +++ b/src/main/java/competition/subsystems/shooter/ShooterSubsystem.java @@ -1,6 +1,7 @@ package competition.subsystems.shooter; import competition.electrical_contract.ElectricalContract; +import competition.electrical_contract.Hardware; import competition.subsystems.shooter.commands.WaitForShooterAtGoalCommand; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.wpilibj2.command.Command; @@ -82,7 +83,7 @@ public ShooterSubsystem(XCANMotorController.XCANMotorControllerFactory xcanMotor this.voltageRampTime = propertyFactory.createPersistentProperty("VoltageRampTime", 0.2); - if (electricalContract.isLeftShooterReady()) { + if (electricalContract.isReady(Hardware.LeftShooter)) { this.leftShooterMotor = xcanMotorControllerFactory.create(electricalContract.getLeftShooterMotor(), getPrefix(), "leftShooterMotor", shooterLeftMotorDefaultPIDProperties); this.leftShooterMotor.setClosedLoopRampRates( @@ -93,7 +94,7 @@ public ShooterSubsystem(XCANMotorController.XCANMotorControllerFactory xcanMotor this.leftShooterMotor = null; } - if (electricalContract.isMiddleShooterReady()) { + if (electricalContract.isReady(Hardware.MiddleShooter)) { this.middleShooterMotor = xcanMotorControllerFactory.create(electricalContract.getMiddleShooterMotor(), getPrefix(), "middleShooterMotor", shooterMiddleMotorDefaultPIDProperties); this.middleShooterMotor.setClosedLoopRampRates( @@ -104,7 +105,7 @@ public ShooterSubsystem(XCANMotorController.XCANMotorControllerFactory xcanMotor this.middleShooterMotor = null; } - if (electricalContract.isRightShooterReady()) { + if (electricalContract.isReady(Hardware.RightShooter)) { this.rightShooterMotor = xcanMotorControllerFactory.create(electricalContract.getRightShooterMotor(), getPrefix(), "rightShooterMotor", shooterRightMotorDefaultPIDProperties); this.rightShooterMotor.setClosedLoopRampRates( diff --git a/src/main/java/competition/subsystems/shooter_feeder/ShooterFeederSubsystem.java b/src/main/java/competition/subsystems/shooter_feeder/ShooterFeederSubsystem.java index c431dff2..5738b20f 100644 --- a/src/main/java/competition/subsystems/shooter_feeder/ShooterFeederSubsystem.java +++ b/src/main/java/competition/subsystems/shooter_feeder/ShooterFeederSubsystem.java @@ -1,6 +1,7 @@ package competition.subsystems.shooter_feeder; import competition.electrical_contract.ElectricalContract; +import competition.electrical_contract.Hardware; import edu.wpi.first.units.measure.Current; import xbot.common.command.BaseSubsystem; import javax.inject.Inject; @@ -40,7 +41,7 @@ public ShooterFeederSubsystem(ElectricalContract electricalContract, this.voltageRampTime = pf.createPersistentProperty("VoltageRampTime", 0.1); - if (electricalContract.isShooterFeederReady()) { + if (electricalContract.isReady(Hardware.ShooterFeeder)) { this.shooterFeederMotor = motorFactory.create(electricalContract.getShooterFeederMotor(), getPrefix(), "ShooterFeederMotorPID", defaultPIDProperties); this.shooterFeederMotor.setOpenLoopRampRates(