From 3afa24c1cbfd4e8fb03c5a7a9f95808a6d3657cd Mon Sep 17 00:00:00 2001 From: theof Date: Mon, 17 Mar 2025 18:54:42 +0800 Subject: [PATCH 01/47] add kanye lazycan --- src/main/java/raidzero/lib/LazyCan.java | 80 ++++++++++++++++++- .../telescopingarm/CoralIntake.java | 26 ++---- 2 files changed, 84 insertions(+), 22 deletions(-) diff --git a/src/main/java/raidzero/lib/LazyCan.java b/src/main/java/raidzero/lib/LazyCan.java index 819036c..1eeab48 100644 --- a/src/main/java/raidzero/lib/LazyCan.java +++ b/src/main/java/raidzero/lib/LazyCan.java @@ -1,8 +1,19 @@ package raidzero.lib; +import au.grapplerobotics.ConfigurationFailedException; import au.grapplerobotics.LaserCan; +import edu.wpi.first.wpilibj.DriverStation; public class LazyCan extends LaserCan { + private int canId; + + private RangingMode rangingMode; + private RegionOfInterest regionOfInterest; + private TimingBudget timingBudget; + + private Measurement measurement; + + private int threshold; /** * Creates a new LaserCAN sensor. @@ -11,6 +22,7 @@ public class LazyCan extends LaserCan { */ public LazyCan(int canId) { super(canId); + this.canId = canId; } /** @@ -19,8 +31,74 @@ public LazyCan(int canId) { * @return The distance in mm, -1 if the sensor cannot be found */ public int getDistanceMm() { - Measurement measurement = getMeasurement(); + measurement = getMeasurement(); return measurement != null ? measurement.distance_mm : -1; } + + public boolean getStatus() { + measurement = getMeasurement(); + + return measurement != null ? getMeasurement().distance_mm <= threshold : false; + } + + public LazyCan withRegionOfInterest(int x, int y, int w, int h) { + regionOfInterest = new RegionOfInterest(x, y, w, h); + + try { + this.setRegionOfInterest(regionOfInterest); + } catch (ConfigurationFailedException e) { + DriverStation.reportError("LaserCan " + canId + ": RegionOfInterest Configuration failed! " + e, true); + } + + return this; + } + + public void build() { + if (regionOfInterest == null) { + regionOfInterest = new RegionOfInterest(4, 4, 8, 8); + } + if (rangingMode == null) { + rangingMode = RangingMode.SHORT; + } + if (timingBudget == null) { + timingBudget = TimingBudget.TIMING_BUDGET_33MS; + } + if (threshold == 0) { + threshold = 500; + } + + try { + this.setRegionOfInterest(regionOfInterest); + this.setRangingMode(rangingMode); + this.setTimingBudget(timingBudget); + } catch (ConfigurationFailedException e) { + DriverStation.reportError("LaserCan " + canId + ": Configuration failed! " + e, true); + } + } + + public LazyCan withRangingMode(RangingMode rangingMode) { + this.rangingMode = rangingMode; + try { + this.setRangingMode(rangingMode); + } catch (ConfigurationFailedException e) { + System.out.println("LaserCan " + canId + ": RangingMode Configuration failed! " + e); + } + return this; + } + + public LazyCan withTimingBudget(TimingBudget timingBudget) { + this.timingBudget = timingBudget; + try { + this.setTimingBudget(timingBudget); + } catch (ConfigurationFailedException e) { + DriverStation.reportError("LaserCan " + canId + ": TimingBudget Configuration failed! " + e, true); + } + return this; + } + + public LazyCan withThreshold(int threshold) { + this.threshold = threshold; + return this; + } } \ No newline at end of file diff --git a/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java b/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java index 6e06456..9e87447 100644 --- a/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java +++ b/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java @@ -15,7 +15,7 @@ import raidzero.robot.Constants; public class CoralIntake extends SubsystemBase { - private TalonFXS roller, follow; + private TalonFXS roller; private LazyCan bottomLaser, topLaser; @@ -28,27 +28,11 @@ private CoralIntake() { roller = new TalonFXS(Constants.TelescopingArm.Intake.MOTOR_ID); roller.getConfigurator().apply(rollerConfiguration()); - follow = new TalonFXS(Constants.TelescopingArm.Intake.FOLLOW_ID); - 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 4c1905c7762026668c47bffcbd9242442395ee97 Mon Sep 17 00:00:00 2001 From: theof Date: Mon, 17 Mar 2025 18:54:59 +0800 Subject: [PATCH 02/47] fix resource leak in lazycan --- src/main/java/raidzero/lib/LazyCan.java | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/main/java/raidzero/lib/LazyCan.java b/src/main/java/raidzero/lib/LazyCan.java index 1eeab48..e8363c2 100644 --- a/src/main/java/raidzero/lib/LazyCan.java +++ b/src/main/java/raidzero/lib/LazyCan.java @@ -101,4 +101,9 @@ public LazyCan withThreshold(int threshold) { this.threshold = threshold; return this; } + + @Override + public void close() throws Exception { + super.close(); + } } \ No newline at end of file From e9411d45199904d5a1e0afde2289cd15fc998799 Mon Sep 17 00:00:00 2001 From: theof Date: Mon, 17 Mar 2025 19:01:23 +0800 Subject: [PATCH 03/47] fix errors with lazycan class resource leaks fr this time --- src/main/java/raidzero/lib/LazyCan.java | 32 +++++++++---------- .../telescopingarm/CoralIntake.java | 2 -- 2 files changed, 16 insertions(+), 18 deletions(-) diff --git a/src/main/java/raidzero/lib/LazyCan.java b/src/main/java/raidzero/lib/LazyCan.java index e8363c2..52e8325 100644 --- a/src/main/java/raidzero/lib/LazyCan.java +++ b/src/main/java/raidzero/lib/LazyCan.java @@ -2,9 +2,14 @@ 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; @@ -21,7 +26,7 @@ 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; } @@ -31,22 +36,22 @@ public LazyCan(int canId) { * @return The distance in mm, -1 if the sensor cannot be found */ public int getDistanceMm() { - measurement = getMeasurement(); + measurement = laserCan.getMeasurement(); return measurement != null ? measurement.distance_mm : -1; } public boolean getStatus() { - measurement = getMeasurement(); + measurement = laserCan.getMeasurement(); - return measurement != null ? getMeasurement().distance_mm <= threshold : false; + return measurement != null ? measurement.distance_mm <= threshold : false; } public LazyCan withRegionOfInterest(int x, int y, int w, int h) { regionOfInterest = new RegionOfInterest(x, y, w, h); try { - this.setRegionOfInterest(regionOfInterest); + laserCan.setRegionOfInterest(regionOfInterest); } catch (ConfigurationFailedException e) { DriverStation.reportError("LaserCan " + canId + ": RegionOfInterest Configuration failed! " + e, true); } @@ -69,9 +74,9 @@ public void build() { } try { - this.setRegionOfInterest(regionOfInterest); - this.setRangingMode(rangingMode); - this.setTimingBudget(timingBudget); + laserCan.setRegionOfInterest(regionOfInterest); + laserCan.setRangingMode(rangingMode); + laserCan.setTimingBudget(timingBudget); } catch (ConfigurationFailedException e) { DriverStation.reportError("LaserCan " + canId + ": Configuration failed! " + e, true); } @@ -80,7 +85,7 @@ public void build() { public LazyCan withRangingMode(RangingMode rangingMode) { this.rangingMode = rangingMode; try { - this.setRangingMode(rangingMode); + laserCan.setRangingMode(rangingMode); } catch (ConfigurationFailedException e) { System.out.println("LaserCan " + canId + ": RangingMode Configuration failed! " + e); } @@ -90,7 +95,7 @@ public LazyCan withRangingMode(RangingMode rangingMode) { public LazyCan withTimingBudget(TimingBudget timingBudget) { this.timingBudget = timingBudget; try { - this.setTimingBudget(timingBudget); + laserCan.setTimingBudget(timingBudget); } catch (ConfigurationFailedException e) { DriverStation.reportError("LaserCan " + canId + ": TimingBudget Configuration failed! " + e, true); } @@ -101,9 +106,4 @@ public LazyCan withThreshold(int threshold) { this.threshold = threshold; return this; } - - @Override - public void close() throws Exception { - super.close(); - } } \ No newline at end of file diff --git a/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java b/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java index 9e87447..4b0384a 100644 --- a/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java +++ b/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java @@ -1,10 +1,8 @@ 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; From 14dc4e77432c47c695b14a2738b0df0624a9d990 Mon Sep 17 00:00:00 2001 From: theof Date: Mon, 17 Mar 2025 20:29:36 +0800 Subject: [PATCH 04/47] add new intake logic --- src/main/java/raidzero/lib/LazyCan.java | 6 +- src/main/java/raidzero/robot/Constants.java | 25 ++++++-- src/main/java/raidzero/robot/Robot.java | 2 +- .../telescopingarm/CoralIntake.java | 60 +++++++------------ 4 files changed, 46 insertions(+), 47 deletions(-) diff --git a/src/main/java/raidzero/lib/LazyCan.java b/src/main/java/raidzero/lib/LazyCan.java index 52e8325..05baf22 100644 --- a/src/main/java/raidzero/lib/LazyCan.java +++ b/src/main/java/raidzero/lib/LazyCan.java @@ -18,7 +18,7 @@ public class LazyCan { private Measurement measurement; - private int threshold; + private double threshold; /** * Creates a new LaserCAN sensor. @@ -41,7 +41,7 @@ public int getDistanceMm() { return measurement != null ? measurement.distance_mm : -1; } - public boolean getStatus() { + public boolean withinThreshold() { measurement = laserCan.getMeasurement(); return measurement != null ? measurement.distance_mm <= threshold : false; @@ -102,7 +102,7 @@ public LazyCan withTimingBudget(TimingBudget timingBudget) { return this; } - public LazyCan withThreshold(int threshold) { + public LazyCan withThreshold(double threshold) { this.threshold = threshold; return this; } diff --git a/src/main/java/raidzero/robot/Constants.java b/src/main/java/raidzero/robot/Constants.java index 74b5e47..ed3a796 100644 --- a/src/main/java/raidzero/robot/Constants.java +++ b/src/main/java/raidzero/robot/Constants.java @@ -1,8 +1,12 @@ package raidzero.robot; import com.ctre.phoenix6.signals.GravityTypeValue; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.MotorArrangementValue; + import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; + import java.util.ArrayList; import java.util.List; @@ -151,17 +155,26 @@ public static enum REEFS { public class TelescopingArm { 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_LOWER_SPEED = 0.05; + public static final MotorArrangementValue MOTOR_ARRANGEMENT = MotorArrangementValue.Minion_JST; - public static final double EXTAKE_SPEED = 0.1; - public static final double EXTAKE_TIMEOUT_S = 1.0; + public static final InvertedValue INVERTED_VALUE = InvertedValue.CounterClockwise_Positive; + + public static final int STATOR_CURRENT_LIMIT = 40; + public static final int SUPPLY_CURRENT_LIMIT = 50; + public static final double SUPPLY_CURRENT_LOWER_TIME = 0.0; public static final double LASERCAN_DISTANCE_THRESHOLD_MM = 50.0; - public static final int CURRENT_LIMIT = 25; + public static final double INTAKE_SPEED = 0.20; + public static final double INTAKE_LOWER_SPEED = 0.10; + public static final double EJECT_SPEED = -0.1; + + public static final int STALL_CURRENT_THRESHOLD = 40; + + public static final double EXTAKE_SPEED = 0.1; + public static final double EXTAKE_TIMEOUT_S = 1.0; + } public class Joint { diff --git a/src/main/java/raidzero/robot/Robot.java b/src/main/java/raidzero/robot/Robot.java index 1a98712..a1d3d48 100644 --- a/src/main/java/raidzero/robot/Robot.java +++ b/src/main/java/raidzero/robot/Robot.java @@ -51,7 +51,7 @@ public void disabledPeriodic() { @Override public void disabledExit() { - CoralIntake.system().getRoller().setControl(new StaticBrake()); + CoralIntake.system().enableStaticBrake();; ArmStrip.system().resetAnimation(); } diff --git a/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java b/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java index 4b0384a..fe32ee8 100644 --- a/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java +++ b/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java @@ -3,14 +3,15 @@ import au.grapplerobotics.interfaces.LaserCanInterface.RangingMode; import au.grapplerobotics.interfaces.LaserCanInterface.TimingBudget; import com.ctre.phoenix6.configs.TalonFXSConfiguration; +import com.ctre.phoenix6.controls.StaticBrake; 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 edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.SubsystemBase; import raidzero.lib.LazyCan; import raidzero.robot.Constants; +import raidzero.robot.Constants.TelescopingArm.Intake; public class CoralIntake extends SubsystemBase { private TalonFXS roller; @@ -27,19 +28,19 @@ private CoralIntake() { roller.getConfigurator().apply(rollerConfiguration()); bottomLaser = new LazyCan(0).withRangingMode(RangingMode.SHORT) - .withRegionOfInterest(8, 4, 6, 8).withTimingBudget(TimingBudget.TIMING_BUDGET_20MS); + .withRegionOfInterest(8, 4, 6, 8).withTimingBudget(TimingBudget.TIMING_BUDGET_20MS) + .withThreshold(Intake.LASERCAN_DISTANCE_THRESHOLD_MM); topLaser = new LazyCan(1).withRangingMode(RangingMode.SHORT) - .withRegionOfInterest(8, 4, 6, 8).withTimingBudget(TimingBudget.TIMING_BUDGET_20MS); + .withRegionOfInterest(8, 4, 6, 8).withTimingBudget(TimingBudget.TIMING_BUDGET_20MS) + .withThreshold(Intake.LASERCAN_DISTANCE_THRESHOLD_MM); } /** - * Gets the roller motor controller for disabled init to check for position - * - * @return The Roller motor + * Sets the motor to static brake mode for disabled init to check for position */ - public TalonFXS getRoller() { - return roller; + public void enableStaticBrake() { + roller.setControl(new StaticBrake()); } /** @@ -49,22 +50,18 @@ public TalonFXS getRoller() { * @return The command to be scheduled and run */ public Command intake() { - return run(() -> roller.set(Constants.TelescopingArm.Intake.INTAKE_SPEED)) - .until(() -> getBottomLaserDistance() <= Constants.TelescopingArm.Intake.LASERCAN_DISTANCE_THRESHOLD_MM) + return run(() -> roller.set(Intake.INTAKE_SPEED)).until(() -> topLaser.withinThreshold()) .andThen( - run(() -> roller.set(-Constants.TelescopingArm.Intake.INTAKE_SPEED)) - .withTimeout(0.1) + new SequentialCommandGroup( + run(() -> roller.set(Intake.EJECT_SPEED)).onlyIf(() -> isStalling()).withTimeout(0.5), + run(() -> roller.set(Intake.INTAKE_LOWER_SPEED)).onlyIf(() -> !isStalling()).until(() -> bottomLaser.withinThreshold()) + .andThen(() -> roller.set(Intake.EJECT_SPEED)).until(() -> !bottomLaser.withinThreshold()) + ) ); } - /** - * Creates a {@link Command} to scooch the coral upwards if too low - * - * @return A {@link Command} to scooch the coral upwards - */ - public Command scoochCoral() { - return run(() -> roller.set(-Constants.TelescopingArm.Intake.INTAKE_SPEED)) - .until(() -> getTopLaserDistance() <= Constants.TelescopingArm.Intake.LASERCAN_DISTANCE_THRESHOLD_MM); + private boolean isStalling() { + return roller.getStatorCurrent().getValueAsDouble() > Intake.STALL_CURRENT_THRESHOLD; } /** @@ -123,23 +120,12 @@ public int getBottomLaserDistance() { private TalonFXSConfiguration rollerConfiguration() { TalonFXSConfiguration configuration = new TalonFXSConfiguration(); - configuration.Commutation.MotorArrangement = MotorArrangementValue.Minion_JST; - configuration.MotorOutput.Inverted = InvertedValue.Clockwise_Positive; - - configuration.MotorOutput.NeutralMode = NeutralModeValue.Brake; - - return configuration; - } - - /** - * Gets the {@link TalonFXSConfiguration} for the roller follower - * - * @return The {@link TalonFXSConfiguration} for the roller follower - */ - private TalonFXSConfiguration followConfiguration() { - TalonFXSConfiguration configuration = new TalonFXSConfiguration(); + configuration.Commutation.MotorArrangement = Intake.MOTOR_ARRANGEMENT; + configuration.MotorOutput.Inverted = Intake.INVERTED_VALUE; - configuration.Commutation.MotorArrangement = MotorArrangementValue.Minion_JST; + configuration.CurrentLimits.StatorCurrentLimit = Intake.STATOR_CURRENT_LIMIT; + configuration.CurrentLimits.SupplyCurrentLimit = Intake.SUPPLY_CURRENT_LIMIT; + configuration.CurrentLimits.SupplyCurrentLowerTime = Intake.SUPPLY_CURRENT_LOWER_TIME; configuration.MotorOutput.NeutralMode = NeutralModeValue.Brake; From 5c758f718ad0fffc02bae265846981b7237620c8 Mon Sep 17 00:00:00 2001 From: theof Date: Mon, 17 Mar 2025 20:40:36 +0800 Subject: [PATCH 05/47] add missing javadoc and remove extraneous methods --- src/main/java/raidzero/lib/LazyCan.java | 55 +++++++++++-------- .../telescopingarm/CoralIntake.java | 5 ++ 2 files changed, 37 insertions(+), 23 deletions(-) diff --git a/src/main/java/raidzero/lib/LazyCan.java b/src/main/java/raidzero/lib/LazyCan.java index 05baf22..0519506 100644 --- a/src/main/java/raidzero/lib/LazyCan.java +++ b/src/main/java/raidzero/lib/LazyCan.java @@ -41,12 +41,26 @@ public int getDistanceMm() { 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); @@ -59,29 +73,12 @@ public LazyCan withRegionOfInterest(int x, int y, int w, int h) { return this; } - public void build() { - if (regionOfInterest == null) { - regionOfInterest = new RegionOfInterest(4, 4, 8, 8); - } - if (rangingMode == null) { - rangingMode = RangingMode.SHORT; - } - if (timingBudget == null) { - timingBudget = TimingBudget.TIMING_BUDGET_33MS; - } - if (threshold == 0) { - threshold = 500; - } - - try { - laserCan.setRegionOfInterest(regionOfInterest); - laserCan.setRangingMode(rangingMode); - laserCan.setTimingBudget(timingBudget); - } catch (ConfigurationFailedException e) { - DriverStation.reportError("LaserCan " + canId + ": Configuration failed! " + e, true); - } - } - + /** + * 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 { @@ -92,6 +89,12 @@ public LazyCan withRangingMode(RangingMode rangingMode) { 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 { @@ -102,6 +105,12 @@ public LazyCan withTimingBudget(TimingBudget timingBudget) { 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; diff --git a/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java b/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java index fe32ee8..952a97c 100644 --- a/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java +++ b/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java @@ -60,6 +60,11 @@ public Command intake() { ); } + /** + * Returns true if the current is above a pre-defined threshold to consider it stalling + * + * @return if the motor is stalliing + */ private boolean isStalling() { return roller.getStatorCurrent().getValueAsDouble() > Intake.STALL_CURRENT_THRESHOLD; } From 37320776a9de68b2d595f551b6c8d14e9ed8f006 Mon Sep 17 00:00:00 2001 From: theof Date: Tue, 18 Mar 2025 07:50:52 +0800 Subject: [PATCH 06/47] remove scooch --- src/main/java/raidzero/robot/RobotContainer.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/java/raidzero/robot/RobotContainer.java b/src/main/java/raidzero/robot/RobotContainer.java index d2390b1..ee6b3d1 100644 --- a/src/main/java/raidzero/robot/RobotContainer.java +++ b/src/main/java/raidzero/robot/RobotContainer.java @@ -169,7 +169,6 @@ private void configureBindings() { operator.button(Constants.Bindings.CORAL_EXTAKE).whileTrue(coralIntake.extake()); operator.button(Constants.Bindings.CORAL_INTAKE).onTrue(coralIntake.intake()); - operator.button(Constants.Bindings.CORAL_SCOOCH).onTrue(coralIntake.scoochCoral()); operator.button(Constants.Bindings.CLIMB_DEPLOY) .onTrue( From 635e97e788ce75efd7605772846437bf91409121 Mon Sep 17 00:00:00 2001 From: theof Date: Tue, 18 Mar 2025 09:08:37 +0800 Subject: [PATCH 07/47] simple intake tested --- src/main/java/raidzero/lib/LazyCan.java | 12 +++++------ src/main/java/raidzero/robot/Constants.java | 18 ++++++++-------- .../java/raidzero/robot/RobotContainer.java | 6 +++--- .../robot/subsystems/LEDStrip/ArmStrip.java | 12 +++++------ .../telescopingarm/CoralIntake.java | 21 ++++++++++++------- 5 files changed, 37 insertions(+), 32 deletions(-) diff --git a/src/main/java/raidzero/lib/LazyCan.java b/src/main/java/raidzero/lib/LazyCan.java index 0519506..b274dde 100644 --- a/src/main/java/raidzero/lib/LazyCan.java +++ b/src/main/java/raidzero/lib/LazyCan.java @@ -43,18 +43,18 @@ public int getDistanceMm() { /** * 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 @@ -75,7 +75,7 @@ public LazyCan withRegionOfInterest(int x, int y, int w, int h) { /** * Sets the ranging mode of the LaserCan - * + * * @param rangingMode the new ranging mode * @return the current LazyCan Object */ @@ -91,7 +91,7 @@ public LazyCan withRangingMode(RangingMode rangingMode) { /** * Sets the timing budget of the LaserCan - * + * * @param timingBudget the new timing budget * @return the current LazyCan Object */ @@ -107,7 +107,7 @@ public LazyCan withTimingBudget(TimingBudget timingBudget) { /** * Sets the distance threshold of the LaserCan - * + * * @param threshold the new threshold in milimeters * @return the current LazyCan object */ diff --git a/src/main/java/raidzero/robot/Constants.java b/src/main/java/raidzero/robot/Constants.java index ed3a796..f99ff0c 100644 --- a/src/main/java/raidzero/robot/Constants.java +++ b/src/main/java/raidzero/robot/Constants.java @@ -3,10 +3,8 @@ import com.ctre.phoenix6.signals.GravityTypeValue; import com.ctre.phoenix6.signals.InvertedValue; import com.ctre.phoenix6.signals.MotorArrangementValue; - import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; - import java.util.ArrayList; import java.util.List; @@ -164,15 +162,17 @@ public class Intake { public static final int SUPPLY_CURRENT_LIMIT = 50; public static final double SUPPLY_CURRENT_LOWER_TIME = 0.0; - public static final double LASERCAN_DISTANCE_THRESHOLD_MM = 50.0; + public static final double TOP_LASER_THRESHOLD_MM = 100.0; + public static final double BOTTOM_LASER_THRESHOLD_MM = 50.0; - public static final double INTAKE_SPEED = 0.20; - public static final double INTAKE_LOWER_SPEED = 0.10; - public static final double EJECT_SPEED = -0.1; + public static final double INTAKE_SPEED = 0.50; + public static final double INTAKE_LOWER_SPEED = 0.25; + public static final double EJECT_SPEED = -0.50; + public static final double REVERSE_SPEED = -0.2; - public static final int STALL_CURRENT_THRESHOLD = 40; + public static final double STALL_CURRENT_THRESHOLD = 20.0; - public static final double EXTAKE_SPEED = 0.1; + public static final double EXTAKE_SPEED = 0.25; public static final double EXTAKE_TIMEOUT_S = 1.0; } @@ -218,7 +218,7 @@ public class Positions { public static final double[] L1_SCORING_POS_M = { 0.0, 0.0 }; public static final double[] INTAKE_POS_M = { 0.5, 0.835 }; - public static final double[] INTAKE_POS_M_BLUE = { 0.5, 0.81 }; + public static final double[] INTAKE_POS_M_BLUE = { 0.5, 0.88 }; public static final double[] HOME_POS_M = { 0.0, 0.0 }; } diff --git a/src/main/java/raidzero/robot/RobotContainer.java b/src/main/java/raidzero/robot/RobotContainer.java index ee6b3d1..66d0b7b 100644 --- a/src/main/java/raidzero/robot/RobotContainer.java +++ b/src/main/java/raidzero/robot/RobotContainer.java @@ -110,7 +110,7 @@ private void configureBindings() { ); joystick.leftTrigger().whileTrue(coralIntake.extake()); - joystick.rightTrigger().onTrue(coralIntake.intake()); + joystick.rightTrigger().onTrue(coralIntake.intakeSimple()); joystick.b().whileTrue( swerve.pathToStation() @@ -245,8 +245,8 @@ private void registerPathplannerCommands() { "ExtakeCoral", coralIntake.run(0.1).until( () -> { - return coralIntake.getBottomLaserDistance() >= Constants.TelescopingArm.Intake.LASERCAN_DISTANCE_THRESHOLD_MM && - coralIntake.getTopLaserDistance() >= Constants.TelescopingArm.Intake.LASERCAN_DISTANCE_THRESHOLD_MM; + return coralIntake.getBottomLaserDistance() >= Constants.TelescopingArm.Intake.TOP_LASER_THRESHOLD_MM && + coralIntake.getTopLaserDistance() >= Constants.TelescopingArm.Intake.TOP_LASER_THRESHOLD_MM; } ).withTimeout(1.0).andThen(() -> coralIntake.stop()) ); diff --git a/src/main/java/raidzero/robot/subsystems/LEDStrip/ArmStrip.java b/src/main/java/raidzero/robot/subsystems/LEDStrip/ArmStrip.java index f9d14d4..c6dd68a 100644 --- a/src/main/java/raidzero/robot/subsystems/LEDStrip/ArmStrip.java +++ b/src/main/java/raidzero/robot/subsystems/LEDStrip/ArmStrip.java @@ -80,14 +80,14 @@ private void updateStates() { armIsLegal = arm.getJointPosition() >= Constants.CANdle.ARM_JOINT_LOWER_BOUND && arm.getJointPosition() <= Constants.CANdle.ARM_JOINT_UPPER_BOUND; - coralTooDown = CoralIntake.system().getTopLaserDistance() > Constants.TelescopingArm.Intake.LASERCAN_DISTANCE_THRESHOLD_MM && - CoralIntake.system().getBottomLaserDistance() < Constants.TelescopingArm.Intake.LASERCAN_DISTANCE_THRESHOLD_MM; + coralTooDown = CoralIntake.system().getTopLaserDistance() > Constants.TelescopingArm.Intake.TOP_LASER_THRESHOLD_MM && + CoralIntake.system().getBottomLaserDistance() < Constants.TelescopingArm.Intake.TOP_LASER_THRESHOLD_MM; - coralTooUp = CoralIntake.system().getTopLaserDistance() < Constants.TelescopingArm.Intake.LASERCAN_DISTANCE_THRESHOLD_MM && - CoralIntake.system().getBottomLaserDistance() > Constants.TelescopingArm.Intake.LASERCAN_DISTANCE_THRESHOLD_MM; + coralTooUp = CoralIntake.system().getTopLaserDistance() < Constants.TelescopingArm.Intake.TOP_LASER_THRESHOLD_MM && + CoralIntake.system().getBottomLaserDistance() > Constants.TelescopingArm.Intake.TOP_LASER_THRESHOLD_MM; - coralIsIn = CoralIntake.system().getTopLaserDistance() < Constants.TelescopingArm.Intake.LASERCAN_DISTANCE_THRESHOLD_MM && - CoralIntake.system().getBottomLaserDistance() < Constants.TelescopingArm.Intake.LASERCAN_DISTANCE_THRESHOLD_MM; + coralIsIn = CoralIntake.system().getTopLaserDistance() < Constants.TelescopingArm.Intake.TOP_LASER_THRESHOLD_MM && + CoralIntake.system().getBottomLaserDistance() < Constants.TelescopingArm.Intake.TOP_LASER_THRESHOLD_MM; } /** diff --git a/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java b/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java index 952a97c..bd4ca56 100644 --- a/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java +++ b/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java @@ -27,13 +27,13 @@ private CoralIntake() { roller = new TalonFXS(Constants.TelescopingArm.Intake.MOTOR_ID); roller.getConfigurator().apply(rollerConfiguration()); - bottomLaser = new LazyCan(0).withRangingMode(RangingMode.SHORT) - .withRegionOfInterest(8, 4, 6, 8).withTimingBudget(TimingBudget.TIMING_BUDGET_20MS) - .withThreshold(Intake.LASERCAN_DISTANCE_THRESHOLD_MM); + bottomLaser = new LazyCan(1).withRangingMode(RangingMode.SHORT) + .withRegionOfInterest(8, 8, 16, 16).withTimingBudget(TimingBudget.TIMING_BUDGET_20MS) + .withThreshold(Intake.BOTTOM_LASER_THRESHOLD_MM); - topLaser = new LazyCan(1).withRangingMode(RangingMode.SHORT) - .withRegionOfInterest(8, 4, 6, 8).withTimingBudget(TimingBudget.TIMING_BUDGET_20MS) - .withThreshold(Intake.LASERCAN_DISTANCE_THRESHOLD_MM); + topLaser = new LazyCan(0).withRangingMode(RangingMode.SHORT) + .withRegionOfInterest(8, 8, 16, 16).withTimingBudget(TimingBudget.TIMING_BUDGET_20MS) + .withThreshold(Intake.TOP_LASER_THRESHOLD_MM); } /** @@ -55,14 +55,19 @@ public Command intake() { new SequentialCommandGroup( run(() -> roller.set(Intake.EJECT_SPEED)).onlyIf(() -> isStalling()).withTimeout(0.5), run(() -> roller.set(Intake.INTAKE_LOWER_SPEED)).onlyIf(() -> !isStalling()).until(() -> bottomLaser.withinThreshold()) - .andThen(() -> roller.set(Intake.EJECT_SPEED)).until(() -> !bottomLaser.withinThreshold()) + .andThen(() -> roller.set(Intake.REVERSE_SPEED)).until(() -> !bottomLaser.withinThreshold()) ) ); } + public Command intakeSimple() { + return run(() -> roller.set(Intake.INTAKE_SPEED)).until(() -> bottomLaser.withinThreshold()) + .andThen(run(() -> roller.set(Intake.REVERSE_SPEED)).withTimeout(0.5)); + } + /** * Returns true if the current is above a pre-defined threshold to consider it stalling - * + * * @return if the motor is stalliing */ private boolean isStalling() { From 9e16ea8a822b06bf274d8bc7789c3d4d27cbe22d Mon Sep 17 00:00:00 2001 From: kayneyao <42885964+kayneyao@users.noreply.github.com> Date: Fri, 28 Mar 2025 13:14:08 +0800 Subject: [PATCH 08/47] Add Kaynebus --- src/main/java/raidzero/robot/Constants.java | 4 +++- .../java/raidzero/robot/subsystems/LEDStrip/ArmStrip.java | 2 +- .../java/raidzero/robot/subsystems/climb/ClimbJoint.java | 2 +- src/main/java/raidzero/robot/subsystems/climb/Winch.java | 2 +- .../java/raidzero/robot/subsystems/telescopingarm/Arm.java | 6 +++--- .../robot/subsystems/telescopingarm/CoralIntake.java | 2 +- 6 files changed, 10 insertions(+), 8 deletions(-) diff --git a/src/main/java/raidzero/robot/Constants.java b/src/main/java/raidzero/robot/Constants.java index f99ff0c..cc3a096 100644 --- a/src/main/java/raidzero/robot/Constants.java +++ b/src/main/java/raidzero/robot/Constants.java @@ -255,7 +255,9 @@ public class Telescope { } } - public static final String CANIVORE_NAME = "CANdoAttitude"; + public static final String BASE_CANIVORE = "CANdoAttitude"; + public static final String KAYNE_BUS = "Kaynebus"; + public static final String RIO_BUS = "rio"; public static final double STICK_DEADBAND = 0.2; } \ No newline at end of file diff --git a/src/main/java/raidzero/robot/subsystems/LEDStrip/ArmStrip.java b/src/main/java/raidzero/robot/subsystems/LEDStrip/ArmStrip.java index c6dd68a..144de72 100644 --- a/src/main/java/raidzero/robot/subsystems/LEDStrip/ArmStrip.java +++ b/src/main/java/raidzero/robot/subsystems/LEDStrip/ArmStrip.java @@ -38,7 +38,7 @@ public class ArmStrip implements Subsystem { * Constructs a {@link ArmStrip} subsystem. */ private ArmStrip() { - this.candle = new CANdle(Constants.CANdle.CAN_ID, Constants.CANIVORE_NAME); + this.candle = new CANdle(Constants.CANdle.CAN_ID, Constants.BASE_CANIVORE); this.arm = Arm.system(); this.candle.configAllSettings(candleConfig()); diff --git a/src/main/java/raidzero/robot/subsystems/climb/ClimbJoint.java b/src/main/java/raidzero/robot/subsystems/climb/ClimbJoint.java index 372a195..9eb0668 100644 --- a/src/main/java/raidzero/robot/subsystems/climb/ClimbJoint.java +++ b/src/main/java/raidzero/robot/subsystems/climb/ClimbJoint.java @@ -22,7 +22,7 @@ public class ClimbJoint extends SubsystemBase { * Constructs a {@link ClimbJoint} subsystem instance */ private ClimbJoint() { - joint = new TalonFX(Constants.Climb.Joint.MOTOR_ID); + joint = new TalonFX(Constants.Climb.Joint.MOTOR_ID, Constants.RIO_BUS); joint.getConfigurator().apply(jointConfiguration()); joint.setNeutralMode(NeutralModeValue.Brake); } diff --git a/src/main/java/raidzero/robot/subsystems/climb/Winch.java b/src/main/java/raidzero/robot/subsystems/climb/Winch.java index ef2eee1..80780a9 100644 --- a/src/main/java/raidzero/robot/subsystems/climb/Winch.java +++ b/src/main/java/raidzero/robot/subsystems/climb/Winch.java @@ -16,7 +16,7 @@ public class Winch extends SubsystemBase { * Constructs a {@link Winch} subsystem instance */ private Winch() { - winch = new TalonFX(Constants.Climb.Winch.MOTOR_ID); + winch = new TalonFX(Constants.Climb.Winch.MOTOR_ID, Constants.RIO_BUS); winch.getConfigurator().apply(winchConfiguration()); winch.setNeutralMode(NeutralModeValue.Brake); } diff --git a/src/main/java/raidzero/robot/subsystems/telescopingarm/Arm.java b/src/main/java/raidzero/robot/subsystems/telescopingarm/Arm.java index 05b7118..433585e 100644 --- a/src/main/java/raidzero/robot/subsystems/telescopingarm/Arm.java +++ b/src/main/java/raidzero/robot/subsystems/telescopingarm/Arm.java @@ -34,15 +34,15 @@ public class Arm extends SubsystemBase { * Constructs an {@link Arm} subsystem instance */ private Arm() { - telescope = new TalonFX(Constants.TelescopingArm.Telescope.MOTOR_ID); + telescope = new TalonFX(Constants.TelescopingArm.Telescope.MOTOR_ID, "Kaynebus"); telescope.getConfigurator().apply(telescopeConfiguration()); telescope.setNeutralMode(NeutralModeValue.Brake); - joint = new TalonFX(Constants.TelescopingArm.Joint.MOTOR_ID); + joint = new TalonFX(Constants.TelescopingArm.Joint.MOTOR_ID, "Kaynebus"); joint.getConfigurator().apply(jointConfiguration()); joint.setNeutralMode(NeutralModeValue.Brake); - jointCANcoder = new CANcoder(Constants.TelescopingArm.Joint.CANCODER_ID); + jointCANcoder = new CANcoder(Constants.TelescopingArm.Joint.CANCODER_ID, "Kaynebus"); jointCANcoder.getConfigurator().apply(jointCANCoderConfiguration()); currentPose = new double[] { 0.0, 0.0 }; diff --git a/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java b/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java index bd4ca56..f927d6f 100644 --- a/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java +++ b/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java @@ -24,7 +24,7 @@ public class CoralIntake extends SubsystemBase { * Constructs a {@link CoralIntake} subsystem instance */ private CoralIntake() { - roller = new TalonFXS(Constants.TelescopingArm.Intake.MOTOR_ID); + roller = new TalonFXS(Constants.TelescopingArm.Intake.MOTOR_ID, "rio"); roller.getConfigurator().apply(rollerConfiguration()); bottomLaser = new LazyCan(1).withRangingMode(RangingMode.SHORT) From 7a6cdd91aed8f498cb885a5c17add91f4b502be5 Mon Sep 17 00:00:00 2001 From: Theo Date: Sat, 5 Apr 2025 19:20:56 +0800 Subject: [PATCH 09/47] add velocity pid --- src/main/java/raidzero/robot/Constants.java | 4 ++++ .../robot/subsystems/telescopingarm/CoralIntake.java | 9 +++++++++ 2 files changed, 13 insertions(+) diff --git a/src/main/java/raidzero/robot/Constants.java b/src/main/java/raidzero/robot/Constants.java index cc3a096..15bb71f 100644 --- a/src/main/java/raidzero/robot/Constants.java +++ b/src/main/java/raidzero/robot/Constants.java @@ -175,6 +175,10 @@ public class Intake { public static final double EXTAKE_SPEED = 0.25; public static final double EXTAKE_TIMEOUT_S = 1.0; + public static final double KP = 1.0; + public static final double KI = 0.0; + public static final double KD = 0.0; + } 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 f927d6f..1d8b3fb 100644 --- a/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java +++ b/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java @@ -2,8 +2,11 @@ import au.grapplerobotics.interfaces.LaserCanInterface.RangingMode; import au.grapplerobotics.interfaces.LaserCanInterface.TimingBudget; + +import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.TalonFXSConfiguration; import com.ctre.phoenix6.controls.StaticBrake; +import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.TalonFXS; import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.wpilibj2.command.Command; @@ -65,6 +68,10 @@ public Command intakeSimple() { .andThen(run(() -> roller.set(Intake.REVERSE_SPEED)).withTimeout(0.5)); } + public Command runWithVelocity(double velocity) { + return run(() -> roller.setControl(new VelocityVoltage(velocity))); + } + /** * Returns true if the current is above a pre-defined threshold to consider it stalling * @@ -137,6 +144,8 @@ private TalonFXSConfiguration rollerConfiguration() { configuration.CurrentLimits.SupplyCurrentLimit = Intake.SUPPLY_CURRENT_LIMIT; configuration.CurrentLimits.SupplyCurrentLowerTime = Intake.SUPPLY_CURRENT_LOWER_TIME; + configuration.Slot0 = new Slot0Configs().withKP(Intake.KP).withKI(Intake.KI).withKD(Intake.KD); + configuration.MotorOutput.NeutralMode = NeutralModeValue.Brake; return configuration; From 87ed7219b500934425d9f1edf757be08b5d3593b Mon Sep 17 00:00:00 2001 From: Theo Date: Sun, 6 Apr 2025 19:45:38 +0800 Subject: [PATCH 10/47] add patel intake logic --- src/main/java/raidzero/robot/Constants.java | 1 + .../telescopingarm/CoralIntake.java | 32 ++++++++++++++++--- 2 files changed, 29 insertions(+), 4 deletions(-) diff --git a/src/main/java/raidzero/robot/Constants.java b/src/main/java/raidzero/robot/Constants.java index 15bb71f..a973c00 100644 --- a/src/main/java/raidzero/robot/Constants.java +++ b/src/main/java/raidzero/robot/Constants.java @@ -171,6 +171,7 @@ public class Intake { public static final double REVERSE_SPEED = -0.2; public static final double STALL_CURRENT_THRESHOLD = 20.0; + public static final double CURRENT_SPIKE_THRESHOLD = 20.0; public static final double EXTAKE_SPEED = 0.25; public static final double EXTAKE_TIMEOUT_S = 1.0; diff --git a/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java b/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java index 1d8b3fb..7904560 100644 --- a/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java +++ b/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java @@ -5,6 +5,8 @@ import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.TalonFXSConfiguration; +import com.ctre.phoenix6.controls.MotionMagicDutyCycle; +import com.ctre.phoenix6.controls.MotionMagicTorqueCurrentFOC; import com.ctre.phoenix6.controls.StaticBrake; import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.TalonFXS; @@ -48,6 +50,8 @@ public void enableStaticBrake() { /** * Creates a {@link Command} to run the intake at the specified speed + * + * old freaky intake logic from bae * * @param speed The speed as a percentage * @return The command to be scheduled and run @@ -63,15 +67,26 @@ public Command intake() { ); } + /** + * patel's logic from teams (PLEASE TUNE THIS) + * + * @return its the command bro + */ + public Command patelIntake() { + return run(() -> roller.set(Intake.INTAKE_SPEED)).until(() -> currentSpike()) + .andThen(() -> roller.setControl(new MotionMagicTorqueCurrentFOC(roller.getPosition().getValueAsDouble() + 5))); + } + + /** + * Literally just run it unitl bottom laser + * + * @return if i need to explain what a command is I don't think you should be reading this + */ public Command intakeSimple() { return run(() -> roller.set(Intake.INTAKE_SPEED)).until(() -> bottomLaser.withinThreshold()) .andThen(run(() -> roller.set(Intake.REVERSE_SPEED)).withTimeout(0.5)); } - public Command runWithVelocity(double velocity) { - return run(() -> roller.setControl(new VelocityVoltage(velocity))); - } - /** * Returns true if the current is above a pre-defined threshold to consider it stalling * @@ -81,6 +96,15 @@ private boolean isStalling() { return roller.getStatorCurrent().getValueAsDouble() > Intake.STALL_CURRENT_THRESHOLD; } + /** + * Not to be confused with currant spike + * + * @return if your currant i mean current is spiking + */ + private boolean currentSpike() { + return roller.getStatorCurrent().getValueAsDouble() > Intake.CURRENT_SPIKE_THRESHOLD; + } + /** * Creates a {@link Command} to stop the intake * From 1f69fd70078624a563c600973eb72418ae9090cd Mon Sep 17 00:00:00 2001 From: Theo Date: Tue, 8 Apr 2025 09:40:43 +0800 Subject: [PATCH 11/47] add new intake logic --- .vscode/settings.json | 3 +- src/main/java/raidzero/robot/Constants.java | 12 ++-- src/main/java/raidzero/robot/Robot.java | 1 - .../java/raidzero/robot/RobotContainer.java | 2 +- .../telescopingarm/CoralIntake.java | 67 +++++-------------- 5 files changed, 27 insertions(+), 58 deletions(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index 168fa10..2236597 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -58,5 +58,6 @@ "edu.wpi.first.math.**.struct.*", ], "java.format.settings.url": "eclipse-formatter.xml", - "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx2G -Xms100m -Xlog:disable" + "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx2G -Xms100m -Xlog:disable", + "wpilib.autoStartRioLog": false } diff --git a/src/main/java/raidzero/robot/Constants.java b/src/main/java/raidzero/robot/Constants.java index a973c00..422fe88 100644 --- a/src/main/java/raidzero/robot/Constants.java +++ b/src/main/java/raidzero/robot/Constants.java @@ -163,15 +163,15 @@ public class Intake { public static final double SUPPLY_CURRENT_LOWER_TIME = 0.0; public static final double TOP_LASER_THRESHOLD_MM = 100.0; - public static final double BOTTOM_LASER_THRESHOLD_MM = 50.0; + public static final double BOTTOM_LASER_THRESHOLD_MM = 100.0; - public static final double INTAKE_SPEED = 0.50; - public static final double INTAKE_LOWER_SPEED = 0.25; + public static final double INTAKE_SPEED = 0.25; + public static final double LOWER_SPEED = 0.15; public static final double EJECT_SPEED = -0.50; public static final double REVERSE_SPEED = -0.2; public static final double STALL_CURRENT_THRESHOLD = 20.0; - public static final double CURRENT_SPIKE_THRESHOLD = 20.0; + public static final double CURRENT_SPIKE_THRESHOLD = 10.0; public static final double EXTAKE_SPEED = 0.25; public static final double EXTAKE_TIMEOUT_S = 1.0; @@ -187,7 +187,7 @@ public class Joint { public static final int CANCODER_ID = 11; public static final double CANCODER_GEAR_RATIO = 28.0 / 80.0; - public static final double CANCODER_OFFSET = -(0.358398 - (0.25 / CANCODER_GEAR_RATIO)); + public static final double CANCODER_OFFSET = -(0.325684 - (0.25 / CANCODER_GEAR_RATIO)); public static final double CANCODER_DISCONTINUITY_POINT = 0.5; public static final double CONVERSION_FACTOR = (120.0 / 12.0) * 20.0; @@ -213,7 +213,7 @@ public class Joint { } public class Positions { - public static final double[] L4_SCORING_POS_M = { -0.24, 2.72 }; + public static final double[] L4_SCORING_POS_M = { -0.24, 2.75 }; public static final double[] L4_SCORING_POS_M_BLUE = { -0.17, 2.68 }; public static final double[] L4_CHECK_POSITION = { -0.25, 2.62 }; public static final double[] L4_GRAND_SLAM = { -0.2, 1.57 }; diff --git a/src/main/java/raidzero/robot/Robot.java b/src/main/java/raidzero/robot/Robot.java index a1d3d48..40c85dc 100644 --- a/src/main/java/raidzero/robot/Robot.java +++ b/src/main/java/raidzero/robot/Robot.java @@ -51,7 +51,6 @@ public void disabledPeriodic() { @Override public void disabledExit() { - CoralIntake.system().enableStaticBrake();; ArmStrip.system().resetAnimation(); } diff --git a/src/main/java/raidzero/robot/RobotContainer.java b/src/main/java/raidzero/robot/RobotContainer.java index 66d0b7b..141984d 100644 --- a/src/main/java/raidzero/robot/RobotContainer.java +++ b/src/main/java/raidzero/robot/RobotContainer.java @@ -110,7 +110,7 @@ private void configureBindings() { ); joystick.leftTrigger().whileTrue(coralIntake.extake()); - joystick.rightTrigger().onTrue(coralIntake.intakeSimple()); + joystick.rightTrigger().onTrue(coralIntake.intake()); joystick.b().whileTrue( swerve.pathToStation() diff --git a/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java b/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java index 7904560..e40dd17 100644 --- a/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java +++ b/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java @@ -2,13 +2,10 @@ import au.grapplerobotics.interfaces.LaserCanInterface.RangingMode; import au.grapplerobotics.interfaces.LaserCanInterface.TimingBudget; - import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.TalonFXSConfiguration; -import com.ctre.phoenix6.controls.MotionMagicDutyCycle; import com.ctre.phoenix6.controls.MotionMagicTorqueCurrentFOC; import com.ctre.phoenix6.controls.StaticBrake; -import com.ctre.phoenix6.controls.VelocityVoltage; import com.ctre.phoenix6.hardware.TalonFXS; import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.wpilibj2.command.Command; @@ -33,73 +30,46 @@ private CoralIntake() { roller.getConfigurator().apply(rollerConfiguration()); bottomLaser = new LazyCan(1).withRangingMode(RangingMode.SHORT) - .withRegionOfInterest(8, 8, 16, 16).withTimingBudget(TimingBudget.TIMING_BUDGET_20MS) + .withRegionOfInterest(14, 8, 4, 16).withTimingBudget(TimingBudget.TIMING_BUDGET_20MS) .withThreshold(Intake.BOTTOM_LASER_THRESHOLD_MM); - topLaser = new LazyCan(0).withRangingMode(RangingMode.SHORT) - .withRegionOfInterest(8, 8, 16, 16).withTimingBudget(TimingBudget.TIMING_BUDGET_20MS) + topLaser = new LazyCan(0).withRangingMode(RangingMode.LONG) + .withRegionOfInterest(8, 14, 16, 4).withTimingBudget(TimingBudget.TIMING_BUDGET_20MS) .withThreshold(Intake.TOP_LASER_THRESHOLD_MM); } /** - * Sets the motor to static brake mode for disabled init to check for position - */ - public void enableStaticBrake() { - roller.setControl(new StaticBrake()); - } - - /** - * Creates a {@link Command} to run the intake at the specified speed - * - * old freaky intake logic from bae + * Intakes coral * - * @param speed The speed as a percentage - * @return The command to be scheduled and run + * @return A {@link Command} */ public Command intake() { - return run(() -> roller.set(Intake.INTAKE_SPEED)).until(() -> topLaser.withinThreshold()) - .andThen( - new SequentialCommandGroup( - run(() -> roller.set(Intake.EJECT_SPEED)).onlyIf(() -> isStalling()).withTimeout(0.5), - run(() -> roller.set(Intake.INTAKE_LOWER_SPEED)).onlyIf(() -> !isStalling()).until(() -> bottomLaser.withinThreshold()) - .andThen(() -> roller.set(Intake.REVERSE_SPEED)).until(() -> !bottomLaser.withinThreshold()) - ) - ); - } - - /** - * patel's logic from teams (PLEASE TUNE THIS) - * - * @return its the command bro - */ - public Command patelIntake() { return run(() -> roller.set(Intake.INTAKE_SPEED)).until(() -> currentSpike()) - .andThen(() -> roller.setControl(new MotionMagicTorqueCurrentFOC(roller.getPosition().getValueAsDouble() + 5))); + .andThen(() -> roller.set(Intake.LOWER_SPEED)).until(() -> bottomLaser.withinThreshold()); } /** - * Literally just run it unitl bottom laser - * - * @return if i need to explain what a command is I don't think you should be reading this + * Runs the intake at the normal speed until a current spike is detected. Used for autons. + * + * @return A {@link Command} */ - public Command intakeSimple() { - return run(() -> roller.set(Intake.INTAKE_SPEED)).until(() -> bottomLaser.withinThreshold()) - .andThen(run(() -> roller.set(Intake.REVERSE_SPEED)).withTimeout(0.5)); + public Command autoIntakeP1() { + return run(() -> roller.set(Intake.INTAKE_SPEED)).until(() -> currentSpike()); } /** - * Returns true if the current is above a pre-defined threshold to consider it stalling + * Runs the intake until the bottom bottom laser sees a coral. Used for autons. * - * @return if the motor is stalliing + * @return A {@link Command} */ - private boolean isStalling() { - return roller.getStatorCurrent().getValueAsDouble() > Intake.STALL_CURRENT_THRESHOLD; + public Command autoIntakeP2() { + return run(() -> roller.set(Intake.LOWER_SPEED)).until(() -> bottomLaser.withinThreshold()); } /** - * Not to be confused with currant spike - * - * @return if your currant i mean current is spiking + * Detects if the stator of the motor is above the predefined threshold + * + * @return True if the current is above the specified spike threshold */ private boolean currentSpike() { return roller.getStatorCurrent().getValueAsDouble() > Intake.CURRENT_SPIKE_THRESHOLD; @@ -117,7 +87,6 @@ public Command stop() { /** * Creates a {@link Command} to extake at the specified speed * - * @param speed The desired speed [0, 1.0] * @return A {@link Command} to extake at the specified speed */ public Command extake() { From 7336c4f7dcd14abdd1bbeba70c159ed3efb78e5c Mon Sep 17 00:00:00 2001 From: Theo Date: Tue, 8 Apr 2025 09:46:59 +0800 Subject: [PATCH 12/47] fix merge errors --- src/main/java/raidzero/robot/RobotContainer.java | 3 --- .../raidzero/robot/subsystems/telescopingarm/CoralIntake.java | 2 +- 2 files changed, 1 insertion(+), 4 deletions(-) diff --git a/src/main/java/raidzero/robot/RobotContainer.java b/src/main/java/raidzero/robot/RobotContainer.java index c7db0d8..9a7cc15 100644 --- a/src/main/java/raidzero/robot/RobotContainer.java +++ b/src/main/java/raidzero/robot/RobotContainer.java @@ -168,11 +168,8 @@ private void configureBindings() { .onlyIf(swerve.isArmDeployable()) ); - operator.button(Constants.Bindings.ALGAE_INTAKE).onTrue(coralIntake.contingencyIntake()); - operator.button(Constants.Bindings.CORAL_EXTAKE).whileTrue(coralIntake.extake()); operator.button(Constants.Bindings.CORAL_INTAKE).onTrue(coralIntake.intake()); - operator.button(Constants.Bindings.CORAL_SCOOCH).onTrue(coralIntake.scoochCoral()); operator.button(Constants.Bindings.CLIMB_DEPLOY) .onTrue( diff --git a/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java b/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java index 69c0037..3535ed9 100644 --- a/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java +++ b/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java @@ -150,7 +150,7 @@ private TalonFXSConfiguration rollerConfiguration() { configuration.MotorOutput.NeutralMode = NeutralModeValue.Brake; - return config; + return configuration; } /** From 21796a9ed705fc7bee9aeadff364adafba73953c Mon Sep 17 00:00:00 2001 From: Theo Date: Tue, 8 Apr 2025 09:49:59 +0800 Subject: [PATCH 13/47] remove unused imports and variables --- src/main/java/raidzero/lib/LazyCan.java | 5 ----- src/main/java/raidzero/robot/Robot.java | 2 -- .../java/raidzero/robot/subsystems/climb/Winch.java | 1 - .../robot/subsystems/telescopingarm/CoralIntake.java | 12 ------------ 4 files changed, 20 deletions(-) diff --git a/src/main/java/raidzero/lib/LazyCan.java b/src/main/java/raidzero/lib/LazyCan.java index b33148d..869e8e0 100644 --- a/src/main/java/raidzero/lib/LazyCan.java +++ b/src/main/java/raidzero/lib/LazyCan.java @@ -1,6 +1,5 @@ package raidzero.lib; -import au.grapplerobotics.ConfigurationFailedException; import au.grapplerobotics.ConfigurationFailedException; import au.grapplerobotics.LaserCan; import au.grapplerobotics.interfaces.LaserCanInterface.Measurement; @@ -13,9 +12,7 @@ public class LazyCan { private LaserCan laserCan; private int canId; - private RangingMode rangingMode; private RegionOfInterest regionOfInterest; - private TimingBudget timingBudget; private Measurement measurement; @@ -84,7 +81,6 @@ public LazyCan withRegionOfInterest(int x, int y, int w, int h) { * @return the current LazyCan Object */ public LazyCan withRangingMode(RangingMode rangingMode) { - this.rangingMode = rangingMode; try { laserCan.setRangingMode(rangingMode); } catch (ConfigurationFailedException e) { @@ -100,7 +96,6 @@ public LazyCan withRangingMode(RangingMode rangingMode) { * @return the current LazyCan Object */ public LazyCan withTimingBudget(TimingBudget timingBudget) { - this.timingBudget = timingBudget; try { laserCan.setTimingBudget(timingBudget); } catch (ConfigurationFailedException e) { diff --git a/src/main/java/raidzero/robot/Robot.java b/src/main/java/raidzero/robot/Robot.java index 40c85dc..77bfa72 100644 --- a/src/main/java/raidzero/robot/Robot.java +++ b/src/main/java/raidzero/robot/Robot.java @@ -5,7 +5,6 @@ package raidzero.robot; import au.grapplerobotics.CanBridge; -import com.ctre.phoenix6.controls.StaticBrake; import edu.wpi.first.net.WebServer; import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.Filesystem; @@ -17,7 +16,6 @@ import raidzero.robot.subsystems.LEDStrip.ArmStrip; import raidzero.robot.subsystems.drivetrain.Swerve; import raidzero.robot.subsystems.telescopingarm.Arm; -import raidzero.robot.subsystems.telescopingarm.CoralIntake; public class Robot extends TimedRobot { private Command m_autonomousCommand; diff --git a/src/main/java/raidzero/robot/subsystems/climb/Winch.java b/src/main/java/raidzero/robot/subsystems/climb/Winch.java index 6258bb4..f92b891 100644 --- a/src/main/java/raidzero/robot/subsystems/climb/Winch.java +++ b/src/main/java/raidzero/robot/subsystems/climb/Winch.java @@ -4,7 +4,6 @@ import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.NeutralModeValue; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import raidzero.robot.Constants; diff --git a/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java b/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java index 3535ed9..e1405cb 100644 --- a/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java +++ b/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java @@ -4,29 +4,17 @@ import au.grapplerobotics.interfaces.LaserCanInterface.TimingBudget; import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.TalonFXSConfiguration; -import com.ctre.phoenix6.controls.MotionMagicTorqueCurrentFOC; -import com.ctre.phoenix6.controls.StaticBrake; import com.ctre.phoenix6.hardware.TalonFXS; 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.SequentialCommandGroup; import edu.wpi.first.wpilibj2.command.SubsystemBase; import raidzero.lib.LazyCan; import raidzero.robot.Constants; import raidzero.robot.Constants.TelescopingArm.Intake; -import raidzero.robot.Constants.TelescopingArm.Intake; public class CoralIntake extends SubsystemBase { private TalonFXS roller; - private ServoHub servoHub; - private ServoChannel intakeBlock; - private LazyCan bottomLaser, topLaser; private static CoralIntake system; From 841a1d80d8b55a5c16a23da95644fc151cc5956f Mon Sep 17 00:00:00 2001 From: Theo Date: Thu, 10 Apr 2025 06:39:54 +0800 Subject: [PATCH 14/47] adjust settings for alex climb --- src/main/java/raidzero/robot/Constants.java | 2 +- src/main/java/raidzero/robot/subsystems/climb/Winch.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/raidzero/robot/Constants.java b/src/main/java/raidzero/robot/Constants.java index 09ead4c..169f704 100644 --- a/src/main/java/raidzero/robot/Constants.java +++ b/src/main/java/raidzero/robot/Constants.java @@ -111,7 +111,7 @@ public static class Joint { public static class Winch { public static final int MOTOR_ID = 16; - public static final double SPEED = 0.75; + public static final double SPEED = 1.0; } } diff --git a/src/main/java/raidzero/robot/subsystems/climb/Winch.java b/src/main/java/raidzero/robot/subsystems/climb/Winch.java index f92b891..a3c2826 100644 --- a/src/main/java/raidzero/robot/subsystems/climb/Winch.java +++ b/src/main/java/raidzero/robot/subsystems/climb/Winch.java @@ -50,7 +50,7 @@ private TalonFXConfiguration winchConfiguration() { TalonFXConfiguration configuration = new TalonFXConfiguration() .withOpenLoopRamps( new OpenLoopRampsConfigs() - .withDutyCycleOpenLoopRampPeriod(0.2) + .withDutyCycleOpenLoopRampPeriod(0.1) ); return configuration; From 129a9acb26b18b44001d6ca0c320fe5935ed8dac Mon Sep 17 00:00:00 2001 From: Theo Date: Fri, 11 Apr 2025 07:50:01 +0800 Subject: [PATCH 15/47] add algae behaviors --- src/main/java/raidzero/robot/Constants.java | 26 ++++++++---- .../java/raidzero/robot/RobotContainer.java | 42 +++++++++++++++---- .../robot/subsystems/telescopingarm/Arm.java | 8 ++++ .../telescopingarm/CoralIntake.java | 27 +++++++++++- 4 files changed, 83 insertions(+), 20 deletions(-) diff --git a/src/main/java/raidzero/robot/Constants.java b/src/main/java/raidzero/robot/Constants.java index 169f704..e3a6e0a 100644 --- a/src/main/java/raidzero/robot/Constants.java +++ b/src/main/java/raidzero/robot/Constants.java @@ -158,24 +158,28 @@ public static class Intake { public static final InvertedValue INVERTED_VALUE = InvertedValue.CounterClockwise_Positive; - public static final int STATOR_CURRENT_LIMIT = 40; + public static final int STATOR_CURRENT_LIMIT = 50; public static final int SUPPLY_CURRENT_LIMIT = 50; public static final double SUPPLY_CURRENT_LOWER_TIME = 0.0; public static final double TOP_LASER_THRESHOLD_MM = 100.0; public static final double BOTTOM_LASER_THRESHOLD_MM = 100.0; - public static final double INTAKE_SPEED = 0.25; - public static final double LOWER_SPEED = 0.15; - public static final double EJECT_SPEED = -0.50; + public static final double INTAKE_SPEED = 0.85; + public static final double LOWER_SPEED = 0.25; + public static final double EJECT_SPEED = -0.80; public static final double REVERSE_SPEED = -0.2; public static final double STALL_CURRENT_THRESHOLD = 20.0; public static final double CURRENT_SPIKE_THRESHOLD = 10.0; - public static final double EXTAKE_SPEED = 0.25; + public static final double EXTAKE_SPEED = 0.80; public static final double EXTAKE_TIMEOUT_S = 1.0; + public static final double ALGAE_INTAKE_SPEED = 1.0; + public static final double ALGAE_EJECT_SPEED = -1.0; + public static final double HOLD_SPEED = 0.4; + public static final double KP = 1.0; public static final double KI = 0.0; public static final double KD = 0.0; @@ -212,19 +216,23 @@ public static class Joint { public static final double SUPPLY_CURRENT_LOWER_TIME = 0.0; } - public class Positions { + public static class Positions { public static final double[] L4_SCORING_POS_M = { -0.24, 2.75 }; public static final double[] L4_SCORING_POS_M_BLUE = { -0.17, 2.68 }; public static final double[] L4_CHECK_POSITION = { -0.25, 2.62 }; public static final double[] L4_GRAND_SLAM = { -0.2, 1.57 }; - public static double[] L3_SCORING_POS_M = { -0.20, 1.57 }; - public static double[] L2_SCORING_POS_M = { -0.2, 0.9 }; - public static double[] L1_SCORING_POS_M = { 0.0, 0.0 }; + public static final double[] L3_SCORING_POS_M = { -0.20, 1.57 }; + public static final double[] L2_SCORING_POS_M = { -0.2, 0.9 }; + public static final double[] L1_SCORING_POS_M = { 0.0, 0.0 }; public static final double[] INTAKE_POS_M = { 0.5, 0.835 }; public static final double[] INTAKE_POS_M_BLUE = { 0.5, 0.88 }; + public static final double[] L3_ALGAE_POS_M = { 0.75, 1.3 }; + public static final double[] L2_ALGAE_POS_M = { 0.5, 0.853 }; + public static final double[] BARGE_SCORE_POS_M = { 0, 2.8 }; + public static double[] HOME_POS_M = { 0.0, 0.0 }; } diff --git a/src/main/java/raidzero/robot/RobotContainer.java b/src/main/java/raidzero/robot/RobotContainer.java index 9a7cc15..4a33df8 100644 --- a/src/main/java/raidzero/robot/RobotContainer.java +++ b/src/main/java/raidzero/robot/RobotContainer.java @@ -21,6 +21,7 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.button.CommandGenericHID; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import raidzero.robot.Constants.TelescopingArm.Positions; import raidzero.robot.subsystems.LEDStrip.ArmStrip; import raidzero.robot.subsystems.climb.ClimbJoint; import raidzero.robot.subsystems.climb.Winch; @@ -154,22 +155,24 @@ private void configureBindings() { operator.button(Constants.Bindings.BOTTOM_LEFT).onTrue(new InstantCommand(() -> arm.decreaseIntakeYOffset(-0.01), arm)); operator.button(Constants.Bindings.TOP_RIGHT).onTrue(new InstantCommand(() -> arm.removeIntakeOffset(), arm)); - operator.button(Constants.Bindings.L2).whileTrue( + operator.button(Constants.Bindings.L2).and(operator.button(Constants.Bindings.BOTTOM_RIGHT).negate()).whileTrue( arm.moveTo(Constants.TelescopingArm.Positions.L2_SCORING_POS_M) .onlyIf(swerve.isArmDeployable()) ); - operator.button(Constants.Bindings.L3).whileTrue( + operator.button(Constants.Bindings.L3).and(operator.button(Constants.Bindings.BOTTOM_RIGHT).negate()).whileTrue( arm.moveTo(Constants.TelescopingArm.Positions.L3_SCORING_POS_M) .onlyIf(swerve.isArmDeployable()) ); - operator.button(Constants.Bindings.L4).whileTrue( + operator.button(Constants.Bindings.L4).and(operator.button(Constants.Bindings.BOTTOM_RIGHT).negate()).whileTrue( arm.moveToL4() .onlyIf(swerve.isArmDeployable()) ); - operator.button(Constants.Bindings.CORAL_EXTAKE).whileTrue(coralIntake.extake()); + operator.button(Constants.Bindings.CORAL_EXTAKE).and(operator.button(Constants.Bindings.BOTTOM_RIGHT).negate()) + .whileTrue(coralIntake.extake()); operator.button(Constants.Bindings.CORAL_INTAKE).onTrue(coralIntake.intake()); + operator.button(Constants.Bindings.CORAL_SCOOCH).whileTrue(coralIntake.run(Constants.TelescopingArm.Intake.REVERSE_SPEED)); operator.button(Constants.Bindings.CLIMB_DEPLOY) .onTrue( @@ -186,17 +189,38 @@ private void configureBindings() { ); operator.button(Constants.Bindings.CLIMB_DEPLOY).onTrue(arm.climbPos()); - // operator.button(Constants.Bindings.CLIMB_UP) - // .whileTrue(climbWinch.run(Constants.Climb.Winch.SPEED).onlyIf(climbJoint.isDeployed())); - operator.button(Constants.Bindings.CLIMB_UP).whileTrue(climbWinch.run(Constants.Climb.Winch.SPEED, climbJoint.getPosition() > 0.3)); operator.button(Constants.Bindings.CLIMB_UP).onTrue(climbJoint.retract()); operator.button(Constants.Bindings.CLIMB_DOWN) .whileTrue(climbWinch.run(-Constants.Climb.Winch.SPEED, climbJoint.getPosition() > 0.3).onlyIf(climbJoint.isDeployed())); - // operator.axisGreaterThan(0, 0.6).whileTrue(climbJoint.run(0.125)); - // operator.axisGreaterThan(1, 0.6).whileTrue(climbJoint.run(0.28)); + // default arm if the top laser sees but botom doesn't it goes to vertical + run intake at 0.2 + operator.button(Constants.Bindings.BOTTOM_RIGHT).and(operator.button(Constants.Bindings.L3).negate()) + .and(operator.button(Constants.Bindings.L2).negate()).and(operator.button(Constants.Bindings.L4).negate()) + // .and(operator.button(Constants.Bindings.CORAL_EXTAKE).negate()) + .whileTrue(arm.moveWithRotations(0.25, 0)); + + operator.button(Constants.Bindings.BOTTOM_RIGHT).and(operator.button(Constants.Bindings.L3).negate()) + .and(operator.button(Constants.Bindings.L2).negate()) + .whileTrue(coralIntake.holdAlgae()); + + // bottom right + l3 = l3 algae + intake + operator.button(Constants.Bindings.BOTTOM_RIGHT).and(operator.button(Constants.Bindings.L3)) + .whileTrue(arm.moveTo(Positions.L3_ALGAE_POS_M).alongWith(coralIntake.intakeAlgae())); + + // bottom right + l2 = l2 algae + intake + operator.button(Constants.Bindings.BOTTOM_RIGHT).and(operator.button(Constants.Bindings.L2)) + .whileTrue(arm.moveTo(Positions.L2_ALGAE_POS_M).alongWith(coralIntake.intakeAlgae())); + + // bottom right + l4 = barge position? (near l4) + operator.button(Constants.Bindings.BOTTOM_RIGHT).and(operator.button(Constants.Bindings.L4)) + .whileTrue(arm.moveTo(Positions.BARGE_SCORE_POS_M)); + // .alongWith(Commands.waitSeconds(0.7).andThen(coralIntake.extaxeAlgae()))); + + // bottom right + out = out algae + operator.button(Constants.Bindings.BOTTOM_RIGHT).and(operator.button(Constants.Bindings.CORAL_EXTAKE)) + .whileTrue(coralIntake.extaxeAlgae()); swerve.registerTelemetry(logger::telemeterize); } diff --git a/src/main/java/raidzero/robot/subsystems/telescopingarm/Arm.java b/src/main/java/raidzero/robot/subsystems/telescopingarm/Arm.java index c8d6952..e209882 100644 --- a/src/main/java/raidzero/robot/subsystems/telescopingarm/Arm.java +++ b/src/main/java/raidzero/robot/subsystems/telescopingarm/Arm.java @@ -18,6 +18,7 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; +import edu.wpi.first.wpilibj2.command.WaitCommand; import raidzero.robot.Constants; import raidzero.robot.subsystems.climb.ClimbJoint; @@ -77,6 +78,12 @@ public Command moveTo(double[] desiredPosition) { } } + public Command moveWithRotations(double jointSetpoint, double telescopeSetpoint) { + return run( + () -> moveJoint(jointSetpoint) + ).alongWith(Commands.waitSeconds(0.3).andThen(() -> moveTelescope(telescopeSetpoint))); + } + /** * Moves the arm to the desired x and y setpoints without delay * @@ -155,6 +162,7 @@ public Command moveToIntake() { ); } + } /** diff --git a/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java b/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java index e1405cb..ecd89c3 100644 --- a/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java +++ b/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java @@ -41,8 +41,23 @@ private CoralIntake() { * @return A {@link Command} */ public Command intake() { - return run(() -> roller.set(Intake.INTAKE_SPEED)).until(() -> currentSpike()) - .andThen(() -> roller.set(Intake.LOWER_SPEED)).until(() -> bottomLaser.withinThreshold()); + return run(() -> roller.set(Intake.INTAKE_SPEED)).until(() -> bottomLaser.withinThreshold()); + } + + public Command intakeAlgae() { + return run(() -> roller.set(Intake.INTAKE_SPEED)); + } + + public Command extaxeAlgae() { + return run(() -> roller.set(Intake.ALGAE_EJECT_SPEED)); + } + + public Command extaxeAlgaeSlow() { + return run(() -> roller.set(-0.2)); + } + + public Command holdAlgae() { + return run(() -> roller.set(Intake.HOLD_SPEED)); } /** @@ -110,6 +125,14 @@ public int getTopLaserDistance() { return topLaser.getDistanceMm(); } + public boolean topLaserWithinThreshold() { + return topLaser.withinThreshold(); + } + + public boolean bottomLaserWithinThreshold() { + return bottomLaser.withinThreshold(); + } + /** * Gets the distance from the LaserCAN * From 755dad5dcdf08cf579804b7855da7fb6bd701eea Mon Sep 17 00:00:00 2001 From: Theo Date: Fri, 11 Apr 2025 07:52:19 +0800 Subject: [PATCH 16/47] remove unnecessary comments and imports --- src/main/java/raidzero/robot/RobotContainer.java | 7 ------- .../java/raidzero/robot/subsystems/telescopingarm/Arm.java | 1 - 2 files changed, 8 deletions(-) diff --git a/src/main/java/raidzero/robot/RobotContainer.java b/src/main/java/raidzero/robot/RobotContainer.java index 4a33df8..85ccf93 100644 --- a/src/main/java/raidzero/robot/RobotContainer.java +++ b/src/main/java/raidzero/robot/RobotContainer.java @@ -195,30 +195,23 @@ private void configureBindings() { operator.button(Constants.Bindings.CLIMB_DOWN) .whileTrue(climbWinch.run(-Constants.Climb.Winch.SPEED, climbJoint.getPosition() > 0.3).onlyIf(climbJoint.isDeployed())); - // default arm if the top laser sees but botom doesn't it goes to vertical + run intake at 0.2 operator.button(Constants.Bindings.BOTTOM_RIGHT).and(operator.button(Constants.Bindings.L3).negate()) .and(operator.button(Constants.Bindings.L2).negate()).and(operator.button(Constants.Bindings.L4).negate()) - // .and(operator.button(Constants.Bindings.CORAL_EXTAKE).negate()) .whileTrue(arm.moveWithRotations(0.25, 0)); operator.button(Constants.Bindings.BOTTOM_RIGHT).and(operator.button(Constants.Bindings.L3).negate()) .and(operator.button(Constants.Bindings.L2).negate()) .whileTrue(coralIntake.holdAlgae()); - // bottom right + l3 = l3 algae + intake operator.button(Constants.Bindings.BOTTOM_RIGHT).and(operator.button(Constants.Bindings.L3)) .whileTrue(arm.moveTo(Positions.L3_ALGAE_POS_M).alongWith(coralIntake.intakeAlgae())); - // bottom right + l2 = l2 algae + intake operator.button(Constants.Bindings.BOTTOM_RIGHT).and(operator.button(Constants.Bindings.L2)) .whileTrue(arm.moveTo(Positions.L2_ALGAE_POS_M).alongWith(coralIntake.intakeAlgae())); - // bottom right + l4 = barge position? (near l4) operator.button(Constants.Bindings.BOTTOM_RIGHT).and(operator.button(Constants.Bindings.L4)) .whileTrue(arm.moveTo(Positions.BARGE_SCORE_POS_M)); - // .alongWith(Commands.waitSeconds(0.7).andThen(coralIntake.extaxeAlgae()))); - // bottom right + out = out algae operator.button(Constants.Bindings.BOTTOM_RIGHT).and(operator.button(Constants.Bindings.CORAL_EXTAKE)) .whileTrue(coralIntake.extaxeAlgae()); diff --git a/src/main/java/raidzero/robot/subsystems/telescopingarm/Arm.java b/src/main/java/raidzero/robot/subsystems/telescopingarm/Arm.java index e209882..4c04844 100644 --- a/src/main/java/raidzero/robot/subsystems/telescopingarm/Arm.java +++ b/src/main/java/raidzero/robot/subsystems/telescopingarm/Arm.java @@ -18,7 +18,6 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import edu.wpi.first.wpilibj2.command.WaitCommand; import raidzero.robot.Constants; import raidzero.robot.subsystems.climb.ClimbJoint; From c49fcbe8a43e899c2b8ff6c2964a052b9e4520e2 Mon Sep 17 00:00:00 2001 From: Theo Date: Fri, 11 Apr 2025 07:54:24 +0800 Subject: [PATCH 17/47] add javadoc to methods --- .../telescopingarm/CoralIntake.java | 19 +++++++++++++++---- 1 file changed, 15 insertions(+), 4 deletions(-) diff --git a/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java b/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java index ecd89c3..d11bfd7 100644 --- a/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java +++ b/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java @@ -44,18 +44,29 @@ public Command intake() { return run(() -> roller.set(Intake.INTAKE_SPEED)).until(() -> bottomLaser.withinThreshold()); } + /** + * Intakes Algae + + * @return A {@link Command} + */ public Command intakeAlgae() { return run(() -> roller.set(Intake.INTAKE_SPEED)); } + /** + * Extakes Algae + * + * @return A {@link Command} + */ public Command extaxeAlgae() { return run(() -> roller.set(Intake.ALGAE_EJECT_SPEED)); } - public Command extaxeAlgaeSlow() { - return run(() -> roller.set(-0.2)); - } - + /** + * Holds the algae by applying a small amount of voltage + * + * @return A {@link Command} + */ public Command holdAlgae() { return run(() -> roller.set(Intake.HOLD_SPEED)); } From 5d4e900e588ca66a198987ceaaad8aa4e45f8180 Mon Sep 17 00:00:00 2001 From: Theo Date: Fri, 11 Apr 2025 07:56:09 +0800 Subject: [PATCH 18/47] remove old unused logic --- .../telescopingarm/CoralIntake.java | 27 ------------------- 1 file changed, 27 deletions(-) diff --git a/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java b/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java index d11bfd7..8386699 100644 --- a/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java +++ b/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java @@ -71,33 +71,6 @@ public Command holdAlgae() { return run(() -> roller.set(Intake.HOLD_SPEED)); } - /** - * Runs the intake at the normal speed until a current spike is detected. Used for autons. - * - * @return A {@link Command} - */ - public Command autoIntakeP1() { - return run(() -> roller.set(Intake.INTAKE_SPEED)).until(() -> currentSpike()); - } - - /** - * Runs the intake until the bottom bottom laser sees a coral. Used for autons. - * - * @return A {@link Command} - */ - public Command autoIntakeP2() { - return run(() -> roller.set(Intake.LOWER_SPEED)).until(() -> bottomLaser.withinThreshold()); - } - - /** - * Detects if the stator of the motor is above the predefined threshold - * - * @return True if the current is above the specified spike threshold - */ - private boolean currentSpike() { - return roller.getStatorCurrent().getValueAsDouble() > Intake.CURRENT_SPIKE_THRESHOLD; - } - /** * Creates a {@link Command} to stop the intake * From 26ffb9adb669243f424ca7b553193ea851b45739 Mon Sep 17 00:00:00 2001 From: Theo Date: Fri, 11 Apr 2025 08:04:06 +0800 Subject: [PATCH 19/47] increase extake 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 e3a6e0a..87ee300 100644 --- a/src/main/java/raidzero/robot/Constants.java +++ b/src/main/java/raidzero/robot/Constants.java @@ -173,7 +173,7 @@ public static class Intake { public static final double STALL_CURRENT_THRESHOLD = 20.0; public static final double CURRENT_SPIKE_THRESHOLD = 10.0; - public static final double EXTAKE_SPEED = 0.80; + public static final double EXTAKE_SPEED = 1.0; public static final double EXTAKE_TIMEOUT_S = 1.0; public static final double ALGAE_INTAKE_SPEED = 1.0; From b3aa7d298b7f780a53e3cf89831d46a5f49f78cf Mon Sep 17 00:00:00 2001 From: Theo Date: Fri, 11 Apr 2025 17:36:35 +0800 Subject: [PATCH 20/47] new intake --- .../java/raidzero/robot/RobotContainer.java | 15 +++------- .../robot/subsystems/climb/Winch.java | 2 +- .../telescopingarm/CoralIntake.java | 28 +++++++++++++++++-- 3 files changed, 30 insertions(+), 15 deletions(-) diff --git a/src/main/java/raidzero/robot/RobotContainer.java b/src/main/java/raidzero/robot/RobotContainer.java index 85ccf93..01b6c27 100644 --- a/src/main/java/raidzero/robot/RobotContainer.java +++ b/src/main/java/raidzero/robot/RobotContainer.java @@ -178,22 +178,15 @@ private void configureBindings() { .onTrue( Commands.waitSeconds(0.2) .andThen( - climbJoint.run(Constants.Climb.Joint.DEPLOYED_POS) - .until(() -> operator.button(Constants.Bindings.CLIMB_UP).getAsBoolean()) - .andThen(() -> climbJoint.stop()).alongWith( - new InstantCommand( - () -> climbJoint.setDeployedState() - ) - ) - ) + climbWinch.run(-1.0) + ).withTimeout(1.75) ); operator.button(Constants.Bindings.CLIMB_DEPLOY).onTrue(arm.climbPos()); - operator.button(Constants.Bindings.CLIMB_UP).whileTrue(climbWinch.run(Constants.Climb.Winch.SPEED, climbJoint.getPosition() > 0.3)); - operator.button(Constants.Bindings.CLIMB_UP).onTrue(climbJoint.retract()); + operator.button(Constants.Bindings.CLIMB_UP).whileTrue(climbWinch.run(Constants.Climb.Winch.SPEED)); operator.button(Constants.Bindings.CLIMB_DOWN) - .whileTrue(climbWinch.run(-Constants.Climb.Winch.SPEED, climbJoint.getPosition() > 0.3).onlyIf(climbJoint.isDeployed())); + .whileTrue(climbWinch.run(-Constants.Climb.Winch.SPEED)); operator.button(Constants.Bindings.BOTTOM_RIGHT).and(operator.button(Constants.Bindings.L3).negate()) .and(operator.button(Constants.Bindings.L2).negate()).and(operator.button(Constants.Bindings.L4).negate()) diff --git a/src/main/java/raidzero/robot/subsystems/climb/Winch.java b/src/main/java/raidzero/robot/subsystems/climb/Winch.java index a3c2826..15252b2 100644 --- a/src/main/java/raidzero/robot/subsystems/climb/Winch.java +++ b/src/main/java/raidzero/robot/subsystems/climb/Winch.java @@ -28,7 +28,7 @@ private Winch() { * @param speed The speed to run the winch at * @return A {@link Command} that runs the winch at the specified speed */ - public Command run(double speed, boolean ramp) { + public Command run(double speed) { return run(() -> winch.set(speed)); } diff --git a/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java b/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java index 8386699..c38cc64 100644 --- a/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java +++ b/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java @@ -4,6 +4,7 @@ import au.grapplerobotics.interfaces.LaserCanInterface.TimingBudget; import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.TalonFXSConfiguration; +import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.hardware.TalonFXS; import com.ctre.phoenix6.signals.NeutralModeValue; import edu.wpi.first.wpilibj2.command.Command; @@ -13,7 +14,7 @@ import raidzero.robot.Constants.TelescopingArm.Intake; public class CoralIntake extends SubsystemBase { - private TalonFXS roller; + private TalonFXS roller, follower; private LazyCan bottomLaser, topLaser; @@ -26,6 +27,10 @@ private CoralIntake() { roller = new TalonFXS(Constants.TelescopingArm.Intake.MOTOR_ID, "rio"); roller.getConfigurator().apply(rollerConfiguration()); + follower = new TalonFXS(13); + follower.getConfigurator().apply(followerConfiguration()); + follower.setControl(new Follower(Intake.MOTOR_ID, false)); + bottomLaser = new LazyCan(1).withRangingMode(RangingMode.SHORT) .withRegionOfInterest(14, 8, 4, 16).withTimingBudget(TimingBudget.TIMING_BUDGET_20MS) .withThreshold(Intake.BOTTOM_LASER_THRESHOLD_MM); @@ -55,7 +60,7 @@ public Command intakeAlgae() { /** * Extakes Algae - * + * * @return A {@link Command} */ public Command extaxeAlgae() { @@ -64,7 +69,7 @@ public Command extaxeAlgae() { /** * Holds the algae by applying a small amount of voltage - * + * * @return A {@link Command} */ public Command holdAlgae() { @@ -148,6 +153,23 @@ private TalonFXSConfiguration rollerConfiguration() { return configuration; } + private TalonFXSConfiguration followerConfiguration() { + TalonFXSConfiguration configuration = new TalonFXSConfiguration(); + + configuration.Commutation.MotorArrangement = Intake.MOTOR_ARRANGEMENT; + configuration.MotorOutput.Inverted = Intake.INVERTED_VALUE; + + configuration.CurrentLimits.StatorCurrentLimit = Intake.STATOR_CURRENT_LIMIT; + configuration.CurrentLimits.SupplyCurrentLimit = Intake.SUPPLY_CURRENT_LIMIT; + configuration.CurrentLimits.SupplyCurrentLowerTime = Intake.SUPPLY_CURRENT_LOWER_TIME; + + configuration.Slot0 = new Slot0Configs().withKP(Intake.KP).withKI(Intake.KI).withKD(Intake.KD); + + configuration.MotorOutput.NeutralMode = NeutralModeValue.Brake; + + return configuration; + } + /** * Gets the {@link CoralIntake} subsystem instance * From b60712e5ba562f83bff9dd865a5c6ecac21dd4d3 Mon Sep 17 00:00:00 2001 From: 0x5b62656e5d Date: Fri, 11 Apr 2025 22:05:17 +0800 Subject: [PATCH 21/47] Adjust autons --- src/main/deploy/pathplanner/paths/Left-2.path | 8 ++++---- src/main/deploy/pathplanner/paths/Left-3.path | 8 ++++---- src/main/deploy/pathplanner/paths/Left-4.path | 8 ++++---- src/main/deploy/pathplanner/paths/Left-5.path | 8 ++++---- src/main/deploy/pathplanner/paths/LeftBLUE-2.path | 8 ++++---- src/main/deploy/pathplanner/paths/LeftBLUE-3.path | 8 ++++---- src/main/deploy/pathplanner/paths/LeftBLUE-4.path | 8 ++++---- src/main/deploy/pathplanner/paths/LeftBLUE-5.path | 8 ++++---- src/main/deploy/pathplanner/paths/ProcessorBLUE-1.path | 8 ++++---- src/main/deploy/pathplanner/paths/ProcessorBLUE-2.path | 8 ++++---- 10 files changed, 40 insertions(+), 40 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/Left-2.path b/src/main/deploy/pathplanner/paths/Left-2.path index 9dc811b..75e4dea 100644 --- a/src/main/deploy/pathplanner/paths/Left-2.path +++ b/src/main/deploy/pathplanner/paths/Left-2.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 1.17635, - "y": 7.075 + "x": 1.126, + "y": 7.025 }, "prevControl": { - "x": 2.8865131578947363, - "y": 5.391282894736841 + "x": 2.836163157894736, + "y": 5.341282894736842 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Left-3.path b/src/main/deploy/pathplanner/paths/Left-3.path index 40beb50..c227883 100644 --- a/src/main/deploy/pathplanner/paths/Left-3.path +++ b/src/main/deploy/pathplanner/paths/Left-3.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.17635, - "y": 7.075 + "x": 1.126, + "y": 7.025 }, "prevControl": null, "nextControl": { - "x": 2.6555921052631577, - "y": 6.0359375 + "x": 2.6052421052631574, + "y": 5.9859375 }, "isLocked": false, "linkedName": "Station-Left" diff --git a/src/main/deploy/pathplanner/paths/Left-4.path b/src/main/deploy/pathplanner/paths/Left-4.path index 65dcd50..6d3a843 100644 --- a/src/main/deploy/pathplanner/paths/Left-4.path +++ b/src/main/deploy/pathplanner/paths/Left-4.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 1.17635, - "y": 7.075 + "x": 1.126, + "y": 7.025 }, "prevControl": { - "x": 2.6074835526315785, - "y": 6.0551809210526315 + "x": 2.5571335526315786, + "y": 6.005180921052632 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Left-5.path b/src/main/deploy/pathplanner/paths/Left-5.path index 50de860..af7786e 100644 --- a/src/main/deploy/pathplanner/paths/Left-5.path +++ b/src/main/deploy/pathplanner/paths/Left-5.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.17635, - "y": 7.075 + "x": 1.126, + "y": 7.025 }, "prevControl": null, "nextControl": { - "x": 2.3092105263157894, - "y": 6.15139802631579 + "x": 2.258860526315789, + "y": 6.10139802631579 }, "isLocked": false, "linkedName": "Station-Left" diff --git a/src/main/deploy/pathplanner/paths/LeftBLUE-2.path b/src/main/deploy/pathplanner/paths/LeftBLUE-2.path index 0561ec5..cfca06e 100644 --- a/src/main/deploy/pathplanner/paths/LeftBLUE-2.path +++ b/src/main/deploy/pathplanner/paths/LeftBLUE-2.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 1.17635, - "y": 7.075 + "x": 1.126, + "y": 7.025 }, "prevControl": { - "x": 2.8865131578947363, - "y": 5.391282894736841 + "x": 2.836163157894736, + "y": 5.341282894736842 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/LeftBLUE-3.path b/src/main/deploy/pathplanner/paths/LeftBLUE-3.path index 3659566..fdaee3e 100644 --- a/src/main/deploy/pathplanner/paths/LeftBLUE-3.path +++ b/src/main/deploy/pathplanner/paths/LeftBLUE-3.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.17635, - "y": 7.075 + "x": 1.126, + "y": 7.025 }, "prevControl": null, "nextControl": { - "x": 2.6555921052631577, - "y": 6.0359375 + "x": 2.6052421052631574, + "y": 5.9859375 }, "isLocked": false, "linkedName": "Station-Left" diff --git a/src/main/deploy/pathplanner/paths/LeftBLUE-4.path b/src/main/deploy/pathplanner/paths/LeftBLUE-4.path index 0399713..615697d 100644 --- a/src/main/deploy/pathplanner/paths/LeftBLUE-4.path +++ b/src/main/deploy/pathplanner/paths/LeftBLUE-4.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 1.17635, - "y": 7.075 + "x": 1.126, + "y": 7.025 }, "prevControl": { - "x": 2.6074835526315785, - "y": 6.0551809210526315 + "x": 2.5571335526315786, + "y": 6.005180921052632 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/LeftBLUE-5.path b/src/main/deploy/pathplanner/paths/LeftBLUE-5.path index c675eac..941f9e2 100644 --- a/src/main/deploy/pathplanner/paths/LeftBLUE-5.path +++ b/src/main/deploy/pathplanner/paths/LeftBLUE-5.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.17635, - "y": 7.075 + "x": 1.126, + "y": 7.025 }, "prevControl": null, "nextControl": { - "x": 2.3092105263157894, - "y": 6.15139802631579 + "x": 2.258860526315789, + "y": 6.10139802631579 }, "isLocked": false, "linkedName": "Station-Left" diff --git a/src/main/deploy/pathplanner/paths/ProcessorBLUE-1.path b/src/main/deploy/pathplanner/paths/ProcessorBLUE-1.path index 35b3337..35498c1 100644 --- a/src/main/deploy/pathplanner/paths/ProcessorBLUE-1.path +++ b/src/main/deploy/pathplanner/paths/ProcessorBLUE-1.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 4.992, - "y": 2.88 + "x": 4.99, + "y": 2.883 }, "prevControl": { - "x": 5.493026965738442, - "y": 2.800079637703237 + "x": 5.49102706845917, + "y": 2.803080281670129 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/ProcessorBLUE-2.path b/src/main/deploy/pathplanner/paths/ProcessorBLUE-2.path index dcceefd..7681d49 100644 --- a/src/main/deploy/pathplanner/paths/ProcessorBLUE-2.path +++ b/src/main/deploy/pathplanner/paths/ProcessorBLUE-2.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 4.9918, - "y": 2.88 + "x": 5.01, + "y": 2.9 }, "prevControl": null, "nextControl": { - "x": 4.51986213887891, - "y": 1.6572191695918486 + "x": 4.538062138878912, + "y": 1.6772191695918486 }, "isLocked": true, "linkedName": null From fdd04b0b7a9cfa6f8ebe92b63b5e066b6eedffec Mon Sep 17 00:00:00 2001 From: 0x5b62656e5d Date: Fri, 11 Apr 2025 22:06:08 +0800 Subject: [PATCH 22/47] Change OTF positions --- src/main/java/raidzero/robot/Constants.java | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/raidzero/robot/Constants.java b/src/main/java/raidzero/robot/Constants.java index 87ee300..6d58dc9 100644 --- a/src/main/java/raidzero/robot/Constants.java +++ b/src/main/java/raidzero/robot/Constants.java @@ -129,9 +129,9 @@ public static enum REEFS { public static final List LEFT_REEF_WAYPOINTS = new ArrayList( List.of( - new Pose2d(3.70, 3.16, Rotation2d.fromDegrees(60)), // 17 Left + new Pose2d(3.735, 3.14, Rotation2d.fromDegrees(60)), // 17 Left new Pose2d(3.30, 4.15, Rotation2d.fromDegrees(0)), // 18 Left - new Pose2d(4.05, 5.1, Rotation2d.fromDegrees(300)), // 19 Left + new Pose2d(4.06, 5.105, Rotation2d.fromDegrees(300)), // 19 Left new Pose2d(5.2619, 4.99953, Rotation2d.fromDegrees(240)), // 20 Left new Pose2d(5.70, 3.85, Rotation2d.fromDegrees(180)), // 21 Left new Pose2d(4.9113, 2.93927, Rotation2d.fromDegrees(120)) // 22 Left @@ -142,8 +142,8 @@ public static enum REEFS { List.of( new Pose2d(4.05, 2.95, Rotation2d.fromDegrees(60)), // 17 Right new Pose2d(3.30, 3.85, Rotation2d.fromDegrees(0)), // 18 Right - new Pose2d(3.70, 4.89, Rotation2d.fromDegrees(300)), // 19 Right - new Pose2d(4.9419, 5.16453, Rotation2d.fromDegrees(240)), // 20 Right + new Pose2d(3.713, 4.925, Rotation2d.fromDegrees(300)), // 19 Right + new Pose2d(4.9489, 5.16, Rotation2d.fromDegrees(240)), // 20 Right new Pose2d(5.70, 4.20, Rotation2d.fromDegrees(180)), // 21 Right new Pose2d(5.2619, 3.05047, Rotation2d.fromDegrees(120)) // 22 Right ) From 1358412e131e38948138d8b5b1002d969d529837 Mon Sep 17 00:00:00 2001 From: 0x5b62656e5d Date: Fri, 11 Apr 2025 22:06:16 +0800 Subject: [PATCH 23/47] Adjust L2 scoring position --- 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 6d58dc9..296eb3f 100644 --- a/src/main/java/raidzero/robot/Constants.java +++ b/src/main/java/raidzero/robot/Constants.java @@ -223,7 +223,7 @@ public static class Positions { public static final double[] L4_GRAND_SLAM = { -0.2, 1.57 }; public static final double[] L3_SCORING_POS_M = { -0.20, 1.57 }; - public static final double[] L2_SCORING_POS_M = { -0.2, 0.9 }; + public static final double[] L2_SCORING_POS_M = { -0.15, 0.9 }; public static final double[] L1_SCORING_POS_M = { 0.0, 0.0 }; public static final double[] INTAKE_POS_M = { 0.5, 0.835 }; From 144f41bd776db399a04963811909110fad80ac4b Mon Sep 17 00:00:00 2001 From: 0x5b62656e5d Date: Fri, 11 Apr 2025 22:06:26 +0800 Subject: [PATCH 24/47] Reduce intake top lasercan threshold --- 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 296eb3f..bb5c76b 100644 --- a/src/main/java/raidzero/robot/Constants.java +++ b/src/main/java/raidzero/robot/Constants.java @@ -162,7 +162,7 @@ public static class Intake { public static final int SUPPLY_CURRENT_LIMIT = 50; public static final double SUPPLY_CURRENT_LOWER_TIME = 0.0; - public static final double TOP_LASER_THRESHOLD_MM = 100.0; + public static final double TOP_LASER_THRESHOLD_MM = 50.0; public static final double BOTTOM_LASER_THRESHOLD_MM = 100.0; public static final double INTAKE_SPEED = 0.85; From f676ddf0f6beae2022c584f12fd9b62e9a46a832 Mon Sep 17 00:00:00 2001 From: 0x5b62656e5d Date: Fri, 11 Apr 2025 22:08:04 +0800 Subject: [PATCH 25/47] Update arm/intake coast modes during disabled --- src/main/java/raidzero/robot/Robot.java | 2 ++ .../robot/subsystems/LEDStrip/ArmStrip.java | 4 ++-- .../robot/subsystems/telescopingarm/Arm.java | 2 +- .../subsystems/telescopingarm/CoralIntake.java | 14 ++++++++++++++ 4 files changed, 19 insertions(+), 3 deletions(-) diff --git a/src/main/java/raidzero/robot/Robot.java b/src/main/java/raidzero/robot/Robot.java index 77bfa72..dfa2470 100644 --- a/src/main/java/raidzero/robot/Robot.java +++ b/src/main/java/raidzero/robot/Robot.java @@ -16,6 +16,7 @@ import raidzero.robot.subsystems.LEDStrip.ArmStrip; import raidzero.robot.subsystems.drivetrain.Swerve; import raidzero.robot.subsystems.telescopingarm.Arm; +import raidzero.robot.subsystems.telescopingarm.CoralIntake; public class Robot extends TimedRobot { private Command m_autonomousCommand; @@ -43,6 +44,7 @@ public void disabledInit() {} @Override public void disabledPeriodic() { Arm.system().updateCoastMode(); + CoralIntake.system().updateCoastMode(); Swerve.system().initializeOtf(); } diff --git a/src/main/java/raidzero/robot/subsystems/LEDStrip/ArmStrip.java b/src/main/java/raidzero/robot/subsystems/LEDStrip/ArmStrip.java index fca434b..dc314d8 100644 --- a/src/main/java/raidzero/robot/subsystems/LEDStrip/ArmStrip.java +++ b/src/main/java/raidzero/robot/subsystems/LEDStrip/ArmStrip.java @@ -135,7 +135,7 @@ private void loopDisabled() { animation2Applied = false; animation3Applied = true; } - } else if (armIsLegal && ClimbJoint.system().getPosition() < 0.1 && !ClimbJoint.system().isDeployed().getAsBoolean()) { + } else if (armIsLegal && CoralIntake.system().getTopLaserDistance() < 10 && !ClimbJoint.system().isDeployed().getAsBoolean()) { if (!animation2Applied) { candle.clearAnimation(0); candle.clearAnimation(1); @@ -144,7 +144,7 @@ private void loopDisabled() { animationApplied = false; animation3Applied = false; } - } else if (!armIsLegal && ClimbJoint.system().getPosition() < 0.1 && !ClimbJoint.system().isDeployed().getAsBoolean()) { + } else if (!armIsLegal && CoralIntake.system().getTopLaserDistance() < 10 && !ClimbJoint.system().isDeployed().getAsBoolean()) { if (animationApplied || animation2Applied || animation3Applied) { candle.clearAnimation(0); candle.clearAnimation(1); diff --git a/src/main/java/raidzero/robot/subsystems/telescopingarm/Arm.java b/src/main/java/raidzero/robot/subsystems/telescopingarm/Arm.java index 4c04844..f5dd622 100644 --- a/src/main/java/raidzero/robot/subsystems/telescopingarm/Arm.java +++ b/src/main/java/raidzero/robot/subsystems/telescopingarm/Arm.java @@ -231,7 +231,7 @@ public void updateCoastMode() { * @return True if the arm joint should be in coast mode, false otherwise */ private boolean shouldBeInCoast() { - return (ClimbJoint.system().getPosition() < Constants.CANdle.CLIMB_JOINT_THRESHOLD); + return CoralIntake.system().getTopLaserDistance() < 10; } /** diff --git a/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java b/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java index c38cc64..5d36713 100644 --- a/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java +++ b/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java @@ -105,6 +105,20 @@ public Command run(double speed) { return run(() -> roller.set(speed)); } + public void updateCoastMode() { + if (shouldBeInCoast()) { + roller.setNeutralMode(NeutralModeValue.Coast); + follower.setNeutralMode(NeutralModeValue.Coast); + } else { + roller.setNeutralMode(NeutralModeValue.Brake); + follower.setNeutralMode(NeutralModeValue.Brake); + } + } + + private boolean shouldBeInCoast() { + return getTopLaserDistance() < 10; + } + /** * Gets the distance from the LaserCAN * From 9261f3ae7a2aeb10ccf20a58b7b6e1b6726f49d7 Mon Sep 17 00:00:00 2001 From: 0x5b62656e5d Date: Fri, 11 Apr 2025 22:08:17 +0800 Subject: [PATCH 26/47] Add intake commands for auton --- .../robot/subsystems/telescopingarm/CoralIntake.java | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java b/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java index 5d36713..e5d5b5b 100644 --- a/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java +++ b/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java @@ -49,6 +49,14 @@ public Command intake() { return run(() -> roller.set(Intake.INTAKE_SPEED)).until(() -> bottomLaser.withinThreshold()); } + public Command autoIntakeP1() { + return run(() -> roller.set(Intake.INTAKE_SPEED)).until(() -> topLaser.withinThreshold()); + } + + public Command autoIntakeP2() { + return run(() -> roller.set(Intake.INTAKE_SPEED)).until(() -> bottomLaser.withinThreshold()); + } + /** * Intakes Algae From 4a1739c1f3562249b298a49e6753fcafcaff4b92 Mon Sep 17 00:00:00 2001 From: 0x5b62656e5d Date: Fri, 11 Apr 2025 22:08:31 +0800 Subject: [PATCH 27/47] Update operator keybinds --- src/main/java/raidzero/robot/RobotContainer.java | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/main/java/raidzero/robot/RobotContainer.java b/src/main/java/raidzero/robot/RobotContainer.java index 01b6c27..a4a1542 100644 --- a/src/main/java/raidzero/robot/RobotContainer.java +++ b/src/main/java/raidzero/robot/RobotContainer.java @@ -172,7 +172,8 @@ private void configureBindings() { operator.button(Constants.Bindings.CORAL_EXTAKE).and(operator.button(Constants.Bindings.BOTTOM_RIGHT).negate()) .whileTrue(coralIntake.extake()); operator.button(Constants.Bindings.CORAL_INTAKE).onTrue(coralIntake.intake()); - operator.button(Constants.Bindings.CORAL_SCOOCH).whileTrue(coralIntake.run(Constants.TelescopingArm.Intake.REVERSE_SPEED)); + operator.button(Constants.Bindings.CORAL_SCOOCH).and(operator.button(Constants.Bindings.BOTTOM_RIGHT).negate()) + .whileTrue(coralIntake.run(Constants.TelescopingArm.Intake.REVERSE_SPEED)); operator.button(Constants.Bindings.CLIMB_DEPLOY) .onTrue( @@ -208,6 +209,9 @@ private void configureBindings() { operator.button(Constants.Bindings.BOTTOM_RIGHT).and(operator.button(Constants.Bindings.CORAL_EXTAKE)) .whileTrue(coralIntake.extaxeAlgae()); + operator.button(Constants.Bindings.BOTTOM_RIGHT).and(operator.button(Constants.Bindings.CORAL_SCOOCH)) + .whileTrue(coralIntake.run(-0.2)); + swerve.registerTelemetry(logger::telemeterize); } From 0840c489af87ebe1539b3f402e54cc440bc9be85 Mon Sep 17 00:00:00 2001 From: 0x5b62656e5d Date: Fri, 11 Apr 2025 22:08:44 +0800 Subject: [PATCH 28/47] Update auton extake coral command --- src/main/java/raidzero/robot/RobotContainer.java | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/src/main/java/raidzero/robot/RobotContainer.java b/src/main/java/raidzero/robot/RobotContainer.java index a4a1542..b08c16e 100644 --- a/src/main/java/raidzero/robot/RobotContainer.java +++ b/src/main/java/raidzero/robot/RobotContainer.java @@ -14,6 +14,7 @@ import com.pathplanner.lib.auto.NamedCommands; import com.pathplanner.lib.commands.PathfindingCommand; import edu.wpi.first.math.filter.SlewRateLimiter; +import edu.wpi.first.wpilibj.DriverStation; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; @@ -261,12 +262,14 @@ private void registerPathplannerCommands() { NamedCommands.registerCommand( "ExtakeCoral", - coralIntake.run(0.1).until( - () -> { - return coralIntake.getBottomLaserDistance() >= Constants.TelescopingArm.Intake.TOP_LASER_THRESHOLD_MM && - coralIntake.getTopLaserDistance() >= Constants.TelescopingArm.Intake.TOP_LASER_THRESHOLD_MM; - } - ).withTimeout(1.0).andThen(() -> coralIntake.stop()) + coralIntake.run(Constants.TelescopingArm.Intake.EXTAKE_SPEED) + .onlyIf(() -> DriverStation.isAutonomousEnabled() && DriverStation.getMatchTime() >= 0.75) + .until( + () -> { + return coralIntake.getBottomLaserDistance() >= Constants.TelescopingArm.Intake.TOP_LASER_THRESHOLD_MM && + coralIntake.getTopLaserDistance() >= Constants.TelescopingArm.Intake.TOP_LASER_THRESHOLD_MM; + } + ).andThen(() -> coralIntake.stop()) ); NamedCommands.registerCommand("IntakeCoral", coralIntake.intake().andThen(coralIntake.stop())); } From bdd4ff26d256e9ecf0a5f7f224318c8bc7dc1bce Mon Sep 17 00:00:00 2001 From: 0x5b62656e5d Date: Fri, 11 Apr 2025 22:28:01 +0800 Subject: [PATCH 29/47] Update javadocs --- .../robot/subsystems/climb/ClimbJoint.java | 3 +- .../robot/subsystems/telescopingarm/Arm.java | 11 +++- .../telescopingarm/CoralIntake.java | 63 +++++++++++++++---- 3 files changed, 62 insertions(+), 15 deletions(-) diff --git a/src/main/java/raidzero/robot/subsystems/climb/ClimbJoint.java b/src/main/java/raidzero/robot/subsystems/climb/ClimbJoint.java index 29ccf07..dafab7e 100644 --- a/src/main/java/raidzero/robot/subsystems/climb/ClimbJoint.java +++ b/src/main/java/raidzero/robot/subsystems/climb/ClimbJoint.java @@ -39,6 +39,7 @@ public Command run(double setpoint) { /** * Retracts the joint and stops the motor once vertical + * * @return A {@link Command} that retracts the joint */ public Command retract() { @@ -88,7 +89,7 @@ public double getVelocity() { } /** - * Returns a {@link BooleanSupplier} that checks if the joint is deployed + * Checks if the joint is deployed * * @return A {@link BooleanSupplier} that checks if the joint is deployed */ diff --git a/src/main/java/raidzero/robot/subsystems/telescopingarm/Arm.java b/src/main/java/raidzero/robot/subsystems/telescopingarm/Arm.java index f5dd622..c1f9c1a 100644 --- a/src/main/java/raidzero/robot/subsystems/telescopingarm/Arm.java +++ b/src/main/java/raidzero/robot/subsystems/telescopingarm/Arm.java @@ -77,6 +77,13 @@ public Command moveTo(double[] desiredPosition) { } } + /** + * Moves the arm to the desired joint and telescope setpoints + * + * @param jointSetpoint The desired joint setpoint in rotations + * @param telescopeSetpoint The desired telescope setpoint in percentage of full range of motion + * @return A {@link Command} that moves the arm to the desired setpoints + */ public Command moveWithRotations(double jointSetpoint, double telescopeSetpoint) { return run( () -> moveJoint(jointSetpoint) @@ -123,9 +130,9 @@ public Command moveWithDelay(double[] desiredPosition) { } /** - * Decreases the intake Y offset by a desired amount + * Adjusts the intake Y offset by a desired amount * - * @param ammount The desired offset amount + * @param amount The desired offset amount */ public void decreaseIntakeYOffset(double ammount) { intakePosYOffset += ammount; diff --git a/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java b/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java index e5d5b5b..8665b08 100644 --- a/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java +++ b/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java @@ -41,35 +41,49 @@ private CoralIntake() { } /** - * Intakes coral + * Intakes a coral * - * @return A {@link Command} + * @return A {@link Command} that intakes a coral */ public Command intake() { return run(() -> roller.set(Intake.INTAKE_SPEED)).until(() -> bottomLaser.withinThreshold()); } + /** + * Intakes a coral until the top laser is triggered + * + *

Note: This method should only be used during the autonomous period.

+ * + * @return A {@link Command} that intakes a coral until the top laser is triggered + */ public Command autoIntakeP1() { return run(() -> roller.set(Intake.INTAKE_SPEED)).until(() -> topLaser.withinThreshold()); } + /** + * Intakes a coral until the bottom laser is triggered + * + *

Note: This method should only be used during the autonomous period.

+ * + * @return A {@link Command} that intakes a coral until the bottom laser is triggered + */ public Command autoIntakeP2() { return run(() -> roller.set(Intake.INTAKE_SPEED)).until(() -> bottomLaser.withinThreshold()); } /** - * Intakes Algae + * Intakes an lgae - * @return A {@link Command} + * @return A {@link Command} that intakes an algae */ public Command intakeAlgae() { return run(() -> roller.set(Intake.INTAKE_SPEED)); } /** - * Extakes Algae + * Extakes an algae * - * @return A {@link Command} + * @return A {@link Command} that extakes an algae */ public Command extaxeAlgae() { return run(() -> roller.set(Intake.ALGAE_EJECT_SPEED)); @@ -78,25 +92,25 @@ public Command extaxeAlgae() { /** * Holds the algae by applying a small amount of voltage * - * @return A {@link Command} + * @return A {@link Command} that holds the algae */ public Command holdAlgae() { return run(() -> roller.set(Intake.HOLD_SPEED)); } /** - * Creates a {@link Command} to stop the intake + * Stops the intake * - * @return A {@link Command} to stop the intake + * @return A {@link Command} that stops the intake */ public Command stop() { return runOnce(() -> roller.stopMotor()); } /** - * Creates a {@link Command} to extake the coral + * Extakes a coral * - * @return A {@link Command} to extake at the specified speed + * @return A {@link Command} that extakes a coral */ public Command extake() { return run(() -> roller.set(Constants.TelescopingArm.Intake.EXTAKE_SPEED)) @@ -104,7 +118,7 @@ public Command extake() { } /** - * Creates a {@link Command} to run the roller at the specified speed + * Runs the roller at the specified speed * * @param speed The speed to run the roller at [-1, 1] * @return A {@link Command} to run the roller at the specified speed @@ -113,6 +127,11 @@ public Command run(double speed) { return run(() -> roller.set(speed)); } + /** + * Updates the coast mode of the roller and follower motors + * + *

Note: This should only be called during disabled.

+ */ public void updateCoastMode() { if (shouldBeInCoast()) { roller.setNeutralMode(NeutralModeValue.Coast); @@ -123,6 +142,11 @@ public void updateCoastMode() { } } + /** + * Checks if the roller should be in coast mode + * + * @return True if the roller should be in coast mode, false otherwise + */ private boolean shouldBeInCoast() { return getTopLaserDistance() < 10; } @@ -136,10 +160,20 @@ public int getTopLaserDistance() { return topLaser.getDistanceMm(); } + /** + * Checks if the top laser is within the threshold + * + * @return True if the top laser is within the threshold, false otherwise + */ public boolean topLaserWithinThreshold() { return topLaser.withinThreshold(); } + /** + * Checks if the bottom laser is within the threshold + * + * @return True if the bottom laser is within the threshold, false otherwise + */ public boolean bottomLaserWithinThreshold() { return bottomLaser.withinThreshold(); } @@ -175,6 +209,11 @@ private TalonFXSConfiguration rollerConfiguration() { return configuration; } + /** + * Gets the {@link TalonFXSConfiguration} for the follower motor + * + * @return The {@link TalonFXSConfiguration} for the follower motor + */ private TalonFXSConfiguration followerConfiguration() { TalonFXSConfiguration configuration = new TalonFXSConfiguration(); From adfc73b96c32fb2a4494341138e836106b434463 Mon Sep 17 00:00:00 2001 From: 0x5b62656e5d Date: Fri, 11 Apr 2025 22:28:10 +0800 Subject: [PATCH 30/47] Fix typos --- src/main/java/raidzero/robot/RobotContainer.java | 2 +- .../java/raidzero/robot/subsystems/telescopingarm/Arm.java | 4 ++-- .../raidzero/robot/subsystems/telescopingarm/CoralIntake.java | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/java/raidzero/robot/RobotContainer.java b/src/main/java/raidzero/robot/RobotContainer.java index b08c16e..d2072fc 100644 --- a/src/main/java/raidzero/robot/RobotContainer.java +++ b/src/main/java/raidzero/robot/RobotContainer.java @@ -208,7 +208,7 @@ private void configureBindings() { .whileTrue(arm.moveTo(Positions.BARGE_SCORE_POS_M)); operator.button(Constants.Bindings.BOTTOM_RIGHT).and(operator.button(Constants.Bindings.CORAL_EXTAKE)) - .whileTrue(coralIntake.extaxeAlgae()); + .whileTrue(coralIntake.extakeAlgae()); operator.button(Constants.Bindings.BOTTOM_RIGHT).and(operator.button(Constants.Bindings.CORAL_SCOOCH)) .whileTrue(coralIntake.run(-0.2)); diff --git a/src/main/java/raidzero/robot/subsystems/telescopingarm/Arm.java b/src/main/java/raidzero/robot/subsystems/telescopingarm/Arm.java index c1f9c1a..b03030b 100644 --- a/src/main/java/raidzero/robot/subsystems/telescopingarm/Arm.java +++ b/src/main/java/raidzero/robot/subsystems/telescopingarm/Arm.java @@ -134,8 +134,8 @@ public Command moveWithDelay(double[] desiredPosition) { * * @param amount The desired offset amount */ - public void decreaseIntakeYOffset(double ammount) { - intakePosYOffset += ammount; + public void decreaseIntakeYOffset(double amount) { + intakePosYOffset += amount; } /** diff --git a/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java b/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java index 8665b08..978297e 100644 --- a/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java +++ b/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java @@ -85,7 +85,7 @@ public Command intakeAlgae() { * * @return A {@link Command} that extakes an algae */ - public Command extaxeAlgae() { + public Command extakeAlgae() { return run(() -> roller.set(Intake.ALGAE_EJECT_SPEED)); } From 27e7bfb69eb2e6f1168cee1fd7de4519bd6f94aa Mon Sep 17 00:00:00 2001 From: 0x5b62656e5d Date: Fri, 11 Apr 2025 22:28:51 +0800 Subject: [PATCH 31/47] Format code --- .../raidzero/robot/subsystems/climb/ClimbJoint.java | 2 +- .../raidzero/robot/subsystems/telescopingarm/Arm.java | 2 +- .../robot/subsystems/telescopingarm/CoralIntake.java | 10 +++++----- 3 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/main/java/raidzero/robot/subsystems/climb/ClimbJoint.java b/src/main/java/raidzero/robot/subsystems/climb/ClimbJoint.java index dafab7e..4048e37 100644 --- a/src/main/java/raidzero/robot/subsystems/climb/ClimbJoint.java +++ b/src/main/java/raidzero/robot/subsystems/climb/ClimbJoint.java @@ -39,7 +39,7 @@ public Command run(double setpoint) { /** * Retracts the joint and stops the motor once vertical - * + * * @return A {@link Command} that retracts the joint */ public Command retract() { diff --git a/src/main/java/raidzero/robot/subsystems/telescopingarm/Arm.java b/src/main/java/raidzero/robot/subsystems/telescopingarm/Arm.java index b03030b..55e5efb 100644 --- a/src/main/java/raidzero/robot/subsystems/telescopingarm/Arm.java +++ b/src/main/java/raidzero/robot/subsystems/telescopingarm/Arm.java @@ -79,7 +79,7 @@ public Command moveTo(double[] desiredPosition) { /** * Moves the arm to the desired joint and telescope setpoints - * + * * @param jointSetpoint The desired joint setpoint in rotations * @param telescopeSetpoint The desired telescope setpoint in percentage of full range of motion * @return A {@link Command} that moves the arm to the desired setpoints diff --git a/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java b/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java index 978297e..65b4a09 100644 --- a/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java +++ b/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java @@ -53,7 +53,7 @@ public Command intake() { * Intakes a coral until the top laser is triggered * *

Note: This method should only be used during the autonomous period.

- * + * * @return A {@link Command} that intakes a coral until the top laser is triggered */ public Command autoIntakeP1() { @@ -64,7 +64,7 @@ public Command autoIntakeP1() { * Intakes a coral until the bottom laser is triggered * *

Note: This method should only be used during the autonomous period.

- * + * * @return A {@link Command} that intakes a coral until the bottom laser is triggered */ public Command autoIntakeP2() { @@ -144,7 +144,7 @@ public void updateCoastMode() { /** * Checks if the roller should be in coast mode - * + * * @return True if the roller should be in coast mode, false otherwise */ private boolean shouldBeInCoast() { @@ -162,7 +162,7 @@ public int getTopLaserDistance() { /** * Checks if the top laser is within the threshold - * + * * @return True if the top laser is within the threshold, false otherwise */ public boolean topLaserWithinThreshold() { @@ -171,7 +171,7 @@ public boolean topLaserWithinThreshold() { /** * Checks if the bottom laser is within the threshold - * + * * @return True if the bottom laser is within the threshold, false otherwise */ public boolean bottomLaserWithinThreshold() { From 6ee8bfaf42b08c3312006275a5d6c15b2b83851b Mon Sep 17 00:00:00 2001 From: 0x5b62656e5d Date: Fri, 11 Apr 2025 22:29:52 +0800 Subject: [PATCH 32/47] Use constants instead of literal --- .../raidzero/robot/subsystems/telescopingarm/CoralIntake.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java b/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java index 65b4a09..2052892 100644 --- a/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java +++ b/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java @@ -24,7 +24,7 @@ public class CoralIntake extends SubsystemBase { * Constructs a {@link CoralIntake} subsystem instance */ private CoralIntake() { - roller = new TalonFXS(Constants.TelescopingArm.Intake.MOTOR_ID, "rio"); + roller = new TalonFXS(Constants.TelescopingArm.Intake.MOTOR_ID, Constants.RIO_BUS); roller.getConfigurator().apply(rollerConfiguration()); follower = new TalonFXS(13); @@ -73,7 +73,7 @@ public Command autoIntakeP2() { /** * Intakes an lgae - + * @return A {@link Command} that intakes an algae */ public Command intakeAlgae() { From 7752d3d88339d228ac9d8d860be0d9401ed5f57b Mon Sep 17 00:00:00 2001 From: 0x5b62656e5d Date: Sat, 12 Apr 2025 09:18:58 +0800 Subject: [PATCH 33/47] Remove LED animation for coral too low --- .../raidzero/robot/subsystems/LEDStrip/ArmStrip.java | 10 ---------- 1 file changed, 10 deletions(-) diff --git a/src/main/java/raidzero/robot/subsystems/LEDStrip/ArmStrip.java b/src/main/java/raidzero/robot/subsystems/LEDStrip/ArmStrip.java index dc314d8..385739d 100644 --- a/src/main/java/raidzero/robot/subsystems/LEDStrip/ArmStrip.java +++ b/src/main/java/raidzero/robot/subsystems/LEDStrip/ArmStrip.java @@ -216,16 +216,6 @@ private void loopAutonomous() { private void loopTeleop() { if (ClimbJoint.system().isDeployed().getAsBoolean()) { candle.animate(new StrobeAnimation(0, 0, 255, 0, 0.05, -1)); - } else if (coralTooDown) { - if (!animation2Applied) { - candle.clearAnimation(0); - candle.clearAnimation(1); - candle.animate(new ColorFlowAnimation(250, 160, 10, 0, 0.75, 25, Direction.Backward, 8), 0); - candle.animate(new ColorFlowAnimation(250, 160, 10, 0, 0.75, 27, Direction.Forward, 33), 1); - animationApplied = false; - animation2Applied = true; - animation3Applied = false; - } } else if (coralTooUp) { if (!animation2Applied) { candle.clearAnimation(0); From 798a61cf17b69b693f260e0cd27ef1987519a5e8 Mon Sep 17 00:00:00 2001 From: Theo Date: Sat, 12 Apr 2025 10:41:13 +0800 Subject: [PATCH 34/47] adjust intake holding behavior --- src/main/java/raidzero/robot/Constants.java | 6 +++--- src/main/java/raidzero/robot/RobotContainer.java | 8 ++++++-- .../robot/subsystems/telescopingarm/CoralIntake.java | 1 - 3 files changed, 9 insertions(+), 6 deletions(-) diff --git a/src/main/java/raidzero/robot/Constants.java b/src/main/java/raidzero/robot/Constants.java index 87ee300..4f27749 100644 --- a/src/main/java/raidzero/robot/Constants.java +++ b/src/main/java/raidzero/robot/Constants.java @@ -158,8 +158,8 @@ public static class Intake { public static final InvertedValue INVERTED_VALUE = InvertedValue.CounterClockwise_Positive; - public static final int STATOR_CURRENT_LIMIT = 50; - public static final int SUPPLY_CURRENT_LIMIT = 50; + public static final int STATOR_CURRENT_LIMIT = 30; + public static final int SUPPLY_CURRENT_LIMIT = 30; public static final double SUPPLY_CURRENT_LOWER_TIME = 0.0; public static final double TOP_LASER_THRESHOLD_MM = 100.0; @@ -178,7 +178,7 @@ public static class Intake { public static final double ALGAE_INTAKE_SPEED = 1.0; public static final double ALGAE_EJECT_SPEED = -1.0; - public static final double HOLD_SPEED = 0.4; + public static final double HOLD_SPEED = 0.1; public static final double KP = 1.0; public static final double KI = 0.0; diff --git a/src/main/java/raidzero/robot/RobotContainer.java b/src/main/java/raidzero/robot/RobotContainer.java index 01b6c27..5ee64eb 100644 --- a/src/main/java/raidzero/robot/RobotContainer.java +++ b/src/main/java/raidzero/robot/RobotContainer.java @@ -197,10 +197,14 @@ private void configureBindings() { .whileTrue(coralIntake.holdAlgae()); operator.button(Constants.Bindings.BOTTOM_RIGHT).and(operator.button(Constants.Bindings.L3)) - .whileTrue(arm.moveTo(Positions.L3_ALGAE_POS_M).alongWith(coralIntake.intakeAlgae())); + .whileTrue( + arm.moveTo(Positions.L3_ALGAE_POS_M).alongWith(coralIntake.intakeAlgae()) + ); operator.button(Constants.Bindings.BOTTOM_RIGHT).and(operator.button(Constants.Bindings.L2)) - .whileTrue(arm.moveTo(Positions.L2_ALGAE_POS_M).alongWith(coralIntake.intakeAlgae())); + .whileTrue( + arm.moveTo(Positions.L2_ALGAE_POS_M).alongWith(coralIntake.intakeAlgae()) + ); operator.button(Constants.Bindings.BOTTOM_RIGHT).and(operator.button(Constants.Bindings.L4)) .whileTrue(arm.moveTo(Positions.BARGE_SCORE_POS_M)); diff --git a/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java b/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java index c38cc64..24d4217 100644 --- a/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java +++ b/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java @@ -15,7 +15,6 @@ public class CoralIntake extends SubsystemBase { private TalonFXS roller, follower; - private LazyCan bottomLaser, topLaser; private static CoralIntake system; From e6eccd218dae542e87d75030a9016f9ad474573a Mon Sep 17 00:00:00 2001 From: Theo Date: Sat, 12 Apr 2025 10:52:58 +0800 Subject: [PATCH 35/47] l2 descore --- 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 4f27749..cd35a08 100644 --- a/src/main/java/raidzero/robot/Constants.java +++ b/src/main/java/raidzero/robot/Constants.java @@ -230,7 +230,7 @@ public static class Positions { public static final double[] INTAKE_POS_M_BLUE = { 0.5, 0.88 }; public static final double[] L3_ALGAE_POS_M = { 0.75, 1.3 }; - public static final double[] L2_ALGAE_POS_M = { 0.5, 0.853 }; + public static final double[] L2_ALGAE_POS_M = { 0.6, 0.7 }; public static final double[] BARGE_SCORE_POS_M = { 0, 2.8 }; public static double[] HOME_POS_M = { 0.0, 0.0 }; From 3726c73884499e208dca2c84e5fb1251f3242823 Mon Sep 17 00:00:00 2001 From: 0x5b62656e5d Date: Sat, 12 Apr 2025 13:51:28 +0800 Subject: [PATCH 36/47] Increase bottom lasercan region width --- .../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 1dd981e..9344cf1 100644 --- a/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java +++ b/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java @@ -31,7 +31,7 @@ private CoralIntake() { follower.setControl(new Follower(Intake.MOTOR_ID, false)); bottomLaser = new LazyCan(1).withRangingMode(RangingMode.SHORT) - .withRegionOfInterest(14, 8, 4, 16).withTimingBudget(TimingBudget.TIMING_BUDGET_20MS) + .withRegionOfInterest(14, 8, 16, 16).withTimingBudget(TimingBudget.TIMING_BUDGET_20MS) .withThreshold(Intake.BOTTOM_LASER_THRESHOLD_MM); topLaser = new LazyCan(0).withRangingMode(RangingMode.LONG) From 796aedc583f2d25e75158881d59831dc5e0d4ddf Mon Sep 17 00:00:00 2001 From: 0x5b62656e5d Date: Sat, 12 Apr 2025 13:51:44 +0800 Subject: [PATCH 37/47] Add SmartDashboard debug values for lasercans --- .../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 9344cf1..3509137 100644 --- a/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java +++ b/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java @@ -7,6 +7,7 @@ import com.ctre.phoenix6.controls.Follower; import com.ctre.phoenix6.hardware.TalonFXS; import com.ctre.phoenix6.signals.NeutralModeValue; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import raidzero.lib.LazyCan; @@ -186,6 +187,14 @@ public int getBottomLaserDistance() { return bottomLaser.getDistanceMm(); } + @Override + public void periodic() { + SmartDashboard.putNumber("Top laser mm", getTopLaserDistance()); + SmartDashboard.putNumber("Bottom laser mm", getBottomLaserDistance()); + SmartDashboard.putBoolean("Top laser", topLaserWithinThreshold()); + SmartDashboard.putBoolean("Bottom laser", bottomLaserWithinThreshold()); + } + /** * Gets the {@link TalonFXSConfiguration} for the roller motor * From aba939a290f1c2158509afcb9c30b6b685c58325 Mon Sep 17 00:00:00 2001 From: 0x5b62656e5d Date: Sat, 12 Apr 2025 13:52:58 +0800 Subject: [PATCH 38/47] Remove arm default command Rebinds default command when pressing L2, L3, L4 --- src/main/java/raidzero/robot/RobotContainer.java | 4 +++- .../raidzero/robot/subsystems/telescopingarm/Arm.java | 10 ++++++++++ 2 files changed, 13 insertions(+), 1 deletion(-) diff --git a/src/main/java/raidzero/robot/RobotContainer.java b/src/main/java/raidzero/robot/RobotContainer.java index 191d450..156b16b 100644 --- a/src/main/java/raidzero/robot/RobotContainer.java +++ b/src/main/java/raidzero/robot/RobotContainer.java @@ -98,7 +98,6 @@ private void configureBindings() { ); // arm.setDefaultCommand(arm.moveArmWithDelay(Constants.TelescopingArm.Positions.INTAKE_POS_M)); - arm.setDefaultCommand(arm.moveToIntake()); coralIntake.setDefaultCommand(coralIntake.stop()); // algaeIntake.setDefaultCommand(algaeIntake.moveJoint(Constants.AlgaeIntake.Joint.HOME_POSITION)); @@ -159,15 +158,18 @@ private void configureBindings() { operator.button(Constants.Bindings.L2).and(operator.button(Constants.Bindings.BOTTOM_RIGHT).negate()).whileTrue( arm.moveTo(Constants.TelescopingArm.Positions.L2_SCORING_POS_M) .onlyIf(swerve.isArmDeployable()) + .alongWith(new InstantCommand(() -> arm.checkDefaultCommand())) ); operator.button(Constants.Bindings.L3).and(operator.button(Constants.Bindings.BOTTOM_RIGHT).negate()).whileTrue( arm.moveTo(Constants.TelescopingArm.Positions.L3_SCORING_POS_M) .onlyIf(swerve.isArmDeployable()) + .alongWith(new InstantCommand(() -> arm.checkDefaultCommand())) ); operator.button(Constants.Bindings.L4).and(operator.button(Constants.Bindings.BOTTOM_RIGHT).negate()).whileTrue( arm.moveToL4() .onlyIf(swerve.isArmDeployable()) + .alongWith(new InstantCommand(() -> arm.checkDefaultCommand())) ); operator.button(Constants.Bindings.CORAL_EXTAKE).and(operator.button(Constants.Bindings.BOTTOM_RIGHT).negate()) diff --git a/src/main/java/raidzero/robot/subsystems/telescopingarm/Arm.java b/src/main/java/raidzero/robot/subsystems/telescopingarm/Arm.java index 55e5efb..a252e05 100644 --- a/src/main/java/raidzero/robot/subsystems/telescopingarm/Arm.java +++ b/src/main/java/raidzero/robot/subsystems/telescopingarm/Arm.java @@ -197,6 +197,15 @@ public Command climbPos() { }); } + /** + * Checks if a default command is present. If it isn't present, it will assign {@link Arm#moveToIntake()} as the default command. + */ + public void checkDefaultCommand() { + if (this.getDefaultCommand() == null) { + this.setDefaultCommand(this.moveToIntake()); + } + } + /** * Runs just the telescope to the supplied setpoint * @@ -325,6 +334,7 @@ public void stopAll() { public void periodic() { SmartDashboard.putNumber("Elevator pos", getTelescopePosition()); SmartDashboard.putNumber("Intake Y Offset", Math.round(intakePosYOffset * 100) / 100.0); + SmartDashboard.putBoolean("DefaultCommand", this.getDefaultCommand() != null); } /** From 8064d37dfc8f5721e5a984bf84d291f2e21ee194 Mon Sep 17 00:00:00 2001 From: 0x5b62656e5d Date: Sat, 12 Apr 2025 13:53:21 +0800 Subject: [PATCH 39/47] Update ExtakeCoral auton command --- src/main/java/raidzero/robot/RobotContainer.java | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/main/java/raidzero/robot/RobotContainer.java b/src/main/java/raidzero/robot/RobotContainer.java index 156b16b..f15fe3f 100644 --- a/src/main/java/raidzero/robot/RobotContainer.java +++ b/src/main/java/raidzero/robot/RobotContainer.java @@ -20,6 +20,7 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.WaitCommand; import edu.wpi.first.wpilibj2.command.button.CommandGenericHID; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import raidzero.robot.Constants.TelescopingArm.Positions; @@ -273,9 +274,11 @@ private void registerPathplannerCommands() { .until( () -> { return coralIntake.getBottomLaserDistance() >= Constants.TelescopingArm.Intake.TOP_LASER_THRESHOLD_MM && - coralIntake.getTopLaserDistance() >= Constants.TelescopingArm.Intake.TOP_LASER_THRESHOLD_MM; + coralIntake.getTopLaserDistance() >= Constants.TelescopingArm.Intake.BOTTOM_LASER_THRESHOLD_MM; } - ).andThen(() -> coralIntake.stop()) + ) + .andThen(new WaitCommand(0.1)) + .andThen(() -> coralIntake.stop()) ); NamedCommands.registerCommand("IntakeCoral", coralIntake.intake().andThen(coralIntake.stop())); } From a9053fff5cec21d1291d55a9ac43dd28c7af6dc6 Mon Sep 17 00:00:00 2001 From: 0x5b62656e5d Date: Sat, 12 Apr 2025 13:53:53 +0800 Subject: [PATCH 40/47] Add processor OTF --- src/main/java/raidzero/robot/Constants.java | 3 +++ .../java/raidzero/robot/RobotContainer.java | 2 +- .../robot/subsystems/drivetrain/Swerve.java | 19 +++++++++++++++++++ 3 files changed, 23 insertions(+), 1 deletion(-) diff --git a/src/main/java/raidzero/robot/Constants.java b/src/main/java/raidzero/robot/Constants.java index 7829d72..656d82a 100644 --- a/src/main/java/raidzero/robot/Constants.java +++ b/src/main/java/raidzero/robot/Constants.java @@ -148,6 +148,9 @@ public static enum REEFS { new Pose2d(5.2619, 3.05047, Rotation2d.fromDegrees(120)) // 22 Right ) ); + + public static final Pose2d BLUE_PROCESSOR = new Pose2d(5.987542, 0.78, Rotation2d.fromDegrees(90)); + public static final Pose2d RED_PROCESSOR = new Pose2d(17.55 - 5.987542, 8.05 - 0.78, Rotation2d.fromDegrees(180)); } public static class TelescopingArm { diff --git a/src/main/java/raidzero/robot/RobotContainer.java b/src/main/java/raidzero/robot/RobotContainer.java index f15fe3f..cd74e49 100644 --- a/src/main/java/raidzero/robot/RobotContainer.java +++ b/src/main/java/raidzero/robot/RobotContainer.java @@ -148,7 +148,7 @@ private void configureBindings() { ); joystick.povDown().whileTrue( - arm.moveWithDelay(Constants.TelescopingArm.Positions.INTAKE_POS_M_BLUE) + swerve.pathToProcessor() ); // * Operator controls diff --git a/src/main/java/raidzero/robot/subsystems/drivetrain/Swerve.java b/src/main/java/raidzero/robot/subsystems/drivetrain/Swerve.java index a8d1004..3e093b4 100644 --- a/src/main/java/raidzero/robot/subsystems/drivetrain/Swerve.java +++ b/src/main/java/raidzero/robot/subsystems/drivetrain/Swerve.java @@ -306,6 +306,25 @@ public Command pathToReef(Constants.Swerve.REEFS reef) { }); } + /** + * Moves the robot to the red or blue processor depending on current alliance + * + * @return A {@link DeferredCommand} that moves the robot to the red or blue processor + */ + public Command pathToProcessor() { + return defer(() -> { + Pose2d target = null; + + if (DriverStation.getAlliance().orElse(Alliance.Blue) == Alliance.Red) { + target = Constants.Swerve.RED_PROCESSOR; + } else { + target = Constants.Swerve.BLUE_PROCESSOR; + } + + return goToPose(target).withTimeout(0.01).andThen(goToPose(target)); + }); + } + /** * Moves the robot to the nearest coral station * From 928a2faeb8e7c52bddc2c6eefee45931ede030a7 Mon Sep 17 00:00:00 2001 From: 0x5b62656e5d Date: Sat, 12 Apr 2025 13:54:09 +0800 Subject: [PATCH 41/47] Decrease blue intake pos height --- 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 656d82a..45cd499 100644 --- a/src/main/java/raidzero/robot/Constants.java +++ b/src/main/java/raidzero/robot/Constants.java @@ -230,7 +230,7 @@ public static class Positions { public static final double[] L1_SCORING_POS_M = { 0.0, 0.0 }; public static final double[] INTAKE_POS_M = { 0.5, 0.835 }; - public static final double[] INTAKE_POS_M_BLUE = { 0.5, 0.88 }; + public static final double[] INTAKE_POS_M_BLUE = { 0.5, 0.875 }; public static final double[] L3_ALGAE_POS_M = { 0.75, 1.3 }; public static final double[] L2_ALGAE_POS_M = { 0.6, 0.7 }; From 2e46df060abc788e7ab38114c7aa266e249bd50a Mon Sep 17 00:00:00 2001 From: 0x5b62656e5d Date: Sat, 12 Apr 2025 13:54:32 +0800 Subject: [PATCH 42/47] Fix disabled coast mode logic --- src/main/java/raidzero/robot/Robot.java | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/src/main/java/raidzero/robot/Robot.java b/src/main/java/raidzero/robot/Robot.java index dfa2470..a31817c 100644 --- a/src/main/java/raidzero/robot/Robot.java +++ b/src/main/java/raidzero/robot/Robot.java @@ -23,12 +23,15 @@ public class Robot extends TimedRobot { private final RobotContainer m_robotContainer; + private boolean alreadyEnabled; + public Robot() { m_robotContainer = new RobotContainer(); CanBridge.runTCP(); WebServer.start(5800, Filesystem.getDeployDirectory().getPath()); Elastic.selectTab("Setup"); + alreadyEnabled = false; } @Override @@ -43,10 +46,11 @@ public void disabledInit() {} @Override public void disabledPeriodic() { - Arm.system().updateCoastMode(); - CoralIntake.system().updateCoastMode(); - - Swerve.system().initializeOtf(); + if (!alreadyEnabled) { + Arm.system().updateCoastMode(); + CoralIntake.system().updateCoastMode(); + Swerve.system().initializeOtf(); + } } @Override @@ -63,6 +67,7 @@ public void autonomousInit() { } Elastic.selectTab("Autonomous"); + alreadyEnabled = true; } @Override From 243c0c97cb4eb34f71c54e5384cf8d01ecb4ef88 Mon Sep 17 00:00:00 2001 From: 0x5b62656e5d Date: Sat, 12 Apr 2025 13:54:48 +0800 Subject: [PATCH 43/47] Refactor ArmStrip.java --- .../raidzero/robot/subsystems/LEDStrip/ArmStrip.java | 10 +++------- 1 file changed, 3 insertions(+), 7 deletions(-) diff --git a/src/main/java/raidzero/robot/subsystems/LEDStrip/ArmStrip.java b/src/main/java/raidzero/robot/subsystems/LEDStrip/ArmStrip.java index 385739d..855cb9b 100644 --- a/src/main/java/raidzero/robot/subsystems/LEDStrip/ArmStrip.java +++ b/src/main/java/raidzero/robot/subsystems/LEDStrip/ArmStrip.java @@ -22,7 +22,7 @@ public class ArmStrip implements Subsystem { private CANdle candle; private Arm arm; - private boolean armIsLegal, coralTooDown, coralTooUp, coralIsIn = false; + private boolean armIsLegal, coralTooUp, coralIsIn = false; private boolean strobeAlternate = false; private Timer strobeTimer = new Timer(); @@ -80,14 +80,10 @@ private void updateStates() { armIsLegal = arm.getJointPosition() >= Constants.CANdle.ARM_JOINT_LOWER_BOUND && arm.getJointPosition() <= Constants.CANdle.ARM_JOINT_UPPER_BOUND; - coralTooDown = CoralIntake.system().getTopLaserDistance() > Constants.TelescopingArm.Intake.TOP_LASER_THRESHOLD_MM && - CoralIntake.system().getBottomLaserDistance() < Constants.TelescopingArm.Intake.TOP_LASER_THRESHOLD_MM; - coralTooUp = CoralIntake.system().getTopLaserDistance() < Constants.TelescopingArm.Intake.TOP_LASER_THRESHOLD_MM && - CoralIntake.system().getBottomLaserDistance() > Constants.TelescopingArm.Intake.TOP_LASER_THRESHOLD_MM; + CoralIntake.system().getBottomLaserDistance() > Constants.TelescopingArm.Intake.BOTTOM_LASER_THRESHOLD_MM; - coralIsIn = CoralIntake.system().getTopLaserDistance() < Constants.TelescopingArm.Intake.TOP_LASER_THRESHOLD_MM && - CoralIntake.system().getBottomLaserDistance() < Constants.TelescopingArm.Intake.TOP_LASER_THRESHOLD_MM; + coralIsIn = CoralIntake.system().bottomLaserWithinThreshold(); } /** From d21da5aa822531c8ab76b2ca3ed7a069be1962e4 Mon Sep 17 00:00:00 2001 From: 0x5b62656e5d Date: Sat, 12 Apr 2025 13:55:06 +0800 Subject: [PATCH 44/47] Remove unnecessary code --- src/main/java/raidzero/robot/RobotContainer.java | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/main/java/raidzero/robot/RobotContainer.java b/src/main/java/raidzero/robot/RobotContainer.java index cd74e49..138126e 100644 --- a/src/main/java/raidzero/robot/RobotContainer.java +++ b/src/main/java/raidzero/robot/RobotContainer.java @@ -98,11 +98,8 @@ private void configureBindings() { ) ); - // arm.setDefaultCommand(arm.moveArmWithDelay(Constants.TelescopingArm.Positions.INTAKE_POS_M)); coralIntake.setDefaultCommand(coralIntake.stop()); - // algaeIntake.setDefaultCommand(algaeIntake.moveJoint(Constants.AlgaeIntake.Joint.HOME_POSITION)); - climbJoint.setDefaultCommand(climbJoint.run(Constants.Climb.Joint.HOME_POS)); climbWinch.setDefaultCommand(climbWinch.stop()); From dc34fc8938aa03cc3ead863ddbb3af6f85ddf281 Mon Sep 17 00:00:00 2001 From: 0x5b62656e5d Date: Sat, 12 Apr 2025 13:55:17 +0800 Subject: [PATCH 45/47] Remove extra spaces --- .../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 3509137..1234589 100644 --- a/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java +++ b/src/main/java/raidzero/robot/subsystems/telescopingarm/CoralIntake.java @@ -73,7 +73,7 @@ public Command autoIntakeP2() { /** * Intakes an lgae - + * @return A {@link Command} that intakes an algae */ public Command intakeAlgae() { From 5f02b2f7365e9ab9f8b3e4f78d356b84104d8bc9 Mon Sep 17 00:00:00 2001 From: 0x5b62656e5d Date: Sat, 12 Apr 2025 14:05:41 +0800 Subject: [PATCH 46/47] Fix climb deploy LED animation --- src/main/java/raidzero/robot/RobotContainer.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/raidzero/robot/RobotContainer.java b/src/main/java/raidzero/robot/RobotContainer.java index 138126e..7d16418 100644 --- a/src/main/java/raidzero/robot/RobotContainer.java +++ b/src/main/java/raidzero/robot/RobotContainer.java @@ -183,7 +183,8 @@ private void configureBindings() { climbWinch.run(-1.0) ).withTimeout(1.75) ); - operator.button(Constants.Bindings.CLIMB_DEPLOY).onTrue(arm.climbPos()); + operator.button(Constants.Bindings.CLIMB_DEPLOY) + .onTrue(arm.climbPos().alongWith(new InstantCommand(() -> climbJoint.setDeployedState()))); operator.button(Constants.Bindings.CLIMB_UP).whileTrue(climbWinch.run(Constants.Climb.Winch.SPEED)); From 4c6602be41df696b0b9a6adeb32dbf2348d92362 Mon Sep 17 00:00:00 2001 From: 0x5b62656e5d Date: Sat, 12 Apr 2025 14:06:54 +0800 Subject: [PATCH 47/47] Revert "Adjust autons" This reverts commit b60712e5ba562f83bff9dd865a5c6ecac21dd4d3. --- src/main/deploy/pathplanner/paths/Left-2.path | 8 ++++---- src/main/deploy/pathplanner/paths/Left-3.path | 8 ++++---- src/main/deploy/pathplanner/paths/Left-4.path | 8 ++++---- src/main/deploy/pathplanner/paths/Left-5.path | 8 ++++---- src/main/deploy/pathplanner/paths/LeftBLUE-2.path | 8 ++++---- src/main/deploy/pathplanner/paths/LeftBLUE-3.path | 8 ++++---- src/main/deploy/pathplanner/paths/LeftBLUE-4.path | 8 ++++---- src/main/deploy/pathplanner/paths/LeftBLUE-5.path | 8 ++++---- src/main/deploy/pathplanner/paths/ProcessorBLUE-1.path | 8 ++++---- src/main/deploy/pathplanner/paths/ProcessorBLUE-2.path | 8 ++++---- 10 files changed, 40 insertions(+), 40 deletions(-) diff --git a/src/main/deploy/pathplanner/paths/Left-2.path b/src/main/deploy/pathplanner/paths/Left-2.path index 75e4dea..9dc811b 100644 --- a/src/main/deploy/pathplanner/paths/Left-2.path +++ b/src/main/deploy/pathplanner/paths/Left-2.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 1.126, - "y": 7.025 + "x": 1.17635, + "y": 7.075 }, "prevControl": { - "x": 2.836163157894736, - "y": 5.341282894736842 + "x": 2.8865131578947363, + "y": 5.391282894736841 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Left-3.path b/src/main/deploy/pathplanner/paths/Left-3.path index c227883..40beb50 100644 --- a/src/main/deploy/pathplanner/paths/Left-3.path +++ b/src/main/deploy/pathplanner/paths/Left-3.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.126, - "y": 7.025 + "x": 1.17635, + "y": 7.075 }, "prevControl": null, "nextControl": { - "x": 2.6052421052631574, - "y": 5.9859375 + "x": 2.6555921052631577, + "y": 6.0359375 }, "isLocked": false, "linkedName": "Station-Left" diff --git a/src/main/deploy/pathplanner/paths/Left-4.path b/src/main/deploy/pathplanner/paths/Left-4.path index 6d3a843..65dcd50 100644 --- a/src/main/deploy/pathplanner/paths/Left-4.path +++ b/src/main/deploy/pathplanner/paths/Left-4.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 1.126, - "y": 7.025 + "x": 1.17635, + "y": 7.075 }, "prevControl": { - "x": 2.5571335526315786, - "y": 6.005180921052632 + "x": 2.6074835526315785, + "y": 6.0551809210526315 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/Left-5.path b/src/main/deploy/pathplanner/paths/Left-5.path index af7786e..50de860 100644 --- a/src/main/deploy/pathplanner/paths/Left-5.path +++ b/src/main/deploy/pathplanner/paths/Left-5.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.126, - "y": 7.025 + "x": 1.17635, + "y": 7.075 }, "prevControl": null, "nextControl": { - "x": 2.258860526315789, - "y": 6.10139802631579 + "x": 2.3092105263157894, + "y": 6.15139802631579 }, "isLocked": false, "linkedName": "Station-Left" diff --git a/src/main/deploy/pathplanner/paths/LeftBLUE-2.path b/src/main/deploy/pathplanner/paths/LeftBLUE-2.path index cfca06e..0561ec5 100644 --- a/src/main/deploy/pathplanner/paths/LeftBLUE-2.path +++ b/src/main/deploy/pathplanner/paths/LeftBLUE-2.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 1.126, - "y": 7.025 + "x": 1.17635, + "y": 7.075 }, "prevControl": { - "x": 2.836163157894736, - "y": 5.341282894736842 + "x": 2.8865131578947363, + "y": 5.391282894736841 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/LeftBLUE-3.path b/src/main/deploy/pathplanner/paths/LeftBLUE-3.path index fdaee3e..3659566 100644 --- a/src/main/deploy/pathplanner/paths/LeftBLUE-3.path +++ b/src/main/deploy/pathplanner/paths/LeftBLUE-3.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.126, - "y": 7.025 + "x": 1.17635, + "y": 7.075 }, "prevControl": null, "nextControl": { - "x": 2.6052421052631574, - "y": 5.9859375 + "x": 2.6555921052631577, + "y": 6.0359375 }, "isLocked": false, "linkedName": "Station-Left" diff --git a/src/main/deploy/pathplanner/paths/LeftBLUE-4.path b/src/main/deploy/pathplanner/paths/LeftBLUE-4.path index 615697d..0399713 100644 --- a/src/main/deploy/pathplanner/paths/LeftBLUE-4.path +++ b/src/main/deploy/pathplanner/paths/LeftBLUE-4.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 1.126, - "y": 7.025 + "x": 1.17635, + "y": 7.075 }, "prevControl": { - "x": 2.5571335526315786, - "y": 6.005180921052632 + "x": 2.6074835526315785, + "y": 6.0551809210526315 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/LeftBLUE-5.path b/src/main/deploy/pathplanner/paths/LeftBLUE-5.path index 941f9e2..c675eac 100644 --- a/src/main/deploy/pathplanner/paths/LeftBLUE-5.path +++ b/src/main/deploy/pathplanner/paths/LeftBLUE-5.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 1.126, - "y": 7.025 + "x": 1.17635, + "y": 7.075 }, "prevControl": null, "nextControl": { - "x": 2.258860526315789, - "y": 6.10139802631579 + "x": 2.3092105263157894, + "y": 6.15139802631579 }, "isLocked": false, "linkedName": "Station-Left" diff --git a/src/main/deploy/pathplanner/paths/ProcessorBLUE-1.path b/src/main/deploy/pathplanner/paths/ProcessorBLUE-1.path index 35498c1..35b3337 100644 --- a/src/main/deploy/pathplanner/paths/ProcessorBLUE-1.path +++ b/src/main/deploy/pathplanner/paths/ProcessorBLUE-1.path @@ -16,12 +16,12 @@ }, { "anchor": { - "x": 4.99, - "y": 2.883 + "x": 4.992, + "y": 2.88 }, "prevControl": { - "x": 5.49102706845917, - "y": 2.803080281670129 + "x": 5.493026965738442, + "y": 2.800079637703237 }, "nextControl": null, "isLocked": false, diff --git a/src/main/deploy/pathplanner/paths/ProcessorBLUE-2.path b/src/main/deploy/pathplanner/paths/ProcessorBLUE-2.path index 7681d49..dcceefd 100644 --- a/src/main/deploy/pathplanner/paths/ProcessorBLUE-2.path +++ b/src/main/deploy/pathplanner/paths/ProcessorBLUE-2.path @@ -3,13 +3,13 @@ "waypoints": [ { "anchor": { - "x": 5.01, - "y": 2.9 + "x": 4.9918, + "y": 2.88 }, "prevControl": null, "nextControl": { - "x": 4.538062138878912, - "y": 1.6772191695918486 + "x": 4.51986213887891, + "y": 1.6572191695918486 }, "isLocked": true, "linkedName": null