diff --git a/.envrc b/.envrc new file mode 100644 index 0000000..44610e5 --- /dev/null +++ b/.envrc @@ -0,0 +1 @@ +use flake; diff --git a/.gitignore b/.gitignore index 960ef98..bced768 100644 --- a/.gitignore +++ b/.gitignore @@ -186,5 +186,4 @@ compile_commands.json # Version file src/main/java/frc/robot/BuildConstants.java .direnv/ -.envrc -flake.nix \ No newline at end of file +flake.lock \ No newline at end of file diff --git a/.prettierrc b/.prettierrc new file mode 100644 index 0000000..ab28f3e --- /dev/null +++ b/.prettierrc @@ -0,0 +1,3 @@ +{ + "tabWidth": 4 +} \ No newline at end of file diff --git a/.vscode/settings.json b/.vscode/settings.json index 0be0819..3d910e1 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -1,73 +1,67 @@ { - "java.configuration.updateBuildConfiguration": "automatic", - "java.server.launchMode": "Standard", - "files.exclude": { - "**/.git": true, - "**/.svn": true, - "**/.hg": true, - "**/CVS": true, - "**/.DS_Store": true, - "bin/": true, - "**/.classpath": true, - "**/.project": true, - "**/.settings": true, - "**/.factorypath": true, - "**/*~": true - }, - "java.test.config": [ - { - "name": "WPIlibUnitTests", - "workingDirectory": "${workspaceFolder}/build/jni/release", - "vmargs": [ - "-Djava.library.path=${workspaceFolder}/build/jni/release" - ], - "env": { - "LD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release", - "DYLD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release" - } - }, - null - ], - "java.test.defaultConfig": "WPIlibUnitTests", - "spotlessGradle.format.enable": true, - "spotlessGradle.diagnostics.enable": false, - "java.import.gradle.annotationProcessing.enabled": false, - "java.completion.favoriteStaticMembers": [ - "org.junit.Assert.*", - "org.junit.Assume.*", - "org.junit.jupiter.api.Assertions.*", - "org.junit.jupiter.api.Assumptions.*", - "org.junit.jupiter.api.DynamicContainer.*", - "org.junit.jupiter.api.DynamicTest.*", - "org.mockito.Mockito.*", - "org.mockito.ArgumentMatchers.*", - "org.mockito.Answers.*", - "edu.wpi.first.units.Units.*" - ], - "java.completion.filteredTypes": [ - "java.awt.*", - "com.sun.*", - "sun.*", - "jdk.*", - "org.graalvm.*", - "io.micrometer.shaded.*", - "java.beans.*", - "java.util.Base64.*", - "java.util.Timer", - "java.sql.*", - "javax.swing.*", - "javax.management.*", - "javax.smartcardio.*", - "edu.wpi.first.math.proto.*", - "edu.wpi.first.math.**.proto.*", - "edu.wpi.first.math.**.struct.*" - ], - "java.dependency.enableDependencyCheckup": false, - "editor.defaultFormatter": "richardwillis.vscode-spotless-gradle", - "[json]": { - "editor.defaultFormatter": "richardwillis.vscode-spotless-gradle" - }, - "[java]": { - "editor.defaultFormatter": "richardwillis.vscode-spotless-gradle" - } -} + "java.configuration.updateBuildConfiguration": "automatic", + "java.server.launchMode": "Standard", + "files.exclude": { + "**/.git": true, + "**/.svn": true, + "**/.hg": true, + "**/CVS": true, + "**/.DS_Store": true, + "bin/": true, + "**/.classpath": true, + "**/.project": true, + "**/.settings": true, + "**/.factorypath": true, + "**/*~": true + }, + "java.test.config": [ + { + "name": "WPIlibUnitTests", + "workingDirectory": "${workspaceFolder}/build/jni/release", + "vmargs": [ + "-Djava.library.path=${workspaceFolder}/build/jni/release" + ], + "env": { + "LD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release", + "DYLD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release" + } + } + ], + "java.test.defaultConfig": "WPIlibUnitTests", + "spotlessGradle.format.enable": true, + "spotlessGradle.diagnostics.enable": false, + "java.import.gradle.annotationProcessing.enabled": false, + "java.completion.favoriteStaticMembers": [ + "org.junit.Assert.*", + "org.junit.Assume.*", + "org.junit.jupiter.api.Assertions.*", + "org.junit.jupiter.api.Assumptions.*", + "org.junit.jupiter.api.DynamicContainer.*", + "org.junit.jupiter.api.DynamicTest.*", + "org.mockito.Mockito.*", + "org.mockito.ArgumentMatchers.*", + "org.mockito.Answers.*", + "edu.wpi.first.units.Units.*" + ], + "java.completion.filteredTypes": [ + "java.awt.*", + "com.sun.*", + "sun.*", + "jdk.*", + "org.graalvm.*", + "io.micrometer.shaded.*", + "java.beans.*", + "java.util.Base64.*", + "java.util.Timer", + "java.sql.*", + "javax.swing.*", + "javax.management.*", + "javax.smartcardio.*", + "edu.wpi.first.math.proto.*", + "edu.wpi.first.math.**.proto.*", + "edu.wpi.first.math.**.struct.*" + ], + "java.dependency.enableDependencyCheckup": false, + "editor.formatOnSave": true, + "prettier-plugin-java-vscode.prettierConfigPath": "${workspaceFolder}/.pretterrc" +} \ No newline at end of file diff --git a/build.gradle b/build.gradle index 4ff7215..16a5874 100644 --- a/build.gradle +++ b/build.gradle @@ -2,6 +2,7 @@ plugins { id "java" id "edu.wpi.first.GradleRIO" version "2026.1.1" id "com.peterabeles.gversion" version "1.10" + id "net.ltgt.errorprone" version "5.0.0" } java { @@ -92,6 +93,7 @@ dependencies { testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1' testRuntimeOnly 'org.junit.platform:junit-platform-launcher' + errorprone "com.google.errorprone:error_prone_core:2.25.0" def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text) annotationProcessor "org.littletonrobotics.akit:akit-autolog:$akitJson.version" @@ -173,3 +175,7 @@ task(eventDeploy) { } } createVersionFile.dependsOn(eventDeploy) + +tasks.withType(JavaCompile).configureEach { + options.errorprone.disableWarningsInGeneratedCode = true +} diff --git a/flake.nix b/flake.nix new file mode 100644 index 0000000..56c9b73 --- /dev/null +++ b/flake.nix @@ -0,0 +1,48 @@ +{ + description = "Robotics Development Environment"; + + inputs = { + nixpkgs.url = "github:NixOS/nixpkgs/nixos-unstable"; + flake-utils.url = "github:numtide/flake-utils"; + }; + + outputs = + { + self, + nixpkgs, + flake-utils, + }: + flake-utils.lib.eachDefaultSystem ( + system: + let + pkgs = import nixpkgs { inherit system; }; + + applications = with pkgs; [ + nil + nixd + + javaPackages.compiler.temurin-bin.jdk-17 + gradle + jdt-language-server + + nodejs + prettier + ]; + + libraries = with pkgs; [ + openssl + gcc + ]; + in + { + devShells.default = pkgs.mkShell { + buildInputs = applications ++ libraries; + + OPENSSL_DIR = pkgs.openssl.dev; + OPENSSL_LIB_DIR = "${pkgs.openssl.out}/lib"; + LD_LIBRARY_PATH = pkgs.lib.makeLibraryPath libraries; + PKG_CONFIG_PATH = pkgs.lib.makeSearchPath "lib/pkgconfig" libraries; + }; + } + ); +} diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index 7a5d644..66e930c 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/src/main/java/frc/robot/commands/DriveCommands.java @@ -22,11 +22,12 @@ import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; +import frc.robot.constants.ControlConstants; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.drive.DriveConstants; import java.text.DecimalFormat; import java.text.NumberFormat; -import java.util.LinkedList; +import java.util.ArrayList; import java.util.List; import java.util.function.DoubleSupplier; import java.util.function.Supplier; @@ -81,9 +82,9 @@ public static Command joystickDrive( // Convert to field relative speeds & send command ChassisSpeeds speeds = new ChassisSpeeds( - linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(), - linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(), - omega * drive.getMaxAngularSpeedRadPerSec()); + linearVelocity.getX() * ControlConstants.Drive.kMaxLinearSpeed, + linearVelocity.getY() * ControlConstants.Drive.kMaxLinearSpeed, + omega * ControlConstants.Drive.kMaxAngularSpeed); boolean isFlipped = DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == Alliance.Red; @@ -132,8 +133,8 @@ public static Command joystickDriveAtAngle( // Convert to field relative speeds & send command ChassisSpeeds speeds = new ChassisSpeeds( - linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(), - linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(), + linearVelocity.getX() * ControlConstants.Drive.kMaxLinearSpeed, + linearVelocity.getY() * ControlConstants.Drive.kMaxLinearSpeed, omega); boolean isFlipped = DriverStation.getAlliance().isPresent() @@ -157,8 +158,8 @@ public static Command joystickDriveAtAngle( *

