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..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 @@ -10,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; @@ -31,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 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; @@ -81,13 +80,14 @@ 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 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)); @@ -96,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){ @@ -181,95 +181,157 @@ private void setDesiredOrderFromAprilTag(int tagID) { } } - private enum ShootState { - IDLE, - ALIGNING, - SHOOTING - } - private ShootState shootState = ShootState.IDLE; - - private void sortingSpindexer() { - if (!isLaunching) return; + private void sortArtifact() throws InterruptedException { - ArtifactColor wanted = desiredOrder.isEmpty() ? ArtifactColor.UNKNOWN : desiredOrder.get(0); + ArtifactColor firstWanted = desiredOrder.get(0); + ArtifactColor secondWanted = desiredOrder.get(1); ArtifactColor transferSeen = detectColor(transferColorSensor); ArtifactColor intakeSeen = detectColor(spindexerColorSensor); - switch (shootState) { + 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); + } - 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; - } + private void sortingSpindexer() throws InterruptedException { + telemetry.update(); - 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; + if (desiredOrder.isEmpty()) { + isLaunching = false; + return; + } - 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; + else if(desiredOrder.size() == 1){ + shootArtifact(); + isLaunching = false; + return; } - } - private boolean shooterHasCorrectColor() { - ArtifactColor seen = detectColor(transferColorSensor); - return seen != ArtifactColor.UNKNOWN && seen == desiredOrder.get(0); - } - private void spinUntilCorrectColor() { - transferMotor.setPower(transferMotorRotatingValue); - spindexerMotor.setPower(spindexerMotorValue); + ArtifactColor wanted = desiredOrder.get(0); + + ArtifactColor transferSeen = detectColor(transferColorSensor); + ArtifactColor intakeSeen = detectColor(spindexerColorSensor); + + //correct ball is at shooting position + if (transferSeen == wanted) { + // Correct color is ready → shoot + 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 + 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 + transferMotor.setPower(transferMotorRotatingValue); + sleep(500); + spindexerMotor.setPower(0); + transferMotor.setPower(0); + sortingSpindexer(); + } + + if (desiredOrder.isEmpty()){ + isLaunching = false; + } + else { + sleep(1500); + sortingSpindexer(); + } } - private void stopSpindexer() { + + private void shootArtifact() throws InterruptedException { + // Correct color is ready → shoot + telemetry.addLine("ball shot"); + desiredOrder.remove(0); + transferMotor.setPower(transferMotorReleaseValue); + spindexerMotor.setPower(spindexerRelease); + 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() @@ -289,17 +351,15 @@ public void autonomousPathUpdate() throws InterruptedException { arcLeftServo.setPosition(arcServoCloseLeft); arcRightServo.setPosition(arcServoCloseRight); flywheelMotor.setPower(flywheelMotorReleaseClose); - turretServo.setPosition(0.5); // move turret for 0.5s + turretServo.setPosition(0.95); // move turret to 0.95 // start path following - follower.followPath(scorePreload); - pathTimer.resetTimer(); + follower.followPath(readAprilTag); setPathState(1); 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() && @@ -310,32 +370,31 @@ public void autonomousPathUpdate() throws InterruptedException { .get(0) .getFiducialId(); } - - setDesiredOrderFromAprilTag(aprilTagIndex); - flywheelMotor.setPower(flywheelMotorReleaseClose); - pathTimer.resetTimer(); // optional, for next timed actions - isLaunching = true; // start shooting } + setDesiredOrderFromAprilTag(aprilTagIndex); - // advance path when shooting is complete - if (!isLaunching && !follower.isBusy()) { - flywheelMotor.setPower(0); - isIntaking = true; - intakeMotor.setPower(intakeMotorIntakeValue); - follower.followPath(pickupArtifact1Initial); - setPathState(2); + if (pathTimer.getElapsedTimeSeconds()>1.8){ + turretServo.setPosition(0.35); // move turret to 0.8 + follower.followPath(scorePreload); + if (pathTimer.getElapsedTimeSeconds()>4){ + sortArtifact(); + } } break; case 2: - if(!follower.isBusy()){ - sleep(500); - follower.followPath(pickupArtifact1Final, driveMaxPowerIntaking, true); - setPathState(3); + if(pathTimer.getElapsedTimeSeconds()>4.2 && !follower.isBusy()){ + flywheelMotor.setPower(0); + isIntaking=true; + intakeMotor.setPower(intakeMotorIntakeValue); + follower.followPath(pickupArtifact1Initial); + if (!follower.isBusy()){ + follower.followPath(pickupArtifact1Final, driveMaxPowerIntaking, true); + setPathState(3); + } } break; case 3: - if(!follower.isBusy()){ - sleep(300); + if(pathTimer.getElapsedTimeSeconds()>13 && !follower.isBusy()){ intakeMotor.setPower(0); isIntaking=false; follower.followPath(scoreFirstPickUp); @@ -346,8 +405,7 @@ public void autonomousPathUpdate() throws InterruptedException { case 4: if(!follower.isBusy()){ - transferMotor.setPower(transferMotorReleaseValue); - spindexerMotor.setPower(spindexerMotorValue); + sortingSpindexer(); } break; @@ -362,42 +420,25 @@ public void setPathState(int pState) { @Override public void loop() { follower.update(); - - 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() && - !limelight.getLatestResult().getFiducialResults().isEmpty()) { - - aprilTagIndex = limelight.getLatestResult() - .getFiducialResults() - .get(0) - .getFiducialId(); - } - - setDesiredOrderFromAprilTag(aprilTagIndex); 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 e872f96..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; @@ -43,8 +41,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 flywheelMotorRelease = -1; + 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 @@ -56,7 +54,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; @@ -64,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 @@ -74,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); @@ -111,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; @@ -145,10 +142,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 = -1400; + flywheelMotorRelease = -0.6; } else{ arcLeftServo.setPosition(arcServoFarLeft); arcRightServo.setPosition(arcServoFarRight); + minimumFlywheelValue = -1800; + flywheelMotorRelease = -1; } } @@ -182,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); } @@ -201,30 +202,19 @@ else if(isOuttaking){ } } - - /* - if(gamepad1.bWasPressed()){ - if(spindexerMotor.getPower()==0){ - spindexerMotor.setPower(spindexerMotorValue); - intakeMotor.setPower(intakeMotorRotatingValue); + else{ + if(flywheelMotor.getVelocity()