Implement servo block on intake

src/main/java/raidzero/lib/LazyCan.java

@@ -1,16 +1,33 @@
 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.
      *
      * @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;
+    }
 }

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 {

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)

src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java

@@ -1,14 +1,18 @@
 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;
 import com.ctre.phoenix6.hardware.TalonFXS;
 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
      *