Vision + Odometry Subsystem

src/main/java/com/stuypulse/robot/RobotContainer.java

@@ -7,6 +7,10 @@
 
 import com.stuypulse.robot.commands.auton.DoNothingAuton;
 import com.stuypulse.robot.constants.Ports;
+import com.stuypulse.robot.subsystems.odometry.Odometry;
+import com.stuypulse.robot.subsystems.swerve.SwerveDrive;
+import com.stuypulse.robot.subsystems.vision.AprilTagVision;
+import com.stuypulse.robot.subsystems.vision.NoteVision;
 import com.stuypulse.robot.subsystems.intake.Intake;
 import com.stuypulse.robot.subsystems.shooter.Shooter;
 import com.stuypulse.robot.subsystems.conveyor.Conveyor;
@@ -24,6 +28,11 @@ public class RobotContainer {
     public final Gamepad operator = new AutoGamepad(Ports.Gamepad.OPERATOR);
 
     // Subsystem
+
+    public final SwerveDrive swerve = SwerveDrive.getInstance();
+    public final Odometry odometry = Odometry.getInstance();
+    public final AprilTagVision vision = AprilTagVision.getInstance();
+    public final NoteVision noteVision = NoteVision.getInstance();
     public final Intake intake = Intake.getInstance();
     public final Shooter shooter = Shooter.getInstance();
     public final Conveyor conveyor = Conveyor.getInstance();

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

@@ -0,0 +1,57 @@
+package com.stuypulse.robot.commands.swerve;
+
+import com.stuypulse.robot.constants.Settings;
+import com.stuypulse.robot.constants.Settings.Driver.Drive;
+import com.stuypulse.robot.constants.Settings.Driver.Turn;
+import com.stuypulse.robot.subsystems.swerve.SwerveDrive;
+import com.stuypulse.stuylib.input.Gamepad;
+import com.stuypulse.stuylib.math.SLMath;
+import com.stuypulse.stuylib.streams.numbers.IStream;
+import com.stuypulse.stuylib.streams.numbers.filters.RateLimit;
+import com.stuypulse.stuylib.streams.vectors.VStream;
+import com.stuypulse.stuylib.streams.vectors.filters.VDeadZone;
+import com.stuypulse.stuylib.streams.vectors.filters.VLowPassFilter;
+import com.stuypulse.stuylib.streams.vectors.filters.VRateLimit;
+
+import edu.wpi.first.wpilibj2.command.Command;
+
+public class SwerveDriveDrive extends Command {
+
+    private SwerveDrive swerve;
+
+    private VStream speed; 
+    private IStream turn;
+
+    private final Gamepad driver;
+
+    public SwerveDriveDrive(Gamepad driver) {
+        this.driver = driver;
+
+        swerve = SwerveDrive.getInstance();
+
+        speed = VStream.create(driver::getLeftStick)
+            .filtered(
+                new VDeadZone(Drive.DEADBAND),
+                x -> x.clamp(1),
+                x -> Settings.vpow(x, Drive.POWER.get()),
+                x -> x.mul(Drive.MAX_TELEOP_SPEED.get()),
+                new VRateLimit(Drive.MAX_TELEOP_ACCEL.get()),
+                new VLowPassFilter(Drive.RC.get())
+        );
+
+        turn = IStream.create(driver::getRightX)
+            .filtered(
+                x -> SLMath.deadband(x, Turn.DEADBAND.get()),
+                x -> SLMath.spow(x, Turn.POWER.get()),
+                x -> x * Turn.MAX_TELEOP_TURNING.get(),
+                new RateLimit(Turn.RC.get())
+        ); 
+
+        addRequirements(swerve);
+    }
+
+    @Override
+    public void execute() {
+        swerve.drive(speed.get(), turn.get());
+    }
+}

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

@@ -0,0 +1,50 @@
+package com.stuypulse.robot.constants;
+
+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;
+
+/**
+ * This interface stores information about each camera.
+ */
+public interface Cameras {
+
+    public interface Limelight {
+        String [] LIMELIGHTS = {
+            "limelight"
+        };
+
+        int[] PORTS = { 5800, 5801, 5802, 5803, 5804, 5805 };
+
+        Pose3d [] POSITIONS = new Pose3d[] {
+            new Pose3d(
+                new Translation3d(Units.inchesToMeters(0), 0, Units.inchesToMeters(0)),
+                new Rotation3d(0, Math.toRadians(0), Math.toRadians(0))
+            )
+        };
+    }
+
+    public CameraConfig[] APRILTAG_CAMERAS = new CameraConfig[] {
+        new CameraConfig("samera0", new Pose3d(new Translation3d(), new Rotation3d())),
+        new CameraConfig("samera1", new Pose3d(new Translation3d(), new Rotation3d()))
+    };
+
+    public static class CameraConfig {
+        private String name;
+        private Pose3d location;
+
+        public CameraConfig(String name, Pose3d location) {
+            this.name = name;
+            this.location = location;
+        }
+
+        public String getName() {
+            return name;
+        }
+
+        public Pose3d getLocation() {
+            return location;
+        }
+    }
+}

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

@@ -0,0 +1,87 @@
+package com.stuypulse.robot.constants;
+
+import java.util.ArrayList;
+
+import com.stuypulse.robot.util.Fiducial;
+
+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;
+
+/**
+ * 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);
+
+    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)))),
+        new Fiducial(2,  new Pose3d(new Translation3d(Units.inchesToMeters(637.21), Units.inchesToMeters(34.79), Units.inchesToMeters(53.38)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(120)))),
+        new Fiducial(3,  new Pose3d(new Translation3d(Units.inchesToMeters(652.73), Units.inchesToMeters(196.17), Units.inchesToMeters(57.13)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(180)))),
+        new Fiducial(4,  new Pose3d(new Translation3d(Units.inchesToMeters(652.73), Units.inchesToMeters(218.42), Units.inchesToMeters(57.13)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(180)))),
+        new Fiducial(5,  new Pose3d(new Translation3d(Units.inchesToMeters(578.77), Units.inchesToMeters(323.0), Units.inchesToMeters(53.38)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(270)))),
+        new Fiducial(6,  new Pose3d(new Translation3d(Units.inchesToMeters(72.5), Units.inchesToMeters(323.0), Units.inchesToMeters(53.38)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(270)))),
+        new Fiducial(7,  new Pose3d(new Translation3d(Units.inchesToMeters(-1.5), Units.inchesToMeters(218.42), Units.inchesToMeters(57.13)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(0)))),
+        new Fiducial(8,  new Pose3d(new Translation3d(Units.inchesToMeters(-1.5), Units.inchesToMeters(196.17), Units.inchesToMeters(57.13)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(0)))),
+        new Fiducial(9,  new Pose3d(new Translation3d(Units.inchesToMeters(14.02), Units.inchesToMeters(34.79), Units.inchesToMeters(53.38)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(60)))),
+        new Fiducial(10, new Pose3d(new Translation3d(Units.inchesToMeters(57.54), Units.inchesToMeters(9.68), Units.inchesToMeters(53.38)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(60)))),
+        new Fiducial(11, new Pose3d(new Translation3d(Units.inchesToMeters(468.69), Units.inchesToMeters(146.19), Units.inchesToMeters(52.0)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(300)))),
+        new Fiducial(12, new Pose3d(new Translation3d(Units.inchesToMeters(468.69), Units.inchesToMeters(177.10), Units.inchesToMeters(52.0)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(60)))),
+        new Fiducial(13, new Pose3d(new Translation3d(Units.inchesToMeters(441.74), Units.inchesToMeters(161.62), Units.inchesToMeters(52.0)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(180)))),
+        new Fiducial(14, new Pose3d(new Translation3d(Units.inchesToMeters(209.48), Units.inchesToMeters(161.62), Units.inchesToMeters(52.0)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(0)))),
+        new Fiducial(15, new Pose3d(new Translation3d(Units.inchesToMeters(182.73), Units.inchesToMeters(177.10), Units.inchesToMeters(52.0)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(120)))),
+        new Fiducial(16, new Pose3d(new Translation3d(Units.inchesToMeters(182.73), Units.inchesToMeters(146.19), Units.inchesToMeters(52.0)), new Rotation3d(Units.degreesToRadians(0), Units.degreesToRadians(0), Units.degreesToRadians(240)))),
+    };
+
+    public static boolean isValidFiducial(int fid) {
+        for(Fiducial fiducial : FIDUCIALS) { 
+            if (fiducial.getFID() == fid) { 
+                return true;
+            }
+        }
+        return false;
+    }
+
+    public static Fiducial[] getFiducialLayout(int... fids) {
+        ArrayList<Fiducial> fiducials = new ArrayList<Fiducial>();
+
+        for (int fid : fids) {
+            for (Fiducial fiducial : FIDUCIALS) {
+                if (fiducial.getFID() == fid) { 
+                    fiducials.add(fiducial);
+                }
+            }
+        }
+
+        Fiducial[] fiducials_array = new Fiducial[fiducials.size()];
+        return fiducials.toArray(fiducials_array);
+    }
+
+    public static double[] getLayoutAsDoubleArray(Fiducial[] fiducials) {
+        double[] layout = new double[fiducials.length * 7];
+        for (int i = 0; i < fiducials.length; i++) {
+            layout[i * 7 + 0] = fiducials[i].getFID();
+            layout[i * 7 + 1] = fiducials[i].getLocation().getX();
+            layout[i * 7 + 2] = fiducials[i].getLocation().getY();
+            layout[i * 7 + 3] = fiducials[i].getLocation().getZ();
+            layout[i * 7 + 4] = fiducials[i].getLocation().getRotation().getX();
+            layout[i * 7 + 5] = fiducials[i].getLocation().getRotation().getY();
+            layout[i * 7 + 6] = fiducials[i].getLocation().getRotation().getZ();
+        }
+        return layout;
+    }
+
+    public static Fiducial getFiducial(int fid) {
+        for (Fiducial fiducial : FIDUCIALS) {
+            if (fiducial.getFID() == fid) {
+                return fiducial;
+            }
+        }
+        return null;
+    }
+}

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

@@ -8,6 +8,7 @@
 import com.ctre.phoenix.motorcontrol.NeutralMode;
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
 import com.ctre.phoenix.motorcontrol.can.WPI_VictorSPX;
+
 import com.revrobotics.CANSparkMax;
 import static com.revrobotics.CANSparkMax.IdleMode;
 
@@ -21,8 +22,11 @@
  *  - The Open Loop Ramp Rate
  */
 public interface Motors {
-
     /** Classes to store all of the values a motor needs */
+    public interface Swerve {
+        public CANSparkMaxConfig DRIVE_CONFIG = new CANSparkMaxConfig(false, IdleMode.kBrake);
+        public CANSparkMaxConfig TURN_CONFIG = new CANSparkMaxConfig(false, IdleMode.kBrake);
+    }
 
     public interface Intake {
         CANSparkMaxConfig MOTOR_CONFIG = new CANSparkMaxConfig(false, IdleMode.kBrake, 80, 0.1);

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

@@ -13,6 +13,32 @@ public interface Gamepad {
         int DEBUGGER = 2;
     }
 
+    public interface Swerve {
+        public interface FrontRight {
+            int DRIVE = 10;
+            int TURN = 11;
+            int ENCODER = 1;
+        }
+
+        public interface FrontLeft {
+            int DRIVE = 12;
+            int TURN = 13;
+            int ENCODER = 2;
+        }
+
+        public interface BackLeft{
+            int DRIVE = 14;
+            int TURN = 15;
+            int ENCODER = 3;
+        }
+
+        public interface BackRight {
+            int DRIVE = 16;
+            int TURN = 17;
+            int ENCODER = 4;
+        }
+    }
+
     public interface Intake {
         int MOTOR = 40;
         int SENSOR = 1;