Add Targeting support when target is not in camera

FRC-Team-620 · Feb 29, 2024 · 0371b00 · 0371b00
1 parent 7bbd73c
commit 0371b00
Show file tree

Hide file tree

Showing 2 changed files with 11 additions and 7 deletions.
diff --git a/src/main/java/org/jmhsrobotics/frc2024/RobotContainer.java b/src/main/java/org/jmhsrobotics/frc2024/RobotContainer.java
@@ -83,7 +83,7 @@ public RobotContainer() {
 		autoChooser.setDefaultOption("BaseLineAuto", new DriveTimeCommand(1.535, 0.3, driveSubsystem));
 		SmartDashboard.putData("Auto Chooser", autoChooser);
 		SmartDashboard.putData("Scheduler", CommandScheduler.getInstance());
-		SmartDashboard.putData(new ArmVision(armSubsystem, visionSubsystem));
+		SmartDashboard.putData(new ArmVision(armSubsystem, visionSubsystem, driveSubsystem));
 		// ShooterCommand shooterCommand = new ShooterCommand(2000, shooterSubsystem);
 		// SmartDashboard.putData("Shooter Command", shooterCommand);
 	}

diff --git a/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/commands/ArmVision.java b/src/main/java/org/jmhsrobotics/frc2024/subsystems/arm/commands/ArmVision.java
@@ -1,12 +1,11 @@
 package org.jmhsrobotics.frc2024.subsystems.arm.commands;
 
 import org.jmhsrobotics.frc2024.subsystems.arm.ArmPIDSubsystem;
+import org.jmhsrobotics.frc2024.subsystems.drive.DriveSubsystem;
 import org.jmhsrobotics.frc2024.subsystems.vision.VisionSubsystem;
 import org.photonvision.targeting.PhotonTrackedTarget;
 
 import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Pose3d;
-import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.interpolation.InterpolatingDoubleTreeMap;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
@@ -15,11 +14,12 @@
 public class ArmVision extends Command {
 	private ArmPIDSubsystem arm;
 	private VisionSubsystem vision;
+	private DriveSubsystem drive;
 
 	private Pose2d lastAprilTag;
 	private InterpolatingDoubleTreeMap armAngles = new InterpolatingDoubleTreeMap();
 
-	public ArmVision(ArmPIDSubsystem arm, VisionSubsystem vision) {
+	public ArmVision(ArmPIDSubsystem arm, VisionSubsystem vision, DriveSubsystem drive) {
 		this.arm = arm;
 		this.vision = vision;
 		armAngles.put(0d, 0d);
@@ -29,6 +29,8 @@ public ArmVision(ArmPIDSubsystem arm, VisionSubsystem vision) {
 		armAngles.put(5d, 90d);
 		// SmartDashboard.putNumber("Armangle", 0);
 
+		this.drive = drive;
+
 		addRequirements(this.arm, this.vision);
 	}
 
@@ -42,12 +44,14 @@ public void execute() {
 		// arm.setGoal(SmartDashboard.getNumber("Armangle", 0));
 		PhotonTrackedTarget aprilTag = this.vision.getTarget(7);
 		if (aprilTag != null) {
-			this.lastAprilTag = new Pose3d().transformBy(aprilTag.getBestCameraToTarget()).toPose2d(); // TODO: Clean up
-																										// hack
+			this.lastAprilTag = vision.targetToField(aprilTag.getBestCameraToTarget(), drive.getPose()).toPose2d(); // TODO:
+																													// Clean
+																													// up
+			// hack
 
 		}
 		if (this.lastAprilTag != null) {
-			double dist = lastAprilTag.getTranslation().getDistance(new Translation2d());
+			double dist = lastAprilTag.getTranslation().getDistance(drive.getPose().getTranslation());
 			SmartDashboard.putNumber("Distance", dist);
 			double angle = armAngles.get(dist);
 			arm.setGoal(angle);