From b73e2917a93d6e349e3b17b4765ce87cc64884b4 Mon Sep 17 00:00:00 2001 From: theof Date: Tue, 18 Mar 2025 10:29:56 +0800 Subject: [PATCH 1/9] add lazycan class --- src/main/java/raidzero/lib/LazyCan.java | 98 ++++++++++++++++++++++++- 1 file changed, 95 insertions(+), 3 deletions(-) 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 From 619a9e3671b926b7ce6612399fe08dc257826103 Mon Sep 17 00:00:00 2001 From: 0x5b62656e5d Date: Tue, 18 Mar 2025 13:21:38 +0800 Subject: [PATCH 2/9] Update LaserCAN configuration for LazyCan --- .../telescopingarm/CoralIntake.java | 25 ++++++------------- 1 file changed, 8 insertions(+), 17 deletions(-) diff --git a/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java b/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java index 6e06456..e8d3de8 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; @@ -32,23 +31,15 @@ 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); } /** From a0c9cb71f88e11cf5e07a1fdd811dffb31b6770f Mon Sep 17 00:00:00 2001 From: 0x5b62656e5d Date: Tue, 18 Mar 2025 13:24:56 +0800 Subject: [PATCH 3/9] Add servo motor configs --- src/main/java/raidzero/robot/Constants.java | 6 ++++ .../telescopingarm/CoralIntake.java | 34 +++++++++++++++++++ 2 files changed, 40 insertions(+) diff --git a/src/main/java/raidzero/robot/Constants.java b/src/main/java/raidzero/robot/Constants.java index 74b5e47..cc2c297 100644 --- a/src/main/java/raidzero/robot/Constants.java +++ b/src/main/java/raidzero/robot/Constants.java @@ -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/subsystems/telescopingarm/CoralIntake.java b/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java index e8d3de8..212cc70 100644 --- a/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java +++ b/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java @@ -8,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; @@ -16,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; @@ -40,6 +48,13 @@ private CoralIntake() { .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); } /** @@ -155,6 +170,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 * From 419337c7a7d91838b7e55538651b17bf8619d07c Mon Sep 17 00:00:00 2001 From: 0x5b62656e5d Date: Tue, 18 Mar 2025 13:25:16 +0800 Subject: [PATCH 4/9] Add servo block implementation --- .../robot/subsystems/telescopingarm/CoralIntake.java | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java b/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java index 212cc70..ff753c4 100644 --- a/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java +++ b/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java @@ -121,6 +121,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 * From 8e3e98c924ea93a4b9f0b0e1511261dd72bb1586 Mon Sep 17 00:00:00 2001 From: 0x5b62656e5d Date: Tue, 18 Mar 2025 13:25:25 +0800 Subject: [PATCH 5/9] Remove retract on coral intake --- .../robot/subsystems/telescopingarm/CoralIntake.java | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java b/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java index ff753c4..dec74bd 100644 --- a/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java +++ b/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java @@ -74,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); } /** From 7394f3c49d11b66b735d4d39fc4f3472fade4bbc Mon Sep 17 00:00:00 2001 From: 0x5b62656e5d Date: Tue, 18 Mar 2025 13:25:33 +0800 Subject: [PATCH 6/9] Reduce coral scooch speed --- .../raidzero/robot/subsystems/telescopingarm/CoralIntake.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java b/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java index dec74bd..cf43627 100644 --- a/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java +++ b/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java @@ -83,7 +83,7 @@ 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); } From b233da91184d85a5ab510be97cae14d8671dee65 Mon Sep 17 00:00:00 2001 From: 0x5b62656e5d Date: Tue, 18 Mar 2025 13:25:41 +0800 Subject: [PATCH 7/9] Add method to unstuck coral from servo block --- .../robot/subsystems/telescopingarm/CoralIntake.java | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java b/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java index cf43627..b8c418e 100644 --- a/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java +++ b/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java @@ -87,6 +87,16 @@ public Command scoochCoral() { .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 * From 8877811f88e2d1e1439fff414e927245219b57c2 Mon Sep 17 00:00:00 2001 From: 0x5b62656e5d Date: Tue, 18 Mar 2025 13:25:54 +0800 Subject: [PATCH 8/9] Increase default intake speed --- src/main/java/raidzero/robot/Constants.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/java/raidzero/robot/Constants.java b/src/main/java/raidzero/robot/Constants.java index cc2c297..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; From 7a4e0cff9f0269ac6783162c66a0c2c707057fd4 Mon Sep 17 00:00:00 2001 From: 0x5b62656e5d Date: Tue, 18 Mar 2025 13:26:33 +0800 Subject: [PATCH 9/9] Bind operator button to unstuck servo block --- src/main/java/raidzero/robot/RobotContainer.java | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/java/raidzero/robot/RobotContainer.java b/src/main/java/raidzero/robot/RobotContainer.java index d2390b1..358b301 100644 --- a/src/main/java/raidzero/robot/RobotContainer.java +++ b/src/main/java/raidzero/robot/RobotContainer.java @@ -171,6 +171,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)