This command should only be used in voltage control mode. */ public static Command feedforwardCharacterization(Drive drive) { - List velocitySamples = new LinkedList<>(); - List voltageSamples = new LinkedList<>(); + List velocitySamples = new ArrayList<>(); + List voltageSamples = new ArrayList<>(); Timer timer = new Timer(); return Commands.sequence( diff --git a/src/main/java/frc/robot/commands/pathfind/Nearby.java b/src/main/java/frc/robot/commands/pathfind/Nearby.java new file mode 100644 index 0000000..4140b18 --- /dev/null +++ b/src/main/java/frc/robot/commands/pathfind/Nearby.java @@ -0,0 +1,158 @@ +package frc.robot.commands.pathfind; + +import static frc.robot.util.MathPlus.kTau; + +import edu.wpi.first.math.controller.HolonomicDriveController; +import edu.wpi.first.math.controller.PIDController; +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.kinematics.ChassisSpeeds; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.constants.ControlConstants; +import frc.robot.subsystems.drive.Drive; +import java.util.function.Supplier; + +/** + * Command for nearby pose-targeted navigation using PIDs. + * + *

This command is used as a helper for {@link Pathfind}. It is + * used as the final step: close to the target, switch to PID control. + * + *

Navigate uses a {@link HolonomicDriveController} with three independent PID controllers + * to move the robot to a target pose. The controller calculates the required + * chassis speeds (x, y, rotation) to reach the target. + */ +class Nearby extends Command { + + /** Get the supplier on {@link #initialize()} */ + final Supplier target; + + /** PID for X position (field-relative). */ + PIDController xController = new PIDController( + ControlConstants.Nearby.kOrthoP, + ControlConstants.Nearby.kOrthoI, + ControlConstants.Nearby.kOrthoD + ); + /** PID for Y position (field-relative). */ + PIDController yController = new PIDController( + ControlConstants.Nearby.kOrthoP, + ControlConstants.Nearby.kOrthoI, + ControlConstants.Nearby.kOrthoD + ); + /** Profiled PID for rotation with velocity/acceleration constraints. */ + ProfiledPIDController rotController = new ProfiledPIDController( + ControlConstants.Nearby.kTurnP, + ControlConstants.Nearby.kTurnI, + ControlConstants.Nearby.kTurnD, + new TrapezoidProfile.Constraints( + ControlConstants.Nearby.kMaxTurnVelocity, + ControlConstants.Nearby.kMaxTurnAcceleration + ) + ); + + /** High-level holonomic controller combining all three PID loops. */ + final HolonomicDriveController controller = new HolonomicDriveController( + xController, + yController, + rotController + ); + + /** Drive subsystem for motion control. */ + final Drive drive; + /** Current target pose being navigated to. */ + Pose2d currentTarget; + + /** Flag to exit early if target pose is invalid. */ + boolean exit = false; + + /** + * Constructs a Nearby command. + * + *

Initializes the HolonomicDriveController with the three PID controllers, + * enables continuous input for rotation (angles wrap around), and sets the + * position tolerance for determining when the target is reached. + * + * @param drive Drive subsystem for movement control + * @param target The target supplier + */ + Nearby(Drive drive, Supplier target) { + this.drive = drive; + this.target = target; + + // Enable continuous input: angles wrap at 2π so -0.1 rad = 2π - 0.1 rad + rotController.enableContinuousInput(0, kTau); + // Set tolerance threshold for "at reference" check + controller.setTolerance(ControlConstants.Nearby.kTolerance); + + addRequirements(drive); + } + + /** + * Command initialization: Get target pose and prepare for navigation. + * + *

Called once when the command starts. Fetches the target pose from + * {@link #target}, logs it, and displays it on the field view + * for debugging. + */ + @Override + public final void initialize() { + exit = false; + currentTarget = target.get(); + + if (currentTarget == null) { + exit = true; + return; + } + } + + /** + * Command execution: Calculate and send chassis speeds to reach target. + * + *

Uses the {@link HolonomicDriveController} to calculate the required + * chassis speeds (vx, vy, ω) based on current pose, target pose, and + * controller gains. + */ + @Override + public final void execute() { + if (currentTarget == null) return; + + Pose2d robot = drive.getPose(); + // Calculate speeds using holonomic controller, with a target velocity of 1 cm/s (0.01 m/s) + ChassisSpeeds speeds = controller.calculate( + robot, + currentTarget, + 0.01, + currentTarget.getRotation() + ); + // Send robot-relative speeds to drive + drive.runVelocity(speeds); + } + + /** + * Command end: Stop robot and clean up target visualization. + * + *

Called when the command finishes (either by {@link #isFinished()} or interruption). + * Removes the target from the field visualization and stops the robot. + * + * @param interrupted true if command was interrupted, false if finished normally + */ + @Override + public void end(boolean interrupted) { + // Stop the robot + drive.runVelocity(new ChassisSpeeds()); + } + + /** + * Checks if the robot has reached the target pose. + * + *

Uses the {@link HolonomicDriveController}'s tolerance checking to determine if + * both position and rotation are within acceptable thresholds. + * + * @return true if at target within tolerance, false otherwise + */ + @Override + public boolean isFinished() { + return controller.atReference(); + } +} diff --git a/src/main/java/frc/robot/commands/pathfind/Pathfind.java b/src/main/java/frc/robot/commands/pathfind/Pathfind.java new file mode 100644 index 0000000..bfbbb0f --- /dev/null +++ b/src/main/java/frc/robot/commands/pathfind/Pathfind.java @@ -0,0 +1,124 @@ +package frc.robot.commands.pathfind; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import frc.robot.subsystems.drive.Drive; +import java.util.function.Supplier; +import org.littletonrobotics.junction.Logger; + +/** + * Abstract command for pose-based robot navigation using holonomic drive control. + * + *

This is an abstract command that subclasses override via {@link Supplied} or + * by implementing {@link #target()}. + * + *

Default concrete implementation: {@link Supplied} + */ +public abstract class Pathfind extends SequentialCommandGroup { + + /** + * Concrete {@link Pathfind} implementation that accepts a target pose via Supplier. + * + *

Useful for dynamic targets that may change during the command, or for + * creating {@link Pathfind} instances with a fixed target pose. + */ + public static class Supplied extends Pathfind { + + /** Supplier for the target pose (can be fixed or dynamic). */ + final Supplier target; + + /** + * Constructs {@link Pathfind} with a dynamic target supplier. + * + * @param drive Drive subsystem for movement control + * @param target Supplier providing target pose each execute cycle + */ + public Supplied(Drive drive, Supplier target) { + super(drive); + this.target = target; + } + + /** + * Constructs {@link Pathfind} with a fixed target pose. + * + * @param drive Drive subsystem for movement control + * @param target Fixed target pose for this navigation + */ + public Supplied(Drive drive, Pose2d target) { + this(drive, () -> target); + } + + @Override + protected Pose2d target() { + return target.get(); + } + } + + /** Cached target pose for this navigation. */ + Pose2d cache; + + final Drive drive; + + /** + * Constructs a Navigate command. + * + *

    + *
  1. Calls {@link #target()} to get the target pose and caches it. + *
  2. Logs the target pose to the field for visualization. + *
  3. Runs {@link Repulse} for faraway pathfinding. + *
  4. Runs {@link Nearby} for close-range precision movement. + *
  5. Clears the target pose from the field logs. + *
+ * + * @param drive Drive subsystem for movement control + */ + public Pathfind(Drive drive) { + this.drive = drive; + + addCommands( + new InstantCommand(this::prepare), + new Repulse(drive, this::currentTarget), + new Nearby(drive, this::currentTarget), + new InstantCommand(this::release) + ); + } + + /** + * Gets the target pose for this navigation. + * + *

Subclasses must implement this to provide the target, which may be + * constant or dynamic (e.g., based on vision measurements). + * + * @return Target pose, or null if no valid target + */ + protected abstract Pose2d target(); + + /** + * Caches and logs the target pose + */ + protected final void prepare() { + cache = target(); + + drive.field.getObject("Pathfind/Target").setPose(cache); + Logger.recordOutput("Pathfind/Target", cache); + } + + /** + * Deletes the target pose from the logs + */ + protected final void release() { + Pose2d invalid = new Pose2d(-1, -1, new Rotation2d()); + drive.field.getObject("Pathfind/Target").setPose(invalid); + Logger.recordOutput("Pathfind/Target", invalid); + } + + /** + * Gets the currently cached target pose. + * @return The cached target pose + */ + protected final Pose2d currentTarget() { + return cache; + } +} diff --git a/src/main/java/frc/robot/commands/pathfind/Repulse.java b/src/main/java/frc/robot/commands/pathfind/Repulse.java new file mode 100644 index 0000000..cceddec --- /dev/null +++ b/src/main/java/frc/robot/commands/pathfind/Repulse.java @@ -0,0 +1,177 @@ +package frc.robot.commands.pathfind; + +import choreo.trajectory.SwerveSample; +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 frc.robot.constants.ControlConstants; +import frc.robot.subsystems.drive.Drive; +import frc.robot.util.Repulsor; +import frc.robot.util.Vec2; +import java.util.List; +import java.util.function.Supplier; +import org.littletonrobotics.junction.Logger; + +/** + * Faraway pathfinding command. + * + *

Uses {@link Repulsor} for faraway pathfinding to a target pose. Repulsor works + * by generating a potential field around obstacles and the goal, then using + * the laws of physics to "push" the robot away from obstacles and "pull" it + * toward the goal. + * + *

This command is used as the first step in {@link Pathfind}: far from the target, + * use Repulsor to pathfind around obstacles. When close to the target, switch to + * {@link Nearby} for precision movement. + */ +class Repulse extends Command { + + /** Supplier for the target pose. */ + final Supplier target; + + /** Repulsor instance for pathfinding calculations. */ + final Repulsor repulsor = new Repulsor(); + + final Drive drive; + + /** PID controller for orthogonal movement correction. */ + final PIDController orthoController = new PIDController( + ControlConstants.Auto.kOrthoP, + ControlConstants.Auto.kOrthoI, + ControlConstants.Auto.kOrthoD + ); + + /** PID controller for rotational movement correction. */ + final PIDController rotController = new PIDController( + ControlConstants.Auto.kTurnP, + ControlConstants.Auto.kTurnI, + ControlConstants.Auto.kTurnD + ); + + /** Currently targeted pose, reset every {@link #initialize()} */ + Pose2d currentTarget; + + /** + * Constructor for Repulse command. + * + * @param drive Drive subsystem for motion control + * @param target Supplier providing the target pose + */ + Repulse(Drive drive, Supplier target) { + this.target = target; + this.drive = drive; + + addRequirements(drive); + } + + /** + * Initializes the Repulse command. + * + *

    + *
  1. Gets the current target pose from the supplier. + *
  2. Sets the Repulsor goal to the target translation. + *
  3. Logs the planned trajectory for visualization. + *
+ */ + @Override + public void initialize() { + currentTarget = target.get(); + repulsor.setGoal(currentTarget.getTranslation()); + + List traj = repulsor.getTrajectory( + drive.getPose().getTranslation(), + currentTarget.getTranslation(), + 0.1 // 10cm steps + ); + Logger.recordOutput( + "Pathfind/RepulseTrajectory", + traj.stream().toArray(Translation2d[]::new) + ); + + drive.field + .getObject("Pathfind/RepulseTrajectory") + .setPoses( + traj + .stream() + .map(t -> new Pose2d(t, new Rotation2d())) + .toArray(Pose2d[]::new) + ); + } + + /** + * Runs the Repulse pathfinding logic each cycle. + * + *
    + *
  1. Gets the next SwerveSample from Repulsor based on current pose and velocity. + *
  2. Calculates the distance vector to the target. + *
  3. Computes PID correction power based on distance. + *
  4. Adds PID correction to Repulsor's suggested velocities. + *
  5. Drives the drive with the combined velocities and rotational PID output. + *
+ */ + @Override + public void execute() { + SwerveSample sample = repulsor.getCmd( + drive.getPose(), + drive.getChassisSpeeds(), + ControlConstants.Drive.kMaxLinearSpeed, + true + ); + + Logger.recordOutput("Pathfind/RepulseTarget", sample.getPose()); + drive.field.getObject("Pathfind/RepulseTarget").setPose(currentTarget); + + Vec2 pose = new Vec2(drive.getPose()); + Vec2 target = new Vec2(sample.getPose()); + Vec2 dist = target.sub(pose); + Vec2 pidpower = dist.norm().mul(orthoController.calculate(dist.mag())); + Vec2 result = new Vec2(sample.vx, sample.vy).add(pidpower); + + drive.drive( + new ChassisSpeeds( + result.x, + result.y, + rotController.calculate( + drive.getPose().getRotation().getRadians(), + currentTarget.getRotation().getRadians() + ) + ) + ); + } + + /** + * Cleans up after the Repulse command ends. + * + *

Clears the target and trajectory logs from the field visualization. + */ + @Override + public void end(boolean interrupted) { + Logger.recordOutput( + "Pathfind/RepulseTarget", + new Pose2d(-1, -1, new Rotation2d()) + ); + Logger.recordOutput("Pathfind/RepulseTrajectory", new Translation2d[0]); + + drive.field + .getObject("Pathfind/RepulseTrajectory") + .setPoses(new Pose2d[0]); + } + + /** + * Determines if the Repulse command is finished. + * + *

Finishes when the robot is within the nearby threshold distance of the target. + * See {@link ControlConstants.Auto#kNearbyThreshold}. + */ + @Override + public boolean isFinished() { + Vec2 pose = new Vec2(drive.getPose()); + Vec2 target = new Vec2(currentTarget); + Vec2 dist = target.sub(pose); + + return dist.mag() < ControlConstants.Auto.kNearbyThreshold; + } +} diff --git a/src/main/java/frc/robot/constants/ControlConstants.java b/src/main/java/frc/robot/constants/ControlConstants.java new file mode 100644 index 0000000..123f829 --- /dev/null +++ b/src/main/java/frc/robot/constants/ControlConstants.java @@ -0,0 +1,117 @@ +package frc.robot.constants; + +import static frc.robot.util.MathPlus.kTau; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; + +/** + * Control loop constants for robot motion control. + * + *

Contains PID gains, feedforward coefficients, and control limits for: + *

+ */ +public final class ControlConstants { + + private ControlConstants() {} + + /** + * Autonomous trajectory following PID constants. + * + *

Used by {@link frc.robot.autos.AutoManager} for Choreo trajectory following. + */ + public static final class Auto { + + private Auto() {} + + /** Proportional gain for X and Y position errors (orthogonal axes). */ + public static final double kOrthoP = 1.25; + /** Integral gain for orthogonal position errors. */ + public static final double kOrthoI = 0.00; + /** Derivative gain for orthogonal position errors. */ + public static final double kOrthoD = 0.00; + + /** Proportional gain for rotation error. */ + public static final double kTurnP = 0.00; + /** Integral gain for rotation error. */ + public static final double kTurnI = 0.00; + /** Derivative gain for rotation error. */ + public static final double kTurnD = 0.00; + + /** Threshold to switch to nearby control. */ + public static final double kNearbyThreshold = 0.25; + } + + /** + * Vision-based alignment PID constants. + * + *

Used by {@link frc.robot.commands.pathfind.Nearby} for fine alignment + */ + public static final class Nearby { + + private Nearby() {} + + /** Proportional gain for X and Y alignment errors. */ + public static final double kOrthoP = 4.75; + /** Integral gain for alignment position errors. */ + public static final double kOrthoI = 0.00; + /** Derivative gain for alignment position errors. */ + public static final double kOrthoD = 0.00; + + /** Proportional gain for alignment rotation error. */ + public static final double kTurnP = 2.60; + /** Integral gain for alignment rotation error. */ + public static final double kTurnI = 0.00; + /** Derivative gain for alignment rotation error. */ + public static final double kTurnD = 0.04; + + /** Maximum angular velocity during alignment (radians/second). TODO: May be increased. */ + public static final double kMaxTurnVelocity = kTau; + /** Maximum angular acceleration during alignment (radians/second²). */ + public static final double kMaxTurnAcceleration = kTau; + + /** Position and rotation tolerance for "at target" detection. TODO: May need adjustment for shooter vs PnP. */ + public static final Pose2d kTolerance = new Pose2d( + 0.03, + 0.03, + new Rotation2d(0.05) + ); + } + + /** + * Drivetrain motion constraints. + */ + public static final class Drive { + + private Drive() {} + + /** Maximum translational speed (meters/second). */ + public static final double kMaxLinearSpeed = 5.45; + + /** Maximum angular speed (radians/second). */ + public static final double kMaxAngularSpeed = + kMaxLinearSpeed / RobotConstants.DriveBase.kDriveRadius; + } + + /** + * Slew rate limiters for driver input smoothing. + * + *

Limits acceleration of joystick commands to prevent abrupt movements + * and reduce mechanical stress on the drivetrain. + */ + public static final class SlewRateLimit { + + private SlewRateLimit() {} + + /** Maximum translational acceleration (meters/second²). TODO: Can be increased (less top-heavy than previous design). */ + public static final double kOrthogonal = 1.667; + /** Maximum rotational acceleration (radians/second²). */ + public static final double kRotation = 3.87; + } +} diff --git a/src/main/java/frc/robot/constants/GameConstants.java b/src/main/java/frc/robot/constants/GameConstants.java new file mode 100644 index 0000000..ecbe148 --- /dev/null +++ b/src/main/java/frc/robot/constants/GameConstants.java @@ -0,0 +1,44 @@ +package frc.robot.constants; + +import static edu.wpi.first.units.Units.Inches; +import static edu.wpi.first.units.Units.Meters; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; + +/** + * FRC 2026 game-specific field dimensions and reference points. + * + *

Contains field geometry constants derived from the official game manual. + * All dimensions are converted from inches to meters. + * + *

These values define the coordinate system used for autonomous navigation + * and alliance-aware positioning. + * + *

Sources: + *

+ */ +@SuppressWarnings("SuspiciousNameCombination") +public class GameConstants { + + private GameConstants() {} + + /** Field width (short dimension) */ + public static final double kFieldWidth = Inches.of(317.69).in(Meters); + + /** Field length (long dimension) */ + public static final double kFieldLength = Inches.of(651.22).in(Meters); + + /** + * Red alliance coordinate system origin. + * + *

Located at the far corner of the field with {@code 180°} rotation. + */ + public static final Pose2d kRedOrigin = new Pose2d( + kFieldLength, + kFieldWidth, + Rotation2d.kPi + ); +} diff --git a/src/main/java/frc/robot/constants/MotorConstants.java b/src/main/java/frc/robot/constants/MotorConstants.java new file mode 100644 index 0000000..f496b44 --- /dev/null +++ b/src/main/java/frc/robot/constants/MotorConstants.java @@ -0,0 +1,118 @@ +package frc.robot.constants; + +import static edu.wpi.first.units.Units.RPM; +import static edu.wpi.first.units.Units.RadiansPerSecond; + +/** + * REV Robotics brushless motor specifications. + * + *

Contains empirically measured free speeds and recommended current limits + * for each motor type used on the robot. Free speeds are converted from RPM + * to radians/second for kinematic calculations. + * + *

Specification sources: + *

+ */ +public final class MotorConstants { + + private MotorConstants() {} + + /** + * NEO brushless motor specifications (REV-21-1650). + * + *

Used for swerve module drive motors. + */ + public static final class Neo { + + private Neo() {} + + /** + * Empirical free speed: 5676 RPM (594.7 rad/s). + * + * @see Datasheet + */ + public static final double kFreeSpeed = RPM.of(5676.0).in( + RadiansPerSecond + ); + + /** Recommended current limit: 40 amps. */ + public static final double kCurrentLimit = 40; + + /** + * PWM modulation period for UVW commutation: 10ms. + * + *

Controls the switching frequency of the motor controller for brushless motor commutation. + * Lower periods result in higher commutation frequencies and smoother motor operation. + */ + public static final int kUvwPeriod = 10; + + /** + * UVW pulse depth or dead-time setting: 2. + * + *

Defines the dead-time interval between switching commutation phases to prevent + * shoot-through and ensure safe transistor switching in the motor controller. + */ + public static final int kUvwDepth = 2; + } + + /** + * NEO Vortex brushless motor specifications (REV-21-1652). + * + *

Higher power motor for demanding applications. + */ + public static final class NeoVortex { + + private NeoVortex() {} + + /** + * Free speed: 6784 RPM (710.3 rad/s). + * + * @see Datasheet + */ + public static final double kFreeSpeed = RPM.of(6784.0).in( + RadiansPerSecond + ); + + /** Recommended current limit: 60 amps. */ + public static final double kCurrentLimit = 60; + } + + /** + * NEO 550 brushless motor specifications (REV-21-1651). + * + *

Compact, high-speed motor used for swerve module steering. + */ + public static final class Neo550 { + + private Neo550() {} + + /** + * Free speed: 11000 RPM (1151.9 rad/s). + * + * @see Datasheet + */ + public static final double kFreeSpeed = RPM.of(11000.0).in( + RadiansPerSecond + ); + + /** Recommended current limit: 20 amps. */ + public static final double kCurrentLimit = 20; + + /** + * UVW pulse depth or dead-time setting: 2. + * + *

Defines the dead-time interval between switching commutation phases to prevent + * shoot-through and ensure safe transistor switching in the motor controller. + * + * This number came from the template code + */ + public static final int kUvwDepth = 2; + } + + /** Nominal battery voltage for motor control (12.0 volts). */ + public static final double kNominalVoltage = 12.0; +} \ No newline at end of file diff --git a/src/main/java/frc/robot/constants/RobotConstants.java b/src/main/java/frc/robot/constants/RobotConstants.java new file mode 100644 index 0000000..6e54271 --- /dev/null +++ b/src/main/java/frc/robot/constants/RobotConstants.java @@ -0,0 +1,85 @@ +package frc.robot.constants; + +import static edu.wpi.first.units.Units.Inches; +import static edu.wpi.first.units.Units.Meters; + +import edu.wpi.first.math.util.Units; + +/** + * Global robot physical properties and dimensions. + * TODO: Update values based on actual measurements and Ross + */ +public class RobotConstants { + + private RobotConstants() {} + + /** Robot mass including battery and bumpers. TODO: measure */ + public static final double kMass = 68.023; + + /** + * Robot moment of inertia about vertical axis. + * TODO: Ross will calculate this + */ + public static final double kMoI = 4.235; + + /** + * Swerve drive base geometric dimensions. + * + *

Defines wheelbase, track width, bumper extents, and radii for + * kinematics calculations and obstacle clearance checks. + */ + public static final class DriveBase { + + private DriveBase() {} + + /** + * Wheel base: front-to-back distance between modules (meters). + * @see #kBumperLength for front-to-back outer dimension + */ + public static final double kWheelBase = Units.inchesToMeters(26.5); + + /** + * Track width: side-to-side distance between modules (meters). + * @see #kBumperWidth for side-to-side outer dimension + */ + public static final double kTrackWidth = Units.inchesToMeters(26.5); + + /** + * Bumper width: side-to-side outer dimension + * @see #kTrackWidth for distance between wheels in the same axis + */ + public static final double kBumperWidth = Inches.of(36.5).in(Meters); + + /** + * Bumper length: front-to-back outer dimension + * @see #kWheelBase for distance between wheels in the same axis + */ + public static final double kBumperLength = Inches.of(36.5).in(Meters); + + /** + * Radius of circle bumpers fit within. + */ + public static final double kBumperRadius = + 0.5 * Math.hypot(kBumperLength, kBumperWidth); + + /** + * Drive base radius (module diagonal half-distance). + */ + public static final double kDriveRadius = + 0.5 * Math.hypot(kWheelBase, kTrackWidth); + } + + /** + * Wheel physical properties. + */ + public static final class Wheel { + + private Wheel() {} + + /** Wheel radius in meters. */ + public static final double kRadius = Units.inchesToMeters(1.5); + + /** Coefficient of friction between wheel and carpet. */ + public static final double kFriction = 1.3; + } +} diff --git a/src/main/java/frc/robot/constants/VisionConstants.java b/src/main/java/frc/robot/constants/VisionConstants.java index ec02d78..3ecd0e4 100644 --- a/src/main/java/frc/robot/constants/VisionConstants.java +++ b/src/main/java/frc/robot/constants/VisionConstants.java @@ -2,6 +2,7 @@ import static edu.wpi.first.units.Units.Centimeters; import static edu.wpi.first.units.Units.Degrees; +import static edu.wpi.first.units.Units.Inches; import static edu.wpi.first.units.Units.Meters; import static edu.wpi.first.units.Units.Radians; @@ -9,36 +10,43 @@ import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Transform3d; +import edu.wpi.first.math.geometry.Translation3d; import edu.wpi.first.math.numbers.N1; import edu.wpi.first.math.numbers.N3; /** * Vision system constants for AprilTag-based localization. * - *

Contains camera configurations, measurement uncertainty parameters, + *

+ * Contains camera configurations, measurement uncertainty parameters, * and field layout definitions for both PhotonVision and Quest Pro cameras. * - *

Used by {@link frc.robot.subsystems.photon.Photon} and + *

+ * Used by {@link frc.robot.subsystems.photon.Photon} and * {@link frc.robot.subsystems.quest.Quest} for pose estimation. */ public final class VisionConstants { - private VisionConstants() {} + private VisionConstants() { + } /** * Camera hardware configurations. * - *

Each camera requires a name (for NetworkTables) and transform + *

+ * Each camera requires a name (for NetworkTables) and transform * from robot center to camera optical frame. */ public enum CameraConfig { /** * Primary AprilTag camera. * - *

TODO: Define actual {@link Transform3d} from robot center to camera. + *

+ * TODO: Define actual {@link Transform3d} from robot center to camera. */ - kCamera("camera", new Transform3d()); + kCamera("camera", new Transform3d(), false); /** Camera name for PhotonVision NetworkTables. */ public final String name; @@ -46,44 +54,54 @@ public enum CameraConfig { /** Transform from robot center to camera (robot coords → camera coords). */ public final Transform3d bot2cam; - CameraConfig(String name, Transform3d bot2cam) { + /** The camera may be used as a dashboard stream, skipping vision use (AKA: driver mode) */ + public final boolean passthrough; + + CameraConfig(String name, Transform3d bot2cam, boolean passthrough) { this.name = name; this.bot2cam = bot2cam; + this.passthrough = passthrough; } } /** * Transform from robot center to Quest Pro headset. * - *

TODO: Measure and configure actual transform. + *

+ * TODO: Measure and configure actual transform. */ - public static Transform3d kBotToQuest = new Transform3d(); + public static Transform3d kBotToQuest = new Transform3d( + new Translation3d(0, Inches.of(12).in(Meters), 0), + new Rotation3d(0, 0, Math.PI / 2)); /** * Quest Pro measurement standard deviations. * - *

Uncertainty in X, Y (2 cm each) and rotation (0.035 degrees). + *

+ * Uncertainty in X, Y (2 cm each) and rotation (0.035 degrees). * Lower values = more trust in Quest measurements. */ public static Matrix kQuestDevs = VecBuilder.fill( - Centimeters.of(2).in(Meters), - Centimeters.of(2).in(Meters), - Degrees.of(0.035).in(Radians) - ); + Centimeters.of(2).in(Meters), + Centimeters.of(2).in(Meters), + Degrees.of(0.035).in(Radians)); /** * Vision alignment constraints. * - *

Limits vision-based control commands to reasonable ranges. + *

+ * Limits vision-based control commands to reasonable ranges. */ public static final class Align { - private Align() {} + private Align() { + } /** * Maximum distance for vision-based alignment (1.5 meters). * - *

Beyond this range, AprilTag detection becomes unreliable. + *

+ * Beyond this range, AprilTag detection becomes unreliable. */ public static final double kMaxDistance = 1.50; } @@ -91,61 +109,66 @@ private Align() {} /** * AprilTag field layout configuration. * - *

Defines tag positions and IDs for the game field. + *

+ * Defines tag positions and IDs for the game field. */ public static final class AprilTag { - private AprilTag() {} + private AprilTag() { + } /** * Field layout for 2025 Reefscape (Welded variant). * - *

TODO: Update for 2026 field when released. + *

+ * TODO: Update for 2026 field when released. */ - public static final AprilTagFieldLayout kLayout = - AprilTagFieldLayout.loadField(AprilTagFields.k2025ReefscapeWelded); + public static final AprilTagFieldLayout kLayout = AprilTagFieldLayout + .loadField(AprilTagFields.k2025ReefscapeWelded); } /** * Measurement uncertainty standard deviations. * - *

Used by {@link edu.wpi.first.math.estimator.SwerveDrivePoseEstimator} + *

+ * Used by {@link edu.wpi.first.math.estimator.SwerveDrivePoseEstimator} * to weight vision measurements vs. odometry. */ public static final class StdDevs { - private StdDevs() {} + private StdDevs() { + } /** * Single AprilTag measurement uncertainty. * - *

Higher uncertainty (4m, 4m, 8 rad) due to ambiguity with one tag. + *

+ * Higher uncertainty (4m, 4m, 8 rad) due to ambiguity with one tag. *

*/ public static final Matrix kSingleTag = VecBuilder.fill( - 4, - 4, - 8 - ); + 4, + 4, + 8); /** * Multiple AprilTag measurement uncertainty. * - *

Lower uncertainty (0.5m, 0.5m, 1 rad) with multiple tags visible. + *

+ * Lower uncertainty (0.5m, 0.5m, 1 rad) with multiple tags visible. *

*/ public static final Matrix kMultiTag = VecBuilder.fill( - 0.5, - 0.5, - 1 - ); + 0.5, + 0.5, + 1); } } diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java index a0dddc0..1ac43a7 100644 --- a/src/main/java/frc/robot/subsystems/drive/Drive.java +++ b/src/main/java/frc/robot/subsystems/drive/Drive.java @@ -24,16 +24,24 @@ import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.Timer; +import edu.wpi.first.wpilibj.smartdashboard.Field2d; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.Constants; import frc.robot.Constants.Mode; +import frc.robot.subsystems.photon.CameraPhoton; +import frc.robot.subsystems.photon.CameraReplay; +import frc.robot.subsystems.photon.Photon; import frc.robot.subsystems.quest.Meta3S; import frc.robot.subsystems.quest.Quest; import frc.robot.subsystems.quest.QuestReplay; +import frc.robot.util.Vec2; import frc.robot.util.VisionMeasurement; - +import java.util.ArrayList; +import java.util.List; +import java.util.Optional; import java.util.concurrent.locks.Lock; import java.util.concurrent.locks.ReentrantLock; import org.littletonrobotics.junction.AutoLogOutput; @@ -41,295 +49,407 @@ public class Drive extends SubsystemBase { - static final Lock odometryLock = new ReentrantLock(); - private final GyroIO gyroIO; - private final GyroIOInputsAutoLogged gyroInputs = - new GyroIOInputsAutoLogged(); - private final Module[] modules = new Module[4]; // FL, FR, BL, BR - private final SysIdRoutine sysId; - private final Alert gyroDisconnectedAlert = new Alert( - "Disconnected gyro, using kinematics as fallback.", - AlertType.kError - ); - - private SwerveDriveKinematics kinematics = new SwerveDriveKinematics( - moduleTranslations - ); - private Rotation2d rawGyroRotation = Rotation2d.kZero; - private SwerveModulePosition[] lastModulePositions = // For delta tracking - new SwerveModulePosition[] { - new SwerveModulePosition(), - new SwerveModulePosition(), - new SwerveModulePosition(), - new SwerveModulePosition(), - }; - private SwerveDrivePoseEstimator poseEstimator = new SwerveDrivePoseEstimator( - kinematics, - rawGyroRotation, - lastModulePositions, - Pose2d.kZero - ); - - /** the questnav used for primary vision things */ - final Quest quest; - - public Drive( - GyroIO gyroIO, - ModuleIO flModuleIO, - ModuleIO frModuleIO, - ModuleIO blModuleIO, - ModuleIO brModuleIO - ) { - this.quest = new Quest( - switch (Constants.currentMode) { - case REAL -> new Meta3S(); - case REPLAY -> new QuestReplay(); - case SIM -> new QuestReplay(); - }, - this::addVisionMeasurement + static final Lock odometryLock = new ReentrantLock(); + private final GyroIO gyroIO; + private final GyroIOInputsAutoLogged gyroInputs = + new GyroIOInputsAutoLogged(); + private final Module[] modules = new Module[4]; // FL, FR, BL, BR + private final SysIdRoutine sysId; + private final Alert gyroDisconnectedAlert = new Alert( + "Disconnected gyro, using kinematics as fallback.", + AlertType.kError ); - this.gyroIO = gyroIO; - modules[0] = new Module(flModuleIO, 0); - modules[1] = new Module(frModuleIO, 1); - modules[2] = new Module(blModuleIO, 2); - modules[3] = new Module(brModuleIO, 3); + private SwerveDriveKinematics kinematics = new SwerveDriveKinematics( + moduleTranslations + ); + private Rotation2d rawGyroRotation = Rotation2d.kZero; + private SwerveModulePosition[] lastModulePositions = // For delta tracking + new SwerveModulePosition[] { + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition(), + new SwerveModulePosition(), + }; + private SwerveDrivePoseEstimator poseEstimator = + new SwerveDrivePoseEstimator( + kinematics, + rawGyroRotation, + lastModulePositions, + Pose2d.kZero + ); - // Usage reporting for swerve template - HAL.report( - tResourceType.kResourceType_RobotDrive, - tInstances.kRobotDriveSwerve_AdvantageKit + /** the QuestNav used for primary vision things */ + final Quest quest = new Quest( + switch (Constants.currentMode) { + case REAL -> new Meta3S(); + case REPLAY -> new QuestReplay(); + case SIM -> new QuestReplay(); + }, + this::addVisionMeasurement ); - // Start odometry thread - SparkOdometryThread.getInstance().start(); - - // Configure SysId - sysId = new SysIdRoutine( - new SysIdRoutine.Config(null, null, null, state -> - Logger.recordOutput("Drive/SysIdState", state.toString()) - ), - new SysIdRoutine.Mechanism( - voltage -> runCharacterization(voltage.in(Volts)), - null, - this - ) + /** Field widget */ + public final Field2d field = new Field2d(); + + /** vision measurements collected before robot start */ + final List calibrators = new ArrayList<>(); + + /** PhotonVision, used to calibrate starting position */ + final Photon photon = new Photon( + switch (Constants.currentMode) { + case REAL -> CameraPhoton::new; + case REPLAY -> CameraReplay::new; + case SIM -> CameraReplay::new; + }, + this.calibrators::add ); - } - - @Override - public void periodic() { - odometryLock.lock(); // Prevents odometry updates while reading data - gyroIO.updateInputs(gyroInputs); - Logger.processInputs("Drive/Gyro", gyroInputs); - for (var module : modules) { - module.periodic(); + + public Drive( + GyroIO gyroIO, + ModuleIO flModuleIO, + ModuleIO frModuleIO, + ModuleIO blModuleIO, + ModuleIO brModuleIO + ) { + this.gyroIO = gyroIO; + modules[0] = new Module(flModuleIO, 0); + modules[1] = new Module(frModuleIO, 1); + modules[2] = new Module(blModuleIO, 2); + modules[3] = new Module(brModuleIO, 3); + + // Usage reporting for swerve template + HAL.report( + tResourceType.kResourceType_RobotDrive, + tInstances.kRobotDriveSwerve_AdvantageKit + ); + + // Start odometry thread + SparkOdometryThread.getInstance().start(); + + // Configure SysId + sysId = new SysIdRoutine( + new SysIdRoutine.Config(null, null, null, state -> + Logger.recordOutput("Drive/SysIdState", state.toString()) + ), + new SysIdRoutine.Mechanism( + voltage -> runCharacterization(voltage.in(Volts)), + null, + this + ) + ); } - odometryLock.unlock(); - // Stop moving when disabled - if (DriverStation.isDisabled()) { - for (var module : modules) { - module.stop(); - } + @Override + public void periodic() { + // Collect updates when disabled; calibrate once enabled. + // although calibrate() is called many times, it only acts once + // since we clear the calibrators list at the end of the method. + if (DriverStation.isDisabled()) photon.update(); + else calibrate(); + quest.update(); + + // If the quest has disconnected, fallback on photon for vision + if (!quest.data().connected && DriverStation.isEnabled()) { + photon.update(); + this.calibrators.stream().forEach(this::addVisionMeasurement); + this.calibrators.clear(); + } + + odometryLock.lock(); // Prevents odometry updates while reading data + try { + gyroIO.updateInputs(gyroInputs); + Logger.processInputs("Drive/Gyro", gyroInputs); + for (var module : modules) { + module.periodic(); + } + } finally { + odometryLock.unlock(); + } + + // Stop moving when disabled + if (DriverStation.isDisabled()) { + for (var module : modules) { + module.stop(); + } + } + + // Log empty setpoint states when disabled + if (DriverStation.isDisabled()) { + Logger.recordOutput( + "SwerveStates/Setpoints", + new SwerveModuleState[] {} + ); + Logger.recordOutput( + "SwerveStates/SetpointsOptimized", + new SwerveModuleState[] {} + ); + } + + // Update odometry + double[] sampleTimestamps = modules[0].getOdometryTimestamps(); // All signals are sampled together + int sampleCount = sampleTimestamps.length; + for (int i = 0; i < sampleCount; i++) { + // Read wheel positions and deltas from each module + SwerveModulePosition[] modulePositions = + new SwerveModulePosition[4]; + SwerveModulePosition[] moduleDeltas = new SwerveModulePosition[4]; + for (int moduleIndex = 0; moduleIndex < 4; moduleIndex++) { + modulePositions[moduleIndex] = + modules[moduleIndex].getOdometryPositions()[i]; + moduleDeltas[moduleIndex] = new SwerveModulePosition( + modulePositions[moduleIndex].distanceMeters - + lastModulePositions[moduleIndex].distanceMeters, + modulePositions[moduleIndex].angle + ); + lastModulePositions[moduleIndex] = modulePositions[moduleIndex]; + } + + Optional questHeading = quest.headingAt( + sampleTimestamps[i] + ); + + // Update gyro angle + if (gyroInputs.connected) { + // Use the real gyro angle + rawGyroRotation = gyroInputs.odometryYawPositions[i]; + } else if (questHeading.isPresent()) { + // Use the Quest heading if available + rawGyroRotation = questHeading.get(); + } else { + // Use the angle delta from the kinematics and module deltas + Twist2d twist = kinematics.toTwist2d(moduleDeltas); + rawGyroRotation = rawGyroRotation.plus( + new Rotation2d(twist.dtheta) + ); + } + + // Apply update + poseEstimator.updateWithTime( + sampleTimestamps[i], + rawGyroRotation, + modulePositions + ); + } + + // Update gyro alert + gyroDisconnectedAlert.set( + !gyroInputs.connected && Constants.currentMode != Mode.SIM + ); + + field.setRobotPose(this.getPose()); } - // Log empty setpoint states when disabled - if (DriverStation.isDisabled()) { - Logger.recordOutput("SwerveStates/Setpoints", new SwerveModuleState[] {}); - Logger.recordOutput( - "SwerveStates/SetpointsOptimized", - new SwerveModuleState[] {} - ); + /** + * Runs the drive at the desired robot-relative velocity. + * + * @param speeds Speeds in meters/sec + * @see #drive(ChassisSpeeds) drive() for field-relative driving + */ + public void runVelocity(ChassisSpeeds speeds) { + Vec2 velocity = new Vec2( + speeds.vxMetersPerSecond, + speeds.vyMetersPerSecond + ); + + // Limit to maximum linear speed, if necessary + // Can't limit components: <100% vx, 100%vy> = sqrt(2)*100% total speed + if (velocity.mag() > maxSpeedMetersPerSec) { + velocity = velocity.norm().mul(maxSpeedMetersPerSec); + speeds.vxMetersPerSecond = velocity.x; + speeds.vyMetersPerSecond = velocity.y; + } + + if (speeds.omegaRadiansPerSecond > maxAngularSpeedRadPerSec) { + speeds.omegaRadiansPerSecond = maxAngularSpeedRadPerSec; + } + + // Calculate module setpoints + ChassisSpeeds discreteSpeeds = ChassisSpeeds.discretize(speeds, 0.02); + SwerveModuleState[] setpointStates = kinematics.toSwerveModuleStates( + discreteSpeeds + ); + SwerveDriveKinematics.desaturateWheelSpeeds( + setpointStates, + maxSpeedMetersPerSec + ); + + // Log unoptimized setpoints + Logger.recordOutput("SwerveStates/Setpoints", setpointStates); + Logger.recordOutput("SwerveChassisSpeeds/Setpoints", discreteSpeeds); + + // Send setpoints to modules + for (int i = 0; i < 4; i++) { + modules[i].runSetpoint(setpointStates[i]); + } + + // Log optimized setpoints (runSetpoint mutates each state) + Logger.recordOutput("SwerveStates/SetpointsOptimized", setpointStates); } - // Update odometry - double[] sampleTimestamps = modules[0].getOdometryTimestamps(); // All signals are sampled together - int sampleCount = sampleTimestamps.length; - for (int i = 0; i < sampleCount; i++) { - // Read wheel positions and deltas from each module - SwerveModulePosition[] modulePositions = new SwerveModulePosition[4]; - SwerveModulePosition[] moduleDeltas = new SwerveModulePosition[4]; - for (int moduleIndex = 0; moduleIndex < 4; moduleIndex++) { - modulePositions[moduleIndex] = - modules[moduleIndex].getOdometryPositions()[i]; - moduleDeltas[moduleIndex] = new SwerveModulePosition( - modulePositions[moduleIndex].distanceMeters - - lastModulePositions[moduleIndex].distanceMeters, - modulePositions[moduleIndex].angle + /** + * Commands swerve drive, field-relative with specified speeds. + * + * @param speeds The desired chassis speeds + * @see #runVelocity(ChassisSpeeds) runVelocity() for robot-relative driving + */ + public void drive(ChassisSpeeds speeds) { + runVelocity( + ChassisSpeeds.fromFieldRelativeSpeeds(speeds, getRotation()) ); - lastModulePositions[moduleIndex] = modulePositions[moduleIndex]; - } - - // Update gyro angle - if (gyroInputs.connected) { - // Use the real gyro angle - rawGyroRotation = gyroInputs.odometryYawPositions[i]; - } else { - // Use the angle delta from the kinematics and module deltas - Twist2d twist = kinematics.toTwist2d(moduleDeltas); - rawGyroRotation = rawGyroRotation.plus(new Rotation2d(twist.dtheta)); - } - - // Apply update - poseEstimator.updateWithTime( - sampleTimestamps[i], - rawGyroRotation, - modulePositions - ); } - // Update gyro alert - gyroDisconnectedAlert.set( - !gyroInputs.connected && Constants.currentMode != Mode.SIM - ); - } - - /** - * Runs the drive at the desired velocity. - * - * @param speeds Speeds in meters/sec - */ - public void runVelocity(ChassisSpeeds speeds) { - // Calculate module setpoints - ChassisSpeeds discreteSpeeds = ChassisSpeeds.discretize(speeds, 0.02); - SwerveModuleState[] setpointStates = kinematics.toSwerveModuleStates( - discreteSpeeds - ); - SwerveDriveKinematics.desaturateWheelSpeeds( - setpointStates, - maxSpeedMetersPerSec - ); + /** Runs the drive in a straight line with the specified drive output. */ + public void runCharacterization(double output) { + for (int i = 0; i < 4; i++) { + modules[i].runCharacterization(output); + } + } - // Log unoptimized setpoints - Logger.recordOutput("SwerveStates/Setpoints", setpointStates); - Logger.recordOutput("SwerveChassisSpeeds/Setpoints", discreteSpeeds); + /** Stops the drive. */ + public void stop() { + runVelocity(new ChassisSpeeds()); + } - // Send setpoints to modules - for (int i = 0; i < 4; i++) { - modules[i].runSetpoint(setpointStates[i]); + /** + * Stops the drive and turns the modules to an X arrangement to resist movement. The modules will + * return to their normal orientations the next time a nonzero velocity is requested. + */ + public void stopWithX() { + Rotation2d[] headings = new Rotation2d[4]; + for (int i = 0; i < 4; i++) { + headings[i] = moduleTranslations[i].getAngle(); + } + kinematics.resetHeadings(headings); + stop(); } - // Log optimized setpoints (runSetpoint mutates each state) - Logger.recordOutput("SwerveStates/SetpointsOptimized", setpointStates); - } + /** Returns a command to run a quasistatic test in the specified direction. */ + public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { + return run(() -> runCharacterization(0.0)) + .withTimeout(1.0) + .andThen(sysId.quasistatic(direction)); + } - /** Runs the drive in a straight line with the specified drive output. */ - public void runCharacterization(double output) { - for (int i = 0; i < 4; i++) { - modules[i].runCharacterization(output); + /** Returns a command to run a dynamic test in the specified direction. */ + public Command sysIdDynamic(SysIdRoutine.Direction direction) { + return run(() -> runCharacterization(0.0)) + .withTimeout(1.0) + .andThen(sysId.dynamic(direction)); } - } - - /** Stops the drive. */ - public void stop() { - runVelocity(new ChassisSpeeds()); - } - - /** - * Stops the drive and turns the modules to an X arrangement to resist movement. The modules will - * return to their normal orientations the next time a nonzero velocity is requested. - */ - public void stopWithX() { - Rotation2d[] headings = new Rotation2d[4]; - for (int i = 0; i < 4; i++) { - headings[i] = moduleTranslations[i].getAngle(); + + /** Returns the module states (turn angles and drive velocities) for all of the modules. */ + @AutoLogOutput(key = "SwerveStates/Measured") + private SwerveModuleState[] getModuleStates() { + SwerveModuleState[] states = new SwerveModuleState[4]; + for (int i = 0; i < 4; i++) { + states[i] = modules[i].getState(); + } + return states; } - kinematics.resetHeadings(headings); - stop(); - } - - /** Returns a command to run a quasistatic test in the specified direction. */ - public Command sysIdQuasistatic(SysIdRoutine.Direction direction) { - return run(() -> runCharacterization(0.0)) - .withTimeout(1.0) - .andThen(sysId.quasistatic(direction)); - } - - /** Returns a command to run a dynamic test in the specified direction. */ - public Command sysIdDynamic(SysIdRoutine.Direction direction) { - return run(() -> runCharacterization(0.0)) - .withTimeout(1.0) - .andThen(sysId.dynamic(direction)); - } - - /** Returns the module states (turn angles and drive velocities) for all of the modules. */ - @AutoLogOutput(key = "SwerveStates/Measured") - private SwerveModuleState[] getModuleStates() { - SwerveModuleState[] states = new SwerveModuleState[4]; - for (int i = 0; i < 4; i++) { - states[i] = modules[i].getState(); + + /** Returns the module positions (turn angles and drive positions) for all of the modules. */ + private SwerveModulePosition[] getModulePositions() { + SwerveModulePosition[] states = new SwerveModulePosition[4]; + for (int i = 0; i < 4; i++) { + states[i] = modules[i].getPosition(); + } + return states; } - return states; - } - - /** Returns the module positions (turn angles and drive positions) for all of the modules. */ - private SwerveModulePosition[] getModulePositions() { - SwerveModulePosition[] states = new SwerveModulePosition[4]; - for (int i = 0; i < 4; i++) { - states[i] = modules[i].getPosition(); + + /** Returns the measured chassis speeds of the robot. */ + @AutoLogOutput(key = "SwerveChassisSpeeds/Measured") + public ChassisSpeeds getChassisSpeeds() { + return kinematics.toChassisSpeeds(getModuleStates()); } - return states; - } - - /** Returns the measured chassis speeds of the robot. */ - @AutoLogOutput(key = "SwerveChassisSpeeds/Measured") - private ChassisSpeeds getChassisSpeeds() { - return kinematics.toChassisSpeeds(getModuleStates()); - } - - /** Returns the position of each module in radians. */ - public double[] getWheelRadiusCharacterizationPositions() { - double[] values = new double[4]; - for (int i = 0; i < 4; i++) { - values[i] = modules[i].getWheelRadiusCharacterizationPosition(); + + /** Returns the position of each module in radians. */ + public double[] getWheelRadiusCharacterizationPositions() { + double[] values = new double[4]; + for (int i = 0; i < 4; i++) { + values[i] = modules[i].getWheelRadiusCharacterizationPosition(); + } + return values; } - return values; - } - - /** Returns the average velocity of the modules in rad/sec. */ - public double getFFCharacterizationVelocity() { - double output = 0.0; - for (int i = 0; i < 4; i++) { - output += modules[i].getFFCharacterizationVelocity() / 4.0; + + /** Returns the average velocity of the modules in rad/sec. */ + public double getFFCharacterizationVelocity() { + double output = 0.0; + for (int i = 0; i < 4; i++) { + output += modules[i].getFFCharacterizationVelocity() / 4.0; + } + return output; } - return output; - } - - /** Returns the current odometry pose. */ - @AutoLogOutput(key = "Odometry/Robot") - public Pose2d getPose() { - return poseEstimator.getEstimatedPosition(); - } - - /** Returns the current odometry rotation. */ - public Rotation2d getRotation() { - return getPose().getRotation(); - } - - /** Resets the current odometry pose. */ - public void setPose(Pose2d pose) { - poseEstimator.resetPosition(rawGyroRotation, getModulePositions(), pose); - } - - /** Adds a new timestamped vision measurement. */ - public void addVisionMeasurement( - VisionMeasurement measurement - ) { - poseEstimator.addVisionMeasurement( - measurement.estimate2(), - measurement.timestamp(), - measurement.stdDevs() - ); - } - /** Returns the maximum linear speed in meters per sec. */ - public double getMaxLinearSpeedMetersPerSec() { - return maxSpeedMetersPerSec; - } + /** Returns the current odometry pose. */ + @AutoLogOutput(key = "Odometry/Robot") + public Pose2d getPose() { + return poseEstimator.getEstimatedPosition(); + } + + /** Returns the current odometry rotation. */ + public Rotation2d getRotation() { + return getPose().getRotation(); + } + + /** Resets the current odometry pose. */ + public void setPose(Pose2d pose) { + poseEstimator.resetPosition( + rawGyroRotation, + getModulePositions(), + pose + ); + } - /** Returns the maximum angular speed in radians per sec. */ - public double getMaxAngularSpeedRadPerSec() { - return maxSpeedMetersPerSec / driveBaseRadius; - } + /** Adds a new timestamped vision measurement. */ + public void addVisionMeasurement(VisionMeasurement measurement) { + poseEstimator.addVisionMeasurement( + measurement.estimate2(), + measurement.timestamp(), + measurement.stdDevs() + ); + } + + /** + * Calibrate the initial pose of the robot + * + * Photon/AprilTag measurements are recorded and collected while the robot is disabled. + * Once enabled, we average the latest few measurements and set our starting pose to that. + */ + private void calibrate() { + if (calibrators.isEmpty()) return; + + // Average all calibrator measurements + double x = 0; + double y = 0; + double sin = 0; + double cos = 0; + int n = 0; + + for (VisionMeasurement vm : calibrators) { + double age = Timer.getFPGATimestamp() - vm.timestamp(); + if (age > 15) continue; // Ignore old measurements + + Pose2d est = vm.estimate2(); + + x += est.getX(); + y += est.getY(); + sin += est.getRotation().getSin(); + cos += est.getRotation().getCos(); + n++; + } + + x /= n; + y /= n; + sin /= n; + cos /= n; + + Pose2d avg = new Pose2d(x, y, new Rotation2d(cos, sin)); + + // Reset odometry and gyro to the averaged pose + this.setPose(avg); + + // Clear calibrators for next use + calibrators.clear(); + } } diff --git a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java index 2880aa7..20bf0fd 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveConstants.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveConstants.java @@ -10,85 +10,99 @@ import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.math.util.Units; +import frc.robot.constants.ControlConstants; +import frc.robot.constants.RobotConstants; public class DriveConstants { - public static final double maxSpeedMetersPerSec = 5.45; - public static final double odometryFrequency = 100.0; // Hz - public static final double trackWidth = Units.inchesToMeters(26.5); - public static final double wheelBase = Units.inchesToMeters(26.5); - public static final double driveBaseRadius = Math.hypot(trackWidth / 2.0, wheelBase / 2.0); - public static final Translation2d[] moduleTranslations = - new Translation2d[] { - new Translation2d(trackWidth / 2.0, wheelBase / 2.0), - new Translation2d(trackWidth / 2.0, -wheelBase / 2.0), - new Translation2d(-trackWidth / 2.0, wheelBase / 2.0), - new Translation2d(-trackWidth / 2.0, -wheelBase / 2.0) - }; - // Zeroed rotation values for each module, see setup instructions - public static final Rotation2d frontLeftZeroRotation = Rotation2d.fromDegrees(-90); - public static final Rotation2d frontRightZeroRotation = Rotation2d.fromDegrees(0); - public static final Rotation2d backLeftZeroRotation = Rotation2d.fromDegrees(180); - public static final Rotation2d backRightZeroRotation = Rotation2d.fromDegrees(90); + public static final double maxSpeedMetersPerSec = + ControlConstants.Drive.kMaxLinearSpeed; + public static final double odometryFrequency = 100.0; // Hz + public static final double trackWidth = + RobotConstants.DriveBase.kTrackWidth; + public static final double wheelBase = RobotConstants.DriveBase.kWheelBase; + public static final double driveBaseRadius = + RobotConstants.DriveBase.kDriveRadius; + public static final double maxAngularSpeedRadPerSec = + ControlConstants.Drive.kMaxAngularSpeed; + public static final Translation2d[] moduleTranslations = + new Translation2d[] { + new Translation2d(trackWidth / 2.0, wheelBase / 2.0), + new Translation2d(trackWidth / 2.0, -wheelBase / 2.0), + new Translation2d(-trackWidth / 2.0, wheelBase / 2.0), + new Translation2d(-trackWidth / 2.0, -wheelBase / 2.0), + }; - // Device CAN IDs - // public static final int pigeonCanId = 9; + // Zeroed rotation values for each module, see setup instructions + public static final Rotation2d frontLeftZeroRotation = + Rotation2d.fromDegrees(-90); + public static final Rotation2d frontRightZeroRotation = + Rotation2d.fromDegrees(0); + public static final Rotation2d backLeftZeroRotation = + Rotation2d.fromDegrees(180); + public static final Rotation2d backRightZeroRotation = + Rotation2d.fromDegrees(90); - public static final int frontLeftDriveCanId = 10; - public static final int frontRightDriveCanId = 20; - public static final int backLeftDriveCanId = 30; - public static final int backRightDriveCanId = 40; + // Device CAN IDs + // public static final int pigeonCanId = 9; - public static final int frontLeftTurnCanId = 15; - public static final int frontRightTurnCanId = 25; - public static final int backLeftTurnCanId = 35; - public static final int backRightTurnCanId = 46; + public static final int frontLeftDriveCanId = 10; + public static final int frontRightDriveCanId = 20; + public static final int backLeftDriveCanId = 30; + public static final int backRightDriveCanId = 40; - // Drive motor configuration - public static final int driveMotorCurrentLimit = 47;//old 60 - public static final double wheelRadiusMeters = Units.inchesToMeters(3); - public static final double driveMotorReduction = - (45.0 * 22.0) / (14.0 * 15.0); // MAXSwerve with 14 pinion teeth and 22 spur teeth - public static final DCMotor driveGearbox = DCMotor.getNEO(1); + public static final int frontLeftTurnCanId = 15; + public static final int frontRightTurnCanId = 25; + public static final int backLeftTurnCanId = 35; + public static final int backRightTurnCanId = 46; - // Drive encoder configuration - public static final double driveEncoderPositionFactor = - 2 * Math.PI / driveMotorReduction; // Rotor Rotations -> Wheel Radians - public static final double driveEncoderVelocityFactor = - (2 * Math.PI) / 60.0 / driveMotorReduction; // Rotor RPM -> Wheel Rad/Sec + // Drive motor configuration + public static final int driveMotorCurrentLimit = 47; // old 60 + public static final double wheelRadiusMeters = RobotConstants.Wheel.kRadius; + public static final double driveMotorReduction = + (45.0 * 22.0) / (14.0 * 15.0); // MAXSwerve with 14 pinion teeth and + // 22 spur teeth + public static final DCMotor driveGearbox = DCMotor.getNEO(1); - // Drive PID configuration - public static final double driveKp = 0.0; - public static final double driveKd = 0.0; - public static final double driveKs = 0.12319; - public static final double driveKv = 0.09165; - public static final double driveSimP = 0.05; - public static final double driveSimD = 0.0; - public static final double driveSimKs = 0.0; - public static final double driveSimKv = 0.0789; + // Drive encoder configuration + public static final double driveEncoderPositionFactor = + (2 * Math.PI) / driveMotorReduction; // Rotor Rotations -> Wheel + // Radians + public static final double driveEncoderVelocityFactor = + (2 * Math.PI) / 60.0 / driveMotorReduction; // Rotor RPM -> + // Wheel Rad/Sec - // Turn motor configuration - public static final boolean turnInverted = false; - public static final int turnMotorCurrentLimit = 30; - public static final double turnMotorReduction = 9424.0 / 203.0; - public static final DCMotor turnGearbox = DCMotor.getNeo550(1); + // Drive PID configuration + public static final double driveKp = 0.0; + public static final double driveKd = 0.0; + public static final double driveKs = 0.12319; + public static final double driveKv = 0.09165; + public static final double driveSimP = 0.05; + public static final double driveSimD = 0.0; + public static final double driveSimKs = 0.0; + public static final double driveSimKv = 0.0789; - // Turn encoder configuration - public static final boolean turnEncoderInverted = true; - public static final double turnEncoderPositionFactor = 2 * Math.PI; // Rotations -> Radians - public static final double turnEncoderVelocityFactor = (2 * Math.PI) / 60.0; // RPM -> Rad/Sec + // Turn motor configuration + public static final boolean turnInverted = false; + public static final int turnMotorCurrentLimit = 30; + public static final double turnMotorReduction = 9424.0 / 203.0; + public static final DCMotor turnGearbox = DCMotor.getNeo550(1); - // Turn PID configuration - public static final double turnKp = 2.0; - public static final double turnKd = 0.0; - public static final double turnSimP = 8.0; - public static final double turnSimD = 0.0; - public static final double turnPIDMinInput = 0; // Radians - public static final double turnPIDMaxInput = 2 * Math.PI; // Radians + // Turn encoder configuration + public static final boolean turnEncoderInverted = true; + public static final double turnEncoderPositionFactor = 2 * Math.PI; // Rotations -> Radians + public static final double turnEncoderVelocityFactor = (2 * Math.PI) / 60.0; // RPM -> Rad/Sec - // PathPlanner configuration - public static final double robotMassKg = 68.023; - public static final double robotMOI = 6.883; - public static final double wheelCOF = 4.235; + // Turn PID configuration + public static final double turnKp = 2.0; + public static final double turnKd = 0.0; + public static final double turnSimP = 8.0; + public static final double turnSimD = 0.0; + public static final double turnPIDMinInput = 0; // Radians + public static final double turnPIDMaxInput = 2 * Math.PI; // Radians + + // PathPlanner configuration + public static final double robotMassKg = RobotConstants.kMass; + public static final double robotMOI = RobotConstants.kMoI; + public static final double wheelCOF = RobotConstants.Wheel.kFriction; } diff --git a/src/main/java/frc/robot/subsystems/photon/CameraIO.java b/src/main/java/frc/robot/subsystems/photon/CameraIO.java new file mode 100644 index 0000000..4f7dcad --- /dev/null +++ b/src/main/java/frc/robot/subsystems/photon/CameraIO.java @@ -0,0 +1,159 @@ +package frc.robot.subsystems.photon; + +import edu.wpi.first.math.Matrix; +import edu.wpi.first.math.VecBuilder; +import edu.wpi.first.math.geometry.Pose3d; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N3; +import frc.robot.constants.VisionConstants; +import frc.robot.util.VisionMeasurement; +import java.util.List; +import java.util.Optional; +import org.photonvision.EstimatedRobotPose; +import org.photonvision.PhotonPoseEstimator; +import org.photonvision.targeting.PhotonPipelineResult; +import org.photonvision.targeting.PhotonTrackedTarget; + +/** + * Abstract base class for vision camera implementations. + * + *

This class provides the core vision processing pipeline: + *

    + *
  • Queries camera results for detected AprilTags + *
  • Uses {@link PhotonPoseEstimator} for multi-tag pose estimation + *
  • Calculates measurement uncertainty based on number and distance of detected tags + *
  • Provides pose measurements to the drivetrain for odometry correction + *
+ */ +public abstract class CameraIO { + + /** Auto-logged data structure for camera vision measurements. */ + public static class CameraData { + + CameraData() {} + + /** Whether or not the camera is connected */ + public boolean connected = false; + + /** The current list of vision results */ + public PhotonPipelineResult[] results = new PhotonPipelineResult[0]; + } + + /** Logged data from this camera. */ + public final CameraData data = new CameraData(); + + /** Configuration for this camera (name, position, etc.). */ + public final VisionConstants.CameraConfig config; + + /** PhotonVision pose estimator for AprilTag-based localization. */ + private final PhotonPoseEstimator estimator; + + /** + * Constructs a camera IO instance. + * + *

Initializes the {@link PhotonPoseEstimator} with the AprilTag field layout and + * multi-tag PnP strategy for robust pose estimation. + * + * @param config Configuration including camera name and robot-to-camera transform + */ + public CameraIO(VisionConstants.CameraConfig config) { + this.config = config; + this.estimator = new PhotonPoseEstimator( + VisionConstants.AprilTag.kLayout, + config.bot2cam + ); + } + + /** + * Sets whether the camera should display driver view or processing view. + * + * @param enabled true for driver view, which exposes a camera feed + */ + public abstract void update(); + + /** + * Computes the {@code VisionMeasurement} of a set of results. + */ + public VisionMeasurement measurementOf(PhotonPipelineResult result) { + Optional estimation = Optional.empty(); + + // Process all available results + estimation = estimator.estimateCoprocMultiTagPose(result); + Matrix stdDevs = stdDevsOf(estimation, result.getTargets()); + + // Store measurement if pose estimation succeeded + if (estimation.isEmpty()) return null; + + EstimatedRobotPose est = estimation.get(); + + return new VisionMeasurement( + est.estimatedPose, + stdDevs, + est.timestampSeconds + ); + } + + /** + * Updates measurement standard deviations based on pose estimation quality. + * + *

Strategy: + *

    + *
  • No estimation: Use single-tag uncertainty (high) + *
  • Multiple tags: Use lower multi-tag uncertainty + *
  • Single tag: Scale uncertainty by distance (farther = less certain) + *
  • Single tag {@literal >} 4m: Mark as completely unreliable + *
+ * + * @param estimation The estimated robot pose, if available + * @param targets The detected targets used for estimation + */ + private final Matrix stdDevsOf( + Optional estimation, + List targets + ) { + if (estimation.isEmpty()) return VisionConstants.StdDevs.kSingleTag; + + Matrix devsEst = VisionConstants.StdDevs.kSingleTag; + int numTags = 0; + double avgDist = 0; + + // Calculate average distance to detected tags + for (PhotonTrackedTarget target : targets) { + Optional tagPose = estimator + .getFieldTags() + .getTagPose(target.getFiducialId()); + + if (tagPose.isEmpty()) continue; + + numTags++; + double dist = tagPose + .get() + .toPose2d() + .getTranslation() + .getDistance( + estimation.get().estimatedPose.toPose2d().getTranslation() + ); + + avgDist += dist; + } + + if (numTags == 0) { + return VisionConstants.StdDevs.kSingleTag; + } + + avgDist /= numTags; + final double untrusted = Double.MAX_VALUE; + + // Use multi-tag devs if we have multiple tags + if (numTags > 1) return VisionConstants.StdDevs.kMultiTag; + + // Single tag detection at long range is unreliable + if (numTags == 1 && avgDist > 4) return VecBuilder.fill( + untrusted, + untrusted, + untrusted + ); + // Scale uncertainty by distance squared + else return devsEst.times(1 + ((avgDist * avgDist) / 30)); + } +} diff --git a/src/main/java/frc/robot/subsystems/photon/CameraPhoton.java b/src/main/java/frc/robot/subsystems/photon/CameraPhoton.java new file mode 100644 index 0000000..edaacc8 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/photon/CameraPhoton.java @@ -0,0 +1,53 @@ +package frc.robot.subsystems.photon; + +import frc.robot.alerter.Alerter; +import frc.robot.constants.VisionConstants; +import org.photonvision.PhotonCamera; +import org.photonvision.targeting.PhotonPipelineResult; + +/** + * Concrete implementation of {@link CameraIO} for PhotonVision-compatible cameras. + * + *

This class interfaces with physical USB or network-connected vision cameras running + * PhotonVision firmware. It handles: + *

    + *
  • Camera initialization and configuration + *
  • Reading pipeline results from the camera + *
  • Setting driver mode for dashboard view + *
+ * + *

Works with any camera supported by PhotonVision, including standard USB webcams. + */ +public class CameraPhoton extends CameraIO { + + /** The PhotonVision camera interface. */ + final PhotonCamera camera; + + /** + * Constructs a PhotonVision camera instance. + * + * Creates a PhotonCamera using the name from the configuration and initializes + * the parent class with pose estimation setup. + * + * @param config Camera configuration including name and robot-to-camera transform + */ + public CameraPhoton(VisionConstants.CameraConfig config) { + super(config); + camera = new PhotonCamera(config.name); + camera.setDriverMode(config.passthrough); + + Alerter.getInstance().register( + String.format("Camera `%s`", config.name()), + camera, + PhotonCamera::isConnected + ); + } + + @Override + public void update() { + data.connected = camera.isConnected(); + data.results = camera + .getAllUnreadResults() + .toArray(PhotonPipelineResult[]::new); + } +} diff --git a/src/main/java/frc/robot/subsystems/photon/CameraReplay.java b/src/main/java/frc/robot/subsystems/photon/CameraReplay.java new file mode 100644 index 0000000..52e595c --- /dev/null +++ b/src/main/java/frc/robot/subsystems/photon/CameraReplay.java @@ -0,0 +1,22 @@ +package frc.robot.subsystems.photon; + +import frc.robot.constants.VisionConstants; + +/** + * Replay implementation of {@link CameraIO} for simulation/log replay. + * + *

This class is used when replaying recorded robot data from AdvantageKit logs. + * It does not interact with any real hardware or simulation - all QuestNav data is + * read directly from the logged {@link CameraIO.CameraData} values. + * + *

All methods are no-ops because all replayed data is immutable. + */ +public class CameraReplay extends CameraIO { + + public CameraReplay(VisionConstants.CameraConfig config) { + super(config); + } + + @Override + public void update() {} +} diff --git a/src/main/java/frc/robot/subsystems/photon/Photon.java b/src/main/java/frc/robot/subsystems/photon/Photon.java new file mode 100644 index 0000000..83e90b1 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/photon/Photon.java @@ -0,0 +1,79 @@ +package frc.robot.subsystems.photon; + +import frc.robot.constants.VisionConstants; +import frc.robot.constants.VisionConstants.CameraConfig; +import frc.robot.util.VisionMeasurement; +import java.util.Arrays; +import java.util.List; +import java.util.function.Consumer; +import java.util.function.Function; +import org.littletonrobotics.junction.Logger; +import org.photonvision.targeting.PhotonPipelineResult; + +/** + * Photon vision subsystem for camera-based pose estimation. + * + *

This class manages multiple vision cameras for detecting AprilTags and calculating + * robot position corrections. Unlike a typical subsystem, this does NOT extend SubsystemBase + * because it should not be used as a command requirement - it operates independently. + * + *

The subsystem: + *

    + *
  • Manages one or more Photon vision cameras + *
  • Updates camera measurements periodically + *
  • Reports vision measurements to a listener (typically the drivetrain) + *
  • Integrates with the Periodic utility for non-subsystem periodic updates + *
+ * + *

Vision measurements are used to correct odometry drift from encoder/gyro estimates. + */ +public class Photon { + + /** Callback function invoked when a new vision measurement is available. */ + final Consumer onMeasurement; + + /** List of all vision cameras managed by this subsystem. */ + final List cameras; + + /** + * Constructs a Photon vision subsystem. + * + * Creates and manages the specified cameras using the provided factory function. + * Registers a periodic callback to update the cameras each robot tick. + * + * @param onMeasurement Callback to invoke when a measurement is ready + * @param factory Factory function to create camera implementations + */ + public Photon( + Function factory, + Consumer onMeasurement + ) { + this.onMeasurement = onMeasurement; + this.cameras = Arrays.stream(CameraConfig.values()) + .map(factory) + .toList(); + } + + /** + * Periodic update method called every robot tick. + * + * Updates all cameras and reports any new vision measurements to the listener. + * This allows the drivetrain to incorporate vision data into its pose estimate. + */ + public void update() { + // Update all camera measurements + this.cameras.forEach(CameraIO::update); + + // Process vision measurements from each camera + for (CameraIO camera : cameras) { + String path = String.format("Camera/%s", camera.config.name); + + for (PhotonPipelineResult result : camera.data.results) { + VisionMeasurement measurement = camera.measurementOf(result); + Logger.recordOutput(path + "/Measurement", measurement); + + this.onMeasurement.accept(measurement); + } + } + } +} diff --git a/src/main/java/frc/robot/subsystems/quest/Quest.java b/src/main/java/frc/robot/subsystems/quest/Quest.java index 3932471..617eee6 100644 --- a/src/main/java/frc/robot/subsystems/quest/Quest.java +++ b/src/main/java/frc/robot/subsystems/quest/Quest.java @@ -6,6 +6,7 @@ import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; +import frc.robot.subsystems.quest.QuestIO.QuestData; import frc.robot.util.VisionMeasurement; import gg.questnav.questnav.PoseFrame; import java.util.Arrays; @@ -69,6 +70,14 @@ public void reset(Pose2d pose) { this.quest.reset(quest); } + /** + * Get the Quest's data + * @return a copy of the quest's data + */ + public QuestData data() { + return quest.data.clone(); + } + /** * Get the latest heading from the Quest device. * @return The latest heading, if available @@ -89,6 +98,55 @@ public Optional heading() { ); } + /** + * Get the estimated heading at a timestamp from the Quest device. + * @param timestamp The timestamp to query + * @return The estimated heading + */ + public Optional headingAt(double timestamp) { + // find two closest frames + PoseFrame[] frames = this.quest.data.readings; + PoseFrame before = null; + PoseFrame after = null; + + int lo = 0; + int hi = frames.length - 1; + while (lo <= hi) { + int mid = (lo + hi) / 2; + PoseFrame frame = frames[mid]; + double ts = frame.dataTimestamp(); + + if (ts <= timestamp) { + if (frame.isTracking()) before = frame; + lo = mid + 1; + } else { + if (frame.isTracking()) after = frame; + hi = mid + 1; + } + } + + if (before == null || after == null) return Optional.empty(); + + // interpolate between the two frames + Rotation2d a = before + .questPose3d() + .transformBy(kBotToQuest.inverse()) + .toPose2d() + .getRotation(); + + Rotation2d b = after + .questPose3d() + .transformBy(kBotToQuest.inverse()) + .toPose2d() + .getRotation(); + + double t = + (timestamp - before.dataTimestamp()) / + (after.dataTimestamp() - before.dataTimestamp()); + + return Optional.of(a.interpolate(b, t)); + } + /** * Periodic update method called every robot tick. * diff --git a/src/main/java/frc/robot/util/MathPlus.java b/src/main/java/frc/robot/util/MathPlus.java new file mode 100644 index 0000000..cd73589 --- /dev/null +++ b/src/main/java/frc/robot/util/MathPlus.java @@ -0,0 +1,83 @@ +package frc.robot.util; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Transform2d; + +/** + * Mathematical utility functions for robotics calculations. + * + *

This class provides helper methods for common operations involving poses, + * rotations, and transformations in 2D space. + * + *

Called {@link MathPlus} because WPI stole MathUtil, smh. + */ +public class MathPlus { + + private MathPlus() {} + + /** Tau (2π) constant for angle calculations. */ + public static final double kTau = 2.0 * Math.PI; + + /** An arbitrarily small number. */ + public static final double kEpsilon = 1e-6; + + /** + * Calculates the transformation from one pose to another in the first pose's frame. + * + *

Given two poses, returns the transformation that would move an object from the + * first pose to the second pose, as viewed from the first pose's perspective. + * In other words, the returned transform represents how something at pose 'a' should + * move (relative to itself) to reach pose 'b'. + * + * @param a The starting pose (reference frame) + * @param b The target pose + * @return The transformation from pose a to pose b in a's coordinate frame + */ + public static Transform2d transformationOf(Pose2d a, Pose2d b) { + double dx = b.getX() - a.getX(); + double dy = b.getY() - a.getY(); + double omega = b.getRotation().minus(a.getRotation()).getRadians(); + + double cosA = a.getRotation().getCos(); + double sinA = a.getRotation().getSin(); + + double dxA = cosA * dx + sinA * dy; + double dyA = -sinA * dx + cosA * dy; + return new Transform2d(dxA, dyA, new Rotation2d(omega)); + } + + /** + * Normalizes an angle to be within the range [0, 2π). + * + *

This is useful for standardizing angles so they can be easily compared. + * Negative angles are wrapped to their positive equivalents. + * + * @param angle The angle to normalize (in radians) + * @return The normalized angle in the range [0, 2π) + */ + public static double normalizeAngle(double angle) { + return ((angle % kTau) + kTau) % kTau; // Fix negative angles + } + + /** + * Normalizes a Rotation2d to be within the range [0, 2π). + * + * @param rotation The rotation to normalize + * @return A new Rotation2d with the normalized angle + */ + public static Rotation2d normalizeAngle(Rotation2d rotation) { + return new Rotation2d(normalizeAngle(rotation.getRadians())); + } + + /** + * Adds a really small number to avoid division by zero. + * + * @param value The value to adjust + * @return The adjusted value + */ + public static double avoidZero(double value) { + if (Math.abs(value) < kEpsilon) return Math.copySign(kEpsilon, value); + return value; + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/util/Repulsor.java b/src/main/java/frc/robot/util/Repulsor.java new file mode 100644 index 0000000..0979a61 --- /dev/null +++ b/src/main/java/frc/robot/util/Repulsor.java @@ -0,0 +1,545 @@ +package frc.robot.util; + +import static edu.wpi.first.units.Units.Microseconds; + +import choreo.trajectory.SwerveSample; +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.math.util.Units; +import edu.wpi.first.wpilibj.TimedRobot; +import frc.robot.constants.GameConstants; +import java.util.ArrayList; +import java.util.List; +import java.util.Optional; +import org.littletonrobotics.junction.AutoLogOutput; +import org.littletonrobotics.junction.AutoLogOutputManager; +import org.littletonrobotics.junction.Logger; + +// Taken straight from 6995's code. Praise be to 6995!! +public class Repulsor { + + abstract static class Obstacle { + + double strength = 1.0; + boolean positive = true; + + public Obstacle(double strength, boolean positive) { + this.strength = strength; + this.positive = positive; + } + + /** + * Finds the Force from this obstacle to a position. + * + * @param position The current position. + * @param target The goal position. + * @return The force at a certain position. + */ + public abstract Vec2 getForceAtPosition( + Translation2d position, + Translation2d target + ); + + /** + * @param dist The distance from the position to the obstacle. + * @return The force magnitude from that position + */ + protected double distToForceMag(double dist) { + double forceMag = strength / (0.00001 + Math.pow(dist, 2)); + return forceMag * (positive ? 1 : -1); + } + + /** + * @param dist The distance from the position to the obstacle + * @param falloff The falloff. + * @return The force magnitude from the position, with subtracted falloff. + */ + protected double distToForceMag(double dist, double falloff) { + double original = strength / (0.00001 + Math.pow(dist, 2)); + double falloffMag = strength / (0.00001 + Math.pow(falloff, 2)); + return Math.max(original - falloffMag, 0) * (positive ? 1 : -1); + } + } + + static class PointObstacle extends Obstacle { + + Translation2d loc; + double radius = 0.5; + + public PointObstacle( + Translation2d loc, + double strength, + boolean positive + ) { + super(strength, positive); + this.loc = loc; + } + + public Vec2 getForceAtPosition( + Translation2d position, + Translation2d target + ) { + // displacement from obstacle + double dist = loc.getDistance(position); + if (dist > 4) { + return Vec2.ZERO; + } + // distance from the position to the outer radius of the target. + double outwardsMag = distToForceMag( + loc.getDistance(position) - radius + ); + + // initial calculated force; vector from the obstacle to the position. + Vec2 initial = new Vec2( + outwardsMag, + position.minus(loc).getAngle() + ); + + // theta = angle between position->target vector and obstacle->position vector + Rotation2d theta = target + .minus(position) + .getAngle() + .minus(position.minus(loc).getAngle()); + + // divide magnitude by 2 and multiply by the sign of theta + double mag = + (outwardsMag * Math.signum(Math.sin(theta.getRadians() / 2))) / + 2; + + return initial + .rotate(Rotation2d.kCCW_90deg) // rotate left 90 degrees + .div(initial.mag()) // normalize + .mul(mag) // set magnitude + .add(initial); // add initial force + } + } + + static class CircleObstacle extends Obstacle { + + Translation2d loc; + double radius = 0.5; + + public CircleObstacle( + Translation2d loc, + double strength, + double radius, + boolean positive + ) { + super(strength, positive); + this.loc = loc; + this.radius = radius; + } + + public Vec2 getForceAtPosition( + Translation2d position, + Translation2d target + ) { + // displacement from obstacle + Translation2d targetToLoc = loc.minus(target); + + // 1 meter from loc, direction is away from target + Translation2d sidewaysCircle = new Translation2d( + 1, + targetToLoc.getAngle() + ).plus(loc); + + // force magnitude from the sidewaysCircle to the position + double sidewaysMag = distToForceMag( + sidewaysCircle.getDistance(position) + ); + + // force magnitude from the outward radius of the obstacle. + double outwardsMag = distToForceMag( + Math.max(0.01, loc.getDistance(position) - radius) + ); + + // initial force from the obstacle. + Vec2 initial = new Vec2( + outwardsMag, + position.minus(loc).getNorm() > 1e-4 + ? position.minus(loc).getAngle() + : Rotation2d.kZero + ); + + // flip the sidewaysMag based on which side of the goal-sideways circle the robot is on + Rotation2d sidewaysTheta = + target.minus(position).getNorm() > 1e-4 + ? target + .minus(position) + .getAngle() + .minus(position.minus(sidewaysCircle).getAngle()) + : Rotation2d.kZero; + + // sideways force calculations to go AROUND objects. sine sign is used to figure out which way + // to go around + double sideways = + sidewaysMag * Math.signum(Math.sin(sidewaysTheta.getRadians())); + Rotation2d sidewaysAngle = targetToLoc + .getAngle() + .rotateBy(Rotation2d.kCCW_90deg); + + // adds sideways to force for resultant force + return new Vec2(sideways, sidewaysAngle).add(initial); + } + } + + static class HorizontalObstacle extends Obstacle { + + double y; + + public HorizontalObstacle(double y, double strength, boolean positive) { + super(strength, positive); + this.y = y; + } + + public Vec2 getForceAtPosition( + Translation2d position, + Translation2d target + ) { + return new Vec2(0, distToForceMag(y - position.getY(), 1)); + } + } + + static class VerticalObstacle extends Obstacle { + + double x; + + public VerticalObstacle(double x, double strength, boolean positive) { + super(strength, positive); + this.x = x; + } + + public Vec2 getForceAtPosition( + Translation2d position, + Translation2d target + ) { + return new Vec2(distToForceMag(x - position.getX(), 1), 0); + } + } + + public static final double GOAL_STRENGTH = 0.65; + + // TODO: season: replace with actual obsticles (use choreo?) + public static final List FIELD_OBSTACLES = List.of( + new CircleObstacle( + new Translation2d(4.49, 4), + 0.6, + Units.inchesToMeters(65.5 / 2.0), + true + ), + new CircleObstacle( + new Translation2d(13.08, 4), + 0.6, + Units.inchesToMeters(65.5 / 2.0), + true + ) + ); + static final double FIELD_LENGTH = GameConstants.kFieldLength; + static final double FIELD_WIDTH = GameConstants.kFieldWidth; + public static final List WALLS = List.of( + new HorizontalObstacle(0.0, 0.5, true), + new HorizontalObstacle(FIELD_WIDTH, 0.5, false), + new VerticalObstacle(0.0, 0.5, true), + new VerticalObstacle(FIELD_LENGTH, 0.5, false) + ); + + private List fixedObstacles = new ArrayList<>(); + private Optional goalOpt = Optional.empty(); + + @AutoLogOutput + public Pose2d goal() { + return new Pose2d( + goalOpt.orElse(Translation2d.kZero), + Rotation2d.kZero + ); + } + + // A grid of arrows drawn in AScope + // private static final int ARROWS_X = RobotBase.isSimulation() ? 40 : 0; + // private static final int ARROWS_Y = RobotBase.isSimulation() ? 20 : 0; + // private static final int ARROWS_SIZE = (ARROWS_X + 1) * (ARROWS_Y + 1); + // private ArrayList arrows = new ArrayList<>(ARROWS_SIZE); + + private SwerveSample prevSample; + + public Repulsor() { + fixedObstacles.addAll(FIELD_OBSTACLES); + fixedObstacles.addAll(WALLS); + // for (int i = 0; i < ARROWS_SIZE; i++) { + // // arrows.add(new Pose2d()); + // } + this.prevSample = sample( + Translation2d.kZero, + Rotation2d.kZero, + 0, + 0, + 0 + ); + + AutoLogOutputManager.addObject(this); + } + + // private boolean useGoalInArrows = false; + + // private boolean useObstaclesInArrows = true; + + // private boolean useWallsInArrows = true; + + // private Pose2d arrowBackstage = new Pose2d(-10, -10, Rotation2d.kZero); + + /** Updates the grid of vectors // */ + // void updateArrows() { + // for (int x = 0; x <= ARROWS_X; x++) { + // for (int y = 0; y <= ARROWS_Y; y++) { + // Translation2d position = + // new Translation2d(x * FIELD_LENGTH / ARROWS_X, y * FIELD_WIDTH / ARROWS_Y); + // Force force = Force.kZero; + // if (useObstaclesInArrows) + // force = force.plus(getObstacleForce(position, goal().getTranslation())); + // if (useWallsInArrows) + // force = force.plus(getWallForce(position, goal().getTranslation())); + // if (useGoalInArrows) { + // force = force.plus(getGoalForce(position, goal().getTranslation())); + // } + // if (force.getNorm() < 1e-6) { + // arrows.set(x * (ARROWS_Y + 1) + y, arrowBackstage); + // } else { + // var rotation = force.getAngle(); + + // arrows.set(x * (ARROWS_Y + 1) + y, new Pose2d(position, rotation)); + // } + // } + // } + // } + + /** + * Force towards the goal. + * + * @param curLocation Location of the robot. + * @param goal Position of the goal. + * @return The force to the goal. + */ + Vec2 getGoalForce(Translation2d curLocation, Translation2d goal) { + var displacement = goal.minus(curLocation); + if (displacement.getNorm() == 0) { + return Vec2.ZERO; + } + var direction = displacement.getAngle(); + var mag = + GOAL_STRENGTH * + (1 + + 1.0 / + (0.0001 + displacement.getNorm() * displacement.getNorm())); + return new Vec2(mag, direction); + } + + /** + * Force from the walls. + * + * @param curLocation Location of the robot. + * @param target Position of the goal. + * @return The force from the walls. + */ + Vec2 getWallForce(Translation2d curLocation, Translation2d target) { + var force = Vec2.ZERO; + for (Obstacle obs : WALLS) { + force = force.add(obs.getForceAtPosition(curLocation, target)); + } + return force; + } + + /** + * Force from obstacles. + * + * @param curLocation Location of the robot. + * @param target Position of the goal. + * @return The force from the obstacles. + */ + Vec2 getObstacleForce(Translation2d curLocation, Translation2d target) { + var force = Vec2.ZERO; + for (Obstacle obs : FIELD_OBSTACLES) { + force = force.add(obs.getForceAtPosition(curLocation, target)); + } + return force; + } + + /** + * Complete force from obstacles, goal and walls. + * + * @param curLocation Location of the robot. + * @param target Position of the goal. + * @return The total resultant force from field elements. + */ + Vec2 getForce(Translation2d curLocation, Translation2d target) { + var goalForce = getGoalForce(curLocation, target) + .add(getObstacleForce(curLocation, target)) + .add(getWallForce(curLocation, target)); + return goalForce; + } + + /** + * Creates a {@link SwerveSample} from relevant values. + * + * @param trans Position of the robot. + * @param rot Heading of the robot. + * @param vx x-component of the field relative robot velocity. + * @param vy y-component of the field relative robot velocity. + * @param omega Rotational velocity of the robot. + * @return A Choreo SwerveSample. + */ + public static SwerveSample sample( + Translation2d trans, + Rotation2d rot, + double vx, + double vy, + double omega + ) { + return new SwerveSample( + 0, + trans.getX(), + trans.getY(), + rot.getRadians(), + vx, + vy, + omega, + 0, + 0, + 0, + new double[4], + new double[4] + ); + } + + /** + * Changes the goal of the robot. + * + * @param goal The new goal of the pathfinder. + */ + public void setGoal(Translation2d goal) { + this.goalOpt = Optional.of(goal); + // updateArrows(); + } + + /** + * @param pose The current pose of the robot. + * @param currentSpeeds The current {@link ChassisSpeeds} of the robot. + * @param maxSpeed The desired maximum speed of the robot. + * @param useGoal Whether or not to use the given goal. + * @return A {@link SwerveSample} representing the next desired robot swerve state to get to the + * goal. Includes obstacle avoidance. + */ + public SwerveSample getCmd( + Pose2d pose, + ChassisSpeeds currentSpeeds, + double maxSpeed, + boolean useGoal + ) { + return getCmd( + pose, + currentSpeeds, + maxSpeed, + useGoal, + pose.getRotation() + ); + } + + public SwerveSample getCmd( + Pose2d pose, + ChassisSpeeds currentSpeeds, + double maxSpeed, + boolean useGoal, + Rotation2d goalRotation + ) { + // Distance travelled in one period + double stepSize_m = maxSpeed * TimedRobot.kDefaultPeriod; + + if (goalOpt.isEmpty()) { + // Tells the robot to stop moving if there is no goal. + return sample(pose.getTranslation(), pose.getRotation(), 0, 0, 0); + } else { + long startTime = System.nanoTime(); + + // sets the goal the position and the error + Translation2d goal = goalOpt.get(); + Translation2d position = pose.getTranslation(); + Translation2d err = position.minus(goal); + + if (useGoal && err.getNorm() < stepSize_m * 1.5) { + // Tells the robot to stop moving if it's already there. + return sample(goal, goalRotation, 0, 0, 0); + } else { + // Add in all forces, ternary operator for the useGoal -> getGoalForce + Vec2 netForce = getObstacleForce(position, goal) + .add(getWallForce(position, goal)) + .add(useGoal ? getGoalForce(position, goal) : Vec2.ZERO); + + // Change stepSize_m if we are using goal + stepSize_m = useGoal + ? Math.min( + maxSpeed, + maxSpeed * Math.min(err.getNorm() / 2, 1) + ) * + 0.02 + : stepSize_m; + + // Next desired displacement from the max speed and angle of the net force + Translation2d step = new Translation2d( + stepSize_m, + netForce.theta() + ); + + // Next desired position + var intermediateGoal = position.plus(step); + + var endTime = System.nanoTime(); + Logger.recordOutput( + "Repulsor/Latency", + Microseconds.of((endTime - startTime) / 1e3) + ); + + // set the previous sample as the current sample + prevSample = sample( + intermediateGoal, + goalRotation, + step.getX() / TimedRobot.kDefaultPeriod, + step.getY() / TimedRobot.kDefaultPeriod, + 0 + ); + return prevSample; + } + } + } + + public double pathLength = 0; + + public ArrayList getTrajectory( + Translation2d current, + Translation2d goalTranslation, + double stepSize_m + ) { + pathLength = 0; + // goalTranslation = goalOpt.orElse(goalTranslation); + ArrayList traj = new ArrayList<>(); + Translation2d robot = current; + for (int i = 0; i < 400; i++) { + var err = robot.minus(goalTranslation); + if (err.getNorm() < stepSize_m * 1.5) { + traj.add(goalTranslation); + break; + } else { + var netForce = getForce(robot, goalTranslation); + if (netForce.mag() == 0) { + break; + } + var step = new Translation2d(stepSize_m, netForce.theta()); + var intermediateGoal = robot.plus(step); + traj.add(intermediateGoal); + pathLength += stepSize_m; + robot = intermediateGoal; + } + } + return traj; + } +} diff --git a/src/main/java/frc/robot/util/Vec2.java b/src/main/java/frc/robot/util/Vec2.java new file mode 100644 index 0000000..4e25184 --- /dev/null +++ b/src/main/java/frc/robot/util/Vec2.java @@ -0,0 +1,265 @@ +package frc.robot.util; + +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; + +/** + * A 2D vector class for representing and manipulating 2D coordinates and directions. + * + *

This immutable class provides common vector operations including addition, subtraction, + * scalar multiplication, dot product, and magnitude calculations. It's commonly used for + * representing velocities, positions, and forces in 2D space. + * + *

The class is immutable, meaning all operations return new Vec2 instances rather than + * modifying the original object. + */ +public class Vec2 { + + /** The X component of the vector. */ + public final double x; + + /** The Y component of the vector. */ + public final double y; + + /** Constant for the zero vector (0, 0). */ + public static final Vec2 ZERO = new Vec2(0, 0); + + /** Constant for the unit vector (1, 1). */ + public static final Vec2 ONE = new Vec2(1, 1); + + /** Constant for the unit vector along the X axis (1, 0). */ + public static final Vec2 UNIT_X = new Vec2(1, 0); + + /** Constant for the unit vector along the Y axis (0, 1). */ + public static final Vec2 UNIT_Y = new Vec2(0, 1); + + /** + * Constructs a new Vec2 with the given X and Y components. + * + * @param x The X component + * @param y The Y component + */ + public Vec2(double x, double y) { + this.x = x; + this.y = y; + } + + /** + * Constructs a new Vec2 with the given magnitude and angle. + * + * @param mag The magnitude for both components + * @param angle The angle in radians + */ + public Vec2(double mag, Rotation2d angle) { + this.x = mag * angle.getCos(); + this.y = mag * angle.getSin(); + } + + /** + * Turns a {@link Translation2d} into a Vec2 + * @param translation the translation + */ + public Vec2(Translation2d translation) { + this.x = translation.getX(); + this.y = translation.getY(); + } + + /** + * Turns a {@link Pose2d} into a Vec2 using its Translation + * @param pose the pose + */ + public Vec2(Pose2d pose) { + this(pose.getTranslation()); + } + + /** + * Creates a new Vec2 with both components set to the same value. + * + * @param value The value for both x and y components + * @return A new Vec2 with equal x and y components + */ + public static Vec2 fill(double value) { + return new Vec2(value, value); + } + + /** + * Adds another vector to this vector. + * + * @param other The vector to add + * @return A new Vec2 representing the sum of this and other + */ + public Vec2 add(Vec2 other) { + return new Vec2(this.x + other.x, this.y + other.y); + } + + /** + * Subtracts another vector from this vector. + * + * @param other The vector to subtract + * @return A new Vec2 representing the difference (this - other) + */ + public Vec2 sub(Vec2 other) { + return new Vec2(this.x - other.x, this.y - other.y); + } + + /** + * Multiplies this vector by a scalar value. + * + * @param scalar The scalar multiplier + * @return A new Vec2 with both components multiplied by the scalar + */ + public Vec2 mul(double scalar) { + return new Vec2(this.x * scalar, this.y * scalar); + } + + /** + * Performs element-wise multiplication with another vector. + * + * @param elementwise The vector to multiply element-wise + * @return A new Vec2 with each component multiplied (x*x, y*y) + */ + public Vec2 mul(Vec2 elementwise) { + return new Vec2(this.x * elementwise.x, this.y * elementwise.y); + } + + /** + * Raises each component to a given power. + * + * @param exp The exponent + * @return A new Vec2 with each component raised to the power + */ + public Vec2 pow(double exp) { + return new Vec2(Math.pow(this.x, exp), Math.pow(this.y, exp)); + } + + /** + * Divides this vector by a scalar value. + * + * @param scalar The scalar divisor + * @return A new Vec2 with both components divided by the scalar + */ + public Vec2 div(double scalar) { + return new Vec2(this.x / scalar, this.y / scalar); + } + + /** + * Performs element-wise division with another vector. + * + * @param elementwise The vector to divide element-wise + * @return A new Vec2 with each component divided (x/x, y/y) + */ + public Vec2 div(Vec2 elementwise) { + return new Vec2(this.x / elementwise.x, this.y / elementwise.y); + } + + /** + * Returns the component-wise absolute value of this vector. + * + * @return A new Vec2 with each component set to its absolute value + */ + public Vec2 abs() { + return new Vec2(Math.abs(this.x), Math.abs(this.y)); + } + + /** + * Returns the sign of each component. + * + * Each component becomes -1 (negative), 0 (zero), or 1 (positive). + * + * @return A new Vec2 with the sign of each component + */ + public Vec2 sign() { + return new Vec2(Math.signum(this.x), Math.signum(this.y)); + } + + /** + * Negates this vector by negating both components. + * + * @return A new Vec2 representing the negation of this vector + */ + public Vec2 neg() { + return new Vec2(-this.x, -this.y); + } + + /** + * Computes the dot product with another vector. + * + * The dot product represents the projection of one vector onto another. + * For perpendicular vectors, the dot product is zero. + * + * @param other The other vector + * @return The dot product as a scalar (this · other) + */ + public double dot(Vec2 other) { + return this.x * other.x + this.y * other.y; + } + + /** + * Returns the normalized (unit) version of this vector. + * + * The normalized vector has a magnitude of 1 but points in the same direction. + * For zero vectors, returns a zero vector to avoid division by zero. + * + * @return A new Vec2 with the same direction but magnitude of 1 + */ + public Vec2 norm() { + double len = mag(); + if (len == 0) return new Vec2(0, 0); + return new Vec2(this.x / len, this.y / len); + } + + /** + * Gets the angle of this vector from the positive X-axis. + * + * @return A Rotation2d representing the angle of this vector + */ + public Rotation2d theta() { + return new Rotation2d(Math.atan2(this.y, this.x)); + } + + /** + * Rotates this vector by a given angle, counterclockwise. + * + *

This multiplies the translation vector by a counterclockwise rotation matrix of the given + * angle. + * + * 

+     * [x_new] = [other.cos, -other.sin][x]
+     * [y_new] = [other.sin,  other.cos][y]
+     *
+ * + * @param angle The angle to rotate by + * @return A new Vec2 representing the rotated vector + */ + public Vec2 rotate(Rotation2d angle) { + double cosA = angle.getCos(); + double sinA = angle.getSin(); + double newX = cosA * this.x - sinA * this.y; + double newY = sinA * this.x + cosA * this.y; + return new Vec2(newX, newY); + } + + /** + * Calculates the magnitude (length) of this vector. + * + * Computed as sqrt(x² + y²). + * + * @return The magnitude of this vector + */ + public double mag() { + return Math.hypot(this.x, this.y); + } + + /** + * Calculates the squared magnitude of this vector. + * + * Computed as x² + y². This is more efficient than mag() when you only + * need to compare magnitudes, since comparing squares preserves the order. + * + * @return The squared magnitude of this vector + */ + public double mag2() { + return this.dot(this); + } +} diff --git a/test b/test deleted file mode 100644 index e69de29..0000000 diff --git a/vendordeps/photonlib.json b/vendordeps/photonlib.json new file mode 100644 index 0000000..a1bc5a5 --- /dev/null +++ b/vendordeps/photonlib.json @@ -0,0 +1,71 @@ +{ + "fileName": "photonlib.json", + "name": "photonlib", + "version": "v2026.1.1", + "uuid": "515fe07e-bfc6-11fa-b3de-0242ac130004", + "frcYear": "2026", + "mavenUrls": [ + "https://maven.photonvision.org/repository/internal", + "https://maven.photonvision.org/repository/snapshots" + ], + "jsonUrl": "https://maven.photonvision.org/repository/internal/org/photonvision/photonlib-json/1.0/photonlib-json-1.0.json", + "jniDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-cpp", + "version": "v2026.1.1", + "skipInvalidPlatforms": true, + "isJar": false, + "validPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "cppDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photonlib-cpp", + "version": "v2026.1.1", + "libName": "photonlib", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-cpp", + "version": "v2026.1.1", + "libName": "photontargeting", + "headerClassifier": "headers", + "sharedLibrary": true, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "windowsx86-64", + "linuxathena", + "linuxx86-64", + "osxuniversal" + ] + } + ], + "javaDependencies": [ + { + "groupId": "org.photonvision", + "artifactId": "photonlib-java", + "version": "v2026.1.1" + }, + { + "groupId": "org.photonvision", + "artifactId": "photontargeting-java", + "version": "v2026.1.1" + } + ] +} \ No newline at end of file