diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 52a5960..9f9ce61 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,11 @@ 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/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"); 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..d4072ea --- /dev/null +++ b/src/main/java/frc/robot/commands/SourceStrafingCommands.java @@ -0,0 +1,107 @@ +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.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; +import frc.robot.FieldConstants; +import frc.robot.commands.controllers.JoystickInputController; +import frc.robot.subsystems.drive.Drive; +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) { + 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 strafeAlongLine( + drive, + xbox, + AllianceFlipUtil.apply( + FieldConstants.CoralStation.rightCenterFace.getRotation().minus(Rotation2d.k180deg)), + Translation2d.kZero, // TODO + Translation2d.kZero); + } + } + + 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); + + Runnable keepAtRange = + () -> { + Translation2d inputTranslation = input.getTranslationMetersPerSecond(); + Pose2d robotPose = drivetrain.getRobotPose(); + double targetHeadingRadians = desiredHeading.getRadians(); + + double omega = angleController.calculate(desiredHeading.getRadians()); + + double targetX = findTargetX(a, b, robotPose.getY()); + + boolean notAtDesiredAngle = + !MathUtil.isNear( + robotPose.getRotation().getRadians(), targetHeadingRadians, Math.PI / 4); + + drivetrain.setRobotSpeeds( + new ChassisSpeeds( + xController.calculate(robotPose.getX(), targetX), + inputTranslation.getY(), + notAtDesiredAngle ? omega : 0), + true); + }; + + return Commands.sequence( + DriveCommands.pathfindToPoseCommand(drivetrain, startingPose, 1, 0), + 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, + () -> -xbox.getLeftY(), + () -> -xbox.getLeftX(), + () -> -xbox.getRightY(), + () -> -xbox.getRightX()); + } +} 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..903dc9b --- /dev/null +++ b/src/main/java/frc/robot/subsystems/rangefinder/Rangefinder.java @@ -0,0 +1,25 @@ +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..cbc1942 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/rangefinder/RangefinderIOHardware.java @@ -0,0 +1,36 @@ +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 { + private final LaserCan sensor; + + public RangefinderIOHardware(int id) { + sensor = new LaserCan(id); + + try { + sensor.setRangingMode(RangingMode.SHORT); + } catch (ConfigurationFailedException e) { + e.printStackTrace(); + } + } + + @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(); + } + } +} 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