feat: limelight

src/main/java/frc/robot/RobotContainer.java

@@ -32,6 +32,7 @@
 import frc.robot.subsystems.drive.ModuleIOSim;
 import frc.robot.subsystems.drive.ModuleIOSparkMax;
 import frc.robot.subsystems.shooter.*;
+import frc.robot.util.Limelight;
 import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
 import org.littletonrobotics.junction.networktables.LoggedDashboardNumber;
 
@@ -47,6 +48,8 @@ public class RobotContainer {
   private final Flywheel flywheel;
   private final Shooter shooter;
 
+  private final Limelight limelight = new Limelight("limelight");
+
   // Controller
   private final CommandXboxController controller = new CommandXboxController(0);
 
@@ -174,6 +177,13 @@ private void configureButtonBindings() {
         .whileTrue(
             Commands.startEnd(
                 () -> flywheel.runVelocity(flywheelSpeedInput.get()), flywheel::stop, flywheel));
+    controller.x()
+            .whileTrue(DriveCommands.LimelightRotDrive(
+              drive,
+              () -> -controller.getLeftY(),
+              () -> -controller.getLeftX(),
+                    limelight))
+            .whileTrue(new ShooterCommands.Shoot(shooter,limelight));
   }
 
   /**

src/main/java/frc/robot/commands/DriveCommands.java

@@ -22,6 +22,8 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 import frc.robot.subsystems.drive.Drive;
+import frc.robot.util.Limelight;
+
 import java.util.function.DoubleSupplier;
 
 public class DriveCommands {
@@ -67,4 +69,39 @@ public static Command joystickDrive(
         },
         drive);
   }
+
+    public static Command LimelightRotDrive(
+            Drive drive,
+            DoubleSupplier xSupplier,
+            DoubleSupplier ySupplier,
+            Limelight limelight) {
+        return Commands.run(
+                () -> {
+                    // Apply deadband
+                    double linearMagnitude =
+                            MathUtil.applyDeadband(
+                                    Math.hypot(xSupplier.getAsDouble(), ySupplier.getAsDouble()), DEADBAND);
+                    Rotation2d linearDirection =
+                            new Rotation2d(xSupplier.getAsDouble(), ySupplier.getAsDouble());
+                    double omega = MathUtil.applyDeadband(limelight.getTx() * limelight.getHorizontalFov()/(2*100), 0.01);
+                    // Square values
+                    linearMagnitude = linearMagnitude * linearMagnitude;
+                    omega = Math.copySign(omega * omega, omega);
+
+                    // Calcaulate new linear velocity
+                    Translation2d linearVelocity =
+                            new Pose2d(new Translation2d(), linearDirection)
+                                    .transformBy(new Transform2d(linearMagnitude, 0.0, new Rotation2d()))
+                                    .getTranslation();
+
+                    // Convert to field relative speeds & send command
+                    drive.runVelocity(
+                            ChassisSpeeds.fromFieldRelativeSpeeds(
+                                    linearVelocity.getX() * drive.getMaxLinearSpeedMetersPerSec(),
+                                    linearVelocity.getY() * drive.getMaxLinearSpeedMetersPerSec(),
+                                    omega * drive.getMaxAngularSpeedRadPerSec(),
+                                    drive.getRotation()));
+                },
+                drive);
+    }
 }

src/main/java/frc/robot/commands/ShooterCommands.java

@@ -2,6 +2,7 @@
 
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.shooter.Shooter;
+import frc.robot.util.Limelight;
 
 public class ShooterCommands {
   public static class Shoot extends Command {
@@ -10,6 +11,8 @@ public static class Shoot extends Command {
     double mainRPM;
     double secondaryRPM;
 
+    Limelight limelight;
+
     enum State {
       INIT,
       SPINUP,
@@ -19,24 +22,28 @@ enum State {
 
     State state = State.SPINUP;
 
-    public Shoot(Shooter shooter) {
+    public Shoot(Shooter shooter, Limelight limelight) {
       this.shooter = shooter;
       addRequirements(shooter);
+
+      this.limelight = limelight;
     }
 
     @Override
     public void execute() {
-
       switch (state) {
         case INIT -> {
           shooter.setLoadingVoltage(-8);
           state = State.SPINUP;
         }
         case SPINUP -> {
+          //TODO calculation based on distance;
+
           shooter.setMainVelocity(mainRPM);
           shooter.setSecondaryVelocity(secondaryRPM);
           if (shooter.getMainVelocityRPM() == mainRPM
-              && shooter.getSecondaryVelocityRPM() == secondaryRPM) {
+              && shooter.getSecondaryVelocityRPM() == secondaryRPM
+              && limelight.aimed()) {
             shooter.setLoadingVoltage(10);
             state = State.SHOOTING;
           }

src/main/java/frc/robot/util/Limelight.java

@@ -0,0 +1,61 @@
+package frc.robot.util;
+
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableInstance;
+
+public class Limelight {
+    private double tv, tx, ty, ta;
+
+    private final double mountingAngleDegrees = 10.0;
+    private final double limelightLensHeightInches = 20.0;//TODO Find this
+    private final double goalHeightInches = 25.0;//TODO Find this
+    private final NetworkTable table = NetworkTableInstance.getDefault().getTable("limelight");
+    //private final AHRS navx;
+    public final String name;
+    private double[] data;
+//    private final PoseZeroFilter zeroFilter = new PoseZeroFilter(50, 48);
+//    private final PoseMedianFilter medianFilter = new PoseMedianFilter(10);
+    private double latency;
+    private static  double HORIZONTAL_FOV = Math.toRadians(63.3);
+    private static  double VERTICAL_FOV = Math.toRadians(49.7);
+
+    public Limelight(String name) {
+        this.name = name;
+
+    }
+
+    public void update(){
+        tv = table.getEntry("tv").getDouble(0);
+        tx = table.getEntry("tx").getDouble(0);
+        ty = table.getEntry("ty").getDouble(0);
+        ta = table.getEntry("ta").getDouble(0);
+    }
+
+    public double getTa() {
+        return ta;
+    }
+
+    public double getTx() {
+        return tx;
+    }
+
+    public double getTy() {
+        return ty;
+    }
+
+    public double getTv() {
+        return tv;
+    }
+
+    public double getDistance(){
+        return (goalHeightInches - limelightLensHeightInches)/Math.tan(Math.toRadians(ty + mountingAngleDegrees));
+    }
+
+    public boolean aimed(){
+        return tx < 1;
+    }
+
+    public double getHorizontalFov() {
+        return HORIZONTAL_FOV;
+    }
+}