diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/Camera.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/Camera.java index f48d616..cea9751 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/Camera.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Mechanisms/Camera.java @@ -108,7 +108,7 @@ public void init(HardwareMap hwMap) { visionPortal = new VisionPortal.Builder().setCamera(hwMap.get(WebcamName.class, cameraName)) .setCameraResolution(new Size(640, 480)) .setLiveViewContainerId(viewPortID) - .addProcessor(aprilTag) + //.addProcessor(aprilTag) .addProcessor(teamPropDetector) .build(); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/RRTests/PlacingOnTapeAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/RRTests/PlacingOnTapeAuto.java index 62581d1..2747ea3 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/RRTests/PlacingOnTapeAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/OpModes/RRTests/PlacingOnTapeAuto.java @@ -5,19 +5,24 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import org.firstinspires.ftc.teamcode.ftc16072.OpModes.QQOpMode; +import org.firstinspires.ftc.teamcode.ftc16072.OpModes.VisionBase; import org.firstinspires.ftc.teamcode.ftc16072.Robot; import org.firstinspires.ftc.teamcode.ftc16072.Util.TeamPropLocation; @Autonomous -public class PlacingOnTapeAuto extends OpMode { +public class PlacingOnTapeAuto extends VisionBase { Robot robot = new Robot(); Trajectory trajectory; private enum State {BEGIN, FORWARD,TURN,FORWARD_TO_TAPE, DONE} State state = State.BEGIN; @Override public void init() { - robot.makeDriveOnly(); robot.init(hardwareMap); + super.init(); // for vision + } + public void init_loop(){ + } @Override @@ -26,7 +31,7 @@ public void loop() { Pose2d currentPose = robot.nav.getPoseEstimate(); TeamPropLocation teamPropLocation = robot.cameraBack.getTeamPropPosition(); - telemetry.addData("STATE", state); + telemetry.addData("team prop", teamPropLocation); telemetry.addData("POSE", "x = %.2f y = %.2f h = %.1f", currentPose.getX(), currentPose.getY(), Math.toDegrees(currentPose.getHeading())); switch (state) { case BEGIN: @@ -51,38 +56,56 @@ public void loop() { robot.nav.follower.followTrajectory(trajectory); break; case FORWARD: - state = State.TURN; - if (teamPropLocation == TeamPropLocation.LEFT_SPIKE){ - if (robot.nav.isDoneFollowing(currentPose)) { - robot.nav.turnAsync(-0.25*Math.PI); - } - } else if (teamPropLocation == TeamPropLocation.RIGHT_SPIKE) { - if (robot.nav.isDoneFollowing(currentPose)) { - robot.nav.turnAsync(0.25*Math.PI); + + if (robot.nav.isDoneFollowing(currentPose)) { + if (teamPropLocation == TeamPropLocation.LEFT_SPIKE){ + if (robot.nav.isDoneFollowing(currentPose)) { + robot.nav.turnAsync(-0.25*Math.PI); + } + } else if (teamPropLocation == TeamPropLocation.RIGHT_SPIKE) { + if (robot.nav.isDoneFollowing(currentPose)) { + robot.nav.turnAsync(0.25*Math.PI); + } + } + state = State.TURN; } + + break; case TURN: state = State.FORWARD_TO_TAPE; - if (teamPropLocation == TeamPropLocation.LEFT_SPIKE){ - trajectory = robot.nav.trajectoryBuilder(currentPose, false) - .back(12) - .build(); + if (robot.nav.isDoneFollowing(currentPose)) { - } else if (teamPropLocation == TeamPropLocation.RIGHT_SPIKE){ - trajectory = robot.nav.trajectoryBuilder(currentPose, false) - .back(12) - .build(); + if (teamPropLocation == TeamPropLocation.LEFT_SPIKE) { + trajectory = robot.nav.trajectoryBuilder(currentPose, false) + .back(7) + .build(); + + + } else if (teamPropLocation == TeamPropLocation.RIGHT_SPIKE) { + trajectory = robot.nav.trajectoryBuilder(currentPose, false) + .back(7) + .build(); + } } break; case FORWARD_TO_TAPE: - state = State.DONE; + if (robot.nav.isDoneFollowing(currentPose)) { + state = State.DONE; + } + + + + break; + case DONE: break; + } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Util/TeamPropDetector.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Util/TeamPropDetector.java index 6560487..1b92738 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Util/TeamPropDetector.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ftc16072/Util/TeamPropDetector.java @@ -7,6 +7,7 @@ import com.acmerobotics.dashboard.config.Config; import org.firstinspires.ftc.robotcore.internal.camera.calibration.CameraCalibration; +import org.firstinspires.ftc.teamcode.ftc16072.Mechanisms.Camera; import org.firstinspires.ftc.vision.VisionProcessor; import org.opencv.core.Core; import org.opencv.core.Mat; @@ -16,10 +17,12 @@ @Config public class TeamPropDetector implements VisionProcessor { //makes "detection zones" for each tape zone - TeamPropLocation location = TeamPropLocation.NOT_DETECTED; - public Rect leftTapeDetectionZone = new Rect(100,100,75,75); - public Rect middleTapeDetectionZone = new Rect(400,130,75,75); - public Rect rightTapeDetectionZone = new Rect(800,100,75,75); + TeamPropLocation location = TeamPropLocation.LEFT_SPIKE; + public Rect leftTapeDetectionZone = new Rect(0,155,120,75); + public Rect middleTapeDetectionZone = new Rect(250,165,120,75); + public Rect rightTapeDetectionZone = new Rect(500,155,120,75); + + //submats are smaller portions of the frame that you can get values from Mat submat = new Mat(); @@ -28,6 +31,7 @@ public class TeamPropDetector implements VisionProcessor { @Override public void init(int width, int height, CameraCalibration calibration) { + }