diff --git a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java index 8ba2b72..4605f6a 100644 --- a/src/main/java/frc4388/robot/subsystems/SwerveDrive.java +++ b/src/main/java/frc4388/robot/subsystems/SwerveDrive.java @@ -341,7 +341,7 @@ public class SwerveDrive extends Subsystem { vision.setLastOdomPose(curpose); setLastOdomSpeed(curpose, lastPose, freq); - if (vision.isTag()) { + if (vision.isTag() || true) { Pose2d pose = vision.getPose2d(); if (!robotKnowsWhereItIs) { robotKnowsWhereItIs = true; diff --git a/src/main/java/frc4388/robot/subsystems/Vision.java b/src/main/java/frc4388/robot/subsystems/Vision.java index 1b9ac39..d53bcad 100644 --- a/src/main/java/frc4388/robot/subsystems/Vision.java +++ b/src/main/java/frc4388/robot/subsystems/Vision.java @@ -191,7 +191,7 @@ public class Vision extends Subsystem { // lastVisionPose = new Pose2d(X/cams, Y/cams, Rotation2d.fromDegrees(curAngle + rotations*360)); - lastVisionPose = new Pose2d(0, 0, Rotation2d.fromDegrees(curAngle + rotations*360)); + lastVisionPose = new Pose2d(10, 5, Rotation2d.fromDegrees(curAngle + rotations*360)); SmartDashboard.putNumber("curAngle", lastVisionPose.getRotation().getRadians()); SmartDashboard.putNumber("Rotations", rotations);