From 457eaf5feb14654bd4c9edda75818e376460b18e Mon Sep 17 00:00:00 2001 From: KeshavAnandCode Date: Thu, 26 Feb 2026 23:17:16 -0600 Subject: [PATCH] fixed sxonwe color sorting...jusyt have to have a working auto --- .../teamcode/autonomous/Auto_LT_Close.java | 33 ++++----- .../autonomous/actions/AutoActions.java | 71 ++++++++++++++----- 2 files changed, 67 insertions(+), 37 deletions(-) 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 99a040d..a2148ea 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 @@ -59,19 +59,19 @@ public class Auto_LT_Close extends LinearOpMode { public static double hood0MoveTime = 2; public static double spindexerSpeedIncrease = 0.014; - public static double shootAllTime = 3.5; + public static double shootAllTime = 10.0; public static double intake1Time = 3.3; public static double intake2Time = 3.8; public static double intake3Time = 4.2; - public static double flywheel0Time = 1.5; - public static double pickup1Speed = 12; + public static double flywheel0Time = 2.2; + public static double pickup1Speed = 22; // ---- POSITION TOLERANCES ---- public static double posXTolerance = 5.0; public static double posYTolerance = 5.0; // ---- OBELISK DETECTION ---- - public static double shoot1Time = 2.5; + public static double shoot1Time = 3.0; public static double shoot2Time = 2.5; public static double shoot3Time = 2.5; public static double colorSenseTime = 1.2; @@ -544,25 +544,17 @@ public class Auto_LT_Close extends LinearOpMode { Actions.runBlocking( new ParallelAction( shoot0.build(), - autoActions.manageShooterAuto( + autoActions.prepareShootAll( + colorSenseTime, flywheel0Time, + motif, x1, y1, - h1, - false - ), - autoActions.detectObelisk( - flywheel0Time, - 0.501, - 0.501, - 0.501, - 0.501, - obeliskTurrPosAutoStart + h1 ) ) ); - motif = turret.getObeliskID(); } @@ -573,15 +565,18 @@ public class Auto_LT_Close extends LinearOpMode { Actions.runBlocking( new ParallelAction( shoot0.build(), - autoActions.manageShooterAuto( + autoActions.prepareShootAll( + colorSenseTime, flywheel0Time, + motif, xShoot0, yShoot0, - hShoot0, - false + hShoot0 ) ) ); + + } void cycleStackClose() { 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 7914d32..52c4c02 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 @@ -18,7 +18,9 @@ import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; import com.acmerobotics.dashboard.telemetry.TelemetryPacket; import com.acmerobotics.roadrunner.Action; import com.acmerobotics.roadrunner.Pose2d; +import com.qualcomm.robotcore.hardware.NormalizedRGBA; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; import org.firstinspires.ftc.teamcode.constants.ServoPositions; import org.firstinspires.ftc.teamcode.constants.StateEnums; import org.firstinspires.ftc.teamcode.libs.RR.MecanumDrive; @@ -145,18 +147,35 @@ public class AutoActions { servos.setSpinPos(spindexer_intakePos1 + spindexerWiggle); - spindexer.detectBalls(true, true); - if (Objects.equals(spindexer.GetFrontDriverColor(), Spindexer.BallColor.GREEN)) { - driverSlotGreen++; +// Rear Center (Position 1) + double distanceRearCenter = robot.color1.getDistance(DistanceUnit.MM); + if (distanceRearCenter < 52) { + NormalizedRGBA color1RGBA = robot.color1.getNormalizedColors(); + double gP1 = color1RGBA.green / (color1RGBA.green + color1RGBA.red + color1RGBA.blue); + if (gP1 >= 0.38) { + rearSlotGreen++; + } } - if (Objects.equals(spindexer.GetFrontPassengerColor(), Spindexer.BallColor.GREEN)) { - passengerSlotGreen++; +// Front Driver (Position 2) + double distanceFrontDriver = robot.color2.getDistance(DistanceUnit.MM); + if (distanceFrontDriver < 50) { + NormalizedRGBA color2RGBA = robot.color2.getNormalizedColors(); + double gP2 = color2RGBA.green / (color2RGBA.green + color2RGBA.red + color2RGBA.blue); + if (gP2 >= 0.4) { + driverSlotGreen++; + } } - if (Objects.equals(spindexer.GetRearCenterColor(), Spindexer.BallColor.GREEN)) { - rearSlotGreen++; +// Front Passenger (Position 3) + double distanceFrontPassenger = robot.color3.getDistance(DistanceUnit.MM); + if (distanceFrontPassenger < 29) { + NormalizedRGBA color3RGBA = robot.color3.getNormalizedColors(); + double gP3 = color3RGBA.green / (color3RGBA.green + color3RGBA.red + color3RGBA.blue); + if (gP3 >= 0.4) { + passengerSlotGreen++; + } } spindexer.setIntakePower(-0.1); @@ -178,29 +197,45 @@ public class AutoActions { if (motif_id == 21) { if (mostGreenSlot == 1) { - spin1PosFirst(); + firstSpindexShootPos = spindexer_outtakeBall1; + shootForward = true; + spinEndPos = 0.95; } else if (mostGreenSlot == 2) { - spin2PosFirst(); + firstSpindexShootPos = spindexer_outtakeBall2; + shootForward = false; + spinEndPos = 0.05; } else { - spin3PosFirst(); - } + firstSpindexShootPos = spindexer_outtakeBall3b; + shootForward = true; + spinEndPos = 0.95; } } else if (motif_id == 22) { if (mostGreenSlot == 1) { - spin2PosFirst(); + firstSpindexShootPos = spindexer_outtakeBall3b; + shootForward = true; + spinEndPos = 0.95; } else if (mostGreenSlot == 2) { - spin3PosFirst(); + firstSpindexShootPos = spindexer_outtakeBall1; + shootForward = true; + spinEndPos = 0.95; } else { - reverseSpin2PosFirst(); + firstSpindexShootPos = spindexer_outtakeBall1; + shootForward = false; + spinEndPos = 0.03; } } else { if (mostGreenSlot == 1) { - spin3PosFirst(); + firstSpindexShootPos = spindexer_outtakeBall3; + shootForward = false; + spinEndPos = 0.05; } else if (mostGreenSlot == 2) { - oddSpin3PosFirst(); + firstSpindexShootPos = spindexer_outtakeBall3b; + shootForward = true; + spinEndPos = 0.95; } else { - spin1PosFirst(); - } + firstSpindexShootPos = spindexer_outtakeBall1; + shootForward = true; + spinEndPos = 0.95; } } return true;