diff --git a/src/main/java/frc/robot/config/RobotConfigInferno.java b/src/main/java/frc/robot/config/RobotConfigInferno.java index 57e4049c..72b6fecc 100644 --- a/src/main/java/frc/robot/config/RobotConfigInferno.java +++ b/src/main/java/frc/robot/config/RobotConfigInferno.java @@ -132,8 +132,8 @@ public RobotConfigInferno() { "shooter", "1188", new Transform3d( - new Translation3d(-Units.inchesToMeters(10.5), 0, Units.inchesToMeters(13)), - new Rotation3d(0, Units.degreesToRadians(-28), Units.degreesToRadians(180))))); + new Translation3d(-Units.inchesToMeters(9.5), 0, Units.inchesToMeters(14)), + new Rotation3d(0, Units.degreesToRadians(-32), Units.degreesToRadians(180))))); cameras.add( new VisionCamera( diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java index 34225e1c..d3aeaef8 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOSparkMax.java @@ -15,6 +15,7 @@ public class IntakeIOSparkMax implements IntakeIO { // Gets the NEO encoder private final RelativeEncoder encoder; DigitalInput limitSwitchIntake = new DigitalInput(1); + DigitalInput limitSwitchIntakeSecondary = new DigitalInput(2); public IntakeIOSparkMax(int id, boolean inverted) { leader = new CANSparkMax(id, MotorType.kBrushless); @@ -31,7 +32,10 @@ public IntakeIOSparkMax(int id, boolean inverted) { @Override public void updateInputs(IntakeIOInputs inputs) { inputs.appliedVolts = leader.getAppliedOutput() * leader.getBusVoltage(); - inputs.limitSwitchIntake = !limitSwitchIntake.get(); + inputs.limitSwitchIntake = + !limitSwitchIntake.get() + || !limitSwitchIntakeSecondary + .get(); // Assume note in intake if either sensor is triggered inputs.current = leader.getOutputCurrent(); inputs.velocityRadPerSec = Units.rotationsPerMinuteToRadiansPerSecond(encoder.getVelocity() / GEAR_RATIO); diff --git a/src/main/java/frc/robot/util/DevilBotState.java b/src/main/java/frc/robot/util/DevilBotState.java index 6a000ba8..68fb777c 100644 --- a/src/main/java/frc/robot/util/DevilBotState.java +++ b/src/main/java/frc/robot/util/DevilBotState.java @@ -137,7 +137,7 @@ public enum SpeakerShootingMode { SPEAKER_VISION_BASED, } - private static SpeakerShootingMode shootingMode = SpeakerShootingMode.SPEAKER_FROM_SUBWOOFER; + private static SpeakerShootingMode shootingMode = SpeakerShootingMode.SPEAKER_VISION_BASED; public static void setShootingMode(SpeakerShootingMode position) { DevilBotState.shootingMode = position;