From dc9886855b3ffae7a7b15854bab7cfc6ec63a4cb Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Thu, 26 Feb 2026 22:18:36 -0600 Subject: [PATCH] sorting ahh thing --- .../teamcode/autonomous/Auto_LT_Close.java | 119 +++++++++--------- .../autonomous/actions/AutoActions.java | 36 ++++-- .../ftc/teamcode/constants/Front_Poses.java | 4 +- .../teamcode/constants/ServoPositions.java | 14 ++- .../ftc/teamcode/tests/ColorTest.java | 13 +- .../ftc/teamcode/tests/SortingTest.java | 107 ++++++++++++++++ .../ftc/teamcode/utils/Spindexer.java | 20 +-- .../ftc/teamcode/utils/Turret.java | 9 +- 8 files changed, 226 insertions(+), 96 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/SortingTest.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java index 2ba0c8a..99a040d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/Auto_LT_Close.java @@ -5,10 +5,16 @@ import static org.firstinspires.ftc.teamcode.constants.Front_Poses.*; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.blueObeliskTurrPos1; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.blueObeliskTurrPos2; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.blueObeliskTurrPos3; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.redObeliskTurrPos0; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.redObeliskTurrPos1; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.redObeliskTurrPos2; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.redObeliskTurrPos3; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinStartPos; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos1; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall1; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall2; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall3; +import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_outtakeBall3b; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.transferServo_out; import static org.firstinspires.ftc.teamcode.utils.Turret.limelightUsed; import static org.firstinspires.ftc.teamcode.utils.Turret.turrDefault; @@ -127,6 +133,7 @@ public class Auto_LT_Close extends LinearOpMode { double obeliskTurrPos2 = 0.0; double obeliskTurrPos3 = 0.0; double waitToPickupGate = 0; + double obeliskTurrPosAutoStart = 0; // initialize path variables here TrajectoryActionBuilder shoot0 = null; @@ -168,19 +175,45 @@ public class Auto_LT_Close extends LinearOpMode { autoActions = new AutoActions(robot, drive, TELE, servos, flywheel, spindexer, targeting, targetingSettings, turret, light); - servos.setSpinPos(spinStartPos); + servos.setSpinPos(spindexer_intakePos1); servos.setTransferPos(transferServo_out); - - limelightUsed = true; + limelightUsed = false; robot.light.setPosition(1); hardwareMap.get(GoBildaPinpointDriver.class, "pinpoint").resetPosAndIMU(); - while (opModeInInit()) { + if (limelightUsed && !gateCycle){ + Actions.runBlocking( + autoActions.detectObelisk( + 0.1, + 0.501, + 0.501, + 0.501, + 0.501, + obeliskTurrPosAutoStart + ) + ); + motif = turret.getObeliskID(); + if (motif == 21){ + AutoActions.firstSpindexShootPos = spindexer_outtakeBall1; + } else if (motif == 22){ + AutoActions.firstSpindexShootPos = spindexer_outtakeBall3b; + } else { + AutoActions.firstSpindexShootPos = spindexer_outtakeBall2; + } + } + + if (!gateCycle) { + turret.pipelineSwitch(1); + } else if (redAlliance) { + turret.pipelineSwitch(4); + } else { + turret.pipelineSwitch(2); + } if (gateCycle) { servos.setHoodPos(hoodGate0Start); @@ -188,8 +221,6 @@ public class Auto_LT_Close extends LinearOpMode { servos.setHoodPos(shoot0Hood); } - turret.setTurret(turrDefault); - if (gamepad2.crossWasPressed()) { redAlliance = !redAlliance; } @@ -209,24 +240,17 @@ public class Auto_LT_Close extends LinearOpMode { ballCycles--; } + if (gamepad2.triangleWasPressed()){ + gateCycle = !gateCycle; + } + if (gamepad2.squareWasPressed()) { - drive = new MecanumDrive(hardwareMap,new Pose2d(0,0,0)); - - - if (!gateCycle) { - turret.pipelineSwitch(1); - } else if (redAlliance) { - turret.pipelineSwitch(4); - } else { - turret.pipelineSwitch(2); - } robot.limelight.start(); - - + limelightUsed = true; gamepad2.rumble(500); } @@ -283,6 +307,7 @@ public class Auto_LT_Close extends LinearOpMode { pickupGateBY = rPickupGateBY; pickupGateBH = rPickupGateBH; + obeliskTurrPosAutoStart = turrDefault + redObeliskTurrPos0; obeliskTurrPos1 = turrDefault + redObeliskTurrPos1; obeliskTurrPos2 = turrDefault + redObeliskTurrPos2; obeliskTurrPos3 = turrDefault + redObeliskTurrPos3; @@ -338,6 +363,7 @@ public class Auto_LT_Close extends LinearOpMode { pickupGateBY = bPickupGateBY; pickupGateBH = bPickupGateBH; + obeliskTurrPosAutoStart = turrDefault + redObeliskTurrPos0; obeliskTurrPos1 = turrDefault + blueObeliskTurrPos1; obeliskTurrPos2 = turrDefault + blueObeliskTurrPos2; obeliskTurrPos3 = turrDefault + blueObeliskTurrPos3; @@ -414,10 +440,6 @@ public class Auto_LT_Close extends LinearOpMode { .strafeToLinearHeading(new Vector2d(xShoot, yShoot), Math.toRadians(hShoot)); } - - - - pickup3 = drive.actionBuilder(new Pose2d(xShoot, yShoot, Math.toRadians(hShoot))) .strafeToLinearHeading(new Vector2d(x4a, y4a), Math.toRadians(h4a)) .strafeToLinearHeading(new Vector2d(x4b, y4b), Math.toRadians(h4b), @@ -432,13 +454,13 @@ public class Auto_LT_Close extends LinearOpMode { waitToPickupGate = waitToPickupGateSolo; } - - - teleStart = drive.localizer.getPose(); TELE.addData("Red?", redAlliance); TELE.addData("Turret Default", turrDefault); + TELE.addData("Gate Cycle?", gateCycle); TELE.addData("Ball Cycles", ballCycles); + TELE.addData("Limelight Started?", limelightUsed); + TELE.addData("Motif", motif); TELE.update(); } @@ -528,9 +550,19 @@ public class Auto_LT_Close extends LinearOpMode { y1, h1, false + ), + autoActions.detectObelisk( + flywheel0Time, + 0.501, + 0.501, + 0.501, + 0.501, + obeliskTurrPosAutoStart ) ) ); + + motif = turret.getObeliskID(); } @@ -561,24 +593,10 @@ public class Auto_LT_Close extends LinearOpMode { x2b, y2b, h2b - ), - autoActions.detectObelisk( - intake1Time, - x2b, - y2b, - posXTolerance, - posYTolerance, - obeliskTurrPos1 ) - ) ); - motif = turret.getObeliskID(); - - if (motif == 0) motif = 22; - prevMotif = motif; - double posX; double posY; double posH; @@ -616,24 +634,10 @@ public class Auto_LT_Close extends LinearOpMode { x3b, y3b, h3b - ), - autoActions.detectObelisk( - intake2Time, - x3b, - y3b, - posXTolerance, - posYTolerance, - obeliskTurrPos2 ) - ) ); - motif = turret.getObeliskID(); - - if (motif == 0) motif = prevMotif; - prevMotif = motif; - double posX; double posY; double posH; @@ -670,16 +674,7 @@ public class Auto_LT_Close extends LinearOpMode { x4b, y4b, h4b - ), - autoActions.detectObelisk( - intake3Time, - x4b, - y4b, - posXTolerance, - posYTolerance, - obeliskTurrPos3 ) - ) ); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/actions/AutoActions.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/actions/AutoActions.java index 9d1cf0a..7914d32 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/actions/AutoActions.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/autonomous/actions/AutoActions.java @@ -48,7 +48,7 @@ public class AutoActions { private int passengerSlotGreen = 0; private int rearSlotGreen = 0; private int mostGreenSlot = 0; - private double firstSpindexShootPos = spinStartPos; + public static double firstSpindexShootPos = spinStartPos; private boolean shootForward = true; public int motif = 0; double spinEndPos = ServoPositions.spinEndPos; @@ -85,13 +85,13 @@ public class AutoActions { void spin1PosFirst() { firstSpindexShootPos = spindexer_outtakeBall1; shootForward = true; - spinEndPos = spindexer_outtakeBall3 + 0.1; + spinEndPos = 0.95; } void spin2PosFirst() { firstSpindexShootPos = spindexer_outtakeBall2; shootForward = false; - spinEndPos = spindexer_outtakeBall3b - 0.1; + spinEndPos = 0.05; } void reverseSpin2PosFirst() { @@ -103,13 +103,13 @@ public class AutoActions { void spin3PosFirst() { firstSpindexShootPos = spindexer_outtakeBall3; shootForward = false; - spinEndPos = spindexer_outtakeBall1 - 0.1; + spinEndPos = 0.05; } void oddSpin3PosFirst() { firstSpindexShootPos = spindexer_outtakeBall3b; shootForward = true; - spinEndPos = spindexer_outtakeBall2 + 0.1; + spinEndPos = 0.95; } Action manageShooter = null; @@ -119,6 +119,9 @@ public class AutoActions { if (ticker == 0) { stamp = System.currentTimeMillis(); manageShooter = manageShooterAuto(time, posX, posY, posH, false); + driverSlotGreen = 0; + passengerSlotGreen = 0; + rearSlotGreen = 0; } ticker++; servos.setTransferPos(transferServo_out); @@ -130,6 +133,12 @@ public class AutoActions { manageShooter.run(telemetryPacket); + TELE.addData("Most Green Slot", mostGreenSlot); + TELE.addData("Driver Slot Greeness", driverSlotGreen); + TELE.addData("Passenger Slot Greeness", passengerSlotGreen); + TELE.addData("Rear Greeness", rearSlotGreen); + TELE.update(); + if ((System.currentTimeMillis() - stamp) < (colorSenseTime * 1000)) { spindexerWiggle *= -1.0; @@ -150,7 +159,7 @@ public class AutoActions { rearSlotGreen++; } - spindexer.setIntakePower(1); + spindexer.setIntakePower(-0.1); decideGreenSlot = true; @@ -242,9 +251,9 @@ public class AutoActions { boolean end; if (shootForward) { - end = prevSpinPos > spinEndPos; + end = servos.getSpinPos() > spinEndPos; } else { - end = prevSpinPos < spinEndPos; + end = servos.getSpinPos() < spinEndPos; } if (System.currentTimeMillis() - stamp < shootTime * 1000 && (!end || shooterTicker < Spindexer.waitFirstBallTicks + 1)) { @@ -380,7 +389,6 @@ public class AutoActions { manageShooter.run(telemetryPacket); if ((System.currentTimeMillis() - stamp) > (time * 1000) || spindexer.isFull()) { - spindexer.setIntakePower(-0.1); return false; } else { return true; @@ -408,6 +416,7 @@ public class AutoActions { double stamp = 0.0; int ticker = 0; + int prevMotif = 0; @Override public boolean run(@NonNull TelemetryPacket telemetryPacket) { @@ -418,18 +427,23 @@ public class AutoActions { if (ticker == 0) { stamp = System.currentTimeMillis(); turret.pipelineSwitch(1); + ticker++; } - ticker++; motif = turret.detectObelisk(); + if (prevMotif == motif){ + ticker++; + } + prevMotif = motif; + turret.setTurret(turrPos); boolean timeDone = timeFallback && (System.currentTimeMillis() - stamp) > time * 1000; boolean xDone = posXFallback && Math.abs(currentPose.position.x - posX) < posXTolerance; boolean yDone = posYFallback && Math.abs(currentPose.position.y - posY) < posYTolerance; - boolean shouldFinish = timeDone || (xDone && yDone) || spindexer.isFull(); + boolean shouldFinish = timeDone || (xDone && yDone) || spindexer.isFull() || ticker > 10; teleStart = currentPose; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Front_Poses.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Front_Poses.java index a947943..9a8274f 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Front_Poses.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Front_Poses.java @@ -10,10 +10,10 @@ public class Front_Poses { public static double rx1 = 20, ry1 = 0.5, rh1 = 0.1; public static double bx1 = 20, by1 = -0.5, bh1 = -0.1; - public static double rx2a = 41, ry2a = 18, rh2a = 155; + public static double rx2a = 41, ry2a = 18, rh2a = 140; public static double bx2a = 41, by2a = -18, bh2a = -140; - public static double rx2b = 21, ry2b = 34, rh2b = 155.1; + public static double rx2b = 21, ry2b = 34, rh2b = 140.1; public static double bx2b = 23, by2b = -36, bh2b = -140.1; public static double rx3a = 55, ry3a = 39, rh3a = 140; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java index f678d0a..73ac55d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/ServoPositions.java @@ -33,12 +33,14 @@ public class ServoPositions { public static double turret_blueClose = 0; // These values are ADDED to turrDefault - public static double redObeliskTurrPos1 = 0.12; - public static double redObeliskTurrPos2 = 0.13; - public static double redObeliskTurrPos3 = 0.14; - public static double blueObeliskTurrPos1 = -0.12; - public static double blueObeliskTurrPos2 = -0.13; - public static double blueObeliskTurrPos3 = -0.14; + public static double redObeliskTurrPos0 = -0.35; + public static double redObeliskTurrPos1 = 0.15; + public static double redObeliskTurrPos2 = 0.16; + public static double redObeliskTurrPos3 = 0.17; + public static double blueObeliskTurrPos0 = 0.35; + public static double blueObeliskTurrPos1 = -0.15; + public static double blueObeliskTurrPos2 = -0.16; + public static double blueObeliskTurrPos3 = -0.17; public static double redTurretShootPos = 0.05; public static double blueTurretShootPos = -0.05; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ColorTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ColorTest.java index fd86f1d..233a608 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ColorTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ColorTest.java @@ -7,6 +7,7 @@ import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.NormalizedRGBA; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; import org.firstinspires.ftc.teamcode.utils.Robot; @@ -28,16 +29,20 @@ public class ColorTest extends LinearOpMode { if (isStopRequested()) return; while(opModeIsActive()){ - double green1 = robot.color1.getNormalizedColors().green; - double blue1 = robot.color1.getNormalizedColors().blue; - double red1 = robot.color1.getNormalizedColors().red; + + + NormalizedRGBA color1RGBA = robot.color1.getNormalizedColors(); + + double gP1 = color1RGBA.green / (color1RGBA.green + color1RGBA.red + color1RGBA.blue); + double dist1 = robot.color1.getDistance(DistanceUnit.MM); color1Distance = (colorFilterAlpha * dist1) + ((1-colorFilterAlpha) * color1Distance); TELE.addData("Color1 toColor", robot.color1.getNormalizedColors().toColor()); - TELE.addData("Color1 green", green1 / (green1 + blue1 + red1)); + TELE.addData("Color1 green", gP1); TELE.addData("Color1 distance (mm)", color1Distance); + // ----- COLOR 2 ----- double green2 = robot.color2.getNormalizedColors().green; double blue2 = robot.color2.getNormalizedColors().blue; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/SortingTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/SortingTest.java new file mode 100644 index 0000000..6c63f72 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/SortingTest.java @@ -0,0 +1,107 @@ +package org.firstinspires.ftc.teamcode.tests; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.acmerobotics.roadrunner.ftc.Actions; +import com.acmerobotics.roadrunner.Pose2d; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import org.firstinspires.ftc.teamcode.autonomous.actions.AutoActions; +import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive; +import org.firstinspires.ftc.teamcode.utils.Flywheel; +import org.firstinspires.ftc.teamcode.utils.Light; +import org.firstinspires.ftc.teamcode.utils.Robot; +import org.firstinspires.ftc.teamcode.utils.Servos; +import org.firstinspires.ftc.teamcode.utils.Spindexer; +import org.firstinspires.ftc.teamcode.utils.Targeting; +import org.firstinspires.ftc.teamcode.utils.Turret; + +@Config +@TeleOp +public class SortingTest extends LinearOpMode { + Robot robot; + MultipleTelemetry TELE; + MecanumDrive drive; + Servos servos; + Spindexer spindexer; + Flywheel flywheel; + Turret turret; + Targeting targeting; + Targeting.Settings targetingSettings; + AutoActions autoActions; + Light light; + @Override + public void runOpMode() throws InterruptedException { + robot = new Robot(hardwareMap); + + TELE = new MultipleTelemetry( + telemetry, FtcDashboard.getInstance().getTelemetry() + ); + + flywheel = new Flywheel(hardwareMap); + + targeting = new Targeting(); + targetingSettings = new Targeting.Settings(0.0, 0.0); + + spindexer = new Spindexer(hardwareMap); + + servos = new Servos(hardwareMap); + + turret = new Turret(robot, TELE, robot.limelight); + + drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0)); + + light = Light.getInstance(); + + light.init(robot.light, spindexer, turret); + + autoActions = new AutoActions(robot, drive, TELE, servos, flywheel, spindexer, targeting, targetingSettings, turret, light); + + int motif = 21; + boolean intaking = true; + + waitForStart(); + if (isStopRequested()) return; + + while (opModeIsActive()){ + spindexer.setIntakePower(1); + robot.transfer.setPower(1); + + if (gamepad1.crossWasPressed()){ + motif = 21; + } else if (gamepad1.squareWasPressed()){ + motif = 22; + } else if (gamepad1.triangleWasPressed()){ + motif = 23; + } + flywheel.manageFlywheel(2500); + + if (gamepad1.leftBumperWasPressed()){ + intaking = false; + Actions.runBlocking( + autoActions.prepareShootAll( + 3, + 5, + motif, + 0.501, + 0.501, + 0.501 + ) + ); + } else if (gamepad1.rightBumperWasPressed()){ + intaking = false; + Actions.runBlocking( + autoActions.shootAllAuto( + 3.5, + 0.014 + ) + ); + intaking = true; + } else if (intaking){ + spindexer.processIntake(); + } + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Spindexer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Spindexer.java index 0c6f178..089743c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Spindexer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Spindexer.java @@ -187,7 +187,7 @@ public class Spindexer { distanceFrontPassenger = (colorFilterAlpha * dFrontPassenger) + ((1-colorFilterAlpha) * distanceFrontPassenger); // Position 1 - if (distanceRearCenter < 48) { + if (distanceRearCenter < 52) { // Mark Ball Found newPos1Detection = true; @@ -200,9 +200,9 @@ public class Spindexer { // FIXIT - Add filtering to improve accuracy. if (gP >= 0.38) { - ballPositions[commandedIntakePosition].ballColor = BallColor.GREEN; // green + ballPositions[0].ballColor = BallColor.GREEN; // green } else { - ballPositions[commandedIntakePosition].ballColor = BallColor.PURPLE; // purple + ballPositions[0].ballColor = BallColor.PURPLE; // purple } } } @@ -218,9 +218,9 @@ public class Spindexer { double gP = color2RGBA.green / (color2RGBA.green + color2RGBA.red + color2RGBA.blue); if (gP >= 0.4) { - ballPositions[spindexerBallPos].ballColor = BallColor.GREEN; // green + ballPositions[2].ballColor = BallColor.GREEN; // green } else { - ballPositions[spindexerBallPos].ballColor = BallColor.PURPLE; // purple + ballPositions[2].ballColor = BallColor.PURPLE; // purple } } } else { @@ -244,10 +244,10 @@ public class Spindexer { double gP = color3RGBA.green / (color3RGBA.green + color3RGBA.red + color3RGBA.blue); - if (gP >= 0.42) { - ballPositions[spindexerBallPos].ballColor = BallColor.GREEN; // green + if (gP >= 0.4) { + ballPositions[1].ballColor = BallColor.GREEN; // green } else { - ballPositions[spindexerBallPos].ballColor = BallColor.PURPLE; // purple + ballPositions[1].ballColor = BallColor.PURPLE; // purple } } } else { @@ -671,8 +671,10 @@ public class Spindexer { return ballPositions[RotatedBallPositions[commandedIntakePosition][RotatedBallPositionNames.REARCENTER.ordinal()]].ballColor; } private double prevPow = 0.501; + private boolean firstIntakePow = true; public void setIntakePower(double pow){ - if (prevPow != 0.501 && prevPow != pow){ + if (firstIntakePow || prevPow != pow){ + firstIntakePow = false; robot.intake.setPower(pow); } prevPow = pow; diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java index 1f3b81e..ad71b6a 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Turret.java @@ -14,6 +14,7 @@ import com.qualcomm.robotcore.hardware.DcMotor; import org.firstinspires.ftc.robotcore.external.navigation.Pose3D; import org.firstinspires.ftc.teamcode.constants.Color; +import java.util.ArrayList; import java.util.List; @Config @@ -130,7 +131,7 @@ public class Turret { } } if (xPos != null){ - if (zPos>0) { + if (zPos<0) { limelightTagX = (alphaPosConstant * xPos) + ((1 - alphaPosConstant) * limelightTagX); limelightTagY = (alphaPosConstant * yPos) + ((1 - alphaPosConstant) * limelightTagY); limelightTagZ = (alphaPosConstant * zPos) + ((1 - alphaPosConstant) * limelightTagZ); @@ -171,8 +172,12 @@ public class Turret { LLResult result = webcam.getLatestResult(); if (result != null && result.isValid()) { List fiducials = result.getFiducialResults(); + double prevTx = -1000; for (LLResultTypes.FiducialResult fiducial : fiducials) { - obeliskID = fiducial.getFiducialId(); + double currentTx = fiducial.getTargetXDegrees(); + if (currentTx > prevTx){ + obeliskID = fiducial.getFiducialId(); + } } } return obeliskID;