Merge pull request #22 from aaditsangvikar/main

Updating GradleRIO version and adding NoteVisionRotation

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"
 }
 
 java {

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

@@ -4,27 +4,15 @@
 
 package frc.robot;
 
-import edu.wpi.first.math.VecBuilder;
-import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
-import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.kinematics.SwerveModulePosition;
-import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
+import frc.robot.subsystems.*;
 import frc.robot.commands.*;
-import frc.robot.constants.DrivetrainConstants;
-import frc.robot.subsystems.IndexerSubsystem;
-import frc.robot.subsystems.IntakeSubsystem;
-import frc.robot.subsystems.ShooterSubsystem;
-import frc.robot.commands.DriveCommands;
+import frc.robot.constants.*;
 
-import frc.robot.commands.PathCommand;
-import frc.robot.constants.DrivetrainConstants;
-import frc.robot.constants.LEDConstants;
-import frc.robot.subsystems.SwerveSubsystem;
 
 /**
  * This class is where the bulk of the robot should be declared. Since Command-based is a
@@ -67,9 +55,7 @@ public void configureButtonBindings(){
 
     // Primary controller
     new JoystickButton(primaryController, XboxController.Button.kY.value).whileTrue(
-            new RunCommand(() -> {
-              swerveSubsystem.zeroGyro();
-            })
+            new RunCommand(() -> swerveSubsystem.zeroGyro())
     );
 
     // Secondary controller
@@ -85,7 +71,7 @@ public void configureButtonBindings(){
             new AmpCommand(indexerSubsystem, secondaryController)
     );
 
-    new JoystickButton(secondaryController, XboxController.Button.kBack.value).whileTrue(
+    new JoystickButton(secondaryController, XboxController.Button.kB.value).whileTrue(
             new AmpCommand2(indexerSubsystem, secondaryController) // This is kinda fucked
     );
 

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

@@ -21,6 +21,7 @@ public class PathCommand extends Command {
 
     public PathCommand(SwerveSubsystem swerveSubsystem) {
         this.swerveSubsystem = swerveSubsystem;
+        addRequirements(swerveSubsystem);
     }
 
     @Override

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

@@ -0,0 +1,69 @@
+package frc.robot.commands;
+
+import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.constants.VisionConstants;
+import frc.robot.subsystems.SwerveSubsystem;
+import frc.robot.utils.VisionUtils;
+import java.util.function.DoubleSupplier;
+
+public class RotateToNoteAndDriveCommand extends Command {
+    private SwerveSubsystem swerveSubsystem;
+    private final DoubleSupplier forward;
+    private final DoubleSupplier sideways;
+    private final DoubleSupplier radians;
+    private final double visionRotation;
+
+    private final ProfiledPIDController rotationPID = new ProfiledPIDController(
+            VisionConstants.rotateToNoteP,
+            VisionConstants.rotateToNoteI,
+            VisionConstants.rotateToNoteD,
+            VisionConstants.rotateToNoteConstraints
+    );
+
+    private final XboxController controller;
+
+    public RotateToNoteAndDriveCommand(SwerveSubsystem swerveSubsystem, XboxController controller, DoubleSupplier forward, DoubleSupplier sideways, DoubleSupplier radians) {
+        this.swerveSubsystem = swerveSubsystem;
+        this.forward = forward;
+        this.sideways = sideways;
+        this.radians = radians;
+        this.controller = controller;
+
+        rotationPID.setTolerance(0.05);
+
+        this.visionRotation = rotationPID.calculate(VisionUtils.getNoteTX(), 0.0);
+
+        addRequirements(swerveSubsystem);
+    }
+
+
+    @Override
+    public void execute() {
+        if (VisionUtils.getNoteTV() == 1.0) {
+            swerveSubsystem.drive(
+                    forward.getAsDouble(),
+                    sideways.getAsDouble(),
+                    visionRotation,
+                    true,
+                    true
+            );
+        } else {
+            swerveSubsystem.drive(
+                    forward.getAsDouble(),
+                    sideways.getAsDouble(),
+                    radians.getAsDouble(),
+                    true,
+                    true
+            );
+            controller.setRumble(GenericHID.RumbleType.kBothRumble, 1.0);
+        }
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        controller.setRumble(GenericHID.RumbleType.kBothRumble, 0.0);
+    }
+}

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

@@ -10,7 +10,9 @@ public class SetLEDCommand extends Command {
     private final LEDConstants.Status status;
     public SetLEDCommand(LEDSubsystem subsystem, LEDConstants.Status status) {
         this.status = status;
+
         this.subsystem = subsystem;
+        addRequirements(subsystem);
     }
 
     @Override

src/main/java/frc/robot/constants/VisionConstants.java

@@ -17,4 +17,9 @@ public class VisionConstants {
     public static final double alignRotationI = 0;
     public static final double alignRotationD = 0;
     public static final TrapezoidProfile.Constraints alignRotationConstraints = new TrapezoidProfile.Constraints(0.75, 0.5);
+
+    public static final double rotateToNoteP = 0.6;
+    public static final double rotateToNoteI = 0;
+    public static final double rotateToNoteD = 0;
+    public static final TrapezoidProfile.Constraints rotateToNoteConstraints = new TrapezoidProfile.Constraints(Math.PI, Math.PI/2);
 }

src/main/java/frc/robot/subsystems/SwerveSubsystem.java

@@ -279,6 +279,7 @@ public ChassisSpeeds getRobotRelativeSpeeds() {
                 rearLeft.getState(),
                 rearRight.getState()
         );
+
     }
 
     /**

src/main/java/frc/robot/utils/NetworkTableUtils.java

@@ -8,7 +8,9 @@ public class NetworkTableUtils {
 
     /**
      * This class is a utility class for interfacing with network tables
+
      * @param table The string ID for a network table
+
      */
     public NetworkTableUtils(String table) {
         this.table = NetworkTableInstance.getDefault().getTable(table);

src/main/java/frc/robot/utils/VisionUtils.java

@@ -5,8 +5,17 @@
 import edu.wpi.first.networktables.NetworkTableInstance;
 
 import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableEntry;
+import edu.wpi.first.networktables.NetworkTableInstance;
 
 public class VisionUtils {
+
+    //Note Vision Limelight
+    public static NetworkTable noteLimelight = NetworkTableInstance.getDefault().getTable("limelight-note");
+    public static double height = 0.4;
+    public static double forward = 0.4;
+
     /**
      * Gets the requested entry from as well as optionally adding the alliance. Used internally
      * @param pose The pose you want as defined by https://docs.limelightvision.io/docs/docs-limelight/apis/complete-networktables-api#apriltag-and-3d-data
@@ -67,4 +76,33 @@ public static double getLatencyPipeline() {
     public static double getLatencyCapture() {
         return NetworkTableInstance.getDefault().getTable("limelight").getEntry("cl").getDouble(0.0);
     }
+
+    public static double getNoteTX(){
+        return noteLimelight.getEntry("tx").getDouble(0.0);
+    }
+
+    public static double getNoteTY() {
+        return noteLimelight.getEntry("ty").getDouble(0.0);
+    }
+
+    public static double getNoteTV(){
+        return noteLimelight.getEntry("tv").getDouble(0.0);
+    }
+
+    public static double YDistanceToNote() {
+        return height * Math.tan(getNoteTY());
+    }
+
+    public static double XDistanceToNote() {
+        return YDistanceToNote() * Math.tan(getNoteTX());
+    }
+
+    public static double distanceToNote() {
+        return Math.sqrt(
+                Math.pow(YDistanceToNote(), 2)
+                + Math.pow(XDistanceToNote(), 2)
+        );
+    }
+
+
 }