44
55import com .ctre .phoenix6 .swerve .SwerveDrivetrain ;
66import com .ctre .phoenix6 .swerve .SwerveRequest ;
7- import edu .wpi .first .apriltag .AprilTag ;
8- import edu .wpi .first .apriltag .AprilTagFieldLayout ;
9- import edu .wpi .first .apriltag .AprilTagFields ;
107import edu .wpi .first .math .geometry .Pose2d ;
118import edu .wpi .first .math .geometry .Rotation2d ;
129import edu .wpi .first .math .kinematics .ChassisSpeeds ;
2219import frc .lib .NTDouble ;
2320import frc .lib .Subsystem ;
2421import frc .lib .swerves .SwerveOutput ;
25- import java .util .List ;
26- import java .util .function .Predicate ;
2722import java .util .function .Supplier ;
2823
2924public class Drive extends Subsystem {
@@ -34,10 +29,6 @@ public class Drive extends Subsystem {
3429
3530 private final Field2d field ;
3631
37- private final AprilTagFieldLayout tagLayout ;
38-
39- private final List <Pose2d > scoringPoses ;
40-
4132 private Supplier <Pose2d > targetSupplier ;
4233
4334 private boolean hasSetPerspective = false ;
@@ -48,19 +39,9 @@ public Drive(SwerveOutput swerve) {
4839 this .swerve = swerve ;
4940 this .state = new SwerveDrivetrain .SwerveDriveState ();
5041 this .field = new Field2d ();
51- this .tagLayout = AprilTagFieldLayout .loadField (AprilTagFields .kDefaultField );
5242 this .driverAssistance =
5343 new DriverAssistance (MetersPerSecond .per (Meter ).ofNative (8 ), Meters .of (1 ), Meters .of (2 ));
5444 this .targetSupplier = Pose2d ::new ;
55-
56- Predicate <AprilTag > isBlueScoringTag = tag -> 17 <= tag .ID && tag .ID <= 22 ;
57- Predicate <AprilTag > isRedScoringTag = tag -> 6 <= tag .ID && tag .ID <= 11 ;
58- this .scoringPoses =
59- tagLayout .getTags ().stream ()
60- .filter (isBlueScoringTag .or (isRedScoringTag ))
61- .map (tag -> tag .pose .toPose2d ())
62- .map (pose -> pose .rotateAround (pose .getTranslation (), Rotation2d .k180deg ))
63- .toList ();
6445 }
6546
6647 @ Override
0 commit comments