Se/swerve commands

src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDrive.java

@@ -46,7 +46,7 @@ public SwerveDriveDrive(Gamepad driver) {
                 x -> x * Turn.MAX_TELEOP_TURNING.get(),
                 new RateLimit(Turn.RC.get())
         ); 
-
+        
         addRequirements(swerve);
     }
 

src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToPose.java

@@ -0,0 +1,70 @@
+package com.stuypulse.robot.commands.swerve;
+
+import com.stuypulse.robot.constants.Settings.Alignment;
+import com.stuypulse.robot.constants.Settings.Alignment.Rotation;
+import com.stuypulse.robot.constants.Settings.Alignment.Translation;
+import com.stuypulse.robot.subsystems.odometry.Odometry;
+import com.stuypulse.robot.subsystems.swerve.SwerveDrive;
+import com.stuypulse.robot.util.HolonomicController;
+import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController;
+import com.stuypulse.stuylib.control.feedback.PIDController;
+import com.stuypulse.stuylib.streams.booleans.BStream;
+import com.stuypulse.stuylib.streams.booleans.filters.BDebounceRC;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d;
+import edu.wpi.first.wpilibj2.command.Command;
+
+public class SwerveDriveToPose extends Command {
+
+    private final SwerveDrive swerve;
+
+    private final HolonomicController controller;
+    private final Pose2d targetPose;
+    private final BStream isAligned;
+
+    private final FieldObject2d targetPose2d;
+
+    public SwerveDriveToPose(Pose2d targetPose) {
+        swerve = SwerveDrive.getInstance();
+        this.targetPose = targetPose;
+
+        targetPose2d = Odometry.getInstance().getField().getObject("Target Pose");
+
+        controller = new HolonomicController(
+            new PIDController(Translation.P, Translation.I, Translation.D),
+            new PIDController(Translation.P, Translation.I, Translation.D),
+            new AnglePIDController(Rotation.P, Rotation.I, Rotation.D));
+
+        isAligned = BStream.create(this::isAligned)
+            .filtered(new BDebounceRC.Both(Alignment.DEBOUNCE_TIME.get()));
+
+        addRequirements(swerve);
+    }
+
+    private boolean isAligned() {
+        return controller.isDone(Alignment.X_TOLERANCE.get(), Alignment.Y_TOLERANCE.get(), Alignment.ANGLE_TOLERANCE.get());
+    }
+
+    @Override
+    public void execute() {
+        Pose2d currentPose = Odometry.getInstance().getPose();
+        targetPose2d.setPose(targetPose);
+
+        controller.update(targetPose, currentPose);
+        swerve.setChassisSpeeds(controller.getOutput());
+    }
+
+    @Override
+    public boolean isFinished() {
+        return isAligned.get();
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        swerve.stop();
+        targetPose2d.setPose(Double.NaN, Double.NaN, new Rotation2d(Double.NaN));
+    }
+
+}

src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveToShoot.java

