Merge pull request #73 from DeepBlueRobotics/limelight-megatag2

Limelight megatag2

src/main/java/org/carlmontrobotics/Constants.java

@@ -377,6 +377,14 @@ public static final class Limelightc {
 		public static final String INTAKE_LL_NAME = "limelight-intake";
 		public static final String SHOOTER_LL_NAME = "limelight-shooter";
 
+		public static final int[] VALID_IDS = { 4, 7 };
+
+		public static final double STD_DEV_X_METERS = 0.7; // uncertainty of 0.7 meters on the field
+		public static final double STD_DEV_Y_METERS = 0.7; // uncertainty of 0.7 meters on the field
+		public static final int STD_DEV_HEADING_RADS = 9999999; // (gyro) heading standard deviation, set extremely high
+																// to represent unreliable heading
+		public static final int MAX_TRUSTED_ANG_VEL_DEG_PER_SEC = 720; // maximum trusted angular velocity
+
 		public static final double ERROR_TOLERANCE_RAD = 0.1; // unused
 		public static final double HORIZONTAL_FOV_DEG = 0; // unused
 		public static final double RESOLUTION_WIDTH_PIX = 640; // unused

src/main/java/org/carlmontrobotics/commands/AimArmSpeaker.java

@@ -6,9 +6,6 @@
 
 import org.carlmontrobotics.subsystems.Arm;
 import org.carlmontrobotics.subsystems.Limelight;
-import edu.wpi.first.math.MathUtil;
-import edu.wpi.first.math.trajectory.TrapezoidProfile;
-import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.Command;
 
 public class AimArmSpeaker extends Command {
@@ -41,6 +38,6 @@ public void end(boolean interrupted) {}
   // Returns true when the command should end.
   @Override
   public boolean isFinished() {
-    return false;
+    return arm.armAtSetpoint();
   }
 }

src/main/java/org/carlmontrobotics/commands/AimArmSpeakerMT2.java

@@ -0,0 +1,44 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package org.carlmontrobotics.commands;
+
+import org.carlmontrobotics.subsystems.Arm;
+import org.carlmontrobotics.subsystems.Limelight;
+import edu.wpi.first.wpilibj2.command.Command;
+
+public class AimArmSpeakerMT2 extends Command {
+    private final Arm arm;
+    private final Limelight ll;
+
+    /** Creates a new AimOuttakeSpeaker. */
+    public AimArmSpeakerMT2(Arm arm, Limelight ll) {
+        // Use addRequirements() here to declare subsystem dependencies.
+        addRequirements(this.arm = arm);
+        this.ll = ll;
+    }
+
+    // Called when the command is initially scheduled.
+    @Override
+    public void initialize() {
+        double goal = ll.getOptimizedArmAngleRadsMT2();
+        arm.setArmTarget(goal);
+    }
+
+    // Called every time the scheduler runs while the command is scheduled.
+    @Override
+    public void execute() {
+    }
+
+    // Called once the command ends or is interrupted.
+    @Override
+    public void end(boolean interrupted) {
+    }
+
+    // Returns true when the command should end.
+    @Override
+    public boolean isFinished() {
+        return arm.armAtSetpoint();
+    }
+}

src/main/java/org/carlmontrobotics/commands/AlignToApriltag.java

@@ -5,11 +5,9 @@
 package org.carlmontrobotics.commands;
 
 import static org.carlmontrobotics.Constants.Drivetrainc.*;
-import static org.carlmontrobotics.Constants.Limelightc.*;
 
 import org.carlmontrobotics.subsystems.Drivetrain;
 import org.carlmontrobotics.subsystems.Limelight;
-import org.carlmontrobotics.subsystems.LimelightHelpers;
 
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.controller.PIDController;

src/main/java/org/carlmontrobotics/commands/AlignToApriltagMegatag2.java

@@ -0,0 +1,62 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package org.carlmontrobotics.commands;
+
+import static org.carlmontrobotics.Constants.Drivetrainc.*;
+
+import org.carlmontrobotics.subsystems.Drivetrain;
+import org.carlmontrobotics.subsystems.Limelight;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.util.sendable.SendableRegistry;
+import edu.wpi.first.wpilibj2.command.Command;
+
+public class AlignToApriltagMegatag2 extends Command {
+
+    public final TeleopDrive teleopDrive;
+    public final Drivetrain drivetrain;
+    private Limelight limelight;
+
+    public final PIDController rotationPID = new PIDController(thetaPIDController[0], thetaPIDController[1],
+            thetaPIDController[2]);
+
+    public AlignToApriltagMegatag2(Drivetrain drivetrain, Limelight limelight) {
+        this.limelight = limelight;
+        this.drivetrain = drivetrain;
+        this.teleopDrive = (TeleopDrive) drivetrain.getDefaultCommand();
+
+        rotationPID.enableContinuousInput(-180, 180);
+        Rotation2d targetAngle = Rotation2d.fromDegrees(drivetrain.getHeading())
+                .plus(Rotation2d.fromRadians(limelight.getRotateAngleRadMT2()));
+        rotationPID.setSetpoint(MathUtil.inputModulus(targetAngle.getDegrees(), -180, 180));
+        rotationPID.setTolerance(positionTolerance[2], velocityTolerance[2]);
+        SendableRegistry.addChild(this, rotationPID);
+        addRequirements(drivetrain);
+    }
+
+    @Override
+    public void execute() {
+        Rotation2d targetAngle = Rotation2d.fromDegrees(drivetrain.getHeading())
+                .plus(Rotation2d.fromRadians(limelight.getRotateAngleRadMT2()));
+        rotationPID.setSetpoint(MathUtil.inputModulus(targetAngle.getDegrees(), -180, 180));
+        if (teleopDrive == null)
+            drivetrain.drive(0, 0, rotationPID.calculate(drivetrain.getHeading()));
+        else {
+            double[] driverRequestedSpeeds = teleopDrive.getRequestedSpeeds();
+            drivetrain.drive(driverRequestedSpeeds[0], driverRequestedSpeeds[1],
+                    rotationPID.calculate(drivetrain.getHeading()));
+        }
+    }
+
+    @Override
+    public boolean isFinished() {
+        return false;
+        // SmartDashboard.putBoolean("At Setpoint", rotationPID.atSetpoint());
+        // SmartDashboard.putNumber("Error", rotationPID.getPositionError());
+        // return rotationPID.atSetpoint();
+    }
+}

src/main/java/org/carlmontrobotics/subsystems/Arm.java

@@ -175,6 +175,9 @@ public void setBooleanDrive(boolean climb) {
 
     @Override
     public void periodic() {
+
+        SmartDashboard.putNumber("arm angle", getArmPos()); // for limelight testing
+
         babyMode = SmartDashboard.getBoolean("babymode", false);
 
 

src/main/java/org/carlmontrobotics/subsystems/Drivetrain.java

@@ -1004,5 +1004,8 @@ public void keepRotateMotorsAtDegrees(int angle) {
         }
     }
 
+    public double getGyroRate() {
+        return gyro.getRate();
+    }
     // #endregion
 }

src/main/java/org/carlmontrobotics/subsystems/Limelight.java

@@ -3,10 +3,12 @@
 import static org.carlmontrobotics.Constants.Limelightc.*;
 import static org.carlmontrobotics.Constants.Limelightc.Apriltag.*;
 
+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.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
@@ -18,28 +20,39 @@ public class Limelight extends SubsystemBase {
   // private double[] targetPose = null;
   private Pose3d botPose;
 
+  private final InterpolatingDoubleTreeMap shooterMap;
+
   public Limelight(Drivetrain drivetrain) {
     this.drivetrain = drivetrain;
     poseEstimator = new SwerveDrivePoseEstimator(
         drivetrain.getKinematics(),
         Rotation2d.fromDegrees(drivetrain.getHeading()),
         drivetrain.getModulePositions(),
         new Pose2d());
+
+    LimelightHelpers.SetFiducialIDFiltersOverride(SHOOTER_LL_NAME, VALID_IDS);
+
+    shooterMap = new InterpolatingDoubleTreeMap(); // add values after testing
+    // key is distance (meters), value is angle (rads)
+    shooterMap.put(0.0, 0.0);
   }
 
   @Override
   public void periodic() {
     poseEstimator.update(Rotation2d.fromDegrees(drivetrain.getHeading()), drivetrain.getModulePositions());
     updateBotPose3d();
-    getDistanceToSpeakerMeters();
-    getCurrentPose();
-    getDistanceToNoteMeters();
+
+    updateMT2Odometry();
 
     // intake limelight testing
     SmartDashboard.putBoolean("see note", LimelightHelpers.getTV(INTAKE_LL_NAME));
     SmartDashboard.putNumber("distance to note", getDistanceToNoteMeters());
     SmartDashboard.putNumber("intake tx", LimelightHelpers.getTX(INTAKE_LL_NAME));
     SmartDashboard.putNumber("rotation to align", getRotateAngleRad());
+
+    // shooter limelight testing
+    SmartDashboard.putNumber("distance to speaker (meters)", getDistanceToSpeakerMetersMT2());
+    SmartDashboard.putNumber("optimized arm angle", getArmAngleToShootSpeakerRad());
   }
 
   public void updateBotPose3d() {
@@ -101,4 +114,53 @@ public double getRotateAngleRad() {
     double realHorizontalOffset = Math.atan(cameraLensHorizontalOffset / getDistanceToSpeakerMeters());
     return Math.atan(realHorizontalOffset / getDistanceToSpeakerMeters());
   }
+
+  // megatag2
+
+  public void updateMT2Odometry() {
+    boolean rejectVisionUpdate = false;
+
+    LimelightHelpers.SetRobotOrientation(SHOOTER_LL_NAME,
+        poseEstimator.getEstimatedPosition().getRotation().getDegrees(), 0, 0, 0, 0, 0);
+    LimelightHelpers.PoseEstimate visionPoseEstimate = LimelightHelpers
+        .getBotPoseEstimate_wpiBlue_MegaTag2(SHOOTER_LL_NAME);
+
+    if (Math.abs(drivetrain.getGyroRate()) > MAX_TRUSTED_ANG_VEL_DEG_PER_SEC) { // degrees per second
+      rejectVisionUpdate = true;
+    }
+
+    if (visionPoseEstimate.tagCount == 0) {
+      rejectVisionUpdate = true;
+    }
+
+    if (!rejectVisionUpdate) {
+      poseEstimator
+          .setVisionMeasurementStdDevs(VecBuilder.fill(STD_DEV_X_METERS, STD_DEV_Y_METERS, STD_DEV_HEADING_RADS));
+      poseEstimator.addVisionMeasurement(visionPoseEstimate.pose, visionPoseEstimate.timestampSeconds);
+    }
+  }
+
+  public double getRotateAngleRadMT2() {
+    Pose3d targetPoseRobotSpace = LimelightHelpers.getTargetPose3d_RobotSpace(SHOOTER_LL_NAME); // pose of the target
+
+    double targetX = targetPoseRobotSpace.getX(); // the forward offset between the center of the robot and target
+    double targetY = targetPoseRobotSpace.getY(); // the sideways offset
+
+    double targetOffsetRads = Math.atan2(targetY, targetX);
+
+    return targetOffsetRads;
+  }
+
+  public double getDistanceToSpeakerMetersMT2() {
+    Pose3d targetPoseRobotSpace = LimelightHelpers.getTargetPose3d_RobotSpace(SHOOTER_LL_NAME);
+
+    double x = targetPoseRobotSpace.getX();
+    double y = targetPoseRobotSpace.getY();
+
+    return Math.sqrt(x * x + y * y);
+  }
+
+  public double getOptimizedArmAngleRadsMT2() {
+    return shooterMap.get(getDistanceToSpeakerMetersMT2());
+  }
 }