From 9059a46400e75baef41b4c262be2bb7504f1c774 Mon Sep 17 00:00:00 2001 From: annasunnysun Date: Sun, 28 Dec 2025 17:19:57 -0500 Subject: [PATCH 1/4] test --- README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/README.md b/README.md index 522fb80..569cf73 100644 --- a/README.md +++ b/README.md @@ -1,5 +1,5 @@ ## NOTICE - +test This repository contains the public FTC SDK for the DECODE (2025-2026) competition season. ## Welcome! From 2a311b6ce9af3ce6b41ebc8e393abadc6a8336f1 Mon Sep 17 00:00:00 2001 From: annasunnysun Date: Sun, 4 Jan 2026 19:06:46 -0500 Subject: [PATCH 2/4] turretControls/auto --- .../teamcode/pedroPathing/BlueGoalAuto.java | 143 ++++++------------ .../ftc/teamcode/pedroPathing/TeleOp_1.java | 27 ++-- 2 files changed, 61 insertions(+), 109 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/BlueGoalAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/BlueGoalAuto.java index 4f466e8..e74b8bc 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/BlueGoalAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/BlueGoalAuto.java @@ -1,6 +1,5 @@ package org.firstinspires.ftc.teamcode.pedroPathing; -import static java.lang.Thread.sleep; import com.pedropathing.follower.Follower; import com.pedropathing.geometry.BezierLine; @@ -37,7 +36,7 @@ public class BlueGoalAuto extends OpMode { private double transferMotorReleaseValue = 1; private double flywheelMotorReleaseFar = -1; private double flywheelMotorReleaseClose = -0.2; - private double spindexerRelease = 0.25; + private double spindexerRelease = 0.4; private double arcServoFarRight = 0.405; //higher hood, higher arc, higher range private double arcServoFarLeft = 0.72; //higher hood, higher arc, higher range @@ -81,13 +80,13 @@ enum ArtifactColor { // Color thresholds private final double PURPLE_THRESHOLD = 0.001; //checking blue min (for purple) - private final double GREEN_THRESHOLD = 0.0015; //checking green min + private final double GREEN_THRESHOLD = 0.002; //checking green min // declaring poses - private final Pose startPose = new Pose(28.5, 128, Math.toRadians(145)); - private final Pose scorePreloadPose = new Pose(60, 90, Math.toRadians(145)); - private final Pose pickup1InitialPose = new Pose(45, 86, Math.toRadians(180)); + private final Pose startPose = new Pose(28.5, 128, Math.toRadians(135)); + private final Pose scorePreloadPose = new Pose(60, 85, Math.toRadians(125)); + private final Pose pickup1InitialPose = new Pose(55, 86, Math.toRadians(180)); private final Pose pickup1FinalPose = new Pose(17, 86, Math.toRadians(180)); private final Pose scoreFirstPickUpPose = new Pose(60, 75, Math.toRadians(180)); @@ -181,80 +180,47 @@ private void setDesiredOrderFromAprilTag(int tagID) { } } - private enum ShootState { - IDLE, - ALIGNING, - SHOOTING - } - private ShootState shootState = ShootState.IDLE; private void sortingSpindexer() { if (!isLaunching) return; + if(desiredOrder.isEmpty()){ + isLaunching = false; + return; + } - ArtifactColor wanted = desiredOrder.isEmpty() ? ArtifactColor.UNKNOWN : desiredOrder.get(0); + ArtifactColor wanted = desiredOrder.get(0); ArtifactColor transferSeen = detectColor(transferColorSensor); ArtifactColor intakeSeen = detectColor(spindexerColorSensor); - switch (shootState) { - - case IDLE: - shootState = ShootState.ALIGNING; - break; - - case ALIGNING: - if (transferSeen == wanted && transferSeen != ArtifactColor.UNKNOWN) { - // Correct color is ready → shoot - launchStartTicks = spindexerMotor.getCurrentPosition(); - transferMotor.setPower(transferMotorReleaseValue); - spindexerMotor.setPower(spindexerRelease); - shootState = ShootState.SHOOTING; - return; - } - - if (intakeSeen == wanted) { - // Desired ball is at intake → rotate clockwise until it reaches transfer - transferMotor.setPower(transferMotorRotatingValue); - spindexerMotor.setPower(-spindexerMotorValue); // rotate clockwise - } else { - // Desired ball not at intake → rotate counter-clockwise one pocket - launchStartTicks = spindexerMotor.getCurrentPosition(); - transferMotor.setPower(transferMotorRotatingValue); // reverse - spindexerMotor.setPower(spindexerMotorValue); //rotate counter clockwise - shootState = ShootState.SHOOTING; - return; - } - break; - - case SHOOTING: - int ticksMoved = Math.abs(spindexerMotor.getCurrentPosition() - launchStartTicks); - if (ticksMoved >= TICKS_PER_POCKET) { - stopSpindexer(); - - ArtifactColor seenAfter = detectColor(transferColorSensor); - - // Shot or ready - if (seenAfter == ArtifactColor.UNKNOWN || seenAfter == wanted) { - if (!desiredOrder.isEmpty()) desiredOrder.remove(0); - isLaunching = false; - shootState = ShootState.IDLE; - } else { - // Not the desired color yet → go back to aligning - shootState = ShootState.ALIGNING; - } - } - break; + //correct ball is at shooting position + if (transferSeen == wanted && transferSeen != ArtifactColor.UNKNOWN) { + // Correct color is ready → shoot + shootArtifact(transferSeen, wanted); } + else if (intakeSeen == wanted) { + // Desired ball is at intake → rotate clockwise until it reaches transfer + transferMotor.setPower(-transferMotorRotatingValue); + spindexerMotor.setPower(-spindexerMotorValue); // rotate clockwise + shootArtifact(transferSeen, wanted); + } + else { + // Desired ball not at intake → rotate counter-clockwise one pocket + transferMotor.setPower(transferMotorRotatingValue); // reverse + spindexerMotor.setPower(spindexerMotorValue); //rotate counter clockwise + shootArtifact(transferSeen, wanted); + } + sortingSpindexer(); } - private boolean shooterHasCorrectColor() { - ArtifactColor seen = detectColor(transferColorSensor); - return seen != ArtifactColor.UNKNOWN && seen == desiredOrder.get(0); - } - private void spinUntilCorrectColor() { - transferMotor.setPower(transferMotorRotatingValue); - spindexerMotor.setPower(spindexerMotorValue); + private void shootArtifact(ArtifactColor transferSeen, ArtifactColor wanted){ + // Correct color is ready → shoot + launchStartTicks = spindexerMotor.getCurrentPosition(); + transferMotor.setPower(transferMotorReleaseValue); + spindexerMotor.setPower(spindexerRelease); + desiredOrder.remove(0); } + private void stopSpindexer() { transferMotor.setPower(0); spindexerMotor.setPower(0); @@ -282,14 +248,14 @@ public void buildPaths() { .build(); } - public void autonomousPathUpdate() throws InterruptedException { + public void autonomousPathUpdate() { switch (pathState) { case 0: // start flywheel and move along path arcLeftServo.setPosition(arcServoCloseLeft); arcRightServo.setPosition(arcServoCloseRight); flywheelMotor.setPower(flywheelMotorReleaseClose); - turretServo.setPosition(0.5); // move turret for 0.5s + turretServo.setPosition(0.85); // move turret to 0.8 // start path following follower.followPath(scorePreload); pathTimer.resetTimer(); @@ -297,9 +263,8 @@ public void autonomousPathUpdate() throws InterruptedException { break; case 1: // shooting at scorePreload - double currentTime = pathTimer.getElapsedTimeSeconds(); // start shooting only after follower is done - if (!follower.isBusy() && !isLaunching) { + if (!follower.isBusy()) { // read AprilTag once if (limelight.getLatestResult() != null && limelight.getLatestResult().isValid() && @@ -312,30 +277,30 @@ public void autonomousPathUpdate() throws InterruptedException { } setDesiredOrderFromAprilTag(aprilTagIndex); - flywheelMotor.setPower(flywheelMotorReleaseClose); - pathTimer.resetTimer(); // optional, for next timed actions - isLaunching = true; // start shooting } // advance path when shooting is complete - if (!isLaunching && !follower.isBusy()) { - flywheelMotor.setPower(0); - isIntaking = true; - intakeMotor.setPower(intakeMotorIntakeValue); + if (pathTimer.getElapsedTimeSeconds()>2.5) { follower.followPath(pickupArtifact1Initial); + if(!follower.isBusy()){ + isLaunching = true; // start shooting + sortingSpindexer(); + sortingSpindexer(); + } setPathState(2); } break; case 2: - if(!follower.isBusy()){ - sleep(500); + if(pathTimer.getElapsedTimeSeconds()>10 && !isLaunching){ + flywheelMotor.setPower(0); + isIntaking=true; + intakeMotor.setPower(intakeMotorIntakeValue); follower.followPath(pickupArtifact1Final, driveMaxPowerIntaking, true); setPathState(3); } break; case 3: if(!follower.isBusy()){ - sleep(300); intakeMotor.setPower(0); isIntaking=false; follower.followPath(scoreFirstPickUp); @@ -362,21 +327,11 @@ public void setPathState(int pState) { @Override public void loop() { follower.update(); + autonomousPathUpdate(); - sortingSpindexer(); - - try { - autonomousPathUpdate(); - } catch (InterruptedException e) { - throw new RuntimeException(e); - } if (isIntaking) { spindexerControls(); } - else { - spindexerMotor.setPower(0); - transferMotor.setPower(0); - } if (limelight.getLatestResult() != null && limelight.getLatestResult().isValid() && @@ -388,8 +343,6 @@ public void loop() { .getFiducialId(); } - setDesiredOrderFromAprilTag(aprilTagIndex); - telemetry.addData("AprilTag ID", aprilTagIndex); ArtifactColor spindexerColor = detectColor(spindexerColorSensor); ArtifactColor transferColor = detectColor(transferColorSensor); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TeleOp_1.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TeleOp_1.java index e872f96..f521698 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TeleOp_1.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TeleOp_1.java @@ -45,6 +45,7 @@ public class TeleOp_1 extends OpMode { private double transferMotorReleaseValue = 1; private double flywheelMotorReleaseFar = -1; private double flywheelMotorReleaseNear = -0.5; + private double minimumFlywheelValue = -1700; private double arcServoFarRight = 0.405; //higher hood, higher arc, higher range private double arcServoFarLeft = 0.72; //higher hood, higher arc, higher range @@ -200,20 +201,14 @@ else if(isOuttaking){ transferMotor.setPower(0); } } - - - /* - if(gamepad1.bWasPressed()){ - if(spindexerMotor.getPower()==0){ - spindexerMotor.setPower(spindexerMotorValue); - intakeMotor.setPower(intakeMotorRotatingValue); + else{ + if(transferMotor.getVelocity() Date: Sun, 4 Jan 2026 19:53:36 -0500 Subject: [PATCH 3/4] teleop fixes --- .../teamcode/pedroPathing/BlueGoalAuto.java | 2 -- .../ftc/teamcode/pedroPathing/TeleOp_1.java | 27 +++++++++---------- 2 files changed, 13 insertions(+), 16 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/BlueGoalAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/BlueGoalAuto.java index e74b8bc..c6a609c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/BlueGoalAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/BlueGoalAuto.java @@ -200,13 +200,11 @@ private void sortingSpindexer() { } else if (intakeSeen == wanted) { // Desired ball is at intake → rotate clockwise until it reaches transfer - transferMotor.setPower(-transferMotorRotatingValue); spindexerMotor.setPower(-spindexerMotorValue); // rotate clockwise shootArtifact(transferSeen, wanted); } else { // Desired ball not at intake → rotate counter-clockwise one pocket - transferMotor.setPower(transferMotorRotatingValue); // reverse spindexerMotor.setPower(spindexerMotorValue); //rotate counter clockwise shootArtifact(transferSeen, wanted); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TeleOp_1.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TeleOp_1.java index f521698..c5c51b1 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TeleOp_1.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TeleOp_1.java @@ -43,9 +43,8 @@ public class TeleOp_1 extends OpMode { private double intakeMotorIntakeValue = 1; private double transferMotorRotatingValue = -0.4; private double transferMotorReleaseValue = 1; - private double flywheelMotorReleaseFar = -1; - private double flywheelMotorReleaseNear = -0.5; - private double minimumFlywheelValue = -1700; + private double flywheelMotorRelease = -1; + private double minimumFlywheelValue = -1850; private double arcServoFarRight = 0.405; //higher hood, higher arc, higher range private double arcServoFarLeft = 0.72; //higher hood, higher arc, higher range @@ -57,7 +56,6 @@ public class TeleOp_1 extends OpMode { //button variables boolean topRightBumper1Previous = false; boolean topLeftBumper1Previous = false; - boolean continueSpindexerLoop = false; boolean isLaunching = false; boolean isIntaking = false; boolean isOuttaking = false; @@ -146,10 +144,14 @@ public void loop() { if(arcRightServo.getPosition()==arcServoFarRight && arcLeftServo.getPosition() == arcServoFarLeft){ //if at servo position 0, set to low arc position arcLeftServo.setPosition(arcServoCloseLeft); arcRightServo.setPosition(arcServoCloseRight); + minimumFlywheelValue = -1600; + flywheelMotorRelease = -0.4; } else{ arcLeftServo.setPosition(arcServoFarLeft); arcRightServo.setPosition(arcServoFarRight); + minimumFlywheelValue = -1900; + flywheelMotorRelease = -1; } } @@ -201,8 +203,9 @@ else if(isOuttaking){ transferMotor.setPower(0); } } + else{ - if(transferMotor.getVelocity() Date: Tue, 13 Jan 2026 17:57:14 -0500 Subject: [PATCH 4/4] auto blue goal updates --- .../teamcode/pedroPathing/BlueGoalAuto.java | 212 +++++++++++++----- .../ftc/teamcode/pedroPathing/TeleOp_1.java | 43 ++-- 2 files changed, 167 insertions(+), 88 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/BlueGoalAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/BlueGoalAuto.java index c6a609c..d7afda9 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/BlueGoalAuto.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/BlueGoalAuto.java @@ -1,5 +1,6 @@ package org.firstinspires.ftc.teamcode.pedroPathing; +import static java.lang.Thread.sleep; import com.pedropathing.follower.Follower; import com.pedropathing.geometry.BezierLine; @@ -9,7 +10,6 @@ import com.qualcomm.hardware.limelightvision.Limelight3A; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.OpMode; -import com.qualcomm.robotcore.hardware.CRServo; import com.qualcomm.robotcore.hardware.DcMotorEx; import com.qualcomm.robotcore.hardware.NormalizedColorSensor; import com.qualcomm.robotcore.hardware.NormalizedRGBA; @@ -30,18 +30,18 @@ public class BlueGoalAuto extends OpMode { private int pathState; // motor values - private double spindexerMotorValue = 0.2; + private double spindexerMotorValue = 0.4; private double intakeMotorIntakeValue = 1; - private double transferMotorRotatingValue = -0.6; + private double transferMotorRotatingValue = -0.8; private double transferMotorReleaseValue = 1; private double flywheelMotorReleaseFar = -1; - private double flywheelMotorReleaseClose = -0.2; - private double spindexerRelease = 0.4; + private double flywheelMotorReleaseClose = -0.6; + private double spindexerRelease = 0.25; private double arcServoFarRight = 0.405; //higher hood, higher arc, higher range private double arcServoFarLeft = 0.72; //higher hood, higher arc, higher range - private double arcServoCloseRight = 0.435; //lower hood, shorter range, front triangle - private double arcServoCloseLeft = 0.69; + private double arcServoCloseRight = 0.43; //lower hood, shorter range, front triangle + private double arcServoCloseLeft = 0.695; private double driveMaxPowerIntaking = 0.3; @@ -85,8 +85,9 @@ enum ArtifactColor { // declaring poses private final Pose startPose = new Pose(28.5, 128, Math.toRadians(135)); - private final Pose scorePreloadPose = new Pose(60, 85, Math.toRadians(125)); - private final Pose pickup1InitialPose = new Pose(55, 86, Math.toRadians(180)); + private final Pose readAprilTagPosition = new Pose(60, 100, Math.toRadians(125)); + private final Pose scorePreloadPosition = new Pose(75, 65, Math.toRadians(160)); + private final Pose pickup1InitialPose = new Pose(55, 65, Math.toRadians(180)); private final Pose pickup1FinalPose = new Pose(17, 86, Math.toRadians(180)); private final Pose scoreFirstPickUpPose = new Pose(60, 75, Math.toRadians(180)); @@ -95,7 +96,7 @@ enum ArtifactColor { private final Pose pickup3InitialPose = new Pose(40, 35, Math.toRadians(180)); private final Pose pickup3FinalPose = new Pose(30, 30, Math.toRadians(180)); - private PathChain scorePreload, pickupArtifact1Initial, pickupArtifact1Final, scoreFirstPickUp; + private PathChain readAprilTag, scorePreload, pickupArtifact1Initial, pickupArtifact1Final, scoreFirstPickUp; public void setSpindexerRelease() throws InterruptedException { if(isLaunching){ @@ -180,10 +181,74 @@ private void setDesiredOrderFromAprilTag(int tagID) { } } + private void sortArtifact() throws InterruptedException { - private void sortingSpindexer() { - if (!isLaunching) return; - if(desiredOrder.isEmpty()){ + ArtifactColor firstWanted = desiredOrder.get(0); + ArtifactColor secondWanted = desiredOrder.get(1); + + ArtifactColor transferSeen = detectColor(transferColorSensor); + ArtifactColor intakeSeen = detectColor(spindexerColorSensor); + + if(transferSeen == firstWanted && intakeSeen == secondWanted){ + transferMotor.setPower(transferMotorReleaseValue); + spindexerMotor.setPower(-spindexerRelease); + sleep(3500); + transferMotor.setPower(0); + spindexerMotor.setPower(0); + } + else if (transferSeen == firstWanted && intakeSeen != ArtifactColor.UNKNOWN){ + transferMotor.setPower(transferMotorReleaseValue); + spindexerMotor.setPower(spindexerRelease); + sleep(3000); + spindexerMotor.setPower(0); + transferMotor.setPower(0); + } else if (transferSeen == secondWanted && intakeSeen == firstWanted) { + spindexerMotor.setPower(-spindexerMotorValue); + sleep(1000); + transferMotor.setPower(transferMotorReleaseValue); + spindexerMotor.setPower(-spindexerRelease); + sleep(600); + spindexerMotor.setPower(spindexerRelease); + sleep(2000); + transferMotor.setPower(0); + spindexerMotor.setPower(0); + } else if (transferSeen != ArtifactColor.UNKNOWN && intakeSeen == firstWanted) { + spindexerMotor.setPower(-spindexerMotorValue); + sleep(500); + transferMotor.setPower(transferMotorReleaseValue); + sleep(3000); + spindexerMotor.setPower(0); + transferMotor.setPower(0); + } else if (transferSeen != firstWanted && intakeSeen != firstWanted) { + transferMotor.setPower(transferMotorRotatingValue); + spindexerMotor.setPower(spindexerMotorValue); + sleep(400); + transferMotor.setPower(transferMotorReleaseValue); + spindexerMotor.setPower(spindexerRelease); + sleep(2000); + transferMotor.setPower(0); + spindexerMotor.setPower(0); + } + else { + transferMotor.setPower(transferMotorReleaseValue); + spindexerMotor.setPower(spindexerRelease); + sleep(3000); + transferMotor.setPower(0); + spindexerMotor.setPower(0); + } + + } + + private void sortingSpindexer() throws InterruptedException { + telemetry.update(); + + if (desiredOrder.isEmpty()) { + isLaunching = false; + return; + } + + else if(desiredOrder.size() == 1){ + shootArtifact(); isLaunching = false; return; } @@ -194,46 +259,79 @@ private void sortingSpindexer() { ArtifactColor intakeSeen = detectColor(spindexerColorSensor); //correct ball is at shooting position - if (transferSeen == wanted && transferSeen != ArtifactColor.UNKNOWN) { + if (transferSeen == wanted) { // Correct color is ready → shoot - shootArtifact(transferSeen, wanted); + shootArtifact(); } else if (intakeSeen == wanted) { + transferMotor.setPower(transferMotorRotatingValue); + sleep(300); + telemetry.addLine("ball shot intake"); + spindexerMotor.setPower(-spindexerRelease); // rotate clockwise + sleep(600); // Desired ball is at intake → rotate clockwise until it reaches transfer - spindexerMotor.setPower(-spindexerMotorValue); // rotate clockwise - shootArtifact(transferSeen, wanted); + transferMotor.setPower(-transferMotorRotatingValue); + sleep(200); + transferMotor.setPower(transferMotorReleaseValue); + sleep(700); + desiredOrder.remove(0); + transferMotor.setPower(0); + sleep(200); + spindexerMotor.setPower(0); + } + else if (transferSeen!= ArtifactColor.UNKNOWN || intakeSeen != ArtifactColor.UNKNOWN){ + spindexerMotor.setPower(spindexerMotorValue); + transferMotor.setPower(transferMotorRotatingValue); + sleep(300); + shootArtifact(); } else { // Desired ball not at intake → rotate counter-clockwise one pocket spindexerMotor.setPower(spindexerMotorValue); //rotate counter clockwise - shootArtifact(transferSeen, wanted); + transferMotor.setPower(transferMotorRotatingValue); + sleep(500); + spindexerMotor.setPower(0); + transferMotor.setPower(0); + sortingSpindexer(); + } + + if (desiredOrder.isEmpty()){ + isLaunching = false; + } + else { + sleep(1500); + sortingSpindexer(); } - sortingSpindexer(); } - private void shootArtifact(ArtifactColor transferSeen, ArtifactColor wanted){ + private void shootArtifact() throws InterruptedException { // Correct color is ready → shoot - launchStartTicks = spindexerMotor.getCurrentPosition(); + telemetry.addLine("ball shot"); + desiredOrder.remove(0); transferMotor.setPower(transferMotorReleaseValue); spindexerMotor.setPower(spindexerRelease); - desiredOrder.remove(0); - } - - private void stopSpindexer() { + sleep(1000); transferMotor.setPower(0); + sleep(300); spindexerMotor.setPower(0); + sleep(700); } public void buildPaths() { + readAprilTag = follower.pathBuilder() + .addPath(new BezierLine(startPose, readAprilTagPosition)) + .setLinearHeadingInterpolation(startPose.getHeading(), readAprilTagPosition.getHeading()) + .build(); + scorePreload = follower.pathBuilder() - .addPath(new BezierLine(startPose, scorePreloadPose)) - .setLinearHeadingInterpolation(startPose.getHeading(), scorePreloadPose.getHeading()) + .addPath(new BezierLine(readAprilTagPosition, scorePreloadPosition)) + .setLinearHeadingInterpolation(readAprilTagPosition.getHeading(), scorePreloadPosition.getHeading()) .build(); pickupArtifact1Initial = follower.pathBuilder() - .addPath(new BezierLine(scorePreloadPose, pickup1InitialPose)) - .setLinearHeadingInterpolation(scorePreloadPose.getHeading(), pickup1InitialPose.getHeading()) + .addPath(new BezierLine(scorePreloadPosition, pickup1InitialPose)) + .setLinearHeadingInterpolation(scorePreloadPosition.getHeading(), pickup1InitialPose.getHeading()) .build(); pickupArtifact1Final = follower.pathBuilder() @@ -246,17 +344,16 @@ public void buildPaths() { .build(); } - public void autonomousPathUpdate() { + public void autonomousPathUpdate() throws InterruptedException { switch (pathState) { case 0: // start flywheel and move along path arcLeftServo.setPosition(arcServoCloseLeft); arcRightServo.setPosition(arcServoCloseRight); flywheelMotor.setPower(flywheelMotorReleaseClose); - turretServo.setPosition(0.85); // move turret to 0.8 + turretServo.setPosition(0.95); // move turret to 0.95 // start path following - follower.followPath(scorePreload); - pathTimer.resetTimer(); + follower.followPath(readAprilTag); setPathState(1); break; @@ -273,32 +370,31 @@ public void autonomousPathUpdate() { .get(0) .getFiducialId(); } - - setDesiredOrderFromAprilTag(aprilTagIndex); } + setDesiredOrderFromAprilTag(aprilTagIndex); - // advance path when shooting is complete - if (pathTimer.getElapsedTimeSeconds()>2.5) { - follower.followPath(pickupArtifact1Initial); - if(!follower.isBusy()){ - isLaunching = true; // start shooting - sortingSpindexer(); - sortingSpindexer(); + if (pathTimer.getElapsedTimeSeconds()>1.8){ + turretServo.setPosition(0.35); // move turret to 0.8 + follower.followPath(scorePreload); + if (pathTimer.getElapsedTimeSeconds()>4){ + sortArtifact(); } - setPathState(2); } break; case 2: - if(pathTimer.getElapsedTimeSeconds()>10 && !isLaunching){ + if(pathTimer.getElapsedTimeSeconds()>4.2 && !follower.isBusy()){ flywheelMotor.setPower(0); isIntaking=true; intakeMotor.setPower(intakeMotorIntakeValue); - follower.followPath(pickupArtifact1Final, driveMaxPowerIntaking, true); - setPathState(3); + follower.followPath(pickupArtifact1Initial); + if (!follower.isBusy()){ + follower.followPath(pickupArtifact1Final, driveMaxPowerIntaking, true); + setPathState(3); + } } break; case 3: - if(!follower.isBusy()){ + if(pathTimer.getElapsedTimeSeconds()>13 && !follower.isBusy()){ intakeMotor.setPower(0); isIntaking=false; follower.followPath(scoreFirstPickUp); @@ -309,8 +405,7 @@ public void autonomousPathUpdate() { case 4: if(!follower.isBusy()){ - transferMotor.setPower(transferMotorReleaseValue); - spindexerMotor.setPower(spindexerMotorValue); + sortingSpindexer(); } break; @@ -325,30 +420,25 @@ public void setPathState(int pState) { @Override public void loop() { follower.update(); - autonomousPathUpdate(); + try { + autonomousPathUpdate(); + } catch (InterruptedException e) { + throw new RuntimeException(e); + } if (isIntaking) { spindexerControls(); } - if (limelight.getLatestResult() != null && - limelight.getLatestResult().isValid() && - !limelight.getLatestResult().getFiducialResults().isEmpty()) { - - aprilTagIndex = limelight.getLatestResult() - .getFiducialResults() - .get(0) - .getFiducialId(); - } - telemetry.addData("AprilTag ID", aprilTagIndex); + telemetry.addData("Desired List: ", desiredOrder); ArtifactColor spindexerColor = detectColor(spindexerColorSensor); ArtifactColor transferColor = detectColor(transferColorSensor); telemetry.addData("Path State", pathState); telemetry.addData("Checking Revolution", checkingRevolution); telemetry.addData("RevStartPos", revStartPos); - telemetry.addData("Spindexer Sensor Color", spindexerColor); + telemetry.addData("Intake Sensor Color", spindexerColor); telemetry.addData("Transfer Sensor Color", transferColor); telemetry.update(); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TeleOp_1.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TeleOp_1.java index c5c51b1..6c38358 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TeleOp_1.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TeleOp_1.java @@ -8,11 +8,9 @@ import com.pedropathing.paths.HeadingInterpolator; import com.pedropathing.paths.Path; import com.pedropathing.paths.PathChain; -import com.qualcomm.hardware.limelightvision.LLResult; import com.qualcomm.hardware.limelightvision.Limelight3A; import com.qualcomm.robotcore.eventloop.opmode.OpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; -import com.qualcomm.robotcore.hardware.CRServo; import com.qualcomm.robotcore.hardware.DcMotorEx; import com.qualcomm.robotcore.hardware.NormalizedColorSensor; import com.qualcomm.robotcore.hardware.NormalizedRGBA; @@ -44,7 +42,7 @@ public class TeleOp_1 extends OpMode { private double transferMotorRotatingValue = -0.4; private double transferMotorReleaseValue = 1; private double flywheelMotorRelease = -1; - private double minimumFlywheelValue = -1850; + private double minimumFlywheelValue = -1800; private double arcServoFarRight = 0.405; //higher hood, higher arc, higher range private double arcServoFarLeft = 0.72; //higher hood, higher arc, higher range @@ -63,7 +61,6 @@ public class TeleOp_1 extends OpMode { @Override public void init() { - //hardware map declarations intakeMotor = hardwareMap.get(DcMotorEx.class, "intakeMotor"); //2, expansion transferMotor = hardwareMap.get(DcMotorEx.class, "transferMotor"); //3, expansion @@ -73,7 +70,7 @@ public void init() { arcRightServo = hardwareMap.get(Servo.class, "arcRightServo"); //2, expansion flywheelMotor = hardwareMap.get(DcMotorEx.class, "flywheelMotor"); //0, expansion spindexerColorSensor = hardwareMap.get(NormalizedColorSensor.class, "spindexerColorSensor"); //i2c bus 2 - spindexerColorSensor = hardwareMap.get(NormalizedColorSensor.class, "transferColorSensor"); //i2c bus 1 + transferColorSensor = hardwareMap.get(NormalizedColorSensor.class, "transferColorSensor"); //i2c bus 1 limelight = hardwareMap.get(Limelight3A.class, "limelight"); spindexerMotor.setMode(DcMotorEx.RunMode.STOP_AND_RESET_ENCODER); @@ -110,7 +107,8 @@ public void start() { @Override public void loop() { //variables - NormalizedRGBA colors = spindexerColorSensor.getNormalizedColors(); + NormalizedRGBA colorsIntake = spindexerColorSensor.getNormalizedColors(); + NormalizedRGBA colorsTransfer = transferColorSensor.getNormalizedColors(); boolean topRightBumper1 = gamepad1.right_bumper; boolean topLeftBumper1 = gamepad1.left_bumper; @@ -144,13 +142,13 @@ public void loop() { if(arcRightServo.getPosition()==arcServoFarRight && arcLeftServo.getPosition() == arcServoFarLeft){ //if at servo position 0, set to low arc position arcLeftServo.setPosition(arcServoCloseLeft); arcRightServo.setPosition(arcServoCloseRight); - minimumFlywheelValue = -1600; - flywheelMotorRelease = -0.4; + minimumFlywheelValue = -1400; + flywheelMotorRelease = -0.6; } else{ arcLeftServo.setPosition(arcServoFarLeft); arcRightServo.setPosition(arcServoFarRight); - minimumFlywheelValue = -1900; + minimumFlywheelValue = -1800; flywheelMotorRelease = -1; } } @@ -185,7 +183,7 @@ public void loop() { //collecting spindexer if(!isLaunching){ if (isIntaking) { - if(colors.red > 0.0006 || colors.green > 0.0009 || colors.blue > 0.0009) { + if(colorsIntake.red > 0.0006 || colorsIntake.green > 0.0009 || colorsIntake.blue > 0.0009) { spindexerMotor.setPower(spindexerMotorValue); transferMotor.setPower(transferMotorRotatingValue); } @@ -239,14 +237,14 @@ else if(isOuttaking){ else{ // transferMotor.setPower(0); spindexerMotor.setPower(0); - isLaunching = true; + isLaunching = false; topLeftBumper1Previous = false; } } //turret movement if(gamepad1.dpad_right){ - if(turretServo.getPosition()<0.85){ + if(turretServo.getPosition()<0.95){ turretServo.setPosition(turretServo.getPosition()+0.01); } } else if (gamepad1.dpad_left) { @@ -254,22 +252,13 @@ else if(isOuttaking){ } //Determining the amount of red, green, and blue - telemetry.addData("Red", colors.red); - telemetry.addData("Green", colors.green); - telemetry.addData("Blue", colors.blue); - - //april tag - if (limelight.getLatestResult() != null && - limelight.getLatestResult().isValid() && - !limelight.getLatestResult().getFiducialResults().isEmpty()) { - - aprilTagIndex = limelight.getLatestResult() - .getFiducialResults() - .get(0) - .getFiducialId(); - } + telemetry.addData("Intake Red", colorsIntake.red); + telemetry.addData("Intake Green", colorsIntake.green); + telemetry.addData("Intake Blue", colorsIntake.blue); - telemetry.addData("AprilTag ID", aprilTagIndex); + telemetry.addData("Transfer Red", colorsTransfer.red); + telemetry.addData("Transfer Green", colorsTransfer.green); + telemetry.addData("Transfer Blue", colorsTransfer.blue); //updating telemetry telemetryM.debug("position", follower.getPose());