Try to fix the camera
This commit is contained in:
parent
87399f0670
commit
d01f3b1d7e
|
@ -246,6 +246,9 @@ public class AutoBlueTwoPlusTwo extends LinearOpMode {
|
|||
|
||||
this.telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
|
||||
this.robot = new Robot().init(hardwareMap);
|
||||
while(!this.robot.getCamera().isReady()) {
|
||||
sleep(20);
|
||||
}
|
||||
// this.robot.getCamera().setAlliance(Alliance.Blue);
|
||||
// this.robot.getCamera().initTargetingCamera();
|
||||
this.initialPosition = new Pose2d(-34, -59.5, Math.toRadians(270));
|
||||
|
|
|
@ -117,4 +117,8 @@ public class Camera {
|
|||
}
|
||||
return this.aprilTagPoseEstimator.estimatePose(detection);
|
||||
}
|
||||
|
||||
public boolean isReady() {
|
||||
return this.propPortal.getCameraState() == VisionPortal.CameraState.CAMERA_DEVICE_READY;
|
||||
}
|
||||
}
|
Loading…
Reference in New Issue