diff --git a/src/main/java/raidzero/lib/LazyCan.java b/src/main/java/raidzero/lib/LazyCan.java index 819036c..b274dde 100644 --- a/src/main/java/raidzero/lib/LazyCan.java +++ b/src/main/java/raidzero/lib/LazyCan.java @@ -1,8 +1,24 @@ package raidzero.lib; +import au.grapplerobotics.ConfigurationFailedException; import au.grapplerobotics.LaserCan; +import au.grapplerobotics.interfaces.LaserCanInterface.Measurement; +import au.grapplerobotics.interfaces.LaserCanInterface.RangingMode; +import au.grapplerobotics.interfaces.LaserCanInterface.RegionOfInterest; +import au.grapplerobotics.interfaces.LaserCanInterface.TimingBudget; +import edu.wpi.first.wpilibj.DriverStation; -public class LazyCan extends LaserCan { +public class LazyCan { + private LaserCan laserCan; + private int canId; + + private RangingMode rangingMode; + private RegionOfInterest regionOfInterest; + private TimingBudget timingBudget; + + private Measurement measurement; + + private double threshold; /** * Creates a new LaserCAN sensor. @@ -10,7 +26,8 @@ public class LazyCan extends LaserCan { * @param canId The CAN ID for the LaserCAN sensor */ public LazyCan(int canId) { - super(canId); + laserCan = new LaserCan(canId); + this.canId = canId; } /** @@ -19,8 +36,83 @@ public LazyCan(int canId) { * @return The distance in mm, -1 if the sensor cannot be found */ public int getDistanceMm() { - Measurement measurement = getMeasurement(); + measurement = laserCan.getMeasurement(); return measurement != null ? measurement.distance_mm : -1; } + + /** + * Returns true if the LaserCan finds an object within the distance threshold + * + * @return if there is an object within the distance threshold + */ + public boolean withinThreshold() { + measurement = laserCan.getMeasurement(); + + return measurement != null ? measurement.distance_mm <= threshold : false; + } + + /** + * Sets the reigon of interest for the lasercan + * + * @param x the x start position for the reigon + * @param y the y start position for the reigon + * @param w the width of the reigon + * @param h the height of the reigon + * @return the current LazyCan Object + */ + public LazyCan withRegionOfInterest(int x, int y, int w, int h) { + regionOfInterest = new RegionOfInterest(x, y, w, h); + + try { + laserCan.setRegionOfInterest(regionOfInterest); + } catch (ConfigurationFailedException e) { + DriverStation.reportError("LaserCan " + canId + ": RegionOfInterest Configuration failed! " + e, true); + } + + return this; + } + + /** + * Sets the ranging mode of the LaserCan + * + * @param rangingMode the new ranging mode + * @return the current LazyCan Object + */ + public LazyCan withRangingMode(RangingMode rangingMode) { + this.rangingMode = rangingMode; + try { + laserCan.setRangingMode(rangingMode); + } catch (ConfigurationFailedException e) { + System.out.println("LaserCan " + canId + ": RangingMode Configuration failed! " + e); + } + return this; + } + + /** + * Sets the timing budget of the LaserCan + * + * @param timingBudget the new timing budget + * @return the current LazyCan Object + */ + public LazyCan withTimingBudget(TimingBudget timingBudget) { + this.timingBudget = timingBudget; + try { + laserCan.setTimingBudget(timingBudget); + } catch (ConfigurationFailedException e) { + DriverStation.reportError("LaserCan " + canId + ": TimingBudget Configuration failed! " + e, true); + } + return this; + } + + /** + * Sets the distance threshold of the LaserCan + * + * @param threshold the new threshold in milimeters + * @return the current LazyCan object + */ + public LazyCan withThreshold(double threshold) { + this.threshold = threshold; + return this; + } } \ No newline at end of file diff --git a/src/main/java/raidzero/robot/Constants.java b/src/main/java/raidzero/robot/Constants.java index 74b5e47..b26e517 100644 --- a/src/main/java/raidzero/robot/Constants.java +++ b/src/main/java/raidzero/robot/Constants.java @@ -153,7 +153,7 @@ public class Intake { public static final int MOTOR_ID = 12; public static final int FOLLOW_ID = 13; - public static final double INTAKE_SPEED = 0.08; + public static final double INTAKE_SPEED = 0.25; public static final double INTAKE_LOWER_SPEED = 0.05; public static final double EXTAKE_SPEED = 0.1; @@ -162,6 +162,12 @@ public class Intake { public static final double LASERCAN_DISTANCE_THRESHOLD_MM = 50.0; public static final int CURRENT_LIMIT = 25; + + public static final int SERVO_HUB_ID = 3; + + public static final int SERVO_RETRACTED = 1950; + public static final int SERVO_EXTENDED = 1300; + public static final int SERVO_CENTER_WIDTH = 1625; } public class Joint { diff --git a/src/main/java/raidzero/robot/RobotContainer.java b/src/main/java/raidzero/robot/RobotContainer.java index 1bfdad3..a8f540d 100644 --- a/src/main/java/raidzero/robot/RobotContainer.java +++ b/src/main/java/raidzero/robot/RobotContainer.java @@ -177,6 +177,8 @@ private void configureBindings() { operator.button(Constants.Bindings.CORAL_INTAKE).onTrue(coralIntake.intake()); operator.button(Constants.Bindings.CORAL_SCOOCH).onTrue(coralIntake.scoochCoral()); + operator.button(Constants.Bindings.BOTTOM_RIGHT).onTrue(coralIntake.unstuckServo()); + operator.button(Constants.Bindings.CLIMB_DEPLOY) .onTrue( Commands.waitSeconds(0.2) diff --git a/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java b/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java index 6e06456..b8c418e 100644 --- a/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java +++ b/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java @@ -1,7 +1,6 @@ package raidzero.robot.subsystems.telescopingarm; import au.grapplerobotics.interfaces.LaserCanInterface.RangingMode; -import au.grapplerobotics.interfaces.LaserCanInterface.RegionOfInterest; import au.grapplerobotics.interfaces.LaserCanInterface.TimingBudget; import com.ctre.phoenix6.configs.TalonFXSConfiguration; import com.ctre.phoenix6.controls.Follower; @@ -9,6 +8,11 @@ import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.MotorArrangementValue; import com.ctre.phoenix6.signals.NeutralModeValue; +import com.revrobotics.servohub.ServoChannel; +import com.revrobotics.servohub.ServoChannel.ChannelId; +import com.revrobotics.servohub.ServoHub; +import com.revrobotics.servohub.config.ServoChannelConfig.BehaviorWhenDisabled; +import com.revrobotics.servohub.config.ServoHubConfig; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import raidzero.lib.LazyCan; @@ -17,6 +21,9 @@ public class CoralIntake extends SubsystemBase { private TalonFXS roller, follow; + private ServoHub servoHub; + private ServoChannel intakeBlock; + private LazyCan bottomLaser, topLaser; private static CoralIntake system; @@ -32,23 +39,22 @@ private CoralIntake() { follow.setControl(new Follower(Constants.TelescopingArm.Intake.MOTOR_ID, true)); follow.getConfigurator().apply(followConfiguration()); - bottomLaser = new LazyCan(0); - try { - bottomLaser.setRangingMode(RangingMode.SHORT); - bottomLaser.setRegionOfInterest(new RegionOfInterest(8, 4, 6, 8)); - bottomLaser.setTimingBudget(TimingBudget.TIMING_BUDGET_20MS); - } catch (Exception e) { - System.out.println("LaserCan Config Error"); - } + bottomLaser = new LazyCan(0) + .withRangingMode(RangingMode.SHORT) + .withRegionOfInterest(8, 4, 6, 8) + .withTimingBudget(TimingBudget.TIMING_BUDGET_20MS); - topLaser = new LazyCan(1); - try { - topLaser.setRangingMode(RangingMode.SHORT); - topLaser.setRegionOfInterest(new RegionOfInterest(8, 4, 6, 8)); - topLaser.setTimingBudget(TimingBudget.TIMING_BUDGET_20MS); - } catch (Exception e) { - System.out.println("LaserCan Config Error"); - } + topLaser = new LazyCan(1) + .withRangingMode(RangingMode.SHORT) + .withRegionOfInterest(8, 4, 6, 8) + .withTimingBudget(TimingBudget.TIMING_BUDGET_20MS); + + servoHub = new ServoHub(Constants.TelescopingArm.Intake.SERVO_HUB_ID); + servoHub.configure(getServoHubConfig(), ServoHub.ResetMode.kResetSafeParameters); + + intakeBlock = servoHub.getServoChannel(ChannelId.kChannelId2); + intakeBlock.setPowered(true); + intakeBlock.setEnabled(true); } /** @@ -68,11 +74,7 @@ public TalonFXS getRoller() { */ public Command intake() { return run(() -> roller.set(Constants.TelescopingArm.Intake.INTAKE_SPEED)) - .until(() -> getBottomLaserDistance() <= Constants.TelescopingArm.Intake.LASERCAN_DISTANCE_THRESHOLD_MM) - .andThen( - run(() -> roller.set(-Constants.TelescopingArm.Intake.INTAKE_SPEED)) - .withTimeout(0.1) - ); + .until(() -> getBottomLaserDistance() <= Constants.TelescopingArm.Intake.LASERCAN_DISTANCE_THRESHOLD_MM); } /** @@ -81,10 +83,20 @@ public Command intake() { * @return A {@link Command} to scooch the coral upwards */ public Command scoochCoral() { - return run(() -> roller.set(-Constants.TelescopingArm.Intake.INTAKE_SPEED)) + return run(() -> roller.set(-Constants.TelescopingArm.Intake.INTAKE_LOWER_SPEED)) .until(() -> getTopLaserDistance() <= Constants.TelescopingArm.Intake.LASERCAN_DISTANCE_THRESHOLD_MM); } + /** + * Creates a {@link Command} to move the coral upwards to unstuck the servo block + * + * @return A {@link Command} to move the coral upwards + */ + public Command unstuckServo() { + return run(() -> roller.set(-Constants.TelescopingArm.Intake.INTAKE_LOWER_SPEED)) + .until(() -> getBottomLaserDistance() >= Constants.TelescopingArm.Intake.LASERCAN_DISTANCE_THRESHOLD_MM); + } + /** * Creates a {@link Command} to stop the intake * @@ -115,6 +127,15 @@ public Command run(double speed) { return run(() -> roller.set(speed)); } + @Override + public void periodic() { + if (getBottomLaserDistance() > Constants.TelescopingArm.Intake.LASERCAN_DISTANCE_THRESHOLD_MM) { + intakeBlock.setPulseWidth(Constants.TelescopingArm.Intake.SERVO_EXTENDED); + } else { + intakeBlock.setPulseWidth(Constants.TelescopingArm.Intake.SERVO_RETRACTED); + } + } + /** * Gets the distance from the LaserCAN * @@ -164,6 +185,25 @@ private TalonFXSConfiguration followConfiguration() { return configuration; } + /** + * Gets the {@link ServoHubConfig} for the REV Servo Hub + * + * @return The {@link ServoHubConfig} for the REV Servo Hub + */ + private ServoHubConfig getServoHubConfig() { + ServoHubConfig config = new ServoHubConfig(); + + config.channel2 + .pulseRange( + Constants.TelescopingArm.Intake.SERVO_EXTENDED, + Constants.TelescopingArm.Intake.SERVO_CENTER_WIDTH, + Constants.TelescopingArm.Intake.SERVO_RETRACTED + ) + .disableBehavior(BehaviorWhenDisabled.kSupplyPower); + + return config; + } + /** * Gets the {@link CoralIntake} subsystem instance *