April tags better

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

@@ -13,8 +13,11 @@
 import java.util.function.BooleanSupplier;
 import java.util.function.DoubleSupplier;
 
+import org.littletonrobotics.urcl.URCL;
+
 import static frc.robot.settings.Constants.DriveConstants.*;
 
+import com.ctre.phoenix6.SignalLogger;
 import com.ctre.phoenix6.hardware.Pigeon2;
 import com.fasterxml.jackson.core.sym.Name;
 import com.pathplanner.lib.auto.AutoBuilder;
@@ -77,9 +80,9 @@
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.units.Angle;
+import edu.wpi.first.wpilibj.DataLogManager;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.PS4Controller;
-import frc.robot.commands.ManualShoot;
 import frc.robot.commands.AngleShooter;
 import frc.robot.commands.IntakeCommand;
 import frc.robot.settings.IntakeDirection;
@@ -138,7 +141,11 @@ public RobotContainer() {
     Preferences.initBoolean("Use Limelight", false);
     Preferences.initBoolean("Use 2 Limelights", false);
     Preferences.initDouble("wait # of seconds", 0);
-
+
+    // DataLogManager.start();
+    // URCL.start();
+    // SignalLogger.setPath("/media/sda1/ctre-logs/");
+    // SignalLogger.start();
     driverController = new PS4Controller(DRIVE_CONTROLLER_ID);
     operatorController = new PS4Controller(OPERATOR_CONTROLLER_ID);
     pigeon = new Pigeon2(DRIVETRAIN_PIGEON_ID);
@@ -204,7 +211,7 @@ private void indexInit() {
     indexer = new IndexerSubsystem();
   }
   private void indexCommandInst() {
-    defaulNoteHandlingCommand = new IndexCommand(indexer, driverController::getR2Button, driverController::getL2Button, shooter, intake, driveTrain, angleShooterSubsystem, driverController::getR1Button);
+    defaulNoteHandlingCommand = new IndexCommand(indexer, driverController::getR2Button, driverController::getL2Button, shooter, intake, driveTrain, angleShooterSubsystem, driverController::getR1Button, driverController::getPOV);
     indexer.setDefaultCommand(defaulNoteHandlingCommand);
   }
 
@@ -242,7 +249,7 @@ private void configureBindings() {
       () -> modifyAxis(-driverController.getRawAxis(Y_AXIS), DEADBAND_NORMAL),
       () -> modifyAxis(-driverController.getRawAxis(X_AXIS), DEADBAND_NORMAL),
       driverController::getL2Button));
