From b9e6dff3f84206fab31c388cfb5247c9b41f9632 Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Fri, 9 Jan 2026 22:18:25 -0600 Subject: [PATCH] intake test to be further tested into crowded balls --- .../ftc/teamcode/tests/ColorTest.java | 55 ++++++++ .../ftc/teamcode/tests/IntakeTest.java | 123 +++++++++++------- .../utils/PositionalServoProgrammer.java | 4 +- 3 files changed, 134 insertions(+), 48 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ColorTest.java 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 new file mode 100644 index 0000000..f0ce6b6 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ColorTest.java @@ -0,0 +1,55 @@ +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.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.teamcode.utils.Robot; + +@Config +@TeleOp +public class ColorTest extends LinearOpMode { + Robot robot; + MultipleTelemetry TELE; + @Override + public void runOpMode() throws InterruptedException { + robot = new Robot(hardwareMap); + TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); + + waitForStart(); + if (isStopRequested()) return; + + while(opModeIsActive()){ + double green1 = robot.color1.getNormalizedColors().green; + double blue1 = robot.color1.getNormalizedColors().blue; + double red1 = robot.color1.getNormalizedColors().red; + + TELE.addData("Color1 toColor", robot.color1.getNormalizedColors().toColor()); + TELE.addData("Color1 green", green1 / (green1 + blue1 + red1)); + TELE.addData("Color1 distance (mm)", robot.color1.getDistance(DistanceUnit.MM)); + +// ----- COLOR 2 ----- + double green2 = robot.color2.getNormalizedColors().green; + double blue2 = robot.color2.getNormalizedColors().blue; + double red2 = robot.color2.getNormalizedColors().red; + + TELE.addData("Color2 toColor", robot.color2.getNormalizedColors().toColor()); + TELE.addData("Color2 green", green2 / (green2 + blue2 + red2)); + TELE.addData("Color2 distance (mm)", robot.color2.getDistance(DistanceUnit.MM)); + +// ----- COLOR 3 ----- + double green3 = robot.color3.getNormalizedColors().green; + double blue3 = robot.color3.getNormalizedColors().blue; + double red3 = robot.color3.getNormalizedColors().red; + + TELE.addData("Color3 toColor", robot.color3.getNormalizedColors().toColor()); + TELE.addData("Color3 green", green3 / (green3 + blue3 + red3)); + TELE.addData("Color3 distance (mm)", robot.color3.getDistance(DistanceUnit.MM)); + + TELE.update(); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/IntakeTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/IntakeTest.java index 0625f6e..673f116 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/IntakeTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/IntakeTest.java @@ -5,6 +5,7 @@ import static org.firstinspires.ftc.teamcode.constants.ServoPositions.*; import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.qualcomm.hardware.lynx.LynxModule; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; @@ -12,25 +13,42 @@ import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; import org.firstinspires.ftc.teamcode.utils.Robot; import org.firstinspires.ftc.teamcode.utils.Servos; +import java.util.ArrayList; +import java.util.List; + @Config @TeleOp public class IntakeTest extends LinearOpMode { Robot robot; MultipleTelemetry TELE; Servos servo; - + public boolean green1 = false; + public boolean green2 = false; + public boolean green3 = false; + List s1G = new ArrayList<>(); + List s2G = new ArrayList<>(); + List s3G = new ArrayList<>(); + List s1T = new ArrayList<>(); + List s2T = new ArrayList<>(); + List s3T = new ArrayList<>(); + List s1 = new ArrayList<>(); + List s2 = new ArrayList<>(); + List s3 = new ArrayList<>(); public static int mode = 0; // 0 for teleop, 1 for auto public static double manualPow = 1.0; double stamp = 0; int ticker = 0; - boolean b1 = false; - boolean b2 = false; - boolean b3 = false; boolean steadySpin = false; double powPID = 0.0; + boolean intake = true; double spindexerPos = spindexer_intakePos1; + boolean wasMoving = false; @Override public void runOpMode() throws InterruptedException { + List allHubs = hardwareMap.getAll(LynxModule.class); + for (LynxModule hub : allHubs) { + hub.setBulkCachingMode(LynxModule.BulkCachingMode.MANUAL); + } robot = new Robot(hardwareMap); servo = new Servos(hardwareMap); TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); @@ -63,7 +81,7 @@ public class IntakeTest extends LinearOpMode { //TODO: test this monstrosity } else if (mode == 1) { - if (gamepad1.cross){ + if (gamepad1.cross && intake){ robot.intake.setPower(1); } else if (gamepad1.circle){ robot.intake.setPower(-1); @@ -74,8 +92,8 @@ public class IntakeTest extends LinearOpMode { colorDetect(); spindexer(); - if (b1 && steadySpin && getRuntime() - stamp > 1.0){ - if (!b2){ + if (ballIn(1) && steadySpin && intake && getRuntime() - stamp > 0.5){ + if (!ballIn(2)){ if (servo.spinEqual(spindexer_intakePos1)){ spindexerPos = spindexer_intakePos2; } else if (servo.spinEqual(spindexer_intakePos2)){ @@ -83,7 +101,7 @@ public class IntakeTest extends LinearOpMode { } else if (servo.spinEqual(spindexer_intakePos3)){ spindexerPos = spindexer_intakePos1; } - } else if (!b3){ + } else if (!ballIn(3)){ if (servo.spinEqual(spindexer_intakePos1)){ spindexerPos = spindexer_intakePos3; } else if (servo.spinEqual(spindexer_intakePos2)){ @@ -100,64 +118,77 @@ public class IntakeTest extends LinearOpMode { stamp = getRuntime(); ticker = 0; spindexer(); + intake = true; + } + for (LynxModule hub : allHubs) { + hub.clearBulkCache(); } TELE.addData("Manual Power", manualPow); TELE.addData("PID Power", powPID); - TELE.addData("B1", b1); - TELE.addData("B2", b2); - TELE.addData("B3", b3); + TELE.addData("B1", ballIn(1)); + TELE.addData("B2", ballIn(2)); + TELE.addData("B3", ballIn(3)); TELE.addData("Spindex Pos", servo.getSpinPos()); + TELE.update(); } } public void colorDetect() { - // ----- COLOR 1 ----- - double green1 = robot.color1.getNormalizedColors().green; - double blue1 = robot.color1.getNormalizedColors().blue; - double red1 = robot.color1.getNormalizedColors().red; - - b1 = robot.color1.getDistance(DistanceUnit.MM) < 40; - - TELE.addData("Color1 toColor", robot.color1.getNormalizedColors().toColor()); - TELE.addData("Color1 green", green1 / (green1 + blue1 + red1)); - TELE.addData("Color1 distance (mm)", robot.color1.getDistance(DistanceUnit.MM)); - -// ----- COLOR 2 ----- - double green2 = robot.color2.getNormalizedColors().green; - double blue2 = robot.color2.getNormalizedColors().blue; - double red2 = robot.color2.getNormalizedColors().red; - - b2 = robot.color2.getDistance(DistanceUnit.MM) < 40; - - TELE.addData("Color2 toColor", robot.color2.getNormalizedColors().toColor()); - TELE.addData("Color2 green", green2 / (green2 + blue2 + red2)); - TELE.addData("Color2 distance (mm)", robot.color2.getDistance(DistanceUnit.MM)); - -// ----- COLOR 3 ----- - double green3 = robot.color3.getNormalizedColors().green; - double blue3 = robot.color3.getNormalizedColors().blue; - double red3 = robot.color3.getNormalizedColors().red; - - b3 = robot.color3.getDistance(DistanceUnit.MM) < 30; - - TELE.addData("Color3 toColor", robot.color3.getNormalizedColors().toColor()); - TELE.addData("Color3 green", green3 / (green3 + blue3 + red3)); - TELE.addData("Color3 distance (mm)", robot.color3.getDistance(DistanceUnit.MM)); + double s1D = robot.color1.getDistance(DistanceUnit.MM); + double s2D = robot.color2.getDistance(DistanceUnit.MM); + double s3D = robot.color3.getDistance(DistanceUnit.MM); + TELE.addData("Color 1 Distance", s1D); + TELE.addData("Color 2 Distance", s2D); + TELE.addData("Color 3 Distance", s3D); TELE.update(); + + if (s1D < 40) { + s1T.add(getRuntime()); + } + + if (s2D < 40) { + s2T.add(getRuntime()); + } + + if (s3D < 30) { + s3T.add(getRuntime()); + } } - public void spindexer(){ - if (!servo.spinEqual(spindexerPos)){ + public void spindexer() { + boolean atTarget = servo.spinEqual(spindexerPos); + + if (!atTarget) { powPID = servo.setSpinPos(spindexerPos); robot.spin1.setPower(powPID); robot.spin2.setPower(-powPID); + steadySpin = false; + wasMoving = true; // remember we were moving stamp = getRuntime(); - } else{ + } else { robot.spin1.setPower(0); robot.spin2.setPower(0); steadySpin = true; + wasMoving = false; } } + + + boolean ballIn(int slot) { + List times; + + if (slot == 1) {times = s1T;} + else if (slot == 2) {times = s2T;} + else if (slot == 3) {times = s3T;} + else return false; + + if (!times.isEmpty()){ + return times.get(times.size() - 1) > getRuntime() - 2; + } else { + return false; + } + } + } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/PositionalServoProgrammer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/PositionalServoProgrammer.java index b951b5f..4f65624 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/PositionalServoProgrammer.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/PositionalServoProgrammer.java @@ -52,8 +52,8 @@ public class PositionalServoProgrammer extends LinearOpMode { robot.turr1.setPower(turrHoldPow); robot.turr2.setPower(turrHoldPow); } else { - robot.spin1.setPower(turretPow); - robot.spin2.setPower(-turretPow); + robot.turr1.setPower(turretPow); + robot.turr2.setPower(-turretPow); } if (transferPos != 0.501){ robot.transferServo.setPosition(transferPos);