@@ -0,0 +1,90 @@
+package com.stuypulse.robot.commands.swerve;
+
+import com.stuypulse.robot.constants.Field;
+import com.stuypulse.robot.constants.Settings.Alignment;
+import com.stuypulse.robot.constants.Settings.Alignment.Rotation;
+import com.stuypulse.robot.constants.Settings.Alignment.Translation;
+import com.stuypulse.robot.subsystems.odometry.Odometry;
+import com.stuypulse.robot.subsystems.swerve.SwerveDrive;
+import com.stuypulse.robot.util.HolonomicController;
+import com.stuypulse.stuylib.control.angle.feedback.AnglePIDController;
+import com.stuypulse.stuylib.control.feedback.PIDController;
+import com.stuypulse.stuylib.math.Vector2D;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.smartdashboard.FieldObject2d;
+import edu.wpi.first.wpilibj2.command.Command;
+
+public class SwerveDriveToShoot extends Command {
+
+    private final SwerveDrive swerve;
+
+    private final HolonomicController controller;
+    private Pose2d targetPose;
+
+    private final FieldObject2d targetPose2d;
+    private final FieldObject2d speakerPose2d;
+
+    public SwerveDriveToShoot() {
+        swerve = SwerveDrive.getInstance();
+
+        targetPose2d = Odometry.getInstance().getField().getObject("Target Pose");
+        speakerPose2d = Odometry.getInstance().getField().getObject("Speaker Pose");
+
+        controller = new HolonomicController(
+            new PIDController(Translation.P, Translation.I, Translation.D),
+            new PIDController(Translation.P, Translation.I, Translation.D),
+            new AnglePIDController(Rotation.P, Rotation.I, Rotation.D));
+
+        addRequirements(swerve);
+    }
+
+    private Pose2d getSpeakerTargetPose() {
+        Vector2D speakerCenterVec = new Vector2D(Field.getAllianceSpeakerPose().getTranslation());
+        Vector2D robotVec = new Vector2D(Odometry.getInstance().getPose().getTranslation());
+
+        // the distances between the robot and the target
+        double Dx = speakerCenterVec.x - robotVec.x;
+        double Dy = speakerCenterVec.y - robotVec.y;
+
+        // the offset of the speakers opening width from center using similar triangles 
+        double dy = (Dy / Dx) * Field.SPEAKER_OPENING_X; 
+
+        // gets the new speaker target vector to aim at
+        Vector2D speakerTargetVec = new Vector2D(Field.getAllianceSpeakerPose().getX(), Field.getAllianceSpeakerPose().getY() + dy);
+
+        speakerPose2d.setPose(speakerTargetVec.x, speakerTargetVec.y, Rotation2d.fromDegrees(0));
+
+        // gets the target vector away from the speaker to the target distance to shoot from
+        Vector2D targetVec = speakerTargetVec.add(robotVec.sub(speakerTargetVec).normalize().mul(Units.inchesToMeters(Alignment.TARGET_DISTANCE_IN.get())));
+        Rotation2d targetAngle = targetVec.getTranslation2d().minus(robotVec.getTranslation2d()).getAngle().plus(Rotation2d.fromDegrees(180));
+        return new Pose2d(targetVec.x, targetVec.y, targetAngle);
+    }
+
+    @Override
+    public void initialize() {
+        targetPose = getSpeakerTargetPose();
+    }
+
+    @Override 
+    public void execute() {
+        Pose2d currentPose = Odometry.getInstance().getPose();
+
+        controller.update(targetPose, currentPose);
+        swerve.setChassisSpeeds(controller.getOutput());
+        targetPose2d.setPose(targetPose);
+    }
+
+    @Override
+    public boolean isFinished() {
+        return controller.isDone(Alignment.X_TOLERANCE.get(), Alignment.Y_TOLERANCE.get(), Alignment.ANGLE_TOLERANCE.get());
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        swerve.stop();
+    }
+
+}

src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveXMode.java

@@ -0,0 +1,30 @@
+package com.stuypulse.robot.commands.swerve;
+
+import com.stuypulse.robot.subsystems.swerve.SwerveDrive;
+
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.wpilibj2.command.Command;
+
+public class SwerveDriveXMode extends Command {
+
+    // { front right, front left, back right, back left }
+    private static final SwerveModuleState[] states = new SwerveModuleState[] {
+        new SwerveModuleState(0, Rotation2d.fromDegrees(225)),
+        new SwerveModuleState(0, Rotation2d.fromDegrees(315)),
+        new SwerveModuleState(0, Rotation2d.fromDegrees(45)),
+        new SwerveModuleState(0, Rotation2d.fromDegrees(135))
+    };
+
+    private final SwerveDrive swerve;
+
+    public SwerveDriveXMode() {
+        swerve = SwerveDrive.getInstance();
+        addRequirements(swerve);
+    } 
+
+    @Override
+    public void execute() {
+        swerve.setModuleStates(states);
+    }
+}