-    new Trigger(driverController::getOptionsButton).whileTrue(new InstantCommand(driveTrain::forceUpdateOdometryWithVision));
+    new Trigger(driverController::getOptionsButton).onTrue(new InstantCommand(()->SmartDashboard.putBoolean("force use limelight", true))).onFalse(new InstantCommand(()->SmartDashboard.putBoolean("force use limelight", false)));
     new Trigger(driverController::getSquareButton).onTrue(new SequentialCommandGroup(
       new CollectNote(driveTrain, limelight),
       new DriveTimeCommand(-2, 0, 0, 0.5, driveTrain)
@@ -254,7 +261,7 @@ private void configureBindings() {
       SmartDashboard.putData("Manual Angle Shooter Up", new AngleShooter(angleShooterSubsystem, ()->ShooterConstants.MAXIMUM_SHOOTER_ANGLE));
     }
     if(indexerExists) {
-      new Trigger(driverController::getL1Button).whileTrue(new ManualShoot(indexer));
+      new Trigger(driverController::getL1Button).whileTrue(new ManualShoot(indexer, driverController::getPOV));
     }
     if(climberExists) {
       // new Trigger(driverController::getCrossButton).whileTrue(new AutoClimb(climber)).onFalse(new InstantCommand(()-> climber.climberStop()));

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

@@ -30,6 +30,7 @@ public class IndexCommand extends Command {
   BooleanSupplier revUpSupplier;
   Boolean revUp;
   BooleanSupplier shootIfReadySupplier;
+  DoubleSupplier ampSupplier;;
   Boolean shootIfReady;
   // DoubleSupplier POVSupplier;
   BooleanSupplier humanPlayerSupplier;
@@ -42,16 +43,18 @@ public class IndexCommand extends Command {
   double runsEmpty = 0;
 
   /** Creates a new IndexCommand. */
-  public IndexCommand(IndexerSubsystem m_IndexerSubsystem, BooleanSupplier shootIfReadySupplier, BooleanSupplier revUpSupplier, ShooterSubsystem shooter, IntakeSubsystem intake, DrivetrainSubsystem drivetrain, AngleShooterSubsystem angleShooterSubsystem, BooleanSupplier humanPlaySupplier) {
+  public IndexCommand(IndexerSubsystem m_IndexerSubsystem, BooleanSupplier shootIfReadySupplier, BooleanSupplier revUpSupplier, ShooterSubsystem shooter, IntakeSubsystem intake, DrivetrainSubsystem drivetrain, AngleShooterSubsystem angleShooterSubsystem, BooleanSupplier humanPlaySupplier, DoubleSupplier ampSupplier) {
     this.m_Indexer = m_IndexerSubsystem;
     this.shootIfReadySupplier = shootIfReadySupplier;
     this.revUpSupplier = revUpSupplier;
     this.shooter = shooter;
     this.intake = intake;
     this.drivetrain = drivetrain;
+    this.ampSupplier = ampSupplier;
     this.angleShooterSubsytem = angleShooterSubsystem;
     this.humanPlayerSupplier = humanPlaySupplier;
     SmartDashboard.putNumber("amp RPS", AMP_RPS);
+    SmartDashboard.putNumber("indexer amp speed", IndexerConstants.INDEXER_AMP_SPEED);
     SmartDashboard.putNumber("amp angle", Field.AMPLIFIER_ANGLE);
     // Use addRequirements() here to declare subsystem dependencies.
     addRequirements(m_IndexerSubsystem, shooter, intake);
@@ -106,7 +109,11 @@ public void execute() {
       indexer = true;
     }
     if (indexer) {
-      m_Indexer.set(IndexerConstants.INDEXER_SHOOTING_SPEED);
+      if(ampSupplier.getAsDouble() == 90) {
+        m_Indexer.set(SmartDashboard.getNumber("indexer amp speed", IndexerConstants.INDEXER_AMP_SPEED));
+      } else {
+        m_Indexer.set(IndexerConstants.INDEXER_SHOOTING_SPEED);
+      }
     } else {
       m_Indexer.off();
     }

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

@@ -6,15 +6,21 @@
 import frc.robot.settings.Constants.IndexerConstants;
 import frc.robot.settings.Constants.ShooterConstants;
 import frc.robot.subsystems.IndexerSubsystem;
+
+import java.util.function.BooleanSupplier;
+import java.util.function.DoubleSupplier;
+
 import edu.wpi.first.wpilibj2.command.Command;
 
 public class ManualShoot extends Command {
   private IndexerSubsystem indexer;
+  DoubleSupplier ampSupplier;
   /** Creates a new ManualShoot. */
-  public ManualShoot(IndexerSubsystem indexer) {
+  public ManualShoot(IndexerSubsystem indexer, DoubleSupplier ampSupplier) {
     // Use addRequirements() here to declare subsystem dependencies.
     addRequirements(indexer);
     this.indexer = indexer;
+    this.ampSupplier = ampSupplier;
   }
 
   // Called when the command is initially scheduled.
@@ -24,7 +30,11 @@ public void initialize() {}
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
-    indexer.set(IndexerConstants.INDEXER_SHOOTING_SPEED);
+    if(ampSupplier.getAsDouble() == 90 || ampSupplier.getAsDouble() == 45|| ampSupplier.getAsDouble() == 135) {
+      indexer.set(IndexerConstants.INDEXER_AMP_SPEED);
+    } else {
+      indexer.set(IndexerConstants.INDEXER_SHOOTING_SPEED);
+    }
   }
 
   // Called once the command ends or is interrupted.

src/main/java/frc/robot/settings/Constants.java

@@ -108,7 +108,7 @@ private DriveConstants() {
     /**
      * The diameter of the module's wheel in meters.
      */
-    public static final double DRIVETRAIN_WHEEL_DIAMETER = 0.097;
+    public static final double DRIVETRAIN_WHEEL_DIAMETER = 0.098;
 
     /**
      * The overall drive reduction of the module. Multiplying motor rotations by
@@ -272,7 +272,8 @@ public static final class ShooterConstants{
   public static final double AUTO_AIM_ROBOT_kD = 0.00;
 
   public static final double SHOOTING_RPS = 90;
-  public static final double AMP_RPS = 6.5;
+  public static final double AMP_RPS = 7.5;
+
 
   //the PID values used on the PID loop on the motor controller that control the position of the shooter angle
   public static final double ANGLE_SHOOTER_POWER_KP = 0.026;
@@ -291,7 +292,7 @@ public static final class ShooterConstants{
   public static final double DISTANCE_MULTIPLIER = 0.15;
   public static final double OFFSET_MULTIPLIER = 1;
   public static final double MINIMUM_SHOOTER_ANGLE = 10;//still has to be found
-  public static final double MAXIMUM_SHOOTER_ANGLE = 98;//still has to be found
+  public static final double MAXIMUM_SHOOTER_ANGLE = 104;//still has to be found
   public static final double HUMAN_PLAYER_ANGLE = 97;//still has to be found
   public static final double HUMAN_PLAYER_RPS = -10;//still has to be found
 
@@ -327,9 +328,10 @@ public static final class ClimberConstants{
 }
 public static final class IndexerConstants{
   public static final int INDEXER_MOTOR = 11;
-  public static final double INDEXER_INTAKE_SPEED = -0.25;//should be 0.5 TODO change to positive
+  public static final double INDEXER_INTAKE_SPEED = 0.5;//should be 0.5 TODO change to positive
   public static final double HUMAN_PLAYER_INDEXER_SPEED = -0.5;//should be 0.5 TODO change to positive
   public static final double INDEXER_SHOOTING_SPEED = 1;
+  public static final double INDEXER_AMP_SPEED = 0.4;
 }
 public static final class IntakeConstants{
   public static final int INTAKE_1_MOTOR = 20;
@@ -418,15 +420,17 @@ public final class Field{
     public static final double SPEAKER_Z = 2.08; //height of opening
     public static final double MAX_SHOOTING_DISTANCE = 2491;
 
-    public static final double AMPLIFIER_ANGLE = 97;
+    public static final double AMPLIFIER_ANGLE = 101;
     //angle at 60 for bounce techinque, didn't work
   }
 
 public final class Vision{
   public static final String APRILTAG_LIMELIGHT2_NAME = "limelight-aprill";
   public static final String APRILTAG_LIMELIGHT3_NAME = "limelight-aprilr";
   public static final String OBJ_DETECITON_LIMELIGHT_NAME = "limelight-neural";
-
+
+  public static final double APRILTAG_CLOSENESS = 0.5;
+  public static final double MAX_TAG_DISTANCE = 3.05;
 
   public static final Translation2d fieldCorner = new Translation2d(16.54, 8.02);
 

src/main/java/frc/robot/settings/LimelightFiducialData.java

@@ -35,7 +35,7 @@ public Pose2d getbotPose(){
          * @return true if the most recent vision pose estimation is inside the field and is close enough to the provided pose.
          */
         public boolean isPoseTrustworthy(Pose2d robotPose){
-            Pose2d poseEstimate = this.getbotPose();
+            Pose2d poseEstimate = this.botPoseBlue;
             if ((poseEstimate.getX()<Vision.fieldCorner.getX() && poseEstimate.getY()<Vision.fieldCorner.getY()) //Don't trust estimations that are outside the field perimeter.
                 && robotPose.getTranslation().getDistance(poseEstimate.getTranslation()) < 0.5) //Dont trust pose estimations that are more than half a meter from current pose.
                 return true;

src/main/java/frc/robot/settings/LimelightValues.java

@@ -11,6 +11,9 @@
 import edu.wpi.first.wpilibj.Timer;
 import frc.robot.LimelightHelpers;
 import frc.robot.LimelightHelpers.Results;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import static frc.robot.settings.Constants.Vision.MAX_TAG_DISTANCE;
+import static frc.robot.settings.Constants.Vision.APRILTAG_CLOSENESS;
 
 import edu.wpi.first.networktables.NetworkTableInstance;
 
@@ -24,6 +27,7 @@ public class LimelightValues {
         double[] ta = new double[5];
         Pose2d botPoseRed;
         Pose2d botPoseBlue;
+        double tagDistance;
 
         private final Translation2d fieldCorner = new Translation2d(16.54, 8.02);
 
@@ -36,9 +40,13 @@ public LimelightValues(Results llresults, boolean valid){
                     this.tx[i] = llresults.targets_Fiducials[i].tx;
                     this.ty[i] = llresults.targets_Fiducials[i].ty;
                     this.ta[i] = llresults.targets_Fiducials[i].ta;
+                    this.tagDistance = llresults.targets_Fiducials[0].getTargetPose_RobotSpace2D().getTranslation().getNorm();
                 }
                 this.botPoseRed = llresults.getBotPose2d_wpiRed();
                 this.botPoseBlue = llresults.getBotPose2d_wpiBlue();
+                // SmartDashboard.putNumber("VISION dist to apriltag [0]", tagDistance);
+                // SmartDashboard.putNumber("VISION dist to apriltag [1]", llresults.targets_Fiducials[1].getTargetPose_RobotSpace2D().getTranslation().getNorm());
+                // SmartDashboard.putNumber("VISION dist to apriltag [2]", llresults.targets_Fiducials[2].getTargetPose_RobotSpace2D().getTranslation().getNorm());
             }
         }
         public double gettx(int index){return tx[index];}
@@ -55,12 +63,18 @@ public Pose2d getbotPose(){
         public Pose2d getBotPoseBlue() {
             return botPoseBlue;
         }
+        public double getTagDistance() {
+            return tagDistance;
+        }
         public boolean isPoseTrustworthy(Pose2d robotPose){
-            Pose2d poseEstimate = this.getbotPose();
+            Pose2d poseEstimate = this.botPoseBlue;
             if ((poseEstimate.getX()<fieldCorner.getX() && poseEstimate.getY()<fieldCorner.getY()) //Don't trust estimations that are outside the field perimeter.
-                && robotPose.getTranslation().getDistance(poseEstimate.getTranslation()) < 0.5) //Dont trust pose estimations that are more than half a meter from current pose.
+                && robotPose.getTranslation().getDistance(poseEstimate.getTranslation()) < APRILTAG_CLOSENESS//Dont trust pose estimations that are more than half a meter from current pose.
+                && tagDistance<=MAX_TAG_DISTANCE) { //Dont trust pose estimations if the april tag is more than X meters away
                 return true;
-            else return false;
+            } else {
+                return false;
+            }
         }
         public double gettimestamp(){
             return (Timer.getFPGATimestamp()

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

@@ -431,6 +431,12 @@ public void periodic() {
 					LimelightValues ll3 = limelight.getLimelightValues(Vision.APRILTAG_LIMELIGHT3_NAME);
 					Boolean isLL2VisionValid = ll2.isResultValid;
 					Boolean isLL3VisionValid = ll3.isResultValid;
+					if(isLL2VisionValid) {
+						SmartDashboard.putNumber("VISION left LL tag [0] distance", ll2.getTagDistance());
+					}
+					if(isLL3VisionValid) {
+						SmartDashboard.putNumber("VISION right LL tag [0] distance", ll3.getTagDistance());
+					}
 					Boolean isLL2VisionTrustworthy = isLL2VisionValid && ll2.isPoseTrustworthy(odometer.getEstimatedPosition());
 					Boolean isLL3VisionTrustworthy = isLL3VisionValid && ll3.isPoseTrustworthy(odometer.getEstimatedPosition());
 					SmartDashboard.putBoolean("LL2visionValid", isLL2VisionTrustworthy);

vendordeps/URCL.json

@@ -0,0 +1,65 @@
+{
+    "fileName": "URCL.json",
+    "name": "URCL",
+    "version": "2024.1.0",
+    "frcYear": "2024",
+    "uuid": "84246d17-a797-4d1e-bd9f-c59cd8d2477c",
+    "mavenUrls": [
+        "https://raw.githubusercontent.com/Mechanical-Advantage/URCL/2024.1.0"
+    ],
+    "jsonUrl": "https://raw.githubusercontent.com/Mechanical-Advantage/URCL/maven/URCL.json",
+    "javaDependencies": [
+        {
+            "groupId": "org.littletonrobotics.urcl",
+            "artifactId": "URCL-java",
+            "version": "2024.1.0"
+        }
+    ],
+    "jniDependencies": [
+        {
+            "groupId": "org.littletonrobotics.urcl",
+            "artifactId": "URCL-driver",
+            "version": "2024.1.0",
+            "skipInvalidPlatforms": true,
+            "isJar": false,
+            "validPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxathena",
+                "osxuniversal"
+            ]
+        }
+    ],
+    "cppDependencies": [
+        {
+            "groupId": "org.littletonrobotics.urcl",
+            "artifactId": "URCL-cpp",
+            "version": "2024.1.0",
+            "libName": "URCL",
+            "headerClassifier": "headers",
+            "sharedLibrary": false,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxathena",
+                "osxuniversal"
+            ]
+        },
+        {
+            "groupId": "org.littletonrobotics.urcl",
+            "artifactId": "URCL-driver",
+            "version": "2024.1.0",
+            "libName": "URCLDriver",
+            "headerClassifier": "headers",
+            "sharedLibrary": false,
+            "skipInvalidPlatforms": true,
+            "binaryPlatforms": [
+                "windowsx86-64",
+                "linuxx86-64",
+                "linuxathena",
+                "osxuniversal"
+            ]
+        }
+    ]
+}