From 4a6d94505fd54f9551f08c8857780d26e906fc8d Mon Sep 17 00:00:00 2001 From: Simon Weil <113215817+WeilSimon@users.noreply.github.com> Date: Tue, 5 Dec 2023 14:06:15 -0800 Subject: [PATCH] feat: limelight --- src/main/java/frc/robot/RobotContainer.java | 10 +++ .../frc/robot/commands/DriveCommands.java | 37 +++++++++++ .../frc/robot/commands/ShooterCommands.java | 13 +++- src/main/java/frc/robot/util/Limelight.java | 61 +++++++++++++++++++ 4 files changed, 118 insertions(+), 3 deletions(-) create mode 100644 src/main/java/frc/robot/util/Limelight.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index b3b116e..37591ec 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/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)); } /** diff --git a/src/main/java/frc/robot/commands/DriveCommands.java b/src/main/java/frc/robot/commands/DriveCommands.java index 638744a..ceeacd0 100644 --- a/src/main/java/frc/robot/commands/DriveCommands.java +++ b/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); + } } diff --git a/src/main/java/frc/robot/commands/ShooterCommands.java b/src/main/java/frc/robot/commands/ShooterCommands.java index c1ef0e3..6302cfa 100644 --- a/src/main/java/frc/robot/commands/ShooterCommands.java +++ b/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; } diff --git a/src/main/java/frc/robot/util/Limelight.java b/src/main/java/frc/robot/util/Limelight.java new file mode 100644 index 0000000..40ba7a1 --- /dev/null +++ b/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; + } +}