|
59 | 59 | import frc.robot.util.LimelightHelpers.PoseEstimate; |
60 | 60 | import frc.robot.util.LimelightHelpers.RawFiducial; |
61 | 61 | import frc.robot.util.LocalADStarAK; |
62 | | - |
63 | 62 | import java.util.HashMap; |
64 | 63 | import java.util.List; |
65 | 64 | import java.util.Optional; |
@@ -735,25 +734,28 @@ public Translation2d getTargetNoteLocation() { |
735 | 734 | Translation2d fieldCoords = noteLocations.get(getNote()); |
736 | 735 | boolean useVisionNoteCoords = |
737 | 736 | getCachedNoteLocation().getDistance(fieldCoords) < 1.25 |
738 | | - && getCachedNoteLocation() != null && getCachedNoteTime() != -1 && noteImageIsNew(); |
| 737 | + && getCachedNoteLocation() != null |
| 738 | + && getCachedNoteTime() != -1 |
| 739 | + && noteImageIsNew(); |
739 | 740 |
|
740 | 741 | Logger.recordOutput( |
741 | | - "cached note distance to field ", getCachedNoteLocation().getDistance(fieldCoords)); |
| 742 | + "cached note distance to field ", getCachedNoteLocation().getDistance(fieldCoords)); |
742 | 743 | Logger.recordOutput("use vision note coords", useVisionNoteCoords); |
743 | 744 |
|
744 | 745 | if (useVisionNoteCoords) return visionCoords; |
745 | 746 | return fieldCoords; |
746 | 747 | } |
747 | 748 |
|
748 | | - public PathPlannerPath generateTrajectoryToNote( |
| 749 | + public PathPlannerPath generateTrajectoryToNote( |
749 | 750 | Translation2d target, |
750 | 751 | double maxVelMetersPerSec, |
751 | 752 | double maxAccelMetersPerSecSquared, |
752 | 753 | double maxAngVelDegPerSec, |
753 | 754 | double maxAngAccelDegPerSecSquared, |
754 | 755 | double endVelMetersPerSec) { |
755 | 756 | Rotation2d targetRotation = |
756 | | - AllianceFlipUtil.apply(new Rotation2d(target.getX() - getPose().getX(), target.getY() - getPose().getY())); |
| 757 | + AllianceFlipUtil.apply( |
| 758 | + new Rotation2d(target.getX() - getPose().getX(), target.getY() - getPose().getY())); |
757 | 759 |
|
758 | 760 | Logger.recordOutput("Target Note Pose3d", new Pose3d(new Pose2d(target, new Rotation2d()))); |
759 | 761 | List<Translation2d> points = |
|
0 commit comments