Merge branch 'main' into se/amp-trap-lift

StuyPulse · Jan 26, 2024 · a03aa1a · a03aa1a
2 parents e8ff583 + 3daa165
commit a03aa1a
Show file tree

Hide file tree

Showing 34 changed files with 2,252 additions and 4 deletions.
diff --git a/build.gradle b/build.gradle
@@ -1,6 +1,6 @@
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2024.1.1"
+    id "edu.wpi.first.GradleRIO" version "2024.2.1"
     id "com.diffplug.spotless" version "6.22.0"
 }
 

diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java
@@ -8,6 +8,13 @@
 import com.stuypulse.robot.commands.auton.DoNothingAuton;
 import com.stuypulse.robot.constants.Ports;
 import com.stuypulse.robot.subsystems.amper.Amper;
+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;
 import com.stuypulse.stuylib.input.Gamepad;
 import com.stuypulse.stuylib.input.gamepads.AutoGamepad;
 
@@ -23,7 +30,14 @@ public class RobotContainer {
 
     // Subsystem
     public final Amper amper = Amper.getInstance();
-
+    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();
+
     // Autons
     private static SendableChooser<Command> autonChooser = new SendableChooser<>();
 

diff --git a/src/main/java/com/stuypulse/robot/commands/intake/IntakeAcquire.java b/src/main/java/com/stuypulse/robot/commands/intake/IntakeAcquire.java
@@ -0,0 +1,30 @@
+package com.stuypulse.robot.commands.intake;
+
+import com.stuypulse.robot.subsystems.intake.Intake;
+
+import edu.wpi.first.wpilibj2.command.Command;
+
+public class IntakeAcquire extends Command{
+
+    private Intake intake;
+
+    public IntakeAcquire() {
+        intake = Intake.getInstance();
+        addRequirements(intake);
+    }
+
+    @Override
+    public void initialize() {
+        intake.acquire();
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        intake.stop();
+    }
+
+    @Override
+    public boolean isFinished() {
+        return intake.hasNote();
+    }
+}
diff --git a/src/main/java/com/stuypulse/robot/commands/intake/IntakeAcquireForever.java b/src/main/java/com/stuypulse/robot/commands/intake/IntakeAcquireForever.java
@@ -0,0 +1,20 @@
+package com.stuypulse.robot.commands.intake;
+
+import com.stuypulse.robot.subsystems.intake.Intake;
+
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+
+public class IntakeAcquireForever extends InstantCommand {
+
+    private Intake intake;
+
+    public IntakeAcquireForever() {
+        intake = Intake.getInstance();
+        addRequirements(intake);
+    }
+
+    @Override
+    public void initialize() {
+        intake.acquire();
+    }
+}
diff --git a/src/main/java/com/stuypulse/robot/commands/intake/IntakeDeacquire.java b/src/main/java/com/stuypulse/robot/commands/intake/IntakeDeacquire.java
@@ -0,0 +1,19 @@
+package com.stuypulse.robot.commands.intake;
+
+import com.stuypulse.robot.subsystems.intake.Intake;
+
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+
+public class IntakeDeacquire extends InstantCommand {
+    private Intake intake;
+
+    public IntakeDeacquire() {
+        intake = Intake.getInstance();
+        addRequirements(intake);
+    }
+
+    @Override
+    public void initialize() {
+        intake.deacquire();
+    }
+}
diff --git a/src/main/java/com/stuypulse/robot/commands/intake/IntakeStop.java b/src/main/java/com/stuypulse/robot/commands/intake/IntakeStop.java
@@ -0,0 +1,20 @@
+package com.stuypulse.robot.commands.intake;
+
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+
+import com.stuypulse.robot.subsystems.intake.Intake;
+
+public class IntakeStop extends InstantCommand {
+
+    private Intake intake;
+
+    public IntakeStop() {
+        intake = Intake.getInstance();
+        addRequirements(intake);
+    }
+
+    @Override
+    public void initialize() {
+        intake.stop();
+    }
+}
diff --git a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterPodiumShot.java b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterPodiumShot.java
@@ -0,0 +1,10 @@
+package com.stuypulse.robot.commands.shooter;
+
+import com.stuypulse.robot.constants.Settings;
+
+public class ShooterPodiumShot extends ShooterSetRPM{
+
+    public ShooterPodiumShot() {
+        super(Settings.Shooter.PODIUM_SHOT_LEFT_RPM, Settings.Shooter.PODIUM_SHOT_RIGHT_RPM);
+    }
+}
diff --git a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterSetRPM.java b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterSetRPM.java
@@ -0,0 +1,27 @@
+package com.stuypulse.robot.commands.shooter;
+
+import com.stuypulse.robot.subsystems.shooter.Shooter;
+
+import edu.wpi.first.wpilibj2.command.InstantCommand;
+
+public class ShooterSetRPM extends InstantCommand {
+    private final Shooter shooter;
+    private final Number leftTargetRPM;
+    private final Number rightTargetRPM;
+
+    public ShooterSetRPM(Number leftTargetRPM, Number rightTargetRPM) {
+        shooter = Shooter.getInstance();
+        this.leftTargetRPM = leftTargetRPM;
+        this.rightTargetRPM = rightTargetRPM;
+
+        addRequirements(shooter);
+    }
+
+    @Override
+    public void initialize() {
+        shooter.setLeftTargetRPM(leftTargetRPM);
+        shooter.setRightTargetRPM(rightTargetRPM);
+    }
+
+}
+
diff --git a/src/main/java/com/stuypulse/robot/commands/shooter/ShooterStop.java b/src/main/java/com/stuypulse/robot/commands/shooter/ShooterStop.java
@@ -0,0 +1,8 @@
+package com.stuypulse.robot.commands.shooter;
+
+public class ShooterStop extends ShooterSetRPM {    
+
+    public ShooterStop(){
+        super(0, 0);
+    }
+}
diff --git a/src/main/java/com/stuypulse/robot/commands/swerve/SwerveDriveDrive.java b/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());
+    }
+}
diff --git a/src/main/java/com/stuypulse/robot/constants/Cameras.java b/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;
+        }
+    }
+}
diff --git a/src/main/java/com/stuypulse/robot/constants/Field.java b/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;
+    }
+}