diff --git a/src/main/java/competition/electrical_contract/CompetitionContract.java b/src/main/java/competition/electrical_contract/CompetitionContract.java index 9965b5b7..0667cc2f 100644 --- a/src/main/java/competition/electrical_contract/CompetitionContract.java +++ b/src/main/java/competition/electrical_contract/CompetitionContract.java @@ -30,15 +30,15 @@ public boolean areCanCodersReady() { return true; } - private String getDriveControllerName(SwerveInstance swerveInstance) { + protected String getDriveControllerName(SwerveInstance swerveInstance) { return "DriveSubsystem/" + swerveInstance.label() + "/Drive"; } - private String getSteeringControllerName(SwerveInstance swerveInstance) { + protected String getSteeringControllerName(SwerveInstance swerveInstance) { return "DriveSubsystem/" + swerveInstance.label() + "/Steering"; } - private String getSteeringEncoderControllerName(SwerveInstance swerveInstance) { + protected String getSteeringEncoderControllerName(SwerveInstance swerveInstance) { return "DriveSubsystem/" + swerveInstance.label() + "/SteeringEncoder"; } @@ -46,13 +46,13 @@ private String getSteeringEncoderControllerName(SwerveInstance swerveInstance) { public DeviceInfo getDriveMotor(SwerveInstance swerveInstance) { return switch (swerveInstance.label()) { case "FrontLeftDrive" -> - new DeviceInfo(getDriveControllerName(swerveInstance), 31, false, simulationScalingValue); + new DeviceInfo(getDriveControllerName(swerveInstance), 20, false, simulationScalingValue); case "FrontRightDrive" -> new DeviceInfo(getDriveControllerName(swerveInstance), 29, false, simulationScalingValue); case "RearLeftDrive" -> - new DeviceInfo(getDriveControllerName(swerveInstance), 38, false, simulationScalingValue); + new DeviceInfo(getDriveControllerName(swerveInstance), 31, false, simulationScalingValue); case "RearRightDrive" -> - new DeviceInfo(getDriveControllerName(swerveInstance), 21, false, simulationScalingValue); + new DeviceInfo(getDriveControllerName(swerveInstance), 39, false, simulationScalingValue); default -> null; }; } @@ -63,13 +63,13 @@ public DeviceInfo getSteeringMotor(SwerveInstance swerveInstance) { return switch (swerveInstance.label()) { case "FrontLeftDrive" -> - new DeviceInfo(getSteeringControllerName(swerveInstance), 30, false, simulationScalingValue); - case "FrontRightDrive" -> new DeviceInfo(getSteeringControllerName(swerveInstance), 28, false, simulationScalingValue); + case "FrontRightDrive" -> + new DeviceInfo(getSteeringControllerName(swerveInstance), 21, false, simulationScalingValue); case "RearLeftDrive" -> - new DeviceInfo(getSteeringControllerName(swerveInstance), 39, false, simulationScalingValue); + new DeviceInfo(getSteeringControllerName(swerveInstance), 30, false, simulationScalingValue); case "RearRightDrive" -> - new DeviceInfo(getSteeringControllerName(swerveInstance), 20, false, simulationScalingValue); + new DeviceInfo(getSteeringControllerName(swerveInstance), 38, false, simulationScalingValue); default -> null; }; } @@ -188,6 +188,9 @@ public DeviceInfo getArmMotorRight() { return new DeviceInfo("ArmMotorRight", 11, true); } + @Override + public DeviceInfo getBrakeSolenoid(){return new DeviceInfo("BrakeSolenoid", 1, false);} + @Override public boolean getArmEncoderInverted() { return false; diff --git a/src/main/java/competition/electrical_contract/ElectricalContract.java b/src/main/java/competition/electrical_contract/ElectricalContract.java index 23c284a9..8ef2c3c5 100644 --- a/src/main/java/competition/electrical_contract/ElectricalContract.java +++ b/src/main/java/competition/electrical_contract/ElectricalContract.java @@ -46,12 +46,14 @@ public abstract class ElectricalContract implements XSwerveDriveElectricalContra public abstract DeviceInfo getArmMotorLeft(); public abstract DeviceInfo getArmMotorRight(); + public abstract DeviceInfo getBrakeSolenoid(); public abstract boolean getArmEncoderInverted(); public abstract boolean getArmEncoderIsOnLeftMotor(); public abstract DeviceInfo getCollectorMotor(); + // ShooterSubsystem public abstract DeviceInfo getShooterMotorLeader(); diff --git a/src/main/java/competition/electrical_contract/PracticeContract.java b/src/main/java/competition/electrical_contract/PracticeContract.java index 7a3a9402..d1b382a3 100644 --- a/src/main/java/competition/electrical_contract/PracticeContract.java +++ b/src/main/java/competition/electrical_contract/PracticeContract.java @@ -1,8 +1,107 @@ package competition.electrical_contract; +import xbot.common.injection.electrical_contract.DeviceInfo; +import xbot.common.injection.swerve.SwerveInstance; +import xbot.common.math.XYPair; +import com.ctre.phoenix.motorcontrol.FeedbackDevice; +import competition.subsystems.pose.PoseSubsystem; +import xbot.common.injection.electrical_contract.CANTalonInfo; +import xbot.common.injection.electrical_contract.DeviceInfo; +import xbot.common.injection.swerve.SwerveInstance; +import xbot.common.math.XYPair; + import javax.inject.Inject; public class PracticeContract extends CompetitionContract { @Inject - public PracticeContract() {} + public PracticeContract() { + + } + + public boolean isScoocherReady() { + return false; + } + + @Override + public boolean isArmReady() { + return false; + } + + @Override + public boolean isShooterReady() { + return false; + } + + @Override + public boolean isDriveReady() { + return true; + } + + @Override + public boolean areCanCodersReady() { + return true; + } + + + @Override + public DeviceInfo getDriveMotor(SwerveInstance swerveInstance) { + return switch (swerveInstance.label()) { + case "FrontLeftDrive" -> + new DeviceInfo(getDriveControllerName(swerveInstance), 20, false, simulationScalingValue); + case "FrontRightDrive" -> + new DeviceInfo(getDriveControllerName(swerveInstance), 29, false, simulationScalingValue); + case "RearLeftDrive" -> + new DeviceInfo(getDriveControllerName(swerveInstance), 31, false, simulationScalingValue); + case "RearRightDrive" -> + new DeviceInfo(getDriveControllerName(swerveInstance), 39, false, simulationScalingValue); + default -> null; + }; + } + + @Override + public DeviceInfo getSteeringMotor(SwerveInstance swerveInstance) { + double simulationScalingValue = 1.0; + + return switch (swerveInstance.label()) { + case "FrontLeftDrive" -> + new DeviceInfo(getSteeringControllerName(swerveInstance), 28, false, simulationScalingValue); + case "FrontRightDrive" -> + new DeviceInfo(getSteeringControllerName(swerveInstance), 21, false, simulationScalingValue); + case "RearLeftDrive" -> + new DeviceInfo(getSteeringControllerName(swerveInstance), 30, false, simulationScalingValue); + case "RearRightDrive" -> + new DeviceInfo(getSteeringControllerName(swerveInstance), 38, false, simulationScalingValue); + default -> null; + }; + } + + @Override + public DeviceInfo getSteeringEncoder(SwerveInstance swerveInstance) { + double simulationScalingValue = 1.0; + + return switch (swerveInstance.label()) { + case "FrontLeftDrive" -> + new DeviceInfo(getSteeringEncoderControllerName(swerveInstance), 51, false, simulationScalingValue); + case "FrontRightDrive" -> + new DeviceInfo(getSteeringEncoderControllerName(swerveInstance), 52, false, simulationScalingValue); + case "RearLeftDrive" -> + new DeviceInfo(getSteeringEncoderControllerName(swerveInstance), 53, false, simulationScalingValue); + case "RearRightDrive" -> + new DeviceInfo(getSteeringEncoderControllerName(swerveInstance), 54, false, simulationScalingValue); + default -> null; + }; + } + + @Override + public XYPair getSwerveModuleOffsets(SwerveInstance swerveInstance) { + return switch (swerveInstance.label()) { + case "FrontLeftDrive" -> new XYPair(15, 15); + case "FrontRightDrive" -> new XYPair(15, -15); + case "RearLeftDrive" -> new XYPair(-15, 15); + case "RearRightDrive" -> new XYPair(-15, -15); + default -> new XYPair(0, 0); + }; + } } + + diff --git a/src/main/java/competition/subsystems/SubsystemDefaultCommandMap.java b/src/main/java/competition/subsystems/SubsystemDefaultCommandMap.java index 33d9816f..a45f4da7 100644 --- a/src/main/java/competition/subsystems/SubsystemDefaultCommandMap.java +++ b/src/main/java/competition/subsystems/SubsystemDefaultCommandMap.java @@ -9,6 +9,8 @@ import competition.subsystems.collector.commands.StopCollectorCommand; import competition.subsystems.schoocher.ScoocherSubsystem; import competition.subsystems.schoocher.commands.StopScoocherCommand; +import competition.subsystems.shooter.ShooterWheelSubsystem; +import competition.subsystems.shooter.commands.ShooterWheelMaintainerCommand; import xbot.common.injection.swerve.FrontLeftDrive; import xbot.common.injection.swerve.FrontRightDrive; import xbot.common.injection.swerve.RearLeftDrive; @@ -43,4 +45,9 @@ public void setupScoocherSubsystem(ScoocherSubsystem scoocherSubsystem, StopScoo public void setUpCollectorSubsystem(CollectorSubsystem collectorSubsystem, StopCollectorCommand command) { collectorSubsystem.setDefaultCommand(command); } + + @Inject + public void setupShooterWheelSubsystem(ShooterWheelSubsystem shooterWheelSubsystem, ShooterWheelMaintainerCommand command) { + shooterWheelSubsystem.setDefaultCommand(command); + } } diff --git a/src/main/java/competition/subsystems/arm/ArmSubsystem.java b/src/main/java/competition/subsystems/arm/ArmSubsystem.java index dcae8701..ead57232 100644 --- a/src/main/java/competition/subsystems/arm/ArmSubsystem.java +++ b/src/main/java/competition/subsystems/arm/ArmSubsystem.java @@ -8,8 +8,10 @@ import xbot.common.advantage.DataFrameRefreshable; import xbot.common.command.BaseSetpointSubsystem; import xbot.common.controls.actuators.XCANSparkMax; +import xbot.common.controls.actuators.XSolenoid; import xbot.common.controls.sensors.XSparkAbsoluteEncoder; import xbot.common.math.MathUtils; +import xbot.common.properties.BooleanProperty; import xbot.common.properties.DoubleProperty; import xbot.common.properties.PropertyFactory; @@ -21,9 +23,8 @@ public class ArmSubsystem extends BaseSetpointSubsystem implements DataF public XCANSparkMax armMotorLeft; public XCANSparkMax armMotorRight; - + public XSolenoid armBrakeSolenoid; public XSparkAbsoluteEncoder armAbsoluteEncoder; - public final ElectricalContract contract; public ArmState armState; @@ -50,6 +51,8 @@ public class ArmSubsystem extends BaseSetpointSubsystem implements DataF private double targetAngle; + + public enum ArmState { EXTENDING, RETRACTING, @@ -73,11 +76,13 @@ public enum UsefulArmPosition { @Inject public ArmSubsystem(PropertyFactory pf, XCANSparkMax.XCANSparkMaxFactory sparkMaxFactory, + XSolenoid.XSolenoidFactory xSolenoidFactory, ElectricalContract contract) { - + this.armBrakeSolenoid = xSolenoidFactory.create(contract.getBrakeSolenoid().channel); + // THIS IS FOR END OF DAY COMMIT pf.setPrefix(this); this.contract = contract; - + setArmBrakeSolenoid(false); extendPower = pf.createPersistentProperty("ExtendPower", 0.1); retractPower = pf.createPersistentProperty("RetractPower", 0.1); @@ -190,6 +195,8 @@ public void setPowerToLeftAndRightArms(double leftPower, double rightPower) { armMotorRight.set(rightPower); } } + //brake solenoid + public void setArmBrakeSolenoid(boolean on){armBrakeSolenoid.setOn(on);} @Override public void setPower(Double power) { diff --git a/src/main/java/competition/subsystems/shooter/commands/ShooterWheelMaintainerCommand.java b/src/main/java/competition/subsystems/shooter/commands/ShooterWheelMaintainerCommand.java index c79fc09c..383f21bb 100644 --- a/src/main/java/competition/subsystems/shooter/commands/ShooterWheelMaintainerCommand.java +++ b/src/main/java/competition/subsystems/shooter/commands/ShooterWheelMaintainerCommand.java @@ -6,11 +6,14 @@ import xbot.common.logic.HumanVsMachineDecider; import xbot.common.properties.PropertyFactory; +import javax.inject.Inject; + public class ShooterWheelMaintainerCommand extends BaseMaintainerCommand { final ShooterWheelSubsystem wheel; final OperatorInterface oi; + @Inject public ShooterWheelMaintainerCommand(OperatorInterface oi, ShooterWheelSubsystem wheel, PropertyFactory pf, HumanVsMachineDecider.HumanVsMachineDeciderFactory hvmFactory) { super(wheel, pf, hvmFactory, 0, 0);