Tuning Auto Aim

FRC-Team-620 · Apr 5, 2024 · cefedc3 · cefedc3
1 parent fdb949e
commit cefedc3
Showing 1 changed file with 4 additions and 0 deletions.
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
@@ -49,6 +49,9 @@ public ArmVision(ArmPIDSubsystem arm, VisionSubsystem vision, DriveSubsystem dri
 		armAngles.put(2.42d, 23.5d);
 		armAngles.put(2.54d, 24d);
 		armAngles.put(3.14d, 27.5d);
+		armAngles.put(3.32d, 28d);
+		armAngles.put(3.69d, 29d);
+		armAngles.put(4d, 30.5d);
 
 		SmartDashboard.putNumber("Armangle", 0);
 
@@ -81,6 +84,7 @@ public void execute() {
 			double dist = lastAprilTag.getTranslation().getDistance(drive.getPose().getTranslation());
 			SmartDashboard.putNumber("Distance", dist);
 
+			// arm.setGoal(SmartDashboard.getNumber("ArmAngle", 0));
 			double angle = armAngles.get(dist);
 			arm.setGoal(angle);