From f9a220bf51c7a8ba6ae4a436d6396cf5604fa46f Mon Sep 17 00:00:00 2001 From: KeshavAnandCode Date: Sun, 30 Nov 2025 16:59:23 -0600 Subject: [PATCH] daniel files added --- .../ftc/teamcode/constants/blank.java | 4 - .../teamcode/tests/ActiveColorSensorTest.java | 144 ++++++++++++ .../ftc/teamcode/tests/ColorSensorTest.java | 63 ++++++ .../firstinspires/ftc/teamcode/tests/ed.java | 210 ++++++++++++++++++ 4 files changed, 417 insertions(+), 4 deletions(-) delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/blank.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ActiveColorSensorTest.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ColorSensorTest.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ed.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/blank.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/blank.java deleted file mode 100644 index 53e7fcb..0000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/constants/blank.java +++ /dev/null @@ -1,4 +0,0 @@ -package org.firstinspires.ftc.teamcode.constants; - -public class blank { -} 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..790f8f2 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ActiveColorSensorTest.java @@ -0,0 +1,144 @@ +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_intakePos1 + 0.015; + } else { + position = spindexer_intakePos1 - 0.015; + } + robot.spin1.setPosition(position); + robot.spin2.setPosition(1-position); + + 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 ++; + } + b1Total++; + totalStamp1 = getRuntime(); + } + if (getRuntime() - totalStamp1 > ColorCounterResetDelay) { + // Too Much time has passed without detecting ball + b1 = "none"; + b1Total = 1; + b1Purple = 1; + }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 ++; + } + b2Total++; + totalStamp2 = getRuntime(); + } + if (getRuntime() - totalStamp2 > ColorCounterResetDelay) { + // Too Much time has passed without detecting ball + b2 = "none"; + b2Total = 1; + b2Purple = 1; + }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 ++; + } + b3Total++; + totalStamp3 = getRuntime(); + } + if (getRuntime() - totalStamp3 > ColorCounterResetDelay) { + // Too Much time has passed without detecting ball + b3 = "none"; + b3Total = 1; + b3Purple = 1; + }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()); + 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(); + } + } + +} \ No newline at end of file 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 new file mode 100644 index 0000000..61e2427 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ColorSensorTest.java @@ -0,0 +1,63 @@ +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; + +@TeleOp +@Config +public class ColorSensorTest 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()) { + +// ----- 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/tests/ed.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ed.java new file mode 100644 index 0000000..91ec518 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tests/ed.java @@ -0,0 +1,210 @@ +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 com.qualcomm.robotcore.hardware.DcMotor; + +import org.firstinspires.ftc.teamcode.utils.Robot; +import org.firstinspires.ftc.teamcode.subsystems.Shooter; + +@TeleOp +@Config +public class ShooterTest extends LinearOpMode { + + Robot robot; + + public static double pow = 0.0; + public static double vel = 0.0; + public static double ecpr = 1024.0; // CPR of the encoder + public static double hoodPos = 0.5; + public static double turretPos = 0.9; + + public static String flyMode = "VEL"; + + public static boolean AutoTrack = false; + + double initPos = 0.0; + + double velo = 0.0; + double velo1 = 0.0; + double velo2 = 0.0; + double velo3 = 0.0; + double velo4 = 0.0; + double velo5 = 0.0; + + double stamp1 = 0.0; + + double initPos1 = 0.0; + + double powPID = 0.0; + + public static int maxVel = 4500; + + public static boolean shoot = false; + + public static int spindexPos = 1; + + public static boolean intake = true; + + public static int tolerance = 50; + + double stamp = 0.0; + + public static double kP = 0.001; // small proportional gain (tune this) + public static double maxStep = 0.06; // prevents sudden jumps + public static double distance = 50; + + MultipleTelemetry TELE; + + @Override + public void runOpMode() throws InterruptedException { + + robot = new Robot(hardwareMap); + + TELE = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); + + Shooter shooter = new Shooter(robot, TELE); + + robot.shooter1.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + robot.shooter2.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER); + + shooter.setTelemetryOn(true); + + shooter.setShooterMode(flyMode); + + initPos = shooter.getECPRPosition(); + + int ticker = 0; + + waitForStart(); + + if (isStopRequested()) return; + + while (opModeIsActive()) { + + ticker++; + + if (AutoTrack){ + hoodPos = hoodAnglePrediction(distance); + vel = velPrediction(distance); + } + + + + shooter.setShooterMode(flyMode); + + shooter.setManualPower(pow); + + robot.hood.setPosition(hoodPos); + robot.turr1.setPosition(turretPos); + robot.turr2.setPosition(1 - turretPos); + if (intake) { + robot.transfer.setPower(0); + robot.intake.setPower(0.75); + robot.spin1.setPosition(spindexer_intakePos1); + robot.spin2.setPosition(1 - spindexer_intakePos1); + } else { + robot.transfer.setPower(.75 + (powPID/4)); + robot.intake.setPower(0); + if (spindexPos == 1) { + robot.spin1.setPosition(spindexer_outtakeBall1); + robot.spin2.setPosition(1 - spindexer_outtakeBall1); + } else if (spindexPos == 2) { + robot.spin1.setPosition(spindexer_outtakeBall2); + robot.spin2.setPosition(1 - spindexer_outtakeBall2); + } else if (spindexPos == 3) { + robot.spin1.setPosition(spindexer_outtakeBall3); + robot.spin2.setPosition(1 - spindexer_outtakeBall3); + } + } + + double penguin = 0; + if (ticker % 8 ==0){ + penguin = shooter.getECPRPosition(); + stamp = getRuntime(); + velo1 = -60 * ((penguin - initPos1) / (stamp - stamp1)); + initPos1 = penguin; + stamp1 = stamp; + } + + + velo = velo1; + + double feed = vel / maxVel; // Example: vel=2500 → feed=0.5 + + if (vel > 500){ + feed = Math.log((668.39 / (vel + 591.96)) - 0.116) / -4.18; + } + + // --- PROPORTIONAL CORRECTION --- + double error = vel - velo1; + double correction = kP * error; + + // limit how fast power changes (prevents oscillation) + correction = Math.max(-maxStep, Math.min(maxStep, correction)); + + // --- FINAL MOTOR POWER --- + powPID = feed + correction; + + // clamp to allowed range + powPID = Math.max(0, Math.min(1, powPID)); + + if (vel - velo > 1000){ + powPID = 1; + } else if (velo - vel > 1000){ + powPID = 0; + } + + shooter.setVelocity(powPID); + + if (shoot) { + robot.transferServo.setPosition(transferServo_in); + } else { + robot.transferServo.setPosition(transferServo_out); + } + + shooter.update(); + + TELE.addData("Revolutions", shooter.getECPRPosition()); + 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("Velocity", velo); + TELE.update(); + + + } + + } + + public double hoodAnglePrediction(double distance) { + double L = 0.298317; + double A = 1.02124; + double k = 0.0157892; + double n = 3.39375; + + double dist = Math.sqrt(distance*distance+24*24); + + return L + A * Math.exp(-Math.pow(k * dist, n)); + } + public static double velPrediction(double distance) { + + double x = Math.sqrt(distance*distance+24*24); + + + + double A = -211149.992; + double B = -1.19943; + double C = 3720.15909; + + return A * Math.pow(x, B) + C; + } + +}