src/main/java/com/stuypulse/robot/constants/Field.java

@@ -4,20 +4,25 @@
 
 import com.stuypulse.robot.util.Fiducial;
 
+import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Translation3d;
 
 import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.DriverStation;
 
 /**
  * This interface stores information about the field elements.
  */
 public interface Field {
 
-    public final double FIDUCIAL_SIZE = Units.inchesToMeters(6.125);
     double NOTE_LENGTH = Units.inchesToMeters(14.0);
 
+    /*** APRILTAGS ***/
+
+    double FIDUCIAL_SIZE = Units.inchesToMeters(6.125);
+
     Fiducial FIDUCIALS[] = {
         // 2024 Field Fiducial Layout
         new Fiducial(1,  new Pose3d(new Translation3d(Units.inchesToMeters(593.68), Units.inchesToMeters(9.68), Units.inchesToMeters(53.38)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(120)))),
@@ -84,4 +89,18 @@ public static Fiducial getFiducial(int fid) {
         }
         return null;
     }
+
+    /*** SPEAKER ***/
+
+    Pose2d SPEAKER_POSES[] = {
+        getFiducial(7).getLocation().toPose2d(), // BLUE CENTER
+        getFiducial(4).getLocation().toPose2d(), // RED CENTER
+    };
+
+    public static Pose2d getAllianceSpeakerPose() {
+        boolean isBlue = DriverStation.getAlliance().isPresent() && DriverStation.getAlliance().get() == DriverStation.Alliance.Blue;
+        return SPEAKER_POSES[isBlue ? 0 : 1];
+    }
+
+    double SPEAKER_OPENING_X = Units.inchesToMeters(13.6);
 }

src/main/java/com/stuypulse/robot/constants/Settings.java

@@ -5,8 +5,7 @@
 
 package com.stuypulse.robot.constants;
 
-import edu.wpi.first.math.util.Units;
-
+import com.pathplanner.lib.util.PIDConstants;
 import com.stuypulse.stuylib.math.Vector2D;
 import com.stuypulse.stuylib.network.SmartBoolean;
 import com.stuypulse.stuylib.network.SmartNumber;
