Added note vision to only return angle and modified autoalign to just…

… move straight and rotate.

src/main/java/frc/robot/subsystems/AutoAlign/AutoAlignIO.java

@@ -68,23 +68,27 @@ public void driveToTargetPose() {
 	}
 	public void driveToNotePose() {
 		Pose2d currentPose2d = driveSubsystem.getPose();
-		Pose2d visionPose = noteVision.getNotePose(currentPose2d); 
-
-		if (visionPose != null) {
-			targetPose2d = visionPose;  // Sets target pose to (best) note seen rather than whatever it is set to
+		double targetAngle;
+		targetAngle = 2; // need to tune for optimal speed
+
+		if (noteVision.noteAngle() != 180) {
+			targetAngle = noteVision.noteAngle(); // Sets target pose to (best) note seen rather than whatever it is set to
+		} else {
+			targetAngle = 0;
+			appliedX = 0;
 		}
 
 		/*uses run velocity from drive and PID controllers to go to target pose */
-		appliedX = translationalPIDController.calculate(currentPose2d.getX(), targetPose2d.getX());
-		appliedY = translationalPIDController.calculate(currentPose2d.getY(), targetPose2d.getY());
+		//appliedX = translationalPIDController.calculate(currentPose2d.getX(), targetPose2d.getX());
+		//appliedY = translationalPIDController.calculate(currentPose2d.getY(), targetPose2d.getY());
 		appliedRotational = rotationalPIDController.calculate(
 			currentPose2d.getRotation().getRadians(),
-			targetPose2d.getRotation().getRadians()
+			targetAngle*(Math.PI/180)
 		);
 		driveSubsystem.runVelocity(
 			ChassisSpeeds.fromFieldRelativeSpeeds(
-				appliedX,
-				appliedY,
+				2,
+				0,
 				appliedRotational,
 				driveSubsystem.getRotation()
 			)

src/main/java/frc/robot/subsystems/noteVision/NoteVision.java

@@ -1,9 +1,12 @@
 package frc.robot.subsystems.noteVision;
 
 import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Rotation3d;
 import edu.wpi.first.math.geometry.Transform2d;
 import edu.wpi.first.math.geometry.Transform3d;
 import edu.wpi.first.math.util.Units;
+import edu.wpi.first.units.Angle;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
 import java.util.List;
@@ -24,19 +27,14 @@ public NoteVision() {
 		hasTarget = false;
 	}
 
-	public Pose2d getNotePose(Pose2d botPose2d) {
+	public double noteAngle() {
 		PhotonPipelineResult lastResult = noteCam.getLatestResult();
 		List<PhotonTrackedTarget> noteData = lastResult.targets;
+		Rotation3d rotation3d = new Rotation3d(0, 0, 180);
 		for (PhotonTrackedTarget t : noteData) {
-			Transform3d noteTransform = t.getBestCameraToTarget();
-			Pose2d notePose2d = new Pose2d(
-				botPose2d.getX() + noteTransform.getX(),
-				botPose2d.getY() + noteTransform.getY(),
-				botPose2d.getRotation()
-			);
-			return notePose2d;
+			rotation3d = t.getBestCameraToTarget().getRotation();
 		}
-		return null;
+		return rotation3d.getAngle();
 	}
 
 	public void periodic() {
@@ -46,9 +44,7 @@ public void periodic() {
 		} else {
 			hasTarget = false;
 		}
-		SmartDashboard.putNumber("noteX", getNotePose(pose).getX()); // for note vision testing
-		SmartDashboard.putNumber("noteY", getNotePose(pose).getY()); // for note vision testing
-
+		SmartDashboard.putNumber("Angle", noteAngle());
 		Logger.recordOutput("NoteVision/hasTarget", hasTarget);
 	}
 }