From b52ca90c4ca8f2e6dd15a5c28423107a1513a6e4 Mon Sep 17 00:00:00 2001 From: "Aceius E." Date: Wed, 12 Mar 2025 16:09:08 -0700 Subject: [PATCH 1/9] Install vendor deps --- vendordeps/libgrapplefrc2025.json | 74 +++++++++++++++++++++++++++++++ 1 file changed, 74 insertions(+) create mode 100644 vendordeps/libgrapplefrc2025.json diff --git a/vendordeps/libgrapplefrc2025.json b/vendordeps/libgrapplefrc2025.json new file mode 100644 index 0000000..a8884ff --- /dev/null +++ b/vendordeps/libgrapplefrc2025.json @@ -0,0 +1,74 @@ +{ + "fileName": "libgrapplefrc2025.json", + "name": "libgrapplefrc", + "version": "2025.0.8", + "frcYear": "2025", + "uuid": "8ef3423d-9532-4665-8339-206dae1d7168", + "mavenUrls": [ + "https://storage.googleapis.com/grapple-frc-maven" + ], + "jsonUrl": "https://storage.googleapis.com/grapple-frc-maven/libgrapplefrc2025.json", + "javaDependencies": [ + { + "groupId": "au.grapplerobotics", + "artifactId": "libgrapplefrcjava", + "version": "2025.0.8" + } + ], + "jniDependencies": [ + { + "groupId": "au.grapplerobotics", + "artifactId": "libgrapplefrcdriver", + "version": "2025.0.8", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "au.grapplerobotics", + "artifactId": "libgrapplefrccpp", + "version": "2025.0.8", + "libName": "grapplefrc", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + }, + { + "groupId": "au.grapplerobotics", + "artifactId": "libgrapplefrcdriver", + "version": "2025.0.8", + "libName": "grapplefrcdriver", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "windowsx86", + "linuxarm64", + "linuxx86-64", + "linuxathena", + "linuxarm32", + "osxuniversal" + ] + } + ] +} \ No newline at end of file From e7728fe9f51a3ab272613bbc6fc15ca63fda2ea4 Mon Sep 17 00:00:00 2001 From: "Aceius E." Date: Wed, 12 Mar 2025 16:25:11 -0700 Subject: [PATCH 2/9] Implement lazer can sensor and subsystem --- .../subsystems/rangefinder/Rangefinder.java | 27 +++++++++++++++++ .../subsystems/rangefinder/RangefinderIO.java | 18 +++++++++++ .../rangefinder/RangefinderIOHardware.java | 30 +++++++++++++++++++ 3 files changed, 75 insertions(+) create mode 100644 src/main/java/frc/robot/subsystems/rangefinder/Rangefinder.java create mode 100644 src/main/java/frc/robot/subsystems/rangefinder/RangefinderIO.java create mode 100644 src/main/java/frc/robot/subsystems/rangefinder/RangefinderIOHardware.java diff --git a/src/main/java/frc/robot/subsystems/rangefinder/Rangefinder.java b/src/main/java/frc/robot/subsystems/rangefinder/Rangefinder.java new file mode 100644 index 0000000..6cc0839 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/rangefinder/Rangefinder.java @@ -0,0 +1,27 @@ +package frc.robot.subsystems.rangefinder; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +import java.util.Optional; + +import org.littletonrobotics.junction.Logger; + +public class Rangefinder extends SubsystemBase { + private final RangefinderIO io; + private final RangefinderIOInputsAutoLogged inputs = new RangefinderIOInputsAutoLogged(); + + public Rangefinder(RangefinderIO io) { + this.io = io; + } + + @Override + public void periodic() { + io.updateInputs(inputs); + Logger.processInputs("Rangefinder", inputs); + } + + /** Get the measurement in millimetres */ + public Optional getMeasurement() { + return io.getMeasurementMillimetres(); + } +} diff --git a/src/main/java/frc/robot/subsystems/rangefinder/RangefinderIO.java b/src/main/java/frc/robot/subsystems/rangefinder/RangefinderIO.java new file mode 100644 index 0000000..3d4f63f --- /dev/null +++ b/src/main/java/frc/robot/subsystems/rangefinder/RangefinderIO.java @@ -0,0 +1,18 @@ +package frc.robot.subsystems.rangefinder; + +import java.util.Optional; +import org.littletonrobotics.junction.AutoLog; + +public interface RangefinderIO { + @AutoLog + public static class RangefinderIOInputs { + public int measurementMillimetres = 0; + } + + /** Updates the set of loggable inputs. */ + public default void updateInputs(RangefinderIOInputs inputs) {} + + public default Optional getMeasurementMillimetres() { + return Optional.empty(); + } +} diff --git a/src/main/java/frc/robot/subsystems/rangefinder/RangefinderIOHardware.java b/src/main/java/frc/robot/subsystems/rangefinder/RangefinderIOHardware.java new file mode 100644 index 0000000..d1aaefa --- /dev/null +++ b/src/main/java/frc/robot/subsystems/rangefinder/RangefinderIOHardware.java @@ -0,0 +1,30 @@ +package frc.robot.subsystems.rangefinder; + +import au.grapplerobotics.LaserCan; +import java.util.Optional; + +public class RangefinderIOHardware implements RangefinderIO { + LaserCan sensor; + + int measurementMillimetres; + + public RangefinderIOHardware(int id) { + sensor = new LaserCan(id); + } + + @Override + public void updateInputs(RangefinderIOInputs inputs) { + inputs.measurementMillimetres = getMeasurementMillimetres().orElse(-1); + } + + @Override + public Optional getMeasurementMillimetres() { + LaserCan.Measurement measurement = sensor.getMeasurement(); + + if (measurement != null && measurement.status == LaserCan.LASERCAN_STATUS_VALID_MEASUREMENT) { + return Optional.of(measurement.distance_mm); + } else { + return Optional.empty(); + } + } +} From 3185becd9a7abdb8fa8a64a7d293cfcacdf39ab4 Mon Sep 17 00:00:00 2001 From: "Aceius E." Date: Wed, 12 Mar 2025 16:26:00 -0700 Subject: [PATCH 3/9] Auto-format --- src/main/java/frc/robot/subsystems/rangefinder/Rangefinder.java | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/rangefinder/Rangefinder.java b/src/main/java/frc/robot/subsystems/rangefinder/Rangefinder.java index 6cc0839..903dc9b 100644 --- a/src/main/java/frc/robot/subsystems/rangefinder/Rangefinder.java +++ b/src/main/java/frc/robot/subsystems/rangefinder/Rangefinder.java @@ -1,9 +1,7 @@ package frc.robot.subsystems.rangefinder; import edu.wpi.first.wpilibj2.command.SubsystemBase; - import java.util.Optional; - import org.littletonrobotics.junction.Logger; public class Rangefinder extends SubsystemBase { From 649974add6192bc24854374363df62e46849ec32 Mon Sep 17 00:00:00 2001 From: "Aceius E." Date: Wed, 12 Mar 2025 16:32:05 -0700 Subject: [PATCH 4/9] spotless --- .../frc/robot/subsystems/rangefinder/RangefinderIOHardware.java | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/rangefinder/RangefinderIOHardware.java b/src/main/java/frc/robot/subsystems/rangefinder/RangefinderIOHardware.java index d1aaefa..94e4814 100644 --- a/src/main/java/frc/robot/subsystems/rangefinder/RangefinderIOHardware.java +++ b/src/main/java/frc/robot/subsystems/rangefinder/RangefinderIOHardware.java @@ -6,8 +6,6 @@ public class RangefinderIOHardware implements RangefinderIO { LaserCan sensor; - int measurementMillimetres; - public RangefinderIOHardware(int id) { sensor = new LaserCan(id); } From 243e173e66996a542ea07f6b883826231fc00ee7 Mon Sep 17 00:00:00 2001 From: "Aceius E." Date: Wed, 12 Mar 2025 16:54:09 -0700 Subject: [PATCH 5/9] Configure for proper range --- .../subsystems/rangefinder/RangefinderIOHardware.java | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/src/main/java/frc/robot/subsystems/rangefinder/RangefinderIOHardware.java b/src/main/java/frc/robot/subsystems/rangefinder/RangefinderIOHardware.java index 94e4814..c336cd1 100644 --- a/src/main/java/frc/robot/subsystems/rangefinder/RangefinderIOHardware.java +++ b/src/main/java/frc/robot/subsystems/rangefinder/RangefinderIOHardware.java @@ -1,13 +1,22 @@ package frc.robot.subsystems.rangefinder; +import au.grapplerobotics.ConfigurationFailedException; import au.grapplerobotics.LaserCan; +import au.grapplerobotics.interfaces.LaserCanInterface.RangingMode; + import java.util.Optional; public class RangefinderIOHardware implements RangefinderIO { - LaserCan sensor; + private final LaserCan sensor; public RangefinderIOHardware(int id) { sensor = new LaserCan(id); + + try { + sensor.setRangingMode(RangingMode.SHORT); + } catch (ConfigurationFailedException e) { + e.printStackTrace(); + } } @Override From 9350db92d6eb6d8fa75010ace6e8066fefa881fa Mon Sep 17 00:00:00 2001 From: "Aceius E." Date: Wed, 12 Mar 2025 18:39:16 -0700 Subject: [PATCH 6/9] THis is untested --- src/main/java/frc/robot/commands/DriveToPose.java | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/java/frc/robot/commands/DriveToPose.java b/src/main/java/frc/robot/commands/DriveToPose.java index 90008f6..15cf863 100644 --- a/src/main/java/frc/robot/commands/DriveToPose.java +++ b/src/main/java/frc/robot/commands/DriveToPose.java @@ -26,6 +26,7 @@ import java.util.function.Supplier; import org.littletonrobotics.junction.Logger; +@Deprecated public class DriveToPose extends Command { private static final LoggedTunableNumber drivekP = new LoggedTunableNumber("DriveToPose/DrivekP"); private static final LoggedTunableNumber drivekD = new LoggedTunableNumber("DriveToPose/DrivekD"); From 0a7b9c88df1ee48c3c0f02fe3b035e593d58f0b0 Mon Sep 17 00:00:00 2001 From: "Aceius E." Date: Wed, 12 Mar 2025 18:56:23 -0700 Subject: [PATCH 7/9] Start work on intake alignment --- src/main/java/frc/robot/RobotContainer.java | 3 +- .../commands/SourceStrafingCommands.java | 81 +++++++++++++++++++ 2 files changed, 83 insertions(+), 1 deletion(-) create mode 100644 src/main/java/frc/robot/commands/SourceStrafingCommands.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 52a5960..77e06de 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -25,6 +25,7 @@ import frc.robot.commands.AdaptiveAutoAlignCommands; import frc.robot.commands.DriveCommands; import frc.robot.commands.ManualAlignCommands; +import frc.robot.commands.SourceStrafingCommands; import frc.robot.commands.controllers.JoystickInputController; import frc.robot.commands.controllers.SpeedLevelController; import frc.robot.subsystems.dashboard.DriverDashboard; @@ -456,7 +457,7 @@ private void configureDriverControllerBindings( intakeAlignmentCommands.setEndCommand(() -> rumbleController(xbox, 0.3).withTimeout(0.1)); xbox.leftTrigger() - .onTrue(intakeAlignmentCommands.driveToClosest(drive).withName("Align INTAKE")) + .onTrue(intakeAlignmentCommands.driveToClosest(drive).withName("Align INTAKE").andThen(SourceStrafingCommands.takeAction(drive, xbox))) .onFalse(intakeAlignmentCommands.stop(drive)); xbox.leftTrigger() diff --git a/src/main/java/frc/robot/commands/SourceStrafingCommands.java b/src/main/java/frc/robot/commands/SourceStrafingCommands.java new file mode 100644 index 0000000..8393da0 --- /dev/null +++ b/src/main/java/frc/robot/commands/SourceStrafingCommands.java @@ -0,0 +1,81 @@ +package frc.robot.commands; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.button.CommandXboxController; +import frc.robot.FieldConstants; +import frc.robot.commands.controllers.JoystickInputController; +import frc.robot.subsystems.drive.Drive; +import frc.robot.utility.AllianceFlipUtil; + +public class SourceStrafingCommands { + /** IF auto alignment is finished and placed us by a source */ + public static Command takeAction(Drive drive, CommandXboxController xbox) { + Pose2d robotPose = drive.getRobotPose(); + boolean bySourceLeft = false, bySourceRight = false; + + if (bySourceLeft) { + return strafeOnSourceLeft(drive, xbox).until(xbox.leftBumper().negate()); + } else { // Since autoalign just ran we must be on the right + // return + } + } + + public static Command strafeOnSourceLeft(Drive drivetrain, CommandXboxController controller) { + final JoystickInputController input = getInput(drivetrain, controller); + final PIDController xController = new PIDController(0, 0, 0); + final PIDController angleController = new PIDController(0, 0, 0); + + Pose2d startingPose = // TODO offset this + AllianceFlipUtil.apply(FieldConstants.CoralStation.leftCenterFace); + + Runnable keepAtRange = + () -> { + Translation2d inputTranslation = input.getTranslationMetersPerSecond(); + double targetHeadingRadians = + AllianceFlipUtil.apply( + FieldConstants.CoralStation.leftCenterFace + .getRotation() + .minus(Rotation2d.k180deg)) + .getRadians(); + + double omega = + angleController.calculate( + AllianceFlipUtil.apply(drivetrain.getRobotPose().getRotation()).getRadians()); + + double targetX = 0; // TODO + + boolean notAtDesiredAngle = + !MathUtil.isNear( + drivetrain.getRobotPose().getRotation().getRadians(), + targetHeadingRadians, + Math.PI / 4); + + drivetrain.setRobotSpeeds( + new ChassisSpeeds( + xController.calculate(drivetrain.getRobotPose().getX(), targetX), + inputTranslation.getY(), + notAtDesiredAngle ? omega : 0), + true); + }; + + return Commands.sequence( + DriveCommands.pathfindToPoseCommand(drivetrain, startingPose, 1, 0), + drivetrain.run(keepAtRange)); + } + + private static JoystickInputController getInput(Drive drive, CommandXboxController xbox) { + return new JoystickInputController( + drive, + () -> -xbox.getLeftY(), + () -> -xbox.getLeftX(), + () -> -xbox.getRightY(), + () -> -xbox.getRightX()); + } +} From aeca9b6b553f3864b1c525d2db99fb115e57cc7d Mon Sep 17 00:00:00 2001 From: "Aceius E." Date: Wed, 12 Mar 2025 19:30:01 -0700 Subject: [PATCH 8/9] Mostly finish source strafing --- .../commands/SourceStrafingCommands.java | 74 +++++++++++++------ 1 file changed, 50 insertions(+), 24 deletions(-) diff --git a/src/main/java/frc/robot/commands/SourceStrafingCommands.java b/src/main/java/frc/robot/commands/SourceStrafingCommands.java index 8393da0..d4072ea 100644 --- a/src/main/java/frc/robot/commands/SourceStrafingCommands.java +++ b/src/main/java/frc/robot/commands/SourceStrafingCommands.java @@ -6,6 +6,8 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; @@ -15,22 +17,48 @@ import frc.robot.utility.AllianceFlipUtil; public class SourceStrafingCommands { + private static final PIDController xController = new PIDController(0, 0, 0); // TODO + private static final PIDController angleController = new PIDController(0, 0, 0); + /** IF auto alignment is finished and placed us by a source */ public static Command takeAction(Drive drive, CommandXboxController xbox) { - Pose2d robotPose = drive.getRobotPose(); - boolean bySourceLeft = false, bySourceRight = false; - - if (bySourceLeft) { - return strafeOnSourceLeft(drive, xbox).until(xbox.leftBumper().negate()); + if (bySourceLeft(drive.getRobotPose().getY())) { + return strafeAlongLine( + drive, + xbox, + AllianceFlipUtil.apply( + FieldConstants.CoralStation.leftCenterFace + .getRotation() + .minus(Rotation2d.k180deg)), + Translation2d.kZero, // TODO + Translation2d.kZero) + .until(xbox.leftBumper().negate()); } else { // Since autoalign just ran we must be on the right - // return + return strafeAlongLine( + drive, + xbox, + AllianceFlipUtil.apply( + FieldConstants.CoralStation.rightCenterFace.getRotation().minus(Rotation2d.k180deg)), + Translation2d.kZero, // TODO + Translation2d.kZero); } } - public static Command strafeOnSourceLeft(Drive drivetrain, CommandXboxController controller) { - final JoystickInputController input = getInput(drivetrain, controller); - final PIDController xController = new PIDController(0, 0, 0); - final PIDController angleController = new PIDController(0, 0, 0); + private static boolean bySourceLeft(double y) { + Alliance alliance = DriverStation.getAlliance().orElse(Alliance.Blue); + boolean isTopHalf = y >= 4; + + return (alliance == Alliance.Blue && isTopHalf) || (alliance == Alliance.Red && !isTopHalf); + } + + /** Do not flip input translations, they will be handled by the function */ + public static Command strafeAlongLine( + Drive drivetrain, + CommandXboxController xbox, + Rotation2d desiredHeading, + Translation2d a, + Translation2d b) { + final JoystickInputController input = getInput(drivetrain, xbox); Pose2d startingPose = // TODO offset this AllianceFlipUtil.apply(FieldConstants.CoralStation.leftCenterFace); @@ -38,28 +66,20 @@ public static Command strafeOnSourceLeft(Drive drivetrain, CommandXboxController Runnable keepAtRange = () -> { Translation2d inputTranslation = input.getTranslationMetersPerSecond(); - double targetHeadingRadians = - AllianceFlipUtil.apply( - FieldConstants.CoralStation.leftCenterFace - .getRotation() - .minus(Rotation2d.k180deg)) - .getRadians(); + Pose2d robotPose = drivetrain.getRobotPose(); + double targetHeadingRadians = desiredHeading.getRadians(); - double omega = - angleController.calculate( - AllianceFlipUtil.apply(drivetrain.getRobotPose().getRotation()).getRadians()); + double omega = angleController.calculate(desiredHeading.getRadians()); - double targetX = 0; // TODO + double targetX = findTargetX(a, b, robotPose.getY()); boolean notAtDesiredAngle = !MathUtil.isNear( - drivetrain.getRobotPose().getRotation().getRadians(), - targetHeadingRadians, - Math.PI / 4); + robotPose.getRotation().getRadians(), targetHeadingRadians, Math.PI / 4); drivetrain.setRobotSpeeds( new ChassisSpeeds( - xController.calculate(drivetrain.getRobotPose().getX(), targetX), + xController.calculate(robotPose.getX(), targetX), inputTranslation.getY(), notAtDesiredAngle ? omega : 0), true); @@ -70,6 +90,12 @@ public static Command strafeOnSourceLeft(Drive drivetrain, CommandXboxController drivetrain.run(keepAtRange)); } + private static double findTargetX(Translation2d a, Translation2d b, double y) { + double m = (b.getY() - a.getY()) / (b.getX() - a.getX()); + + return ((y - a.getY()) / m) + a.getX(); + } + private static JoystickInputController getInput(Drive drive, CommandXboxController xbox) { return new JoystickInputController( drive, From 6dc66339f79dc0a9f43689430950aa9c4b57fed0 Mon Sep 17 00:00:00 2001 From: "Aceius E." Date: Thu, 13 Mar 2025 18:13:04 -0700 Subject: [PATCH 9/9] ok --- src/main/java/frc/robot/RobotContainer.java | 6 +++++- .../robot/subsystems/rangefinder/RangefinderIOHardware.java | 1 - 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 77e06de..9f9ce61 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -457,7 +457,11 @@ private void configureDriverControllerBindings( intakeAlignmentCommands.setEndCommand(() -> rumbleController(xbox, 0.3).withTimeout(0.1)); xbox.leftTrigger() - .onTrue(intakeAlignmentCommands.driveToClosest(drive).withName("Align INTAKE").andThen(SourceStrafingCommands.takeAction(drive, xbox))) + .onTrue( + intakeAlignmentCommands + .driveToClosest(drive) + .withName("Align INTAKE") + .andThen(SourceStrafingCommands.takeAction(drive, xbox))) .onFalse(intakeAlignmentCommands.stop(drive)); xbox.leftTrigger() diff --git a/src/main/java/frc/robot/subsystems/rangefinder/RangefinderIOHardware.java b/src/main/java/frc/robot/subsystems/rangefinder/RangefinderIOHardware.java index c336cd1..cbc1942 100644 --- a/src/main/java/frc/robot/subsystems/rangefinder/RangefinderIOHardware.java +++ b/src/main/java/frc/robot/subsystems/rangefinder/RangefinderIOHardware.java @@ -3,7 +3,6 @@ import au.grapplerobotics.ConfigurationFailedException; import au.grapplerobotics.LaserCan; import au.grapplerobotics.interfaces.LaserCanInterface.RangingMode; - import java.util.Optional; public class RangefinderIOHardware implements RangefinderIO {