@@ -93,11 +92,19 @@ public interface Swerve {
         double WIDTH = Units.inchesToMeters(20.75);
         double LENGTH = Units.inchesToMeters(20.75);
 
+        double XY = 0.1;
+        double THETA = 0.1;
+
         SmartNumber MAX_MODULE_SPEED = new SmartNumber("Swerve/Max Module Speed (meter per s)", 5.0);
         SmartNumber MAX_TURNING = new SmartNumber("Swerve/Max Turn Velocity (rad per s)", 6.28);
 
         SmartNumber MODULE_VELOCITY_DEADBAND = new SmartNumber("Swerve/Module Velocity Deadband (m per s)", 0.02);
 
+        public interface Motion {   
+            PIDConstants XY = new PIDConstants(0.7, 0, 0.02);
+            PIDConstants THETA = new PIDConstants(10, 0, 0.1);
+        }
+
         public interface Encoder {
             public interface Drive {
                 double WHEEL_DIAMETER = Units.inchesToMeters(3);
@@ -248,4 +255,27 @@ public interface Conveyor {
     public static Vector2D vpow(Vector2D vec, double power) {
         return vec.mul(Math.pow(vec.magnitude(), power - 1));
     }
+
+    public interface Alignment {
+
+        SmartNumber DEBOUNCE_TIME = new SmartNumber("Alignment/Debounce Time", 0.15);
+        SmartNumber X_TOLERANCE = new SmartNumber("Alignment/X Tolerance", 0.1);
+        SmartNumber Y_TOLERANCE = new SmartNumber("Alignment/Y Tolerance", 0.1);
+        SmartNumber ANGLE_TOLERANCE = new SmartNumber("Alignment/Angle Tolerance", 5);
+
+        SmartNumber TARGET_DISTANCE_IN = new SmartNumber("Alignment/Target Distance (in)", 110);
+        SmartNumber TAKEOVER_DISTANCE_IN = new SmartNumber("Alignment/Takeover Distance (in)", 50);
+
+        public interface Translation {
+            SmartNumber P = new SmartNumber("Alignment/Translation/kP", 2.5);
+            SmartNumber I = new SmartNumber("Alignment/Translation/kI", 0);
+            SmartNumber D = new SmartNumber("Alignment/Translation/kD", 0.0);
+        }
+
+        public interface Rotation {
+            SmartNumber P = new SmartNumber("Alignment/Rotation/kP", 1);
+            SmartNumber I = new SmartNumber("Alignment/Rotation/kI", 0);
+            SmartNumber D = new SmartNumber("Alignment/Rotation/kD", 0);
+        }
+    }
 }

src/main/java/com/stuypulse/robot/subsystems/swerve/SwerveDrive.java

@@ -1,6 +1,10 @@
 package com.stuypulse.robot.subsystems.swerve;
 
 import com.kauailabs.navx.frc.AHRS;
+import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.util.HolonomicPathFollowerConfig;
+import com.pathplanner.lib.util.PathPlannerLogging;
+import com.pathplanner.lib.util.ReplanningConfig;
 import com.stuypulse.robot.constants.Ports;
 import com.stuypulse.robot.constants.Settings;
 import com.stuypulse.robot.constants.Settings.Swerve;
@@ -22,6 +26,7 @@
 import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.RobotBase;
 import edu.wpi.first.wpilibj.SPI;
 import edu.wpi.first.wpilibj.smartdashboard.Field2d;
@@ -108,7 +113,34 @@ public SwerveDrive(SwerveModule... modules){
         this.kinematics = new SwerveDriveKinematics(getModuleOffsets());
         this.gyro = new AHRS(SPI.Port.kMXP);
         this.modules2D = new FieldObject2d[modules.length];
-    }   
+    }  
+
+    public void configureAutoBuilder() {
+        Odometry odometry = Odometry.getInstance();
+
+        AutoBuilder.configureHolonomic(
+            odometry::getPose,
+            odometry::reset,
+            this::getChassisSpeeds, 
+            this::setChassisSpeeds, 
+            new HolonomicPathFollowerConfig(
+                Swerve.Motion.XY, 
+                Swerve.Motion.THETA, 
+                Swerve.MAX_MODULE_SPEED.get(), 
+                Swerve.WIDTH, 
+                new ReplanningConfig(true, true)), 
+            () -> {
+                var alliance = DriverStation.getAlliance();
+                if (alliance.isPresent()) {
+                    return alliance.get() == DriverStation.Alliance.Red;
+                }
+                return false;
+            }, 
+            instance
+        );
+
+        PathPlannerLogging.setLogActivePathCallback((poses) -> odometry.getField().getObject("path").setPoses(poses));
+    }
 
     public void initFieldObject(Field2d field) {
         for (int i = 0; i < modules.length; i++){
@@ -228,6 +260,10 @@ public void periodic() {
         SmartDashboard.putNumber("Swerve/X Acceleration (Gs)", gyro.getWorldLinearAccelX());
         SmartDashboard.putNumber("Swerve/Y Acceleration (Gs)", gyro.getWorldLinearAccelY());
         SmartDashboard.putNumber("Swerve/Z Acceleration (Gs)", gyro.getWorldLinearAccelZ());
+
+        SmartDashboard.putNumber("Swerve/Chassis X Speed", getChassisSpeeds().vxMetersPerSecond);
+        SmartDashboard.putNumber("Swerve/Chassis Y Speed", getChassisSpeeds().vyMetersPerSecond);
+        SmartDashboard.putNumber("Swerve/Chassis Rotation", getChassisSpeeds().omegaRadiansPerSecond);
     }
 
     public void simulationPeriodic() {