From bfa8b52ebc98977bc6fa2ec2f40b13cc943377a3 Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Tue, 25 Nov 2025 21:39:17 -0600 Subject: [PATCH] color sensor test --- .../teamcode/tests/ActiveColorSensorTest.java | 129 ++++++++++++++++++ .../ftc/teamcode/tests/ShooterTest.java | 57 +++----- .../utils/ConfigureColorRangefinder.java | 22 ++- 3 files changed, 162 insertions(+), 46 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ActiveColorSensorTest.java 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 new file mode 100644 index 0000000..226701c --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ActiveColorSensorTest.java @@ -0,0 +1,129 @@ +package org.firstinspires.ftc.teamcode.tests; + +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.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import org.firstinspires.ftc.teamcode.utils.Robot; + + +@TeleOp +@Config +public class ActiveColorSensorTest extends LinearOpMode { + Robot robot; + MultipleTelemetry TELE; + + @Override + public void runOpMode() throws InterruptedException{ + robot = new Robot(hardwareMap); + TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); + int b1Purple = 1; + int b1Total = 1; + int b2Purple = 1; + int b2Total = 1; + int b3Purple = 1; + int b3Total = 1; + + double totalStamp1 = 0.0; + double purpleStamp1 = 0.0; + double totalStamp2 = 0.0; + double purpleStamp2 = 0.0; + double totalStamp3 = 0.0; + double purpleStamp3 = 0.0; + + String b1 = "none"; + String b2 = "none"; + String b3 = "none"; + + double position = 0.0; + + double stamp = getRuntime(); + + waitForStart(); + if (isStopRequested()) return; + + while (opModeIsActive()){ + + if ((getRuntime() % 0.3) >0.15) { + position = spindexer_intakePos + 0.02; + } else { + position = spindexer_intakePos - 0.02; + } + robot.spin1.setPosition(position); + robot.spin2.setPosition(1-position); + + robot.intake.setPower(1); + + if (robot.pin1.getState()){ + if (robot.pin0.getState()){ + b1Purple += 1; + purpleStamp1 = getRuntime(); + } + b1Total += 1; + totalStamp1 = getRuntime(); + } + if (getRuntime() - totalStamp1 > 1){ + b1 = "none"; + b1Total = 1; + b1Purple = 1; + }else if (b1Total > 10 && getRuntime() - purpleStamp1 > 2){ + b1 = "Green"; + }else if (b1Purple > 10){ + b1 = "Purple"; + } + + if (robot.pin3.getState()){ + if (robot.pin2.getState()){ + b2Purple += 1; + purpleStamp2 = getRuntime(); + } + b2Total += 1; + totalStamp2 = getRuntime(); + } + if (getRuntime() - totalStamp2 > 1){ + b2 = "none"; + b2Total = 1; + b2Purple = 1; + }else if (b2Total > 10 && getRuntime() - purpleStamp2 > 2){ + b2 = "Green"; + }else if (b2Purple > 10){ + b2 = "Purple"; + } + + if (robot.pin5.getState()){ + if (robot.pin4.getState()){ + b3Purple += 1; + purpleStamp3 = getRuntime(); + } + b3Total += 1; + totalStamp3 = getRuntime(); + } + if (getRuntime() - totalStamp3 > 1){ + b3 = "none"; + b3Total = 1; + b3Purple = 1; + }else if (b3Total > 10 && getRuntime() - purpleStamp3 > 2){ + b3 = "Green"; + }else if (b3Purple > 10){ + b3 = "Purple"; + } + + 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("1", b1); + TELE.addData("2",b2); + TELE.addData("3",b3); + + TELE.update(); + } + } + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java index 5040729..e35a6d3 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ShooterTest.java @@ -19,10 +19,10 @@ public class ShooterTest extends LinearOpMode { public static double vel = 0.0; public static double mcpr = 28.0; // CPR of the motor public static double ecpr = 1024.0; // CPR of the encoder - public static double pos = 0.0; + public static double hoodPos = 0.5; public static double posPower = 0.0; - public static double posi = 0.5; + public static double turretPos = 0.9; public static double p = 0.0003, i = 0, d = 0, f = 0; @@ -30,34 +30,22 @@ public class ShooterTest extends LinearOpMode { public static String turrMode = "MANUAL"; - public static int posTolerance = 40; + double initPos = 0.0; - public static double servoPosition = 0.501; + double velo1 = 0.0; - public double stamp = 0.0; + double stamp1 = 0.0; - public double initPos = 0.0; - - public static double time = 0.1; - - public double velo = 0.0; - - public double velo1 = 0.0; - - public double stamp1 = 0.0; - - public double initPos1 = 0.0; + double initPos1 = 0.0; double powPID = 0.0; + public static double maxVel = 4500; + public static int tolerance = 300; MultipleTelemetry TELE; - public boolean wait(double time) { - return (getRuntime() - stamp) > time; - } - @Override public void runOpMode() throws InterruptedException { @@ -85,31 +73,23 @@ public class ShooterTest extends LinearOpMode { shooter.setControllerCoefficients(p, i, d, f); - shooter.setTurretMode(turrMode); - shooter.setShooterMode(flyMode); shooter.setManualPower(pow); - shooter.sethoodPosition(pos); - - shooter.setTurretPosition(posi); - - shooter.setTolerance(posTolerance); - - shooter.setPosPower(posPower); - - if (servoPosition != 0.501) { shooter.sethoodPosition(servoPosition); } + if (hoodPos != 0.501) { + robot.hood.setPosition(hoodPos); + } + if (turretPos != 0.501) { + robot.turr1.setPosition(turretPos); + robot.turr2.setPosition(turretPos); + } velo1 = -60 * ((shooter.getECPRPosition() - initPos1) / (getRuntime() - stamp1)); stamp1 = getRuntime(); initPos1 = shooter.getECPRPosition(); - if (Math.abs(vel - velo1) > 1500){ - if (vel - velo1 > 0){ - powPID = 0.75; - } else if (vel - velo1 < 0){ - powPID = 0.25; - } + if (Math.abs(vel - velo1) > 3 * tolerance) { + powPID = vel / maxVel; } else if (vel - tolerance > velo1) { powPID = powPID + 0.001; } else if (vel + tolerance < velo1) { @@ -121,13 +101,12 @@ public class ShooterTest extends LinearOpMode { TELE.addData("ECPR Revolutions", shooter.getECPRPosition()); TELE.addData("MCPR Revolutions", shooter.getMCPRPosition()); - TELE.addData("Velocity", velo); TELE.addData("hoodPos", shooter.gethoodPosition()); TELE.addData("turretPos", shooter.getTurretPosition()); TELE.addData("Power Fly 1", robot.shooter1.getPower()); TELE.addData("Power Fly 2", robot.shooter2.getPower()); TELE.addData("powPID", shooter.getpowPID()); - TELE.addData("Ins Velocity", velo1); + TELE.addData("Velocity", velo1); 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 8f863d4..53d7060 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 @@ -1,13 +1,20 @@ package org.firstinspires.ftc.teamcode.utils; +import com.acmerobotics.dashboard.config.Config; import com.qualcomm.hardware.rev.RevColorSensorV3; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.I2cDeviceSynchSimple; - - +@Config @TeleOp public class ConfigureColorRangefinder extends LinearOpMode { + + public static int LED_Brightness = 35; //1 - 35, 2 - 50, + + public static int lowerGreen = 90; + + public static int higherGreen = 160; + @Override public void runOpMode() throws InterruptedException { ColorRangefinder crf = new ColorRangefinder(hardwareMap.get(RevColorSensorV3.class, "color")); @@ -15,12 +22,12 @@ 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.setPin1Digital(ColorRangefinder.DigitalMode.DISTANCE, 0, 20); // green - crf.setPin0Digital(ColorRangefinder.DigitalMode.HSV, 150 / 360.0 * 255, 360 / 360.0 * 255); // purple - crf.setPin0Digital(ColorRangefinder.DigitalMode.HSV, 0, 100 / 360.0 *255); - crf.setPin0DigitalMaxDistance(ColorRangefinder.DigitalMode.HSV, 20); // 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(10); + crf.setLedBrightness(LED_Brightness); } } @@ -135,6 +142,7 @@ class ColorRangefinder { /** * Read distance via I2C + * * @return distance in millimeters */ public double readDistance() {