From f8369b51ed35d0ce0b8339bbf8526e2cd3dea44b Mon Sep 17 00:00:00 2001 From: DanTheMan-byte Date: Thu, 29 Jan 2026 15:20:02 -0600 Subject: [PATCH] working teleop in progress --- .../ftc/teamcode/constants/Color.java | 5 ++++ .../ftc/teamcode/teleop/TeleopV3.java | 10 ++++++++ .../ftc/teamcode/tests/ColorTest.java | 17 ++++++++++--- .../ftc/teamcode/utils/Spindexer.java | 24 +++++++++++-------- .../ftc/teamcode/utils/Turret.java | 6 ----- 5 files changed, 43 insertions(+), 19 deletions(-) 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 index 6d06db6..02ac4df 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Color.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/Color.java @@ -1,6 +1,11 @@ package org.firstinspires.ftc.teamcode.constants; +import com.acmerobotics.dashboard.config.Config; + +@Config public class Color { public static boolean redAlliance = true; public static double Light0 = 0.28, Light1 = 0.67, Light2 = 0.36, Light3 = 0.5; + + public static double colorFilterAlpha = 0.15; } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java index ee70e46..84210e9 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleop/TeleopV3.java @@ -1,5 +1,6 @@ package org.firstinspires.ftc.teamcode.teleop; +import static org.firstinspires.ftc.teamcode.constants.Color.redAlliance; import static org.firstinspires.ftc.teamcode.constants.Poses.teleStart; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spinStartPos; import static org.firstinspires.ftc.teamcode.constants.ServoPositions.spindexer_intakePos1; @@ -178,6 +179,15 @@ public class TeleopV3 extends LinearOpMode { Turret turret = new Turret(robot, TELE, robot.limelight); robot.light.setPosition(1); + while (opModeInInit()){ + robot.limelight.start(); + if (redAlliance) { + robot.limelight.pipelineSwitch(4); + } else { + robot.limelight.pipelineSwitch(2); + } + } + waitForStart(); if (isStopRequested()) return; 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 f0ce6b6..fd86f1d 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 @@ -1,5 +1,7 @@ package org.firstinspires.ftc.teamcode.tests; +import static org.firstinspires.ftc.teamcode.constants.Color.colorFilterAlpha; + import com.acmerobotics.dashboard.FtcDashboard; import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; @@ -18,6 +20,9 @@ public class ColorTest extends LinearOpMode { public void runOpMode() throws InterruptedException { robot = new Robot(hardwareMap); TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); + double color1Distance = 0; + double color2Distance = 0; + double color3Distance = 0; waitForStart(); if (isStopRequested()) return; @@ -26,28 +31,34 @@ public class ColorTest extends LinearOpMode { double green1 = robot.color1.getNormalizedColors().green; double blue1 = robot.color1.getNormalizedColors().blue; double red1 = robot.color1.getNormalizedColors().red; + 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 distance (mm)", robot.color1.getDistance(DistanceUnit.MM)); + TELE.addData("Color1 distance (mm)", color1Distance); // ----- COLOR 2 ----- double green2 = robot.color2.getNormalizedColors().green; double blue2 = robot.color2.getNormalizedColors().blue; double red2 = robot.color2.getNormalizedColors().red; + double dist2 = robot.color2.getDistance(DistanceUnit.MM); + color2Distance = (colorFilterAlpha * dist2) + ((1-colorFilterAlpha) * color2Distance); 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)); + TELE.addData("Color2 distance (mm)", color2Distance); // ----- COLOR 3 ----- double green3 = robot.color3.getNormalizedColors().green; double blue3 = robot.color3.getNormalizedColors().blue; double red3 = robot.color3.getNormalizedColors().red; + double dist3 = robot.color3.getDistance(DistanceUnit.MM); + color3Distance = (colorFilterAlpha * dist3) + ((1-colorFilterAlpha) * color3Distance); 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.addData("Color3 distance (mm)", color3Distance); TELE.update(); } 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 3552dd9..670d7cb 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 @@ -161,12 +161,15 @@ public class Spindexer { int spindexerBallPos = 0; // Read Distances - distanceRearCenter = robot.color1.getDistance(DistanceUnit.MM); - distanceFrontDriver = robot.color2.getDistance(DistanceUnit.MM); - distanceFrontPassenger = robot.color3.getDistance(DistanceUnit.MM); + double dRearCenter = robot.color1.getDistance(DistanceUnit.MM); + distanceRearCenter = (colorFilterAlpha * dRearCenter) + ((1-colorFilterAlpha) * distanceRearCenter); + double dFrontDriver = robot.color2.getDistance(DistanceUnit.MM); + distanceFrontDriver = (colorFilterAlpha * dFrontDriver) + ((1-colorFilterAlpha) * distanceFrontDriver); + double dFrontPassenger = robot.color3.getDistance(DistanceUnit.MM); + distanceFrontPassenger = (colorFilterAlpha * dFrontPassenger) + ((1-colorFilterAlpha) * distanceFrontPassenger); // Position 1 - if (distanceRearCenter < 50) { + if (distanceRearCenter < 60) { // Mark Ball Found newPos1Detection = true; @@ -218,7 +221,7 @@ public class Spindexer { // Position 3 spindexerBallPos = RotatedBallPositions[commandedIntakePosition][RotatedBallPositionNames.FRONTPASSENGER.ordinal()]; - if (distanceFrontPassenger < 25) { + if (distanceFrontPassenger < 29) { // reset FoundEmpty because looking for 3 in a row before reset ballPositions[spindexerBallPos].foundEmpty = 0; @@ -260,8 +263,8 @@ public class Spindexer { private void moveSpindexerToPos(double pos) { robot.spin1.setPosition(pos); robot.spin2.setPosition(1-pos); -// double currentPos = servos.getSpinPos(); -// if (!servos.spinEqual(pos) && Math.abs(prevPos - currentPos) <= 0){ + double currentPos = servos.getSpinPos(); + if (!servos.spinEqual(pos) && Math.abs(prevPos - currentPos) <= 0){ // if (currentPos > pos){ // robot.spin1.setPosition(servos.getSpinPos() + 0.05); // robot.spin2.setPosition(1 - servos.getSpinPos() - 0.05); @@ -269,8 +272,8 @@ public class Spindexer { // robot.spin1.setPosition(servos.getSpinPos() - 0.05); // robot.spin2.setPosition(1 - servos.getSpinPos() + 0.05); // } -// } -// prevPos = currentPos; + } + prevPos = currentPos; } public void stopSpindexer() { @@ -341,7 +344,8 @@ public class Spindexer { currentIntakeState = Spindexer.IntakeState.FINDNEXT; } else { // Maintain Position - moveSpindexerToPos(intakePositions[commandedIntakePosition]); + spindexerWiggle *= -1.0; + moveSpindexerToPos(intakePositions[commandedIntakePosition]+spindexerWiggle); } break; case FINDNEXT: 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 d2def91..8640b30 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 @@ -61,12 +61,6 @@ public class Turret { this.TELE = tele; this.robot = rob; this.webcam = cam; - webcam.start(); - if (redAlliance) { - webcam.pipelineSwitch(4); - } else { - webcam.pipelineSwitch(2); - } bearingPID = new PIDController(B_PID_P, B_PID_I, B_PID_D); }