From d32033a0155d95a9db0e98707dbddc40870d805d Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Sun, 30 Nov 2025 16:42:45 -0600 Subject: [PATCH] here you go keshav --- .../ftc/teamcode/constants/Color.java | 4 ++ .../teamcode/tests/ActiveColorSensorTest.java | 57 ++++++++++++------- .../ftc/teamcode/tests/ColorSensorTest.java | 46 +++++++++++---- .../utils/ConfigureColorRangefinder.java | 10 ++-- .../ftc/teamcode/utils/Robot.java | 13 +++++ 5 files changed, 93 insertions(+), 37 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Color.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Color.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Color.java new file mode 100644 index 0000000..7f6f524 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Color.java @@ -0,0 +1,4 @@ +package org.firstinspires.ftc.teamcode.constants; + +public class Color { +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ActiveColorSensorTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ActiveColorSensorTest.java index 9615325..790f8f2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ActiveColorSensorTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ActiveColorSensorTest.java @@ -58,58 +58,73 @@ public class ActiveColorSensorTest extends LinearOpMode { robot.intake.setPower(1); + // Reset the counters after 1 second of not reading a ball. + final double ColorCounterResetDelay = 1.0; + // Number of times the loop needs to run before deciding on a color. + final int ColorCounterTotalMinCount = 20; + // If the color sensor reads a color this percentage of time + // out of the total, declare the color. + // Usage: (Color Count)/(Total Count) > ColorCounterThreshold + final double ColorCounterThreshold = 0.65; + if (robot.pin1.getState()){ if (robot.pin0.getState()){ - b1Purple += 1; - purpleStamp1 = getRuntime(); + b1Purple ++; } - b1Total += 1; + b1Total++; totalStamp1 = getRuntime(); } - if (getRuntime() - totalStamp1 > 1){ + if (getRuntime() - totalStamp1 > ColorCounterResetDelay) { + // Too Much time has passed without detecting ball b1 = "none"; b1Total = 1; b1Purple = 1; - }else if (b1Total > 10 && getRuntime() - purpleStamp1 > 2){ - b1 = "Green"; - }else if (b1Purple > 10){ + }else if ((b1Total > ColorCounterTotalMinCount) && ((double) b1Purple / b1Total) >= ColorCounterThreshold){ + // Enough Time has passed and we met the threshold b1 = "Purple"; + }else if (b1Total > ColorCounterTotalMinCount) { + // Enough Time passed WITHOUT meeting the threshold + b1 = "Green"; } if (robot.pin3.getState()){ if (robot.pin2.getState()){ - b2Purple += 1; - purpleStamp2 = getRuntime(); + b2Purple ++; } - b2Total += 1; + b2Total++; totalStamp2 = getRuntime(); } - if (getRuntime() - totalStamp2 > 1){ + if (getRuntime() - totalStamp2 > ColorCounterResetDelay) { + // Too Much time has passed without detecting ball b2 = "none"; b2Total = 1; b2Purple = 1; - }else if (b2Total > 10 && getRuntime() - purpleStamp2 > 2){ - b2 = "Green"; - }else if (b2Purple > 10){ + }else if ((b2Total > ColorCounterTotalMinCount) && ((double) b2Purple / b2Total) >= ColorCounterThreshold){ + // Enough Time has passed and we met the threshold b2 = "Purple"; + }else if (b2Total > ColorCounterTotalMinCount) { + // Enough Time passed WITHOUT meeting the threshold + b2 = "Green"; } if (robot.pin5.getState()){ if (robot.pin4.getState()){ - b3Purple += 1; - purpleStamp3 = getRuntime(); + b3Purple ++; } - b3Total += 1; + b3Total++; totalStamp3 = getRuntime(); } - if (getRuntime() - totalStamp3 > 1){ + if (getRuntime() - totalStamp3 > ColorCounterResetDelay) { + // Too Much time has passed without detecting ball b3 = "none"; b3Total = 1; b3Purple = 1; - }else if (b3Total > 10 && getRuntime() - purpleStamp3 > 2){ - b3 = "Green"; - }else if (b3Purple > 10){ + }else if ((b3Total > ColorCounterTotalMinCount) && ((double) b3Purple / b3Total) >= ColorCounterThreshold){ + // Enough Time has passed and we met the threshold b3 = "Purple"; + }else if (b3Total > ColorCounterTotalMinCount) { + // Enough Time passed WITHOUT meeting the threshold + b3 = "Green"; } TELE.addData("Green1:", robot.pin1.getState()); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ColorSensorTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ColorSensorTest.java index 8c1cd26..61e2427 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ColorSensorTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ColorSensorTest.java @@ -6,34 +6,58 @@ 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; @TeleOp @Config -public class ColorSensorTest extends LinearOpMode{ +public class ColorSensorTest extends LinearOpMode { Robot robot; MultipleTelemetry TELE; @Override - public void runOpMode() throws InterruptedException{ + public void runOpMode() throws InterruptedException { robot = new Robot(hardwareMap); TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); waitForStart(); if (isStopRequested()) return; - while (opModeIsActive()){ - TELE.addData("Green1:", robot.pin1.getState()); - TELE.addData("Purple1:", robot.pin0.getState()); - TELE.addData("Green2:", robot.pin3.getState()); - TELE.addData("Purple2:", robot.pin2.getState()); - TELE.addData("Green3:", robot.pin5.getState()); - TELE.addData("Purple3:", robot.pin4.getState()); - TELE.addData("Hello Keshav (analog)", robot.analogInput.getVoltage()); - TELE.addData("Hello again (analog2)", robot.analogInput2.getVoltage()); + while (opModeIsActive()) { + +// ----- COLOR 1 ----- + 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/utils/ConfigureColorRangefinder.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/ConfigureColorRangefinder.java index a556440..24afbf3 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/ConfigureColorRangefinder.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/ConfigureColorRangefinder.java @@ -11,7 +11,7 @@ public class ConfigureColorRangefinder extends LinearOpMode { public static int LED_Brightness = 50; - public static int lowerGreen = 120; + public static int lowerGreen = 110; public static int higherGreen = 150; @@ -22,10 +22,10 @@ public class ConfigureColorRangefinder extends LinearOpMode { /* Using this example configuration, you can detect both artifact colors based on which pin is reading true: pin0 --> purple pin1 --> green */ - crf.setPin0Analog(ColorRangefinder.AnalogMode.GREEN); // green -// crf.setPin1Digital(ColorRangefinder.DigitalMode.HSV, higherGreen / 360.0 * 255, 360 / 360.0 * 255); // purple -// crf.setPin1Digital(ColorRangefinder.DigitalMode.HSV, 0, lowerGreen/360.0 * 255); -// crf.setPin1DigitalMaxDistance(ColorRangefinder.DigitalMode.HSV, 40); // 10mm or closer requirement + crf.setPin1Digital(ColorRangefinder.DigitalMode.DISTANCE, 0, 40); // green + crf.setPin0Digital(ColorRangefinder.DigitalMode.HSV, higherGreen / 360.0 * 255, 360 / 360.0 * 255); // purple + crf.setPin0Digital(ColorRangefinder.DigitalMode.HSV, 0, lowerGreen/360.0 * 255); + crf.setPin0DigitalMaxDistance(ColorRangefinder.DigitalMode.HSV, 40); // 10mm or closer requirement crf.setLedBrightness(LED_Brightness); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java index 910e671..d07e416 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/utils/Robot.java @@ -1,5 +1,6 @@ package org.firstinspires.ftc.teamcode.utils; +import com.qualcomm.hardware.rev.RevColorSensorV3; import com.qualcomm.robotcore.hardware.AnalogInput; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorEx; @@ -7,6 +8,7 @@ import com.qualcomm.robotcore.hardware.DcMotorImplEx; import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.DigitalChannel; import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.I2cDevice; import com.qualcomm.robotcore.hardware.Servo; import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; @@ -77,6 +79,12 @@ public class Robot { public DcMotorEx shooterEncoder; + public RevColorSensorV3 color1; + + public RevColorSensorV3 color2; + + public RevColorSensorV3 color3; + public Robot(HardwareMap hardwareMap) { //Define components w/ hardware map @@ -150,5 +158,10 @@ public class Robot { webcam = hardwareMap.get(WebcamName.class, "Webcam 1"); + color1 = hardwareMap.get(RevColorSensorV3.class, "c1"); + + color2 = hardwareMap.get(RevColorSensorV3.class, "c2"); + + color3 = hardwareMap.get(RevColorSensorV3.class, "c3"); } }