diff --git a/src/main/java/frc/lib/io/BeamBreakIOCANdi.java b/src/main/java/frc/lib/io/BeamBreakIOCANdi.java new file mode 100644 index 0000000..e5b806b --- /dev/null +++ b/src/main/java/frc/lib/io/BeamBreakIOCANdi.java @@ -0,0 +1,56 @@ +package frc.lib.io; + +import com.ctre.phoenix6.configs.CANdiConfiguration; +import com.ctre.phoenix6.hardware.CANdi; +import com.ctre.phoenix6.signals.S1CloseStateValue; +import com.ctre.phoenix6.signals.S1FloatStateValue; +import com.ctre.phoenix6.signals.S2CloseStateValue; +import com.ctre.phoenix6.signals.S2FloatStateValue; + +import edu.wpi.first.units.measure.Time; + +public class BeamBreakIOCANdi extends BeamBreakIO { + private final CANdi mCANdi; + private final int channel; + + public static BeamBreakIOCANdi makeInverted(int channel, Time debounce, String name, CANdi candi) { + return new BeamBreakIOCANdi(channel, debounce, name, candi) { + @Override + public boolean get() { + return !super.get(); + } + }; + } + + public BeamBreakIOCANdi(int channel, Time debounce, String name, CANdi candi) { + super(debounce, name); + + assert channel == 1 || channel == 2; //Check to make sure that the channel # is valid + + this.mCANdi = candi; + + this.channel = channel; + + CANdiConfiguration candiConfiguration = new CANdiConfiguration(); + + candiConfiguration.DigitalInputs.S1CloseState = S1CloseStateValue.CloseWhenNotHigh; + candiConfiguration.DigitalInputs.S1FloatState = S1FloatStateValue.PullLow; + candiConfiguration.DigitalInputs.S2CloseState = S2CloseStateValue.CloseWhenNotHigh; + candiConfiguration.DigitalInputs.S2FloatState = S2FloatStateValue.PullLow; + + mCANdi.getConfigurator().apply(candiConfiguration); + } + + public boolean getSignalInput1() { + return mCANdi.getS1Closed().getValue(); + } + + public boolean getSignalInput2() { + return mCANdi.getS2Closed().getValue(); + }; + + @Override + public boolean get() { + return channel == 1 ? getSignalInput1() : getSignalInput2(); + } +} diff --git a/src/main/java/frc/robot/Ports.java b/src/main/java/frc/robot/Ports.java index c000a66..a4c60bb 100644 --- a/src/main/java/frc/robot/Ports.java +++ b/src/main/java/frc/robot/Ports.java @@ -16,11 +16,14 @@ public enum Ports { PIVOT(16, "canivore1"), CLIMBER(17, "canivore1"), CLIMBER_ROLLERS(18, "canivore1"), - CANDLE(21, "canivore1"), + CANDLE(23, "canivore1"), - END_EFFECTOR_CORAL_BREAMBREAK(RobotConstants.isComp ? 1 : 5, "RioDigitalIn"), - END_EFFECTOR_ALGAE_BEAMBREAK(RobotConstants.isComp ? 0 : 0, "RioDigitalIn"), + // END_EFFECTOR_CORAL_BREAMBREAK(RobotConstants.isComp ? 1 : 5, "RioDigitalIn"), + // END_EFFECTOR_ALGAE_BEAMBREAK(RobotConstants.isComp ? 0 : 0, "RioDigitalIn"), + CANDI(21,"canivore1"), INDEXER_BEAMBREAK(RobotConstants.isComp ? 8 : 4, "RioDigitalIn"), + EE_CORAL_BEAMBREAK(1, "CANdi"), //Slot 1 or 2 on the CANdi + EE_ALGAE_BEAMBREAK(2, "CANdi"), ENCODER_41T(4, "canivore1"), ENCODER_39T(5, "canivore1"), diff --git a/src/main/java/frc/robot/controlboard/ControlBoard.java b/src/main/java/frc/robot/controlboard/ControlBoard.java index 70b37d7..c396139 100644 --- a/src/main/java/frc/robot/controlboard/ControlBoard.java +++ b/src/main/java/frc/robot/controlboard/ControlBoard.java @@ -407,7 +407,7 @@ private void debugControls() { // operator.pov(POVDirection.Up).onTrue(ClimberRollers.mInstance.setpointCommand(ClimberRollers.INTAKE)); // operator.pov(POVDirection.Down).onTrue(ClimberRollers.mInstance.setpointCommand(ClimberRollers.IDLE)); - + operator.leftBumper() .onTrue(new InstantCommand(() -> Pivot.mInstance.setCurrentPosition( Pivot.mInstance.directCancoder.getPosition().getValue())) diff --git a/src/main/java/frc/robot/subsystems/superstructure/Superstructure.java b/src/main/java/frc/robot/subsystems/superstructure/Superstructure.java index 8347d55..4082703 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/Superstructure.java +++ b/src/main/java/frc/robot/subsystems/superstructure/Superstructure.java @@ -1,5 +1,6 @@ package frc.robot.subsystems.superstructure; +import com.ctre.phoenix6.hardware.CANdi; import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveDriveState; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; @@ -24,6 +25,7 @@ import frc.lib.util.FieldLayout.Branch.Face; import frc.lib.util.FieldLayout.Level; import frc.lib.util.Util; +import frc.robot.Ports; import frc.robot.RobotConstants; import frc.robot.controlboard.ControlBoard; import frc.robot.planners.MotionPlanner; @@ -58,8 +60,11 @@ public class Superstructure extends SubsystemBase { public static final Superstructure mInstance = new Superstructure(); - public static BeamBreakIO endEffectorCoralBreak = BeamBreakConstants.getEndEffectorCoralBeamBreak(); - private static BeamBreakIO endEffectorAlgaeBreak = BeamBreakConstants.getEndEffectorAlgaeBeamBreak(); + public static final CANdi mEECANdi = new CANdi(Ports.CANDI.id, Ports.CANDI.bus); + + public static BeamBreakIO endEffectorCoralBreak = BeamBreakConstants.getEndEffectorCoralBeamBreak(mEECANdi); + private static BeamBreakIO endEffectorAlgaeBreak = BeamBreakConstants.getEndEffectorAlgaeBeamBreak(mEECANdi); + public static BeamBreakIO endEffectorVelocityDip = BeamBreakConstants.getEndEffectorVelocityDip(); public static BeamBreakIO coralRollersCurrentSpike = BeamBreakConstants.getCoralRollersCurrentSpike(); public static BeamBreakIO coralRollersVelocityDip = BeamBreakConstants.getCoralRollersVelocityDip(); @@ -68,6 +73,8 @@ public class Superstructure extends SubsystemBase { public static BeamBreakIO pivotVelocityLow = BeamBreakConstants.getPivotVelocityLow(); public static BeamBreakIO closeCoral = BeamBreakConstants.getCoralClose(); + + public static BeamBreakIO allAlgae = new BeamBreakIOSim( () -> endEffectorVelocityDip.getDebounced() || endEffectorAlgaeBreak.getDebounced(), Units.Seconds.of(0.0), diff --git a/src/main/java/frc/robot/subsystems/superstructure/SuperstructureConstants.java b/src/main/java/frc/robot/subsystems/superstructure/SuperstructureConstants.java index e4bc2cb..518c2a5 100644 --- a/src/main/java/frc/robot/subsystems/superstructure/SuperstructureConstants.java +++ b/src/main/java/frc/robot/subsystems/superstructure/SuperstructureConstants.java @@ -8,6 +8,7 @@ import edu.wpi.first.units.measure.Time; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.lib.io.BeamBreakIO; +import frc.lib.io.BeamBreakIOCANdi; import frc.lib.io.BeamBreakIODigitalIn; import frc.lib.io.BeamBreakIOSim; import frc.lib.util.FieldLayout.Level; @@ -22,16 +23,18 @@ import frc.robot.subsystems.pivot.Pivot; import java.util.function.BooleanSupplier; +import com.ctre.phoenix6.hardware.CANdi; public final class SuperstructureConstants { public static class BeamBreakConstants { - public static BeamBreakIO getEndEffectorCoralBeamBreak() { + public static BeamBreakIO getEndEffectorCoralBeamBreak(CANdi candi) { if (Robot.isReal()) { try { - return new BeamBreakIODigitalIn( - Ports.END_EFFECTOR_CORAL_BREAMBREAK.id, + return BeamBreakIOCANdi.makeInverted( + Ports.EE_CORAL_BEAMBREAK.id, SuperstructureConstants.kEndEffectorCoralDebounce, - "Coral End Effector Break"); + "Coral End Effector Break", + candi); } catch (Exception e) { SmartDashboard.putString("End Effector Beam Break", "Failed"); return new BeamBreakIOSim( @@ -57,13 +60,15 @@ public static BeamBreakIO getEndEffectorVelocityDip() { } } - public static BeamBreakIO getEndEffectorAlgaeBeamBreak() { + public static BeamBreakIO getEndEffectorAlgaeBeamBreak(CANdi candi) { if (Robot.isReal()) { try { - return new BeamBreakIODigitalIn( - Ports.END_EFFECTOR_ALGAE_BEAMBREAK.id, + return BeamBreakIOCANdi.makeInverted( + Ports.EE_ALGAE_BEAMBREAK.id, SuperstructureConstants.kEndEffectorAlgaeDebounce, - "Algae End Effector Break"); + "Algae End Effector Break", + candi + ); } catch (Exception e) { SmartDashboard.putString("End Effector Beam Break", "Failed"); return new BeamBreakIOSim(