diff --git a/src/main/java/org/team1540/bunnybotTank2023/Constants.java b/src/main/java/org/team1540/bunnybotTank2023/Constants.java index a84dec5..b8f62c4 100644 --- a/src/main/java/org/team1540/bunnybotTank2023/Constants.java +++ b/src/main/java/org/team1540/bunnybotTank2023/Constants.java @@ -45,7 +45,8 @@ public static class DrivetrainConstants { } public static class IntakeConstants { - public static final int SOLENOID_PORT_NUMBER = 1; + public static final int SOLENOID_1_PORT_NUMBER = 1; + public static final int SOLENOID_2_PORT_NUMBER = 2; public static final int MOTOR_ID = 5; } diff --git a/src/main/java/org/team1540/bunnybotTank2023/io/intake/IntakeIOReal.java b/src/main/java/org/team1540/bunnybotTank2023/io/intake/IntakeIOReal.java index 08b6895..66d9b75 100644 --- a/src/main/java/org/team1540/bunnybotTank2023/io/intake/IntakeIOReal.java +++ b/src/main/java/org/team1540/bunnybotTank2023/io/intake/IntakeIOReal.java @@ -11,21 +11,24 @@ import javax.naming.ldap.Control; public class IntakeIOReal implements IntakeIO { - private final Solenoid solenoid = new Solenoid(PneumaticsModuleType.CTREPCM, Constants.IntakeConstants.SOLENOID_PORT_NUMBER); + private final Solenoid solenoid1 = new Solenoid(PneumaticsModuleType.CTREPCM, Constants.IntakeConstants.SOLENOID_1_PORT_NUMBER); + private final Solenoid solenoid2 = new Solenoid(PneumaticsModuleType.CTREPCM, Constants.IntakeConstants.SOLENOID_2_PORT_NUMBER); private final VictorSPX motor = new VictorSPX(Constants.IntakeConstants.MOTOR_ID); public IntakeIOReal() { - solenoid.set(false); + solenoid1.set(false); + solenoid2.set(false); motor.setNeutralMode(NeutralMode.Brake); motor.setInverted(false); } @Override public void setFold(boolean fold) { - solenoid.set(fold); + solenoid1.set(fold); + solenoid2.set(fold); } @Override public void updateInputs(IntakeInputs inputs) { - inputs.folded = solenoid.get(); + inputs.folded = solenoid1.get(); inputs.motorAppliedVolts = motor.getMotorOutputVoltage(); inputs.motorCurrentAmps = motor.getMotorOutputPercent(); inputs.motorVelocity = motor.getActiveTrajectoryVelocity();