From c0b06f91fc44ed88156bdf8041e801d23483e4e6 Mon Sep 17 00:00:00 2001 From: anya-codes Date: Sun, 21 Sep 2025 14:48:28 -0700 Subject: [PATCH 001/198] Added parts of the GoBilda starter bot code that control the launching and feeding to CompetitionTeleOp. Have not tested yet. --- .../ftc/team417/CompetitionTeleOp.java | 161 +++++++++++++++++- 1 file changed, 160 insertions(+), 1 deletion(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java index d6e5b3f36f26..7c832f4c3ef1 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java @@ -5,6 +5,12 @@ import com.acmerobotics.roadrunner.PoseVelocity2d; import com.acmerobotics.roadrunner.Vector2d; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.CRServo; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.PIDFCoefficients; +import com.qualcomm.robotcore.util.ElapsedTime; import org.firstinspires.ftc.team417.roadrunner.Drawing; import org.firstinspires.ftc.team417.roadrunner.MecanumDrive; @@ -15,18 +21,123 @@ */ @TeleOp(name="TeleOp", group="Competition") public class CompetitionTeleOp extends BaseOpMode { + final double FEED_TIME_SECONDS = 0.20; //The feeder servos run this long when a shot is requested. + final double STOP_SPEED = 0.0; //We send this power to the servos when we want them to stop. + final double FULL_SPEED = 1.0; + + /* + * When we control our launcher motor, we are using encoders. These allow the control system + * to read the current speed of the motor and apply more or less power to keep it at a constant + * velocity. Here we are setting the target, and minimum velocity that the launcher should run + * at. The minimum velocity is a threshold for determining when to fire. + */ + final double LAUNCHER_TARGET_VELOCITY = 1125; + final double LAUNCHER_MIN_VELOCITY = 1075; + + // Declare OpMode members. + private DcMotorEx launcher = null; + private CRServo leftFeeder = null; + private CRServo rightFeeder = null; + + ElapsedTime feederTimer = new ElapsedTime(); + + /* + * TECH TIP: State Machines + * We use a "state machine" to control our launcher motor and feeder servos in this program. + * The first step of a state machine is creating an enum that captures the different "states" + * that our code can be in. + * The core advantage of a state machine is that it allows us to continue to loop through all + * of our code while only running specific code when it's necessary. We can continuously check + * what "State" our machine is in, run the associated code, and when we are done with that step + * move on to the next state. + * This enum is called the "LaunchState". It reflects the current condition of the shooter + * motor and we move through the enum when the user asks our code to fire a shot. + * It starts at idle, when the user requests a launch, we enter SPIN_UP where we get the + * motor up to speed, once it meets a minimum speed then it starts and then ends the launch process. + * We can use higher level code to cycle through these states. But this allows us to write + * functions and autonomous routines in a way that avoids loops within loops, and "waits". + */ + private enum LaunchState { + IDLE, + SPIN_UP, + LAUNCH, + LAUNCHING, + } + + private LaunchState launchState; + + @Override public void runOpMode() { Pose2d beginPose = new Pose2d(0, 0, 0); MecanumDrive drive = new MecanumDrive(hardwareMap, telemetry, gamepad1, beginPose); + launchState = LaunchState.IDLE; + + /* + * Initialize the hardware variables. Note that the strings used here as parameters + * to 'get' must correspond to the names assigned during the robot configuration + * step. + */ + // leftDrive = hardwareMap.get(DcMotor.class, "left_drive"); + // rightDrive = hardwareMap.get(DcMotor.class, "right_drive"); + launcher = hardwareMap.get(DcMotorEx.class, "launcher"); + leftFeeder = hardwareMap.get(CRServo.class, "left_feeder"); + rightFeeder = hardwareMap.get(CRServo.class, "right_feeder"); + + /* + * To drive forward, most robots need the motor on one side to be reversed, + * because the axles point in opposite directions. Pushing the left stick forward + * MUST make robot go forward. So adjust these two lines based on your first test drive. + * Note: The settings here assume direct drive on left and right wheels. Gear + * Reduction or 90 Deg drives may require direction flips + */ + // leftDrive.setDirection(DcMotor.Direction.REVERSE); + // rightDrive.setDirection(DcMotor.Direction.FORWARD); + + /* + * Here we set our launcher to the RUN_USING_ENCODER runmode. + * If you notice that you have no control over the velocity of the motor, it just jumps + * right to a number much higher than your set point, make sure that your encoders are plugged + * into the port right beside the motor itself. And that the motors polarity is consistent + * through any wiring. + */ + launcher.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + + /* + * Setting zeroPowerBehavior to BRAKE enables a "brake mode". This causes the motor to + * slow down much faster when it is coasting. This creates a much more controllable + * drivetrain. As the robot stops much quicker. + */ + // leftDrive.setZeroPowerBehavior(BRAKE); + // rightDrive.setZeroPowerBehavior(BRAKE); + launcher.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + + /* + * set Feeders to an initial value to initialize the servo controller + */ + leftFeeder.setPower(STOP_SPEED); + rightFeeder.setPower(STOP_SPEED); + + launcher.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, new PIDFCoefficients(300, 0, 0, 10)); + + /* + * Much like our drivetrain motors, we set the left feeder servo to reverse so that they + * both work to feed the ball into the robot. + */ + leftFeeder.setDirection(DcMotorSimple.Direction.REVERSE); + + /* + * Tell the driver that initialization is complete. + */ + telemetry.addData("Status", "Initialized"); + // Wait for Start to be pressed on the Driver Hub! waitForStart(); while (opModeIsActive()) { telemetry.addLine("Running TeleOp!"); - telemetry.update(); // Set the drive motor powers according to the gamepad input: drive.setDrivePowers(new PoseVelocity2d( @@ -50,6 +161,54 @@ public void runOpMode() { packet.fieldOverlay().setStroke("#3F51B5"); Drawing.drawRobot(packet.fieldOverlay(), drive.pose); MecanumDrive.sendTelemetryPacket(packet); + + if (gamepad1.y) { + launcher.setVelocity(LAUNCHER_TARGET_VELOCITY); + } else if (gamepad1.b) { // stop flywheel + launcher.setVelocity(STOP_SPEED); + } + + /* + * Now we call our "Launch" function. + */ + launch(gamepad1.rightBumperWasPressed()); + + /* + * Show the state and motor powers + */ + telemetry.addData("State", launchState); + // telemetry.addData("Motors", "left (%.2f), right (%.2f)", leftPower, rightPower); + telemetry.addData("motorSpeed", launcher.getVelocity()); + + telemetry.update(); + } + } + void launch(boolean shotRequested) { + switch (launchState) { + case IDLE: + if (shotRequested) { + launchState = LaunchState.SPIN_UP; + } + break; + case SPIN_UP: + launcher.setVelocity(LAUNCHER_TARGET_VELOCITY); + if (launcher.getVelocity() > LAUNCHER_MIN_VELOCITY) { + launchState = LaunchState.LAUNCH; + } + break; + case LAUNCH: + leftFeeder.setPower(FULL_SPEED); + rightFeeder.setPower(FULL_SPEED); + feederTimer.reset(); + launchState = LaunchState.LAUNCHING; + break; + case LAUNCHING: + if (feederTimer.seconds() > FEED_TIME_SECONDS) { + launchState = LaunchState.IDLE; + leftFeeder.setPower(STOP_SPEED); + rightFeeder.setPower(STOP_SPEED); + } + break; } } } From a5739e1b2fbdc816efb73cc65064cbfc724b8e67 Mon Sep 17 00:00:00 2001 From: Hankang Zhou <136044263+nobody-particular@users.noreply.github.com> Date: Tue, 30 Sep 2025 20:55:57 -0700 Subject: [PATCH 002/198] Conceptual text menu, needs testing --- team417/build.gradle | 1 + .../ftc/team417/TextMenuTest.java | 62 +++++++++++++++++++ 2 files changed, 63 insertions(+) create mode 100644 team417/src/main/java/org/firstinspires/ftc/team417/TextMenuTest.java diff --git a/team417/build.gradle b/team417/build.gradle index c7861fbf694c..f83dd718b541 100644 --- a/team417/build.gradle +++ b/team417/build.gradle @@ -50,4 +50,5 @@ dependencies { implementation "com.acmerobotics.dashboard:dashboard:0.4.16" implementation 'org.team11260:fast-load:0.1.2' implementation 'com.github.SwerveRobotics:FtcDrivers:0.0.2' + implementation 'com.github.valsei:java-text-menu:v3.2g' } \ No newline at end of file diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/TextMenuTest.java b/team417/src/main/java/org/firstinspires/ftc/team417/TextMenuTest.java new file mode 100644 index 000000000000..214e80a3b050 --- /dev/null +++ b/team417/src/main/java/org/firstinspires/ftc/team417/TextMenuTest.java @@ -0,0 +1,62 @@ +package org.firstinspires.ftc.team417; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +import net.valsei.java_text_menu.*; + +@Autonomous(name = "Text Menu Test") +public class TextMenuTest extends LinearOpMode { + + enum Alliances { + RED, + BLUE, + } + + enum Positions { + NEAR, + FAR, + } + + enum Movements { + MINIMAL, + LAUNCHING, + } + + double minWaitTime = 0.0; + double maxWaitTime = 15.0; + + @Override + public void runOpMode() throws InterruptedException { + TextMenu menu = new TextMenu(); + MenuInput menuInput = new MenuInput(MenuInput.InputType.CONTROLLER); + + menu.add(new MenuHeader("CHOOSE YOUR PLANTS!")) + .add("Shortcut?") + .add() // empty line for spacing + .add("Pick an alliance:") + .add("alliance-picker-1", Alliances.class) // enum selector shortcut + .add() + .add("Pick a starting position:") + .add("position-picker-1", Positions.class) // enum selector shortcut + .add() + .add("Pick a movement:") + .add("movement-picker-1", Movements.class) // enum selector shortcut + .add() + .add("Wait time:") + .add("wait-slider-1", new MenuSlider(minWaitTime, maxWaitTime)) + .add() + .add("finish-button-1", new MenuFinishedButton()); + + while (!menu.isCompleted()) { + // get x,y (stick) and select (A) input from controller + menuInput.update(gamepad1.left_stick_x, gamepad1.left_stick_y, gamepad1.a); + menu.updateWithInput(menuInput); + // display the updated menu + for (String line : menu.toListOfStrings()) { + telemetry.addLine(line); // but with appropriate printing method + } + telemetry.update(); + } + } +} From 83d491d6d339b49136c8c0d79b61f1883dd26701 Mon Sep 17 00:00:00 2001 From: Andrew Date: Thu, 2 Oct 2025 15:34:02 -0700 Subject: [PATCH 003/198] Fix Wily Works build issue, bring the Wily Works GamePad class up to date with the current SDK, and add PC gamepad support. --- WilyCore/build.gradle | 41 +- .../simulator/framework/InputManager.java | 1 + .../simulator/framework/WilyGamepad.java | 471 +++++++++++++++++- 3 files changed, 509 insertions(+), 4 deletions(-) diff --git a/WilyCore/build.gradle b/WilyCore/build.gradle index d852a6b04d8f..fead464319dc 100644 --- a/WilyCore/build.gradle +++ b/WilyCore/build.gradle @@ -19,24 +19,59 @@ kotlin { sourceSets { main { java { - srcDirs = ['src/main/java', {System.getenv('SRC_ROOT')}] + srcDirs = ['src/main/java', '../Sidekick/src/main/java', {System.getenv('SRC_ROOT')}] } } } +// LWJGL for gamepad input, generated from https://www.lwjgl.org/customize for just the "GLFW" +// module for MacOS arm64 and Windows x64. +import org.gradle.internal.os.OperatingSystem + +project.ext.lwjglVersion = "3.3.6" + +switch (OperatingSystem.current()) { + case OperatingSystem.LINUX: + project.ext.lwjglNatives = "natives-linux" + def osArch = System.getProperty("os.arch") + if (osArch.startsWith("arm") || osArch.startsWith("aarch64")) { + project.ext.lwjglNatives += osArch.contains("64") || osArch.startsWith("armv8") ? "-arm64" : "-arm32" + } else if (osArch.startsWith("ppc")) { + project.ext.lwjglNatives += "-ppc64le" + } else if (osArch.startsWith("riscv")) { + project.ext.lwjglNatives += "-riscv64" + } + break + case OperatingSystem.MAC_OS: + project.ext.lwjglNatives = "natives-macos-arm64" + break + case OperatingSystem.WINDOWS: + project.ext.lwjglNatives = "natives-windows" + break +} + repositories { - jcenter() + mavenCentral() maven { url = 'https://maven.brott.dev/' } } dependencies { implementation project(path: ':WilyWorks') implementation "com.acmerobotics.roadrunner:core:1.0.0" // For Pose2d, PoseVelocity2d, etc. - // implementation "uk.co.electronstudio.sdl2gdx:sdl2gdx:1.0.+" // For gamepad control implementation "org.reflections:reflections:0.10.2" // For enumerating classes with annotations implementation "org.jetbrains.kotlin:kotlin-stdlib-jdk8" // For enumerating all classes implementation "com.google.code.gson:gson:2.11.0" // For serializing Java objects to JSON + implementation 'net.bytebuddy:byte-buddy:1.15.10' // For proxying classes rather than interfaces + implementation 'org.objenesis:objenesis:3.2' // Instantiate class without invoking a constructor + + // Lwjgl for gamepad input: + implementation platform("org.lwjgl:lwjgl-bom:$lwjglVersion") + implementation "org.lwjgl:lwjgl" + implementation "org.lwjgl:lwjgl-glfw" + runtimeOnly "org.lwjgl:lwjgl::$lwjglNatives" + runtimeOnly "org.lwjgl:lwjgl-glfw::$lwjglNatives" } + compileKotlin { kotlinOptions { jvmTarget = "1.8" diff --git a/WilyCore/src/main/java/com/wilyworks/simulator/framework/InputManager.java b/WilyCore/src/main/java/com/wilyworks/simulator/framework/InputManager.java index c3d6d9fac007..7798064f58d6 100644 --- a/WilyCore/src/main/java/com/wilyworks/simulator/framework/InputManager.java +++ b/WilyCore/src/main/java/com/wilyworks/simulator/framework/InputManager.java @@ -348,6 +348,7 @@ void update(Gamepad gamepad, int gamepadId) { gamepad.right_trigger = getAxis(gamepadId, SDL.SDL_CONTROLLER_AXIS_TRIGGERRIGHT); gamepad.updateButtonAliases(); + gamepad.updateEdgeDetection(); } // Get a string describing which gamepads the inputs correspond to. diff --git a/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyGamepad.java b/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyGamepad.java index e8776118bb18..428a406d6099 100644 --- a/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyGamepad.java +++ b/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyGamepad.java @@ -1,9 +1,100 @@ package com.wilyworks.simulator.framework; -import com.qualcomm.robotcore.util.Range; +import com.qualcomm.robotcore.hardware.Gamepad; import java.util.ArrayList; +/** + * Monitors changes to buttons on a Gamepad. + *

+ * wasPressed() will return true if the button was pressed since the last call + * of that method. + *

+ * wasReleased() will return ture if the button was released since the last call + * of that method. + *

+ * @see Gamepad + */ +class GamepadStateChanges { + protected ButtonStateMonitor dpadDown = new ButtonStateMonitor(); + protected ButtonStateMonitor dpadLeft = new ButtonStateMonitor(); + protected ButtonStateMonitor dpadRight = new ButtonStateMonitor(); + protected ButtonStateMonitor a = new ButtonStateMonitor(); + protected ButtonStateMonitor dpadUp = new ButtonStateMonitor(); + protected ButtonStateMonitor b = new ButtonStateMonitor(); + protected ButtonStateMonitor x = new ButtonStateMonitor(); + protected ButtonStateMonitor y = new ButtonStateMonitor(); + protected ButtonStateMonitor guide = new ButtonStateMonitor(); + protected ButtonStateMonitor start = new ButtonStateMonitor(); + protected ButtonStateMonitor back = new ButtonStateMonitor(); + protected ButtonStateMonitor leftBumper = new ButtonStateMonitor(); + protected ButtonStateMonitor rightBumper = new ButtonStateMonitor(); + protected ButtonStateMonitor leftStickButton = new ButtonStateMonitor(); + protected ButtonStateMonitor rightStickButton = new ButtonStateMonitor(); + protected ButtonStateMonitor circle = new ButtonStateMonitor(); + protected ButtonStateMonitor cross = new ButtonStateMonitor(); + protected ButtonStateMonitor triangle = new ButtonStateMonitor(); + protected ButtonStateMonitor square = new ButtonStateMonitor(); + protected ButtonStateMonitor share = new ButtonStateMonitor(); + protected ButtonStateMonitor options = new ButtonStateMonitor(); + protected ButtonStateMonitor touchpad = new ButtonStateMonitor(); + protected ButtonStateMonitor ps = new ButtonStateMonitor(); + + protected class ButtonStateMonitor { + private boolean lastPressed = false; + private boolean pressNotify = false; + private boolean releaseNotify = false; + + private void update(boolean nowPressed) { + if (!lastPressed && nowPressed) { + pressNotify = true; + } + if (lastPressed && !nowPressed) { + releaseNotify = true; + } + lastPressed = nowPressed; + } + + protected boolean wasPressed() { + boolean pressed = pressNotify; + pressNotify = false; + return pressed; + } + + protected boolean wasReleased() { + boolean released = releaseNotify; + releaseNotify = false; + return released; + } + } + + protected void updateAllButtons(WilyGamepad gamepad) { + dpadUp.update(gamepad.dpad_up); + dpadDown.update(gamepad.dpad_down); + dpadLeft.update(gamepad.dpad_left); + dpadRight.update(gamepad.dpad_right); + a.update(gamepad.a); + b.update(gamepad.b); + x.update(gamepad.x); + y.update(gamepad.y); + guide.update(gamepad.guide); + start.update(gamepad.start); + back.update(gamepad.back); + leftBumper.update(gamepad.left_bumper); + rightBumper.update(gamepad.right_bumper); + leftStickButton.update(gamepad.left_stick_button); + rightStickButton.update(gamepad.right_stick_button); + circle.update(gamepad.circle); + cross.update(gamepad.cross); + triangle.update(gamepad.triangle); + square.update(gamepad.square); + share.update(gamepad.share); + options.update(gamepad.options); + touchpad.update(gamepad.touchpad); + ps.update(gamepad.ps); + } +} + /** * Wily Works Gamepad implementation that takes input either from a connected gamepad or * from the keyboard. @@ -50,6 +141,11 @@ public class WilyGamepad { public WilyGamepad() { } + /** + * Edge detection for gamepads + */ + private volatile GamepadStateChanges changes = new GamepadStateChanges(); + public void updateButtonAliases(){ // There is no assignment for touchpad because there is no equivalent on XBOX controllers. circle = b; @@ -61,6 +157,10 @@ public void updateButtonAliases(){ ps = guide; } + public void updateEdgeDetection(){ + changes.updateAllButtons(this); + } + public void runRumbleEffect(RumbleEffect effect) { } public void rumble(int durationMs) { } public void rumble(double rumble1, double rumble2, int durationMs) { } @@ -93,4 +193,373 @@ public RumbleEffect build() { } } + /** + * Checks if dpad_up was pressed since the last call of this method + * @return true if dpad_up was pressed since the last call of this method; otherwise false + */ + public boolean dpadUpWasPressed() { + return changes.dpadUp.wasPressed(); + } + + /** + * Checks if dpad_up was released since the last call of this method + * @return true if dpad_up was released since the last call of this method; otherwise false + */ + public boolean dpadUpWasReleased() { + return changes.dpadUp.wasReleased(); + } + + /** + * Checks if dpad_down was pressed since the last call of this method + * @return true if dpad_down was pressed since the last call of this method; otherwise false + */ + public boolean dpadDownWasPressed() { + return changes.dpadDown.wasPressed(); + } + + /** + * Checks if dpad_down was released since the last call of this method + * @return true if dpad_down was released since the last call of this method; otherwise false + */ + public boolean dpadDownWasReleased() { + return changes.dpadDown.wasReleased(); + } + + /** + * Checks if dpad_left was pressed since the last call of this method + * @return true if dpad_left was pressed since the last call of this method; otherwise false + */ + public boolean dpadLeftWasPressed() { + return changes.dpadLeft.wasPressed(); + } + + /** + * Checks if dpad_left was released since the last call of this method + * @return true if dpad_left was released since the last call of this method; otherwise false + */ + public boolean dpadLeftWasReleased() { + return changes.dpadLeft.wasReleased(); + } + + /** + * Checks if dpad_right was pressed since the last call of this method + * @return true if dpad_right was pressed since the last call of this method; otherwise false + */ + public boolean dpadRightWasPressed() { + return changes.dpadRight.wasPressed(); + } + + /** + * Checks if dpad_right was released since the last call of this methmethodod + * @return true if dpad_right was released since the last call of this ; otherwise false + */ + public boolean dpadRightWasReleased() { + return changes.dpadRight.wasReleased(); + } + + /** + * Checks if a was pressed since the last call of this method + * @return true if a was pressed since the last call of this method; otherwise false + */ + public boolean aWasPressed() { + return changes.a.wasPressed(); + } + + /** + * Checks if a was released since the last call of this method + * @return true if a was released since the last call of this method; otherwise false + */ + public boolean aWasReleased() { + return changes.a.wasReleased(); + } + + /** + * Checks if b was pressed since the last call of this method + * @return true if b was pressed since the last call of this method; otherwise false + */ + public boolean bWasPressed() { + return changes.b.wasPressed(); + } + + /** + * Checks if b was released since the last call of this method + * @return true if b was released since the last call of this method; otherwise false + */ + public boolean bWasReleased() { + return changes.b.wasReleased(); + } + + /** + * Checks if x was pressed since the last call of this method + * @return true if x was pressed since the last call of this method; otherwise false + */ + public boolean xWasPressed() { + return changes.x.wasPressed(); + } + + /** + * Checks if x was released since the last call of this method + * @return true if x was released since the last call of this method; otherwise false + */ + public boolean xWasReleased() { + return changes.x.wasReleased(); + } + + /** + * Checks if y was pressed since the last call of this method + * @return true if y was pressed since the last call of this method; otherwise false + */ + public boolean yWasPressed() { + return changes.y.wasPressed(); + } + + /** + * Checks if y was released since the last call of this method + * @return true if y was released since the last call of this method; otherwise false + */ + public boolean yWasReleased() { + return changes.y.wasReleased(); + } + + /** + * Checks if guide was pressed since the last call of this method + * @return true if guide was pressed since the last call of this method; otherwise false + */ + public boolean guideWasPressed() { + return changes.guide.wasPressed(); + } + + /** + * Checks if guide was released since the last call of this method + * @return true if guide was released since the last call of this method; otherwise false + */ + public boolean guideWasReleased() { + return changes.guide.wasReleased(); + } + + /** + * Checks if start was pressed since the last call of this method + * @return true if start was pressed since the last call of this method; otherwise false + */ + public boolean startWasPressed() { + return changes.start.wasPressed(); + } + + /** + * Checks if start was released since the last call of this method + * @return true if start was released since the last call of this method; otherwise false + */ + public boolean startWasReleased() { + return changes.start.wasReleased(); + } + + /** + * Checks if back was pressed since the last call of this method + * @return true if back was pressed since the last call of this method; otherwise false + */ + public boolean backWasPressed() { + return changes.back.wasPressed(); + } + + /** + * Checks if back was released since the last call of this method + * @return true if back was released since the last call of this method; otherwise false + */ + public boolean backWasReleased() { + return changes.back.wasReleased(); + } + + /** + * Checks if left_bumper was pressed since the last call of this method + * @return true if left_bumper was pressed since the last call of this method; otherwise false + */ + public boolean leftBumperWasPressed() { + return changes.leftBumper.wasPressed(); + } + + /** + * Checks if left_bumper was released since the last call of this method + * @return true if left_bumper was released since the last call of this method; otherwise false + */ + public boolean leftBumperWasReleased() { + return changes.leftBumper.wasReleased(); + } + + /** + * Checks if right_bumper was pressed since the last call of this method + * @return true if right_bumper was pressed since the last call of this method; otherwise false + */ + public boolean rightBumperWasPressed() { + return changes.rightBumper.wasPressed(); + } + + /** + * Checks if right_bumper was released since the last call of this method + * @return true if right_bumper was released since the last call of this method; otherwise false + */ + public boolean rightBumperWasReleased() { + return changes.rightBumper.wasReleased(); + } + + /** + * Checks if left_stick_button was pressed since the last call of this method + * @return true if left_stick_button was pressed since the last call of this method; otherwise false + */ + public boolean leftStickButtonWasPressed() { + return changes.leftStickButton.wasPressed(); + } + + /** + * Checks if left_stick_button was released since the last call of this method + * @return true if left_stick_button was released since the last call of this method; otherwise false + */ + public boolean leftStickButtonWasReleased() { + return changes.leftStickButton.wasReleased(); + } + + /** + * Checks if right_stick_button was pressed since the last call of this method + * @return true if right_stick_button was pressed since the last call of this method; otherwise false + */ + public boolean rightStickButtonWasPressed() { + return changes.rightStickButton.wasPressed(); + } + + /** + * Checks if right_stick_button was released since the last call of this method + * @return true if right_stick_button was released since the last call of this method; otherwise false + */ + public boolean rightStickButtonWasReleased() { + return changes.rightStickButton.wasReleased(); + } + + /** + * Checks if circle was pressed since the last call of this method + * @return true if circle was pressed since the last call of this method; otherwise false + */ + public boolean circleWasPressed() { + return changes.circle.wasPressed(); + } + + /** + * Checks if circle was released since the last call of this method + * @return true if circle was released since the last call of this method; otherwise false + */ + public boolean circleWasReleased() { + return changes.circle.wasReleased(); + } + + /** + * Checks if cross was pressed since the last call of this method + * @return true if cross was pressed since the last call of this method; otherwise false + */ + public boolean crossWasPressed() { + return changes.cross.wasPressed(); + } + + /** + * Checks if cross was released since the last call of this method + * @return true if cross was released since the last call of this method; otherwise false + */ + public boolean crossWasReleased() { + return changes.cross.wasReleased(); + } + + /** + * Checks if triangle was pressed since the last call of this method + * @return true if triangle was pressed since the last call of this method; otherwise false + */ + public boolean triangleWasPressed() { + return changes.triangle.wasPressed(); + } + + /** + * Checks if triangle was released since the last call of this method + * @return true if triangle was released since the last call of this method; otherwise false + */ + public boolean triangleWasReleased() { + return changes.triangle.wasReleased(); + } + + /** + * Checks if square was pressed since the last call of this method + * @return true if square was pressed since the last call of this method; otherwise false + */ + public boolean squareWasPressed() { + return changes.square.wasPressed(); + } + + /** + * Checks if square was released since the last call of this method + * @return true if square was released since the last call of this method; otherwise false + */ + public boolean squareWasReleased() { + return changes.square.wasReleased(); + } + + /** + * Checks if share was pressed since the last call of this method + * @return true if share was pressed since the last call of this method; otherwise false + */ + public boolean shareWasPressed() { + return changes.share.wasPressed(); + } + + /** + * Checks if share was released since the last call of this method + * @return true if share was released since the last call of this method; otherwise false + */ + public boolean shareWasReleased() { + return changes.share.wasReleased(); + } + + /** + * Checks if options was pressed since the last call of this method + * @return true if options was pressed since the last call of this method; otherwise false + */ + public boolean optionsWasPressed() { + return changes.options.wasPressed(); + } + + /** + * Checks if options was released since the last call of this method + * @return true if options was released since the last call of this method; otherwise false + */ + public boolean optionsWasReleased() { + return changes.options.wasReleased(); + } + + /** + * Checks if touchpad was pressed since the last call of this method + * @return true if touchpad was pressed since the last call of this method; otherwise false + */ + public boolean touchpadWasPressed() { + return changes.touchpad.wasPressed(); + } + + /** + * Checks if touchpad was released since the last call of this method + * @return true if touchpad was released since the last call of this method; otherwise false + */ + public boolean touchpadWasReleased() { + return changes.touchpad.wasReleased(); + } + + /** + * Checks if ps was pressed since the last call of this method + * @return true if ps was pressed since the last call of this method; otherwise false + */ + public boolean psWasPressed() { + return changes.ps.wasPressed(); + } + + /** + * Checks if ps was released since the last call of this method + * @return true if ps was released since the last call of this method; otherwise false + */ + public boolean psWasReleased() { + return changes.ps.wasReleased(); + } + + } From a7f29d833d32534a6bfd30cfa70111456e46cf9b Mon Sep 17 00:00:00 2001 From: Andrew Date: Thu, 2 Oct 2025 18:36:22 -0700 Subject: [PATCH 004/198] Potential Wily Works fix for Macs. --- WilyCore/build.gradle | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/WilyCore/build.gradle b/WilyCore/build.gradle index fead464319dc..d7e3c0dd1b54 100644 --- a/WilyCore/build.gradle +++ b/WilyCore/build.gradle @@ -1,6 +1,7 @@ plugins { id 'java-library' id 'org.jetbrains.kotlin.jvm' + id 'application' } java { @@ -16,6 +17,10 @@ kotlin { } } +application { + applicationDefaultJvmArgs = ["-XstartOnFirstThread"] +} + sourceSets { main { java { From 8f95d5297300aab5dbe41ca5cfb5a46cce811c55 Mon Sep 17 00:00:00 2001 From: Emmett Date: Thu, 2 Oct 2025 19:06:44 -0700 Subject: [PATCH 005/198] changed control methods and added slowmode --- .../ftc/team417/CompetitionTeleOp.java | 33 +++++++++++++++---- 1 file changed, 27 insertions(+), 6 deletions(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java index 7c832f4c3ef1..86dddf4bf25b 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java @@ -25,13 +25,19 @@ public class CompetitionTeleOp extends BaseOpMode { final double STOP_SPEED = 0.0; //We send this power to the servos when we want them to stop. final double FULL_SPEED = 1.0; + double FASTDRIVE_SPEED = 1.0; + double SLOWDRIVE_SPEED = 0.5; + /* * When we control our launcher motor, we are using encoders. These allow the control system * to read the current speed of the motor and apply more or less power to keep it at a constant * velocity. Here we are setting the target, and minimum velocity that the launcher should run * at. The minimum velocity is a threshold for determining when to fire. */ - final double LAUNCHER_TARGET_VELOCITY = 1125; + final double LAUNCHER_TARGET_VELOCITY = 2250; + //was 1125 + + final double LAUNCHER_LOW_VELOCITY = 1125; final double LAUNCHER_MIN_VELOCITY = 1075; // Declare OpMode members. @@ -142,10 +148,16 @@ public void runOpMode() { // Set the drive motor powers according to the gamepad input: drive.setDrivePowers(new PoseVelocity2d( new Vector2d( - -gamepad1.left_stick_y, - -gamepad1.left_stick_x + -gamepad1.left_stick_y * doSLOWMODE(), + -gamepad1.left_stick_x * doSLOWMODE() + ), -gamepad1.right_stick_x + + + + + )); // Update the current pose: @@ -162,16 +174,18 @@ public void runOpMode() { Drawing.drawRobot(packet.fieldOverlay(), drive.pose); MecanumDrive.sendTelemetryPacket(packet); - if (gamepad1.y) { + if (gamepad2.y) { //high speed launcher.setVelocity(LAUNCHER_TARGET_VELOCITY); - } else if (gamepad1.b) { // stop flywheel + } else if (gamepad2.a) { //slow speed + launcher.setVelocity(LAUNCHER_LOW_VELOCITY); + } else if (gamepad2.b) { // stop flywheel launcher.setVelocity(STOP_SPEED); } /* * Now we call our "Launch" function. */ - launch(gamepad1.rightBumperWasPressed()); + launch(gamepad2.rightBumperWasPressed()); /* * Show the state and motor powers @@ -211,4 +225,11 @@ void launch(boolean shotRequested) { break; } } + public double doSLOWMODE(){ + if (gamepad1.left_stick_button) { + return SLOWDRIVE_SPEED; + } else { + return FASTDRIVE_SPEED; + } + } } From cf35ffe537ddf4b33fdd9ea822cfe2a3684e9b47 Mon Sep 17 00:00:00 2001 From: Emmett Date: Thu, 2 Oct 2025 19:37:25 -0700 Subject: [PATCH 006/198] added second launch state for --- .../ftc/team417/CompetitionTeleOp.java | 35 +++++++++++++------ 1 file changed, 25 insertions(+), 10 deletions(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java index 86dddf4bf25b..6232e7826dc9 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java @@ -34,11 +34,15 @@ public class CompetitionTeleOp extends BaseOpMode { * velocity. Here we are setting the target, and minimum velocity that the launcher should run * at. The minimum velocity is a threshold for determining when to fire. */ - final double LAUNCHER_TARGET_VELOCITY = 2250; + final double LAUNCHER_HIGH_TARGET_VELOCITY = 2250; //was 1125 - final double LAUNCHER_LOW_VELOCITY = 1125; - final double LAUNCHER_MIN_VELOCITY = 1075; + final double LAUNCHER_LOW_TARGET_VELOCITY = 1125; + final double LAUNCHER_LOW_MIN_VELOCITY = 1075; + + final double LAUNCHER_HIGH_MIN_VELOCITY = 2200; + + boolean doHighLaunch = false; // Declare OpMode members. private DcMotorEx launcher = null; @@ -65,7 +69,8 @@ public class CompetitionTeleOp extends BaseOpMode { */ private enum LaunchState { IDLE, - SPIN_UP, + SPIN_UP_HIGH, + SPIN_UP_LOW, LAUNCH, LAUNCHING, } @@ -175,9 +180,10 @@ public void runOpMode() { MecanumDrive.sendTelemetryPacket(packet); if (gamepad2.y) { //high speed - launcher.setVelocity(LAUNCHER_TARGET_VELOCITY); + launcher.setVelocity(LAUNCHER_HIGH_TARGET_VELOCITY); + doHighLaunch = true; } else if (gamepad2.a) { //slow speed - launcher.setVelocity(LAUNCHER_LOW_VELOCITY); + launcher.setVelocity(LAUNCHER_LOW_TARGET_VELOCITY); } else if (gamepad2.b) { // stop flywheel launcher.setVelocity(STOP_SPEED); } @@ -201,15 +207,24 @@ void launch(boolean shotRequested) { switch (launchState) { case IDLE: if (shotRequested) { - launchState = LaunchState.SPIN_UP; + if (doHighLaunch) { + launchState = LaunchState.SPIN_UP_HIGH; + } else { + launchState = LaunchState.SPIN_UP_LOW; + } } break; - case SPIN_UP: - launcher.setVelocity(LAUNCHER_TARGET_VELOCITY); - if (launcher.getVelocity() > LAUNCHER_MIN_VELOCITY) { + case SPIN_UP_LOW: + launcher.setVelocity(LAUNCHER_LOW_TARGET_VELOCITY); + if (launcher.getVelocity() > LAUNCHER_LOW_MIN_VELOCITY) { launchState = LaunchState.LAUNCH; } break; + case SPIN_UP_HIGH: + launcher.setVelocity(LAUNCHER_HIGH_TARGET_VELOCITY); + if (launcher.getVelocity() > LAUNCHER_HIGH_MIN_VELOCITY) { + launchState = LaunchState.LAUNCH; + } case LAUNCH: leftFeeder.setPower(FULL_SPEED); rightFeeder.setPower(FULL_SPEED); From c9e171c05c567a1e3e9ff6258f939e588fd543af Mon Sep 17 00:00:00 2001 From: Team <417skid@swerverobotics.club> Date: Thu, 2 Oct 2025 19:53:46 -0700 Subject: [PATCH 007/198] Updated field image to be 2025 decode. --- .../src/main/java/com/wilyworks/simulator/framework/Field.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/WilyCore/src/main/java/com/wilyworks/simulator/framework/Field.java b/WilyCore/src/main/java/com/wilyworks/simulator/framework/Field.java index 5beddcc7c733..d0a11b5d0759 100644 --- a/WilyCore/src/main/java/com/wilyworks/simulator/framework/Field.java +++ b/WilyCore/src/main/java/com/wilyworks/simulator/framework/Field.java @@ -64,7 +64,7 @@ public Field(Simulation simulation) { InputStream compassStream = classLoader.getResourceAsStream( "background/misc/compass-rose-white-text.png"); InputStream fieldStream = classLoader.getResourceAsStream( - "background/season-2024-intothedeep/field-2024-juice-dark.png"); + "background/season-2025-decode/field-2025-juice-dark.png"); try { if (compassStream != null) { From b3596245b36bdea15c3a15487050d3851a3feeee Mon Sep 17 00:00:00 2001 From: Team <417skid@swerverobotics.club> Date: Thu, 2 Oct 2025 19:53:46 -0700 Subject: [PATCH 008/198] Updated field image to be 2025 decode. --- .../wilyworks/simulator/framework/Field.java | 2 +- .../field-2025-juice-dark.png | Bin 0 -> 57450 bytes 2 files changed, 1 insertion(+), 1 deletion(-) create mode 100644 WilyCore/src/main/resources/background/season-2025-decode/field-2025-juice-dark.png diff --git a/WilyCore/src/main/java/com/wilyworks/simulator/framework/Field.java b/WilyCore/src/main/java/com/wilyworks/simulator/framework/Field.java index 5beddcc7c733..d0a11b5d0759 100644 --- a/WilyCore/src/main/java/com/wilyworks/simulator/framework/Field.java +++ b/WilyCore/src/main/java/com/wilyworks/simulator/framework/Field.java @@ -64,7 +64,7 @@ public Field(Simulation simulation) { InputStream compassStream = classLoader.getResourceAsStream( "background/misc/compass-rose-white-text.png"); InputStream fieldStream = classLoader.getResourceAsStream( - "background/season-2024-intothedeep/field-2024-juice-dark.png"); + "background/season-2025-decode/field-2025-juice-dark.png"); try { if (compassStream != null) { diff --git a/WilyCore/src/main/resources/background/season-2025-decode/field-2025-juice-dark.png b/WilyCore/src/main/resources/background/season-2025-decode/field-2025-juice-dark.png new file mode 100644 index 0000000000000000000000000000000000000000..f0a3fb3f0afc963b6b29ec623f922ccc675a3073 GIT binary patch literal 57450 zcmX`SWk6fc(>9Dd!97@UcSz6z#a)6Hhqk!86nBC{ad)R!vEo5Wi z@8`q$l5=)vXXcunoohBRn(7L;Sd>@@2ne`Jif^9t>poFU#L^dk_$i5j0hG z<+{4M*vA;eu%*cu86gk|Cnr~UcsMc18x#~ALVY|kGI9e013o_f_4RcD0YO6yY;<&V zZf+hcE33uDMSh_?AP~3@WO#Xbq0OM=70CKX5r0F?^!)sM3}hrF!S@3Yv9ohLJUqaK z0%;6UP*Aw}QYa~@=n#?e^72SXNN`_clc10h5D>ETzG`f26vn{9A|z}jqM&E|Ku%6! zLiqj=$cV*_fr5xiOG^j9W98uBq^GCH!6l_eqe~|wBhUu$@bG42WU%(XvLbQ{3=G7f zrhZjS{X)y4uBJ9QIT;fZV{dOiKR=I&iHU`Tjf;!R#hZwThzJIQsi{fv@d<2k3B<(2 zhDoTnc$4ICRdGOAEM4>*T#?F{I2FW{d{`pB)80vU>OiWA+3=H06;n+CzAt522ctpr3ghWI{dIV-(US0&`MzrQPKpsTAZD=RCpIRO?HHUc*G z3^^?l5)v{1nSq_1ohz1v^fe(N5dkt$NlA&EoE$PTG9oJCyLazoWMoKf32jL{Yieo$ z06YLP9u^`Z3L!EcJ`cwf6C;Z|6bi*)MrUrLqx?vEdwa`?$*Tm=VrHg8Lqq%h`}b=c z1z~J)nna4--QBjfHijyiv$Hd{5e9m6rsU-0B}#fcC0tWeQ+Lvjbh$uQ_8UrS5t>gtM$iuf}T1+PFZH8nL7 z7Lu^AFg^kz!z*efC8YrJ7|uB+b0T{b5)?Evq|wn)bb7Rpctl17R%mGGbg!&_Qque2 z;M20P;fP^TQ0ig}V6tIiy(4xdAOPgSal+t)uLd9_0%v7q6&4m&R8){U5V518V^L6; zo0~i15&-0IEOGFqq@)rFNJ*>-iQeL4a-frYl6)hlMWsYRqei_X_(KUtC#H*{p*sQs zHqpNa5g|L5907q2LFtXOuFuj@4_*M#udLIAFBI4E7!c&9^x$TWf|?F3$JAqcV9lmy z2}Xt#S`px|oN1M~pLJ#KoYN!+^2;JdNt(42vf9I(Ob=D8Hj_Bm%IkM<{hFQ667w#|`^0RZfyhyJ=&Cf4tKbf+bUkrdZe`|&wlf!bLD!^YWT!&k9NZEQXFmtUNtU&Qg%GA58<`PL2)AYF zn3XTu+AP@jP)-;O?pMf-Q!dWUsya|0D&R|e@qfZee_0BocybtvSuU7{I#nLpOTAgI zwZ;ZA%a@I~1r#M%olpF>9k!T;ZhAUAx0&yd+0>ga)Tk%dP5Tbt>^C;+7bwkFf8DVC zE^oZyn_H)b1p7{5Ojfnr_PrzVqxF2HAfk)4hHIl;Ubqf9&_N0R>^6H> zKjZo;J^xh`D~%lGV*G4B-&?9;fs&`V-IM3biO-nKw+%j&hu+E>HA_%42Qoy$=PJyX z(>pZi(!*4az4OOCiZ;?d#8_lv7^EP#s$@-V?Xn0^pz@(-?(0V~Txa9wxk1eikb_b=o2X-A(>>ZZW*m^g5T8^&}nNRuTGa!={;Kdn_i^Y8=10VHH85-aLaeD3QkavCgcz1EQV5{-HB zJS^VKy@n`UJh|!xfcT4(5<77W>O~Da%j2QP%G#`(V~Y69NrC z4%H>%yt z)K$RT);ohM^#L|4fN`paz*~m4*e7D-9zHV%dQYCeT>PHOgg4)Id#+YtthzLV3RFyv z`7_D2QjA;^Z}B&pOz#t)ENko&E?>~j`9OXQe9()%eM9%1Z;QV?*7`|4LvxtkIvIcU z)FK4Z{QUUAGuH&KKmELR+-hTe%Dm2NB9aj6NEp#STCVD~V@^GJA$u#j*pS7Lg5S$YlQA1qVaXJ4Ck z#C4-Sv*6_*!2*rj#~j0cw^a?c<)`*pEctCCG}tV^Gn$G+*MD0E@=8i1O6r@g%t$O( zUQT-K*=t4tXaem}(X$Ss&0;k5#Yu1O!uJiB<~ZZ;c){7;MeNNT8x~`;0~oDR&?PC> zMyr|^jF(GoFqqTllGBfL0*aAYh%X9>(7U#B8j6Sryse-?{94a5A2G%KxMX3B1RSHr zh*=AGN2Bd77uLwtT#%mu;pjZsozWFtoJ2Cb6&%1G}pIT z+*TizIEs=ail*o|ladrelK*R5o2XN*XanLbVkR7mc&%n&6|TmO&ZZL zKJ`(LoCz*}<9bl9-7taM(;ozVKk!7)e+=10CuMd(&|&3fqRV5~e_T z4ql8d6&(1rgj;r1Vy-y+uJ^hzeZ^zDvL;U}=kaO7*A`O7MX~830AvL~LP~Io((6#D zW>8>Lif&=R7;9=zgaVRUmDTHv%!)oO^`PZcDG$lWCEvtSmGUm5O*W%a(*zm}Aq;Q_ zBS`Q(K{_7yxG=}5k|His-s8&}hP5S^VB)5}MLQ-nxWjhXMflKG*WYCr5*r4q94eS4 zPU-v8J@3RipV-DFR&P1G=UDDgU>v^KjC}E9xd^b0DAm{ zQ6pE>)$$H8;k}oKV}otsR`l`O>Q4e9za~ZVMZ|zfTCT>A=i4g&#f4J5pE<3 z=JyA>S-bmWlx8E{_ z$Ipvz9ys0p*Gj{;#G8Tm-6F4yP?G^qOcFDxx9?mOb&&y#`>}I!qbwbx>(S;H8oIsg z@=W$%<4wS8*ftOtVB`t}2H9B+H$7kt6MR8*0qXLAuGPdrcHYIftvvY}Hqh0ZS3yJ7 z^NV2Ae(BF&NjpZ}&=0&?war2mmX$`W#K3QPqiHekR}7Q`Y#UyXq~>}-Mkh~{;McUi z6D4tD4!6y#4+IHNw8k|7&>Na1b=Eg#NOdC(UD@wqJN`0MDtS{6_wx$3KpK33Mnqu8yY%=MCYI@rlV=zTI8boFSuLu_k>m7 zw0?OYK`Hz7V^`)6*#M8YAMWJb+{Cl2+^k1H0O($|H9M3F87A!l;U|HeU+D)AzTH6^ zE@B=Iv5~{B#+pgH_*sd?P@0Is+O{eCJWz-NWiAGde;wj4dBWENo)%tTdwJk#0dEN| z<-fdjghgx;TgooLIywFUAR zW8g`K3w~01&EJO=g^W(4<%0I~nh^Z`2xm75h`*_sutB$8i9B3H=`uCRWU`QnZxWi; z^M~+_^!O^F9G|>pX=S{P_-*#fH~SmE9>K&xnq$sARp1}k(e&``8n$6uc(@Vj`A24L z4Y~gG(M^|_TN6FJR9G^<3{*FF^Y0g6%BH*?q`nOFst`ms!Uyi}Up!DekYhKVV-YK+ zcsm)6Asji&aGBBGy_(IKQkncgg)`gH+QpC`2AKeBptZp8oZ&| zS~HNs(Rr%v{W;BzhKNT{5S9ww%@pNM%KK}5W5lpCA(10x%=zaPrNsT?bmdkJF)@(+ zWB!pLLzk5bMkJ|+im_#&O!t>RG6VJmfn=7Ys+yb8>wTxBOI!>vQRDjV^vi0jE!Wra z@Y6AgJ95#wa=4L^Ew3eR;?y-Nql!4GnNF*lSQU$MT&fs>=mW^^W4uFo;)ySnRM1QQ z2XIVk@9@VnN||>?J^9}f(;<7h$dhKYuk6|EEmMBzj5|w3B&Gh5M+5-up)Kuh+P^ZW z-1@4qYO8}cI_kHg6`9EV#x70Oc&@aoL4>WyG3(|aPH-tlLE6)K?;DQbo=vWH1-u?+ z2~&CqLXF~FebE+cdMsU>M`oRM!Vzm<2s%lCNEpxtOpiVUnoET>k(`6(PytZx)>oqa zW#6>&=YeIuz*5po7A^M{wfirDu837&<@4SX=4HKJW{D%k-hA0?x=#{}?r92;4_`UB zZ3;Hm9ILWR21CQFvNW@S!;zjRxgP)qkg=kT1>q9mI*{Ja`R^GgYK0(OBtXF8JX1|B zEaIKt9MjNdi#e-_wo)!(s-(&GBeE!_bf}(QU8?)Imz%&e>9h&9iKS|M>&Wn@b;9g@vs1zdSsTSRPhn5*#8o%+&@n!p|=j zjFlEl2m42a>)76t^f{DNB3wUv5=ZHZPKgKX_bJk?yMT0W&H4y1P@2s|lYXKeyeoCQ z@zvFv-%*4h%(g>5IpDAYJVTJzR`Oc>rJZkNvSHJmg%xS@`E3jQPFOTXZ4XCc=)##~ z9)8FEPET7x=Pm2OVpfXC(&sHE4^g5YlwoDh^*Sw4tuW?8YJa)OY5PwZ0#X*L4dXO( z>3huGJC#f(?jF~xhUihB2leNHYS$1~(u}k;-Ecz4$1yy8qawNE;aKWllx@X&ucp8qdCKULusQ;ju=)^S-s2eb;ED+{pcNKO&d?1kWrR z5~yS2HNx=O@>bE=hRCy8XPx&@q2;xF zms?cO02d=0V6_PEjeO@(7=XqR1lq0cWuNl!n`;xl^IvAal=^r-E2VY?xjWW?ICk$; z+`S8m=<9ECu*x!ARUCBgl9%)QK4Or!KmR76aL6N}!Zm(Umr(J7X|JVxKjmP{%Xd1u zXIj`{rBUe0e{g8I7^2N05{ct^I8PAdyr7kb;co5a#=u2KCOx}P<$!qCzIHnzRY43} z3zYqN-M%&|wV95HE*DkRS^SLxBRof$61?0>WoSO#e_xj^`o52%jo8oNx>LhS965tG z$S7~*AXAEK#lx@tU{}+z*t^y%t;g!UV7lW{`ztF5DhJf@@PC3tdx)N{xTK!j=iH78 z9&g*ssu!H@YKx1jw--R2ucY|La+ux+SW8XWs2OI);nU00!`umv%@PQ(o6?|4cMv@^V*OE%E z3-Tg!bpp^rf4usRX5%NQ<8m99#t^8wX#cXZGti8A{Wgda`1Nt#n+VO_l){CMsh^L} z5aL@3Iw|o5mgoTmBvnIYd#h!Zg;`!nRy*_5L1}*7dOXc0etb0ZQ9(U!jWbC%PWeg` zsnifI@$FO?38V`j%eYT-n9W^~<>_XKl3@jNdO3c0xyB_6Fb&tRQ-C7dxOh8;5>2uHW>HPYaR4=N-x5L`wj>t>&vBE;o)1leR;>-OXypJyrYagkWX&VDkmyykU=`6en z9{TZu{&t0r8Kg_sSW%#K?06AJn{WMoG`ezh+eG)!@k})a^qsv|VRwct801)7l$z-; z#iTjXe{Gw3iRLnFW<1NZHr`9THQP@;d&%4C8KVRkQH3F&N+!45tQLG_PrC!QX8m;4T0!of|CG0bc<)Wrd0Ji;lLu3A z_9KiSqO~cGYmRq_kKwWYDN_4qGULZOYiA&+rn{UZ!TOdhedgmFA&>j8eS?X=nkmz# zh~$gBF3kw;5Ou+E#LY3hl>~nz!n;Sb0}BY88M?rc8#XWg~HF#!Gi zICST?8o^THpl~pI^{gH!AjU`&hyX}8m`*t@J3MnY`uO2G%bHr&s>xz)8z07x06hu` zYn2Z@Z}cdA(@Kjgbz)M=u z!B@wai>{@m2`mI50KuChyQ9TFA2P>^i$~;k?5HypQt>VSdqp&nu&g28XB_tV2l=el zbl}atBPR?2zM9|-1edo_RK~ptJ`)f7b!@|tEV(vI;yK~?n+R}$pGN!kx{DkB?w$9h zIKGqI!eMb}T6sARp=?suqpv zqACcweIq3N&!wJWCCzRVJkMz0)pP|pvqqGbcr_xVVZNeubMmjPOUl=C#&tA8vQ%*E zm3%nvHp?OStVBw}wMyA?0Q!y_3%6*0DRx+u{}ck04FE0X8dA#&WFEuS_TB8oh-<(? zg7a!bH~?Q0jeo*gLrdI_Q_Q+U2TmtSicd2QJ!Vkhb8L_(^x{zV@ul}`wRj!lO+IcL zHY0^Zy=O?wQ3?I$w|~#3vQw_l1@{mS|9IN~_l4Mkbu{h&y`oeyTl0^g{geTXO&O;m zsd@|d83J4~0<S%nP+NEjTF5O1rmEEdt z>ihphjdNYTJYzLGrnIXq;#YaSH=X*2fPW8#vBgVY$@Y6|t!Z%0H~kj1f57?A8M~i& z?!G%~&dye~vE82D3}kR%qlCi!{|^JBDZOovXA~*j$Ys*Grro;(vXN=VTXspFSA4@_+Bj#X;fLt;&D}}d_#XsUDL%{qyrskb)9lmI zdQ#iKNp6I1a=a7p8Q=H!x4-{c17VmiqF+nUc@@t$s7HEUc-u2kR>WbM_&0%oPX~u;l7H9p>QqU?a=yKB8;;|6K%m-=<*sUo&kF0eZ_Z8UO`9y^X@0QetY$mGA*XM~i9}{8% z%fGUTN(cQu_XY7VGX`6wz$0xnL+YIDbn-)v_Ka8FI8`CY4FUSG@R!VXOzDQt2S=v6 zrCuU5xYXt3iuWOyj05(3lHjeDQ5zB)E= zvUkeZ_fGprutG5r6bSyo(WpWcjD0l`eoXsLdRX!35yka6lN9o8^X6u_RmN1 zE)I8#$RFOzFQ+lQ`Y?P_eMr!9LOd}=cP_Gb9hnEyb3!R_If2VdLic^f!r(qI%3zOz^Y zJs_x-@cDlSm+W0Lt@$%w7HjGGZc4n&q7Bv_G$p|+^`S59jnzo&qtqbw-MS}ay8shX z5($BaN6wkJAi0Ql4*l@mglMlCUe)&{6Tx+OME|g3VOYP{?0#r_z{;$kG>n@HkFS46 zH&s8Esk+9OD|LA6`Rg->fo$EN zYdaB!q(N@usPpb^i=AW86ZS|Gd=)U1U^tHg&sNBA02eom9)6<2{hUAdSs41+_mXE+ zcyyHxE=m=y%YR=Xp)X=RKw63nNLPKn)~j=1?gz5+iPc5vV*f#M?gpX!+9Gq3% zMY9#l<>qc6+T`TTrgIy8Rp?)aW)SSGud;^( zZf~;9U@UV4DCR%WtzUOIOq0H_vAma8&AN&Pdt1#OHn*m4s{dfcCJlpIMIZH_HbiL9 z@nRyWE^MxJ=ZHML4Hq*uLeTN+^$t!V2ctq)5BTYuufJZIlo&lT+S66JfCxchHMHdX znrrO=(kKjpbuWB)`vdPSgILacZ1|IEn zj2}C_aj#vLcviX3_J^dQqDNKzp=|_F`}Hu?Rtf((+YQ;EV_BL^A#!|$j)T6gNe)z5 zEp<{}Ld|Nh?KG9JRE3tSHDl~!-%tB+6oO_%45$vPFvPxz)5fE4>ZRtuO<<@86rMl8 zDNf6p5EtHdLN?R^l6ysXLKX1~J}4L#t-V8bVa0@wFKYyx>KCo7`!0t5-(7$z(H%zL zFz4=|Wny*Xg^VIF@HzRb7kuGlHd!GESpnjFkKUDIgY4LVGJVM3gym`$WOaDABwS)# zZLk(jvo9X-7Jz2A8q7PAZNLao+Uu;@-Vg1`yPk%+9;sXdINn_DOR=#e{BGZ9QQ-(= zJPrNnXS-9zLccbYa2Wdiyc&zUb!PMQ!$9t-j;~QhtqEt#dQY?e(sZ+7O>jTxP_|^GDV$k{g zXAfn0O4xus}E|)a_fv9|uvmn)aBguIhNtaQ1!-=Ux zj}4i`X}=6ruq=T~?aKM&`^(evWiAT!?>JfV@m`kBHs%GK-_L?Z1qMX$Vr4tqMaSa( zuTzJ;PQw;Nv9BEhatgy}8x!z=hHX01{{f51x|RUl8vy$Jz!YXUJ*PGKJNRXFw|Q(3@L=H*~B3 zQCTGG-ku&9RVx|r={6NvtE;E)fwhMW(DB7JjDe$uomm2r>cF-2-qQZ>x8>f~AFhVm zXVC}8e}7F!kE)Bw-7~>{)6equU~!E)@eW1boy2#_64`|~A%|-14!3H-N|nVqytc%i zdD2!0+1+}A_cTIv@P5A#6b)u$=;dtBBnwbIi8lMZJR!CweY~Z0_-PYp2m#jW#17TE zwsz|27^RH_HmOvU_JzpDT$FTdtUHj7@qM8iig;!ZXnE4+RF7(y`&7XX-^%mpNc`c) zC_}NcOWI>zUhwtUsB4Aq0tPKBbZ$Gow-!!T=9SH!`8xO9Q1AN*KPJ>|p)!@Kuk)B0zPs+axSo%abW5MjR+ru*mOw8S?>v%d`ARV*Z z-n_p4n-tTojG9k;ho+M;ycHd7jcx2$d%!TOc)jwi|As=q;w=0e#cZ-JpQGe18mFKR z^^T&4rRf8K#tA%&?W=w#T3yx!Jy+K-e+=1iY{2iA->}3IzO|muT&ETAtOf@32=`r}FJ6T$8-47USDub*iC;q0_MEE>-4X5a#2>M5r?rnrvR}K# z|5W>z>vwBCbm#tB7H*74NmkgO1Jj1Vs|mcn+&y2r;`h6HCH^{53m%N)GE&FG#yY}V zjSsC%rWOD-H|s}yCfU#u>ag@c(^jFqLn*Mw?alGmCEZ+ipj6#^mM-An(<8`9LMYgy za_FP2vV3$FA^9(Jk~RE|i;vXxuv$^;=i8ohT@q#v1Jdt^R>Kbg&vc0=`$I?!hyZ`z zdyLPUnqo3)R{rg^@VW(;p(SrL&fx6~g(Q7o`0gJhaXFQduP!Bx8!qPk1PrpO2I4dt z)E1ZNub5s5=->n8E_R+M8nT$ZJ5_a=#V!h+4@5gkGA1GJ2%$NYS#AZ0Xh5w!p(s)z(I|b;*;xs`VSN!JktylN~v7$PJ z5Lg>kVaO6HsQE3ibiJ?C{cfXvKN{~I5hzkG($?)u_VL*~mMU`NilD~n8yKaN`B-T& z)`@GwKoj`uem)!jzIfHt#ly-M@sFJJT}XmI>zm?fWb|UMQ*?vt2N?Rl4SWyy_V~O` zx}uX0ykU89y^^WGSQhGTGr3sb3r_&M`K83-BA0C05`G#@ZFuYMikk^9epq@(KQJl_B0(@f$!&} zF=t96afv3A=zM7z`9?}+kHNpc?$)oKCiP@u-jswbe_6bP5>@NxfPr{hl7qAC!?4fW zqk+Wqwv?`R-^aL|@zBB0sc}HlSFAkBV)|**@X%RI*>IY0WwCH*x-w&k8lMYsoymh^ zuTd|_b&Sj6t)xf=_UFTy<0{-%@rlrlj-^YCm5YN7FY!Mo^c}ZpoD}&I#9TL$8uQo% zr;KbCR&iw393%8h+9obnDK)rpA44%8?QLp!<~vhxzWScnF>33758aMy_D5n>ES3W$X!oVe!8s_&}$bKIU%NK>&w zYs|iVD}m8G;S*Zi76`T(lvkFMh)3Y+L+cL8+yW!8!1pLtF0kqu2;@=(Atk1X3|F`IKjlOVbm|v zh$$7U-^6#-14 zW@LVFx!n7d`u%gee#BP(Wrt;Ec|*p$K*Y{|jaVt;H`H0G@Q#HzP4h=rtKqPcW#i#z z9Y)YW9BhMY!G52S7BdSF&Dn8kBy z3RZC>hJ8*q*L)itlX-7@t|ZRPz4vvi*viXf?`Ak0_0v=Y0(6==kR&joy-tw?Ly*{6 z4q23`oxcG4=(oM9l`Zm2GhBGXdm1io)*CNoqCQcvGL%L)Hbk65XdIT4n-24KH)fGe zre$4S)!f9Kh4`v+D2+6#*Ow+NmSFmn07A02GW>|&(p3?NOtcLFs{$uE3uy=nOs+b& zi|=u&D)(w920vKz-96wBGZ}hdLCyRk;`E_ihjQ?_o#2@0}a4 zj1$JD1bzv9`V_|ZYrXK(r*OVZ`^L;tS5}we^c8BVBL1;V`M2!5K-!jAAuILiJUe)D z>e@;a_tKd$wkT*b@Q5FP(C~pX#>bi%WZhrKhk4Rn%C*csRHMZ0)#T+hkTOEW%mV*@ zZd&X9Y>0^Sz`mUG>vBq>$(4S-@#DQrW?CTkrO1sHX%#xX}-r*T;`>6)F=!%qc^7T5Oa&cTEGxhgm1qZWckyc4S^dJu<* z+t{An5)G+1xj^I1=!eU zd`dfiS^w6t*ML402b^ZlJk@!8j-8s)o^5>{8a>rrPE}J{h$QC6A^~FOXb*UDo@(2; z)LT$vmdMEi*Y^1IF0isFh{htthW)wJrpU^ii3nI-kOkayb3F|jyw}iss0NibIi5xj zo6IVSk+jtO+*N@f=+FsPU!E8=)2YDbe<^2J`H$jhU;8>FTw075CjDU5)7g27cFyLp z&p9vdeUn*Nw9h@h0ZfAz%rk3Li?qu$jAh7&vt`H=$MDN8^v@N1VylKnH3N&3wyw=q zl?o1?#|n@~zLu0Z;7XzdN2w->WSKp*`I&Abae5|1Qd_h!HVwc?zRL4A97%&mM1ODhYD9}A_%!XK`yK7ryWs-47j+8vrgV-k3rIay=IcUJ_ z_gDH;ksBT}zs{yx)hO`5?#|!oO%szKh}SF(5guN#Q*03~o`N3;d+aGrS=f66VnZ*%HnDw$5tQ zb^YTu#kT6*%*aL^y?vWh>GuH96|(Oi9yNJ};m5|LpK6I&znjy!}K%h(z@5L>_}|tvw0}$3RlHwQkY)ZeIq*_^v#d%;~KFy(?THc zDW+KJns$}n=7_+-kdhSLBg}c`_5o5ds#Uy9o1bb*w$P+p;K=MhkT6dE!Bbov7vrHd z=)T<>saP#P!|k;ccjuj0IXGi%k}vH&h3TCvrGMZHMxrVEt|m;3K9M#})YDv6@v%Pw z^+|$D`}B*jd=VS3VZRpG`S^6Y@602lPeAM&4Yde8!!WdvuTSutQ{Mang1Gm^eh^&tRnv)xS00D{uv42R;)HJ8WrAE zSn_RrF|B)ug&t0@L7qcycvQ$R#C`l69Cb!qjeN7(Em=(urWRkjj@Eye->PZ@7*IXM zBf0ZF#LBug|2AsJ&C#>B0yAk~=Pg_qQL%7ILV7d z6o>1^SGd@$<~gi#RRf<;Bhk3Vvuli2IbOitAF|bpb;#p@y~jWhHERhmaI?k()=nP3 zQ(Yie6_?XTV}>(dznt!N_+WWe1I3XWb;o%SgH1pIf#uybFwN_>kmm?7QHyW0EZ;N;}zgFieg2Ny)3G;;K7d2AG+ zht^z|wf}2gv^W4*YBJW?8DM`PzVcAcVA=829s5qQ`?vpyi8@*PJ+#Xw z7-Qw_njtB{}0@bVGW~u_q>0wlUr&4W+RJZd!C0OI~_ZRM>lCHE|fA?RKX20m_6(GU^C$e$~m8pJDE1Blp+QB5}}yu%D0!wxz1Wv2zFGJ&OKekefu* zh&1%*1j1)hS~n%RSKGe4)_-&K3rae-eE79gTPZ7{e4GRD4VV4sdXwllPN;TqjdRGV zctMT^n@tv~1V5m)vIKF}ELaKF8r!{d_>y1YbaC%7i?TWP_$q2+z0q(Fcto_}TJ(|0 z>x+Llh%HeTc$O3eu`})|u3>iw#%Z8l`E7X0gv+@+O|^Z!xAR$h?zNSUg#s8BrKu@%AVl2!yl{x&I^4wXL;5w`5*$(n!DmuLBc1Kzvb`wLy8P~>qv0BxxovE;; z+37*Ie=F^v&vC5yN{HOvvsIBm{NJU#6bJaFm!vMi=LApr-ANCBTt}%J0C{C&SIgwS=~vycHQyxU6=j=SU-3;QvZOSglj>k zch@yMcQX?hs}E)=DispynbgJ}ozGv*I3Yi_VDq@SQrB;5P<|%2O<_h?*k1Y{Rcpww z@B#E_G1O4~Wh`a7Y}7T$ae`>F$i*yOSR3WDXVL4fz@b1(`>3Q3Nz7hv@r@O`@bm~= zSlg;!U4<_0F*&jmt?X(?-JZh#PEeXtd|(MIZLu#PC(`DkL*-ixQYW@7P0d*xVn>H= zh&S~_QoFejPj)>mSDsyY=w2otM5{rT$3PjfJn+-h;bX=lT~u@IAn}G)97o39qH4JO zy(az;s+C#E=u$NnERw)4Y!tg{f-6$)eAFq?60zAJIs1>Yw;R=K>A7{~t3bv)S{;Sn zjySh%(owhLh!nIQHvEp-yQO0zYpRWV z7?1VZMglCGPpwpmtzuT&i-S!vO%y{iRPS)0Typ95nU4Ag?_gk6={u$^qA{+sWUUGnIOB&mI%%%r{Ez9im1iu4LCevQ`tkqo(nLgY6}m zJ=fk-iLpHv=qqhzevT^Mo|b(-cJPDc z-@iyyDyb#(*Y5Z>NJcBPiGj7W58<;Wy`+quZtg<5^eut(lCv$&+a6k_(fQ-+AimR! zwZg?Hn=z_)iO|-nnU?rFb;DdMGph(Ag=}FxG61WHascfSW%rx&UcWa-SuIoNPjyWl zQ>uCxIVNPG7`XBK#|A4e&pRo;#%;Nvrmc_{k9wsBo0pwyCYHZHRD!fCsDJIYTWWHi zbQcKb`Om5FN)z_wuUNI8`X=Dq&!h9{gficNe#GZ&qo;}eFp`oSUUAn?y;$ZNvBK7v zmTYF^SJtztA;n2as&Hn=JMz_Mr%8que+S)uBcP2Rg)e*?eFHMD~2s(*G?E% zD$_(Zc+>yn-ep@rH$zkxTQ2MsS645z)g%-~Z7uEP;H*#Rv!3|%nV{|C?Mgu=iVVre z7p$j!1*!X-~Gt8L@}kE51Qk^gp>#a8fu@vHgn&NM?OMF?)w|Sd(4EY z*~@Y>2c2*0>VImV$QM&FChGlQTB>*Kb~Xsj=cA_a8R7?oV>cD(2}@Rqv(?!d{BplS@~a9AHa{lDzJ6Mj;7xKimI6hv z6udiIJ6fnExg=N>c6;74R@WFgs4)7hoS}8dBVWUp^G&0azdttN0gc@QXOQ2H#vQb- zXBVxCV&Q_5+)#&#y*GcH2E3JI)z(@O@WJGO65be?<)z~!e%UWhPO$q-RoC9}5#+6~Rj1b2tg$|I4paEf0*?usT%Cz(v8hVTFKWp*HrZAY^)9f37+V2PJy4{ z_-ZuG?d@j?!PJuVJs4HV_G)|D0{;j*E%3OgUyCsE8uRJXXc@NEnNKZp!9I=jP%{tc zWSy02rsJZPJH170K6go6K1JjelT&(PwxD(MU6@A2CU68yf?WMwQr`}>JlnLgVV-~M=iKYH3EH~)-BlNA%7EOwOjMyw~MmdCk5gq@ls33e?WroRa|s_JD%$Yb}yvIS#5c!N?Pe zPXUJE9$7Z8C$nV@U3ATPY%Dl;+?2+sT>H0`ty%xTEM0$5reIE1VgWr8mlr3e@2%S^ zd%C;D1K*ymRE~0slnE#t;v3eV$5?VEXXc2;Ca2P_K{Z!y(erlK>C#064^&q>uz;M; zI_gl%!Di-~SfQw9UtoeZXG^P4GB+hGYApW4Bl(wH64uHxPSAoK@f%vHy<=78Y=1E^ z?3Be_(}tU%A&$ml+5KN?V9f^p)MB!?={gLFaE>$#LEFz`q?#OPy8n ze#850;7L{nm~&mV#0%Cbl&Q5v@21v}&g92H=>Efll?R@jYWGxQqefP?*LY~y?;fR< z*OyC{uLQ3Hlrut2N20b-qw-~s8I$Kq`nuS9>UwdDs-J{}o?dOsqe~v0O&_^8S=~SX zgrzN1r>wJOxcKBykX=wG8ev)k2YU~V2A3Nc32<4FO4y$C^40h71 zBCX;6{-%VOck;A8hhns{@c(-=Zs+Q4&?O#{fF)ux0S?N!`|q^!!IXy!Ol2!VY-dh) z*w7{LPeiVXZ9I^6R07l%-k@c$NKcVHW8VercOG9%XO}$k8M*5k^r`5$zmB=lHgaWj z`2{^Wn`f~e(J}S^^5i5+eJ8J%4*3r5@hB%6j!itPv@3dzp>`p`!!Eto4FQTb+bW+| z{dMj%mSB$3p`n*J(S4sQYwOaOPscPg4mEhY_bSuNwuUFan<=AdJl@bkKjJbpS-6D& zeS5RrR208;xdd-j_NI$Xa4j7TeO@V`dHf5H{CB}KyiLo@(3T0~VQD+8h4ZSP!OV;g zMbo}-N$DkM^lbi#NV1gNay!XZv3#uY_pk6?59_C|iutTL0XGHndb#n*GC7JC-?|i9F z-yivt?UMJTj;Z+7dhER!5&s`i*Bwvw_y5VbC9bTjx5~^Y+3rPz>^(wJh>V1Lt&)*_ z!*#8UM6yC56d@TO+-q-Tgh+g$Ocu|fWU7^@a6h!xE)0E=@~WRP=0#WbE55TZ;I07uyw@-yCp@o? z)@RGfX!zn|nnSfpjbsAv{iu51D~ExWS@hC^+H_dc$fL6z`%tH1gqlK2oE5QcapsXDlPOyg|IwLK^OmK^2N@A_ z*O792Y&Kgxv?vL_Tay!zuuvJQ^aXJDQk1JUU0QY4IQ1=&v zhf5L?$w4olwI>QTvUCEK~5h<$9dZz(856P+g5rrP_m zkS{RN@dFLN{c5qW4!D0&YX%Kli#+6yO;B!r9t~YSgnLwKNaFOz5ntp-^&(M|zoe93 zu9ViQHGgsOYqEcNNpU`JA!|gAM!gQOK?L~>O}E%CLt^1IJpabC^5wJtSXXq=o1ojr zcF}%>0a?u_9Oq+cYhb&qd-3+(wGxY!N1ZX?>^m+LKX28*!&~U}^)}dqITQh9{1*gV zM1bmc0>79)bOQJ4f>qU9#y-`@*Cy&UC2tFI&&>SiGdoh!&@YNa60hK%>UEfN6-)_p z&)CWO;Me*-{8%E!ai4sp40pa$;z%(NHGo?rIebR;%yeJnPnG0k~^~@d+T((Yz5uh@Qs;M+_&y* z?e@gxOX^=fpR|Yrx#^D2%Pu+IimtpJCfM?HMTy}|L zEPKjG%W=u$xe;sI#{sU2ORI(c>2E9U;NS6@K8w1)5!2$HxSA% z_XX#35OVXE?|pP zvB$h#`8xq$kPhAKhhoGC7ir3`g<4SPZ4uZk_2kuVJhK)qMd>r;;#_o~EBZ%uKCnj8 z6y`HNxuAuZXTwe7aow-;;^|g*HqN=eZ3$55TATNL;#b*y{V`Zr2z2vJ>6&R~I>=wY z)GwmBoRN3jwJfVHLG)g9R7A}=3w2A22ekoWSLf@)R>>MaGC0oeOuS^K2>@tzLyen@ zqJqZvX;BkHy=BqG=%l>tcUo)nQ)wL^U*f*lH_tk!JTK2A#pF2-3L|xqnkN=Vd!$zd zs1TnRd3~PIxt}h-e^kBTaOtWLuHxPCh-$Llhuj9yMVpANs+hFKbenO(umc(@%*(_>{Z;gR{AY)t)geHyxwl}d|wbo-jsU4|2gWxvP%wgn`-+2 zu?&=SwAaUUXprNUG_v%vuXK3~B6p3w5zBnB9ZYT>E^B6*YGRXz24g-HjfnH6t}3&) z;0Cdir(3n}2<5jvNm-lIG~5a`49J~!_aC;NNl_U1sJ{O^Ki0gBb^UpmsFiqMljIMU zSH9~l!02+HxSNKCe-!5%K7F`&-CcPvXXd7{rn)FSLE%j7m1wu5CmJj5T7-)?>O?9# z<+BGHp~w@#V@tV9dyLq&l>ds%%S10?B`8h$LgQA zy~+G^(XRV7F!*tWTAjFQGU(f`N<1e-{OWmEptYiJuudvRB8qpdeMOG5I7FY= z9;L*Xu%8mY&ZG8TVdP!uSI!bGKCf`|xlcvvoqePGreNW(*$_l6 z8)XN5Hv)qyx1vsy>Z9`Nnk#zAK_?4`7)}WY&Z@Dg6^hriGq>%sNTyKZ9{j8dslKiC z!HRYJiKx<#wfrDyjrL<5cNqu)(Q5W>Cb<5>f$!%)H#h6he*ugJQ=?0z@!}qZJj<(n zA{ED5L7z?@>uh9KQ-xH?*TW zn^dJF*kghm`-iS}bPt$~eq2FmXsNJ=hktvnhb=dZm{f!d)?>#84US_j9;4M_j9;Ab8&^yX%R?BE#4{c)uUJL1 zS_ySW%`T~3B58Nlj>a)rMt;}6pT`;-|LiDB8LEbg$p2ijV$(s#+v}U#=ctYs8);p-Qy2bX`CGwoUreL2odVJoLFy7t&(Ec2NqY&?A!qNt zKU_g5%b=}%#>~!j*X=HN;mZwG&(FsCtCo&r+Zjk9joTW zdEjB*r0t|%m-Y@h+W|Kx?ddYYjs}{|JBVZ;}iND+(~yUd3(}yh0e9h)b;T=zr0) zk3RwZ-ul9mbUrXkrQ)Zwny16*0;MeHYmb%1&(&~${T+zwK5=3w`!)s2ybx9XI#-vAvnn^vLPP`8T)mLnml4 zMMhX~?bk!9vp(e^uX{td`ZMMMrQwA1JJ;Rr{Vs6asr{^mcf9OBTx}UC)QK>%pH0m2 zXs}{0PS@=`w0r$)ncawZgi!Sfl1meoP;pd6TDVK}C_qSS7aI>Krd?)T{!)Na-XU~08)(CcKOt~QNof(bH z{k=PXn-WSk#U`M%mR^~hSnN0YQKnoV7E-12URp#(cLUYVC~*3}sw;PuiW=zbqQ7@W z^Wn;5W#%)7A+~^gNh3_pfDEc3AZY(xi;B?$(*~8BM$UxP(`E|&_kMlCHEw^91ousQ zV!~Yr#+sSrr?D)C$Vm9S{eJzzk|h3^6i+0$RayC%OZFWkcvLJ}bP;P`6a)F%kG6f! z*E)K-y3-$YG(t|z zDWfIuq9&5O+>RE1(X1*}vwX{XEc7Rf!lvO_eB0IYgL#yZ#1bD{w!!!#eXpGcEemR$ z=699_vK>4u4ZlCjZ~9t#k8|o5fdKlzA2@}5T{2s(x)`rJxx09eGwtVg3-Cx{$Xi!7 zaZ+BaD(Hc|1BAa860r3t>tla2T)AQ1lW;7a5jhY$xoRnbvPuruuStyz4-eD3wOvb% zHC`>86nnH}=%Is|+=)+A;dv*jJ#DpB{Bz_`K=VSH%9Km|_|X#rB6){tM1T1x?fm|w z@;ZpUDd%QeXb01ee&b95oc-s$rq;V+ov%akBYXD-f2DJsAJQEDKl36*!tRudS_{l1)Y%A?S>U&Hs6=hr;9 z`psS>#xtGx^_#od>VxO+dla~FHbhH}de@CP;b5=p9f^@ohaCNrZ*hLAJ;rVme!V2G z@e}JB#q#Uos@rE&ahi0T)UV%f?d+632$ryRk4ao6vXQj+XD<`g`nJaQRi^HI8nBiR z{;_je(2i6~3Hd)kl`kA#OK>A(vFL(>!tjdA{M4+6*Pgk$E}s>=gbuUX6nnQNB?gM* z#}aWZ0{9ZwEBbWes1G%>9JNoPO0S&bu9Uf{RTW2x(Wh)v<;$NKoJZrG{S6 z*~mF|nYWoKZYn7}7|fZ8Q76Or%P+o8JouEJ!qBF=aKtiKtHaFTm(K(In=Et)zY!}U zI>94BL%VNMK6I{~7WKvF<=)xvTR{>_ucYYS#rGgl!V3CvZ0@}mPEDH6+COlAmBt1N zG8t$0Aw@3$GL9#N-N(%tTX$sBqVf>(S?*;c9W&H1%sbZ-p&MP# zoQ^`~ocU@IbdBrjtfZoA534GKj2wLPNPVJGH^JR~VWd(|zZ^K!?4JK}F_GVG=81Pg zo92JNb>qQcc`$93$Wom@T6_K9|Gwr5$@=7wp zO!}3W@H=AGLs(?XNAKQfUajNJiEWbMv+XO(G?=-#*(Y;W0%Ac8ghw1bpsDWVtR377 zrdj{WN~Xm)QwUe>&e#5@39Q|%Ep(vSIU)SvVJ`}lkQV3|k-Wi%@TW8vi%mE&ClvCx zD^1A~qIZz~oT{PCn9*5zt(l{VQWEqwHi#hugP6SXJH7Qo7XEaO&AkPE@3bRa^H`)_ zY6s;^2nT3A`pt%DXwuH)uOhEKQR$ld`()#+9jXLyG-A^TV&V~>rQj91CpaLG}rQISMkO}I{&&rqhbTFaz}!`x3C7F z^k7O$tTE_Why*+2g08OkqUW$Ijm}~-T|6<(9k*;6Mk1L9n$1Pqrgi7 zflhydn2* z@kCkNzN4wPomck-k0`2tj|zCX7=fJZ(*O$Q8)!fyJun8?%JFZiXmmFi3zH2@^e(@9 z`qea0^Oq~n(E?LAFb5Ow;fm7$W+>G@dLT*AG+Q}{2?K%J3+(pkekQ|@stxzhW`L}2y z6uKEOk0nT!bpByA>~rplEymuMOYd=`1e-nBun;LZ0JK%{+~xV-k`H&T)1sXo`5q`D zyoUzoxj+TAn-bmis{e!$2JQ{|d@oo=;Rfy`Uq1S;W0DUI6~;+#RUi*fQ z^c{;B&%sZqey^^bL6!f_#fQir$US;Rkq9kwjwc2D*fbG8_X zTPlTZ3GAnCwrPrP{8YSm30?ffzq)UB6?@E6utwVud36^;{9{bgYi65sUA*Tz@K1n; z`{O20r3p8V7-!xDFV=D9O#MSPU^g{HKQ6jy@$O+p6#pL|1wKrA{U12zYKsbg{PC~h zQM9Szn#XrJix(3369A2WRDbRUS4m2u!*s}e-9x*70xU*pebE5NR2B|x+kZVIn9|kj ziN2Rhnff2^1kA=KGyLOcgT%bu?|+#CUaDtEXn(_rMfS*&|F^&Qr%9QiF>Wwl|E!63 z8)2=pJieIjaJDw;^&hW9(Uw?@{;_nG6D{XjruvsPPf%s3Seq~B!B&zBzKH(yHk2{s z&LJ1Nx2I#h&$x-!CjAxm1+0w$a&zC+iz7|;YVqH&0A^554Aw{bXSQVM{Qs<2q4VDh zZ_M5KkOY7D$4cB7I$va;i%k%J-gWAKYHJi4NX9BnZ2hv=w=3H>r4A|JAHntQwDe1U-ge)0yBI_^glG@ICqCP zN#KvG15Zj!>CV*Pp-uH8FqwZC^|9UL+x<+4Qsw-cf1+t=LqEHb(c*H4cZRo+{eKU3 zn0%{K>iu{s@8{jAZx*Ju3kI?9l)9PP^gdIE~R1U_ zC1(-Dl}_00lo=r7oGqk!f#{YDXJu$f*;-#dL4xKmU^b{M3$x7U%s$qOqF`x$r(jk4ybIvP-I2?#W6Z$4jD9lG@wu`Vb)nB2kaRcJ|`aSUI6-LmAt2=QQbGY|r`^hSK|%(`FS< zo*@}_r)S;x= zbo)?x#+esQInvqZ#A(|kLuc%KL$lgI>*ib)Y$^UvQLoauquy$QHeCdgyU;sx+D$*H z#ctR51?;rjPq&4h`#98Q>T?qGR)>ZUDD<*-bzn_MaN0A+fI?*ji!k-H9V4e(VeIVA z86>y3pqK~#e(A$*G}Ofo+L*)nMYcg6*^39U>j`Ru8oJ^=TaAcu$Zb|_$0n<45(=C^ zjAYfi7NYK~+-Oe@ot*&Rh9==*b500q7jI~*lv9injWs0 zVB(+2mEyT~GlH1Dg4T2hagwQIK8!3K1YuF+NRAZOn^edDFXeXUNc+d zfX-B~RnZ1sJw9Q%VZL{VBng=z;aHqsdq>4kvGT#PSXp`c2yUUXU|?u?#n-N(=ao^? zX(L((R0yaDCUr1OwO$B_$OIhCNbAzg&aQ=wxxpp$kl6(BVSLS!$K%K-{aR`S?cT<# z*VmPby>A|5a3ffLhx2XPwUi7Gy^bJ>aef>zW>cLXy?s{(0edcB`+F`C@w>DnwWIA& zcz9LLJV+RZpT!=0=jhR???rXx!oqQA)cWJ=j`j+I2^&~a-@mPSnXmE(Lmjfqi(Z(J z86J*x@Zz|n3pQ(Z4Z9?+Tx)r}c{qW= zU;$+s-cy#gHUIlS`?zkAg(0%>3f%H9Du(^(EI!pIcR|9n{>9+Xn%;R|4fOKhY|Sd(kVr-4eG z0B@!Hat@$i;3okTSYVAM%yL)OpW}~?m3lY{9ypGXfLb=bb#fTBHy;>526so$R<{5p zNx%%6t9Fo@7K!yP5r*1*Ghy1zp;xkXO5z#4`-j?c)G(M5m2_34K^bVQcMt`yL&Z?B z;$pyd>BV<1D)id7&b5~U<#V7DCE#m{B&BUx0X5B`vN(~A2>kY=6*}){usBuQa=Iqp zTX5HlHh?U{edg3$f8#l8YyZ}VW}Y-aB4*Hdd#|*Y$Cp*#VM$>mTWGlbtzh_AxZ#>* zR*M)NE6LmpKsz$hpJTIb&DaT@25q>fcN@;A-JQIgrPu@Z79#?0;Oj~tc$dO68bt8p z=`-If2#Va^ny|{~VKE!KQ6_`jla5p*flJVw*w;_lsl{z%d$%%i(Srw;wmAK5tJ;Ye zpl(W(2wspmpwXGNG4NXvOk>nPC3v^od0Qh4gb@Y`utVqC$&kH~hqxFRtc}GeEDDps z3oy*-kUu7g^j`Yj3sFG9pAu$IeG zgJ;1hG{!Kw4ZZM)+Z%HubT(Zoh8z<(!1?0~EJ9PvOd(58A|DPL+$#OmmRXokoC_RJ zZaVks;b}#9OH=`37C&Vi6mUMlttua<;U{uXsH=9xfwmKOWtio`8RBth_g*>eFUEk+ zC!sdq)8F}GoE!#Lvg9milREA9Yr*EB}bMq%HWmq^x%MREpwlakJ{ zDBq-GIa6Q3(Kq!~C)>JdrKe0 z>aISj_pVzcaq>pwqt6eW|LI=uB;YBdb^e24$)m=9cLB10Wq#g!{5g%_9k?orMmM3( z*h6Oc*f1;yH#CaA@>br}t2I;bS7sBc#12ZD@ekjLLlEVgPp&~EZN-ImE*I?rY-1>m#A*SKP?GaNW@iq`(IJCW+% zyRJldRC5$}=K}{PJID#Pn^CQxaqBJH6dH1oKAFSpbL^IC`i8iu1oCAFFq}Dt;d-Cb zNtA$oGZpFyZp)3mTtdpw6!SIK?0c^N_B7H zh=)2Qni|kRGOzu*FfePm(~Rn-228z%RTo!_>0lxs%sQ*N)uT3@@O1k~I5wRQ+V%Ea zL=^W0IqQQ09jnjVV3Ph#w)lIl9khTz@RG)*S|Qt z0OZ61IX0>WS6{!~UAe3ZzSWf^sA*mLpu@9YOghw{L zOc_mu&?~D!hH~)GQtV_4L!|pj1_?qZ1%gDf?P)_xp!xLSRV~N~5ldfA@l4xs04*sJ zz+mcQg|rNw#Hzlh2ByJ<*kQ0wSkIol{^9jee4c4jbF@n=|H;LoXmj?@FiA2<9c?9R z38QE%A1vsGxjV8G78ZiIH!kqj22pgg+}-bC1|p&0SJ5#E@2F$^0H39~5DyD8u}kdm zq7oQPCcYz)@%{+)2Mr?dKl(0^fcV|0UiP>ycvgr1J{kG(_|36xUo5Xf!SnW`?s)Rb z+f<4>bK>*DnnDXJT?Ba+56iL1T2qoGvakt(1JWg*sKc=43aEB>?^Hh0%G2-HGl5KR z5kqSe`=;#%RA}%mTr&w5B~wppWd-^A1~K&lc1Qtj`rN_J1{-^F9c|mFsqUrMx7j|z zq)H*93pX@$PDa$&zJ5thG_jwVxhU?J8k690+bH|b1CM*BjmfzOdmenrb~={~O+2@L zss|IysioRbM1aNDNsaiWFX#j8=GJYaG%!#|iV*N^pevVphJu_P0pno*@J{ z(d__ndZ3CL2EQw39WXHhs)!fys$b>FI1V$)0ZKZ?hTu_H&qI*QDd5Wi>dEA%im0rk zEcli)MBXE7eaON6ahu9sm{ZKnp(^jSZ!xm*zFl_DBBmT6+B1`zHd*hUe5?MxMwmm3 zCY-AAszB|1F9PSl%&@ZL^YIyksTvGMH^QjBcmU zlFJ1^m{5u>BK0rNJ%9%~Ng$PsJ|cXV#DLiuU}pN{X0BXpLmO&QnL!jBxFdLVX0Exg znj*f`x!FVHB6G~lYejb*(8+4*~T2C7QS@VPK3+$?jiq|PE zEW*KXFX^ky4@W8ze zQWUq~`7S#sg%SBo{L2TPBOd~i`?&wCk7~? zPw06i^8yx_02XK$j6l_ODKBISm+Wq{1{^rOILXk~%Pvd+bdTmFFKZcwH}``e5d z?=u=nR*5s^VOW>R_HRaGtYp1lkXMvYg6l##wR^#0go67lVocC+31qa<{VnD1^Vr72 zKwtaas|U^D@sNcmc*&Y4l+=H%|D<26X;dFb0iZedkOL>e_Ie@Elp~bPe>r$LctM4Ra6F_XN-Y_28f@j*o5!gK7r&* ztyCaz=xP!!ob>(buBya=L*v!CJQkM%2Ml5w1Jl6X+L}T;2HFN(DHK0vm(dgL1%Mlk zcxk6{tNYv`c&t2ga9T~MuF|CcbQ2gKil?HoEY^i+rngRAXg=0BdQ4;d`%A#jvC+5$6qoF3+>tGUwZ-n|0 zg7WIY|BS(o#e*>$M;c)*;32gH(2S#Z1Zj@oTM~fZctf{Boq7(K7gdvLX87Y$RB!D% ztXfR?@U6UQ<53lWL=sI5y)J&fmP3ZbL56wZ<1VuBOYG#(?IWBE|51|5Wl-7?J{ng&jz;`kl6^!0 zb4IvC&*t%;o$M#HybSXqSD4H5j%!O-xZT54iL;3DCSi~Ien6SmyJDJ|7 zUiyHMx0QfaDP)g4@buzScEDFtHRTY|v)&tX<+hg}ox%d1Nr4DrV}4fS+`+EYdo8Xr zPJG(vhGK$oCCO=+*FFeyWGa!m-!{+T*}vv+m1J&C`Re zJ!wyj_om+%1rekZ)II&D2CxHJbO)M?5BQ=pj+V2tJ>HDc^#C=|!uO@9%h;*=Y0~iF z?d`qqOl*tg*_<969vn1e;Zp33e~dOs!;<$QK+muo`^Wl4$I-1TLK1{QF4E{OQMC2I z$>zl;X@OK{3)}7{cj3_lO2FPS!sAGTw3EI^2sr&9Ch%7)vGbO`eE+KEPI8vB8StPo z#*-kHYQD{E2B%9jY327j5?^30vy>dy_hZih1qvu7Dq#AmXAe#udz)vkXzU(iZ+z~{Q7O3OCM>xVsUHZPKZ3I>1VE#V`Ud7<9O86ojdO` zyRqwW5h3S18!OH&iqj+G-*nmF6rcG#T*I8c>ZH))A)kw+dFb@zO~QNkx>)`NEV%iu zJUc@C%`ka_o>0@{h$9t@>dP-!bzjUgaC+gmhRMlyjUw1QeIs{^Zeu)Y@44c-$=`hb6Hc|OSg1P+&OXc2|8T1)}d

t~vrL;M0*|CAOLQ(MZcZfe1od7e3LykSC{aYtoBh)2de?mt?j0Y?6so(> zo#C|}@18@)Ww$8dvqBGqOP)%kt(PwH?<9Q%d_TsBr`7lMPrq&b*2*fl$JG&78_-vx{GmKnc_`L~sqtYMa-cpk@Zt&g4t5y~ zhk_a%tO2%x>efbBjcz||dX3Mi7pwUT_k)AP0sHd2 zaQX|He3$P}hw3XM;hdZsuMJbJI}1|3S>=zgHFa4VF{LT5GWogFbOp-_yyw1)$J?LNqpWdqH4xH_M{f8 zeGpVAAm-q@(){b#=Z|hmzX#dt7(Tq)LfDJLgTxJVK;3O3HP*A6r&BhNnk<}$#pITp zDg4$+%SHd74}?>H4BnO1nx&-g{21_(u8@G5Zeq&-yNf&l)Oc1rP%iYy;QafAYGeNx z;JamzXVq5PEm>B*xtyozIB+A)yriQwk{1`{!?6$EqwX`9GMVy^D1#(j4)sOpuG}TG z(|W?AhLC1dryDdUETGjYya%M4N{ee3q4^s3ZYAs!Gd{1In6qAaFNNTJk)9b)3&uW} zmyti@uwx&FeK3teB?TE;vd9p}!GbBxA8!wQ3X(nR;M@-wEKgXyb~&JIn$G$DVG)o@ zHLLE}DQwh651voK0<_2ukaMNH>zb4r;BZUbjBtqudMcS7-$Fr5jP~wF=hNO#2*X;j zLAy8e6_DeIJuE#aW)BmaUB?!E8dEm3fF767AsZ{>7osGlx;gPJ$B3%JFIdY;ADjm| zY;S`3Qg#VwFqQneG?*3qB95l^COUt2Q}i~RfM?7cv)LE+p#RQ7R&EB;zYaNgYj-tT z9>=#N%^aH@R%*j)YUA&*0i3Q4^@W#RQ=4jD5F{UL&CR5xUE;%!`)%W3m;*Ehnc~2$ zFgxftHR7{^R*zY)(xaMbHjo>gBDc>aa5=hzD(~*hu_Q*b$r`Ws{FC>(LG~_*lvUIH zd~DRXS6@N`*%hje=@FeHmYeI$6xopzzTCPYm-TY-jI9s{kYf{X&D8C=nvX?qj%Uq` zgZ%XYw&+rt#o$kiPIg|9UUQ(R-&fVudk&Z0r(+=iMqD7OBGG>71aE4V8*n*N(8PP> zF`MsBrYH1vfvJX$N=t*AG}<{xP%hU03AD6?njkq@^u~!?t4pO7$xS*xcmIKN+Icwr z&GBkc-P7Eld=GKpKH|fZFV>z?yYdtQmVPueuStH@_w&RFpSDmeM;~e!fNYhEux28l z`n8@se-D(?`2>JE(#jw=#A3d-(C`#@T>v)zJVc$j{ykw~QDg@#9?a|j(q52}H&lx8 z?CF<#ZGi-+{4h#@S9dR}1?#~)3DAS5*dnIEv8`)kgWuq9X;I~#N&!NB4EyN!j%6|u z$Z#&4K&Ni7djSMnx?Tv?z9ywS=nePH0y`TWotyI2ypGwH6uq6MlQA6+!VB&V`8fZgpm#OGuRgDrK@aQ-kA<$$oBw zu_A)J|N8K_ZnUB)DDRX&o?GOvo8^%5pEnylvWr7YPAy|v%(@dOzE(kaUII4~ynw3j zk`$`M0TSqCI+?6~r4q)KBE!{Zx#t*aW4r99Imxv#v^ZaU_>an)5i*}4n~rxtHlo}h zW6It&gDkH*0M1>qi5M*Z2(}qE%SQ3b_>PEGzz9>x?)6J!J14TC5QBa8f z*N|viVog9LJ_irR?jc_!nq(GfZjEC=C zI*!j%YYq(pD{M?!73*gKZ2{H3K2qFIuCZ&XLiKkb$T~ZzaRTO+FP?5~OE4G80=cko zAoeJ)-x6V4T#8-d0VYtSjnNKV>%aM|zsoeug$-jhfEu2j@U1zOt-c8C3qW!>f#LU$ zpUmds5(T`DQ{rUf9BwtT?Voz_`4$a6?-1BRo}1!4_ia|g2PKD)&w)C{blR_|Pe1Vl z&M9xB=_6|t*W;(+IV78*Ia&@}G@hTEx99fFB<2=o7#yN&`1n>w=aW{)vKMeI7m3Zq zKP`F(nM?}_KpzRZR3PuJp3a};j&I|e-GX_{MK&fwotL#-VaLHni|mezI^^-BeQTNx z-(pV8`L(e%-uWar;0C~5k3t!qb7bxSR%c0(Xglbs6x~(zeF#vL;k(Jf63N|%lgOeanFlPp;nVSarPg^~}vu>vzJh1Ito2meruX(=^hvCap z80}vdS_HL}8!Fmagx7fhsOki*9J$Xnm(W>v5ypNIP&)TxPIH^BEtf>t9`msHxP+Bt z7*v*=IW>tgXwxtXIHcjsABIiuLsg>lkypLpaWqhpO1jO(^DY2#X*&&n+m9kThK-Ev z`xFTsCYtchB+P}ZQS>l)eubwkz!o_-`&QezNBS3n?7fo$C!nPiyR;j1x_w3$z?_BH zqM#d$L+JeO=SKjpMh$>fhp57own*S?Ey(Q`wR!x#B;eR+eh5&l$!@a8EuE_>bQ44a z3-T#+e&m}~PD>U5I8$J>y_z&g^4AEoob$Y3TO+9TF|jwQ*SP>oB~*ZWsZoAfgB|L* z4=bcBa;@<3NU0eM2X48UH`=g|QncZ#*Y*3C4)fqawx3ukwA5b!`;W2a(Apd&O#`Rb zZysWSB1{sSqYd*dks$hYH1l>a*O|35M8&fKB#s+5Uc7v(SvI*A$0 z_IoX%zYXBS*I36D@1+4a8cohLN!?AICGM>7riuWAUXVlnIO27EwdBM$jJ`1-6JtN* z%;ru^d>Rvf2TlYgP{YTp>>JrM9|G$ZqQ&LQ{pupgTfF(iS%?6XPNIB1X#+Uy8pcu0 z*55;BM0%nzI-rs~cdR=;r<_&Thwv?#GfhM9=7;4XGVWWanZ5(YVGoTR zc2&-H_%g-^C!azzh;bckUf=S&LVsEgts5M;cU{3E2FRQPP?i}A3_QxlUV6l5dAJPV zVrqEx^O7`7gVJuX8NmIgGeLo?msUOwun#=1lHky4<3OZcwZ_hMnCS1I0U!usd$lok zK)EpZYzI(Z5^!V*kNv9;!+0+vR9P7rjQLUrB%7CuS1(o9)za>xH&THyTRF`A%F37p z;>$PGKfRD(XgkW>i`AS3f(VA+W`RJtQM+@!VK_~-{)#RZ0Ag|?ZX4N-EyGC+IEdk` z?puFCWm8|~E(peUplSjt`wC&lCc%!17;OQOv{#9HetXS7#d$OmwpXSV$5g0 zh&7(wVo;q9R0n=Wny@OquY`V@rVcb-2(wFGgjnU*@6u!qF@S0NK*+MWy(BUAa>~_3 z3^=hSUV(d`$`@;}H%hhogFsf2;sjj@{d8!tMv(yv)@DRS%%K(A;@+Hi3>Vhm5=W*> zab69qAg&-M={5Npm*;tx`rz#71Bx^_JNu`m&Cb-o=B)>PSQG(M-h{3ompQ!jC1Ex( zW4`3QEUIz&5L0vt1_@$LX$894NBG=iXXU_&faqI|-rJ4J^JptF&H%y8JRdvB&HGlW z4PKJ7DYO<_D&EIH+7}On_rt1%jE?ahMwjV>A5;LsgOeP2f)f?+RWEOa!9cdKUp1e| zP&`N@(YLRV?Q1J1N`I- zc;#ni3)+b^;Rr_*A`L))e}s=2na|Vq!HgbF&clEL&h>n z7r&+W>I>mO9$qw)u)A~NWJ+U&;2+#rAu;bs#*BahNOL$?0Tz=gXB=1X`qTl`vLqWK zi6tNwU|z5Qw%Z(l6JZybU|iJ}+##>30?tl7Or5z7pn&3aHeb`60T16vfD_xC+hZ!_ zaGAmcoF>z9-frXIA1pkmri!$1R&7E^_(&wdEyx{07%(5xkbu*pO=?y%nQ^B2kzOv*&u%v|Szt(|4!rQL%DEC|P)jJ4x5E;3)#FMjq38oje@owHM6YLq zuw^fU(N;5Cvk}$xwPMnMHQ0pN3nk(;Z15lm2~b)PRDFkRV!q?LDvTF4qJrR@}z9wj5v-!NygTV8M8#wIaYAfh(pV@!-RvE2L>H!58(vpYb_NwHXoeF z8a{xLM?oQ+^e@RZ$Ssd-4q})D5+%?u^)~B!tFo8Q^@p1Y&8RcZ&^Jx(7n5-t2O7}& z{y2v0U|m}a3{Qh>%%AVjf$an7PKz**aq88H;`$foziN-E01=6C;g0kEEIOrjx32;G zH#|gr*7~h{vF~WSL+{2tVNevxAZ2@*Cd~S~g3o}cgQo-;%aULWH^PRADo_ZB6cnGh zR}yLpFuET_fp}af;A}i;<#-qvhSdn_)nQkAq4dHK@TDdNZjSM;>$KIJBsI9@aK;T} zqr<6>O2N5No*it*luf*QR8t_?@JSSNNuYKF^#T`;i}!5zqVRnX+8>;Rm~Cgc37n59 zJQIF6fZB-*2Rap|fs4=@Yv%n*$;dOE{*i%0=qIQa>xh!qB4o0GJ2deCNzTj?-^!4G z?Rl5XRWR@)tF{5t-C-$-2fMq=J(b)|e#7APNn#UJS-SglOss^rAsK{{M)OiG{gKS+| z8tOWt4(9_K#GIrS?~q>VH_zcC#?HgQ5+kSdwtT`XY5;UW%(+tGoEXkWx}pZ-)1m6j zS$TUOx_k+b*3|2pHwZw&p3u$U*(H*LTNpqn7@){))0NeU+anvAdWy7U3UGUaA1&AW z5nu@{D7u$!67rMV!Q?oAgB*LU4Fd0+wfXglbE_b6Ql z6Avv}_$caPVQiUNV_jPa$jB(j?S0QlC8Yw-01^w1fo}f**iA)UakTvPPDu`+vG34_ zA_H!`$l>77Dh8=hrq?@@CI;16SYM-rv?@cO7CWc_II@@}2{!gDYfl(z1)jSVo#d0c zDS99KfD7WoS(vUWOPcp_%S>~?JLSVkpX4p|E&-_Vo_Rq6cPZ^Q;I{ungb#@^avM(K>RVe zg3-=Zk>fTx<~@n2qoG>l3J1Hv`y$WwQc4n-xpDX9KulxEOavxFNyMH*XX?P1hp-3= zC27&b(3tb*^Ut!pno0-=+~I$~3U2`Lm?N^D*5i9WFtCvS{UlKtHK}}t>$lPy_xg_B zm$v3nLjLI3-`CSyMjW468?zqTHl#;d4&Tx;m7KUgk#cEANxp1SFSnJTWIaaoecZYj`U^mIO+}k_1}H_efMsr zle68|78E+BXu*rkCw$CO?s^+s`k|IY@PvYf6whSLDp`IpZ#(y^N4{al-2t-Qf7vMi z;p}K+^S~(uL1`Yt3WeT>F7=tVXn>Uvu5HpiMSV2%w={jw?Taxi@bgOBRQcI69X!8D zL^W(U-#hnc?RZ2+-gibzjt^SiVaUgRi#S8J!V?Edk?kMjY;je*;UlI# zd(Oc3@mV{_c5in2S6gGsr(J7UPqDT3Vlza@4BNsbNq77E&V_T$%iNbx`=YAeN(67x0OR}Jjvsb~5_gI|%PgIiN z6-EV5NH1T1yRUyzTkawpCE-67Bo@LofwE|$sauwTFV4dgkXz-)MLdSbZu6;tzK}l;rgKM2P96h&F|A;m zO{fzy+yG8VCEbBfTH(L16w@d+~-gzl8p2KLv z06ucI{=bJd8WQh3jES8}JGtOdhn00OI%+2~WNWv4J2>H}$O9Re_Jbt`lt>%K@Kzm1pCKB09i176`A%wy%FM4ZB_3x^QeAp*v$av&K$V;IFN%k z+@B>5U1j_NK7$nugu^?JnH4Xz$ydKQ+5Y#zwm9{qtZEJOU&NlPRg80g1@jJ^;`$Jk za~DMzwi*A5uw@Wjh~qc(c6bw@o$&u_*GXk98L9?4FmJN{S-K#OYM)j?4@mw3Mfbo-6h>1-QC^Y4Bg%F z9lZCx-;Y1fu+PqQ_IlRytOeR8^x3EuSat0)H(q}C-~MK30{efBUlZz+J|(aEt8>Kn zQ2@9SZ5KpTw5mYhkonhRLdhpqT03#fGjpYl9+ z9>Cv&*6f`{lewJY<>~DA?^?GwwJ!x(cK6BAv$eL$16_Vt>OZltUw0SQvEY{H8o;;w z--EKTxOw4?=sg&9{b%j884xe@^#O02``GN7x$@u9mIbts%q;(|Wcax7Kp+LKe?3q~ z{mi+Lb?+Amxc`kc6!xsh^@K~`+*bMTW#O+uvKIIjz1%)jGjj6Z`szX4Rx3uiRySDo zZU5_=A56qE5a+*!e!)asjHc@%C;rQS{Yb#g1k{!fCzye4*@n^r z3Pk%`{@p3>0sz$AdS(4Xn>8?^hc!jV_aUD&nA#1uB>$M-6!8!{qN{H9+a~cN*{+Si2n7V{~l*9sY4CU zScFi5t7iQF8jOwx53w0pX2$# z*M_Ic0!$3k?RD92nm{Z!p&$95 z;Y8>>IA$AQSf)nilfPz4hP2&d9SDkH68(Fqg@57$K2Bv40200-7}^(n^!L3~&45pq zZpw+X09_mo0Z#FQ{g(d$oa@qsw8}#m*Y9=Xe-4E_uti-=m*2BUv2zm+o|gQTz6P}1 zw3A+{r9@c+P@thB(pBOE85ppAn}NWjaRJ|-sFMfN^)TfbY@jX@vj)L7HUmes)A#?3 zr)3489)wUb{Cha)tALjbsx@Lt<$m>Kc;FKOc%d02w1I^q^-cXtnx;&Y2RSIdPs~I6 zwXfNB=qOah`JDfGsV`th$;w9nJDP5UPWS&GqZ3}tNnGHX95+AwFt@go%GFQ)@6l< zZ1e&#e=FmornQt4T*1duMMVS)(oRMQ#t0n#VIKo^0PRwCLaO>;L^Z8SZ5tW$2Qck>UBi}i;s}sXL z0D)N3XU}`oaE_bTmt8DZ>(-1dG~sHo8Jz*-kYIVy!(65LxuMC1y9Bwyx_oPL8M9Pr z0D#8#*$8kIv`I}*s|UJhP5*^<{vBS8Yr?UxkPwH!W}jMISA~gwi>Cl_vzteFe%weE zpC+Vn`iIU(3{-AR#DUGS>%EcT)s}nh3`qgUsD0X&v_j?X==E*V;@A*!$e#x z{hm!;`(Tqh=h+&A3kjXfx%%93(=VS>>giW8- z1m?1D(S2_nE`~SvSX2X;8>#vk^br^B_g6{xh`WvjqsK9I`zLH?yagaTjj^i(0VBFg zh2{IyO2ok3765z$9Lioy+vsH>&9u?_rb%dP)>X6+Qu5xQ&}%tb%8f|6UGQMzlC?Do zzzClzeP$tS=wL)t{k;RCw$?KUa&}kIfoP5bF2brm;{Ngm@9tIg_aYCs@yYQ91QqeR z#>ut7cFW8Q6~gN8cR`l!cW(D1qPH?dz=4uRyqQ%NZDV5bHX%1T% zXUWDQ_L9os#V8C^&`n9HherL<_TJab`g#}62oTXkWT*@aFsDP%_Z&R;yAAJW8#~Ia zbkz_8@_pB&Zc}H}yHS_>_p9hw7cav&fz50LN!j3t?^&>#ypFFC8^FwHf9Y$=Hgni- zD)fMT^~5@uae=mSY^>wue^2~lc8tweN?3RZ7t48SPEc7$YL5AQxkK|-&s~mO;f^EZ!~IP|G$nB4 zmHrKDBiR z_(f?5E(YN}t+=k^GRZ>iSzX#FD0)=>O=Zpt7LxCV4Y!`@(?f+Hgbr?U`fa12q$8`(=e2hIwx#h zV~iIIK=^B3Pgve>SL|O8;MJc^;2R$2Tw(ph(*H>2lBF3}2H|+E;lANuw9k8=1pM;3 zTl>o0B_Tk34rnQSxM6}<>@}8wuvRgyKZ;BCo?YFw9MwnMft|V~InBAmZ;*kn9Kp>( z*DCGMgOePx(UOLW3eNQ5jshx-1QANJ`1FsrYpNxU0Yq$h&z_Xs1pE1KC!{5w(=4_8ks#4{Dm z7`%BHPO|%r4GL;%)w}Orfv{g()1m(UIu?17;ep_tM$%ActGOE>F!8u=Ml*+Q|3S&O*el`@`^<*6G^s4T{gb2%zLRkT_wK zQNiQ>y?%|4FOdiS45N0P!v`q7nb9;dAR~f_ZLNrZxSce!GLBCKm7eau;oQt^_Eys*+jGzwf3u)<>kKe$^{~+ zby6$|JprNYzkdS7(1W;GQ{XIA9vPIz&uTq~BJU`^$+@{vnH`_SSM2!ZdjI4zo=`LznX+QVgF5RBpx<&VjO9q%I@v z5cl-orNg_Xw4cMB^6>Xh&V8<1Vz}T`H>!P)bDIl08PQWOBSf1*%w9^O!co8SxHSg? z%pup**;%cGNA}=rH>x;$d0Hpa?62oq#gUI9zvv$|4C50`DLF<)ynU{vp56yZ z7=3C==k!w#&T3Hdc~dEHNeZ8IIejbJr zM%Ts|H#Hf2F~di^_}b9d@tMeO;;ZN5_FfCMI#k{|x$7FHxLMiyy*+8FV~%jbDZg;# zA4Abok_z#(Tuo@JD7$1{{n9NyLC1L+N;JT&S9B@NeBVs1(8Anw$I{th9!}KRti z20Q^Fcj|Ah5E!`RP-qfpHu+iwz|c_WK|DOv1DG|4c+$$IQrq8O#d#Q4pQuiVG72+% zj1^s@)E3$CY~Gsc18AjHsAC2acmm`$r7^X&OXAlSi%NJ0A3Hn0O%bAIyQT!krb?A& zvj~T%+Pt3Wn7Kf{L&jH-0935sVF#kI;4^k&0V^+ zH*tLaIJrb=w7>I7W!=0m?%yQl{(u{M!5N*wk&ACaHe4cX@&`Avb557!rX9ipXr2e8 z?f|NRa&~!_7M_kauzoL1ZhS5lKeG9$-TB41HlosHc4CBxBW9CXYw+yZ;s6ne2c9ZQI+wh^-H^g?<4eJ4fX{_Rw`72@ikW;&d{b1UHyME^lK z)0;dlw_~I8M9E{Zku6!AS_UF^`oqFrv%YA? z2X&WI6om-u513gFP@6TxVjS$s-7ekT9fUYcH=8RgwGW(O$N$-Wi@Zo#2-8|n=Ii)8 z(1x6i4J2$xW@T2dv~Z_nW^}(3p&YG~+3kI8s((RiruIoJraoew%|6#_598+Aaqb2tybJ2=GyDU@-a*6`*Gef2gZY?RSJybvlRN@va5v3ZU{$TTuv z6pHVgGjSa25d)B7W}!CQu@d(NA?cYVyF~u5^&ylxyR}1qyr2QdAPQ079|*H4K%nuj z_WM~-f7=z-;C^(Nl&y~u?6VXDgphIpkNiYcy`OE1p3Sj9w#mbizi9sFsJBiGnYUfb zM;Oh5mtBRE=t>SE?FSaOky2Qv)!pQMbmAucwe6J<~_R1ORfzWjQ)^ zJgfZEvBN>Dnon}V8_VeN>y6sq6O2|ociM%}VKv{GMXQ0K

;6YP@higy zcL=*jJ{(33nzlmmUqJVLZiCg|y)fincXhq!(hOL56p-rD4n)O#ZpQ!yAP;m@I9ZcZ zkjMh2-bmV=!lXR2QeZ)d#-Y!5*${?iUR@(9pc05SY=jbvAWkhX4AAcQ9S zr=I#{ppyd&a>Co~>1Wm71#HKi5K~5cTnyw4Zm8Uiq)SY@X6DnYfkLm;%O3ldJ%}>8 z0M#zq+6NKV`Dhh@Yq;{AQ?yM8>=`iBL=5oo6lz|`ZfsD_oId9cK=OJ!`RS*v`Q9@G zkYFg4rX5yjd7!iEB>-op9OV|#Ab(Pv3^-}|qULGN3s)^9^V7MQGq`%yU;4O$kdUVR z(FSbudb*Azfw(4spDeoC4~7g}s}@`YLJis%gH2hMf_60+|JZfy^?F~Kd|{@Cj*?3t zyI!o6JHSiJh`yQ3gpuT2yKGM#e(~v*>Cb0K{kGIN?(q(_jBfcO4T1SGKToak0}#tA zvg+Rt@A}&2CGfgtRCwWXzHP2PBt-hE{qV8fh?6HNyRw(S-dlk?grRWrlEBjGHrCJF zW?_v>Bw6&w_~Sr)Q)%uAvz3cVtZnz6#%yjz7lMI_lWi@Z-EaBvtB}-2D0#B~Oz;Cd zm)`6Ph~H?FEpiujwk3X6Th48P()p9YNGncojzBa}GY41mLhYw%Hr~T4HQwo=g>m~^ zm>p=*0EbDdj#DupD@L=!;s;QartTZ8+ehkh%_NK^_dhPcPrX4*EeOMj5ILz;sBa^m zO)9|R)Yi53Vt}@yUmsUrR_`n)Bq{K*MNXaAAlx8 z>c5gbZN?mmMxIge6rfHK4I@NSf9U9l0aRTff&qr+pi zNVs*1c1N#OPG~+P`papWMImWX&D^!<;lc6rX}m3AfpQUfMR4HeGj6^oVDsCBme@m| zZwhH~%e7cT8jPCS!$#kWV0u0L4vR5b8S_huH^UWL}omqBk%2tk6J-|Fi#n;L^N7(j#w9$mZuahh0Z69TJF>q8s``f>T#+O?q1ljYS5mE;jMPr@>-S0IXSN*v>@zJ5Bd#ubN<W+z}cDylsB5f9DZy) zY9)um@M+6S6Kg#-<~G-s>N*l`a?G^u(g6##?gCjUdEV!NnPJ*As6_ zd$j6w6~uvy-bV;X$(ZOhmV*8+SzdJq32^L{>vZ3-dyL|O5DCVb8RC}t>CPQ%jeKdk zGD=96E>>19r!RKF&H;Uxk&>B*tF?bJDZ;<_Fy89ajHvD_-%#O*`JlWgCNR%gw*9bi zzLfK=;i^q84a?MhWg>XJhqVkh@O%si{3m z6_*VA^*HnrXN%NOA&{GnXQ4JDsx)814y(0)LF8xo+ZF92#kF0FwNW2HMErRpnxltPI;cmGN&c z?{UI2d1cEwBUJbN_6Z}}cA|7%rbYn{Xr_kav$oNve-2n#*q2+#Cb<*eJY%aG> zDS?iNn3(wjzbM1q_4lP-bBIoPY4b+hJ#haLww>;xh`Cm$LST;P? zDbtd6IzJX0mjANZmMv|;QR;}lWsIaT8GP1PbBE42Oo64(C>(Mmub_q?5nky#~n67 zHy0E}{V_Xqi+=`)$}a;erAf1Ae=mc@8lQ%|vu~0KjbJWeawSgos*vM09%jHzMmVnR zs@W4P#W)y|J@$d_!tN{^Tvn=((Q&!XH_q=ZwUuOT?^Yc&nl))1Pt<(hwo5%>YCu=& zb6&o5XwhRS-z}4JPtVW!v2cA7Ms~Xd*vCfcT&gs=I!{)I`9?##?=&q}<=9_o-lG?- zyiUUS$?-z0*paS5rRgQvqT`;IaD%_6l%Y-2Da+}$3vFWpn0UKB_?YVvySxXK{>oPbaIP18P3xU#v=fUYs z$$E_*Nz`<%cx4*hnYNQkPO4A4K2>PaA2I@nOt=w?=o&^#9aRXv%CA#>VmIaFk^#P> z*cJCSzsXYeWY}vaGcKjesSvqi#?q{5*w$RTTOWI~FdTA;lq*k_;jHj%cRcV1|K`qJ zc!2W@t2B|jsAV15ECwt%M;JtS_ZqFKXfa*5eiot@lTfhee6sp`g8nly16vA{k%gmL zA8@~zE5CZm-xoJ=x;If?b%}dkP?NkWc>W{Oyx#Q>P!WIx(fRsgi=(59EANNq!)}Ei zcNer>JeJ$;t1T0}`sLeRn|sQPN-C9(Ylp8NGYPe=%_FXvu7h3uEzN|Uv2;d);IfO# z2eYU^b>-^usGyKUc0B)sF9NeJF7!^mv6;2;&yLoz!@4mbI>Dn`9k_Mug4oGYp$ZW( zEBZN^0(_?7DQ7WTr8y~cKTMK`gk*D}YL77*++=fkRRK^jFrE((4Lo@hz*?W(z z=jn1NT<;%M|3SIukPv&k(kNrdTcZR209RRmVhfPn6TfW^;mNq}HNhVrMNjmBm*-7S z<1>!xE%r=Uq>Sw&aT-VYqs1`P>zByXm_-wtezsi#F@1sj%ju~HQ0bsb+Y{vJ5uQaOd(g# zE8m*UQ((buBwK0;K=*&Hi2+iuM<%s{=@G+lbwYtPn?}fr2=hJ|;7$7CHe|<&6y;g- zgy&n&kBsDJ2-1;?{~(%n@K+rl?SXc_RFI?7WG^KiP?)d3x}xB`0#A@QJp=nRzxVN~ zgXuYmgix&;FD&ERJUl!OWjwMpjybMx+G)mX4{c+i17#as(q^VHKgI1AK65=%aBb-YnW_% zhvD>T3s7He1cbiSxE;y{Ea>(G16MhBeUwW_PeCX1_Iy!_AlGwDvopIZ4oSvG@v2SbF-6a%946+?NL&Yq>G&fQL!XpiC^60#y=Lp>3GVIIP7OzO>+B$Z>B5ON-J@MENY_pAE>`);D z0*bNnaipm|=mSZbl|rU%uM0n(9S!gXuxc zysHYh{9~upvW851a=S>w>8sLxF}IeT7@IB;0)uChFm)2SZYY#}m_B0^p6hG8Uf9hDIyL#WHX$;bsHP5^ zn?-Xwyw{XJ*obsQ*BmAl3h&6eX!jj|&zFaS=;T7vr^Pli#}p!u=^M*y zfGzUb^lNQPi;KT$zQG;sfXYu^vKzjcB*cKSuXz;+0`2XkBQJaoiIWeom2q``uEy%y z??}qF^(~VG5CP!{#@5C_g}GGw$W?zk1R!B?NE3~r^bB_{QfvYs1PNgSvF}b$HFxvTdp!^c zqb8%Wn%JN5UNK5nP5)5`z*v@0(r|i-Sr@OSo?WtlG3j`#n-@O26Zju^vU!m%{p$k2ljPH(T*lA@Y&yW&qDvG zI*#!bvX@&NFM}&+&p=-l8|?jihsUYLp$CUeQM{RXwkt`uhW*Lk_d-B2ejn49!Qi)4 z0GId=S4D8WgfmIf1TN1>bFu*4kCt00?r#xXX#TUGk_`h|G{tGl)^z>ll0!oJLoO4N zNAvU3$P$K|_hfZ2K@2)%nRZKud(!JJTw5X18}|Er%f!3A*6dUSkO8hMyRv#I9-(UC zb1I^@KK;Y9!E>r-F{=P*t~NFogB5bMD6HYUthAuEFh1#i%VNH*06Vp}Wdh04;AC?i zi*UBV--(Zh9CdHGF|?W`eYmcp`Vv=7O^LH5nhD2>-&c+lI}b1$pU&V10x>{2Jp)Za z3#S&dKJ~b~b=%J?A|k9EfjoxJ?+bM75W%IM5HzQhhJ5qenU&@{ob0Caa$FwD9Jd1a zDO9!epmF@gn)g*F&zktTZJ`Wm{?kC^Ag>!}5C@&H#+vvw$LP2co9t$P*mR76f?%4C zE+pm?>JKdWz8*PG=@_%{b3UhX9;(ebRR}V* zD_d?W&6ya0#>Y&15GBkd)MZ^hDNhC0Cysv0FZ~%P0~0H6C)MAEL3Z{?R}pa$2E&+) zrC(Bs2(jOZd+Exy^U)^myCfCt%Ng>{r70nBM8k(NxY!_GE$kM`b5QcC*jBa!-hD+D zZz6X z-HeD*X%0o#_C$^d=AyZC2o=%DKKyqa;D?$M$mRc;9TBMhkYcx@pM@84m7TYBR}26+ zVYb6dWH{sSFq{<++rROr3e~L$nOE6jY#JmHlnZ~V zVIn4%g-UMsgPbM~9_4r68m{MS6ilQ_a~z1TZp(ACqTgz9v_VJxQv0?!3__ij(ul9y zV(R?M5<$srL`*~ctP8|j{9rh>0GkJld4yg`n?Zo*C{0{p?{_8JwW|Y!Bn~JoW z$k6M9ZgR61TkJn{yHzg-4P*fH$CN1Vu(Ch_a`jAiyW6W%tBjY^$u~U*rvh|bzN=1G zQkDq;0=q3vW_O*x#?dns|I_@L6w&RyL8wWJf(?vu{39{i`+r(tT99esaZlbta{!l| zFf3S*S9W=pp{|~xAGhztW53H1Uifi39+@5@9 z>9#GRc7M7(h3hv7B`nMCKHWA4C@<#o4{DWdo=vRH8mY+ZNeX5(+pF5%aNilj(9 z>?cSat2bCtQ^EJ`iGR-B>B9BgOP0=;Zs%naiEnKp+7CDj>pUCWvZ5n1fIDC4GrJ$M z*M?{IuQby5JJp=8d_Gm6nsYuX z9}wosj%Z?@qiW}4?Y)yP;uccjo5RH`*+e7Jj0m!X!s}lWspSt2x07PvP9G+QMqna&2QSE9D93 zlMBSv5;_&R+e?k0TYmFQN2)v&HhKvS$BJlT-D-TF@`9?Eyac0|RHm}hKlrLB(YH(pSm9O zxlNA9pA_yYoyolOf)^YFFg-2?lu?V0fh+!Z&%6XoZ{YL!CV5xLMZro1c&H$C9Y_w$ zEYUq+){^Nk(BUgGY8ON@n}~)jry>)`JMrDi%t&sq2|%}b4ItIuodn4BHDuW4fE*}A z#^1^0;I`ugR6>u$IF+$JsxQ#HcYjS{%WgL1}PSH!<4fbHW6jIwi>(DKVM@%w_%V!rcss=B zonvm@QhGf1W+V!caIKxgNMsd^S>m38p`N1jg~T3b%<;3x9`!HFfFs1hkH~(AB-KW; z)tIxT`mH*Sis#(n#n=R|id3Y*wY&Rii}~%+fM3_6kq_;BMibzpQw!$|oE0=bBCA9*#%MNY^t43Cug8V3WE z@#Lz*KfCqM^?`tdfxt$P`BV&AVh*J1i^6nH^R17Yj68Aams4o#FZASh$un~?;k&_> z?hceTudeuc7A|!%l$;2B6x$79R7`TBnH)9enfwHZ4T(NYA7eJXO-YL)oyO#`a}{S# zzYq&x0VQG8Z=a`@938+W-M5!1Dl79Lh(Xo>?)&lWh6x}6RtiW&8fM_-I{HA%$Lq8hg z$TJ2zIJ)#VQGc{>7lzIWDi#Z?08*~xQa1=CN)pAZ+Yp;J^sr?Pn ziM!++FUVr=BBF73_)H~eE`vqc7a2n-h)m5@gpcT={rIEBd%CEsy&~F?M~O1KpUQC> zrPj`ojMvku?48t zm$^DTV6*!IxeA;}yPpg7n1s($yhoTnyK@ zp;iB;M2l<2?WO+ySx2uEZ%|Q_!|s~VZ7Sf4eTK6vQkqdtvjJ&mb9=}XJ#}xoPMQ)i z9GQY>^F`NMX``3BdCJDj-R%xbwjxzWP)_rsqIiT1lTS zr)6O-@Fzk!;}v->RyeF^*$McU#4S-e`STntAmgRsG+*%t2P1tetxd-h@yoBFj;5H{d_fW>J zP?%#kjzD1J%@om0wg=g!-c!(;s{$cofVejMbf9LUM97~2Ax@!4J!309*51Taw%+kt zcB(JSqhkEF$aRz@SeI?6o`BkgRDar^dlY}q5`%=D#gaQQ>&LabFi)4`pGt3u>Qi{c z%6ET^>I#bRt><&~j?{styrUz0YfotHcXeYNtW7D9zFKqs$W-92pbdTdwtle9;HdAt z;_W&O7}41-f_Fp6o|KVmTWjKM1zcngWb0)NHPbt>YAf>rLNvt0G=leLEukFXB4|Kg z?=2io&HMltXocL4pKKGwz3XVUtB^||I~GOl)UR71g$p6=7{Gd)nzE0Zdx$EAllw1^ zh576E{E_^o&B9b>jQznKp6eP{i|c--#ym)M{XD6gd=UZZ+5>USY~^n};+{fj<~bFy z-{tRL&r2oU^f~X>9bIdxD>R(EzTOxwaiD2emnYIc4u#JnI-EKCbN#71xAl~FV!u^5 zeaTZ4&fei+wV6A#W2z{SX$YwJ;|$IPUWh!vb%d}?!2xICZ!UTf7Cw8;cCb*`{>_4M z@lQEyF{#5>Kr~IVj46b}g@Fxcj!%8S;#6<&^svawe#{S}inJINl>#;8r&oL7OajJ2 z+g5wBScmw6!j_?Q`u&a}Mc#0e@PtOpTfl`Y9+t5jDbW2~mlHPjGNuW7>Ic(9+0FYh z>{ZUfK6N%e_!tuH@KNI@p=~Ycn_`-N%tdLo-n&`?^Xycev>AOY_auJ-Z0rFIfP38& z0u*e6#IH8-(Lx!N6aKW}yh}W(R%6F4;!qD105jv+I*;+q)Q<5D4V=CF?E`zU8H{wg zOROZ}Ta}_N@TJ`g#mdN8ss31j*~N33-q|*Z(e0gXZ{QL&oQsW>^j_~1s6E8_b+=oL zjnLaKN1k0_Y6JKkj$UigNw3r!J<mhs{9g00{hG~BS425C^U zGX#)dYFJo-0iZ&RT;N5*3=m<_!*9H&3f)QAZCC~m7wmetb!9oo zlT96iRo!EV;>_xR)Br>sDo+rD$1fg341j#tz^6pe_C5e^uSz{>si08my

zNTB2`ZGEz}?osaLgM0}{D}naV7UzO+ z8B^A#A0jWG;}u!N4-`X^wm{Cq4|leUEfW)V8#?_IrM5Wd0p#h+Pz-$hPctcjx=l=Ut_Sb%u6A*Rd4yDL z*i^vmd~IwzvT50_qgsD;URh>V!5Al&u3_?(F^F5|7u+v0|eC6zpZ2@2~2D#gxBM&7X$-8AgOsPcL(Iq zd=Q{e%|lDA(-s6zGqcBVuEp*$>OeMRtbsXzXY`;m%W^>sc89p($pk9Vb7$F}6Uv*cE-6F)~RU@K+Nv{s^`H1@Fb7l1$3)!|udZ zT5TshHbCD3P`gg*dCI-ANExg52tqY>UTSk>*}vguN0OA022Y!6IWT7C0-@cjl6b$g>Hb< z4R8W5PDpN^vLs}H3uk>5kL6H+*Z-9&J_Sj@4eNsjb5g6e)<7Mz+ClDABS5YN^+1I9 z{lRc*5YN>x86h;S!DVeIsxyk$0u{(C7>)%&$E`hSqEk0=wBw5aDM?4Gl9JGa>e??C z?p8W@FL3XU5tij9ix=mXA#@F=at~$Yx^WP{x30-~QYz|*HV6w3(eQ-{h^O-B1=oHZ z;6uJfoqevU=)(8|Boz6Ic(W>!bXn%vpOCDR#$rkN8AbF_8R;Ec@An`di&O`o8ASDJI zrWe)(7{(v-Km@rifqmV2-k};$c>h(az3I{lTHf??mVra;eGo;0VUPZO;5g{5QhH2@ zL#FD^owD?*(LURuanalPKC1*8sl|4qOGc zi+^7J2#)$i$p90W$bxrO&U8rsw9*RAvQv>=$UK#Gvuo4V+$5-`KqC5Q80|9j>X*Zm z)_vtO$AL;3AfZc!nfZUFUkjqk(R(LI`hn;Yv&69fgDzp=Khp&W%|$wF8sH(}AgZ8~ zBEhB2rw{c|f%)3oq`y8MAT8ZT!y)4?8Yh!`*vSvFvG2OG)@i>;8PIl9m01Y z?Ln7TT7A$iXfFQBwv25e{vmbkUrpxxVbcI_rs8HNP{4Qo7dw;EQ=o~z+Ugr~OU?TX zPgU9>1rEjPt#AKMQ->fhuk~5KC#`c1{cbaua&U3}*QW{Yr4aXfPUN}#WN_gPEAC73 zfBS@Zyec9Z0k595T_vM4t_8B@Ia%G@@2X>&ffZsq6o{(Zi?}3hR;5%jEG2EBHvyt+Cn-?#@&aD13CGuih^DjG;8N+|P|EPj0A|OV^X3QSXpZOzW zhSQVp?Njq<;=0+7_u;QPlD4WTWkWXV&JE0@?rN9=J63KDVkLIgCk`Fw2O9RB7p{4z zP*R}JU|AC#Fa1BcV>}v6qp6i`vVPr$-BwiD+VqEAD7SVs>&pB^8{cbG_$S^I!514~ z?6G;phVtWvuVjKS8dg}ehv*af+-vg}%WwRUP`&1E(Hq{iBG7|hy71{ypGvBzY`3yz;Va*sPT3z&+z6;`Fq`e)|3oM8X@zKh ze@g{rNI&q?9C+xF7bPeKYM^f1VEOsG-HL*1rUcamzetqO8X76^;McOU3tI;@pEpBg zrC=n;s6a0;S{YT~msd0u&|(_{I*beD=CuI(lhOB|mmEI#PoHi+|jf%?$qGgvR=(-qf&`1&?heS^A+<0@$w>&M=RllP!l*=VZQlCM!o z;iU(#S=X8UJF>ID=4mub*(;HnCsUh?WRj-KzbU;y*gl|FKK;Mfep)=dEV*;*nAHeA zhZ&GOC|@nKL^iilu{GwNo5liz+IR~zzJL6~+a~_+6d<)7#wlnbQA9uusklP24?^l; zF*fYaV6*hgT9c5AvCv1T^0l8LA0`DGeQq9c^gdfxKx2#~{HIcVpWN-~7n@~Aqg8zZ z3Z?o&17P|PhGLM0Dxu#H&xYl^-W4zhNwi{p$Zz0Dah6Yf)7#j{v{xrNP6< zTYV8NKI{TK)zuZEdDCM~;#Nsj zF4^Rh8!kYAwLYVB@?D{|1^@a@dE3ERc?D^r=e3*^5&s36&Rn(KJ*LBt^Lgier5BoS zxX#*iU$8B>93Q{Xrmcx_1O?iDKP-|1`fG#QLg@bFX{l&lui^2)&mmCP{jn2Nso%t1d3qE5EhrI1UEGA5d6JR$nYEC!qhy}Q|PbpVQ` z?$sO*1@(+5z$EmTb07pLe|Gn3Dm+5S2TI-8t=7q`+9Bb(QCdqtrDFKcrc{#5{A_Fb z)FSyYBN{fV>qHl8@MP$046DKHllnfhtWJ|Zd%W{jCw7dzg)Pso4mU^WJbpht7~UKq z>ER@Wzbk}}Ncdkb* zo=e^15bT&JLNLhgbwW)uZlCuJce7J{fRiVrrmxCxC#I@|y+%dN;}i#r$K^rYFpRdr z&qFH@#ug7b6YJ4$WPM_gT+nb(n>?2naKFIlRsHP#yhe8Cn4D61%xcpbE2AppoZq+H zdYFm2=pY{DcYcJL^Y|^u;%zq5M>NW|h&l8XDeF?j1kP_z9lW?XP zbFV)BHErHpBU~(YIu2;E_0GWaw?J~vFMAT`kLxy#+L+s9Vr7zsDC4N)m0VGpT(b6K zA0@e-eYK%kr<}N%HOxUdm!jfr+H|z=cq>dGruJG^knZQ)7e5^XBOyc7*FzzS2}ubB z9QYr%Csc53Og$hR^?Q!(!N+=9Vtn!jk7XgqU6OB}%&dA#P(t5dLLV7@wB|_o%va?* z!tyHK#An;PWs}M;bx;Kv$bn9=3i;gSx0ElL?TGLu+Tv+42B&MjSpH5{M@g?Af~T~a z6sz|=Icw)2?X!;6*JuJ0RVE?4_+grOB1CfKGQH_7GxtRQRxz9T02eLkaoH;&r@b@u zdedCA16woVRn#bO|6UA|AzQ+u1()ET^!keF5rWASKY#b(2D8QWJ-rDR=(mPj!#PT+ z`W=dFk71vxhA^y3ZC4u$5)2Bd7uZ!6*@W9{ z6AYfT5B0KJoy_4ivUN6vu#9zt6lPLJ8b{n^dAdt@wRC02euWNljTqBWo+zrj8kcP7D(KBF`ud?HyT4Wal{y3!%xSiEQwRh*Y*+)ND*RlqzCl~czRhxGY-UoGrbhV?cB4E8p5<6PaFhB zf7N{Re*sAjw($x@cHolkrNiQ_=TLHXw)juUH-Z&xec&2$GA zojABH)f2;UQ)T#I5eTU_PvcTm9JCSN)LTcRKCu?w?Jev1sOZbZMoFAG7|JE-=`xQc z()|cL_`Ew}@szdU2jdwbQL;$XS&L7M^6OjRVV~%W+K=GQI2W{5k|GYo8x3m|yb)M@ z-k72|j^9YdQ9gI?sYJ!mhP=flj`C{Ih@!yEavaCO6B%1T5v9_>xl|PgWs`2|TDw;` zf-~a`_LLtPg~omxxX@i(9Qgqrw1Ev8OL5IrfL)}< znBp*^7Y%}bC|bq~kmCgO*`xdzO5BA_A0`RnVD!Vyn-oPC_vEF5H;Cd$8F4_)Xe!N^ zkcdNP1``>jO`K|pV`(Z5%5Rc1LZ1-z31{%(E(pf9bhBkji}h(Q#q7-hKC*d_^p*- zVIJku3FiVO95XGS?#es(UM)AB?@L>pI zyH`LQ0-1w9??s-D5QPOt5L}6r6?=*?JeKbGVR2bxw=_T`!BH1I&qLmT5bfS}G1js7 zywC?bDWc{f4o2^?DS2a0XgEc84DoTK)Kn?2j+-h(aj1xBoaR?)Fz4wp^A8*3`e zspAczI8x^0$WZz?bVPBek2ol_Ok%6xt1alLLnE9Y8yKowCL)F4?JvkZFY>#P5yB3` zo4(A`{dwD~Wg1JdMRWMakFFl}z@tdn#kPC8BQ-xFBZO*(^2N)q&n?)6{Hs2(uX1Q$ zy?{Keb(IBuBEKGvp6oe@gK;=RXBGa926K)fb)N|Famb3JnarvX#WCfcErNIRj@!3_ zK`>MzM#YjLbbI#+(WuY^f*?0)Uj{f6?a%@6a<}I#&<1`$Gby|A)GD4~xdAsX(WmeOA&MjAJ`OUg zqEh03QUV$m_I?zcwbkxLGy%5`ujupQFpja&tpEi54mSW-i@RS&cmFAkMe_)5_PNKR zIfWj&(B1hx6ebk%jqV!V0dN;SpSKb>>xKRHqW_>@K!%TdR^njvPUGu}c+srvRO$`l z<51VE3P%)&+K8h>7(nBCnH@!rqPu;lT@>4~_BaE@2nUG++8`G-dp|-C9rIWsKVU}K zpsDd3LoaxC@42^P#L;tm9o8AJ3mml$^@~JE9N5VFa-l_I$eWospx#jM#+ubx!`b?j zejFr@BZX^KcGj0GW>w0|FDKSR5yhbb;wXgo^ZLxTpVJSwD=FjepSUkVLA?H^-H4#WA$b9iY#Szd_0i-xekXbSyMx)|J+}jx;iuxTy zi(%$a9B}-!{z1tbg(>C@;^Ro=tjgw{^;_bm%9i@e&A_kP<*`fF$6-HT2|z!Z2oZZw zy!d()B#tbaQ zte$HHoN5=xF3reMe~<&c0zbZNs}IGQi^7?{-gY#<5*5b!>UbaDkHW;MLU_~P8qu7( zvKvjuS~SInuN*GMksO-cS5A3~Yc2p_rYJL{2d~K2^^M_4SfRbQ?+l*i6@6Qmjq^N; z3MrgXaWE|1`rVp@f8%Jj*^uhbV$pmYesJQ0Sc*gyA|?4aLZOf`o^#X~()m?9G?!$^ zxe*|zB|mD%ky=roeRNcg024;-*ue?htk%zQ;8JIe=Uk^8|J8Y}Q;q=ZJm;}!ZiRzl zn4-|E?G8R_AC=@z)a&ozP@qRdT!;=L&!sd+nILar0mNn?&sRMNeO2uPvX-1Cb zVPB}%D~EdeqVdCMH0l-3AVz>!1@g-Vjisi>B8QpR)YQmF7WioAGoX$0OHmW%WyV6iHb75iy7C-Dyc?*<4a8h0Zr7JXuO7&1Ej2q zU5G7qvD;%E`*>k=JsL;U%;gb+p|H^!Ls%_iX2rHxd4$&Rd;mF3P;5-ga!!(*Cr?n) zFKpuZweX&cmq$?bxLlg6V^QXvmL0WQy~sd`ioUvKy(RM6R&mFU9q==3Mn*c)65k)Y ze|JQ(RJz}J=ACYAt5`HOyUQ-Y4?P>Ljzvr6mK~`b!#za#lHO%?KFJ2+$LmUV;T3Z+ z#W7sHMS*HV0{W3jPo)eKI!STlPrws0%!83?GsiX@omN-H1YesdIz96nQ+*Za8-J~< zw~QoEodYD|FsB1?EYW-%IVg=V04ICeb?}GR}m6o?5hH_GARdgC) z=V;PJr~Sq~nKRUmRTQ1h*hxxCCslNsP9IZcspcFl!p3C~hlck-Sx|t&KBIP&bq78Z zrsZa4*#<=+=Nq8NXaiqrEX9#8rDnF6+uJ5F1mqqRe2RKoW``_PDLt%XnK*GARC@vD z#&Wq@{g&j;v@zp-@WSzm3Q}}>0g6tik*Tso6$efpC}%n6&!dnnoPHEXjzSd1E(rR-YCMOfMQ$1kiX6p;d_1gAPGMSbo{tK(u|;lpat5H_Qbo0T^%FT}&#m`t`2JRn#S+n6#3RC$sH@__3Z!2^ zG3Q762WrQ8Z`6)*e{sM_rx$Ttv*K6|9Q#q-`d0J)iGw3@?O0S&+7&B0?N@(+X%&>X z_BR8YEffZ^`KqwZR>cs-p*G?`q2h4#57hbWykO_;=x2K`ULBjSDDFu45p%TNb4ufh zbarYyoe_`cUw`pYyhK~v^g#vRhH6U$fwv2SUEl?NY!@OSBbFs;r#QB8Nh^-!QsqK| zg@=hDibHM0!Dvwi9#q1uZ^t-VQD0KmGlXlZWGh3B8Hx_zWzo`s?lj>_wEaicxBJBC_*m}%ep!H~n4i7kFts884*!%RR1jjJ3PFNofIaM!LOjAm zaj4H!f$x3TBn93Kb>y~mNXy}Y)j4JTDy7wNlC$F3s~4|ckk(>=Dw>+ojINTRd4iQK zEiIK&Gft*6!K@o?Nx+Zcv@%nRea65ye3i$5J8=hVr|ElVkWyIoj## z5m{$Q5e$ibTMH@ev}RPHbxJdvlZ`79nK%pwPF_|uP9E@sb%zeE159#fdh)QOHs5d+YFoOhMnj&anKYY4uWe}92BcoI4Ew|px)D)8HR-7(2ZPoZ+dyHh`}-E zh0@2?l>C%tl)+7igWy^bY<8;_&A{ePH<&^7&dmr-POJI)yUS~qFwcdOkoVJ?nT>=v e2nYzS9sdu6ygj Date: Tue, 30 Sep 2025 20:55:57 -0700 Subject: [PATCH 009/198] Conceptual text menu, needs testing --- team417/build.gradle | 1 + .../ftc/team417/TextMenuTest.java | 62 +++++++++++++++++++ 2 files changed, 63 insertions(+) create mode 100644 team417/src/main/java/org/firstinspires/ftc/team417/TextMenuTest.java diff --git a/team417/build.gradle b/team417/build.gradle index c7861fbf694c..f83dd718b541 100644 --- a/team417/build.gradle +++ b/team417/build.gradle @@ -50,4 +50,5 @@ dependencies { implementation "com.acmerobotics.dashboard:dashboard:0.4.16" implementation 'org.team11260:fast-load:0.1.2' implementation 'com.github.SwerveRobotics:FtcDrivers:0.0.2' + implementation 'com.github.valsei:java-text-menu:v3.2g' } \ No newline at end of file diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/TextMenuTest.java b/team417/src/main/java/org/firstinspires/ftc/team417/TextMenuTest.java new file mode 100644 index 000000000000..214e80a3b050 --- /dev/null +++ b/team417/src/main/java/org/firstinspires/ftc/team417/TextMenuTest.java @@ -0,0 +1,62 @@ +package org.firstinspires.ftc.team417; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +import net.valsei.java_text_menu.*; + +@Autonomous(name = "Text Menu Test") +public class TextMenuTest extends LinearOpMode { + + enum Alliances { + RED, + BLUE, + } + + enum Positions { + NEAR, + FAR, + } + + enum Movements { + MINIMAL, + LAUNCHING, + } + + double minWaitTime = 0.0; + double maxWaitTime = 15.0; + + @Override + public void runOpMode() throws InterruptedException { + TextMenu menu = new TextMenu(); + MenuInput menuInput = new MenuInput(MenuInput.InputType.CONTROLLER); + + menu.add(new MenuHeader("CHOOSE YOUR PLANTS!")) + .add("Shortcut?") + .add() // empty line for spacing + .add("Pick an alliance:") + .add("alliance-picker-1", Alliances.class) // enum selector shortcut + .add() + .add("Pick a starting position:") + .add("position-picker-1", Positions.class) // enum selector shortcut + .add() + .add("Pick a movement:") + .add("movement-picker-1", Movements.class) // enum selector shortcut + .add() + .add("Wait time:") + .add("wait-slider-1", new MenuSlider(minWaitTime, maxWaitTime)) + .add() + .add("finish-button-1", new MenuFinishedButton()); + + while (!menu.isCompleted()) { + // get x,y (stick) and select (A) input from controller + menuInput.update(gamepad1.left_stick_x, gamepad1.left_stick_y, gamepad1.a); + menu.updateWithInput(menuInput); + // display the updated menu + for (String line : menu.toListOfStrings()) { + telemetry.addLine(line); // but with appropriate printing method + } + telemetry.update(); + } + } +} From ffd816eca92bd62d8e050d4090934feae1497e0d Mon Sep 17 00:00:00 2001 From: Hankang Zhou <136044263+nobody-particular@users.noreply.github.com> Date: Thu, 2 Oct 2025 21:02:01 -0700 Subject: [PATCH 010/198] TextMenuTest works as a proof-of-concept --- team417/build.gradle | 1 - .../ftc/team417/TextMenuTest.java | 30 +- .../javatextmenu/HoverableMenuElement.java | 19 ++ .../ftc/team417/javatextmenu/Main.java | 118 ++++++++ .../ftc/team417/javatextmenu/MenuElement.java | 10 + .../javatextmenu/MenuFinishedButton.java | 44 +++ .../ftc/team417/javatextmenu/MenuHeader.java | 19 ++ .../ftc/team417/javatextmenu/MenuInput.java | 206 +++++++++++++ .../team417/javatextmenu/MenuSelection.java | 89 ++++++ .../ftc/team417/javatextmenu/MenuSlider.java | 76 +++++ .../ftc/team417/javatextmenu/MenuSwitch.java | 62 ++++ .../ftc/team417/javatextmenu/TextMenu.java | 286 ++++++++++++++++++ .../ftc/team417/javatextmenu/credits.txt | 2 + 13 files changed, 956 insertions(+), 6 deletions(-) create mode 100644 team417/src/main/java/org/firstinspires/ftc/team417/javatextmenu/HoverableMenuElement.java create mode 100644 team417/src/main/java/org/firstinspires/ftc/team417/javatextmenu/Main.java create mode 100644 team417/src/main/java/org/firstinspires/ftc/team417/javatextmenu/MenuElement.java create mode 100644 team417/src/main/java/org/firstinspires/ftc/team417/javatextmenu/MenuFinishedButton.java create mode 100644 team417/src/main/java/org/firstinspires/ftc/team417/javatextmenu/MenuHeader.java create mode 100644 team417/src/main/java/org/firstinspires/ftc/team417/javatextmenu/MenuInput.java create mode 100644 team417/src/main/java/org/firstinspires/ftc/team417/javatextmenu/MenuSelection.java create mode 100644 team417/src/main/java/org/firstinspires/ftc/team417/javatextmenu/MenuSlider.java create mode 100644 team417/src/main/java/org/firstinspires/ftc/team417/javatextmenu/MenuSwitch.java create mode 100644 team417/src/main/java/org/firstinspires/ftc/team417/javatextmenu/TextMenu.java create mode 100644 team417/src/main/java/org/firstinspires/ftc/team417/javatextmenu/credits.txt diff --git a/team417/build.gradle b/team417/build.gradle index f83dd718b541..c7861fbf694c 100644 --- a/team417/build.gradle +++ b/team417/build.gradle @@ -50,5 +50,4 @@ dependencies { implementation "com.acmerobotics.dashboard:dashboard:0.4.16" implementation 'org.team11260:fast-load:0.1.2' implementation 'com.github.SwerveRobotics:FtcDrivers:0.0.2' - implementation 'com.github.valsei:java-text-menu:v3.2g' } \ No newline at end of file diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/TextMenuTest.java b/team417/src/main/java/org/firstinspires/ftc/team417/TextMenuTest.java index 214e80a3b050..b5f8f1af2722 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/TextMenuTest.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/TextMenuTest.java @@ -3,7 +3,7 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import net.valsei.java_text_menu.*; +import org.firstinspires.ftc.team417.javatextmenu.*; @Autonomous(name = "Text Menu Test") public class TextMenuTest extends LinearOpMode { @@ -31,8 +31,7 @@ public void runOpMode() throws InterruptedException { TextMenu menu = new TextMenu(); MenuInput menuInput = new MenuInput(MenuInput.InputType.CONTROLLER); - menu.add(new MenuHeader("CHOOSE YOUR PLANTS!")) - .add("Shortcut?") + menu.add(new MenuHeader("AUTO SETUP")) .add() // empty line for spacing .add("Pick an alliance:") .add("alliance-picker-1", Alliances.class) // enum selector shortcut @@ -48,9 +47,9 @@ public void runOpMode() throws InterruptedException { .add() .add("finish-button-1", new MenuFinishedButton()); - while (!menu.isCompleted()) { + while (!menu.isCompleted() && opModeIsActive()) { // get x,y (stick) and select (A) input from controller - menuInput.update(gamepad1.left_stick_x, gamepad1.left_stick_y, gamepad1.a); + menuInput.update(gamepad1.left_stick_x, -gamepad1.left_stick_y, gamepad1.a); menu.updateWithInput(menuInput); // display the updated menu for (String line : menu.toListOfStrings()) { @@ -58,5 +57,26 @@ public void runOpMode() throws InterruptedException { } telemetry.update(); } + + // the first parameter is the type to return as + Alliances chosenAlliance = menu.getResult(Alliances.class, "alliance-picker-1"); + Positions chosenPosition = menu.getResult(Positions.class, "position-picker-1"); + Movements chosenMovement = menu.getResult(Movements.class, "movement-picker-1"); + double waitTime = menu.getResult(Double.class, "wait-slider-1"); + + double startTime = System.currentTimeMillis(); + + while (opModeIsActive()) { + telemetry.update(); + telemetry.addData("Time (s)", (System.currentTimeMillis() - startTime) / 1000.0); + + telemetry.addData("Alliance", chosenAlliance); + telemetry.addData("Position", chosenPosition); + telemetry.addData("Movement", chosenMovement); + telemetry.addData("Wait time", waitTime); + } + + telemetry.addLine("Opmode is stopped"); + telemetry.update(); } } diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/javatextmenu/HoverableMenuElement.java b/team417/src/main/java/org/firstinspires/ftc/team417/javatextmenu/HoverableMenuElement.java new file mode 100644 index 000000000000..b9ef02bb48d5 --- /dev/null +++ b/team417/src/main/java/org/firstinspires/ftc/team417/javatextmenu/HoverableMenuElement.java @@ -0,0 +1,19 @@ +package org.firstinspires.ftc.team417.javatextmenu; + +/** + * An interface for interactable elements that can be shoved into the menu. + */ +public interface HoverableMenuElement extends MenuElement { + + // start and stop hovering between elements + public void showHover(boolean showHover); + + // allow it to take input + public void updateWithInput(MenuInput input); + + // for checking if the entire menu is complete + public boolean isCompleted(); + + // get the result of the element once the menu is complete + public T result(); +} \ No newline at end of file diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/javatextmenu/Main.java b/team417/src/main/java/org/firstinspires/ftc/team417/javatextmenu/Main.java new file mode 100644 index 000000000000..f879ff5992cd --- /dev/null +++ b/team417/src/main/java/org/firstinspires/ftc/team417/javatextmenu/Main.java @@ -0,0 +1,118 @@ +package org.firstinspires.ftc.team417.javatextmenu; + +/* + * THIS IS A TESTING CLASS USED IN DEVELOPING TEXTMENU! + * it's not necessary to copy this class over when using the project lol + */ + +import java.util.*; + +public class Main { + + static Scanner userIn = new Scanner(System.in); + + public static final Map inputMap = + Map.of( + "w", 1, + "s", -1, + "a", -1, + "d", 1 + ) + ; + + public enum option1 { + RED, + BLUE, + GREEN, + YELLOW, + } + public enum option2 { + ONE, + TWO, + THREE, + } + public enum option3 { + HELLO_WORLD, + GOODBYE, + SUSPICIOUS, + AMONGUS, + } + + public static void main(String[] args) { + + String input = ""; + + TextMenu menu = new TextMenu(); + menu.add("(Sample text for estimating the horizontal space)") + .add("The Robot Controller app is obsolete. You should") + .add("install the new version of this FTC season.") + .add("To ensure correct operation of the IMU in this") + .add() + .add("colors") + .add("op1", option1.class) + .add() + .add("numbers!! YUH") + .add("op2", option2.class) + .add() + .add("Stuff") + .add("op3", option3.class) + .add() + .add("other elements:") + .add() + .add("sl1", new MenuSlider(5.0, 10.0, 4.0)) + .add() + .add("swi1", new MenuSwitch(true)) + .add() + .add("fin1", new MenuFinishedButton()) + ; + /*for (int i = 0; i < 10; i++) { + menu.add("#"+i, new MenuSwitch(true, ""+i, "z")); + } + menu.add("fin1", new MenuFinishedButton());*/ + + MenuInput menuInput = new MenuInput(); + + clear(); + while (!menu.isCompleted()) { + + for (String line : menu.toListOfStrings()) { + System.out.println(line); + } + input = userIn.nextLine().toLowerCase(); + clear(); + + if (input.matches("quit|q")) { + System.out.println("force quitting...\n"); + break; + } else if (input.matches("[wasdc]")) { + int x = input.matches("[ad]") ? inputMap.get(input) : 0; + int y = input.matches("[ws]") ? inputMap.get(input) : 0; + boolean select = input.matches("c"); + + menuInput.update(x, y, select); + menu.updateWithInput(menuInput); + + } + } + + System.out.println(menu.getResult(option1.class, "op1")); + System.out.println(menu.getResult(option2.class, "op2")); + System.out.println(menu.getResult(option3.class, "op3")); + System.out.println(menu.getResult(Double.class, "sl1")); + System.out.println(menu.getResult(Boolean.class, "swi1")); + System.out.println(); + + switch (menu.getResult(option1.class, "op1")) { + case RED:System.out.println("red!");break; + case BLUE:System.out.println("blue!");break; + case GREEN:System.out.println("green!");break; + case YELLOW:System.out.println("yellow!");break; + } + } + + // yoinked from random stack overflow thread + public static void clear() { + System.out.print("\033[H\033[2J"); + System.out.flush(); + } +} \ No newline at end of file diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/javatextmenu/MenuElement.java b/team417/src/main/java/org/firstinspires/ftc/team417/javatextmenu/MenuElement.java new file mode 100644 index 000000000000..2a70d7600415 --- /dev/null +++ b/team417/src/main/java/org/firstinspires/ftc/team417/javatextmenu/MenuElement.java @@ -0,0 +1,10 @@ +package org.firstinspires.ftc.team417.javatextmenu; + +/** + * A general interface for anything that can be shoved into the menu. + */ +public interface MenuElement { + + // for rendering each element in the whole menu + public String getAsString(); +} \ No newline at end of file diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/javatextmenu/MenuFinishedButton.java b/team417/src/main/java/org/firstinspires/ftc/team417/javatextmenu/MenuFinishedButton.java new file mode 100644 index 000000000000..6e1af11035f1 --- /dev/null +++ b/team417/src/main/java/org/firstinspires/ftc/team417/javatextmenu/MenuFinishedButton.java @@ -0,0 +1,44 @@ +package org.firstinspires.ftc.team417.javatextmenu; + +/** + * A type of menu element that can stop the menu from registering as completed before the + * user is sure they have selected the right options. Once pressed, it will count as completed. + * Not meant to be read from; Menu will not be able to finish if there is more than one. + */ +public class MenuFinishedButton implements HoverableMenuElement { + + private boolean isHovered = false, isPressed = false; + + /** + * creates a new finish menu button. + */ + public MenuFinishedButton() {} + + // MenuElement interface required methods + + public String getAsString() { + return (this.isHovered ? "➤" : " ") + "[[ Finish Menu ]]"; + } + + // HoverableMenuElement interface required methods + + public void showHover(boolean showHover) { + this.isHovered = showHover; + this.isPressed = false; // so it can only be pressed last + } + + public void updateWithInput(MenuInput input) { + // once pressed stay pressed (while being hovered) + this.isPressed = this.isPressed || input.getSelect(); + } + + public boolean isCompleted() { + return this.isPressed; + } + + public Void result() { + // this element doesn't have anything to return + // but is required to because of the HoverableMenuElement interface + return null; + } +} diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/javatextmenu/MenuHeader.java b/team417/src/main/java/org/firstinspires/ftc/team417/javatextmenu/MenuHeader.java new file mode 100644 index 000000000000..285da72e7e59 --- /dev/null +++ b/team417/src/main/java/org/firstinspires/ftc/team417/javatextmenu/MenuHeader.java @@ -0,0 +1,19 @@ +package org.firstinspires.ftc.team417.javatextmenu; + +/** + * A type of menu element that displays text. + */ +public class MenuHeader implements MenuElement { + + public String text; + + public MenuHeader(String text) { + this.text = text; + } + + // MenuElement interface required methods + + public String getAsString() { + return this.text; + } +} \ No newline at end of file diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/javatextmenu/MenuInput.java b/team417/src/main/java/org/firstinspires/ftc/team417/javatextmenu/MenuInput.java new file mode 100644 index 000000000000..bb32aa646354 --- /dev/null +++ b/team417/src/main/java/org/firstinspires/ftc/team417/javatextmenu/MenuInput.java @@ -0,0 +1,206 @@ +package org.firstinspires.ftc.team417.javatextmenu; + +/** + * A class for bridging input methods to a text menu. + *

+ * This is made with the intention of supporting controllers! + * That means that when passing input, one should set the input + * type to {@code InputType.CONTROLLER} in the constructor. The user can + * then pass raw input into the {@code .update(...)} method and it + * will be accordingly processed. + */ +public class MenuInput { + + // determines the type of input processing + // raw is good for standalone console debugging via keyboard + private final InputType inputType; + public enum InputType { + CONTROLLER, + RAW, + } + + // a constant for stick deadzone + public static final double INPUT_DEADZONE = 0.25; + // constants for spacing out sustained input (in seconds) + private static final double STICK_TAP_COOLDOWN = 0.3; + private static final double STICK_HOLD_COOLDOWN = 0.1; + + // processed values + private int x, y; + // input timer to stop input spamming + private double stickTimer; // in seconds + // switches on when past tap cooldown, then using hold cooldown + private boolean isHoldingStick = false; + // time passed between update calls to update timers with + private double deltaTime = 0.0; + private Long lastTime = null; + + // processed value + private boolean select; + // so it only registers once per held press + private boolean hasAlreadySelected = false; + + /** + * creates a new input processing object. + * @param inputType determines the type of input processing + */ + public MenuInput(InputType inputType) { + this.inputType = inputType; + } + /** + * creates a new input processing object. + * defaults to raw input type. + */ + public MenuInput() { + this(InputType.RAW); + } + + /** + * updates the input values. considers current input type. + * converts to and sums x,y inputs. + * @param x current raw x stick input + * @param y current raw y stick input + * @param xLeft dPad left pressed + * @param xRight dPad right pressed + * @param yDown dPad down pressed + * @param yUp dPad up pressed + * @param select current raw select input + */ + public MenuInput update(double x, double y, boolean xLeft, boolean xRight, boolean yDown, boolean yUp, boolean select) { + double xVal = clamp((xLeft ? -1 : 0) + (xRight ? 1 : 0) + x, -1, 1); + double yVal = clamp((yDown ? -1 : 0) + (yUp ? 1 : 0) + y, -1, 1); + return this.update(xVal, yVal, select); + } + /** + * updates the input values. considers current input type. + * converts to x,y inputs. + * @param xLeft dPad left pressed + * @param xRight dPad right pressed + * @param yDown dPad down pressed + * @param yUp dPad up pressed + * @param select current raw select input + */ + public MenuInput update(boolean xLeft, boolean xRight, boolean yDown, boolean yUp, boolean select) { + double xVal = (xLeft ? -1 : 0) + (xRight ? 1 : 0); + double yVal = (yDown ? -1 : 0) + (yUp ? 1 : 0); + return this.update(xVal, yVal, select); + } + /** + * updates the input values. considers current input type. + * @param x current raw x stick input + * @param y current raw y stick input + * @param select current raw select input + */ + public MenuInput update(double x, double y, boolean select) { + + // reset all + this.select = false; + this.x = 0; + this.y = 0; + + switch (this.inputType) { + + case RAW: + + // directly apply the values + this.select = select; + this.x = (int)x; + this.y = (int)y; + + break; + + case CONTROLLER: + + updateDeltaTime(); + + // only register one select per sustained input: + // initial select + if (!this.hasAlreadySelected && select) { + this.select = true; + this.hasAlreadySelected = true; + // if select button isn't held anymore reset it + } else if (this.hasAlreadySelected && !select) { + this.hasAlreadySelected = false; + } + + // get new x,y input; consider deadzone + if (Math.hypot(x, y) > INPUT_DEADZONE) { + // snap input vector to axis of greatest magnitude component + if (Math.abs(x) >= Math.abs(y)) { + this.x = (int)Math.signum(x); + } else { + this.y = (int)Math.signum(y); + } + } else { + // stopped holding stick, reset timer and held status + this.isHoldingStick = false; + this.stickTimer = 0; + } + + // slightly cursed stick input spacing implementation: + // (sustained input -> repeated but spaced out input after an initial pause) + + // if it's the initial stick input: + if (!this.isHoldingStick && this.stickTimer <= 0) { + if (Math.hypot(x, y) > INPUT_DEADZONE) { + // only starting adding deltatime if the stick is held + this.stickTimer += this.deltaTime; + } + // allows the x and y values to pass through + } else { + this.stickTimer += this.deltaTime; + + // if it's held for long enough to pass the tap cooldown + if (!this.isHoldingStick && this.stickTimer >= STICK_TAP_COOLDOWN) { + this.stickTimer = this.stickTimer % STICK_TAP_COOLDOWN; + this.isHoldingStick = true; // changes to holding cooldown mode + // allows the x and y values to pass through + + // if it's past the tap cooldown and is also past the hold cooldown + } else if (this.isHoldingStick && this.stickTimer >= STICK_HOLD_COOLDOWN) { + this.stickTimer = this.stickTimer % STICK_HOLD_COOLDOWN; + // allows the x and y values to pass through + + } else { + // otherwise block the x and y values + this.x = 0; + this.y = 0; + } + } + + break; + } + return this; + } + + /** + * checks if there are any active inputs being used + * @return if inputs are active + */ + public boolean isActive() { + return Math.abs(this.x) > 0 || Math.abs(this.y) > 0 || this.select; + } + + // updates the deltatime in seconds + private void updateDeltaTime() { + if (this.lastTime != null) { + this.deltaTime = (double)(System.nanoTime() - this.lastTime) / 1_000_000_000.0; + } + this.lastTime = System.nanoTime(); + } + + public int getX() { + return this.x; + } + public int getY() { + return this.y; + } + public boolean getSelect() { + return this.select; + } + + // clamps value between a minimum and maximum value + private static double clamp(double value, double min, double max) { + return Math.max(min, Math.min(max, value)); + } +} \ No newline at end of file diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/javatextmenu/MenuSelection.java b/team417/src/main/java/org/firstinspires/ftc/team417/javatextmenu/MenuSelection.java new file mode 100644 index 000000000000..9de402104192 --- /dev/null +++ b/team417/src/main/java/org/firstinspires/ftc/team417/javatextmenu/MenuSelection.java @@ -0,0 +1,89 @@ +package org.firstinspires.ftc.team417.javatextmenu; + +import java.util.EnumSet; + +/** + * A type of menu element for choosing enum states. + */ +public class MenuSelection> implements HoverableMenuElement { + + public int selectedIndex = -1; + public int hoverIndex = -1; + private Object[] options; + private Class enumClass; + + // border formats for hovered and selected options + private static final String[] borders = {" x "," ➤x "," [x]","➤[x]"}; + + /** + * creates a new enum selector using an enum type + * param requires that the class type is of an enum + * @param enumClass the class of the enum (do myEnum.class) + */ + public MenuSelection(Class enumClass) { + this.enumClass = enumClass; + // get a list of all of the enum states from the enum group + Object[] options = EnumSet.allOf(enumClass).toArray(); + if (options == null || options.length < 1) { + throw new IllegalArgumentException( + "Enum must have at least one option" + ); + } + this.options = options; + } + /** + * creates a new enum selector using an enum type with a default selected option + * param requires that the class type is of an enum + * @param enumClass the class of the enum (do myEnum.class) + * @param defaultIndex the index of the option to default to (typing 0 is fine usually) + */ + public MenuSelection(Class enumClass, int defaultIndex) { + this(enumClass); + selectedIndex = clamp(defaultIndex, -1, options.length - 1); + } + + // MenuElement interface required methods + + // render the selection and hover into a string to display + public String getAsString() { + String asString = ""; + for (int i = 0; i < options.length; i++) { + // find border format index to use by adding value of hover/select + // 0 is nothing, 1 is hover, 2 is selected, 3 is both + int borderValue = (i == hoverIndex ? 1 : 0) + + (i == selectedIndex ? 2 : 0); + asString += borders[borderValue].replace("x", options[i].toString()); + } + return asString; + } + + // HoverableMenuElement interface required methods + + public void showHover(boolean showHover) { + this.hoverIndex = showHover ? Math.min(0, this.hoverIndex) : -1; + } + + public void updateWithInput(MenuInput input) { + this.hoverIndex = clamp(this.hoverIndex + input.getX(), 0, this.options.length-1); + if (input.getSelect()) { + this.selectedIndex = this.hoverIndex; + } + } + + public boolean isCompleted() { + return selectedIndex != -1; + } + + public E result() { + try { + return Enum.valueOf(this.enumClass, this.options[this.selectedIndex].toString()); + } catch (Exception e) { + return null; // very good yup + } + } + + // clamps value between a minimum and maximum value + private static int clamp(int value, int min, int max) { + return Math.max(min, Math.min(max, value)); + } +} diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/javatextmenu/MenuSlider.java b/team417/src/main/java/org/firstinspires/ftc/team417/javatextmenu/MenuSlider.java new file mode 100644 index 000000000000..73d16f91802b --- /dev/null +++ b/team417/src/main/java/org/firstinspires/ftc/team417/javatextmenu/MenuSlider.java @@ -0,0 +1,76 @@ +package org.firstinspires.ftc.team417.javatextmenu; + +/** + * A type of menu element for choosing a value inside a range. + */ +public class MenuSlider implements HoverableMenuElement { + + private boolean isHovered; + private double value, min, max, scale; + + /** + * creates a new slider. + * @param min the minimum value allowed + * @param max the maximum value allowed + * @param scale scales the units per character when displayed + * @param defaultValue the starting value, instead of using minimum + */ + public MenuSlider(double min, double max, double scale, double defaultValue) { + this.min = min; + this.max = max; + this.scale = scale; + this.value = defaultValue; + } + /** + * creates a new slider. + * @param min the minimum value allowed + * @param max the maximum value allowed + * @param scale scales the units per character when displayed + */ + public MenuSlider(double min, double max, double scale) { + this(min, max, scale, min); + } + /** + * creates a new slider. + * @param min the minimum value allowed + * @param max the maximum value allowed + */ + public MenuSlider(double min, double max) { + this(min, max, 1.0, min); + } + + // MenuElement interface required methods + + public String getAsString() { + String asString = isHovered ? "➤[" : " ["; + double sliderLength = (this.max - this.min) * this.scale; + for (int i = 0; i < Math.round(sliderLength); i++) { + asString += i < (this.value - this.min) * this.scale ? "/" : "-"; + } + return asString + "] " + (Math.round(this.value * 10.0) / 10.0); + } + + // HoverableMenuElement interface required methods + + public void showHover(boolean showHover) { + this.isHovered = showHover; + } + + public void updateWithInput(MenuInput input) { + double nextValue = this.value + input.getX() / this.scale; + this.value = clamp(nextValue, this.min, this.max); + } + + public boolean isCompleted() { + return true; + } + + public Double result() { + return this.value; + } + + // clamps value between a minimum and maximum value + private static double clamp(double value, double min, double max) { + return Math.max(min, Math.min(max, value)); + } +} diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/javatextmenu/MenuSwitch.java b/team417/src/main/java/org/firstinspires/ftc/team417/javatextmenu/MenuSwitch.java new file mode 100644 index 000000000000..ab948fcdb9c4 --- /dev/null +++ b/team417/src/main/java/org/firstinspires/ftc/team417/javatextmenu/MenuSwitch.java @@ -0,0 +1,62 @@ +package org.firstinspires.ftc.team417.javatextmenu; + +/** + * A type of menu element for choosing booleans, switching between on and off. + */ +public class MenuSwitch implements HoverableMenuElement { + + private boolean isHovered = false, switchState; + private String[] optionNames = {"False", "True"}; + + /** + * creates a new boolean switch. + * @param defaultState the starting state of the switch + * @param falseName name of the false option + * @param trueName name of the true option + */ + public MenuSwitch(boolean defaultState, String falseName, String trueName) { + this(defaultState); + this.optionNames = new String[] {falseName, trueName}; + } + /** + * creates a new boolean switch. + * @param defaultState the starting state of the switch + */ + public MenuSwitch(boolean defaultState) { + this.switchState = defaultState; + } + /** + * creates a new boolean switch. + */ + public MenuSwitch() { + this(false); + } + + // MenuElement interface required methods + + public String getAsString() { + return (this.isHovered ? "➤" : " ") + + (this.switchState ? " "+optionNames[0]+" | ["+optionNames[1]+"]" : + "["+optionNames[0]+"] | "+optionNames[1]+" "); + } + + // HoverableMenuElement interface required methods + + public void showHover(boolean showHover) { + this.isHovered = showHover; + } + + public void updateWithInput(MenuInput input) { + if (input.getSelect()) { + this.switchState = !this.switchState; + } + } + + public boolean isCompleted() { + return true; + } + + public Boolean result() { + return this.switchState; + } +} diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/javatextmenu/TextMenu.java b/team417/src/main/java/org/firstinspires/ftc/team417/javatextmenu/TextMenu.java new file mode 100644 index 000000000000..f78423ab60c1 --- /dev/null +++ b/team417/src/main/java/org/firstinspires/ftc/team417/javatextmenu/TextMenu.java @@ -0,0 +1,286 @@ +package org.firstinspires.ftc.team417.javatextmenu; + +import java.util.ArrayList; +import java.util.HashMap; +import java.util.LinkedHashMap; +import java.util.NoSuchElementException; + +/** + * A highly advanced, very cool and definitely + * professionally coded text-based menu predominantly + * aimed at choosing between enums via a controller. + *

+ * By valsei!! [https://github.com/valsei/java-text-menu] + *

+ * Some examples and documentation can be found on the readme in github. + */ +public class TextMenu { + + // entire list of elements, including both hoverable and not, used in printing all in order + private ArrayList elements = new ArrayList<>(); + + // map type used in preventing element name duplicates, linked for indexing order + private LinkedHashMap> hoverableElements = new LinkedHashMap<>(); + + // map that stores each rendered element to prevent rerendering every loop cycle + private HashMap elementRenderCache = new HashMap<>(); + + // index of currently hovered element + private int hoverRow = 0; + + // scroll position + private int scrollPos = -1; + // number of rows that will be displayed at a time + private int viewHeight = 0; + // number of rows to pad the scroll view + private int viewMargin = 0; + + /** + * creates an empty TextMenu. use {@code .add()} to add elements. + */ + public TextMenu() {} + /** + * creates an empty TextMenu. use {@code .add()} to add elements + * @param viewHeight number of rows to show; show all with 0 + */ + public TextMenu(int viewHeight, int viewMargin) { + if (viewHeight < 0 || viewMargin < 0) { + throw new IllegalArgumentException("Menu view params must be greater than or equal to 0!"); + } + this.viewHeight = viewHeight; + this.viewMargin = viewMargin; + } + + /** + * adds a HoverableMenuElement to the end of the menu. + * @param name a unique internal name for the element; used in retrieving result later + * @param element any HoverableMenuElement implementing object (ex. MenuSelection) + * @return returns itself so you can chain {@code .add()} methods + */ + public TextMenu add(String name, HoverableMenuElement element) { + this.elements.add(element); + if (this.hoverableElements.containsKey(name)) { + throw new IllegalArgumentException( + "The menu already contains an element with name: " + name + ); + } + this.hoverableElements.put(name, element); + // show starting hover + this.updateWithInput(new MenuInput().update(0, 1, false)); + return this; + } + /** + * adds a MenuElement to the end of the menu. + * note that HoverableMenuElements require a name parameter. + * @param element any MenuElement implementing object (ex. MenuHeader) + * @return returns itself so you can chain {@code .add()} methods + */ + public TextMenu add(MenuElement element) { + if (element instanceof HoverableMenuElement) { + throw new IllegalArgumentException( + "This is a HoverableMenuElement, so it must have an identifier name! ex. menu.add(name, element)" + ); + } + this.elements.add(element); + return this; + } + /** + * adds an enum selector section to the end of the menu. + * @param requires that the class type is of an enum + * @param name a unique internal name for the element; used in retrieving result later + * @param enumClass the class of the enum (do {@code myEnum.class}) + * @return returns itself so you can chain {@code .add()} methods + */ + public > TextMenu add(String name, Class enumClass) { + return this.add(name, new MenuSelection(enumClass)); + } + /** + * adds an enum selector (with a default option) section to the end of the menu. + * @param requires that the class type is of an enum + * @param name a unique internal name for the element; used in retrieving result later + * @param enumClass the class of the enum (do {@code myEnum.class}) + * @param defaultIndex the index of the option to default to (typing 0 is fine usually) + * @return returns itself so you can chain {@code .add()} methods + */ + public > TextMenu add(String name, Class enumClass, int defaultIndex) { + return this.add(name, new MenuSelection(enumClass, defaultIndex)); + } + /** + * adds a text section to the end of the menu. + * @param text any string of text + * @return returns itself so you can chain {@code .add()} methods + */ + public TextMenu add(String text) { + return this.add(new MenuHeader(text)); + } + /** + * adds an empty line for spacing to the end of the menu. + * @return returns itself so you can chain {@code .add()} methods + */ + public TextMenu add() { + return this.add(""); + } + + /** + * passes input into the menu for navigation and selecting. + * @param input uses a MenuInput object as an inbetween + */ + public void updateWithInput(MenuInput input) { + if (!this.hoverableElements.isEmpty()) { + // update hover row from y input + if (input.getY() != 0) { + // stop hovering previous line + getMapValueAt(this.hoverRow).showHover(false); + updateRenderCacheAtHover(); + // move down to next row + this.hoverRow = clamp(this.hoverRow - input.getY(), 0, this.hoverableElements.size() - 1); + // start hovering new row + getMapValueAt(this.hoverRow).showHover(true); + // render cache will be updated in the if block below since input is active + updateScrollView(); + } + if (input.isActive()) { + // pass input into the hovered element + getMapValueAt(this.hoverRow).updateWithInput(input); + updateRenderCacheAtHover(); + } + } + } + + // updates the render cache for the element currently being hovered over + private void updateRenderCacheAtHover() { + this.elementRenderCache.put(getMapValueAt(this.hoverRow), getMapValueAt(this.hoverRow).getAsString()); + } + + // returns element inside the hoverableElement map at an index + private HoverableMenuElement getMapValueAt(int index) { + return this.hoverableElements.get(this.hoverableElements.keySet().toArray()[index]); + } + + /** + * renders the menu in its current state into a list of strings. + * should then be printed/logged using external methods. + * @return list of strings representing the menu elements + */ + public ArrayList toListOfStrings() { + + if (this.scrollPos < 0) { + updateScrollView(); + } + + ArrayList list = new ArrayList<>(); + + //for (MenuElement element : this.elements) { + for (int i = 0; i < this.elements.size(); i++) { + // only show the lines within the scrolled view + if ((i >= this.scrollPos && i < this.scrollPos + this.viewHeight) || this.viewHeight <= 0) { + MenuElement element = this.elements.get(i); + + // retrieve as string from element render cache + String asString = null; + if (this.elementRenderCache.containsKey(element)) { + asString = this.elementRenderCache.get(element); + } else { + asString = element.getAsString(); + this.elementRenderCache.put(element, asString); + } + + if (element instanceof MenuFinishedButton) { + asString += " (" + countIncompleted() + " incomplete)"; + } + + list.add(asString); + // indicate there's more rows outside of scroll view + } else if (i == this.scrollPos - 1) { + list.add("˄˄˄˄˄"); + } else if (i == this.scrollPos + this.viewHeight) { + list.add("˅˅˅˅˅"); + } + } + return list; + } + + // calculates the starting index of the scrolled view + private void updateScrollView() { + // if viewHeight is 0 (show all), lock scroll position to the top + if (this.viewHeight <= 0) { + this.scrollPos = 0; + } else { + // get the actual index that is being hovered over; snap to start/end + int elementIndex = 0; + if (this.hoverRow == 0) { + elementIndex = 0; + } else if (this.hoverRow == this.hoverableElements.size() - 1) { + elementIndex = this.elements.size() - 1; + } else { + elementIndex = this.elements.indexOf(getMapValueAt(this.hoverRow)); + } + + // yeahhhh i'm not going to explain this one + // logic drafted here: https://www.desmos.com/calculator/zazudhzflp + this.scrollPos = clamp( + (int)(clamp( + this.scrollPos + this.viewHeight / 2.0, + elementIndex - this.viewHeight / 2.0 + this.viewMargin + 1, + elementIndex + this.viewHeight / 2.0 - this.viewMargin + ) - this.viewHeight / 2.0), + 0, + this.elements.size() - this.viewHeight + ); + } + } + + /** + * checks the result of a hoverable element using its name. + * @param the type to return as + * @param clazz the class of the type to return as + * @param name the unique internal name of the desired element + */ + public T getResult(Class clazz, String name) { + if (!this.hoverableElements.containsKey(name)) { + throw new NoSuchElementException("Could not find a menu element with the name: " + name); + } + try { + return clazz.cast(this.hoverableElements.get(name).result()); + } catch (ClassCastException e) { + throw new IllegalArgumentException("Class " + clazz + " is not the correct class for element " + name); + } + + } + + /** + * checks if all the applicable menu elements have been filled out. + * recommended to include a MenuFinishedButton element. + * @return boolean of if the menu is completed + */ + public boolean isCompleted() { + for (HoverableMenuElement sel : this.hoverableElements.values()) { + if (!sel.isCompleted()) { + return false; + } + } + return true; + } + + /** + * checks completion status and counts the incomplete, excludes MenuFinishedButton + * @return number of incomplete elements + */ + public int countIncompleted() { + int incomplete = 0; + for (HoverableMenuElement sel : this.hoverableElements.values()) { + if (!(sel instanceof MenuFinishedButton || sel.isCompleted())) { + incomplete++; + } + } + return incomplete; + } + + // clamps value between a minimum and maximum value + private static int clamp(int value, int min, int max) { + return Math.max(min, Math.min(max, value)); + } + private static double clamp(double value, double min, double max) { + return Math.max(min, Math.min(max, value)); + } +} \ No newline at end of file diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/javatextmenu/credits.txt b/team417/src/main/java/org/firstinspires/ftc/team417/javatextmenu/credits.txt new file mode 100644 index 000000000000..65ba54c257bf --- /dev/null +++ b/team417/src/main/java/org/firstinspires/ftc/team417/javatextmenu/credits.txt @@ -0,0 +1,2 @@ +Derived from java-text-menu by valsei (https://github.com/valsei/java-text-menu) +Permission granted by the author. From f2981fb4762ab80e444a6342b5bb7149f407d9ef Mon Sep 17 00:00:00 2001 From: Andrew Date: Sun, 5 Oct 2025 15:46:41 -0700 Subject: [PATCH 011/198] Potential Wily Works fix for Macs, #2. --- WilyCore/build.gradle | 5 ----- .../simulator/framework/InputManager.java | 17 +++++++++++------ 2 files changed, 11 insertions(+), 11 deletions(-) diff --git a/WilyCore/build.gradle b/WilyCore/build.gradle index d7e3c0dd1b54..fead464319dc 100644 --- a/WilyCore/build.gradle +++ b/WilyCore/build.gradle @@ -1,7 +1,6 @@ plugins { id 'java-library' id 'org.jetbrains.kotlin.jvm' - id 'application' } java { @@ -17,10 +16,6 @@ kotlin { } } -application { - applicationDefaultJvmArgs = ["-XstartOnFirstThread"] -} - sourceSets { main { java { diff --git a/WilyCore/src/main/java/com/wilyworks/simulator/framework/InputManager.java b/WilyCore/src/main/java/com/wilyworks/simulator/framework/InputManager.java index 7798064f58d6..ab675e0734cb 100644 --- a/WilyCore/src/main/java/com/wilyworks/simulator/framework/InputManager.java +++ b/WilyCore/src/main/java/com/wilyworks/simulator/framework/InputManager.java @@ -195,12 +195,17 @@ class GamepadInput { GLFWGamepadState gamepadState; // Gamepad input object, null if no controller is plugged in GamepadInput() { - if (!GLFW.glfwInit()) { - System.out.println("Failed to initialize GLFW!"); - return; - } - if (GLFW.glfwJoystickIsGamepad(GLFW.GLFW_JOYSTICK_1)) { - gamepadState = GLFWGamepadState.create(); + // Don't attempt to do gamepads on Macs because of -XstartOnFirstThread is required + // on Macs and that magical incantation isn't working: + String osName = System.getProperty("os.name").toLowerCase(); + if (!osName.contains("mac")) { + if (!GLFW.glfwInit()) { + System.out.println("Failed to initialize GLFW!"); + return; + } + if (GLFW.glfwJoystickIsGamepad(GLFW.GLFW_JOYSTICK_1)) { + gamepadState = GLFWGamepadState.create(); + } } } From ecc86c0b138aca0d9ae076926ca4a834b4b485e8 Mon Sep 17 00:00:00 2001 From: Andrew Date: Tue, 7 Oct 2025 18:57:22 -0700 Subject: [PATCH 012/198] Add Configuration Tester and Reset ADB utilities. --- .../simulator/framework/WilyGamepad.java | 6 + .../robotcore/internal/ui/GamepadUser.java | 17 ++ .../ftc/team417/roadrunner/LoonyTune.java | 2 +- .../team417/utils/ConfigurationTester.java | 230 ++++++++++++++++++ .../ftc/team417/utils/ResetADB.java | 24 ++ .../ftc/team417/{ => utils}/WilyConfig.java | 2 +- 6 files changed, 279 insertions(+), 2 deletions(-) create mode 100644 WilyCore/src/main/java/org/firstinspires/ftc/robotcore/internal/ui/GamepadUser.java create mode 100644 team417/src/main/java/org/firstinspires/ftc/team417/utils/ConfigurationTester.java create mode 100644 team417/src/main/java/org/firstinspires/ftc/team417/utils/ResetADB.java rename team417/src/main/java/org/firstinspires/ftc/team417/{ => utils}/WilyConfig.java (91%) diff --git a/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyGamepad.java b/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyGamepad.java index 428a406d6099..a582fbdac6f4 100644 --- a/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyGamepad.java +++ b/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyGamepad.java @@ -2,6 +2,8 @@ import com.qualcomm.robotcore.hardware.Gamepad; +import org.firstinspires.ftc.robotcore.internal.ui.GamepadUser; + import java.util.ArrayList; /** @@ -193,6 +195,10 @@ public RumbleEffect build() { } } + public GamepadUser getUser() { + return GamepadUser.ONE; + } + /** * Checks if dpad_up was pressed since the last call of this method * @return true if dpad_up was pressed since the last call of this method; otherwise false diff --git a/WilyCore/src/main/java/org/firstinspires/ftc/robotcore/internal/ui/GamepadUser.java b/WilyCore/src/main/java/org/firstinspires/ftc/robotcore/internal/ui/GamepadUser.java new file mode 100644 index 000000000000..e0c0438a17c6 --- /dev/null +++ b/WilyCore/src/main/java/org/firstinspires/ftc/robotcore/internal/ui/GamepadUser.java @@ -0,0 +1,17 @@ +package org.firstinspires.ftc.robotcore.internal.ui; + +/** A *typed* integer so that we can more easily keep track of things */ +public enum GamepadUser +{ + ONE(1), TWO(2); + + public byte id; + GamepadUser(int id) { this.id = (byte)id; } + + public static GamepadUser from(int user) + { + if (user==1) return ONE; + if (user==2) return TWO; + return null; + } +} diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/roadrunner/LoonyTune.java b/team417/src/main/java/org/firstinspires/ftc/team417/roadrunner/LoonyTune.java index 7843d16638f3..3faf204b6842 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/roadrunner/LoonyTune.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/roadrunner/LoonyTune.java @@ -827,7 +827,7 @@ public static Widget addRunnable(String descriptor, Runnable callback) { * @noinspection UnnecessaryUnicodeEscape, AccessStaticViaInstance, ClassEscapesDefinedScope */ @SuppressLint("DefaultLocale") -@TeleOp(name="Loony Tune", group="Tuning") +@TeleOp(name="Loony Tune", group="Utility") public class LoonyTune extends LinearOpMode { @SuppressLint("SdCardPath") static final String FILE_NAME = "/sdcard/loony_tune.dat"; diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/utils/ConfigurationTester.java b/team417/src/main/java/org/firstinspires/ftc/team417/utils/ConfigurationTester.java new file mode 100644 index 000000000000..5005fa1edf8f --- /dev/null +++ b/team417/src/main/java/org/firstinspires/ftc/team417/utils/ConfigurationTester.java @@ -0,0 +1,230 @@ +package org.firstinspires.ftc.team417.utils; + +import static java.lang.System.nanoTime; + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.CRServo; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.DistanceSensor; +import com.qualcomm.robotcore.hardware.Gamepad; +import com.qualcomm.robotcore.hardware.HardwareDevice; +import com.qualcomm.robotcore.hardware.IMU; +import com.qualcomm.robotcore.hardware.Servo; +import com.qualcomm.robotcore.hardware.ServoController; + +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles; +import java.util.ArrayList; +import java.util.List; + +@TeleOp(name="Configuration Tester", group="Utility") +public class ConfigurationTester extends LinearOpMode { + ArrayList deviceNames = new ArrayList<>(); + /** + * + */ + int menu(String header, List options, int current, boolean topmost) { + while (isActive()) { + if (header != null) { + telemetry.addLine(header); + } + for (int i = 0; i < options.size(); i++) { + String cursor = (i == current) ? ">" : " "; + telemetry.addLine(cursor + options.get(i)); + } + telemetry.update(); + if (gamepad1.dpadUpWasPressed()) { + current--; + if (current < 0) + current = options.size() - 1; + } + if (gamepad1.dpadDownWasPressed()) { + current++; + if (current == options.size()) + current = 0; + } + if (gamepad1.bWasPressed() && !topmost) // Cancel + return -1; + if (gamepad1.aWasPressed()) // Select + return current; + } + return topmost ? 0 : -1; + } + boolean isActive() { + return opModeIsActive(); + } + boolean notCancelled() { + telemetry.update(); + return isActive() && !gamepad1.bWasPressed(); + } + void commonHeader(String deviceName, HardwareDevice device) { + telemetry.addLine(String.format("Name: %s", deviceName)); + telemetry.addLine(device.getDeviceName()); + telemetry.addLine(device.getConnectionInfo()); + telemetry.addLine(String.format("Version: %d", device.getVersion())); + telemetry.addLine(String.format("Manufacturer: %s", device.getManufacturer().name())); + } + void testMotor(String deviceName) { + int previousTicks = 0; + DcMotor motor = (DcMotor) hardwareMap.get(deviceName); + String encoderStatus = ""; + double previousTime = nanoTime()*1e-9; + do { + commonHeader(deviceName, motor); + telemetry.addLine("Right trigger to spin forward, left to spin backward."); + double power = gamepad1.right_trigger - gamepad1.left_trigger; + telemetry.addLine(String.format("Power: %.2f", power)); + motor.setPower(power); + int currentTicks = motor.getCurrentPosition(); + int deltaTicks = currentTicks - previousTicks; + double currentTime = nanoTime()*1e-9; + double deltaTime = currentTime - previousTime; + if (power != 0) { + if ((currentTicks == 0) && (deltaTicks == 0)) { + encoderStatus = "No encoder detected."; + } else if (((deltaTicks < 0) && (power > 0)) || ((deltaTicks > 0) && (power < 0))) { + encoderStatus = "ERROR: Encoder turns opposite of motor; is motor wiring wrong?"; + } else { + encoderStatus = "Encoder detected."; + } + } + if (!encoderStatus.isEmpty()) { + telemetry.addLine(encoderStatus); + if ((currentTicks != 0) || (deltaTicks != 0)) { + telemetry.addLine(String.format("Position: %d", currentTicks)); + telemetry.addLine(String.format("Velocity: %.0f", deltaTicks / deltaTime)); + } + } + previousTicks = currentTicks; + previousTime = currentTime; + } while (notCancelled()); + } + void testCRServo(String deviceName) { + CRServo crServo = (CRServo) hardwareMap.get(deviceName); + do { + commonHeader(deviceName, crServo); + telemetry.addLine("Right trigger to spin forward, left to spin backward."); + double power = gamepad1.right_trigger - gamepad1.left_trigger; + telemetry.addLine(String.format("Power: %.2f", power)); + crServo.setPower(power); + } while (notCancelled()); + } + void testServo(String deviceName) { + Servo servo = (Servo) hardwareMap.get(deviceName); + ServoController controller = servo.getController(); + boolean enabled = false; + do { + commonHeader(deviceName, servo); + telemetry.addLine("Right trigger to control amount of rotation."); + double position = gamepad1.right_trigger - gamepad1.left_trigger; + telemetry.addLine(String.format("Position: %.2f, Status: %s", position, controller.getPwmStatus().toString())); + if (position != 0) + enabled = true; // Don't enable until there's a non-zero input + if (enabled) + servo.setPosition(position); + } while (notCancelled()); + } + void testDistance(String deviceName) { + DistanceSensor distance = (DistanceSensor) hardwareMap.get(deviceName); + do { + commonHeader(deviceName, distance); + telemetry.addLine(String.format("Distance CM: %.2f", distance.getDistance(DistanceUnit.CM))); + } while (notCancelled()); + } + void testGeneric(String deviceName) { + HardwareDevice device = hardwareMap.get(deviceName); + do { + commonHeader(deviceName, device); + } while (notCancelled()); + } + void testIMU(String deviceName) { + IMU imu = (IMU) hardwareMap.get(deviceName); + do { + commonHeader(deviceName, imu); + YawPitchRollAngles angles = imu.getRobotYawPitchRollAngles(); + telemetry.addLine(String.format("Yaw: %.2f, Pitch: %.2f, Roll: %.2f (degrees)", + angles.getYaw(AngleUnit.DEGREES), + angles.getPitch(AngleUnit.DEGREES), + angles.getRoll(AngleUnit.DEGREES))); + } while (notCancelled()); + } + // @@@ Deprecated: + void addDeviceNames(Class classType) { + for (String name: hardwareMap.getAllNames(DcMotor.class)) { + if (!deviceNames.contains(name)) + deviceNames.add(name); + } + } + @Override + public void runOpMode() { + telemetry.addLine("Press START to test the current configuration."); + telemetry.addLine(""); + telemetry.addLine("All devices set via 'Configure Robot' will be listed. Use touch " + + "to scroll if they extend below the bottom of the screen."); + telemetry.addLine(""); + telemetry.addLine("Use A to select, B to cancel, dpad to navigate."); + telemetry.update(); + for (String name: hardwareMap.getAllNames(DcMotor.class)) { + if (!deviceNames.contains(name)) + deviceNames.add(name); + } + for (String name: hardwareMap.getAllNames(CRServo.class)) { + if (!deviceNames.contains(name)) + deviceNames.add(name); + } + for (String name: hardwareMap.getAllNames(Servo.class)) { + if (!deviceNames.contains(name)) + deviceNames.add(name); + } + for (String name: hardwareMap.getAllNames(IMU.class)) { + if (!deviceNames.contains(name)) + deviceNames.add(name); + } + for (String name: hardwareMap.getAllNames(DistanceSensor.class)) { + if (!deviceNames.contains(name)) + deviceNames.add(name); + } + for (HardwareDevice device: hardwareMap) { + for (String name: hardwareMap.getAllNames(device.getClass())) { + if (!deviceNames.contains(name)) + deviceNames.add(name); + } + } + waitForStart(); + if (gamepad1.getUser() == null) { + while (isActive() && !gamepad1.aWasPressed()) { + telemetry.addLine("Please configure Gamepad #1 and press A to continue"); + telemetry.update(); + } + } + + ArrayList options = new ArrayList<>(); + for (String name: deviceNames) { + HardwareDevice device = hardwareMap.get(name); + options.add(String.format("%s: %s", device.getClass().getSimpleName(), name)); + } + int selection = 0; + while (isActive()) { + selection = menu("", options, selection, true); + String deviceName = deviceNames.get(selection); + HardwareDevice device = hardwareMap.get(deviceName); + if (device instanceof DcMotor) { + testMotor(deviceName); + } else if (device instanceof CRServo) { + testCRServo(deviceName); + } else if (device instanceof Servo) { + testServo(deviceName); + } else if (device instanceof DistanceSensor) { + testDistance(deviceName); + } else if (device instanceof IMU) { + testIMU(deviceName); + } else { + testGeneric(deviceName); + } + } + } +} diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/utils/ResetADB.java b/team417/src/main/java/org/firstinspires/ftc/team417/utils/ResetADB.java new file mode 100644 index 000000000000..710cfc252958 --- /dev/null +++ b/team417/src/main/java/org/firstinspires/ftc/team417/utils/ResetADB.java @@ -0,0 +1,24 @@ +package org.firstinspires.ftc.team417.utils; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import java.io.IOException; + +@TeleOp(name = "Reset ADB", group = "Utility") +public class ResetADB extends LinearOpMode { + @Override + public void runOpMode() throws InterruptedException { + telemetry.addLine("Press ▶ to reset the ADB connection."); + telemetry.update(); + waitForStart(); + + try { + // Restart the ADB daemon: + Runtime.getRuntime().exec("setprop ctl.restart adbd"); + } catch (IOException e) { + throw new RuntimeException(e); + } + } +} + diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/WilyConfig.java b/team417/src/main/java/org/firstinspires/ftc/team417/utils/WilyConfig.java similarity index 91% rename from team417/src/main/java/org/firstinspires/ftc/team417/WilyConfig.java rename to team417/src/main/java/org/firstinspires/ftc/team417/utils/WilyConfig.java index 176effa6be11..5bed63bebff9 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/WilyConfig.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/utils/WilyConfig.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.team417; +package org.firstinspires.ftc.team417.utils; import com.wilyworks.common.Wily; import com.wilyworks.common.WilyWorks; From 872ac49fb4f30f101a70f766d98d5788482e8966 Mon Sep 17 00:00:00 2001 From: anya-codes Date: Tue, 7 Oct 2025 19:13:05 -0700 Subject: [PATCH 013/198] Added max velocity for launcher fast launch and slow launch --- .../firstinspires/ftc/team417/CompetitionTeleOp.java | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java index 6232e7826dc9..fdcb236446de 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java @@ -34,13 +34,16 @@ public class CompetitionTeleOp extends BaseOpMode { * velocity. Here we are setting the target, and minimum velocity that the launcher should run * at. The minimum velocity is a threshold for determining when to fire. */ + + final double LAUNCHER_HIGH_MAX_VELOCITY = 2300; //high target velocity + 50 (will need adjusting) final double LAUNCHER_HIGH_TARGET_VELOCITY = 2250; - //was 1125 + final double LAUNCHER_HIGH_MIN_VELOCITY = 2200; + final double LAUNCHER_LOW_MAX_VELOCITY = 1175; //low target velocity + 50 (will need adjusting) final double LAUNCHER_LOW_TARGET_VELOCITY = 1125; final double LAUNCHER_LOW_MIN_VELOCITY = 1075; + final double SORTING_VELOCITY - final double LAUNCHER_HIGH_MIN_VELOCITY = 2200; boolean doHighLaunch = false; @@ -216,13 +219,13 @@ void launch(boolean shotRequested) { break; case SPIN_UP_LOW: launcher.setVelocity(LAUNCHER_LOW_TARGET_VELOCITY); - if (launcher.getVelocity() > LAUNCHER_LOW_MIN_VELOCITY) { + if (launcher.getVelocity() > LAUNCHER_LOW_MIN_VELOCITY && launcher.getVelocity() < LAUNCHER_LOW_MAX_VELOCITY) { launchState = LaunchState.LAUNCH; } break; case SPIN_UP_HIGH: launcher.setVelocity(LAUNCHER_HIGH_TARGET_VELOCITY); - if (launcher.getVelocity() > LAUNCHER_HIGH_MIN_VELOCITY) { + if (launcher.getVelocity() > LAUNCHER_HIGH_MIN_VELOCITY && launcher.getVelocity() < LAUNCHER_HIGH_MAX_VELOCITY) { launchState = LaunchState.LAUNCH; } case LAUNCH: From 571b9043d0f67ab0116189504539034b9d477c6b Mon Sep 17 00:00:00 2001 From: anya-codes Date: Tue, 7 Oct 2025 19:13:18 -0700 Subject: [PATCH 014/198] Added max velocity for launcher fast launch and slow launch --- .../java/org/firstinspires/ftc/team417/CompetitionTeleOp.java | 1 - 1 file changed, 1 deletion(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java index fdcb236446de..530dc0b257ba 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java @@ -42,7 +42,6 @@ public class CompetitionTeleOp extends BaseOpMode { final double LAUNCHER_LOW_MAX_VELOCITY = 1175; //low target velocity + 50 (will need adjusting) final double LAUNCHER_LOW_TARGET_VELOCITY = 1125; final double LAUNCHER_LOW_MIN_VELOCITY = 1075; - final double SORTING_VELOCITY boolean doHighLaunch = false; From d3bcf8848f8a76e8fac0e791c6179ca8ddf52017 Mon Sep 17 00:00:00 2001 From: Emmett Date: Tue, 7 Oct 2025 19:33:04 -0700 Subject: [PATCH 015/198] launch motors/servos renamed --- .../org/firstinspires/ftc/team417/CompetitionTeleOp.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java index 530dc0b257ba..58eb4c567aff 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java @@ -47,9 +47,9 @@ public class CompetitionTeleOp extends BaseOpMode { boolean doHighLaunch = false; // Declare OpMode members. - private DcMotorEx launcher = null; - private CRServo leftFeeder = null; - private CRServo rightFeeder = null; + private motLauncher launcher = null; + private servoBLaunchFeed leftFeeder = null; + private servoFLaunchFeed rightFeeder = null; ElapsedTime feederTimer = new ElapsedTime(); From f89e27c1f5ed3cdb2cb54e61d7c85a2c3a517fa3 Mon Sep 17 00:00:00 2001 From: Emmett Date: Tue, 7 Oct 2025 19:35:35 -0700 Subject: [PATCH 016/198] launch motors/servos renamed fixed --- .../firstinspires/ftc/team417/CompetitionTeleOp.java | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java index 58eb4c567aff..afe351ffc106 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java @@ -47,9 +47,9 @@ public class CompetitionTeleOp extends BaseOpMode { boolean doHighLaunch = false; // Declare OpMode members. - private motLauncher launcher = null; - private servoBLaunchFeed leftFeeder = null; - private servoFLaunchFeed rightFeeder = null; + private DcMotorEx launcher = null; + private CRServo leftFeeder = null; + private CRServo rightFeeder = null; ElapsedTime feederTimer = new ElapsedTime(); @@ -95,9 +95,9 @@ public void runOpMode() { */ // leftDrive = hardwareMap.get(DcMotor.class, "left_drive"); // rightDrive = hardwareMap.get(DcMotor.class, "right_drive"); - launcher = hardwareMap.get(DcMotorEx.class, "launcher"); - leftFeeder = hardwareMap.get(CRServo.class, "left_feeder"); - rightFeeder = hardwareMap.get(CRServo.class, "right_feeder"); + launcher = hardwareMap.get(DcMotorEx.class, "motLauncher"); + leftFeeder = hardwareMap.get(CRServo.class, "servoBLaunchFeed"); + rightFeeder = hardwareMap.get(CRServo.class, "servoFLaunchFeed"); /* * To drive forward, most robots need the motor on one side to be reversed, From aa1f3b4834a755553ce2be1ce47849fc8d9c8c2f Mon Sep 17 00:00:00 2001 From: anya-codes Date: Tue, 7 Oct 2025 21:48:20 -0700 Subject: [PATCH 017/198] Reversed the fastbot wheels for left front and back instead of left back and right back so they spin in the same direction when the robot goes forward/backward --- .../org/firstinspires/ftc/team417/roadrunner/MecanumDrive.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/roadrunner/MecanumDrive.java b/team417/src/main/java/org/firstinspires/ftc/team417/roadrunner/MecanumDrive.java index 11a21104d372..e057eae53123 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/roadrunner/MecanumDrive.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/roadrunner/MecanumDrive.java @@ -374,8 +374,8 @@ public void configure(HardwareMap hardwareMap) { rightBack = hardwareMap.get(DcMotorEx.class, "rightBack"); rightFront = hardwareMap.get(DcMotorEx.class, "rightFront"); + leftFront.setDirection(DcMotorEx.Direction.REVERSE); leftBack.setDirection(DcMotorEx.Direction.REVERSE); - rightBack.setDirection(DcMotorEx.Direction.REVERSE); } // Initialize the tracking drivers, if any: From d9a54c15fceb2219ed7722dab8953a17b7e88ed5 Mon Sep 17 00:00:00 2001 From: anya-codes Date: Wed, 8 Oct 2025 22:53:03 -0700 Subject: [PATCH 018/198] Changed some of the launcher velocity constants to public static instead of final so we can change them using FTC dashboard --- .../ftc/team417/CompetitionTeleOp.java | 25 +++++++++---------- 1 file changed, 12 insertions(+), 13 deletions(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java index afe351ffc106..0168f68f76cf 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java @@ -21,27 +21,26 @@ */ @TeleOp(name="TeleOp", group="Competition") public class CompetitionTeleOp extends BaseOpMode { - final double FEED_TIME_SECONDS = 0.20; //The feeder servos run this long when a shot is requested. - final double STOP_SPEED = 0.0; //We send this power to the servos when we want them to stop. - final double FULL_SPEED = 1.0; + public static double FEED_TIME_SECONDS = 0.20; //The feeder servos run this long when a shot is requested. + public static final double STOP_SPEED = 0.0; //We send this power to the servos when we want them to stop. + public static double FULL_SPEED = 1.0; //We send this power to the servos when we want them to feed an artifact to the launcher double FASTDRIVE_SPEED = 1.0; double SLOWDRIVE_SPEED = 0.5; /* * When we control our launcher motor, we are using encoders. These allow the control system - * to read the current speed of the motor and apply more or less power to keep it at a constant - * velocity. Here we are setting the target, and minimum velocity that the launcher should run - * at. The minimum velocity is a threshold for determining when to fire. + * to read the current speed of the motor and apply more or less power to keep it at a constant velocity. + * Here we are setting the target, minimum, and maximum velocity that the launcher should run at for both our + * far(high) and near(low) launches. The minimum and maximum velocities are thresholds for determining when to launch. */ + public static double LAUNCHER_HIGH_MAX_VELOCITY = 2300; //high target velocity + 50 (will need adjusting) + public static double LAUNCHER_HIGH_TARGET_VELOCITY = 2250; + public static double LAUNCHER_HIGH_MIN_VELOCITY = 2200; - final double LAUNCHER_HIGH_MAX_VELOCITY = 2300; //high target velocity + 50 (will need adjusting) - final double LAUNCHER_HIGH_TARGET_VELOCITY = 2250; - final double LAUNCHER_HIGH_MIN_VELOCITY = 2200; - - final double LAUNCHER_LOW_MAX_VELOCITY = 1175; //low target velocity + 50 (will need adjusting) - final double LAUNCHER_LOW_TARGET_VELOCITY = 1125; - final double LAUNCHER_LOW_MIN_VELOCITY = 1075; + public static double LAUNCHER_LOW_MAX_VELOCITY = 1175; //low target velocity + 50 (will need adjusting) + public static double LAUNCHER_LOW_TARGET_VELOCITY = 1125; + public static double LAUNCHER_LOW_MIN_VELOCITY = 1075; boolean doHighLaunch = false; From 40f9b07fd6e395846b731ded7b095956c1e70ec5 Mon Sep 17 00:00:00 2001 From: anya-codes Date: Wed, 8 Oct 2025 23:25:58 -0700 Subject: [PATCH 019/198] Added code for sorting. Will need testing. --- .../ftc/team417/CompetitionTeleOp.java | 45 ++++++++++++++----- 1 file changed, 33 insertions(+), 12 deletions(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java index 0168f68f76cf..cb7a30f17d26 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java @@ -34,16 +34,24 @@ public class CompetitionTeleOp extends BaseOpMode { * Here we are setting the target, minimum, and maximum velocity that the launcher should run at for both our * far(high) and near(low) launches. The minimum and maximum velocities are thresholds for determining when to launch. */ - public static double LAUNCHER_HIGH_MAX_VELOCITY = 2300; //high target velocity + 50 (will need adjusting) + + //Launcher fast launch (from far) velocities - need to be adjusted + public static double LAUNCHER_HIGH_MAX_VELOCITY = 2300; public static double LAUNCHER_HIGH_TARGET_VELOCITY = 2250; public static double LAUNCHER_HIGH_MIN_VELOCITY = 2200; - public static double LAUNCHER_LOW_MAX_VELOCITY = 1175; //low target velocity + 50 (will need adjusting) + //Launcher slow launch (from closer) velocities - need to be adjusted + public static double LAUNCHER_LOW_MAX_VELOCITY = 1175; public static double LAUNCHER_LOW_TARGET_VELOCITY = 1125; public static double LAUNCHER_LOW_MIN_VELOCITY = 1075; + //Sorting velocities - need to be adjusted + public static double LAUNCHER_SORT_MAX_VELOCITY = 1000; + public static double LAUNCHER_SORT_TARGET_VELOCITY = 950; + public static double LAUNCHER_SORT_MIN_VELOCITY = 900; boolean doHighLaunch = false; + boolean sort = false; // Declare OpMode members. private DcMotorEx launcher = null; @@ -72,6 +80,7 @@ private enum LaunchState { IDLE, SPIN_UP_HIGH, SPIN_UP_LOW, + SORT, LAUNCH, LAUNCHING, } @@ -159,11 +168,6 @@ public void runOpMode() { ), -gamepad1.right_stick_x - - - - - )); // Update the current pose: @@ -181,14 +185,18 @@ public void runOpMode() { MecanumDrive.sendTelemetryPacket(packet); if (gamepad2.y) { //high speed - launcher.setVelocity(LAUNCHER_HIGH_TARGET_VELOCITY); + sort = false; doHighLaunch = true; } else if (gamepad2.a) { //slow speed - launcher.setVelocity(LAUNCHER_LOW_TARGET_VELOCITY); + sort = false; + doHighLaunch = false; } else if (gamepad2.b) { // stop flywheel launcher.setVelocity(STOP_SPEED); + } else if (gamepad2.x) { //sorting + sort = true; } + /* * Now we call our "Launch" function. */ @@ -208,10 +216,14 @@ void launch(boolean shotRequested) { switch (launchState) { case IDLE: if (shotRequested) { - if (doHighLaunch) { - launchState = LaunchState.SPIN_UP_HIGH; + if (sort) { + launchState = LaunchState.SORT; } else { - launchState = LaunchState.SPIN_UP_LOW; + if (doHighLaunch) { + launchState = LaunchState.SPIN_UP_HIGH; + } else if (!doHighLaunch) { + launchState = LaunchState.SPIN_UP_LOW; + } } } break; @@ -226,6 +238,13 @@ void launch(boolean shotRequested) { if (launcher.getVelocity() > LAUNCHER_HIGH_MIN_VELOCITY && launcher.getVelocity() < LAUNCHER_HIGH_MAX_VELOCITY) { launchState = LaunchState.LAUNCH; } + break; + case SORT: + launcher.setVelocity(LAUNCHER_SORT_TARGET_VELOCITY); + if (launcher.getVelocity() > LAUNCHER_SORT_MIN_VELOCITY && launcher.getVelocity() < LAUNCHER_SORT_MAX_VELOCITY) { + launchState = LaunchState.LAUNCH; + } + break; case LAUNCH: leftFeeder.setPower(FULL_SPEED); rightFeeder.setPower(FULL_SPEED); @@ -238,6 +257,8 @@ void launch(boolean shotRequested) { leftFeeder.setPower(STOP_SPEED); rightFeeder.setPower(STOP_SPEED); } + doHighLaunch = false; + sort = false; break; } } From 7cd56f51cab3ee5ec8a9165d8931d228a19a0469 Mon Sep 17 00:00:00 2001 From: anya-codes Date: Thu, 9 Oct 2025 07:32:16 -0700 Subject: [PATCH 020/198] Added code for sorting. Will need testing. (has only been committed not pushed) --- .../java/org/firstinspires/ftc/team417/CompetitionTeleOp.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java index cb7a30f17d26..d431749dd9e8 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java @@ -221,7 +221,7 @@ void launch(boolean shotRequested) { } else { if (doHighLaunch) { launchState = LaunchState.SPIN_UP_HIGH; - } else if (!doHighLaunch) { + } else { launchState = LaunchState.SPIN_UP_LOW; } } From 28ebd6e64a52b57a205097446fae39d1de58898c Mon Sep 17 00:00:00 2001 From: anya-codes Date: Thu, 9 Oct 2025 07:33:18 -0700 Subject: [PATCH 021/198] Added code for sorting. Will need testing. --- .../java/org/firstinspires/ftc/team417/CompetitionTeleOp.java | 1 - 1 file changed, 1 deletion(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java index d431749dd9e8..0a23b4b91745 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java @@ -196,7 +196,6 @@ public void runOpMode() { sort = true; } - /* * Now we call our "Launch" function. */ From eef35430f41dcffb2af422409fc5a4c378c4cb97 Mon Sep 17 00:00:00 2001 From: anya-codes Date: Thu, 9 Oct 2025 21:29:43 -0700 Subject: [PATCH 022/198] Changed launcher low velocities and removed sorting code --- .../ftc/team417/CompetitionTeleOp.java | 48 ++++++------------- 1 file changed, 14 insertions(+), 34 deletions(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java index 0a23b4b91745..b9424f14cb40 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java @@ -34,24 +34,16 @@ public class CompetitionTeleOp extends BaseOpMode { * Here we are setting the target, minimum, and maximum velocity that the launcher should run at for both our * far(high) and near(low) launches. The minimum and maximum velocities are thresholds for determining when to launch. */ + public static double LAUNCHER_HIGH_MAX_VELOCITY = 2000; //high target velocity + 50 (will need adjusting) + public static double LAUNCHER_HIGH_TARGET_VELOCITY = 1950; + public static double LAUNCHER_HIGH_MIN_VELOCITY = 1900; - //Launcher fast launch (from far) velocities - need to be adjusted - public static double LAUNCHER_HIGH_MAX_VELOCITY = 2300; - public static double LAUNCHER_HIGH_TARGET_VELOCITY = 2250; - public static double LAUNCHER_HIGH_MIN_VELOCITY = 2200; - - //Launcher slow launch (from closer) velocities - need to be adjusted - public static double LAUNCHER_LOW_MAX_VELOCITY = 1175; + public static double LAUNCHER_LOW_MAX_VELOCITY = 1175; //low target velocity + 50 (will need adjusting) public static double LAUNCHER_LOW_TARGET_VELOCITY = 1125; public static double LAUNCHER_LOW_MIN_VELOCITY = 1075; - //Sorting velocities - need to be adjusted - public static double LAUNCHER_SORT_MAX_VELOCITY = 1000; - public static double LAUNCHER_SORT_TARGET_VELOCITY = 950; - public static double LAUNCHER_SORT_MIN_VELOCITY = 900; boolean doHighLaunch = false; - boolean sort = false; // Declare OpMode members. private DcMotorEx launcher = null; @@ -80,7 +72,6 @@ private enum LaunchState { IDLE, SPIN_UP_HIGH, SPIN_UP_LOW, - SORT, LAUNCH, LAUNCHING, } @@ -168,6 +159,11 @@ public void runOpMode() { ), -gamepad1.right_stick_x + + + + + )); // Update the current pose: @@ -185,15 +181,12 @@ public void runOpMode() { MecanumDrive.sendTelemetryPacket(packet); if (gamepad2.y) { //high speed - sort = false; + launcher.setVelocity(LAUNCHER_HIGH_TARGET_VELOCITY); doHighLaunch = true; } else if (gamepad2.a) { //slow speed - sort = false; - doHighLaunch = false; + launcher.setVelocity(LAUNCHER_LOW_TARGET_VELOCITY); } else if (gamepad2.b) { // stop flywheel launcher.setVelocity(STOP_SPEED); - } else if (gamepad2.x) { //sorting - sort = true; } /* @@ -215,14 +208,10 @@ void launch(boolean shotRequested) { switch (launchState) { case IDLE: if (shotRequested) { - if (sort) { - launchState = LaunchState.SORT; + if (doHighLaunch) { + launchState = LaunchState.SPIN_UP_HIGH; } else { - if (doHighLaunch) { - launchState = LaunchState.SPIN_UP_HIGH; - } else { - launchState = LaunchState.SPIN_UP_LOW; - } + launchState = LaunchState.SPIN_UP_LOW; } } break; @@ -237,13 +226,6 @@ void launch(boolean shotRequested) { if (launcher.getVelocity() > LAUNCHER_HIGH_MIN_VELOCITY && launcher.getVelocity() < LAUNCHER_HIGH_MAX_VELOCITY) { launchState = LaunchState.LAUNCH; } - break; - case SORT: - launcher.setVelocity(LAUNCHER_SORT_TARGET_VELOCITY); - if (launcher.getVelocity() > LAUNCHER_SORT_MIN_VELOCITY && launcher.getVelocity() < LAUNCHER_SORT_MAX_VELOCITY) { - launchState = LaunchState.LAUNCH; - } - break; case LAUNCH: leftFeeder.setPower(FULL_SPEED); rightFeeder.setPower(FULL_SPEED); @@ -256,8 +238,6 @@ void launch(boolean shotRequested) { leftFeeder.setPower(STOP_SPEED); rightFeeder.setPower(STOP_SPEED); } - doHighLaunch = false; - sort = false; break; } } From ed364e9e320c13a35b1fc7f104effb6da347d7c3 Mon Sep 17 00:00:00 2001 From: Emmett Date: Sun, 12 Oct 2025 13:42:10 -0700 Subject: [PATCH 023/198] added sorting? --- .../ftc/team417/CompetitionTeleOp.java | 27 ++++++++++++++++--- 1 file changed, 23 insertions(+), 4 deletions(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java index b9424f14cb40..b79ea75d0498 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java @@ -32,7 +32,8 @@ public class CompetitionTeleOp extends BaseOpMode { * When we control our launcher motor, we are using encoders. These allow the control system * to read the current speed of the motor and apply more or less power to keep it at a constant velocity. * Here we are setting the target, minimum, and maximum velocity that the launcher should run at for both our - * far(high) and near(low) launches. The minimum and maximum velocities are thresholds for determining when to launch. + * far(high) and near(low) launches, as well as our sorting velocity. + * The minimum and maximum velocities are thresholds for determining when to launch. */ public static double LAUNCHER_HIGH_MAX_VELOCITY = 2000; //high target velocity + 50 (will need adjusting) public static double LAUNCHER_HIGH_TARGET_VELOCITY = 1950; @@ -42,8 +43,12 @@ public class CompetitionTeleOp extends BaseOpMode { public static double LAUNCHER_LOW_TARGET_VELOCITY = 1125; public static double LAUNCHER_LOW_MIN_VELOCITY = 1075; + public static double LAUNCHER_SORTER_MAX_VELOCITY = 550; //sorter target velocity + 50 (will need adjusting) + public static double LAUNCHER_SORTER_TARGET_VELOCITY = 500; + public static double LAUNCHER_SORTER_MIN_VELOCITY = 450; boolean doHighLaunch = false; + boolean doSort = false; // Declare OpMode members. private DcMotorEx launcher = null; @@ -72,6 +77,7 @@ private enum LaunchState { IDLE, SPIN_UP_HIGH, SPIN_UP_LOW, + SPIN_UP_SORT, LAUNCH, LAUNCHING, } @@ -161,9 +167,6 @@ public void runOpMode() { -gamepad1.right_stick_x - - - )); // Update the current pose: @@ -183,8 +186,15 @@ public void runOpMode() { if (gamepad2.y) { //high speed launcher.setVelocity(LAUNCHER_HIGH_TARGET_VELOCITY); doHighLaunch = true; + doSort = false; } else if (gamepad2.a) { //slow speed launcher.setVelocity(LAUNCHER_LOW_TARGET_VELOCITY); + doHighLaunch = false; + doSort = false; + } else if (gamepad2.x) { // sort speed + launcher.setVelocity(LAUNCHER_SORTER_TARGET_VELOCITY); + doHighLaunch = false; + doSort = true; } else if (gamepad2.b) { // stop flywheel launcher.setVelocity(STOP_SPEED); } @@ -210,11 +220,20 @@ void launch(boolean shotRequested) { if (shotRequested) { if (doHighLaunch) { launchState = LaunchState.SPIN_UP_HIGH; + } else if (doSort){ + launchState = LaunchState.SPIN_UP_SORT; } else { launchState = LaunchState.SPIN_UP_LOW; } } break; + + case SPIN_UP_SORT: + launcher.setVelocity(LAUNCHER_SORTER_TARGET_VELOCITY); + if (launcher.getVelocity() > LAUNCHER_SORTER_MIN_VELOCITY && launcher.getVelocity() < LAUNCHER_SORTER_MAX_VELOCITY) { + launchState = LaunchState.LAUNCH; + } + break; case SPIN_UP_LOW: launcher.setVelocity(LAUNCHER_LOW_TARGET_VELOCITY); if (launcher.getVelocity() > LAUNCHER_LOW_MIN_VELOCITY && launcher.getVelocity() < LAUNCHER_LOW_MAX_VELOCITY) { From b9396c30e82797d6404c6ce6a52db4ab88fd6eb6 Mon Sep 17 00:00:00 2001 From: Emmett Date: Sun, 12 Oct 2025 14:17:50 -0700 Subject: [PATCH 024/198] added reverse launching, to add a fallback in the case that sorting fails --- .../ftc/team417/CompetitionTeleOp.java | 28 +++++++++++++++++-- 1 file changed, 26 insertions(+), 2 deletions(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java index b79ea75d0498..d0c1e613cef0 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java @@ -1,5 +1,6 @@ package org.firstinspires.ftc.team417; +import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.TelemetryPacket; import com.acmerobotics.roadrunner.Pose2d; import com.acmerobotics.roadrunner.PoseVelocity2d; @@ -20,6 +21,7 @@ * BaseOpMode class rather than here so that it can be shared between both TeleOp and Autonomous. */ @TeleOp(name="TeleOp", group="Competition") +@Config public class CompetitionTeleOp extends BaseOpMode { public static double FEED_TIME_SECONDS = 0.20; //The feeder servos run this long when a shot is requested. public static final double STOP_SPEED = 0.0; //We send this power to the servos when we want them to stop. @@ -47,8 +49,13 @@ public class CompetitionTeleOp extends BaseOpMode { public static double LAUNCHER_SORTER_TARGET_VELOCITY = 500; public static double LAUNCHER_SORTER_MIN_VELOCITY = 450; + + public static double LAUNCHER_REV_MAX_VELOCITY = 250; + public static double LAUNCHER_REV_TARGET_VELOCITY = 250; + public static double LAUNCHER_REV_MIN_VELOCITY = 250; boolean doHighLaunch = false; boolean doSort = false; + boolean doReverse = false; // Declare OpMode members. private DcMotorEx launcher = null; @@ -78,6 +85,7 @@ private enum LaunchState { SPIN_UP_HIGH, SPIN_UP_LOW, SPIN_UP_SORT, + SPIN_UP_REV, LAUNCH, LAUNCHING, } @@ -187,15 +195,23 @@ public void runOpMode() { launcher.setVelocity(LAUNCHER_HIGH_TARGET_VELOCITY); doHighLaunch = true; doSort = false; + doReverse = false; } else if (gamepad2.a) { //slow speed launcher.setVelocity(LAUNCHER_LOW_TARGET_VELOCITY); doHighLaunch = false; doSort = false; + doReverse = false; } else if (gamepad2.x) { // sort speed launcher.setVelocity(LAUNCHER_SORTER_TARGET_VELOCITY); doHighLaunch = false; doSort = true; - } else if (gamepad2.b) { // stop flywheel + doReverse = false; + } else if (gamepad2.b) { // reverse + launcher.setVelocity(LAUNCHER_SORTER_TARGET_VELOCITY); + doHighLaunch = false; + doSort = false; + doReverse = true; + } else if (gamepad2.left_bumper) { // stop flywheel launcher.setVelocity(STOP_SPEED); } @@ -220,8 +236,10 @@ void launch(boolean shotRequested) { if (shotRequested) { if (doHighLaunch) { launchState = LaunchState.SPIN_UP_HIGH; - } else if (doSort){ + } else if (doSort) { launchState = LaunchState.SPIN_UP_SORT; + } else if (doReverse) { + launchState = LaunchState.SPIN_UP_REV; } else { launchState = LaunchState.SPIN_UP_LOW; } @@ -234,6 +252,12 @@ void launch(boolean shotRequested) { launchState = LaunchState.LAUNCH; } break; + case SPIN_UP_REV: + launcher.setVelocity(LAUNCHER_REV_TARGET_VELOCITY); + if (launcher.getVelocity() > LAUNCHER_REV_MIN_VELOCITY && launcher.getVelocity() < LAUNCHER_REV_MAX_VELOCITY) { + launchState = LaunchState.LAUNCH; + } + break; case SPIN_UP_LOW: launcher.setVelocity(LAUNCHER_LOW_TARGET_VELOCITY); if (launcher.getVelocity() > LAUNCHER_LOW_MIN_VELOCITY && launcher.getVelocity() < LAUNCHER_LOW_MAX_VELOCITY) { From d46b90bc25acc3a0138707fc566c56bf95bc5871 Mon Sep 17 00:00:00 2001 From: anya-codes Date: Sun, 12 Oct 2025 14:57:59 -0700 Subject: [PATCH 025/198] Added if conditional in CompetitionTeleOp to reverse launcher direction for DevBot because the motor is on the opposite side --- .../firstinspires/ftc/team417/CompetitionTeleOp.java | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java index d0c1e613cef0..f00ad023005d 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java @@ -50,9 +50,9 @@ public class CompetitionTeleOp extends BaseOpMode { public static double LAUNCHER_SORTER_MIN_VELOCITY = 450; - public static double LAUNCHER_REV_MAX_VELOCITY = 250; - public static double LAUNCHER_REV_TARGET_VELOCITY = 250; - public static double LAUNCHER_REV_MIN_VELOCITY = 250; + public static double LAUNCHER_REV_MAX_VELOCITY = -250; + public static double LAUNCHER_REV_TARGET_VELOCITY = -250; + public static double LAUNCHER_REV_MIN_VELOCITY = -250; boolean doHighLaunch = false; boolean doSort = false; boolean doReverse = false; @@ -112,6 +112,11 @@ public void runOpMode() { leftFeeder = hardwareMap.get(CRServo.class, "servoBLaunchFeed"); rightFeeder = hardwareMap.get(CRServo.class, "servoFLaunchFeed"); + // Reversed direction of launcher for DevBot because motor is on the other side (compared to FastBot) + if (MecanumDrive.isDevBot) { + launcher.setDirection(DcMotorEx.Direction.REVERSE); + } + /* * To drive forward, most robots need the motor on one side to be reversed, * because the axles point in opposite directions. Pushing the left stick forward From b047c6a9bcf96e78f8875e6a4d71f67ec4589d51 Mon Sep 17 00:00:00 2001 From: anya-codes Date: Sun, 12 Oct 2025 15:01:01 -0700 Subject: [PATCH 026/198] Fixed max and min velocity for launcher reverse so velocity doesn't have to be exactly -250 --- .../java/org/firstinspires/ftc/team417/CompetitionTeleOp.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java index f00ad023005d..795cd4360f27 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java @@ -50,9 +50,9 @@ public class CompetitionTeleOp extends BaseOpMode { public static double LAUNCHER_SORTER_MIN_VELOCITY = 450; - public static double LAUNCHER_REV_MAX_VELOCITY = -250; + public static double LAUNCHER_REV_MAX_VELOCITY = -300; public static double LAUNCHER_REV_TARGET_VELOCITY = -250; - public static double LAUNCHER_REV_MIN_VELOCITY = -250; + public static double LAUNCHER_REV_MIN_VELOCITY = -200; boolean doHighLaunch = false; boolean doSort = false; boolean doReverse = false; From a00a4a0765b82d643f3510e20b9cf38bafb2c138 Mon Sep 17 00:00:00 2001 From: Emmett Date: Sun, 12 Oct 2025 15:22:12 -0700 Subject: [PATCH 027/198] added telemetry --- .../java/org/firstinspires/ftc/team417/CompetitionTeleOp.java | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java index 795cd4360f27..38be8d0c4b93 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java @@ -164,6 +164,7 @@ public void runOpMode() { */ telemetry.addData("Status", "Initialized"); + // Wait for Start to be pressed on the Driver Hub! waitForStart(); @@ -231,6 +232,9 @@ public void runOpMode() { telemetry.addData("State", launchState); // telemetry.addData("Motors", "left (%.2f), right (%.2f)", leftPower, rightPower); telemetry.addData("motorSpeed", launcher.getVelocity()); + telemetry.addData("reverse", doReverse); + telemetry.addData("highLaunch", doHighLaunch); + telemetry.addData("sort", doSort); telemetry.update(); } From d3d94c8b5b0e3d40c068002ff9a56c1c9dcea46d Mon Sep 17 00:00:00 2001 From: bharv Date: Sun, 12 Oct 2025 15:57:48 -0700 Subject: [PATCH 028/198] Text menu working 3 auto paths (roughly) working - need to add waits and mechanism movements. --- .../ftc/team417/CompetitionAuto.java | 91 +++++++++++++++++-- .../ftc/team417/TextMenuTest.java | 82 ----------------- 2 files changed, 85 insertions(+), 88 deletions(-) delete mode 100644 team417/src/main/java/org/firstinspires/ftc/team417/TextMenuTest.java diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java index 58afabee67fe..f2b282235912 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java @@ -8,6 +8,11 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import org.firstinspires.ftc.team417.javatextmenu.MenuFinishedButton; +import org.firstinspires.ftc.team417.javatextmenu.MenuHeader; +import org.firstinspires.ftc.team417.javatextmenu.MenuInput; +import org.firstinspires.ftc.team417.javatextmenu.MenuSlider; +import org.firstinspires.ftc.team417.javatextmenu.TextMenu; import org.firstinspires.ftc.team417.roadrunner.MecanumDrive; /** @@ -16,18 +21,92 @@ */ @Autonomous(name="Auto", group="Competition", preselectTeleOp="CompetitionTeleOp") public class CompetitionAuto extends BaseOpMode { + enum Alliances { + RED, + BLUE, + } + + enum Movements { + NEAR, + FAR, + FAR_MINIMAL, + } + + double minWaitTime = 0.0; + double maxWaitTime = 15.0; + @Override public void runOpMode() { - Pose2d beginPose = new Pose2d(0, 0, 0); - MecanumDrive drive = new MecanumDrive(hardwareMap, telemetry, gamepad1, beginPose); + Pose2d beginPoseNear = new Pose2d(-50, 50, Math.toRadians(41)); + MecanumDrive drive = new MecanumDrive(hardwareMap, telemetry, gamepad1, beginPoseNear); + + Pose2d beginPoseFar = new Pose2d(56, 12, Math.toRadians(135)); + MecanumDrive drive1 = new MecanumDrive(hardwareMap, telemetry, gamepad1, beginPoseFar); // Build the trajectory *before* the start button is pressed because Road Runner // can take multiple seconds for this operation. We wouldn't want to have to wait // as soon as the Start button is pressed! - Action trajectoryAction = drive.actionBuilder(beginPose) - .splineTo(new Vector2d(30, 30), Math.PI / 2) - .splineTo(new Vector2d(0, 60), Math.PI) - .build(); + Action near = drive.actionBuilder(beginPoseNear) + .splineTo(new Vector2d(-20, 51), 0) + .build(); + + Action far = drive1.actionBuilder(beginPoseFar) + .splineTo(new Vector2d(-50, 50), Math.toRadians(41)) + .splineTo(new Vector2d(-20, 51), 0) + .build(); + + Action farMinimal = drive1.actionBuilder(beginPoseFar) + .setTangent(Math.PI/2) + .splineTo(new Vector2d(56, 35), Math.PI/2) + .build(); + + TextMenu menu = new TextMenu(); + MenuInput menuInput = new MenuInput(MenuInput.InputType.CONTROLLER); + + menu.add(new MenuHeader("AUTO SETUP")) + .add() // empty line for spacing + .add("Pick an alliance:") + .add("alliance-picker-1", Alliances.class) // enum selector shortcut + .add() + .add("Pick a movement:") + .add("movement-picker-1", Movements.class) // enum selector shortcut + .add() + .add("Wait time:") + .add("wait-slider-1", new MenuSlider(minWaitTime, maxWaitTime)) + .add() + .add("finish-button-1", new MenuFinishedButton()); + + while (!menu.isCompleted() && opModeIsActive()) { + // get x,y (stick) and select (A) input from controller + menuInput.update(gamepad1.left_stick_x, -gamepad1.left_stick_y, gamepad1.a); + menu.updateWithInput(menuInput); + // display the updated menu + for (String line : menu.toListOfStrings()) { + telemetry.addLine(line); // but with appropriate printing method + } + telemetry.update(); + } + + // the first parameter is the type to return as + Alliances chosenAlliance = menu.getResult(Alliances.class, "alliance-picker-1"); + Movements chosenMovement = menu.getResult(Movements.class, "movement-picker-1"); + double waitTime = menu.getResult(Double.class, "wait-slider-1"); + + Action trajectoryAction; + + switch (chosenMovement) { + case NEAR: + trajectoryAction = near; + break; + case FAR: + trajectoryAction = far; + break; + case FAR_MINIMAL: + trajectoryAction = farMinimal; + break; + default: + trajectoryAction = near; + } // Get a preview of the trajectory's path: Canvas previewCanvas = new Canvas(); diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/TextMenuTest.java b/team417/src/main/java/org/firstinspires/ftc/team417/TextMenuTest.java deleted file mode 100644 index b5f8f1af2722..000000000000 --- a/team417/src/main/java/org/firstinspires/ftc/team417/TextMenuTest.java +++ /dev/null @@ -1,82 +0,0 @@ -package org.firstinspires.ftc.team417; - -import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; - -import org.firstinspires.ftc.team417.javatextmenu.*; - -@Autonomous(name = "Text Menu Test") -public class TextMenuTest extends LinearOpMode { - - enum Alliances { - RED, - BLUE, - } - - enum Positions { - NEAR, - FAR, - } - - enum Movements { - MINIMAL, - LAUNCHING, - } - - double minWaitTime = 0.0; - double maxWaitTime = 15.0; - - @Override - public void runOpMode() throws InterruptedException { - TextMenu menu = new TextMenu(); - MenuInput menuInput = new MenuInput(MenuInput.InputType.CONTROLLER); - - menu.add(new MenuHeader("AUTO SETUP")) - .add() // empty line for spacing - .add("Pick an alliance:") - .add("alliance-picker-1", Alliances.class) // enum selector shortcut - .add() - .add("Pick a starting position:") - .add("position-picker-1", Positions.class) // enum selector shortcut - .add() - .add("Pick a movement:") - .add("movement-picker-1", Movements.class) // enum selector shortcut - .add() - .add("Wait time:") - .add("wait-slider-1", new MenuSlider(minWaitTime, maxWaitTime)) - .add() - .add("finish-button-1", new MenuFinishedButton()); - - while (!menu.isCompleted() && opModeIsActive()) { - // get x,y (stick) and select (A) input from controller - menuInput.update(gamepad1.left_stick_x, -gamepad1.left_stick_y, gamepad1.a); - menu.updateWithInput(menuInput); - // display the updated menu - for (String line : menu.toListOfStrings()) { - telemetry.addLine(line); // but with appropriate printing method - } - telemetry.update(); - } - - // the first parameter is the type to return as - Alliances chosenAlliance = menu.getResult(Alliances.class, "alliance-picker-1"); - Positions chosenPosition = menu.getResult(Positions.class, "position-picker-1"); - Movements chosenMovement = menu.getResult(Movements.class, "movement-picker-1"); - double waitTime = menu.getResult(Double.class, "wait-slider-1"); - - double startTime = System.currentTimeMillis(); - - while (opModeIsActive()) { - telemetry.update(); - telemetry.addData("Time (s)", (System.currentTimeMillis() - startTime) / 1000.0); - - telemetry.addData("Alliance", chosenAlliance); - telemetry.addData("Position", chosenPosition); - telemetry.addData("Movement", chosenMovement); - telemetry.addData("Wait time", waitTime); - } - - telemetry.addLine("Opmode is stopped"); - telemetry.update(); - } -} From e4b42fa097316463a2318de6c74cfd356fec829a Mon Sep 17 00:00:00 2001 From: Emmett Date: Sun, 12 Oct 2025 16:05:25 -0700 Subject: [PATCH 029/198] added telemetry --- .../java/org/firstinspires/ftc/team417/CompetitionTeleOp.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java index 38be8d0c4b93..a0f221e280f6 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java @@ -213,7 +213,7 @@ public void runOpMode() { doSort = true; doReverse = false; } else if (gamepad2.b) { // reverse - launcher.setVelocity(LAUNCHER_SORTER_TARGET_VELOCITY); + launcher.setVelocity(LAUNCHER_REV_TARGET_VELOCITY); doHighLaunch = false; doSort = false; doReverse = true; From 002acb78bdcd57639e3d119b63e3f02d69a6eeea Mon Sep 17 00:00:00 2001 From: anya-codes Date: Sun, 12 Oct 2025 16:50:06 -0700 Subject: [PATCH 030/198] Added code for LED in cases SPIN_UP_LOW, SPIN_UP_HIGH, and LAUNCHING. --- .../ftc/team417/CompetitionTeleOp.java | 27 +++++++++++++++++++ 1 file changed, 27 insertions(+) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java index a0f221e280f6..376f82838815 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java @@ -10,6 +10,7 @@ import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorEx; import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.LED; import com.qualcomm.robotcore.hardware.PIDFCoefficients; import com.qualcomm.robotcore.util.ElapsedTime; @@ -61,6 +62,8 @@ public class CompetitionTeleOp extends BaseOpMode { private DcMotorEx launcher = null; private CRServo leftFeeder = null; private CRServo rightFeeder = null; + private LED redLed; + private LED greenLed; ElapsedTime feederTimer = new ElapsedTime(); @@ -115,6 +118,16 @@ public void runOpMode() { // Reversed direction of launcher for DevBot because motor is on the other side (compared to FastBot) if (MecanumDrive.isDevBot) { launcher.setDirection(DcMotorEx.Direction.REVERSE); + + redLed = null; + greenLed = null; + + } + if (false) { + redLed = hardwareMap.get(LED.class, "redLed"); + greenLed = hardwareMap.get(LED.class, "greenLed"); + redLed.on(); + greenLed.off(); } /* @@ -270,13 +283,23 @@ void launch(boolean shotRequested) { case SPIN_UP_LOW: launcher.setVelocity(LAUNCHER_LOW_TARGET_VELOCITY); if (launcher.getVelocity() > LAUNCHER_LOW_MIN_VELOCITY && launcher.getVelocity() < LAUNCHER_LOW_MAX_VELOCITY) { + if (redLed != null && greenLed != null) { + redLed.off(); + greenLed.on(); + } launchState = LaunchState.LAUNCH; } break; case SPIN_UP_HIGH: launcher.setVelocity(LAUNCHER_HIGH_TARGET_VELOCITY); if (launcher.getVelocity() > LAUNCHER_HIGH_MIN_VELOCITY && launcher.getVelocity() < LAUNCHER_HIGH_MAX_VELOCITY) { + if (redLed != null && greenLed != null) { + redLed.off(); + greenLed.on(); + } launchState = LaunchState.LAUNCH; + + break; } case LAUNCH: leftFeeder.setPower(FULL_SPEED); @@ -290,6 +313,10 @@ void launch(boolean shotRequested) { leftFeeder.setPower(STOP_SPEED); rightFeeder.setPower(STOP_SPEED); } + if (redLed != null && greenLed != null) { + redLed.off(); + greenLed.on(); + } break; } } From bee463477694c317c10cd1161cd1c1e576cd68e9 Mon Sep 17 00:00:00 2001 From: Emmett Date: Sun, 12 Oct 2025 16:56:18 -0700 Subject: [PATCH 031/198] relocated code needed for Auto from CompetionTeleOp to BaseOpMode --- .../firstinspires/ftc/team417/BaseOpMode.java | 169 ++++++++++++++++ .../ftc/team417/CompetitionTeleOp.java | 191 +----------------- 2 files changed, 172 insertions(+), 188 deletions(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java b/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java index 9c33ae2b1949..7a0ebe175058 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java @@ -1,10 +1,179 @@ package org.firstinspires.ftc.team417; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.CRServo; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.PIDFCoefficients; +import com.qualcomm.robotcore.util.ElapsedTime; + +import org.firstinspires.ftc.team417.roadrunner.MecanumDrive; /** * This class contains all of the base logic that is shared between all of the TeleOp and * Autonomous logic. All TeleOp and Autonomous classes should derive from this class. */ abstract public class BaseOpMode extends LinearOpMode { + + public DcMotorEx launcher = null; + public CRServo leftFeeder = null; + public CRServo rightFeeder = null; + public static final double STOP_SPEED = 0.0; //We send this power to the servos when we want them to stop. + public static double FEED_TIME_SECONDS = 0.20; //The feeder servos run this long when a shot is requested. + + public static double FULL_SPEED = 1.0; //We send this power to the servos when we want them to feed an artifact to the launcher + public static double LAUNCHER_HIGH_MAX_VELOCITY = 2000; //high target velocity + 50 (will need adjusting) + public static double LAUNCHER_HIGH_TARGET_VELOCITY = 1950; + public static double LAUNCHER_HIGH_MIN_VELOCITY = 1900; + + public static double LAUNCHER_LOW_MAX_VELOCITY = 1175; //low target velocity + 50 (will need adjusting) + public static double LAUNCHER_LOW_TARGET_VELOCITY = 1125; + public static double LAUNCHER_LOW_MIN_VELOCITY = 1075; + + public static double LAUNCHER_SORTER_MAX_VELOCITY = 550; //sorter target velocity + 50 (will need adjusting) + public static double LAUNCHER_SORTER_TARGET_VELOCITY = 500; + public static double LAUNCHER_SORTER_MIN_VELOCITY = 450; + + + public static double LAUNCHER_REV_MAX_VELOCITY = -300; + public static double LAUNCHER_REV_TARGET_VELOCITY = -250; + public static double LAUNCHER_REV_MIN_VELOCITY = -200; + boolean doHighLaunch = false; + boolean doSort = false; + boolean doReverse = false; + ElapsedTime feederTimer = new ElapsedTime(); + + public enum LaunchState { + IDLE, + SPIN_UP_HIGH, + SPIN_UP_LOW, + SPIN_UP_SORT, + SPIN_UP_REV, + LAUNCH, + LAUNCHING, + } + + public LaunchState launchState; + public void initHardware() { + /* + * Initialize the hardware variables. Note that the strings used here as parameters + * to 'get' must correspond to the names assigned during the robot configuration + * step. + */ + // leftDrive = hardwareMap.get(DcMotor.class, "left_drive"); + // rightDrive = hardwareMap.get(DcMotor.class, "right_drive"); + launcher = hardwareMap.get(DcMotorEx.class, "motLauncher"); + leftFeeder = hardwareMap.get(CRServo.class, "servoBLaunchFeed"); + rightFeeder = hardwareMap.get(CRServo.class, "servoFLaunchFeed"); + + // Reversed direction of launcher for DevBot because motor is on the other side (compared to FastBot) + if (MecanumDrive.isDevBot) { + launcher.setDirection(DcMotorEx.Direction.REVERSE); + } + + /* + * To drive forward, most robots need the motor on one side to be reversed, + * because the axles point in opposite directions. Pushing the left stick forward + * MUST make robot go forward. So adjust these two lines based on your first test drive. + * Note: The settings here assume direct drive on left and right wheels. Gear + * Reduction or 90 Deg drives may require direction flips + */ + // leftDrive.setDirection(DcMotor.Direction.REVERSE); + // rightDrive.setDirection(DcMotor.Direction.FORWARD); + + /* + * Here we set our launcher to the RUN_USING_ENCODER runmode. + * If you notice that you have no control over the velocity of the motor, it just jumps + * right to a number much higher than your set point, make sure that your encoders are plugged + * into the port right beside the motor itself. And that the motors polarity is consistent + * through any wiring. + */ + launcher.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + + /* + * Setting zeroPowerBehavior to BRAKE enables a "brake mode". This causes the motor to + * slow down much faster when it is coasting. This creates a much more controllable + * drivetrain. As the robot stops much quicker. + */ + // leftDrive.setZeroPowerBehavior(BRAKE); + // rightDrive.setZeroPowerBehavior(BRAKE); + launcher.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + + /* + * set Feeders to an initial value to initialize the servo controller + */ + leftFeeder.setPower(STOP_SPEED); + rightFeeder.setPower(STOP_SPEED); + + launcher.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, new PIDFCoefficients(300, 0, 0, 10)); + + /* + * Much like our drivetrain motors, we set the left feeder servo to reverse so that they + * both work to feed the ball into the robot. + */ + leftFeeder.setDirection(DcMotorSimple.Direction.REVERSE); + + /* + * Tell the driver that initialization is complete. + */ + telemetry.addData("Status", "Initialized"); + } + + + public void launch(boolean shotRequested) { + switch (launchState) { + case IDLE: + if (shotRequested) { + if (doHighLaunch) { + launchState = LaunchState.SPIN_UP_HIGH; + } else if (doSort) { + launchState = LaunchState.SPIN_UP_SORT; + } else if (doReverse) { + launchState = LaunchState.SPIN_UP_REV; + } else { + launchState = LaunchState.SPIN_UP_LOW; + } + } + break; + + case SPIN_UP_SORT: + launcher.setVelocity(LAUNCHER_SORTER_TARGET_VELOCITY); + if (launcher.getVelocity() > LAUNCHER_SORTER_MIN_VELOCITY && launcher.getVelocity() < LAUNCHER_SORTER_MAX_VELOCITY) { + launchState = LaunchState.LAUNCH; + } + break; + case SPIN_UP_REV: + launcher.setVelocity(LAUNCHER_REV_TARGET_VELOCITY); + if (launcher.getVelocity() > LAUNCHER_REV_MIN_VELOCITY && launcher.getVelocity() < LAUNCHER_REV_MAX_VELOCITY) { + launchState = LaunchState.LAUNCH; + } + break; + case SPIN_UP_LOW: + launcher.setVelocity(LAUNCHER_LOW_TARGET_VELOCITY); + if (launcher.getVelocity() > LAUNCHER_LOW_MIN_VELOCITY && launcher.getVelocity() < LAUNCHER_LOW_MAX_VELOCITY) { + launchState = LaunchState.LAUNCH; + } + break; + case SPIN_UP_HIGH: + launcher.setVelocity(LAUNCHER_HIGH_TARGET_VELOCITY); + if (launcher.getVelocity() > LAUNCHER_HIGH_MIN_VELOCITY && launcher.getVelocity() < LAUNCHER_HIGH_MAX_VELOCITY) { + launchState = LaunchState.LAUNCH; + } + case LAUNCH: + leftFeeder.setPower(FULL_SPEED); + rightFeeder.setPower(FULL_SPEED); + feederTimer.reset(); + launchState = LaunchState.LAUNCHING; + break; + case LAUNCHING: + if (feederTimer.seconds() > FEED_TIME_SECONDS) { + launchState = LaunchState.IDLE; + leftFeeder.setPower(STOP_SPEED); + rightFeeder.setPower(STOP_SPEED); + } + break; + } + } } + diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java index a0f221e280f6..8966cc23243e 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java @@ -6,12 +6,6 @@ import com.acmerobotics.roadrunner.PoseVelocity2d; import com.acmerobotics.roadrunner.Vector2d; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; -import com.qualcomm.robotcore.hardware.CRServo; -import com.qualcomm.robotcore.hardware.DcMotor; -import com.qualcomm.robotcore.hardware.DcMotorEx; -import com.qualcomm.robotcore.hardware.DcMotorSimple; -import com.qualcomm.robotcore.hardware.PIDFCoefficients; -import com.qualcomm.robotcore.util.ElapsedTime; import org.firstinspires.ftc.team417.roadrunner.Drawing; import org.firstinspires.ftc.team417.roadrunner.MecanumDrive; @@ -23,76 +17,10 @@ @TeleOp(name="TeleOp", group="Competition") @Config public class CompetitionTeleOp extends BaseOpMode { - public static double FEED_TIME_SECONDS = 0.20; //The feeder servos run this long when a shot is requested. - public static final double STOP_SPEED = 0.0; //We send this power to the servos when we want them to stop. - public static double FULL_SPEED = 1.0; //We send this power to the servos when we want them to feed an artifact to the launcher double FASTDRIVE_SPEED = 1.0; double SLOWDRIVE_SPEED = 0.5; - /* - * When we control our launcher motor, we are using encoders. These allow the control system - * to read the current speed of the motor and apply more or less power to keep it at a constant velocity. - * Here we are setting the target, minimum, and maximum velocity that the launcher should run at for both our - * far(high) and near(low) launches, as well as our sorting velocity. - * The minimum and maximum velocities are thresholds for determining when to launch. - */ - public static double LAUNCHER_HIGH_MAX_VELOCITY = 2000; //high target velocity + 50 (will need adjusting) - public static double LAUNCHER_HIGH_TARGET_VELOCITY = 1950; - public static double LAUNCHER_HIGH_MIN_VELOCITY = 1900; - - public static double LAUNCHER_LOW_MAX_VELOCITY = 1175; //low target velocity + 50 (will need adjusting) - public static double LAUNCHER_LOW_TARGET_VELOCITY = 1125; - public static double LAUNCHER_LOW_MIN_VELOCITY = 1075; - - public static double LAUNCHER_SORTER_MAX_VELOCITY = 550; //sorter target velocity + 50 (will need adjusting) - public static double LAUNCHER_SORTER_TARGET_VELOCITY = 500; - public static double LAUNCHER_SORTER_MIN_VELOCITY = 450; - - - public static double LAUNCHER_REV_MAX_VELOCITY = -300; - public static double LAUNCHER_REV_TARGET_VELOCITY = -250; - public static double LAUNCHER_REV_MIN_VELOCITY = -200; - boolean doHighLaunch = false; - boolean doSort = false; - boolean doReverse = false; - - // Declare OpMode members. - private DcMotorEx launcher = null; - private CRServo leftFeeder = null; - private CRServo rightFeeder = null; - - ElapsedTime feederTimer = new ElapsedTime(); - - /* - * TECH TIP: State Machines - * We use a "state machine" to control our launcher motor and feeder servos in this program. - * The first step of a state machine is creating an enum that captures the different "states" - * that our code can be in. - * The core advantage of a state machine is that it allows us to continue to loop through all - * of our code while only running specific code when it's necessary. We can continuously check - * what "State" our machine is in, run the associated code, and when we are done with that step - * move on to the next state. - * This enum is called the "LaunchState". It reflects the current condition of the shooter - * motor and we move through the enum when the user asks our code to fire a shot. - * It starts at idle, when the user requests a launch, we enter SPIN_UP where we get the - * motor up to speed, once it meets a minimum speed then it starts and then ends the launch process. - * We can use higher level code to cycle through these states. But this allows us to write - * functions and autonomous routines in a way that avoids loops within loops, and "waits". - */ - private enum LaunchState { - IDLE, - SPIN_UP_HIGH, - SPIN_UP_LOW, - SPIN_UP_SORT, - SPIN_UP_REV, - LAUNCH, - LAUNCHING, - } - - private LaunchState launchState; - - @Override public void runOpMode() { @@ -101,69 +29,7 @@ public void runOpMode() { launchState = LaunchState.IDLE; - /* - * Initialize the hardware variables. Note that the strings used here as parameters - * to 'get' must correspond to the names assigned during the robot configuration - * step. - */ - // leftDrive = hardwareMap.get(DcMotor.class, "left_drive"); - // rightDrive = hardwareMap.get(DcMotor.class, "right_drive"); - launcher = hardwareMap.get(DcMotorEx.class, "motLauncher"); - leftFeeder = hardwareMap.get(CRServo.class, "servoBLaunchFeed"); - rightFeeder = hardwareMap.get(CRServo.class, "servoFLaunchFeed"); - - // Reversed direction of launcher for DevBot because motor is on the other side (compared to FastBot) - if (MecanumDrive.isDevBot) { - launcher.setDirection(DcMotorEx.Direction.REVERSE); - } - - /* - * To drive forward, most robots need the motor on one side to be reversed, - * because the axles point in opposite directions. Pushing the left stick forward - * MUST make robot go forward. So adjust these two lines based on your first test drive. - * Note: The settings here assume direct drive on left and right wheels. Gear - * Reduction or 90 Deg drives may require direction flips - */ - // leftDrive.setDirection(DcMotor.Direction.REVERSE); - // rightDrive.setDirection(DcMotor.Direction.FORWARD); - - /* - * Here we set our launcher to the RUN_USING_ENCODER runmode. - * If you notice that you have no control over the velocity of the motor, it just jumps - * right to a number much higher than your set point, make sure that your encoders are plugged - * into the port right beside the motor itself. And that the motors polarity is consistent - * through any wiring. - */ - launcher.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - - /* - * Setting zeroPowerBehavior to BRAKE enables a "brake mode". This causes the motor to - * slow down much faster when it is coasting. This creates a much more controllable - * drivetrain. As the robot stops much quicker. - */ - // leftDrive.setZeroPowerBehavior(BRAKE); - // rightDrive.setZeroPowerBehavior(BRAKE); - launcher.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - - /* - * set Feeders to an initial value to initialize the servo controller - */ - leftFeeder.setPower(STOP_SPEED); - rightFeeder.setPower(STOP_SPEED); - - launcher.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, new PIDFCoefficients(300, 0, 0, 10)); - - /* - * Much like our drivetrain motors, we set the left feeder servo to reverse so that they - * both work to feed the ball into the robot. - */ - leftFeeder.setDirection(DcMotorSimple.Direction.REVERSE); - - /* - * Tell the driver that initialization is complete. - */ - telemetry.addData("Status", "Initialized"); - + initHardware(); //initialize hardware and telemetry; found in BaseOpMode // Wait for Start to be pressed on the Driver Hub! waitForStart(); @@ -239,60 +105,9 @@ public void runOpMode() { telemetry.update(); } } - void launch(boolean shotRequested) { - switch (launchState) { - case IDLE: - if (shotRequested) { - if (doHighLaunch) { - launchState = LaunchState.SPIN_UP_HIGH; - } else if (doSort) { - launchState = LaunchState.SPIN_UP_SORT; - } else if (doReverse) { - launchState = LaunchState.SPIN_UP_REV; - } else { - launchState = LaunchState.SPIN_UP_LOW; - } - } - break; - case SPIN_UP_SORT: - launcher.setVelocity(LAUNCHER_SORTER_TARGET_VELOCITY); - if (launcher.getVelocity() > LAUNCHER_SORTER_MIN_VELOCITY && launcher.getVelocity() < LAUNCHER_SORTER_MAX_VELOCITY) { - launchState = LaunchState.LAUNCH; - } - break; - case SPIN_UP_REV: - launcher.setVelocity(LAUNCHER_REV_TARGET_VELOCITY); - if (launcher.getVelocity() > LAUNCHER_REV_MIN_VELOCITY && launcher.getVelocity() < LAUNCHER_REV_MAX_VELOCITY) { - launchState = LaunchState.LAUNCH; - } - break; - case SPIN_UP_LOW: - launcher.setVelocity(LAUNCHER_LOW_TARGET_VELOCITY); - if (launcher.getVelocity() > LAUNCHER_LOW_MIN_VELOCITY && launcher.getVelocity() < LAUNCHER_LOW_MAX_VELOCITY) { - launchState = LaunchState.LAUNCH; - } - break; - case SPIN_UP_HIGH: - launcher.setVelocity(LAUNCHER_HIGH_TARGET_VELOCITY); - if (launcher.getVelocity() > LAUNCHER_HIGH_MIN_VELOCITY && launcher.getVelocity() < LAUNCHER_HIGH_MAX_VELOCITY) { - launchState = LaunchState.LAUNCH; - } - case LAUNCH: - leftFeeder.setPower(FULL_SPEED); - rightFeeder.setPower(FULL_SPEED); - feederTimer.reset(); - launchState = LaunchState.LAUNCHING; - break; - case LAUNCHING: - if (feederTimer.seconds() > FEED_TIME_SECONDS) { - launchState = LaunchState.IDLE; - leftFeeder.setPower(STOP_SPEED); - rightFeeder.setPower(STOP_SPEED); - } - break; - } - } + + public double doSLOWMODE(){ if (gamepad1.left_stick_button) { return SLOWDRIVE_SPEED; From f7ea8977182c63a10a4c8ed3836a31e8d77f910d Mon Sep 17 00:00:00 2001 From: Emmett Date: Sun, 12 Oct 2025 22:28:26 -0700 Subject: [PATCH 032/198] added led code and manually merged CompetionTeleOp and BaseOpMode (again) to accomodate merge errors. --- .../firstinspires/ftc/team417/BaseOpMode.java | 27 +++ .../ftc/team417/CompetitionTeleOp.java | 194 +----------------- 2 files changed, 29 insertions(+), 192 deletions(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java b/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java index 7a0ebe175058..cf16387ba19a 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java @@ -5,6 +5,7 @@ import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorEx; import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.LED; import com.qualcomm.robotcore.hardware.PIDFCoefficients; import com.qualcomm.robotcore.util.ElapsedTime; @@ -42,6 +43,10 @@ abstract public class BaseOpMode extends LinearOpMode { boolean doHighLaunch = false; boolean doSort = false; boolean doReverse = false; + + public LED redLed; + public LED greenLed; + ElapsedTime feederTimer = new ElapsedTime(); public enum LaunchState { @@ -70,6 +75,15 @@ public void initHardware() { // Reversed direction of launcher for DevBot because motor is on the other side (compared to FastBot) if (MecanumDrive.isDevBot) { launcher.setDirection(DcMotorEx.Direction.REVERSE); + redLed = null; + greenLed = null; + + } + if (false) { + redLed = hardwareMap.get(LED.class, "redLed"); + greenLed = hardwareMap.get(LED.class, "greenLed"); + redLed.on(); + greenLed.off(); } /* @@ -152,12 +166,21 @@ public void launch(boolean shotRequested) { case SPIN_UP_LOW: launcher.setVelocity(LAUNCHER_LOW_TARGET_VELOCITY); if (launcher.getVelocity() > LAUNCHER_LOW_MIN_VELOCITY && launcher.getVelocity() < LAUNCHER_LOW_MAX_VELOCITY) { + if (redLed != null && greenLed != null) { + redLed.off(); + greenLed.on(); + } launchState = LaunchState.LAUNCH; + } break; case SPIN_UP_HIGH: launcher.setVelocity(LAUNCHER_HIGH_TARGET_VELOCITY); if (launcher.getVelocity() > LAUNCHER_HIGH_MIN_VELOCITY && launcher.getVelocity() < LAUNCHER_HIGH_MAX_VELOCITY) { + if (redLed != null && greenLed != null) { + redLed.off(); + greenLed.on(); + } launchState = LaunchState.LAUNCH; } case LAUNCH: @@ -172,6 +195,10 @@ public void launch(boolean shotRequested) { leftFeeder.setPower(STOP_SPEED); rightFeeder.setPower(STOP_SPEED); } + if (redLed != null && greenLed != null) { + redLed.off(); + greenLed.on(); + } break; } } diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java index 376f82838815..4598b5cb479a 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java @@ -24,49 +24,10 @@ @TeleOp(name="TeleOp", group="Competition") @Config public class CompetitionTeleOp extends BaseOpMode { - public static double FEED_TIME_SECONDS = 0.20; //The feeder servos run this long when a shot is requested. - public static final double STOP_SPEED = 0.0; //We send this power to the servos when we want them to stop. - public static double FULL_SPEED = 1.0; //We send this power to the servos when we want them to feed an artifact to the launcher double FASTDRIVE_SPEED = 1.0; double SLOWDRIVE_SPEED = 0.5; - /* - * When we control our launcher motor, we are using encoders. These allow the control system - * to read the current speed of the motor and apply more or less power to keep it at a constant velocity. - * Here we are setting the target, minimum, and maximum velocity that the launcher should run at for both our - * far(high) and near(low) launches, as well as our sorting velocity. - * The minimum and maximum velocities are thresholds for determining when to launch. - */ - public static double LAUNCHER_HIGH_MAX_VELOCITY = 2000; //high target velocity + 50 (will need adjusting) - public static double LAUNCHER_HIGH_TARGET_VELOCITY = 1950; - public static double LAUNCHER_HIGH_MIN_VELOCITY = 1900; - - public static double LAUNCHER_LOW_MAX_VELOCITY = 1175; //low target velocity + 50 (will need adjusting) - public static double LAUNCHER_LOW_TARGET_VELOCITY = 1125; - public static double LAUNCHER_LOW_MIN_VELOCITY = 1075; - - public static double LAUNCHER_SORTER_MAX_VELOCITY = 550; //sorter target velocity + 50 (will need adjusting) - public static double LAUNCHER_SORTER_TARGET_VELOCITY = 500; - public static double LAUNCHER_SORTER_MIN_VELOCITY = 450; - - - public static double LAUNCHER_REV_MAX_VELOCITY = -300; - public static double LAUNCHER_REV_TARGET_VELOCITY = -250; - public static double LAUNCHER_REV_MIN_VELOCITY = -200; - boolean doHighLaunch = false; - boolean doSort = false; - boolean doReverse = false; - - // Declare OpMode members. - private DcMotorEx launcher = null; - private CRServo leftFeeder = null; - private CRServo rightFeeder = null; - private LED redLed; - private LED greenLed; - - ElapsedTime feederTimer = new ElapsedTime(); - /* * TECH TIP: State Machines * We use a "state machine" to control our launcher motor and feeder servos in this program. @@ -83,19 +44,6 @@ public class CompetitionTeleOp extends BaseOpMode { * We can use higher level code to cycle through these states. But this allows us to write * functions and autonomous routines in a way that avoids loops within loops, and "waits". */ - private enum LaunchState { - IDLE, - SPIN_UP_HIGH, - SPIN_UP_LOW, - SPIN_UP_SORT, - SPIN_UP_REV, - LAUNCH, - LAUNCHING, - } - - private LaunchState launchState; - - @Override public void runOpMode() { @@ -104,79 +52,8 @@ public void runOpMode() { launchState = LaunchState.IDLE; - /* - * Initialize the hardware variables. Note that the strings used here as parameters - * to 'get' must correspond to the names assigned during the robot configuration - * step. - */ - // leftDrive = hardwareMap.get(DcMotor.class, "left_drive"); - // rightDrive = hardwareMap.get(DcMotor.class, "right_drive"); - launcher = hardwareMap.get(DcMotorEx.class, "motLauncher"); - leftFeeder = hardwareMap.get(CRServo.class, "servoBLaunchFeed"); - rightFeeder = hardwareMap.get(CRServo.class, "servoFLaunchFeed"); - - // Reversed direction of launcher for DevBot because motor is on the other side (compared to FastBot) - if (MecanumDrive.isDevBot) { - launcher.setDirection(DcMotorEx.Direction.REVERSE); - - redLed = null; - greenLed = null; - - } - if (false) { - redLed = hardwareMap.get(LED.class, "redLed"); - greenLed = hardwareMap.get(LED.class, "greenLed"); - redLed.on(); - greenLed.off(); - } - - /* - * To drive forward, most robots need the motor on one side to be reversed, - * because the axles point in opposite directions. Pushing the left stick forward - * MUST make robot go forward. So adjust these two lines based on your first test drive. - * Note: The settings here assume direct drive on left and right wheels. Gear - * Reduction or 90 Deg drives may require direction flips - */ - // leftDrive.setDirection(DcMotor.Direction.REVERSE); - // rightDrive.setDirection(DcMotor.Direction.FORWARD); - - /* - * Here we set our launcher to the RUN_USING_ENCODER runmode. - * If you notice that you have no control over the velocity of the motor, it just jumps - * right to a number much higher than your set point, make sure that your encoders are plugged - * into the port right beside the motor itself. And that the motors polarity is consistent - * through any wiring. - */ - launcher.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - - /* - * Setting zeroPowerBehavior to BRAKE enables a "brake mode". This causes the motor to - * slow down much faster when it is coasting. This creates a much more controllable - * drivetrain. As the robot stops much quicker. - */ - // leftDrive.setZeroPowerBehavior(BRAKE); - // rightDrive.setZeroPowerBehavior(BRAKE); - launcher.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - - /* - * set Feeders to an initial value to initialize the servo controller - */ - leftFeeder.setPower(STOP_SPEED); - rightFeeder.setPower(STOP_SPEED); - - launcher.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, new PIDFCoefficients(300, 0, 0, 10)); - - /* - * Much like our drivetrain motors, we set the left feeder servo to reverse so that they - * both work to feed the ball into the robot. - */ - leftFeeder.setDirection(DcMotorSimple.Direction.REVERSE); - - /* - * Tell the driver that initialization is complete. - */ - telemetry.addData("Status", "Initialized"); - + // Initialize motors, servos, LEDs + initHardware(); // Wait for Start to be pressed on the Driver Hub! waitForStart(); @@ -252,74 +129,7 @@ public void runOpMode() { telemetry.update(); } } - void launch(boolean shotRequested) { - switch (launchState) { - case IDLE: - if (shotRequested) { - if (doHighLaunch) { - launchState = LaunchState.SPIN_UP_HIGH; - } else if (doSort) { - launchState = LaunchState.SPIN_UP_SORT; - } else if (doReverse) { - launchState = LaunchState.SPIN_UP_REV; - } else { - launchState = LaunchState.SPIN_UP_LOW; - } - } - break; - - case SPIN_UP_SORT: - launcher.setVelocity(LAUNCHER_SORTER_TARGET_VELOCITY); - if (launcher.getVelocity() > LAUNCHER_SORTER_MIN_VELOCITY && launcher.getVelocity() < LAUNCHER_SORTER_MAX_VELOCITY) { - launchState = LaunchState.LAUNCH; - } - break; - case SPIN_UP_REV: - launcher.setVelocity(LAUNCHER_REV_TARGET_VELOCITY); - if (launcher.getVelocity() > LAUNCHER_REV_MIN_VELOCITY && launcher.getVelocity() < LAUNCHER_REV_MAX_VELOCITY) { - launchState = LaunchState.LAUNCH; - } - break; - case SPIN_UP_LOW: - launcher.setVelocity(LAUNCHER_LOW_TARGET_VELOCITY); - if (launcher.getVelocity() > LAUNCHER_LOW_MIN_VELOCITY && launcher.getVelocity() < LAUNCHER_LOW_MAX_VELOCITY) { - if (redLed != null && greenLed != null) { - redLed.off(); - greenLed.on(); - } - launchState = LaunchState.LAUNCH; - } - break; - case SPIN_UP_HIGH: - launcher.setVelocity(LAUNCHER_HIGH_TARGET_VELOCITY); - if (launcher.getVelocity() > LAUNCHER_HIGH_MIN_VELOCITY && launcher.getVelocity() < LAUNCHER_HIGH_MAX_VELOCITY) { - if (redLed != null && greenLed != null) { - redLed.off(); - greenLed.on(); - } - launchState = LaunchState.LAUNCH; - break; - } - case LAUNCH: - leftFeeder.setPower(FULL_SPEED); - rightFeeder.setPower(FULL_SPEED); - feederTimer.reset(); - launchState = LaunchState.LAUNCHING; - break; - case LAUNCHING: - if (feederTimer.seconds() > FEED_TIME_SECONDS) { - launchState = LaunchState.IDLE; - leftFeeder.setPower(STOP_SPEED); - rightFeeder.setPower(STOP_SPEED); - } - if (redLed != null && greenLed != null) { - redLed.off(); - greenLed.on(); - } - break; - } - } public double doSLOWMODE(){ if (gamepad1.left_stick_button) { return SLOWDRIVE_SPEED; From eb3a166abb6ec5ff4ca6e1c4ea8305c903595874 Mon Sep 17 00:00:00 2001 From: Emmett Date: Sun, 12 Oct 2025 22:29:13 -0700 Subject: [PATCH 033/198] removed unused imports (relocated to BaseOpMode) --- .../org/firstinspires/ftc/team417/CompetitionTeleOp.java | 7 ------- 1 file changed, 7 deletions(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java index 4598b5cb479a..aea375188cef 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java @@ -6,13 +6,6 @@ import com.acmerobotics.roadrunner.PoseVelocity2d; import com.acmerobotics.roadrunner.Vector2d; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; -import com.qualcomm.robotcore.hardware.CRServo; -import com.qualcomm.robotcore.hardware.DcMotor; -import com.qualcomm.robotcore.hardware.DcMotorEx; -import com.qualcomm.robotcore.hardware.DcMotorSimple; -import com.qualcomm.robotcore.hardware.LED; -import com.qualcomm.robotcore.hardware.PIDFCoefficients; -import com.qualcomm.robotcore.util.ElapsedTime; import org.firstinspires.ftc.team417.roadrunner.Drawing; import org.firstinspires.ftc.team417.roadrunner.MecanumDrive; From eea929615aca3e2f6d82c5fc53a2618847be8fa4 Mon Sep 17 00:00:00 2001 From: Hankang Zhou <136044263+nobody-particular@users.noreply.github.com> Date: Tue, 14 Oct 2025 19:29:36 -0700 Subject: [PATCH 034/198] Added comment to explain controls for text menu on WilyWorks --- .../java/org/firstinspires/ftc/team417/CompetitionAuto.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java index f2b282235912..6370559f764c 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java @@ -77,7 +77,8 @@ public void runOpMode() { .add("finish-button-1", new MenuFinishedButton()); while (!menu.isCompleted() && opModeIsActive()) { - // get x,y (stick) and select (A) input from controller + // get x, y (stick) and select (A) input from controller + // on wilyworks, this is x, y (wasd) and select (enter) on the keyboard menuInput.update(gamepad1.left_stick_x, -gamepad1.left_stick_y, gamepad1.a); menu.updateWithInput(menuInput); // display the updated menu From 6be35bb00adab45ea74c89185c64f99bb1213f7b Mon Sep 17 00:00:00 2001 From: anya-codes Date: Tue, 14 Oct 2025 20:06:31 -0700 Subject: [PATCH 035/198] Added 3 new auto paths for blue alliance and updated enums for blue alliance. Coordinates and angles for blue alliance have not been changed yet --- .../ftc/team417/CompetitionAuto.java | 62 ++++++++++++++----- 1 file changed, 47 insertions(+), 15 deletions(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java index f2b282235912..8bb19d2ec3c9 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java @@ -46,19 +46,36 @@ public void runOpMode() { // Build the trajectory *before* the start button is pressed because Road Runner // can take multiple seconds for this operation. We wouldn't want to have to wait // as soon as the Start button is pressed! - Action near = drive.actionBuilder(beginPoseNear) + + // Red alliance auto paths + Action redNear = drive.actionBuilder(beginPoseNear) .splineTo(new Vector2d(-20, 51), 0) .build(); - Action far = drive1.actionBuilder(beginPoseFar) + Action redFar = drive1.actionBuilder(beginPoseFar) .splineTo(new Vector2d(-50, 50), Math.toRadians(41)) .splineTo(new Vector2d(-20, 51), 0) .build(); - Action farMinimal = drive1.actionBuilder(beginPoseFar) + Action redFarMinimal = drive1.actionBuilder(beginPoseFar) .setTangent(Math.PI/2) .splineTo(new Vector2d(56, 35), Math.PI/2) .build(); + + // Blue alliance auto paths + Action blueNear = drive.actionBuilder(beginPoseNear) + .splineTo(new Vector2d(-20, -51), 0) + .build(); + + Action blueFar = drive1.actionBuilder(beginPoseFar) + .splineTo(new Vector2d(-50, -50), Math.toRadians(41)) + .splineTo(new Vector2d(-20, -51), 0) + .build(); + + Action blueFarMinimal = drive1.actionBuilder(beginPoseFar) + .setTangent(Math.PI/2) + .splineTo(new Vector2d(56, -35), Math.PI/2) + .build(); TextMenu menu = new TextMenu(); MenuInput menuInput = new MenuInput(MenuInput.InputType.CONTROLLER); @@ -92,20 +109,35 @@ public void runOpMode() { Movements chosenMovement = menu.getResult(Movements.class, "movement-picker-1"); double waitTime = menu.getResult(Double.class, "wait-slider-1"); - Action trajectoryAction; - - switch (chosenMovement) { - case NEAR: - trajectoryAction = near; + Action trajectoryAction = null; + switch (chosenAlliance) { + case RED: + switch (chosenMovement) { + case NEAR: + trajectoryAction = redNear; + break; + case FAR: + trajectoryAction = redFar; + break; + case FAR_MINIMAL: + trajectoryAction = redFarMinimal; + break; + } break; - case FAR: - trajectoryAction = far; - break; - case FAR_MINIMAL: - trajectoryAction = farMinimal; + + case BLUE: + switch (chosenMovement) { + case NEAR: + trajectoryAction = blueNear; + break; + case FAR: + trajectoryAction = blueFar; + break; + case FAR_MINIMAL: + trajectoryAction = blueFarMinimal; + break; + } break; - default: - trajectoryAction = near; } // Get a preview of the trajectory's path: From d98b84c57f6dff3d3ec0c77c806cd136a790a174 Mon Sep 17 00:00:00 2001 From: Emmett Date: Thu, 16 Oct 2025 20:32:27 -0700 Subject: [PATCH 036/198] updated values using loonytune --- .../ftc/team417/roadrunner/MecanumDrive.java | 34 +++++++++---------- 1 file changed, 17 insertions(+), 17 deletions(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/roadrunner/MecanumDrive.java b/team417/src/main/java/org/firstinspires/ftc/team417/roadrunner/MecanumDrive.java index e057eae53123..fce2d74b6085 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/roadrunner/MecanumDrive.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/roadrunner/MecanumDrive.java @@ -123,19 +123,19 @@ public static class Params { logoFacingDirection = RevHubOrientationOnRobot.LogoFacingDirection.UP; usbFacingDirection = RevHubOrientationOnRobot.UsbFacingDirection.FORWARD; - inPerTick = 1; - lateralInPerTick = inPerTick; - trackWidthTicks = 0; - - kS = 0; - kV = 0; - kA = 0; - - axialGain = 0.0; - axialVelGain = 0.0; - lateralGain = 0.0; - lateralVelGain = 0.0; - headingGain = 0.0; + inPerTick = 1.0; + lateralInPerTick = 0.798; + trackWidthTicks = 13.82; + + kS = 0.625; + kV = 0.183; + kA = 0.0110; + + axialGain = 2.0; + axialVelGain = 0.55; + lateralGain = 9.0; + lateralVelGain = 2.0; + headingGain = 9.4; headingVelGain = 0.0; otos.offset.x = 0; @@ -144,11 +144,11 @@ public static class Params { otos.linearScalar = 0; otos.angularScalar = 0; - pinpoint.ticksPerMm = 0; + pinpoint.ticksPerMm = 19.589; pinpoint.xReversed = false; - pinpoint.yReversed = false; - pinpoint.xOffset = 0; - pinpoint.yOffset = 0; + pinpoint.yReversed = true; + pinpoint.xOffset = 119.9; + pinpoint.yOffset = 5.4; } } From ccb7f5a0964db7243367be2b56643a4741acecec Mon Sep 17 00:00:00 2001 From: anya-codes Date: Thu, 16 Oct 2025 20:55:59 -0700 Subject: [PATCH 037/198] Reordered code so start position is selected after text menu. Fixed auto path coordinates and tangents. --- .../ftc/team417/CompetitionAuto.java | 86 +++++++++++-------- 1 file changed, 49 insertions(+), 37 deletions(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java index 635b5d424ed0..57aa04b48b58 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java @@ -37,46 +37,16 @@ enum Movements { @Override public void runOpMode() { - Pose2d beginPoseNear = new Pose2d(-50, 50, Math.toRadians(41)); - MecanumDrive drive = new MecanumDrive(hardwareMap, telemetry, gamepad1, beginPoseNear); + Pose2d startPose = new Pose2d(0, 0, 0); - Pose2d beginPoseFar = new Pose2d(56, 12, Math.toRadians(135)); - MecanumDrive drive1 = new MecanumDrive(hardwareMap, telemetry, gamepad1, beginPoseFar); + Pose2d redNearStartPose = new Pose2d(-48, 48, Math.toRadians(41)); + Pose2d redFarStartPose = new Pose2d(56, 12, Math.toRadians(180)); - // Build the trajectory *before* the start button is pressed because Road Runner - // can take multiple seconds for this operation. We wouldn't want to have to wait - // as soon as the Start button is pressed! - - // Red alliance auto paths - Action redNear = drive.actionBuilder(beginPoseNear) - .splineTo(new Vector2d(-20, 51), 0) - .build(); - - Action redFar = drive1.actionBuilder(beginPoseFar) - .splineTo(new Vector2d(-50, 50), Math.toRadians(41)) - .splineTo(new Vector2d(-20, 51), 0) - .build(); - - Action redFarMinimal = drive1.actionBuilder(beginPoseFar) - .setTangent(Math.PI/2) - .splineTo(new Vector2d(56, 35), Math.PI/2) - .build(); - - // Blue alliance auto paths - Action blueNear = drive.actionBuilder(beginPoseNear) - .splineTo(new Vector2d(-20, -51), 0) - .build(); + Pose2d blueNearStartPose = new Pose2d(-48, -48, Math.toRadians(139)); + Pose2d blueFarStartPose = new Pose2d(56, -12, Math.toRadians(180)); - Action blueFar = drive1.actionBuilder(beginPoseFar) - .splineTo(new Vector2d(-50, -50), Math.toRadians(41)) - .splineTo(new Vector2d(-20, -51), 0) - .build(); + MecanumDrive drive = new MecanumDrive(hardwareMap, telemetry, gamepad1, startPose); - Action blueFarMinimal = drive1.actionBuilder(beginPoseFar) - .setTangent(Math.PI/2) - .splineTo(new Vector2d(56, -35), Math.PI/2) - .build(); - TextMenu menu = new TextMenu(); MenuInput menuInput = new MenuInput(MenuInput.InputType.CONTROLLER); @@ -93,7 +63,7 @@ public void runOpMode() { .add() .add("finish-button-1", new MenuFinishedButton()); - while (!menu.isCompleted() && opModeIsActive()) { + while (!menu.isCompleted()) { // get x, y (stick) and select (A) input from controller // on wilyworks, this is x, y (wasd) and select (enter) on the keyboard menuInput.update(gamepad1.left_stick_x, -gamepad1.left_stick_y, gamepad1.a); @@ -110,17 +80,50 @@ public void runOpMode() { Movements chosenMovement = menu.getResult(Movements.class, "movement-picker-1"); double waitTime = menu.getResult(Double.class, "wait-slider-1"); + // Red alliance auto paths + Action redNear = drive.actionBuilder(redNearStartPose) + .splineTo(new Vector2d(-20, 51), Math.toRadians(0)) + .build(); + + Action redFar = drive.actionBuilder(redFarStartPose) + .splineTo(new Vector2d(-50, 50), Math.toRadians(41)) + .splineTo(new Vector2d(-20, 51), 0) + .build(); + + Action redFarMinimal = drive.actionBuilder(redFarStartPose) + .setTangent(Math.PI/2) + .splineTo(new Vector2d(-56, 35), Math.PI/2) + .build(); + + // Blue alliance auto paths + Action blueNear = drive.actionBuilder(blueNearStartPose) + .splineTo(new Vector2d(-20, -51), Math.toRadians(135)) + .build(); + + Action blueFar = drive.actionBuilder(blueFarStartPose) + .splineTo(new Vector2d(-50, -50), Math.toRadians(41)) + .build(); + + Action blueFarMinimal = drive.actionBuilder(blueFarStartPose) + .setTangent(Math.PI/2) + .splineTo(new Vector2d(-56, 35), Math.PI/2) + .build(); + + Action trajectoryAction = null; switch (chosenAlliance) { case RED: switch (chosenMovement) { case NEAR: + drive.setPose(redNearStartPose); trajectoryAction = redNear; break; case FAR: + drive.setPose(redFarStartPose); trajectoryAction = redFar; break; case FAR_MINIMAL: + drive.setPose(redFarStartPose); trajectoryAction = redFarMinimal; break; } @@ -129,12 +132,15 @@ public void runOpMode() { case BLUE: switch (chosenMovement) { case NEAR: + drive.setPose(blueNearStartPose); trajectoryAction = blueNear; break; case FAR: + drive.setPose(blueFarStartPose); trajectoryAction = blueFar; break; case FAR_MINIMAL: + drive.setPose(blueFarStartPose); trajectoryAction = blueFarMinimal; break; } @@ -150,6 +156,12 @@ public void runOpMode() { packet.fieldOverlay().getOperations().addAll(previewCanvas.getOperations()); MecanumDrive.sendTelemetryPacket(packet); + + // Build the trajectory *before* the start button is pressed because Road Runner + // can take multiple seconds for this operation. We wouldn't want to have to wait + // as soon as the Start button is pressed! + + // Wait for Start to be pressed on the Driver Hub! waitForStart(); From 82bed67071a2883f03b6b4d8a84d59b1e335fc01 Mon Sep 17 00:00:00 2001 From: anya-codes Date: Sat, 18 Oct 2025 17:12:34 -0700 Subject: [PATCH 038/198] Decreased feed time, added code to lock the right bumper control for 0.25 seconds after feeding each time, and modified blueNear auto path at League Meet 0 --- .../java/org/firstinspires/ftc/team417/BaseOpMode.java | 2 +- .../org/firstinspires/ftc/team417/CompetitionAuto.java | 7 +++++-- .../org/firstinspires/ftc/team417/CompetitionTeleOp.java | 8 ++++++++ 3 files changed, 14 insertions(+), 3 deletions(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java b/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java index cf16387ba19a..e728ebc41106 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java @@ -21,7 +21,7 @@ abstract public class BaseOpMode extends LinearOpMode { public CRServo leftFeeder = null; public CRServo rightFeeder = null; public static final double STOP_SPEED = 0.0; //We send this power to the servos when we want them to stop. - public static double FEED_TIME_SECONDS = 0.20; //The feeder servos run this long when a shot is requested. + public static double FEED_TIME_SECONDS = 0.15; //The feeder servos run this long when a shot is requested. public static double FULL_SPEED = 1.0; //We send this power to the servos when we want them to feed an artifact to the launcher public static double LAUNCHER_HIGH_MAX_VELOCITY = 2000; //high target velocity + 50 (will need adjusting) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java index 57aa04b48b58..e1031a83c322 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java @@ -34,7 +34,7 @@ enum Movements { double minWaitTime = 0.0; double maxWaitTime = 15.0; - + @Override public void runOpMode() { Pose2d startPose = new Pose2d(0, 0, 0); @@ -97,7 +97,10 @@ public void runOpMode() { // Blue alliance auto paths Action blueNear = drive.actionBuilder(blueNearStartPose) - .splineTo(new Vector2d(-20, -51), Math.toRadians(135)) + .setTangent(Math.toRadians(49)) + .splineTo(new Vector2d(-44, -44), Math.toRadians(49)) + .setTangent(Math.toRadians(139)) + .splineTo(new Vector2d(-54,-38), Math.toRadians(139)) .build(); Action blueFar = drive.actionBuilder(blueFarStartPose) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java index aea375188cef..4af96e26ed31 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java @@ -6,6 +6,7 @@ import com.acmerobotics.roadrunner.PoseVelocity2d; import com.acmerobotics.roadrunner.Vector2d; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.util.ElapsedTime; import org.firstinspires.ftc.team417.roadrunner.Drawing; import org.firstinspires.ftc.team417.roadrunner.MecanumDrive; @@ -21,6 +22,8 @@ public class CompetitionTeleOp extends BaseOpMode { double FASTDRIVE_SPEED = 1.0; double SLOWDRIVE_SPEED = 0.5; + ElapsedTime rightBumperTimer = new ElapsedTime(); + /* * TECH TIP: State Machines * We use a "state machine" to control our launcher motor and feeder servos in this program. @@ -107,6 +110,11 @@ public void runOpMode() { /* * Now we call our "Launch" function. */ + if (rightBumperTimer.seconds() > 0.25) { + launch(gamepad2.rightBumperWasPressed()); + rightBumperTimer.reset(); + } + launch(gamepad2.rightBumperWasPressed()); /* From fbb11bc71185aebab47722e7a6f5a276b4d2f9fe Mon Sep 17 00:00:00 2001 From: Emmett Date: Sun, 19 Oct 2025 13:10:46 -0700 Subject: [PATCH 039/198] updated tuning values for devbot --- .../ftc/team417/roadrunner/MecanumDrive.java | 32 +++++++++---------- 1 file changed, 16 insertions(+), 16 deletions(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/roadrunner/MecanumDrive.java b/team417/src/main/java/org/firstinspires/ftc/team417/roadrunner/MecanumDrive.java index fce2d74b6085..cdbac2c4dddc 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/roadrunner/MecanumDrive.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/roadrunner/MecanumDrive.java @@ -92,20 +92,20 @@ public static class Params { logoFacingDirection = RevHubOrientationOnRobot.LogoFacingDirection.UP; usbFacingDirection = RevHubOrientationOnRobot.UsbFacingDirection.FORWARD; - inPerTick = 1.0; - lateralInPerTick = 1.0; - trackWidthTicks = 0; + inPerTick = 1.000; + lateralInPerTick = 0.772; + trackWidthTicks = 16.35; - kS = 0; - kV = 0; - kA = 0; + kS = 0.722; + kV = 0.174; + kA = 0.0080; - axialGain = 0; - axialVelGain = 0; - lateralGain = 0; - lateralVelGain = 0; - headingGain = 0; - headingVelGain = 0; + axialGain = 8.50; + axialVelGain = 0.20; + lateralGain = 6.70; + lateralVelGain = 0.20; + headingGain = 1.11; + headingVelGain = 0.00; otos.offset.x = 0; otos.offset.y = 0; @@ -113,11 +113,11 @@ public static class Params { otos.linearScalar = 0; otos.angularScalar = 0; - pinpoint.ticksPerMm = 0; - pinpoint.xReversed = false; + pinpoint.ticksPerMm = 71.665; + pinpoint.xReversed = true; pinpoint.yReversed = false; - pinpoint.xOffset = 0; - pinpoint.yOffset = 0; + pinpoint.xOffset = 199.4; + pinpoint.yOffset = 120.2; } else { // Your competition robot Loony Tune configuration is here: logoFacingDirection = RevHubOrientationOnRobot.LogoFacingDirection.UP; From fa563ba70e34dc90b8b77147fe444d3123d9c5e5 Mon Sep 17 00:00:00 2001 From: Team <417skid@swerverobotics.club> Date: Sun, 19 Oct 2025 15:11:55 -0700 Subject: [PATCH 040/198] Launch Actions for Autonomous --- .../firstinspires/ftc/team417/BaseOpMode.java | 43 ++++++++++++++++--- .../ftc/team417/CompetitionAuto.java | 11 +++-- .../ftc/team417/CompetitionTeleOp.java | 2 - .../ftc/team417/roadrunner/MecanumDrive.java | 2 +- 4 files changed, 47 insertions(+), 11 deletions(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java b/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java index e728ebc41106..3b47e1091742 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java @@ -10,6 +10,7 @@ import com.qualcomm.robotcore.util.ElapsedTime; import org.firstinspires.ftc.team417.roadrunner.MecanumDrive; +import org.firstinspires.ftc.team417.roadrunner.RobotAction; /** * This class contains all of the base logic that is shared between all of the TeleOp and @@ -36,7 +37,9 @@ abstract public class BaseOpMode extends LinearOpMode { public static double LAUNCHER_SORTER_TARGET_VELOCITY = 500; public static double LAUNCHER_SORTER_MIN_VELOCITY = 450; + public double ROBOT_WIDTH = 0; + public double ROBOT_LENGTH = 0; public static double LAUNCHER_REV_MAX_VELOCITY = -300; public static double LAUNCHER_REV_TARGET_VELOCITY = -250; public static double LAUNCHER_REV_MIN_VELOCITY = -200; @@ -61,6 +64,8 @@ public enum LaunchState { public LaunchState launchState; public void initHardware() { + launchState = LaunchState.IDLE; + /* * Initialize the hardware variables. Note that the strings used here as parameters * to 'get' must correspond to the names assigned during the robot configuration @@ -74,16 +79,21 @@ public void initHardware() { // Reversed direction of launcher for DevBot because motor is on the other side (compared to FastBot) if (MecanumDrive.isDevBot) { + launcher.setDirection(DcMotorEx.Direction.REVERSE); + ROBOT_LENGTH = 18.5; + ROBOT_WIDTH = 18; redLed = null; greenLed = null; } - if (false) { - redLed = hardwareMap.get(LED.class, "redLed"); - greenLed = hardwareMap.get(LED.class, "greenLed"); - redLed.on(); - greenLed.off(); + else if(MecanumDrive.isFastBot) { + ROBOT_WIDTH = 16; + ROBOT_LENGTH = 17; + //redLed = hardwareMap.get(LED.class, "redLed"); Uncomment one we get LEDs + //greenLed = hardwareMap.get(LED.class, "greenLed"); + //redLed.on(); + //greenLed.off(); } /* @@ -133,7 +143,30 @@ public void initHardware() { */ telemetry.addData("Status", "Initialized"); } + class LaunchAction extends RobotAction { + public boolean run(double ElapsedTime) { + launch(true); + if (ElapsedTime < .2) { + return true; + } + else { + return false; + } + } + + } + class SpinUpAction extends RobotAction { + public boolean run(double ElapsedTime) { + launcher.setVelocity(LAUNCHER_LOW_TARGET_VELOCITY); + if(ElapsedTime < 1) { + return true; + } + else { + return false; + } + } + } public void launch(boolean shotRequested) { switch (launchState) { diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java index e1031a83c322..012a642fe52f 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java @@ -37,12 +37,13 @@ enum Movements { @Override public void runOpMode() { + initHardware(); Pose2d startPose = new Pose2d(0, 0, 0); Pose2d redNearStartPose = new Pose2d(-48, 48, Math.toRadians(41)); Pose2d redFarStartPose = new Pose2d(56, 12, Math.toRadians(180)); - Pose2d blueNearStartPose = new Pose2d(-48, -48, Math.toRadians(139)); + Pose2d blueNearStartPose = new Pose2d(-50, -50.5, Math.toRadians(139)); Pose2d blueFarStartPose = new Pose2d(56, -12, Math.toRadians(180)); MecanumDrive drive = new MecanumDrive(hardwareMap, telemetry, gamepad1, startPose); @@ -98,8 +99,12 @@ public void runOpMode() { // Blue alliance auto paths Action blueNear = drive.actionBuilder(blueNearStartPose) .setTangent(Math.toRadians(49)) - .splineTo(new Vector2d(-44, -44), Math.toRadians(49)) - .setTangent(Math.toRadians(139)) + .stopAndAdd(new SpinUpAction()) + .stopAndAdd(new LaunchAction()) + .stopAndAdd(new LaunchAction()) + .stopAndAdd(new LaunchAction()) +// .splineTo(new Vector2d(-44, -44), Math.toRadians(49)) +// .setTangent(Math.toRadians(139)) .splineTo(new Vector2d(-54,-38), Math.toRadians(139)) .build(); diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java index 4af96e26ed31..89e309f27873 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java @@ -46,8 +46,6 @@ public void runOpMode() { Pose2d beginPose = new Pose2d(0, 0, 0); MecanumDrive drive = new MecanumDrive(hardwareMap, telemetry, gamepad1, beginPose); - launchState = LaunchState.IDLE; - // Initialize motors, servos, LEDs initHardware(); diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/roadrunner/MecanumDrive.java b/team417/src/main/java/org/firstinspires/ftc/team417/roadrunner/MecanumDrive.java index cdbac2c4dddc..7aaf99fd7c14 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/roadrunner/MecanumDrive.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/roadrunner/MecanumDrive.java @@ -206,7 +206,7 @@ public static String getBotName() { return inspection.deviceName; } public static boolean isDevBot = getBotName().equals("DevBot"); - + public static boolean isFastBot = getBotName().equals("417-RC"); public static Params PARAMS = new Params(); public MecanumKinematics kinematics; // Initialized by initializeKinematics() From 97f1b26482004a8be2e86f5e66d4021c1c68a412 Mon Sep 17 00:00:00 2001 From: Emmett Date: Sun, 19 Oct 2025 16:14:24 -0700 Subject: [PATCH 041/198] fixed devbot tuning values removed old reverse launch concept added new reverse launch concept added different feeder times for different launch speeds in an attempt to compat double launching cried --- .../firstinspires/ftc/team417/BaseOpMode.java | 34 ++++++++------ .../ftc/team417/CompetitionTeleOp.java | 44 +++++++++++-------- .../ftc/team417/roadrunner/MecanumDrive.java | 4 +- 3 files changed, 48 insertions(+), 34 deletions(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java b/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java index e728ebc41106..6f34cb3f0784 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java @@ -21,9 +21,17 @@ abstract public class BaseOpMode extends LinearOpMode { public CRServo leftFeeder = null; public CRServo rightFeeder = null; public static final double STOP_SPEED = 0.0; //We send this power to the servos when we want them to stop. - public static double FEED_TIME_SECONDS = 0.15; //The feeder servos run this long when a shot is requested. + public static double FEED_TIME_SECONDS = 0; //The feeder servos run this long when a shot is requested. - public static double FULL_SPEED = 1.0; //We send this power to the servos when we want them to feed an artifact to the launcher + public static double FEED_TIME_LOW = 0.15; + public static double FEED_TIME_HIGH = 0.04; + public static double FEED_TIME_SORT = 0.07; + + + public static double rememberVelocity = 0; + + public static double FULL_SPEED = 1.0;//We send this power to the servos when we want them to feed an artifact to the launcher + public static double REV_SPEED = -1.0; public static double LAUNCHER_HIGH_MAX_VELOCITY = 2000; //high target velocity + 50 (will need adjusting) public static double LAUNCHER_HIGH_TARGET_VELOCITY = 1950; public static double LAUNCHER_HIGH_MIN_VELOCITY = 1900; @@ -37,9 +45,7 @@ abstract public class BaseOpMode extends LinearOpMode { public static double LAUNCHER_SORTER_MIN_VELOCITY = 450; - public static double LAUNCHER_REV_MAX_VELOCITY = -300; public static double LAUNCHER_REV_TARGET_VELOCITY = -250; - public static double LAUNCHER_REV_MIN_VELOCITY = -200; boolean doHighLaunch = false; boolean doSort = false; boolean doReverse = false; @@ -54,9 +60,9 @@ public enum LaunchState { SPIN_UP_HIGH, SPIN_UP_LOW, SPIN_UP_SORT, - SPIN_UP_REV, LAUNCH, LAUNCHING, + } public LaunchState launchState; @@ -141,30 +147,28 @@ public void launch(boolean shotRequested) { if (shotRequested) { if (doHighLaunch) { launchState = LaunchState.SPIN_UP_HIGH; + FEED_TIME_SECONDS = FEED_TIME_HIGH; } else if (doSort) { launchState = LaunchState.SPIN_UP_SORT; - } else if (doReverse) { - launchState = LaunchState.SPIN_UP_REV; + FEED_TIME_SECONDS = FEED_TIME_SORT; } else { launchState = LaunchState.SPIN_UP_LOW; + FEED_TIME_SECONDS = FEED_TIME_LOW; } } break; case SPIN_UP_SORT: launcher.setVelocity(LAUNCHER_SORTER_TARGET_VELOCITY); + rememberVelocity = LAUNCHER_SORTER_TARGET_VELOCITY; if (launcher.getVelocity() > LAUNCHER_SORTER_MIN_VELOCITY && launcher.getVelocity() < LAUNCHER_SORTER_MAX_VELOCITY) { launchState = LaunchState.LAUNCH; } break; - case SPIN_UP_REV: - launcher.setVelocity(LAUNCHER_REV_TARGET_VELOCITY); - if (launcher.getVelocity() > LAUNCHER_REV_MIN_VELOCITY && launcher.getVelocity() < LAUNCHER_REV_MAX_VELOCITY) { - launchState = LaunchState.LAUNCH; - } - break; + case SPIN_UP_LOW: launcher.setVelocity(LAUNCHER_LOW_TARGET_VELOCITY); + rememberVelocity = LAUNCHER_LOW_TARGET_VELOCITY; if (launcher.getVelocity() > LAUNCHER_LOW_MIN_VELOCITY && launcher.getVelocity() < LAUNCHER_LOW_MAX_VELOCITY) { if (redLed != null && greenLed != null) { redLed.off(); @@ -176,6 +180,7 @@ public void launch(boolean shotRequested) { break; case SPIN_UP_HIGH: launcher.setVelocity(LAUNCHER_HIGH_TARGET_VELOCITY); + rememberVelocity = LAUNCHER_HIGH_TARGET_VELOCITY; if (launcher.getVelocity() > LAUNCHER_HIGH_MIN_VELOCITY && launcher.getVelocity() < LAUNCHER_HIGH_MAX_VELOCITY) { if (redLed != null && greenLed != null) { redLed.off(); @@ -189,6 +194,7 @@ public void launch(boolean shotRequested) { feederTimer.reset(); launchState = LaunchState.LAUNCHING; break; + case LAUNCHING: if (feederTimer.seconds() > FEED_TIME_SECONDS) { launchState = LaunchState.IDLE; @@ -200,6 +206,8 @@ public void launch(boolean shotRequested) { greenLed.on(); } break; + + } } } diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java index 4af96e26ed31..8fd034817916 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java @@ -83,29 +83,35 @@ public void runOpMode() { Drawing.drawRobot(packet.fieldOverlay(), drive.pose); MecanumDrive.sendTelemetryPacket(packet); - if (gamepad2.y) { //high speed + + if (gamepad2.a) { //slow speed + launcher.setVelocity(LAUNCHER_LOW_TARGET_VELOCITY); + doHighLaunch = false; + doSort = false; + doReverse = false; + } else if (gamepad2.x) { // sort speed + launcher.setVelocity(LAUNCHER_SORTER_TARGET_VELOCITY); + doHighLaunch = false; + doSort = true; + doReverse = false; + } else if (gamepad2.y) { // high speed launcher.setVelocity(LAUNCHER_HIGH_TARGET_VELOCITY); doHighLaunch = true; doSort = false; doReverse = false; - } else if (gamepad2.a) { //slow speed - launcher.setVelocity(LAUNCHER_LOW_TARGET_VELOCITY); - doHighLaunch = false; - doSort = false; - doReverse = false; - } else if (gamepad2.x) { // sort speed - launcher.setVelocity(LAUNCHER_SORTER_TARGET_VELOCITY); - doHighLaunch = false; - doSort = true; - doReverse = false; - } else if (gamepad2.b) { // reverse - launcher.setVelocity(LAUNCHER_REV_TARGET_VELOCITY); + } else if(gamepad2.b) { //reverse + launcher.setVelocity(LAUNCHER_REV_TARGET_VELOCITY); + leftFeeder.setPower(REV_SPEED); + rightFeeder.setPower(REV_SPEED); doHighLaunch = false; doSort = false; doReverse = true; - } else if (gamepad2.left_bumper) { // stop flywheel - launcher.setVelocity(STOP_SPEED); - } + } else if (gamepad2.left_bumper) { // stop flywheel + launcher.setVelocity(STOP_SPEED); + leftFeeder.setPower(STOP_SPEED); + rightFeeder.setPower(STOP_SPEED); + } + /* * Now we call our "Launch" function. @@ -115,17 +121,17 @@ public void runOpMode() { rightBumperTimer.reset(); } - launch(gamepad2.rightBumperWasPressed()); - /* * Show the state and motor powers */ telemetry.addData("State", launchState); - // telemetry.addData("Motors", "left (%.2f), right (%.2f)", leftPower, rightPower); + //telemetry.addData("Motors", "left (%.2f), right (%.2f)", leftPower, rightPower); telemetry.addData("motorSpeed", launcher.getVelocity()); telemetry.addData("reverse", doReverse); telemetry.addData("highLaunch", doHighLaunch); telemetry.addData("sort", doSort); + telemetry.addData("FEED_TIME_SECONDS", FEED_TIME_SECONDS); + telemetry.addLine("im already crying"); telemetry.update(); } diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/roadrunner/MecanumDrive.java b/team417/src/main/java/org/firstinspires/ftc/team417/roadrunner/MecanumDrive.java index cdbac2c4dddc..abe87020a269 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/roadrunner/MecanumDrive.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/roadrunner/MecanumDrive.java @@ -116,8 +116,8 @@ public static class Params { pinpoint.ticksPerMm = 71.665; pinpoint.xReversed = true; pinpoint.yReversed = false; - pinpoint.xOffset = 199.4; - pinpoint.yOffset = 120.2; + pinpoint.xOffset = -199.4; + pinpoint.yOffset = -120.2; } else { // Your competition robot Loony Tune configuration is here: logoFacingDirection = RevHubOrientationOnRobot.LogoFacingDirection.UP; From a8eb93f4ccd101657dda66852b8769ea7ba5edc0 Mon Sep 17 00:00:00 2001 From: anya-codes Date: Sun, 19 Oct 2025 16:24:49 -0700 Subject: [PATCH 042/198] Added code to always run feeder wheels in reverse unless launching, and to send telemetry to FTC dashboard for graphing --- .../java/org/firstinspires/ftc/team417/BaseOpMode.java | 1 + .../org/firstinspires/ftc/team417/CompetitionTeleOp.java | 8 ++++++++ 2 files changed, 9 insertions(+) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java b/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java index e728ebc41106..2e216b91960b 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java @@ -24,6 +24,7 @@ abstract public class BaseOpMode extends LinearOpMode { public static double FEED_TIME_SECONDS = 0.15; //The feeder servos run this long when a shot is requested. public static double FULL_SPEED = 1.0; //We send this power to the servos when we want them to feed an artifact to the launcher + public static double SLOW_REVERSE_SPEED = -0.5; public static double LAUNCHER_HIGH_MAX_VELOCITY = 2000; //high target velocity + 50 (will need adjusting) public static double LAUNCHER_HIGH_TARGET_VELOCITY = 1950; public static double LAUNCHER_HIGH_MIN_VELOCITY = 1900; diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java index 4af96e26ed31..513c6a3ba54e 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java @@ -75,6 +75,12 @@ public void runOpMode() { // 'packet' is the object used to send data to FTC Dashboard: TelemetryPacket packet = MecanumDrive.getTelemetryPacket(); + // send telemetry to FTC dashboard to graph + packet.put("FlyWheel Speed:", launcher.getVelocity()); + packet.put("Right bumper press:", gamepad2.right_bumper? 0:1000); + packet.put("Feeder wheels:", rightFeeder.getPower()*100); + + // Do the work now for all active Road Runner actions, if any: drive.doActionsWork(packet); @@ -90,6 +96,8 @@ public void runOpMode() { doReverse = false; } else if (gamepad2.a) { //slow speed launcher.setVelocity(LAUNCHER_LOW_TARGET_VELOCITY); + leftFeeder.setPower(SLOW_REVERSE_SPEED); + rightFeeder.setPower(SLOW_REVERSE_SPEED); doHighLaunch = false; doSort = false; doReverse = false; From a272d91dd2b82da7b2e23f6ca8ad06bb78b4d363 Mon Sep 17 00:00:00 2001 From: Team <417skid@swerverobotics.club> Date: Sun, 19 Oct 2025 16:50:24 -0700 Subject: [PATCH 043/198] Launch Actions for Autonomous, doesnt stop shooting --- .../firstinspires/ftc/team417/BaseOpMode.java | 7 +- .../ftc/team417/CompetitionAuto.java | 12 +++- .../ftc/team417/CompetitionTeleOp.java | 68 +++++++++++++++++++ 3 files changed, 79 insertions(+), 8 deletions(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java b/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java index 3b47e1091742..7bea211a0500 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java @@ -146,12 +146,7 @@ else if(MecanumDrive.isFastBot) { class LaunchAction extends RobotAction { public boolean run(double ElapsedTime) { launch(true); - if (ElapsedTime < .2) { - return true; - } - else { - return false; - } + return ElapsedTime < 1; } diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java index 012a642fe52f..79109fb58cb2 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java @@ -4,6 +4,7 @@ import com.acmerobotics.dashboard.telemetry.TelemetryPacket; import com.acmerobotics.roadrunner.Action; import com.acmerobotics.roadrunner.Pose2d; +import com.acmerobotics.roadrunner.SleepAction; import com.acmerobotics.roadrunner.Vector2d; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; @@ -40,7 +41,7 @@ public void runOpMode() { initHardware(); Pose2d startPose = new Pose2d(0, 0, 0); - Pose2d redNearStartPose = new Pose2d(-48, 48, Math.toRadians(41)); + Pose2d redNearStartPose = new Pose2d(-50, 50.5, Math.toRadians(41)); Pose2d redFarStartPose = new Pose2d(56, 12, Math.toRadians(180)); Pose2d blueNearStartPose = new Pose2d(-50, -50.5, Math.toRadians(139)); @@ -83,7 +84,12 @@ public void runOpMode() { // Red alliance auto paths Action redNear = drive.actionBuilder(redNearStartPose) - .splineTo(new Vector2d(-20, 51), Math.toRadians(0)) + .setTangent(Math.toRadians(131)) + .stopAndAdd(new SpinUpAction()) + .stopAndAdd(new LaunchAction()) + .stopAndAdd(new LaunchAction()) + .stopAndAdd(new LaunchAction()) + .splineTo(new Vector2d(-54,38), Math.toRadians(41)) .build(); Action redFar = drive.actionBuilder(redFarStartPose) @@ -101,7 +107,9 @@ public void runOpMode() { .setTangent(Math.toRadians(49)) .stopAndAdd(new SpinUpAction()) .stopAndAdd(new LaunchAction()) + .stopAndAdd(new SleepAction(1)) .stopAndAdd(new LaunchAction()) + .stopAndAdd(new SleepAction(1)) .stopAndAdd(new LaunchAction()) // .splineTo(new Vector2d(-44, -44), Math.toRadians(49)) // .setTangent(Math.toRadians(139)) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java index 89e309f27873..a365d29ca1e8 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java @@ -68,6 +68,74 @@ public void runOpMode() { )); // Update the current pose: + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + drive.updatePoseEstimate(); // 'packet' is the object used to send data to FTC Dashboard: From 86f840ae647163c58addb5bfcd3242177a7f586b Mon Sep 17 00:00:00 2001 From: Emmett Date: Mon, 20 Oct 2025 09:59:27 -0700 Subject: [PATCH 044/198] added constant reverse servo movement to prevent errant launching. renamed SLOW_REVERSE_SPEED to SLOW_REV_SPEED and lowered its value from -0.5 to -0.15. needs testing. commented BaseOpmode, and removed unneccesary comment remnants from the original goBilda code (in baseopmode and competitionteleop) --- .../firstinspires/ftc/team417/BaseOpMode.java | 55 ++++++------------- .../ftc/team417/CompetitionTeleOp.java | 16 ++++-- 2 files changed, 26 insertions(+), 45 deletions(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java b/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java index a40f03ed7d10..c3f1b0d09ebd 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java @@ -30,8 +30,8 @@ abstract public class BaseOpMode extends LinearOpMode { public static double rememberVelocity = 0; public static double FULL_SPEED = 1.0; //We send this power to the servos when we want them to feed an artifact to the launcher - public static double SLOW_REVERSE_SPEED = -0.5; - public static double REV_SPEED = -1.0; + public static double SLOW_REV_SPEED = -0.15; //speed for the constant reverse rotation + public static double REV_SPEED = -1.0;//speed used for the reverse launch function public static double LAUNCHER_HIGH_MAX_VELOCITY = 2000; //high target velocity + 50 (will need adjusting) public static double LAUNCHER_HIGH_TARGET_VELOCITY = 1950; public static double LAUNCHER_HIGH_MIN_VELOCITY = 1900; @@ -66,13 +66,11 @@ public enum LaunchState { public LaunchState launchState; public void initHardware() { - /* - * Initialize the hardware variables. Note that the strings used here as parameters - * to 'get' must correspond to the names assigned during the robot configuration - * step. - */ + // leftDrive = hardwareMap.get(DcMotor.class, "left_drive"); // rightDrive = hardwareMap.get(DcMotor.class, "right_drive"); + + // initialize flywheel motor and feeder servos launcher = hardwareMap.get(DcMotorEx.class, "motLauncher"); leftFeeder = hardwareMap.get(CRServo.class, "servoBLaunchFeed"); rightFeeder = hardwareMap.get(CRServo.class, "servoFLaunchFeed"); @@ -91,15 +89,6 @@ public void initHardware() { greenLed.off(); } - /* - * To drive forward, most robots need the motor on one side to be reversed, - * because the axles point in opposite directions. Pushing the left stick forward - * MUST make robot go forward. So adjust these two lines based on your first test drive. - * Note: The settings here assume direct drive on left and right wheels. Gear - * Reduction or 90 Deg drives may require direction flips - */ - // leftDrive.setDirection(DcMotor.Direction.REVERSE); - // rightDrive.setDirection(DcMotor.Direction.FORWARD); /* * Here we set our launcher to the RUN_USING_ENCODER runmode. @@ -110,39 +99,27 @@ public void initHardware() { */ launcher.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - /* - * Setting zeroPowerBehavior to BRAKE enables a "brake mode". This causes the motor to - * slow down much faster when it is coasting. This creates a much more controllable - * drivetrain. As the robot stops much quicker. - */ - // leftDrive.setZeroPowerBehavior(BRAKE); - // rightDrive.setZeroPowerBehavior(BRAKE); + // set the flywheel to a braking behavior so it slows down faster when left trigger is pressed launcher.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - /* - * set Feeders to an initial value to initialize the servo controller - */ + // set the feeder servos to an initial value to init the servo controller leftFeeder.setPower(STOP_SPEED); rightFeeder.setPower(STOP_SPEED); launcher.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, new PIDFCoefficients(300, 0, 0, 10)); - /* - * Much like our drivetrain motors, we set the left feeder servo to reverse so that they - * both work to feed the ball into the robot. - */ + //set the left feeder servo to rotate in reverse, so that the servos spin in the same relative direction leftFeeder.setDirection(DcMotorSimple.Direction.REVERSE); - /* - * Tell the driver that initialization is complete. - */ + + // Tell the driver that initialization is complete. telemetry.addData("Status", "Initialized"); } public void launch(boolean shotRequested) { switch (launchState) { - case IDLE: + case IDLE: // The default launch state. When a launchmode is selected, the flywheel revs up and waits here for shotRequested if (shotRequested) { if (doHighLaunch) { launchState = LaunchState.SPIN_UP_HIGH; @@ -156,14 +133,14 @@ public void launch(boolean shotRequested) { } break; - case SPIN_UP_SORT: + case SPIN_UP_SORT: //if sorting launchmode is selected and shotRequested is true, check that the flywheel is in the correct velocity range (450 - 500 rpm) launcher.setVelocity(LAUNCHER_SORTER_TARGET_VELOCITY); if (launcher.getVelocity() > LAUNCHER_SORTER_MIN_VELOCITY && launcher.getVelocity() < LAUNCHER_SORTER_MAX_VELOCITY) { launchState = LaunchState.LAUNCH; } break; - case SPIN_UP_LOW: + case SPIN_UP_LOW: //if low launchmode is selected and shotRequested is true, check that the flywheel is in the correct velocity range (1075 - 1175 rpm) launcher.setVelocity(LAUNCHER_LOW_TARGET_VELOCITY); if (launcher.getVelocity() > LAUNCHER_LOW_MIN_VELOCITY && launcher.getVelocity() < LAUNCHER_LOW_MAX_VELOCITY) { if (redLed != null && greenLed != null) { @@ -174,7 +151,7 @@ public void launch(boolean shotRequested) { } break; - case SPIN_UP_HIGH: + case SPIN_UP_HIGH: //if high launchmode is selected and shotRequested is true, check that the flywheel is in the correct velocity range (1900 - 2000 rpm) launcher.setVelocity(LAUNCHER_HIGH_TARGET_VELOCITY); if (launcher.getVelocity() > LAUNCHER_HIGH_MIN_VELOCITY && launcher.getVelocity() < LAUNCHER_HIGH_MAX_VELOCITY) { if (redLed != null && greenLed != null) { @@ -183,13 +160,13 @@ public void launch(boolean shotRequested) { } launchState = LaunchState.LAUNCH; } - case LAUNCH: + case LAUNCH: //when shotRequested, start the feeder servos to init launch leftFeeder.setPower(FULL_SPEED); rightFeeder.setPower(FULL_SPEED); feederTimer.reset(); launchState = LaunchState.LAUNCHING; break; - case LAUNCHING: + case LAUNCHING: //wait until feedTimer surpasses FEED_TIME_SECONDS, then stop the feeder servos. if (feederTimer.seconds() > FEED_TIME_SECONDS) { launchState = LaunchState.IDLE; leftFeeder.setPower(STOP_SPEED); diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java index 0268ddf95dac..f4a7d4e35c8e 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java @@ -88,6 +88,8 @@ public void runOpMode() { packet.fieldOverlay().setStroke("#3F51B5"); Drawing.drawRobot(packet.fieldOverlay(), drive.pose); MecanumDrive.sendTelemetryPacket(packet); + leftFeeder.setPower(SLOW_REV_SPEED); + rightFeeder.setPower(SLOW_REV_SPEED); if (gamepad2.y) { //high speed launcher.setVelocity(LAUNCHER_HIGH_TARGET_VELOCITY); @@ -96,8 +98,6 @@ public void runOpMode() { doReverse = false; } else if (gamepad2.a) { //slow speed launcher.setVelocity(LAUNCHER_LOW_TARGET_VELOCITY); - leftFeeder.setPower(SLOW_REVERSE_SPEED); - rightFeeder.setPower(SLOW_REVERSE_SPEED); doHighLaunch = false; doSort = false; doReverse = false; @@ -106,18 +106,21 @@ public void runOpMode() { doHighLaunch = false; doSort = true; doReverse = false; - } else if(gamepad2.b) { //reverse - launcher.setVelocity(LAUNCHER_REV_TARGET_VELOCITY); + } else if(gamepad2.b) { //reverse + launcher.setVelocity(LAUNCHER_REV_TARGET_VELOCITY); leftFeeder.setPower(REV_SPEED); rightFeeder.setPower(REV_SPEED); doHighLaunch = false; doSort = false; doReverse = true; - } else if (gamepad2.left_bumper) { // stop flywheel + } else if (gamepad2.left_bumper) { // stop flywheel launcher.setVelocity(STOP_SPEED); leftFeeder.setPower(STOP_SPEED); rightFeeder.setPower(STOP_SPEED); - } + } else { + leftFeeder.setPower(SLOW_REV_SPEED); + rightFeeder.setPower(SLOW_REV_SPEED); + } /* @@ -138,6 +141,7 @@ public void runOpMode() { telemetry.addData("highLaunch", doHighLaunch); telemetry.addData("sort", doSort); telemetry.addData("FEED_TIME_SECONDS", FEED_TIME_SECONDS); + telemetry.addData("feederSpeed", leftFeeder.getPower()); telemetry.update(); } From 5f10f582af23e2ed139036424224f17588ae4af6 Mon Sep 17 00:00:00 2001 From: Team <417skid@swerverobotics.club> Date: Tue, 21 Oct 2025 19:49:51 -0700 Subject: [PATCH 045/198] Launch Actions for Autonomous --- .../org/firstinspires/ftc/team417/BaseOpMode.java | 12 ++++++++++-- .../firstinspires/ftc/team417/CompetitionAuto.java | 5 +++-- 2 files changed, 13 insertions(+), 4 deletions(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java b/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java index 7bea211a0500..d8d80c1fcc06 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java @@ -145,8 +145,16 @@ else if(MecanumDrive.isFastBot) { } class LaunchAction extends RobotAction { public boolean run(double ElapsedTime) { - launch(true); - return ElapsedTime < 1; + leftFeeder.setPower(FULL_SPEED); + rightFeeder.setPower(FULL_SPEED); + if (ElapsedTime < 1) { + leftFeeder.setPower(STOP_SPEED); + rightFeeder.setPower(STOP_SPEED); + return false; + } + else { + return true; + } } diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java index 79109fb58cb2..ded9568f6e5d 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java @@ -106,10 +106,11 @@ public void runOpMode() { Action blueNear = drive.actionBuilder(blueNearStartPose) .setTangent(Math.toRadians(49)) .stopAndAdd(new SpinUpAction()) + .stopAndAdd(new SleepAction(2)) .stopAndAdd(new LaunchAction()) - .stopAndAdd(new SleepAction(1)) + .stopAndAdd(new SleepAction(2)) .stopAndAdd(new LaunchAction()) - .stopAndAdd(new SleepAction(1)) + .stopAndAdd(new SleepAction(2)) .stopAndAdd(new LaunchAction()) // .splineTo(new Vector2d(-44, -44), Math.toRadians(49)) // .setTangent(Math.toRadians(139)) From b3941261ca76225de6a90d11685ee7953a01c50b Mon Sep 17 00:00:00 2001 From: Emmett Date: Tue, 21 Oct 2025 19:57:15 -0700 Subject: [PATCH 046/198] reworked enums, and removed unneeded code. --- .../firstinspires/ftc/team417/BaseOpMode.java | 35 +++++++------------ .../ftc/team417/CompetitionTeleOp.java | 19 +++------- 2 files changed, 16 insertions(+), 38 deletions(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java b/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java index c3f1b0d09ebd..414fddd2a279 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java @@ -46,9 +46,7 @@ abstract public class BaseOpMode extends LinearOpMode { public static double LAUNCHER_REV_TARGET_VELOCITY = -250; - boolean doHighLaunch = false; - boolean doSort = false; - boolean doReverse = false; + public LED redLed; public LED greenLed; @@ -57,14 +55,16 @@ abstract public class BaseOpMode extends LinearOpMode { public enum LaunchState { IDLE, - SPIN_UP_HIGH, - SPIN_UP_LOW, - SPIN_UP_SORT, + HIGH, + LOW, + SORT, + REVERSE, LAUNCH, LAUNCHING, } public LaunchState launchState; + public void initHardware() { // leftDrive = hardwareMap.get(DcMotor.class, "left_drive"); @@ -118,29 +118,17 @@ public void initHardware() { public void launch(boolean shotRequested) { + if (shotRequested) { switch (launchState) { - case IDLE: // The default launch state. When a launchmode is selected, the flywheel revs up and waits here for shotRequested - if (shotRequested) { - if (doHighLaunch) { - launchState = LaunchState.SPIN_UP_HIGH; - } else if (doSort) { - launchState = LaunchState.SPIN_UP_SORT; - FEED_TIME_SECONDS = FEED_TIME_SORT; - } else { - launchState = LaunchState.SPIN_UP_LOW; - FEED_TIME_SECONDS = FEED_TIME_LOW; - } - } - break; - - case SPIN_UP_SORT: //if sorting launchmode is selected and shotRequested is true, check that the flywheel is in the correct velocity range (450 - 500 rpm) + case SORT: //if sorting launchmode is selected and shotRequested is true, check that the flywheel is in the correct velocity range (450 - 500 rpm) launcher.setVelocity(LAUNCHER_SORTER_TARGET_VELOCITY); if (launcher.getVelocity() > LAUNCHER_SORTER_MIN_VELOCITY && launcher.getVelocity() < LAUNCHER_SORTER_MAX_VELOCITY) { launchState = LaunchState.LAUNCH; } break; - case SPIN_UP_LOW: //if low launchmode is selected and shotRequested is true, check that the flywheel is in the correct velocity range (1075 - 1175 rpm) + case LOW: //if low launchmode is selected and shotRequested is true, check that the flywheel is in the correct velocity range (1075 - 1175 rpm) + case REVERSE: launcher.setVelocity(LAUNCHER_LOW_TARGET_VELOCITY); if (launcher.getVelocity() > LAUNCHER_LOW_MIN_VELOCITY && launcher.getVelocity() < LAUNCHER_LOW_MAX_VELOCITY) { if (redLed != null && greenLed != null) { @@ -151,7 +139,7 @@ public void launch(boolean shotRequested) { } break; - case SPIN_UP_HIGH: //if high launchmode is selected and shotRequested is true, check that the flywheel is in the correct velocity range (1900 - 2000 rpm) + case HIGH: //if high launchmode is selected and shotRequested is true, check that the flywheel is in the correct velocity range (1900 - 2000 rpm) launcher.setVelocity(LAUNCHER_HIGH_TARGET_VELOCITY); if (launcher.getVelocity() > LAUNCHER_HIGH_MIN_VELOCITY && launcher.getVelocity() < LAUNCHER_HIGH_MAX_VELOCITY) { if (redLed != null && greenLed != null) { @@ -177,6 +165,7 @@ public void launch(boolean shotRequested) { greenLed.on(); } break; + } } } } diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java index f4a7d4e35c8e..a6b54e6307a9 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java @@ -93,26 +93,18 @@ public void runOpMode() { if (gamepad2.y) { //high speed launcher.setVelocity(LAUNCHER_HIGH_TARGET_VELOCITY); - doHighLaunch = true; - doSort = false; - doReverse = false; + launchState = LaunchState.HIGH; } else if (gamepad2.a) { //slow speed launcher.setVelocity(LAUNCHER_LOW_TARGET_VELOCITY); - doHighLaunch = false; - doSort = false; - doReverse = false; + launchState = LaunchState.LOW; } else if (gamepad2.x) { // sort speed launcher.setVelocity(LAUNCHER_SORTER_TARGET_VELOCITY); - doHighLaunch = false; - doSort = true; - doReverse = false; + launchState = LaunchState.SORT; } else if(gamepad2.b) { //reverse launcher.setVelocity(LAUNCHER_REV_TARGET_VELOCITY); leftFeeder.setPower(REV_SPEED); rightFeeder.setPower(REV_SPEED); - doHighLaunch = false; - doSort = false; - doReverse = true; + launchState = LaunchState.REVERSE; } else if (gamepad2.left_bumper) { // stop flywheel launcher.setVelocity(STOP_SPEED); leftFeeder.setPower(STOP_SPEED); @@ -137,9 +129,6 @@ public void runOpMode() { telemetry.addData("State", launchState); // telemetry.addData("Motors", "left (%.2f), right (%.2f)", leftPower, rightPower); telemetry.addData("motorSpeed", launcher.getVelocity()); - telemetry.addData("reverse", doReverse); - telemetry.addData("highLaunch", doHighLaunch); - telemetry.addData("sort", doSort); telemetry.addData("FEED_TIME_SECONDS", FEED_TIME_SECONDS); telemetry.addData("feederSpeed", leftFeeder.getPower()); From 9860ff3bc34728cf1ec1b7f9d5f46079ac0c81e5 Mon Sep 17 00:00:00 2001 From: Emmett Date: Tue, 21 Oct 2025 20:27:25 -0700 Subject: [PATCH 047/198] fixed reverse code again also formatted --- .../firstinspires/ftc/team417/BaseOpMode.java | 80 +++++++++---------- .../ftc/team417/CompetitionTeleOp.java | 24 +++--- 2 files changed, 55 insertions(+), 49 deletions(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java b/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java index 414fddd2a279..c204e6c1294d 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java @@ -119,52 +119,52 @@ public void initHardware() { public void launch(boolean shotRequested) { if (shotRequested) { - switch (launchState) { - case SORT: //if sorting launchmode is selected and shotRequested is true, check that the flywheel is in the correct velocity range (450 - 500 rpm) - launcher.setVelocity(LAUNCHER_SORTER_TARGET_VELOCITY); - if (launcher.getVelocity() > LAUNCHER_SORTER_MIN_VELOCITY && launcher.getVelocity() < LAUNCHER_SORTER_MAX_VELOCITY) { - launchState = LaunchState.LAUNCH; - } - break; - - case LOW: //if low launchmode is selected and shotRequested is true, check that the flywheel is in the correct velocity range (1075 - 1175 rpm) - case REVERSE: - launcher.setVelocity(LAUNCHER_LOW_TARGET_VELOCITY); - if (launcher.getVelocity() > LAUNCHER_LOW_MIN_VELOCITY && launcher.getVelocity() < LAUNCHER_LOW_MAX_VELOCITY) { - if (redLed != null && greenLed != null) { - redLed.off(); - greenLed.on(); + switch (launchState) { + case SORT: //if sorting launchmode is selected and shotRequested is true, check that the flywheel is in the correct velocity range (450 - 500 rpm) + launcher.setVelocity(LAUNCHER_SORTER_TARGET_VELOCITY); + if (launcher.getVelocity() > LAUNCHER_SORTER_MIN_VELOCITY && launcher.getVelocity() < LAUNCHER_SORTER_MAX_VELOCITY) { + launchState = LaunchState.LAUNCH; } - launchState = LaunchState.LAUNCH; + break; + + case LOW: //if low launchmode is selected and shotRequested is true, check that the flywheel is in the correct velocity range (1075 - 1175 rpm) + case REVERSE: + launcher.setVelocity(LAUNCHER_LOW_TARGET_VELOCITY); + if (launcher.getVelocity() > LAUNCHER_LOW_MIN_VELOCITY && launcher.getVelocity() < LAUNCHER_LOW_MAX_VELOCITY) { + if (redLed != null && greenLed != null) { + redLed.off(); + greenLed.on(); + } + launchState = LaunchState.LAUNCH; - } - break; - case HIGH: //if high launchmode is selected and shotRequested is true, check that the flywheel is in the correct velocity range (1900 - 2000 rpm) - launcher.setVelocity(LAUNCHER_HIGH_TARGET_VELOCITY); - if (launcher.getVelocity() > LAUNCHER_HIGH_MIN_VELOCITY && launcher.getVelocity() < LAUNCHER_HIGH_MAX_VELOCITY) { + } + break; + case HIGH: //if high launchmode is selected and shotRequested is true, check that the flywheel is in the correct velocity range (1900 - 2000 rpm) + launcher.setVelocity(LAUNCHER_HIGH_TARGET_VELOCITY); + if (launcher.getVelocity() > LAUNCHER_HIGH_MIN_VELOCITY && launcher.getVelocity() < LAUNCHER_HIGH_MAX_VELOCITY) { + if (redLed != null && greenLed != null) { + redLed.off(); + greenLed.on(); + } + launchState = LaunchState.LAUNCH; + } + case LAUNCH: //when shotRequested, start the feeder servos to init launch + leftFeeder.setPower(FULL_SPEED); + rightFeeder.setPower(FULL_SPEED); + feederTimer.reset(); + launchState = LaunchState.LAUNCHING; + break; + case LAUNCHING: //wait until feedTimer surpasses FEED_TIME_SECONDS, then stop the feeder servos. + if (feederTimer.seconds() > FEED_TIME_SECONDS) { + launchState = LaunchState.IDLE; + leftFeeder.setPower(STOP_SPEED); + rightFeeder.setPower(STOP_SPEED); + } if (redLed != null && greenLed != null) { redLed.off(); greenLed.on(); } - launchState = LaunchState.LAUNCH; - } - case LAUNCH: //when shotRequested, start the feeder servos to init launch - leftFeeder.setPower(FULL_SPEED); - rightFeeder.setPower(FULL_SPEED); - feederTimer.reset(); - launchState = LaunchState.LAUNCHING; - break; - case LAUNCHING: //wait until feedTimer surpasses FEED_TIME_SECONDS, then stop the feeder servos. - if (feederTimer.seconds() > FEED_TIME_SECONDS) { - launchState = LaunchState.IDLE; - leftFeeder.setPower(STOP_SPEED); - rightFeeder.setPower(STOP_SPEED); - } - if (redLed != null && greenLed != null) { - redLed.off(); - greenLed.on(); - } - break; + break; } } } diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java index a6b54e6307a9..ebd5cd7b0c8e 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java @@ -15,7 +15,7 @@ * This class exposes the competition version of TeleOp. As a general rule, add code to the * BaseOpMode class rather than here so that it can be shared between both TeleOp and Autonomous. */ -@TeleOp(name="TeleOp", group="Competition") +@TeleOp(name = "TeleOp", group = "Competition") @Config public class CompetitionTeleOp extends BaseOpMode { @@ -77,8 +77,8 @@ public void runOpMode() { // send telemetry to FTC dashboard to graph packet.put("FlyWheel Speed:", launcher.getVelocity()); - packet.put("Right bumper press:", gamepad2.right_bumper? 0:1000); - packet.put("Feeder wheels:", rightFeeder.getPower()*100); + packet.put("Right bumper press:", gamepad2.right_bumper ? 0 : 1000); + packet.put("Feeder wheels:", rightFeeder.getPower() * 100); // Do the work now for all active Road Runner actions, if any: @@ -94,22 +94,28 @@ public void runOpMode() { if (gamepad2.y) { //high speed launcher.setVelocity(LAUNCHER_HIGH_TARGET_VELOCITY); launchState = LaunchState.HIGH; + leftFeeder.setPower(STOP_SPEED); + rightFeeder.setPower(STOP_SPEED); } else if (gamepad2.a) { //slow speed launcher.setVelocity(LAUNCHER_LOW_TARGET_VELOCITY); launchState = LaunchState.LOW; + leftFeeder.setPower(STOP_SPEED); + rightFeeder.setPower(STOP_SPEED); } else if (gamepad2.x) { // sort speed launcher.setVelocity(LAUNCHER_SORTER_TARGET_VELOCITY); launchState = LaunchState.SORT; - } else if(gamepad2.b) { //reverse + leftFeeder.setPower(STOP_SPEED); + rightFeeder.setPower(STOP_SPEED); + } else if (gamepad2.b) { //reverse launcher.setVelocity(LAUNCHER_REV_TARGET_VELOCITY); leftFeeder.setPower(REV_SPEED); rightFeeder.setPower(REV_SPEED); launchState = LaunchState.REVERSE; } else if (gamepad2.left_bumper) { // stop flywheel - launcher.setVelocity(STOP_SPEED); - leftFeeder.setPower(STOP_SPEED); - rightFeeder.setPower(STOP_SPEED); - } else { + launcher.setVelocity(STOP_SPEED); + leftFeeder.setPower(STOP_SPEED); + rightFeeder.setPower(STOP_SPEED); + } else if (launchState == LaunchState.IDLE) { leftFeeder.setPower(SLOW_REV_SPEED); rightFeeder.setPower(SLOW_REV_SPEED); } @@ -136,7 +142,7 @@ public void runOpMode() { } } - public double doSLOWMODE(){ + public double doSLOWMODE() { if (gamepad1.left_stick_button) { return SLOWDRIVE_SPEED; } else { From d50ee754671b9ee9b3a8e28fa8f2462baffc0cf8 Mon Sep 17 00:00:00 2001 From: Emmett Date: Tue, 21 Oct 2025 20:47:40 -0700 Subject: [PATCH 048/198] reverse testing again - needs testing --- .../ftc/team417/CompetitionTeleOp.java | 13 +++++-------- 1 file changed, 5 insertions(+), 8 deletions(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java index ebd5cd7b0c8e..8d2a8dde5e51 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java @@ -94,18 +94,15 @@ public void runOpMode() { if (gamepad2.y) { //high speed launcher.setVelocity(LAUNCHER_HIGH_TARGET_VELOCITY); launchState = LaunchState.HIGH; - leftFeeder.setPower(STOP_SPEED); - rightFeeder.setPower(STOP_SPEED); + } else if (gamepad2.a) { //slow speed launcher.setVelocity(LAUNCHER_LOW_TARGET_VELOCITY); launchState = LaunchState.LOW; - leftFeeder.setPower(STOP_SPEED); - rightFeeder.setPower(STOP_SPEED); + } else if (gamepad2.x) { // sort speed launcher.setVelocity(LAUNCHER_SORTER_TARGET_VELOCITY); launchState = LaunchState.SORT; - leftFeeder.setPower(STOP_SPEED); - rightFeeder.setPower(STOP_SPEED); + } else if (gamepad2.b) { //reverse launcher.setVelocity(LAUNCHER_REV_TARGET_VELOCITY); leftFeeder.setPower(REV_SPEED); @@ -115,12 +112,12 @@ public void runOpMode() { launcher.setVelocity(STOP_SPEED); leftFeeder.setPower(STOP_SPEED); rightFeeder.setPower(STOP_SPEED); - } else if (launchState == LaunchState.IDLE) { + } + while (launchState == LaunchState.IDLE) { leftFeeder.setPower(SLOW_REV_SPEED); rightFeeder.setPower(SLOW_REV_SPEED); } - /* * Now we call our "Launch" function. */ From 23f3704670e2d8ece03bcbea206df8d39ceb1096 Mon Sep 17 00:00:00 2001 From: Emmett Date: Wed, 22 Oct 2025 08:24:39 -0700 Subject: [PATCH 049/198] weird blank space removed --- .../ftc/team417/CompetitionTeleOp.java | 67 ------------------- 1 file changed, 67 deletions(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java index 9819086205ba..99183681ca57 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java @@ -69,73 +69,6 @@ public void runOpMode() { // Update the current pose: - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - drive.updatePoseEstimate(); // 'packet' is the object used to send data to FTC Dashboard: From d2f43e00db26b2b6cfc8d04c81869dd306c89c6f Mon Sep 17 00:00:00 2001 From: Emmett Date: Fri, 24 Oct 2025 18:46:06 -0700 Subject: [PATCH 050/198] removed case: idle from if(shotrequested) which may fix reverse feeder code. --- .../firstinspires/ftc/team417/BaseOpMode.java | 77 ++++++++++++------- .../ftc/team417/CompetitionTeleOp.java | 9 +-- 2 files changed, 51 insertions(+), 35 deletions(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java b/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java index db51f2d943e9..cb6e5abd2334 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java @@ -61,7 +61,6 @@ public enum LaunchState { HIGH, LOW, SORT, - REVERSE, LAUNCH, LAUNCHING, } @@ -157,29 +156,49 @@ public boolean run(double ElapsedTime) { } public void launch(boolean shotRequested) { - if (shotRequested) { - switch (launchState) { - case SORT: //if sorting launchmode is selected and shotRequested is true, check that the flywheel is in the correct velocity range (450 - 500 rpm) + + + + switch (launchState) { + + case IDLE: + leftFeeder.setPower(SLOW_REV_SPEED); + rightFeeder.setPower(SLOW_REV_SPEED); + break; + + case SORT: //if sorting launchmode is selected and shotRequested is true, check that the flywheel is in the correct velocity range (450 - 500 rpm) + if (shotRequested) { launcher.setVelocity(LAUNCHER_SORTER_TARGET_VELOCITY); + leftFeeder.setPower(STOP_SPEED); + rightFeeder.setPower(STOP_SPEED); if (launcher.getVelocity() > LAUNCHER_SORTER_MIN_VELOCITY && launcher.getVelocity() < LAUNCHER_SORTER_MAX_VELOCITY) { launchState = LaunchState.LAUNCH; + } - break; + } + break; - case LOW: //if low launchmode is selected and shotRequested is true, check that the flywheel is in the correct velocity range (1075 - 1175 rpm) - case REVERSE: + case LOW: //if low launchmode is selected and shotRequested is true, check that the flywheel is in the correct velocity range (1075 - 1175 rpm) + if (shotRequested) { launcher.setVelocity(LAUNCHER_LOW_TARGET_VELOCITY); if (launcher.getVelocity() > LAUNCHER_LOW_MIN_VELOCITY && launcher.getVelocity() < LAUNCHER_LOW_MAX_VELOCITY) { + leftFeeder.setPower(STOP_SPEED); + rightFeeder.setPower(STOP_SPEED); if (redLed != null && greenLed != null) { redLed.off(); greenLed.on(); } launchState = LaunchState.LAUNCH; - } - break; - case HIGH: //if high launchmode is selected and shotRequested is true, check that the flywheel is in the correct velocity range (1900 - 2000 rpm) + } + break; + + case HIGH: //if high launchmode is selected and shotRequested is true, check that the flywheel is in the correct velocity range (1900 - 2000 rpm) + if (shotRequested) { + launcher.setVelocity(LAUNCHER_HIGH_TARGET_VELOCITY); + leftFeeder.setPower(STOP_SPEED); + rightFeeder.setPower(STOP_SPEED); if (launcher.getVelocity() > LAUNCHER_HIGH_MIN_VELOCITY && launcher.getVelocity() < LAUNCHER_HIGH_MAX_VELOCITY) { if (redLed != null && greenLed != null) { redLed.off(); @@ -187,24 +206,26 @@ public void launch(boolean shotRequested) { } launchState = LaunchState.LAUNCH; } - case LAUNCH: //when shotRequested, start the feeder servos to init launch - leftFeeder.setPower(FULL_SPEED); - rightFeeder.setPower(FULL_SPEED); - feederTimer.reset(); - launchState = LaunchState.LAUNCHING; - break; - case LAUNCHING: //wait until feedTimer surpasses FEED_TIME_SECONDS, then stop the feeder servos. - if (feederTimer.seconds() > FEED_TIME_SECONDS) { - launchState = LaunchState.IDLE; - leftFeeder.setPower(STOP_SPEED); - rightFeeder.setPower(STOP_SPEED); - } - if (redLed != null && greenLed != null) { - redLed.off(); - greenLed.on(); - } - break; - } + } + break; + case LAUNCH: //when shotRequested, start the feeder servos to init launch + leftFeeder.setPower(FULL_SPEED); + rightFeeder.setPower(FULL_SPEED); + feederTimer.reset(); + launchState = LaunchState.LAUNCHING; + break; + case LAUNCHING: //wait until feedTimer surpasses FEED_TIME_SECONDS, then stop the feeder servos. + if (feederTimer.seconds() > FEED_TIME_SECONDS) { + leftFeeder.setPower(STOP_SPEED); + rightFeeder.setPower(STOP_SPEED); + launchState = LaunchState.IDLE; + + } + if (redLed != null && greenLed != null) { + redLed.off(); + greenLed.on(); + } + break; } } } diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java index 99183681ca57..cf634de6be6d 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java @@ -87,8 +87,7 @@ public void runOpMode() { packet.fieldOverlay().setStroke("#3F51B5"); Drawing.drawRobot(packet.fieldOverlay(), drive.pose); MecanumDrive.sendTelemetryPacket(packet); - leftFeeder.setPower(SLOW_REV_SPEED); - rightFeeder.setPower(SLOW_REV_SPEED); + if (gamepad2.y) { //high speed launcher.setVelocity(LAUNCHER_HIGH_TARGET_VELOCITY); @@ -106,16 +105,12 @@ public void runOpMode() { launcher.setVelocity(LAUNCHER_REV_TARGET_VELOCITY); leftFeeder.setPower(REV_SPEED); rightFeeder.setPower(REV_SPEED); - launchState = LaunchState.REVERSE; } else if (gamepad2.left_bumper) { // stop flywheel launcher.setVelocity(STOP_SPEED); leftFeeder.setPower(STOP_SPEED); rightFeeder.setPower(STOP_SPEED); } - while (launchState == LaunchState.IDLE) { - leftFeeder.setPower(SLOW_REV_SPEED); - rightFeeder.setPower(SLOW_REV_SPEED); - } + /* * Now we call our "Launch" function. From 7689c944bf6b141e2f09fb78042d57be8261729a Mon Sep 17 00:00:00 2001 From: Emmett Date: Sun, 26 Oct 2025 13:49:50 -0700 Subject: [PATCH 051/198] slow reverse code is now FUNCTIONAL!!!! the robot will return to the previous launch state after launching, and resume slow_reverse_speed on the servos. --- .../firstinspires/ftc/team417/BaseOpMode.java | 25 +++++++++++++++++-- .../ftc/team417/CompetitionTeleOp.java | 3 +++ 2 files changed, 26 insertions(+), 2 deletions(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java b/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java index cb6e5abd2334..05867ee7c8dc 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java @@ -56,6 +56,7 @@ abstract public class BaseOpMode extends LinearOpMode { ElapsedTime feederTimer = new ElapsedTime(); + public String CURRENT_LAUNCHSTATE = "IDLE"; public enum LaunchState { IDLE, HIGH, @@ -164,10 +165,14 @@ public void launch(boolean shotRequested) { case IDLE: leftFeeder.setPower(SLOW_REV_SPEED); rightFeeder.setPower(SLOW_REV_SPEED); + CURRENT_LAUNCHSTATE = "IDLE"; break; case SORT: //if sorting launchmode is selected and shotRequested is true, check that the flywheel is in the correct velocity range (450 - 500 rpm) + leftFeeder.setPower(SLOW_REV_SPEED); + rightFeeder.setPower(SLOW_REV_SPEED); if (shotRequested) { + CURRENT_LAUNCHSTATE = "SORT"; launcher.setVelocity(LAUNCHER_SORTER_TARGET_VELOCITY); leftFeeder.setPower(STOP_SPEED); rightFeeder.setPower(STOP_SPEED); @@ -179,7 +184,10 @@ public void launch(boolean shotRequested) { break; case LOW: //if low launchmode is selected and shotRequested is true, check that the flywheel is in the correct velocity range (1075 - 1175 rpm) + leftFeeder.setPower(SLOW_REV_SPEED); + rightFeeder.setPower(SLOW_REV_SPEED); if (shotRequested) { + CURRENT_LAUNCHSTATE = "LOW"; launcher.setVelocity(LAUNCHER_LOW_TARGET_VELOCITY); if (launcher.getVelocity() > LAUNCHER_LOW_MIN_VELOCITY && launcher.getVelocity() < LAUNCHER_LOW_MAX_VELOCITY) { leftFeeder.setPower(STOP_SPEED); @@ -194,8 +202,10 @@ public void launch(boolean shotRequested) { break; case HIGH: //if high launchmode is selected and shotRequested is true, check that the flywheel is in the correct velocity range (1900 - 2000 rpm) + leftFeeder.setPower(SLOW_REV_SPEED); + rightFeeder.setPower(SLOW_REV_SPEED); if (shotRequested) { - + CURRENT_LAUNCHSTATE = "HIGH"; launcher.setVelocity(LAUNCHER_HIGH_TARGET_VELOCITY); leftFeeder.setPower(STOP_SPEED); rightFeeder.setPower(STOP_SPEED); @@ -218,13 +228,24 @@ public void launch(boolean shotRequested) { if (feederTimer.seconds() > FEED_TIME_SECONDS) { leftFeeder.setPower(STOP_SPEED); rightFeeder.setPower(STOP_SPEED); - launchState = LaunchState.IDLE; } + leftFeeder.setPower(SLOW_REV_SPEED); + rightFeeder.setPower(SLOW_REV_SPEED); if (redLed != null && greenLed != null) { redLed.off(); greenLed.on(); } + if (CURRENT_LAUNCHSTATE.equals("LOW") ) { + launchState = LaunchState.LOW; + } else if (CURRENT_LAUNCHSTATE.equals("HIGH")) { + launchState = LaunchState.HIGH; + + } else if (CURRENT_LAUNCHSTATE.equals("SORT")) { + launchState = LaunchState.SORT; + } else { + launchState = LaunchState.IDLE; + } break; } } diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java index cf634de6be6d..e2192cff3e38 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java @@ -105,10 +105,13 @@ public void runOpMode() { launcher.setVelocity(LAUNCHER_REV_TARGET_VELOCITY); leftFeeder.setPower(REV_SPEED); rightFeeder.setPower(REV_SPEED); + } else if (gamepad2.left_bumper) { // stop flywheel launcher.setVelocity(STOP_SPEED); leftFeeder.setPower(STOP_SPEED); rightFeeder.setPower(STOP_SPEED); + launchState = LaunchState.IDLE; + CURRENT_LAUNCHSTATE = "IDLE"; } From 224b4e23ff7acd414838a83cd92fc6b3ab442833 Mon Sep 17 00:00:00 2001 From: crest0 Date: Sun, 26 Oct 2025 14:23:00 -0700 Subject: [PATCH 052/198] Code that can detect the id and display message. --- .../ftc/team417/AprilTagTest.java | 162 ++++++++++++++++++ 1 file changed, 162 insertions(+) create mode 100644 team417/src/main/java/org/firstinspires/ftc/team417/AprilTagTest.java diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/AprilTagTest.java b/team417/src/main/java/org/firstinspires/ftc/team417/AprilTagTest.java new file mode 100644 index 000000000000..8b508e4c37cf --- /dev/null +++ b/team417/src/main/java/org/firstinspires/ftc/team417/AprilTagTest.java @@ -0,0 +1,162 @@ +package org.firstinspires.ftc.team417; + +/* Copyright (c) 2023 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import org.firstinspires.ftc.robotcore.external.hardware.camera.BuiltinCameraDirection; +import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; +import org.firstinspires.ftc.vision.VisionPortal; +import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; +import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor; + +import java.util.List; + +/* + * This OpMode illustrates the basics of AprilTag recognition and pose estimation, using + * the easy way. + * + * For an introduction to AprilTags, see the FTC-DOCS link below: + * https://ftc-docs.firstinspires.org/en/latest/apriltag/vision_portal/apriltag_intro/apriltag-intro.html + * + * In this sample, any visible tag ID will be detected and displayed, but only tags that are included in the default + * "TagLibrary" will have their position and orientation information displayed. This default TagLibrary contains + * the current Season's AprilTags and a small set of "test Tags" in the high number range. + * + * When an AprilTag in the TagLibrary is detected, the SDK provides location and orientation of the tag, relative to the camera. + * This information is provided in the "ftcPose" member of the returned "detection", and is explained in the ftc-docs page linked below. + * https://ftc-docs.firstinspires.org/apriltag-detection-values + * + * To experiment with using AprilTags to navigate, try out these two driving samples: + * RobotAutoDriveToAprilTagOmni and RobotAutoDriveToAprilTagTank + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list. + */ +@TeleOp(name = "Concept: AprilTag Easy", group = "Concept") +public class AprilTagTest extends LinearOpMode { + + private static final boolean USE_WEBCAM = true; // true for webcam, false for phone camera + + /** + * The variable to store our instance of the AprilTag processor. + */ + private AprilTagProcessor aprilTag; + + /** + * The variable to store our instance of the vision portal. + */ + private VisionPortal visionPortal; + + @Override + public void runOpMode() { + + initAprilTag(); + + // Wait for the DS start button to be touched. + telemetry.addData("DS preview on/off", "3 dots, Camera Stream"); + telemetry.addData(">", "Touch START to start OpMode"); + telemetry.update(); + waitForStart(); + + if (opModeIsActive()) { + while (opModeIsActive()) { + + telemetryAprilTag(); + + // Push telemetry to the Driver Station. + telemetry.update(); + + // Save CPU resources; can resume streaming when needed. + if (gamepad1.dpad_down) { + visionPortal.stopStreaming(); + } else if (gamepad1.dpad_up) { + visionPortal.resumeStreaming(); + } + + // Share the CPU. + sleep(20); + } + } + + // Save more CPU resources when camera is no longer needed. + visionPortal.close(); + + } // end method runOpMode() + + /** + * Initialize the AprilTag processor. + */ + private void initAprilTag() { + + // Create the AprilTag processor the easy way. + aprilTag = AprilTagProcessor.easyCreateWithDefaults(); + + // Create the vision portal the easy way. + if (USE_WEBCAM) { + visionPortal = VisionPortal.easyCreateWithDefaults( + hardwareMap.get(WebcamName.class, "camera"), aprilTag); + } else { + visionPortal = VisionPortal.easyCreateWithDefaults( + BuiltinCameraDirection.BACK, aprilTag); + } + + } // end method initAprilTag() + + /** + * Add telemetry about AprilTag detections. + */ + private void telemetryAprilTag() { + + List currentDetections = aprilTag.getDetections(); + telemetry.addData("# AprilTags Detected", currentDetections.size()); + + // Step through the list of detections and display info for each one. + for (AprilTagDetection detection : currentDetections) { + if (detection.metadata != null) { + telemetry.addLine(String.format("\n==== (ID %d) %s", detection.id, detection.metadata.name)); + telemetry.addLine(String.format("XYZ %6.1f %6.1f %6.1f (inch)", detection.ftcPose.x, detection.ftcPose.y, detection.ftcPose.z)); + telemetry.addLine(String.format("PRY %6.1f %6.1f %6.1f (deg)", detection.ftcPose.pitch, detection.ftcPose.roll, detection.ftcPose.yaw)); + telemetry.addLine(String.format("RBE %6.1f %6.1f %6.1f (inch, deg, deg)", detection.ftcPose.range, detection.ftcPose.bearing, detection.ftcPose.elevation)); + } else { + telemetry.addLine(String.format("\n==== (ID %d) Unknown", detection.id)); + telemetry.addLine(String.format("Center %6.0f %6.0f (pixels)", detection.center.x, detection.center.y)); + } + } // end for() loop + + // Add "key" information to telemetry + telemetry.addLine("\nkey:\nXYZ = X (Right), Y (Forward), Z (Up) dist."); + telemetry.addLine("PRY = Pitch, Roll & Yaw (XYZ Rotation)"); + telemetry.addLine("RBE = Range, Bearing & Elevation"); + + } // end method telemetryAprilTag() + +} // end class From de7b91d19f9e0b94cdc2e92519db400b4d7773b9 Mon Sep 17 00:00:00 2001 From: Team <417skid@swerverobotics.club> Date: Sun, 26 Oct 2025 14:52:30 -0700 Subject: [PATCH 053/198] all 4 auto paths work! --- .../firstinspires/ftc/team417/BaseOpMode.java | 14 ++++--- .../ftc/team417/CompetitionAuto.java | 39 ++++++++++++------- 2 files changed, 34 insertions(+), 19 deletions(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java b/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java index 05867ee7c8dc..297720d4411a 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java @@ -130,15 +130,19 @@ else if(MecanumDrive.isFastBot) { } class LaunchAction extends RobotAction { public boolean run(double ElapsedTime) { - leftFeeder.setPower(FULL_SPEED); - rightFeeder.setPower(FULL_SPEED); - if (ElapsedTime < 1) { + if (ElapsedTime < 0.15) { + leftFeeder.setPower(FULL_SPEED); + rightFeeder.setPower(FULL_SPEED); + + return true; + } + else if(ElapsedTime < 1) { leftFeeder.setPower(STOP_SPEED); rightFeeder.setPower(STOP_SPEED); - return false; + return true; } else { - return true; + return false; } } diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java index ded9568f6e5d..0ec7ff61dc70 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java @@ -41,11 +41,11 @@ public void runOpMode() { initHardware(); Pose2d startPose = new Pose2d(0, 0, 0); - Pose2d redNearStartPose = new Pose2d(-50, 50.5, Math.toRadians(41)); - Pose2d redFarStartPose = new Pose2d(56, 12, Math.toRadians(180)); + Pose2d redNearStartPose = new Pose2d(-60, 48, Math.toRadians(41)); + Pose2d redFarStartPose = new Pose2d(64, 16, Math.toRadians(0)); Pose2d blueNearStartPose = new Pose2d(-50, -50.5, Math.toRadians(139)); - Pose2d blueFarStartPose = new Pose2d(56, -12, Math.toRadians(180)); + Pose2d blueFarStartPose = new Pose2d(64, -16, Math.toRadians(180)); MecanumDrive drive = new MecanumDrive(hardwareMap, telemetry, gamepad1, startPose); @@ -84,46 +84,57 @@ public void runOpMode() { // Red alliance auto paths Action redNear = drive.actionBuilder(redNearStartPose) - .setTangent(Math.toRadians(131)) + .setTangent(Math.toRadians(-49)) .stopAndAdd(new SpinUpAction()) .stopAndAdd(new LaunchAction()) .stopAndAdd(new LaunchAction()) .stopAndAdd(new LaunchAction()) - .splineTo(new Vector2d(-54,38), Math.toRadians(41)) + + .splineToLinearHeading(new Pose2d(-32,54,Math.toRadians(0)), Math.toRadians(90)) .build(); Action redFar = drive.actionBuilder(redFarStartPose) - .splineTo(new Vector2d(-50, 50), Math.toRadians(41)) - .splineTo(new Vector2d(-20, 51), 0) + .setTangent(Math.toRadians(135)) + .splineToLinearHeading(new Pose2d(-57, 36, Math.toRadians(0)), Math.toRadians(90)) + .stopAndAdd(new SpinUpAction()) + .stopAndAdd(new LaunchAction()) + .stopAndAdd(new LaunchAction()) + .stopAndAdd(new LaunchAction()) + .setTangent(Math.toRadians(-90)) + .splineToLinearHeading(new Pose2d(-56, 12, Math.toRadians(0)), Math.toRadians(-90)) .build(); Action redFarMinimal = drive.actionBuilder(redFarStartPose) .setTangent(Math.PI/2) - .splineTo(new Vector2d(-56, 35), Math.PI/2) + .splineTo(new Vector2d(56, 35), Math.PI/2) .build(); // Blue alliance auto paths Action blueNear = drive.actionBuilder(blueNearStartPose) .setTangent(Math.toRadians(49)) .stopAndAdd(new SpinUpAction()) - .stopAndAdd(new SleepAction(2)) .stopAndAdd(new LaunchAction()) - .stopAndAdd(new SleepAction(2)) .stopAndAdd(new LaunchAction()) - .stopAndAdd(new SleepAction(2)) .stopAndAdd(new LaunchAction()) // .splineTo(new Vector2d(-44, -44), Math.toRadians(49)) // .setTangent(Math.toRadians(139)) - .splineTo(new Vector2d(-54,-38), Math.toRadians(139)) + .splineToLinearHeading(new Pose2d(-32,-54,Math.toRadians(180)), Math.toRadians(-90)) .build(); Action blueFar = drive.actionBuilder(blueFarStartPose) - .splineTo(new Vector2d(-50, -50), Math.toRadians(41)) + .setTangent(Math.toRadians(-135)) + .splineToLinearHeading(new Pose2d(-57, -36, Math.toRadians(180)), Math.toRadians(-90)) + .stopAndAdd(new SpinUpAction()) + .stopAndAdd(new LaunchAction()) + .stopAndAdd(new LaunchAction()) + .stopAndAdd(new LaunchAction()) + .setTangent(Math.toRadians(90)) + .splineToLinearHeading(new Pose2d(-56, -12, Math.toRadians(180)), Math.toRadians(90)) .build(); Action blueFarMinimal = drive.actionBuilder(blueFarStartPose) .setTangent(Math.PI/2) - .splineTo(new Vector2d(-56, 35), Math.PI/2) + .splineTo(new Vector2d(56, -35), Math.PI/2) .build(); From f60ecb386a9c89f7904971e335a92784018d8835 Mon Sep 17 00:00:00 2001 From: anya-codes Date: Sun, 26 Oct 2025 15:16:13 -0700 Subject: [PATCH 054/198] Switched slowMode control to right trigger to make it easier for drivers --- .../ftc/team417/CompetitionTeleOp.java | 16 ++++++++++------ 1 file changed, 10 insertions(+), 6 deletions(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java index e2192cff3e38..5028f0642b9c 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java @@ -58,11 +58,11 @@ public void runOpMode() { // Set the drive motor powers according to the gamepad input: drive.setDrivePowers(new PoseVelocity2d( new Vector2d( - -gamepad1.left_stick_y * doSLOWMODE(), - -gamepad1.left_stick_x * doSLOWMODE() + halfLinearHalfCubic(-gamepad1.left_stick_y * doSLOWMODE()), + halfLinearHalfCubic(-gamepad1.left_stick_x * doSLOWMODE()) ), - -gamepad1.right_stick_x + halfLinearHalfCubic(-gamepad1.right_stick_x) )); @@ -137,10 +137,14 @@ public void runOpMode() { } public double doSLOWMODE() { - if (gamepad1.left_stick_button) { - return SLOWDRIVE_SPEED; + if (gamepad1.right_trigger != 0) { + return -gamepad1.right_trigger + 1.1; } else { - return FASTDRIVE_SPEED; + return 1; } } + + public static double halfLinearHalfCubic(double input) { + return (Math.pow(input, 3) + input) / 2; + } } From fe461cfe40e2ae47b2601aa37535e1f4ae49cedc Mon Sep 17 00:00:00 2001 From: crest0 Date: Sun, 26 Oct 2025 15:16:45 -0700 Subject: [PATCH 055/198] Increased resolution --- .../external/samples/ConceptAprilTagEasy.java | 42 ++++++++++++++++--- 1 file changed, 37 insertions(+), 5 deletions(-) diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagEasy.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagEasy.java index 12dcf6e99d21..f7c3733c13d9 100644 --- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagEasy.java +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagEasy.java @@ -29,6 +29,8 @@ package org.firstinspires.ftc.robotcontroller.external.samples; +import android.util.Size; + import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; @@ -121,15 +123,45 @@ private void initAprilTag() { // Create the AprilTag processor the easy way. aprilTag = AprilTagProcessor.easyCreateWithDefaults(); - // Create the vision portal the easy way. +// // Create the vision portal the easy way. +// if (USE_WEBCAM) { +// visionPortal = VisionPortal.easyCreateWithDefaults( +// hardwareMap.get(WebcamName.class, "Webcam 1"), aprilTag); +// } else { +// visionPortal = VisionPortal.easyCreateWithDefaults( +// BuiltinCameraDirection.BACK, aprilTag); +// } + + VisionPortal.Builder builder = new VisionPortal.Builder(); + + // Set the camera (webcam vs. built-in RC phone camera). if (USE_WEBCAM) { - visionPortal = VisionPortal.easyCreateWithDefaults( - hardwareMap.get(WebcamName.class, "Webcam 1"), aprilTag); + builder.setCamera(hardwareMap.get(WebcamName.class, "Webcam 1")); } else { - visionPortal = VisionPortal.easyCreateWithDefaults( - BuiltinCameraDirection.BACK, aprilTag); + builder.setCamera(BuiltinCameraDirection.BACK); } + // Choose a camera resolution. Not all cameras support all resolutions. + builder.setCameraResolution(new Size(1920, 1080)); + + // Enable the RC preview (LiveView). Set "false" to omit camera monitoring. + //builder.enableLiveView(true); + + // Set the stream format; MJPEG uses less bandwidth than default YUY2. + //builder.setStreamFormat(VisionPortal.StreamFormat.YUY2); + + // Choose whether or not LiveView stops if no processors are enabled. + // If set "true", monitor shows solid orange screen if no processors enabled. + // If set "false", monitor shows camera view without annotations. + //builder.setAutoStopLiveView(false); + + // Set and enable the processor. + builder.addProcessor(aprilTag); + + // Build the Vision Portal, using the above settings. + visionPortal = builder.build(); + + } // end method initAprilTag() /** From bc8ba2f63d34893236da792447c6bf3a601b32ba Mon Sep 17 00:00:00 2001 From: Emmett Date: Sun, 26 Oct 2025 16:55:37 -0700 Subject: [PATCH 056/198] added launcherVelocity variable to reduce lag. --- .../java/org/firstinspires/ftc/team417/BaseOpMode.java | 10 ++++++---- .../firstinspires/ftc/team417/CompetitionTeleOp.java | 3 ++- 2 files changed, 8 insertions(+), 5 deletions(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java b/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java index 297720d4411a..025eef462518 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java @@ -18,6 +18,7 @@ */ abstract public class BaseOpMode extends LinearOpMode { + public DcMotorEx launcher = null; public CRServo leftFeeder = null; public CRServo rightFeeder = null; @@ -84,6 +85,7 @@ public void initHardware() { leftFeeder = hardwareMap.get(CRServo.class, "servoBLaunchFeed"); rightFeeder = hardwareMap.get(CRServo.class, "servoFLaunchFeed"); + // Reversed direction of launcher for DevBot because motor is on the other side (compared to FastBot) if (MecanumDrive.isDevBot) { launcher.setDirection(DcMotorEx.Direction.REVERSE); @@ -163,7 +165,7 @@ public boolean run(double ElapsedTime) { public void launch(boolean shotRequested) { - + double launcherVelocity = launcher.getVelocity(); switch (launchState) { case IDLE: @@ -180,7 +182,7 @@ public void launch(boolean shotRequested) { launcher.setVelocity(LAUNCHER_SORTER_TARGET_VELOCITY); leftFeeder.setPower(STOP_SPEED); rightFeeder.setPower(STOP_SPEED); - if (launcher.getVelocity() > LAUNCHER_SORTER_MIN_VELOCITY && launcher.getVelocity() < LAUNCHER_SORTER_MAX_VELOCITY) { + if (launcherVelocity > LAUNCHER_SORTER_MIN_VELOCITY && launcherVelocity < LAUNCHER_SORTER_MAX_VELOCITY) { launchState = LaunchState.LAUNCH; } @@ -193,7 +195,7 @@ public void launch(boolean shotRequested) { if (shotRequested) { CURRENT_LAUNCHSTATE = "LOW"; launcher.setVelocity(LAUNCHER_LOW_TARGET_VELOCITY); - if (launcher.getVelocity() > LAUNCHER_LOW_MIN_VELOCITY && launcher.getVelocity() < LAUNCHER_LOW_MAX_VELOCITY) { + if (launcherVelocity > LAUNCHER_LOW_MIN_VELOCITY && launcherVelocity < LAUNCHER_LOW_MAX_VELOCITY) { leftFeeder.setPower(STOP_SPEED); rightFeeder.setPower(STOP_SPEED); if (redLed != null && greenLed != null) { @@ -213,7 +215,7 @@ public void launch(boolean shotRequested) { launcher.setVelocity(LAUNCHER_HIGH_TARGET_VELOCITY); leftFeeder.setPower(STOP_SPEED); rightFeeder.setPower(STOP_SPEED); - if (launcher.getVelocity() > LAUNCHER_HIGH_MIN_VELOCITY && launcher.getVelocity() < LAUNCHER_HIGH_MAX_VELOCITY) { + if (launcherVelocity > LAUNCHER_HIGH_MIN_VELOCITY && launcherVelocity < LAUNCHER_HIGH_MAX_VELOCITY) { if (redLed != null && greenLed != null) { redLed.off(); greenLed.on(); diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java index e2192cff3e38..6d1d66fbab09 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java @@ -130,7 +130,8 @@ public void runOpMode() { // telemetry.addData("Motors", "left (%.2f), right (%.2f)", leftPower, rightPower); telemetry.addData("motorSpeed", launcher.getVelocity()); telemetry.addData("FEED_TIME_SECONDS", FEED_TIME_SECONDS); - telemetry.addData("feederSpeed", leftFeeder.getPower()); + telemetry.addData("leftFeeder", leftFeeder.getPower()); + telemetry.addData("rightFeeder", rightFeeder.getPower()); telemetry.update(); } From 370c0ecc93e4404cc7b3879ef0b48c3e678338bc Mon Sep 17 00:00:00 2001 From: anya-codes Date: Mon, 27 Oct 2025 23:03:29 -0700 Subject: [PATCH 057/198] Added code to check which robot it is (FastBot, SlowBot, or DevBot) so we can add controls for SlowBot teleop --- .../firstinspires/ftc/team417/BaseOpMode.java | 75 +++++------ .../ftc/team417/CompetitionTeleOp.java | 123 +++++++++--------- .../ftc/team417/roadrunner/MecanumDrive.java | 1 + 3 files changed, 103 insertions(+), 96 deletions(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java b/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java index 025eef462518..336fa640e54f 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java @@ -70,21 +70,6 @@ public enum LaunchState { public LaunchState launchState; public void initHardware() { - launchState = LaunchState.IDLE; - - /* - * Initialize the hardware variables. Note that the strings used here as parameters - * to 'get' must correspond to the names assigned during the robot configuration - * step. - */ - // leftDrive = hardwareMap.get(DcMotor.class, "left_drive"); - // rightDrive = hardwareMap.get(DcMotor.class, "right_drive"); - - // initialize flywheel motor and feeder servos - launcher = hardwareMap.get(DcMotorEx.class, "motLauncher"); - leftFeeder = hardwareMap.get(CRServo.class, "servoBLaunchFeed"); - rightFeeder = hardwareMap.get(CRServo.class, "servoFLaunchFeed"); - // Reversed direction of launcher for DevBot because motor is on the other side (compared to FastBot) if (MecanumDrive.isDevBot) { @@ -102,29 +87,45 @@ else if(MecanumDrive.isFastBot) { //greenLed = hardwareMap.get(LED.class, "greenLed"); //redLed.on(); //greenLed.off(); - } - - /* - * Here we set our launcher to the RUN_USING_ENCODER runmode. - * If you notice that you have no control over the velocity of the motor, it just jumps - * right to a number much higher than your set point, make sure that your encoders are plugged - * into the port right beside the motor itself. And that the motors polarity is consistent - * through any wiring. - */ - launcher.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - - // set the flywheel to a braking behavior so it slows down faster when left trigger is pressed - launcher.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - - // set the feeder servos to an initial value to init the servo controller - leftFeeder.setPower(STOP_SPEED); - rightFeeder.setPower(STOP_SPEED); - - launcher.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, new PIDFCoefficients(300, 0, 0, 10)); - - //set the left feeder servo to rotate in reverse, so that the servos spin in the same relative direction - leftFeeder.setDirection(DcMotorSimple.Direction.REVERSE); + launchState = LaunchState.IDLE; + + /* + * Initialize the hardware variables. Note that the strings used here as parameters + * to 'get' must correspond to the names assigned during the robot configuration + * step. + */ + // leftDrive = hardwareMap.get(DcMotor.class, "left_drive"); + // rightDrive = hardwareMap.get(DcMotor.class, "right_drive"); + + // initialize flywheel motor and feeder servos + launcher = hardwareMap.get(DcMotorEx.class, "motLauncher"); + leftFeeder = hardwareMap.get(CRServo.class, "servoBLaunchFeed"); + rightFeeder = hardwareMap.get(CRServo.class, "servoFLaunchFeed"); + + /* + * Here we set our launcher to the RUN_USING_ENCODER runmode. + * If you notice that you have no control over the velocity of the motor, it just jumps + * right to a number much higher than your set point, make sure that your encoders are plugged + * into the port right beside the motor itself. And that the motors polarity is consistent + * through any wiring. + */ + launcher.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + + // set the flywheel to a braking behavior so it slows down faster when left trigger is pressed + launcher.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + + // set the feeder servos to an initial value to init the servo controller + leftFeeder.setPower(STOP_SPEED); + rightFeeder.setPower(STOP_SPEED); + + launcher.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, new PIDFCoefficients(300, 0, 0, 10)); + + //set the left feeder servo to rotate in reverse, so that the servos spin in the same relative direction + leftFeeder.setDirection(DcMotorSimple.Direction.REVERSE); + } else if (MecanumDrive.isSlowBot) { + //add slowbot initialization code here + } // Tell the driver that initialization is complete. diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java index 1a39fd5d1702..109dd925db3f 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java @@ -74,66 +74,71 @@ public void runOpMode() { // 'packet' is the object used to send data to FTC Dashboard: TelemetryPacket packet = MecanumDrive.getTelemetryPacket(); - // send telemetry to FTC dashboard to graph - packet.put("FlyWheel Speed:", launcher.getVelocity()); - packet.put("Right bumper press:", gamepad2.right_bumper ? 0 : 1000); - packet.put("Feeder wheels:", rightFeeder.getPower() * 100); - - - // Do the work now for all active Road Runner actions, if any: - drive.doActionsWork(packet); - - // Draw the robot and field: - packet.fieldOverlay().setStroke("#3F51B5"); - Drawing.drawRobot(packet.fieldOverlay(), drive.pose); - MecanumDrive.sendTelemetryPacket(packet); - - - if (gamepad2.y) { //high speed - launcher.setVelocity(LAUNCHER_HIGH_TARGET_VELOCITY); - launchState = LaunchState.HIGH; - - } else if (gamepad2.a) { //slow speed - launcher.setVelocity(LAUNCHER_LOW_TARGET_VELOCITY); - launchState = LaunchState.LOW; - - } else if (gamepad2.x) { // sort speed - launcher.setVelocity(LAUNCHER_SORTER_TARGET_VELOCITY); - launchState = LaunchState.SORT; - - } else if (gamepad2.b) { //reverse - launcher.setVelocity(LAUNCHER_REV_TARGET_VELOCITY); - leftFeeder.setPower(REV_SPEED); - rightFeeder.setPower(REV_SPEED); - - } else if (gamepad2.left_bumper) { // stop flywheel - launcher.setVelocity(STOP_SPEED); - leftFeeder.setPower(STOP_SPEED); - rightFeeder.setPower(STOP_SPEED); - launchState = LaunchState.IDLE; - CURRENT_LAUNCHSTATE = "IDLE"; - } - - - /* - * Now we call our "Launch" function. - */ - if (rightBumperTimer.seconds() > 0.25) { - launch(gamepad2.rightBumperWasPressed()); - rightBumperTimer.reset(); + if (MecanumDrive.isFastBot) { + // send telemetry to FTC dashboard to graph + packet.put("FlyWheel Speed:", launcher.getVelocity()); + packet.put("Right bumper press:", gamepad2.right_bumper ? 0 : 1000); + packet.put("Feeder wheels:", rightFeeder.getPower() * 100); + + + // Do the work now for all active Road Runner actions, if any: + drive.doActionsWork(packet); + + // Draw the robot and field: + packet.fieldOverlay().setStroke("#3F51B5"); + Drawing.drawRobot(packet.fieldOverlay(), drive.pose); + MecanumDrive.sendTelemetryPacket(packet); + + + if (gamepad2.y) { //high speed + launcher.setVelocity(LAUNCHER_HIGH_TARGET_VELOCITY); + launchState = LaunchState.HIGH; + + } else if (gamepad2.a) { //slow speed + launcher.setVelocity(LAUNCHER_LOW_TARGET_VELOCITY); + launchState = LaunchState.LOW; + + } else if (gamepad2.x) { // sort speed + launcher.setVelocity(LAUNCHER_SORTER_TARGET_VELOCITY); + launchState = LaunchState.SORT; + + } else if (gamepad2.b) { //reverse + launcher.setVelocity(LAUNCHER_REV_TARGET_VELOCITY); + leftFeeder.setPower(REV_SPEED); + rightFeeder.setPower(REV_SPEED); + + } else if (gamepad2.left_bumper) { // stop flywheel + launcher.setVelocity(STOP_SPEED); + leftFeeder.setPower(STOP_SPEED); + rightFeeder.setPower(STOP_SPEED); + launchState = LaunchState.IDLE; + CURRENT_LAUNCHSTATE = "IDLE"; + } + + + /* + * Now we call our "Launch" function. + */ + if (rightBumperTimer.seconds() > 0.25) { + launch(gamepad2.rightBumperWasPressed()); + rightBumperTimer.reset(); + } + + /* + * Show the state and motor powers + */ + telemetry.addData("State", launchState); + // telemetry.addData("Motors", "left (%.2f), right (%.2f)", leftPower, rightPower); + telemetry.addData("motorSpeed", launcher.getVelocity()); + telemetry.addData("FEED_TIME_SECONDS", FEED_TIME_SECONDS); + telemetry.addData("leftFeeder", leftFeeder.getPower()); + telemetry.addData("rightFeeder", rightFeeder.getPower()); + + telemetry.update(); + + } else if (MecanumDrive.isSlowBot) { + //add slowbot teleop controls here } - - /* - * Show the state and motor powers - */ - telemetry.addData("State", launchState); - // telemetry.addData("Motors", "left (%.2f), right (%.2f)", leftPower, rightPower); - telemetry.addData("motorSpeed", launcher.getVelocity()); - telemetry.addData("FEED_TIME_SECONDS", FEED_TIME_SECONDS); - telemetry.addData("leftFeeder", leftFeeder.getPower()); - telemetry.addData("rightFeeder", rightFeeder.getPower()); - - telemetry.update(); } } diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/roadrunner/MecanumDrive.java b/team417/src/main/java/org/firstinspires/ftc/team417/roadrunner/MecanumDrive.java index 8d2e9fd640a7..00644adb7208 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/roadrunner/MecanumDrive.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/roadrunner/MecanumDrive.java @@ -207,6 +207,7 @@ public static String getBotName() { } public static boolean isDevBot = getBotName().equals("DevBot"); public static boolean isFastBot = getBotName().equals("417-RC"); + public static boolean isSlowBot = getBotName().equals("417-b-RC"); public static Params PARAMS = new Params(); public MecanumKinematics kinematics; // Initialized by initializeKinematics() From 9ac0fd9ca3e439b21f2238b62ee90b982b788ea1 Mon Sep 17 00:00:00 2001 From: Emmett Date: Tue, 28 Oct 2025 19:26:07 -0700 Subject: [PATCH 058/198] incomplete changes --- .../firstinspires/ftc/team417/BaseOpMode.java | 2 ++ .../ftc/team417/CompetitionTeleOp.java | 16 ++++++++++++++++ 2 files changed, 18 insertions(+) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java b/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java index 336fa640e54f..cac8b5e467c2 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java @@ -125,6 +125,8 @@ else if(MecanumDrive.isFastBot) { leftFeeder.setDirection(DcMotorSimple.Direction.REVERSE); } else if (MecanumDrive.isSlowBot) { //add slowbot initialization code here + launcher = hardwareMap.get(DcMotorEx.class, "motLauncher"); + } diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java index 109dd925db3f..47e9c979bfef 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java @@ -138,6 +138,22 @@ public void runOpMode() { } else if (MecanumDrive.isSlowBot) { //add slowbot teleop controls here + if (gamepad2.y) { //high speed + + + } else if (gamepad2.a) { //slow speed + + + } else if (gamepad2.x) { // sort speed + + + } else if (gamepad2.b) { //reverse + + } else if (gamepad2.left_bumper) { // stop launcher + launcher.setVelocity(STOP_SPEED); + + } else if (gamepad2.right_bumper) { //launch + transfer + launcher.setVelocity(); } } } From d4ecc51c24085e3aa7749bc4ffe1ba0b8e81da54 Mon Sep 17 00:00:00 2001 From: Team <417skid@swerverobotics.club> Date: Tue, 28 Oct 2025 20:49:56 -0700 Subject: [PATCH 059/198] all 4 auto paths work! --- .../java/org/firstinspires/ftc/team417/BaseOpMode.java | 4 ++-- .../org/firstinspires/ftc/team417/CompetitionAuto.java | 10 +++++----- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java b/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java index 297720d4411a..537971443fa2 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java @@ -39,7 +39,7 @@ abstract public class BaseOpMode extends LinearOpMode { public static double LAUNCHER_LOW_MAX_VELOCITY = 1175; //low target velocity + 50 (will need adjusting) public static double LAUNCHER_LOW_TARGET_VELOCITY = 1125; - public static double LAUNCHER_LOW_MIN_VELOCITY = 1075; + public static double LAUNCHER_LOW_MIN_VELOCITY = 1075;// jonathan was here public static double LAUNCHER_SORTER_MAX_VELOCITY = 550; //sorter target velocity + 50 (will need adjusting) public static double LAUNCHER_SORTER_TARGET_VELOCITY = 500; @@ -130,7 +130,7 @@ else if(MecanumDrive.isFastBot) { } class LaunchAction extends RobotAction { public boolean run(double ElapsedTime) { - if (ElapsedTime < 0.15) { + if (ElapsedTime < 0.25) { leftFeeder.setPower(FULL_SPEED); rightFeeder.setPower(FULL_SPEED); diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java index 0ec7ff61dc70..5982e7057bb9 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java @@ -95,13 +95,13 @@ public void runOpMode() { Action redFar = drive.actionBuilder(redFarStartPose) .setTangent(Math.toRadians(135)) - .splineToLinearHeading(new Pose2d(-57, 36, Math.toRadians(0)), Math.toRadians(90)) + .splineToLinearHeading(new Pose2d(-56, 40, Math.toRadians(0)), Math.toRadians(90)) .stopAndAdd(new SpinUpAction()) .stopAndAdd(new LaunchAction()) .stopAndAdd(new LaunchAction()) .stopAndAdd(new LaunchAction()) .setTangent(Math.toRadians(-90)) - .splineToLinearHeading(new Pose2d(-56, 12, Math.toRadians(0)), Math.toRadians(-90)) + .splineToLinearHeading(new Pose2d(-57, 12, Math.toRadians(0)), Math.toRadians(-90)) .build(); Action redFarMinimal = drive.actionBuilder(redFarStartPose) @@ -118,18 +118,18 @@ public void runOpMode() { .stopAndAdd(new LaunchAction()) // .splineTo(new Vector2d(-44, -44), Math.toRadians(49)) // .setTangent(Math.toRadians(139)) - .splineToLinearHeading(new Pose2d(-32,-54,Math.toRadians(180)), Math.toRadians(-90)) + .splineToLinearHeading(new Pose2d(-32,-54 ,Math.toRadians(180)), Math.toRadians(-90)) .build(); Action blueFar = drive.actionBuilder(blueFarStartPose) .setTangent(Math.toRadians(-135)) - .splineToLinearHeading(new Pose2d(-57, -36, Math.toRadians(180)), Math.toRadians(-90)) + .splineToLinearHeading(new Pose2d(-56, -40, Math.toRadians(180)), Math.toRadians(-90)) .stopAndAdd(new SpinUpAction()) .stopAndAdd(new LaunchAction()) .stopAndAdd(new LaunchAction()) .stopAndAdd(new LaunchAction()) .setTangent(Math.toRadians(90)) - .splineToLinearHeading(new Pose2d(-56, -12, Math.toRadians(180)), Math.toRadians(90)) + .splineToLinearHeading(new Pose2d(-57, -12, Math.toRadians(180)), Math.toRadians(90)) .build(); Action blueFarMinimal = drive.actionBuilder(blueFarStartPose) From 011bbbbac789d128097df471c97290211fa16202 Mon Sep 17 00:00:00 2001 From: Emmett Date: Tue, 28 Oct 2025 20:54:18 -0700 Subject: [PATCH 060/198] added drum rotation and launch slowbot TeleOp controls. Not finished. --- .../firstinspires/ftc/team417/BaseOpMode.java | 3 + .../ftc/team417/CompetitionTeleOp.java | 59 +++-- .../ftc/team417/SensorColor.java | 227 ++++++++++++++++++ 3 files changed, 274 insertions(+), 15 deletions(-) create mode 100644 team417/src/main/java/org/firstinspires/ftc/team417/SensorColor.java diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java b/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java index cac8b5e467c2..dafa4c06d4b0 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java @@ -20,6 +20,8 @@ abstract public class BaseOpMode extends LinearOpMode { public DcMotorEx launcher = null; + + public DcMotorEx drum = null; public CRServo leftFeeder = null; public CRServo rightFeeder = null; public static final double STOP_SPEED = 0.0; //We send this power to the servos when we want them to stop. @@ -100,6 +102,7 @@ else if(MecanumDrive.isFastBot) { // initialize flywheel motor and feeder servos launcher = hardwareMap.get(DcMotorEx.class, "motLauncher"); + drum = hardwareMap.get(DcMotorEx.class, "drum"); leftFeeder = hardwareMap.get(CRServo.class, "servoBLaunchFeed"); rightFeeder = hardwareMap.get(CRServo.class, "servoFLaunchFeed"); diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java index 47e9c979bfef..f849bb806c90 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java @@ -22,6 +22,12 @@ public class CompetitionTeleOp extends BaseOpMode { double FASTDRIVE_SPEED = 1.0; double SLOWDRIVE_SPEED = 0.5; + int moveOnePosition = 0; //change meeeeeeeee (should be 120 degrees ish) + double drumVelocity = 0; //change meeeeeeeee (needs to be fast) + + double intakeSpeed = gamepad2.left_stick_x; + double Multiply = 0; //need to change, placeholder + ElapsedTime rightBumperTimer = new ElapsedTime(); /* @@ -138,35 +144,58 @@ public void runOpMode() { } else if (MecanumDrive.isSlowBot) { //add slowbot teleop controls here - if (gamepad2.y) { //high speed + if (gamepad2.y) { //rotate drum to purple - } else if (gamepad2.a) { //slow speed + + } else if (gamepad2.a) { //rotate drum to green + + } } else if (gamepad2.x) { // sort speed + drum.setTargetPosition(moveOnePosition); + drum.setVelocity(drumVelocity); - } else if (gamepad2.b) { //reverse + } - } else if (gamepad2.left_bumper) { // stop launcher + /* launcher exclusive block + ----------------------------- + */ + if (gamepad2.left_bumper) { // stop launcher launcher.setVelocity(STOP_SPEED); } else if (gamepad2.right_bumper) { //launch + transfer - launcher.setVelocity(); + launcher.setVelocity(0/*variable*/); + //transfer stuff goes here + } else if (gamepad2.dpad_up) { + launcher.setVelocity(LAUNCHER_HIGH_TARGET_VELOCITY); + launchState = LaunchState.HIGH; + //values will need change, possibly new enum for slowbot launch + } else if (gamepad2.dpad_down) { + launcher.setVelocity(LAUNCHER_LOW_TARGET_VELOCITY); + launchState = LaunchState.LOW; + //values will need change, possibly new enum for slowbot launch + } + + if (gamepad2.left_stick_x == 0 /*change meeeee */){ + launcher.setVelocity(intakeSpeed * Multiply); + + } } } - } - public double doSLOWMODE() { - if (gamepad1.right_trigger != 0) { - return -gamepad1.right_trigger + 1.1; - } else { - return 1; + public double doSLOWMODE () { + if (gamepad1.right_trigger != 0) { + return -gamepad1.right_trigger + 1.1; + } else { + return 1; + } } - } - public static double halfLinearHalfCubic(double input) { - return (Math.pow(input, 3) + input) / 2; + public static double halfLinearHalfCubic ( double input){ + return (Math.pow(input, 3) + input) / 2; + } } -} + diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/SensorColor.java b/team417/src/main/java/org/firstinspires/ftc/team417/SensorColor.java new file mode 100644 index 000000000000..ddfcc839bfba --- /dev/null +++ b/team417/src/main/java/org/firstinspires/ftc/team417/SensorColor.java @@ -0,0 +1,227 @@ +/* Copyright (c) 2017-2020 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.team417; + +import android.app.Activity; +import android.graphics.Color; +import android.view.View; + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DistanceSensor; +import com.qualcomm.robotcore.hardware.NormalizedColorSensor; +import com.qualcomm.robotcore.hardware.NormalizedRGBA; +import com.qualcomm.robotcore.hardware.SwitchableLight; + +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; + +/* + * This OpMode shows how to use a color sensor in a generic + * way, regardless of which particular make or model of color sensor is used. The OpMode + * assumes that the color sensor is configured with a name of "sensor_color". + * + * There will be some variation in the values measured depending on the specific sensor you are using. + * + * If the color sensor supports adjusting the gain, you can increase the gain (a multiplier to make + * the sensor report higher values) by holding down the A button on the gamepad, and decrease the + * gain by holding down the B button on the gamepad. The AndyMark Proximity & Color Sensor does not + * support this. + * + * If the color sensor has a light which is controllable from software, you can use the X button on + * the gamepad to toggle the light on and off. The REV sensors don't support this, but instead have + * a physical switch on them to turn the light on and off, beginning with REV Color Sensor V2. The + * AndyMark Proximity & Color Sensor does not support this. + * + * If the color sensor also supports short-range distance measurements (usually via an infrared + * proximity sensor), the reported distance will be written to telemetry. As of September 2025, + * the only color sensors that support this are the ones from REV Robotics and the AndyMark + * Proximity & Color Sensor. These infrared proximity sensor measurements are only useful at very + * small distances, and are sensitive to ambient light and surface reflectivity. You should use a + * different sensor if you need precise distance measurements. + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list + */ +@TeleOp(name = "Sensor: Color", group = "Sensor") +@Disabled +public class SensorColor extends LinearOpMode { + + /** The colorSensor field will contain a reference to our color sensor hardware object */ + NormalizedColorSensor colorSensor; + + /** The relativeLayout field is used to aid in providing interesting visual feedback + * in this sample application; you probably *don't* need this when you use a color sensor on your + * robot. Note that you won't see anything change on the Driver Station, only on the Robot Controller. */ + View relativeLayout; + + /* + * The runOpMode() method is the root of this OpMode, as it is in all LinearOpModes. + * Our implementation here, though is a bit unusual: we've decided to put all the actual work + * in the runSample() method rather than directly in runOpMode() itself. The reason we do that is + * that in this sample we're changing the background color of the robot controller screen as the + * OpMode runs, and we want to be able to *guarantee* that we restore it to something reasonable + * and palatable when the OpMode ends. The simplest way to do that is to use a try...finally + * block around the main, core logic, and an easy way to make that all clear was to separate + * the former from the latter in separate methods. + */ + @Override public void runOpMode() { + + // Get a reference to the RelativeLayout so we can later change the background + // color of the Robot Controller app to match the hue detected by the RGB sensor. + int relativeLayoutId = hardwareMap.appContext.getResources().getIdentifier("RelativeLayout", "id", hardwareMap.appContext.getPackageName()); + relativeLayout = ((Activity) hardwareMap.appContext).findViewById(relativeLayoutId); + + try { + runSample(); // actually execute the sample + } finally { + // On the way out, *guarantee* that the background is reasonable. It doesn't actually start off + // as pure white, but it's too much work to dig out what actually was used, and this is good + // enough to at least make the screen reasonable again. + // Set the panel back to the default color + relativeLayout.post(new Runnable() { + public void run() { + relativeLayout.setBackgroundColor(Color.WHITE); + } + }); + } + } + + protected void runSample() { + // You can give the sensor a gain value, will be multiplied by the sensor's raw value before the + // normalized color values are calculated. Color sensors (especially the REV Color Sensor V3) + // can give very low values (depending on the lighting conditions), which only use a small part + // of the 0-1 range that is available for the red, green, and blue values. In brighter conditions, + // you should use a smaller gain than in dark conditions. If your gain is too high, all of the + // colors will report at or near 1, and you won't be able to determine what color you are + // actually looking at. For this reason, it's better to err on the side of a lower gain + // (but always greater than or equal to 1). + float gain = 2; + + // Once per loop, we will update this hsvValues array. The first element (0) will contain the + // hue, the second element (1) will contain the saturation, and the third element (2) will + // contain the value. See http://web.archive.org/web/20190311170843/https://infohost.nmt.edu/tcc/help/pubs/colortheory/web/hsv.html + // for an explanation of HSV color. + final float[] hsvValues = new float[3]; + + // xButtonPreviouslyPressed and xButtonCurrentlyPressed keep track of the previous and current + // state of the X button on the gamepad + boolean xButtonPreviouslyPressed = false; + boolean xButtonCurrentlyPressed = false; + + // Get a reference to our sensor object. It's recommended to use NormalizedColorSensor over + // ColorSensor, because NormalizedColorSensor consistently gives values between 0 and 1, while + // the values you get from ColorSensor are dependent on the specific sensor you're using. + colorSensor = hardwareMap.get(NormalizedColorSensor.class, "sensor_color"); + + // If possible, turn the light on in the beginning (it might already be on anyway, + // we just make sure it is if we can). + if (colorSensor instanceof SwitchableLight) { + ((SwitchableLight)colorSensor).enableLight(true); + } + + // Wait for the start button to be pressed. + waitForStart(); + + // Loop until we are asked to stop + while (opModeIsActive()) { + // Explain basic gain information via telemetry + telemetry.addLine("Hold the A button on gamepad 1 to increase gain, or B to decrease it.\n"); + telemetry.addLine("Higher gain values mean that the sensor will report larger numbers for Red, Green, and Blue, and Value\n"); + + // Update the gain value if either of the A or B gamepad buttons is being held + if (gamepad1.a) { + // Only increase the gain by a small amount, since this loop will occur multiple times per second. + gain += 0.005; + } else if (gamepad1.b && gain > 1) { // A gain of less than 1 will make the values smaller, which is not helpful. + gain -= 0.005; + } + + // Show the gain value via telemetry + telemetry.addData("Gain", gain); + + // Tell the sensor our desired gain value (normally you would do this during initialization, + // not during the loop) + colorSensor.setGain(gain); + + // Check the status of the X button on the gamepad + xButtonCurrentlyPressed = gamepad1.x; + + // If the button state is different than what it was, then act + if (xButtonCurrentlyPressed != xButtonPreviouslyPressed) { + // If the button is (now) down, then toggle the light + if (xButtonCurrentlyPressed) { + if (colorSensor instanceof SwitchableLight) { + SwitchableLight light = (SwitchableLight)colorSensor; + light.enableLight(!light.isLightOn()); + } + } + } + xButtonPreviouslyPressed = xButtonCurrentlyPressed; + + // Get the normalized colors from the sensor + NormalizedRGBA colors = colorSensor.getNormalizedColors(); + + /* Use telemetry to display feedback on the driver station. We show the red, green, and blue + * normalized values from the sensor (in the range of 0 to 1), as well as the equivalent + * HSV (hue, saturation and value) values. See http://web.archive.org/web/20190311170843/https://infohost.nmt.edu/tcc/help/pubs/colortheory/web/hsv.html + * for an explanation of HSV color. */ + + // Update the hsvValues array by passing it to Color.colorToHSV() + Color.colorToHSV(colors.toColor(), hsvValues); + + telemetry.addLine() + .addData("Red", "%.3f", colors.red) + .addData("Green", "%.3f", colors.green) + .addData("Blue", "%.3f", colors.blue); + telemetry.addLine() + .addData("Hue", "%.3f", hsvValues[0]) + .addData("Saturation", "%.3f", hsvValues[1]) + .addData("Value", "%.3f", hsvValues[2]); + telemetry.addData("Alpha", "%.3f", colors.alpha); + + /* If this color sensor also has a distance sensor, display the measured distance. + * Note that the reported distance is only useful at very close range, and is impacted by + * ambient light and surface reflectivity. */ + if (colorSensor instanceof DistanceSensor) { + telemetry.addData("Distance (cm)", "%.3f", ((DistanceSensor) colorSensor).getDistance(DistanceUnit.CM)); + } + + telemetry.update(); + + // Change the Robot Controller's background color to match the color detected by the color sensor. + relativeLayout.post(new Runnable() { + public void run() { + relativeLayout.setBackgroundColor(Color.HSVToColor(hsvValues)); + } + }); + } + } +} From ad84c4847bb730ea57841d69787d0c88f4553426 Mon Sep 17 00:00:00 2001 From: anya-codes Date: Tue, 28 Oct 2025 21:38:36 -0700 Subject: [PATCH 061/198] Added code to check which robot is connected and pick an auto to run based on that so we can start coding auto for SlowBot --- .../ftc/team417/CompetitionAuto.java | 271 ++++++++++++------ 1 file changed, 178 insertions(+), 93 deletions(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java index 0ec7ff61dc70..c636d1d6e0ef 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java @@ -4,7 +4,6 @@ import com.acmerobotics.dashboard.telemetry.TelemetryPacket; import com.acmerobotics.roadrunner.Action; import com.acmerobotics.roadrunner.Pose2d; -import com.acmerobotics.roadrunner.SleepAction; import com.acmerobotics.roadrunner.Vector2d; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; @@ -27,7 +26,7 @@ enum Alliances { BLUE, } - enum Movements { + enum FastBotMovements { NEAR, FAR, FAR_MINIMAL, @@ -39,35 +38,53 @@ enum Movements { @Override public void runOpMode() { initHardware(); + // different options for start positions - for both SlowBot and FastBot Pose2d startPose = new Pose2d(0, 0, 0); - Pose2d redNearStartPose = new Pose2d(-60, 48, Math.toRadians(41)); - Pose2d redFarStartPose = new Pose2d(64, 16, Math.toRadians(0)); + Pose2d redFBNearStartPose = new Pose2d(-60, 48, Math.toRadians(41)); + Pose2d redFBFarStartPose = new Pose2d(64, 16, Math.toRadians(0)); - Pose2d blueNearStartPose = new Pose2d(-50, -50.5, Math.toRadians(139)); - Pose2d blueFarStartPose = new Pose2d(64, -16, Math.toRadians(180)); + Pose2d blueFBNearStartPose = new Pose2d(-50, -50.5, Math.toRadians(139)); + Pose2d blueFBFarStartPose = new Pose2d(64, -16, Math.toRadians(180)); MecanumDrive drive = new MecanumDrive(hardwareMap, telemetry, gamepad1, startPose); - TextMenu menu = new TextMenu(); - MenuInput menuInput = new MenuInput(MenuInput.InputType.CONTROLLER); - - menu.add(new MenuHeader("AUTO SETUP")) - .add() // empty line for spacing - .add("Pick an alliance:") - .add("alliance-picker-1", Alliances.class) // enum selector shortcut - .add() - .add("Pick a movement:") - .add("movement-picker-1", Movements.class) // enum selector shortcut - .add() - .add("Wait time:") - .add("wait-slider-1", new MenuSlider(minWaitTime, maxWaitTime)) - .add() - .add("finish-button-1", new MenuFinishedButton()); + TextMenu menu = new TextMenu(); + MenuInput menuInput = new MenuInput(MenuInput.InputType.CONTROLLER); + + // Text menu for FastBot + if (MecanumDrive.isFastBot) { + menu.add(new MenuHeader("AUTO SETUP")) + .add() // empty line for spacing + .add("Pick an alliance:") + .add("alliance-picker-1", Alliances.class) // enum selector shortcut + .add() + .add("Pick a movement:") + .add("movement-picker-1", FastBotMovements.class) // enum selector shortcut + .add() + .add("Wait time:") + .add("wait-slider-1", new MenuSlider(minWaitTime, maxWaitTime)) + .add() + .add("finish-button-1", new MenuFinishedButton()); + // Text menu for SlowBot + } else if (MecanumDrive.isSlowBot) { + menu.add(new MenuHeader("AUTO SETUP")) + .add() // empty line for spacing + .add("Pick an alliance:") + .add("alliance-picker-1", Alliances.class) // enum selector shortcut + .add() + .add("Pick a movement:") + .add("movement-picker-1", FastBotMovements.class) // enum selector shortcut + .add() + .add("Wait time:") + .add("wait-slider-1", new MenuSlider(minWaitTime, maxWaitTime)) + .add() + .add("finish-button-1", new MenuFinishedButton()); + } while (!menu.isCompleted()) { // get x, y (stick) and select (A) input from controller - // on wilyworks, this is x, y (wasd) and select (enter) on the keyboard + // on Wily Works, this is x, y (wasd) and select (enter) on the keyboard menuInput.update(gamepad1.left_stick_x, -gamepad1.left_stick_y, gamepad1.a); menu.updateWithInput(menuInput); // display the updated menu @@ -77,13 +94,9 @@ public void runOpMode() { telemetry.update(); } - // the first parameter is the type to return as - Alliances chosenAlliance = menu.getResult(Alliances.class, "alliance-picker-1"); - Movements chosenMovement = menu.getResult(Movements.class, "movement-picker-1"); - double waitTime = menu.getResult(Double.class, "wait-slider-1"); - // Red alliance auto paths - Action redNear = drive.actionBuilder(redNearStartPose) + // Red alliance FastBot auto paths + Action redNear = drive.actionBuilder(redFBNearStartPose) .setTangent(Math.toRadians(-49)) .stopAndAdd(new SpinUpAction()) .stopAndAdd(new LaunchAction()) @@ -93,7 +106,7 @@ public void runOpMode() { .splineToLinearHeading(new Pose2d(-32,54,Math.toRadians(0)), Math.toRadians(90)) .build(); - Action redFar = drive.actionBuilder(redFarStartPose) + Action redFar = drive.actionBuilder(redFBFarStartPose) .setTangent(Math.toRadians(135)) .splineToLinearHeading(new Pose2d(-57, 36, Math.toRadians(0)), Math.toRadians(90)) .stopAndAdd(new SpinUpAction()) @@ -104,13 +117,13 @@ public void runOpMode() { .splineToLinearHeading(new Pose2d(-56, 12, Math.toRadians(0)), Math.toRadians(-90)) .build(); - Action redFarMinimal = drive.actionBuilder(redFarStartPose) + Action redFarMinimal = drive.actionBuilder(redFBFarStartPose) .setTangent(Math.PI/2) .splineTo(new Vector2d(56, 35), Math.PI/2) .build(); // Blue alliance auto paths - Action blueNear = drive.actionBuilder(blueNearStartPose) + Action blueNear = drive.actionBuilder(blueFBNearStartPose) .setTangent(Math.toRadians(49)) .stopAndAdd(new SpinUpAction()) .stopAndAdd(new LaunchAction()) @@ -121,7 +134,7 @@ public void runOpMode() { .splineToLinearHeading(new Pose2d(-32,-54,Math.toRadians(180)), Math.toRadians(-90)) .build(); - Action blueFar = drive.actionBuilder(blueFarStartPose) + Action blueFar = drive.actionBuilder(blueFBFarStartPose) .setTangent(Math.toRadians(-135)) .splineToLinearHeading(new Pose2d(-57, -36, Math.toRadians(180)), Math.toRadians(-90)) .stopAndAdd(new SpinUpAction()) @@ -132,83 +145,155 @@ public void runOpMode() { .splineToLinearHeading(new Pose2d(-56, -12, Math.toRadians(180)), Math.toRadians(90)) .build(); - Action blueFarMinimal = drive.actionBuilder(blueFarStartPose) + Action blueFarMinimal = drive.actionBuilder(blueFBFarStartPose) .setTangent(Math.PI/2) .splineTo(new Vector2d(56, -35), Math.PI/2) .build(); + // the first parameter is the type to return as + if (MecanumDrive.isFastBot) { + Alliances chosenAlliance = menu.getResult(Alliances.class, "alliance-picker-1"); + FastBotMovements chosenMovement = menu.getResult(FastBotMovements.class, "movement-picker-1"); + double waitTime = menu.getResult(Double.class, "wait-slider-1"); + + Action trajectoryAction = null; + switch (chosenAlliance) { + case RED: + switch (chosenMovement) { + case NEAR: + drive.setPose(redFBNearStartPose); + trajectoryAction = redNear; + break; + case FAR: + drive.setPose(redFBFarStartPose); + trajectoryAction = redFar; + break; + case FAR_MINIMAL: + drive.setPose(redFBFarStartPose); + trajectoryAction = redFarMinimal; + break; + } + break; + + case BLUE: + switch (chosenMovement) { + case NEAR: + drive.setPose(blueFBNearStartPose); + trajectoryAction = blueNear; + break; + case FAR: + drive.setPose(blueFBFarStartPose); + trajectoryAction = blueFar; + break; + case FAR_MINIMAL: + drive.setPose(blueFBFarStartPose); + trajectoryAction = blueFarMinimal; + break; + } + break; + } - Action trajectoryAction = null; - switch (chosenAlliance) { - case RED: - switch (chosenMovement) { - case NEAR: - drive.setPose(redNearStartPose); - trajectoryAction = redNear; - break; - case FAR: - drive.setPose(redFarStartPose); - trajectoryAction = redFar; - break; - case FAR_MINIMAL: - drive.setPose(redFarStartPose); - trajectoryAction = redFarMinimal; - break; - } - break; - - case BLUE: - switch (chosenMovement) { - case NEAR: - drive.setPose(blueNearStartPose); - trajectoryAction = blueNear; - break; - case FAR: - drive.setPose(blueFarStartPose); - trajectoryAction = blueFar; - break; - case FAR_MINIMAL: - drive.setPose(blueFarStartPose); - trajectoryAction = blueFarMinimal; - break; - } - break; - } + // Get a preview of the trajectory's path: + Canvas previewCanvas = new Canvas(); + trajectoryAction.preview(previewCanvas); - // Get a preview of the trajectory's path: - Canvas previewCanvas = new Canvas(); - trajectoryAction.preview(previewCanvas); + // Show the preview on FTC Dashboard now. + TelemetryPacket packet = MecanumDrive.getTelemetryPacket(); + packet.fieldOverlay().getOperations().addAll(previewCanvas.getOperations()); + MecanumDrive.sendTelemetryPacket(packet); - // Show the preview on FTC Dashboard now. - TelemetryPacket packet = MecanumDrive.getTelemetryPacket(); - packet.fieldOverlay().getOperations().addAll(previewCanvas.getOperations()); - MecanumDrive.sendTelemetryPacket(packet); + // Wait for Start to be pressed on the Driver Hub! + waitForStart(); + boolean more = true; + while (opModeIsActive() && more) { + telemetry.addLine("Running Auto!"); - // Build the trajectory *before* the start button is pressed because Road Runner - // can take multiple seconds for this operation. We wouldn't want to have to wait - // as soon as the Start button is pressed! + // 'packet' is the object used to send data to FTC Dashboard: + packet = MecanumDrive.getTelemetryPacket(); + // Draw the preview and then run the next step of the trajectory on top: + packet.fieldOverlay().getOperations().addAll(previewCanvas.getOperations()); + more = trajectoryAction.run(packet); - // Wait for Start to be pressed on the Driver Hub! - waitForStart(); + // Only send the packet if there's more to do in order to keep the very last + // drawing up on the field once the robot is done: + if (more) + MecanumDrive.sendTelemetryPacket(packet); + telemetry.update(); + } - boolean more = true; - while (opModeIsActive() && more) { - telemetry.addLine("Running Auto!"); + } else if (MecanumDrive.isSlowBot) { + Alliances chosenAlliance = menu.getResult(Alliances.class, "alliance-picker-1"); + FastBotMovements chosenMovement = menu.getResult(FastBotMovements.class, "movement-picker-1"); + double waitTime = menu.getResult(Double.class, "wait-slider-1"); + + Action trajectoryAction = null; + switch (chosenAlliance) { + case RED: + switch (chosenMovement) { + case NEAR: + drive.setPose(redFBNearStartPose); + trajectoryAction = redNear; + break; + case FAR: + drive.setPose(redFBFarStartPose); + trajectoryAction = redFar; + break; + case FAR_MINIMAL: + drive.setPose(redFBFarStartPose); + trajectoryAction = redFarMinimal; + break; + } + break; + + case BLUE: + switch (chosenMovement) { + case NEAR: + drive.setPose(blueFBNearStartPose); + trajectoryAction = blueNear; + break; + case FAR: + drive.setPose(blueFBFarStartPose); + trajectoryAction = blueFar; + break; + case FAR_MINIMAL: + drive.setPose(blueFBFarStartPose); + trajectoryAction = blueFarMinimal; + break; + } + break; + } - // 'packet' is the object used to send data to FTC Dashboard: - packet = MecanumDrive.getTelemetryPacket(); + // Get a preview of the trajectory's path: + Canvas previewCanvas = new Canvas(); + trajectoryAction.preview(previewCanvas); - // Draw the preview and then run the next step of the trajectory on top: + // Show the preview on FTC Dashboard now. + TelemetryPacket packet = MecanumDrive.getTelemetryPacket(); packet.fieldOverlay().getOperations().addAll(previewCanvas.getOperations()); - more = trajectoryAction.run(packet); + MecanumDrive.sendTelemetryPacket(packet); - // Only send the packet if there's more to do in order to keep the very last - // drawing up on the field once the robot is done: - if (more) - MecanumDrive.sendTelemetryPacket(packet); - telemetry.update(); + // Wait for Start to be pressed on the Driver Hub! + waitForStart(); + + boolean more = true; + while (opModeIsActive() && more) { + telemetry.addLine("Running Auto!"); + + // 'packet' is the object used to send data to FTC Dashboard: + packet = MecanumDrive.getTelemetryPacket(); + + // Draw the preview and then run the next step of the trajectory on top: + packet.fieldOverlay().getOperations().addAll(previewCanvas.getOperations()); + more = trajectoryAction.run(packet); + + // Only send the packet if there's more to do in order to keep the very last + // drawing up on the field once the robot is done: + if (more) + MecanumDrive.sendTelemetryPacket(packet); + telemetry.update(); + } } } } From af48b331434b418fc5bd337b87bcef4e9058ff16 Mon Sep 17 00:00:00 2001 From: Team <417skid@swerverobotics.club> Date: Wed, 29 Oct 2025 20:52:26 -0700 Subject: [PATCH 062/198] all 4 auto paths work! ready for LM1 --- .../org/firstinspires/ftc/team417/BaseOpMode.java | 11 +++++++++++ .../firstinspires/ftc/team417/CompetitionAuto.java | 9 ++++++--- 2 files changed, 17 insertions(+), 3 deletions(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java b/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java index bd0f06bd2c9c..34d8754015bf 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java @@ -130,6 +130,17 @@ else if(MecanumDrive.isFastBot) { // Tell the driver that initialization is complete. telemetry.addData("Status", "Initialized"); } + class WaitAction extends RobotAction { + RobotAction actionToWaitOn; + WaitAction(RobotAction actionToWaitOn) { + this.actionToWaitOn = actionToWaitOn; + } + + @Override + public boolean run(double elapsedTime) { + return actionToWaitOn.isRunning(); + } + } class LaunchAction extends RobotAction { public boolean run(double ElapsedTime) { if (ElapsedTime < 0.25) { diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java index 5982e7057bb9..24517a810bfc 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java @@ -84,7 +84,9 @@ public void runOpMode() { // Red alliance auto paths Action redNear = drive.actionBuilder(redNearStartPose) + .setTangent(Math.toRadians(-49)) + .splineToLinearHeading(new Pose2d(-48,-48, Math.toRadians(41)),Math.toRadians(-49)) .stopAndAdd(new SpinUpAction()) .stopAndAdd(new LaunchAction()) .stopAndAdd(new LaunchAction()) @@ -95,7 +97,7 @@ public void runOpMode() { Action redFar = drive.actionBuilder(redFarStartPose) .setTangent(Math.toRadians(135)) - .splineToLinearHeading(new Pose2d(-56, 40, Math.toRadians(0)), Math.toRadians(90)) + .splineToLinearHeading(new Pose2d(-55, 43, Math.toRadians(41)), Math.toRadians(90)) .stopAndAdd(new SpinUpAction()) .stopAndAdd(new LaunchAction()) .stopAndAdd(new LaunchAction()) @@ -112,6 +114,7 @@ public void runOpMode() { // Blue alliance auto paths Action blueNear = drive.actionBuilder(blueNearStartPose) .setTangent(Math.toRadians(49)) + .splineToLinearHeading(new Pose2d(-48,-48, Math.toRadians(139)),Math.toRadians(49)) .stopAndAdd(new SpinUpAction()) .stopAndAdd(new LaunchAction()) .stopAndAdd(new LaunchAction()) @@ -123,7 +126,7 @@ public void runOpMode() { Action blueFar = drive.actionBuilder(blueFarStartPose) .setTangent(Math.toRadians(-135)) - .splineToLinearHeading(new Pose2d(-56, -40, Math.toRadians(180)), Math.toRadians(-90)) + .splineToLinearHeading(new Pose2d(-49, -49, Math.toRadians(139)), Math.toRadians(-90)) .stopAndAdd(new SpinUpAction()) .stopAndAdd(new LaunchAction()) .stopAndAdd(new LaunchAction()) @@ -192,7 +195,7 @@ public void runOpMode() { // Wait for Start to be pressed on the Driver Hub! waitForStart(); - + sleep((long)waitTime*1000); boolean more = true; while (opModeIsActive() && more) { telemetry.addLine("Running Auto!"); From ea2df646cc57123b3145855c8ad958c0e1d0a2d0 Mon Sep 17 00:00:00 2001 From: Emmett Date: Thu, 30 Oct 2025 20:21:06 -0700 Subject: [PATCH 063/198] fixed auto and added defaults for text menu --- .../ftc/team417/CompetitionAuto.java | 24 +++++++++++++------ 1 file changed, 17 insertions(+), 7 deletions(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java index 24517a810bfc..566a2fdf2492 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java @@ -86,7 +86,7 @@ public void runOpMode() { Action redNear = drive.actionBuilder(redNearStartPose) .setTangent(Math.toRadians(-49)) - .splineToLinearHeading(new Pose2d(-48,-48, Math.toRadians(41)),Math.toRadians(-49)) + //.splineToLinearHeading(new Pose2d(-54,46, Math.toRadians(41)),Math.toRadians(-49)) .stopAndAdd(new SpinUpAction()) .stopAndAdd(new LaunchAction()) .stopAndAdd(new LaunchAction()) @@ -96,8 +96,18 @@ public void runOpMode() { .build(); Action redFar = drive.actionBuilder(redFarStartPose) - .setTangent(Math.toRadians(135)) - .splineToLinearHeading(new Pose2d(-55, 43, Math.toRadians(41)), Math.toRadians(90)) + /*.setTangent(Math.toRadians(135)) + .splineToLinearHeading(new Pose2d(-60, 48, Math.toRadians(41)), Math.toRadians(90)) + .stopAndAdd(new SpinUpAction()) + .stopAndAdd(new LaunchAction()) + .stopAndAdd(new LaunchAction()) + .stopAndAdd(new LaunchAction()) + .setTangent(Math.toRadians(-90)) + .splineToLinearHeading(new Pose2d(-57, 12, Math.toRadians(0)), Math.toRadians(-90)) + .build();*/ + + .setTangent(Math.toRadians(45)) + .splineToLinearHeading(new Pose2d(-49, 49, Math.toRadians(41)), Math.toRadians(90)) .stopAndAdd(new SpinUpAction()) .stopAndAdd(new LaunchAction()) .stopAndAdd(new LaunchAction()) @@ -136,8 +146,8 @@ public void runOpMode() { .build(); Action blueFarMinimal = drive.actionBuilder(blueFarStartPose) - .setTangent(Math.PI/2) - .splineTo(new Vector2d(56, -35), Math.PI/2) + .setTangent(3*Math.PI/2) + .splineTo(new Vector2d(56, -35), 3*Math.PI/2) .build(); @@ -160,7 +170,7 @@ public void runOpMode() { } break; - case BLUE: + default: switch (chosenMovement) { case NEAR: drive.setPose(blueNearStartPose); @@ -170,7 +180,7 @@ public void runOpMode() { drive.setPose(blueFarStartPose); trajectoryAction = blueFar; break; - case FAR_MINIMAL: + default: drive.setPose(blueFarStartPose); trajectoryAction = blueFarMinimal; break; From 13505e07d6447a487e9328c59cd3f3e19cd08f27 Mon Sep 17 00:00:00 2001 From: Emmett Date: Thu, 30 Oct 2025 20:40:45 -0700 Subject: [PATCH 064/198] fixed red far, auto now works fully --- .../java/org/firstinspires/ftc/team417/CompetitionAuto.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java index 566a2fdf2492..54cf1fca5681 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java @@ -106,7 +106,7 @@ public void runOpMode() { .splineToLinearHeading(new Pose2d(-57, 12, Math.toRadians(0)), Math.toRadians(-90)) .build();*/ - .setTangent(Math.toRadians(45)) + .setTangent(Math.toRadians(180)) .splineToLinearHeading(new Pose2d(-49, 49, Math.toRadians(41)), Math.toRadians(90)) .stopAndAdd(new SpinUpAction()) .stopAndAdd(new LaunchAction()) From 1ef5140081c4937110256d5e0db3f4e3844f6eee Mon Sep 17 00:00:00 2001 From: anya-codes Date: Thu, 30 Oct 2025 21:45:14 -0700 Subject: [PATCH 065/198] Added default for switch chosen movement which we hadn't added earlier --- .../java/org/firstinspires/ftc/team417/CompetitionAuto.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java index 54cf1fca5681..373591d6df97 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java @@ -163,7 +163,7 @@ public void runOpMode() { drive.setPose(redFarStartPose); trajectoryAction = redFar; break; - case FAR_MINIMAL: + default: drive.setPose(redFarStartPose); trajectoryAction = redFarMinimal; break; From 2eca98efbb3a3964a6f82c480bbf4e2840d7212e Mon Sep 17 00:00:00 2001 From: Emmett Date: Sun, 2 Nov 2025 11:49:10 -0800 Subject: [PATCH 066/198] changed red far values at LM1 --- .../java/org/firstinspires/ftc/team417/CompetitionAuto.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java index 373591d6df97..5c962650ceca 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java @@ -107,7 +107,7 @@ public void runOpMode() { .build();*/ .setTangent(Math.toRadians(180)) - .splineToLinearHeading(new Pose2d(-49, 49, Math.toRadians(41)), Math.toRadians(90)) + .splineToLinearHeading(new Pose2d(-50, 50, Math.toRadians(41)), Math.toRadians(90)) .stopAndAdd(new SpinUpAction()) .stopAndAdd(new LaunchAction()) .stopAndAdd(new LaunchAction()) From 960290d2c140fb1ab74477c1395b4000fca290df Mon Sep 17 00:00:00 2001 From: Emmett Date: Sun, 2 Nov 2025 11:58:51 -0800 Subject: [PATCH 067/198] modified elapsedtime values for red near auto --- .../main/java/org/firstinspires/ftc/team417/BaseOpMode.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java b/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java index 34d8754015bf..334b3d7d11b6 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java @@ -149,7 +149,7 @@ public boolean run(double ElapsedTime) { return true; } - else if(ElapsedTime < 1) { + else if(ElapsedTime < 2) { leftFeeder.setPower(STOP_SPEED); rightFeeder.setPower(STOP_SPEED); return true; @@ -164,7 +164,7 @@ else if(ElapsedTime < 1) { class SpinUpAction extends RobotAction { public boolean run(double ElapsedTime) { launcher.setVelocity(LAUNCHER_LOW_TARGET_VELOCITY); - if(ElapsedTime < 1) { + if(ElapsedTime < 2) { return true; } else { From 6030fbc909dace6aec9e7244b0f9d9938204cbe3 Mon Sep 17 00:00:00 2001 From: Emmett Date: Sun, 2 Nov 2025 21:25:35 -0800 Subject: [PATCH 068/198] auto telemetry added --- .../org/firstinspires/ftc/team417/CompetitionAuto.java | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java index 5c962650ceca..183f82debbfd 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java @@ -34,7 +34,7 @@ enum Movements { } double minWaitTime = 0.0; - double maxWaitTime = 15.0; + double maxWaitTime = 20.0; @Override public void runOpMode() { @@ -210,6 +210,14 @@ public void runOpMode() { while (opModeIsActive() && more) { telemetry.addLine("Running Auto!"); + telemetry.addData("State", launchState); + // telemetry.addData("Motors", "left (%.2f), right (%.2f)", leftPower, rightPower); + telemetry.addData("motorSpeed", launcher.getVelocity()); + telemetry.addData("FEED_TIME_SECONDS", FEED_TIME_SECONDS); + telemetry.addData("leftFeeder", leftFeeder.getPower()); + telemetry.addData("rightFeeder", rightFeeder.getPower()); + + // 'packet' is the object used to send data to FTC Dashboard: packet = MecanumDrive.getTelemetryPacket(); From 3584b715fc4e8fe337533a74cb706910f292c026 Mon Sep 17 00:00:00 2001 From: Team <417skid@swerverobotics.club> Date: Thu, 6 Nov 2025 20:32:33 -0800 Subject: [PATCH 069/198] auto mirroring --- .../ftc/team417/CompetitionAuto.java | 184 ++++++++++++------ 1 file changed, 122 insertions(+), 62 deletions(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java index c636d1d6e0ef..8ae77cb26d63 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java @@ -4,6 +4,7 @@ import com.acmerobotics.dashboard.telemetry.TelemetryPacket; import com.acmerobotics.roadrunner.Action; import com.acmerobotics.roadrunner.Pose2d; +import com.acmerobotics.roadrunner.TrajectoryActionBuilder; import com.acmerobotics.roadrunner.Vector2d; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; @@ -19,7 +20,7 @@ * This class exposes the competition version of Autonomous. As a general rule, add code to the * BaseOpMode class rather than here so that it can be shared between both TeleOp and Autonomous. */ -@Autonomous(name="Auto", group="Competition", preselectTeleOp="CompetitionTeleOp") +@Autonomous(name = "Auto", group = "Competition", preselectTeleOp = "CompetitionTeleOp") public class CompetitionAuto extends BaseOpMode { enum Alliances { RED, @@ -46,11 +47,14 @@ public void runOpMode() { Pose2d blueFBNearStartPose = new Pose2d(-50, -50.5, Math.toRadians(139)); Pose2d blueFBFarStartPose = new Pose2d(64, -16, Math.toRadians(180)); + Pose2d SBNearStartPose = new Pose2d(-60, 48, Math.toRadians(139)); + Pose2d SBFarStartPose = new Pose2d(64, 16, Math.toRadians(180)); + MecanumDrive drive = new MecanumDrive(hardwareMap, telemetry, gamepad1, startPose); - TextMenu menu = new TextMenu(); - MenuInput menuInput = new MenuInput(MenuInput.InputType.CONTROLLER); + TextMenu menu = new TextMenu(); + MenuInput menuInput = new MenuInput(MenuInput.InputType.CONTROLLER); // Text menu for FastBot if (MecanumDrive.isFastBot) { @@ -66,7 +70,9 @@ public void runOpMode() { .add("wait-slider-1", new MenuSlider(minWaitTime, maxWaitTime)) .add() .add("finish-button-1", new MenuFinishedButton()); - // Text menu for SlowBot + + + // Text menu for SlowBot } else if (MecanumDrive.isSlowBot) { menu.add(new MenuHeader("AUTO SETUP")) .add() // empty line for spacing @@ -94,19 +100,26 @@ public void runOpMode() { telemetry.update(); } + Alliances chosenAlliance = menu.getResult(Alliances.class, "alliance-picker-1"); + FastBotMovements chosenMovement = menu.getResult(FastBotMovements.class, "movement-picker-1"); + double waitTime = menu.getResult(Double.class, "wait-slider-1"); + + PathFactory redPathFactory = new PathFactory(drive, false); + + PathFactory bluePathFactory = new PathFactory(drive, true); // Red alliance FastBot auto paths - Action redNear = drive.actionBuilder(redFBNearStartPose) + Action redNearFastBot = drive.actionBuilder(redFBNearStartPose) .setTangent(Math.toRadians(-49)) .stopAndAdd(new SpinUpAction()) .stopAndAdd(new LaunchAction()) .stopAndAdd(new LaunchAction()) .stopAndAdd(new LaunchAction()) - .splineToLinearHeading(new Pose2d(-32,54,Math.toRadians(0)), Math.toRadians(90)) + .splineToLinearHeading(new Pose2d(-32, 54, Math.toRadians(0)), Math.toRadians(90)) .build(); - Action redFar = drive.actionBuilder(redFBFarStartPose) + Action redFarFastBot = drive.actionBuilder(redFBFarStartPose) .setTangent(Math.toRadians(135)) .splineToLinearHeading(new Pose2d(-57, 36, Math.toRadians(0)), Math.toRadians(90)) .stopAndAdd(new SpinUpAction()) @@ -117,13 +130,13 @@ public void runOpMode() { .splineToLinearHeading(new Pose2d(-56, 12, Math.toRadians(0)), Math.toRadians(-90)) .build(); - Action redFarMinimal = drive.actionBuilder(redFBFarStartPose) - .setTangent(Math.PI/2) - .splineTo(new Vector2d(56, 35), Math.PI/2) + Action redFarMinimalFastBot = drive.actionBuilder(redFBFarStartPose) + .setTangent(Math.PI / 2) + .splineTo(new Vector2d(56, 35), Math.PI / 2) .build(); // Blue alliance auto paths - Action blueNear = drive.actionBuilder(blueFBNearStartPose) + Action blueNearFastBot = drive.actionBuilder(blueFBNearStartPose) .setTangent(Math.toRadians(49)) .stopAndAdd(new SpinUpAction()) .stopAndAdd(new LaunchAction()) @@ -131,10 +144,10 @@ public void runOpMode() { .stopAndAdd(new LaunchAction()) // .splineTo(new Vector2d(-44, -44), Math.toRadians(49)) // .setTangent(Math.toRadians(139)) - .splineToLinearHeading(new Pose2d(-32,-54,Math.toRadians(180)), Math.toRadians(-90)) + .splineToLinearHeading(new Pose2d(-32, -54, Math.toRadians(180)), Math.toRadians(-90)) .build(); - Action blueFar = drive.actionBuilder(blueFBFarStartPose) + Action blueFarFastBot = drive.actionBuilder(blueFBFarStartPose) .setTangent(Math.toRadians(-135)) .splineToLinearHeading(new Pose2d(-57, -36, Math.toRadians(180)), Math.toRadians(-90)) .stopAndAdd(new SpinUpAction()) @@ -145,32 +158,46 @@ public void runOpMode() { .splineToLinearHeading(new Pose2d(-56, -12, Math.toRadians(180)), Math.toRadians(90)) .build(); - Action blueFarMinimal = drive.actionBuilder(blueFBFarStartPose) - .setTangent(Math.PI/2) - .splineTo(new Vector2d(56, -35), Math.PI/2) + Action blueFarMinimalFastBot = drive.actionBuilder(blueFBFarStartPose) + .setTangent(Math.PI / 2) + .splineTo(new Vector2d(56, -35), Math.PI / 2) + .build(); + + PathFactory pathFactory; + + switch (chosenAlliance) { + case RED: + pathFactory = redPathFactory; + break; + case BLUE: + pathFactory = bluePathFactory; + break; + default: + throw new IllegalArgumentException("Alliance must be red or blue"); + } + + Action farMinimalSlowBot = pathFactory.actionBuilder(SBFarStartPose) + .setTangent(Math.toRadians(90)) + .splineToLinearHeading(new Pose2d(48,32,Math.toRadians(180)), Math.toRadians(180)) .build(); // the first parameter is the type to return as if (MecanumDrive.isFastBot) { - Alliances chosenAlliance = menu.getResult(Alliances.class, "alliance-picker-1"); - FastBotMovements chosenMovement = menu.getResult(FastBotMovements.class, "movement-picker-1"); - double waitTime = menu.getResult(Double.class, "wait-slider-1"); - Action trajectoryAction = null; switch (chosenAlliance) { case RED: switch (chosenMovement) { case NEAR: drive.setPose(redFBNearStartPose); - trajectoryAction = redNear; + trajectoryAction = redNearFastBot; break; case FAR: drive.setPose(redFBFarStartPose); - trajectoryAction = redFar; + trajectoryAction = redFarFastBot; break; case FAR_MINIMAL: drive.setPose(redFBFarStartPose); - trajectoryAction = redFarMinimal; + trajectoryAction = redFarMinimalFastBot; break; } break; @@ -179,15 +206,15 @@ public void runOpMode() { switch (chosenMovement) { case NEAR: drive.setPose(blueFBNearStartPose); - trajectoryAction = blueNear; + trajectoryAction = blueNearFastBot; break; case FAR: drive.setPose(blueFBFarStartPose); - trajectoryAction = blueFar; + trajectoryAction = blueFarFastBot; break; case FAR_MINIMAL: drive.setPose(blueFBFarStartPose); - trajectoryAction = blueFarMinimal; + trajectoryAction = blueFarMinimalFastBot; break; } break; @@ -224,47 +251,25 @@ public void runOpMode() { } } else if (MecanumDrive.isSlowBot) { - Alliances chosenAlliance = menu.getResult(Alliances.class, "alliance-picker-1"); - FastBotMovements chosenMovement = menu.getResult(FastBotMovements.class, "movement-picker-1"); - double waitTime = menu.getResult(Double.class, "wait-slider-1"); - Action trajectoryAction = null; - switch (chosenAlliance) { - case RED: - switch (chosenMovement) { - case NEAR: - drive.setPose(redFBNearStartPose); - trajectoryAction = redNear; - break; - case FAR: - drive.setPose(redFBFarStartPose); - trajectoryAction = redFar; - break; - case FAR_MINIMAL: - drive.setPose(redFBFarStartPose); - trajectoryAction = redFarMinimal; - break; - } - break; - case BLUE: - switch (chosenMovement) { - case NEAR: - drive.setPose(blueFBNearStartPose); - trajectoryAction = blueNear; - break; - case FAR: - drive.setPose(blueFBFarStartPose); - trajectoryAction = blueFar; - break; - case FAR_MINIMAL: - drive.setPose(blueFBFarStartPose); - trajectoryAction = blueFarMinimal; - break; - } + switch (chosenMovement) { + case NEAR: + + drive.setPose(SBNearStartPose); + trajectoryAction = redNearFastBot; + break; + case FAR: + drive.setPose(SBFarStartPose); + trajectoryAction = redFarFastBot; + break; + case FAR_MINIMAL: + drive.setPose(SBFarStartPose); + trajectoryAction = farMinimalSlowBot; break; } + // Get a preview of the trajectory's path: Canvas previewCanvas = new Canvas(); trajectoryAction.preview(previewCanvas); @@ -297,3 +302,58 @@ public void runOpMode() { } } } + +class PathFactory { + MecanumDrive drive; + TrajectoryActionBuilder builder; + boolean mirror; + + public PathFactory(MecanumDrive drive, boolean mirror) { + this.drive = drive; + this.mirror = mirror; + } + + Pose2d mirrorPose(Pose2d pose) { + + + return new Pose2d(pose.position.x, -pose.position.y, -pose.heading.log()); + } + + public PathFactory actionBuilder(Pose2d pose) { + if (mirror) { + builder = drive.actionBuilder(mirrorPose(pose)); + } else { + builder = drive.actionBuilder(pose); + } + return this; + } + + public PathFactory setTangent(double tangent) { + if (mirror) { + builder.setTangent(-tangent); + } else { + builder.setTangent(tangent); + } + return this; + } + + public PathFactory splineToLinearHeading(Pose2d pose, double tangent) { + + if (mirror) { + builder.splineToLinearHeading(mirrorPose(pose), -tangent); + } else { + builder.splineToLinearHeading(pose, tangent); + } + return this; + } + + + public Action build() { + return builder.build(); + } + + public PathFactory stopAndAdd(Action a) { + builder.stopAndAdd(a); + return this; + } +} From 3bd51475deebbc40e78c15b2bfd608d4be400408 Mon Sep 17 00:00:00 2001 From: Emmett Date: Thu, 6 Nov 2025 20:35:28 -0800 Subject: [PATCH 070/198] pushing to fix comflictions removed code preparation under the presumption that the drum would be controlled by a motor --- .../firstinspires/ftc/team417/BaseOpMode.java | 41 ++++++++++++++++--- .../ftc/team417/CompetitionTeleOp.java | 8 +--- 2 files changed, 38 insertions(+), 11 deletions(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java b/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java index dafa4c06d4b0..bf9157dd1226 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java @@ -7,23 +7,29 @@ import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.LED; import com.qualcomm.robotcore.hardware.PIDFCoefficients; +import com.qualcomm.robotcore.hardware.Servo; import com.qualcomm.robotcore.util.ElapsedTime; import org.firstinspires.ftc.team417.roadrunner.MecanumDrive; import org.firstinspires.ftc.team417.roadrunner.RobotAction; +import java.util.ArrayList; +import java.util.Arrays; +import java.util.List; + /** * This class contains all of the base logic that is shared between all of the TeleOp and * Autonomous logic. All TeleOp and Autonomous classes should derive from this class. */ abstract public class BaseOpMode extends LinearOpMode { - + //fastbot hardware variables public DcMotorEx launcher = null; - public DcMotorEx drum = null; public CRServo leftFeeder = null; public CRServo rightFeeder = null; + + //fastbot constants public static final double STOP_SPEED = 0.0; //We send this power to the servos when we want them to stop. public static double FEED_TIME_SECONDS = 0; //The feeder servos run this long when a shot is requested. @@ -53,7 +59,6 @@ abstract public class BaseOpMode extends LinearOpMode { public static double LAUNCHER_REV_TARGET_VELOCITY = -250; - public LED redLed; public LED greenLed; @@ -71,6 +76,29 @@ public enum LaunchState { public LaunchState launchState; + /*---------------------------------------------------------------*/ + //slowbot hardware + + public Servo drum = null; + + //slowbot constants + int moveOnePosition = 0; //change meeeeeeeee (should be 120 degrees ish) + + final double intakeslot0 = -1; + final double intakeslot1 = -0.2; + final double intakeslot2 = 0.6; + final double launcherslot0 = 0.2; + final double launcherslot1 = 1; + final double launcherslot2 = -0.6; + + int [] intakePositions = {0, 1, 2}; + + ArrayList blah; + final List INTAKE_POSITIONS + = new ArrayList<>(Arrays.asList(-1.0, -0.2, 0.6)); + int [] launcherPositions = {0, 1, 2}; + double intakeSpeed = gamepad2.left_stick_x; + double Multiply = 0; //need to change, placeholder public void initHardware() { // Reversed direction of launcher for DevBot because motor is on the other side (compared to FastBot) @@ -102,7 +130,6 @@ else if(MecanumDrive.isFastBot) { // initialize flywheel motor and feeder servos launcher = hardwareMap.get(DcMotorEx.class, "motLauncher"); - drum = hardwareMap.get(DcMotorEx.class, "drum"); leftFeeder = hardwareMap.get(CRServo.class, "servoBLaunchFeed"); rightFeeder = hardwareMap.get(CRServo.class, "servoFLaunchFeed"); @@ -128,7 +155,7 @@ else if(MecanumDrive.isFastBot) { leftFeeder.setDirection(DcMotorSimple.Direction.REVERSE); } else if (MecanumDrive.isSlowBot) { //add slowbot initialization code here - launcher = hardwareMap.get(DcMotorEx.class, "motLauncher"); + drum = hardwareMap.get(Servo.class, "drum"); } @@ -261,5 +288,9 @@ public void launch(boolean shotRequested) { break; } } + + public void drumLogic () { + + } } diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java index f849bb806c90..0587cc69ec4d 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java @@ -22,11 +22,7 @@ public class CompetitionTeleOp extends BaseOpMode { double FASTDRIVE_SPEED = 1.0; double SLOWDRIVE_SPEED = 0.5; - int moveOnePosition = 0; //change meeeeeeeee (should be 120 degrees ish) - double drumVelocity = 0; //change meeeeeeeee (needs to be fast) - double intakeSpeed = gamepad2.left_stick_x; - double Multiply = 0; //need to change, placeholder ElapsedTime rightBumperTimer = new ElapsedTime(); @@ -154,8 +150,8 @@ public void runOpMode() { } else if (gamepad2.x) { // sort speed - drum.setTargetPosition(moveOnePosition); - drum.setVelocity(drumVelocity); +// drum.setTargetPosition(moveOnePosition); +// drum.setVelocity(drumVelocity); } From e443a15eb59c42d40cedfbd350d33389c956eb63 Mon Sep 17 00:00:00 2001 From: Emmett Date: Thu, 6 Nov 2025 20:48:01 -0800 Subject: [PATCH 071/198] started ComplexDrumGlob class --- .../ftc/team417/ComplexDrumGlob.java | 37 +++++++++++++++++++ 1 file changed, 37 insertions(+) create mode 100644 team417/src/main/java/org/firstinspires/ftc/team417/ComplexDrumGlob.java diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/ComplexDrumGlob.java b/team417/src/main/java/org/firstinspires/ftc/team417/ComplexDrumGlob.java new file mode 100644 index 000000000000..d0c9f7fa3f9d --- /dev/null +++ b/team417/src/main/java/org/firstinspires/ftc/team417/ComplexDrumGlob.java @@ -0,0 +1,37 @@ +package org.firstinspires.ftc.team417; + +import com.qualcomm.robotcore.hardware.HardwareMap; +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.team417.roadrunner.MecanumDrive; + +enum LaunchColor { + PURPLE, + GREEN, + EITHER +} +class DrumGlob { + DrumGlob(){} + void intake (double intakeValue){} + + void launch (LaunchColor launchColor) {} + + static DrumGlob create (HardwareMap hardwareMap, Telemetry telemetry){ + if (MecanumDrive.isSlowBot) { + return new ComplexDrumGlob(hardwareMap, telemetry); + + } else { + return new DrumGlob(); + } + } +} + +public class ComplexDrumGlob extends DrumGlob{ + + ComplexDrumGlob(HardwareMap hardwareMap, Telemetry telemetry){} + @Override + void intake (double intakeValue){} + + void launch (LaunchColor launchColor) {} + +} + From 1568ed3c754ca903bc30105207e3cfc324cfe513 Mon Sep 17 00:00:00 2001 From: Team <417skid@swerverobotics.club> Date: Thu, 6 Nov 2025 20:51:56 -0800 Subject: [PATCH 072/198] auto mirroring --- .../firstinspires/ftc/team417/BaseOpMode.java | 2 ++ .../ftc/team417/CompetitionAuto.java | 28 +++++++------------ .../ftc/team417/utils/WilyConfig.java | 2 +- 3 files changed, 13 insertions(+), 19 deletions(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java b/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java index bf9157dd1226..2b482073d621 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java @@ -59,6 +59,7 @@ abstract public class BaseOpMode extends LinearOpMode { public static double LAUNCHER_REV_TARGET_VELOCITY = -250; + public LED redLed; public LED greenLed; @@ -156,6 +157,7 @@ else if(MecanumDrive.isFastBot) { } else if (MecanumDrive.isSlowBot) { //add slowbot initialization code here drum = hardwareMap.get(Servo.class, "drum"); + //launcher = hardwareMap.get(DcMotorEx.class, "motLauncher"); } diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java index 8ae77cb26d63..685c242d42ca 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java @@ -104,10 +104,6 @@ public void runOpMode() { FastBotMovements chosenMovement = menu.getResult(FastBotMovements.class, "movement-picker-1"); double waitTime = menu.getResult(Double.class, "wait-slider-1"); - PathFactory redPathFactory = new PathFactory(drive, false); - - PathFactory bluePathFactory = new PathFactory(drive, true); - // Red alliance FastBot auto paths Action redNearFastBot = drive.actionBuilder(redFBNearStartPose) .setTangent(Math.toRadians(-49)) @@ -167,10 +163,10 @@ public void runOpMode() { switch (chosenAlliance) { case RED: - pathFactory = redPathFactory; + pathFactory = new PathFactory(drive, false); break; case BLUE: - pathFactory = bluePathFactory; + pathFactory = new PathFactory(drive, true); break; default: throw new IllegalArgumentException("Alliance must be red or blue"); @@ -314,8 +310,6 @@ public PathFactory(MecanumDrive drive, boolean mirror) { } Pose2d mirrorPose(Pose2d pose) { - - return new Pose2d(pose.position.x, -pose.position.y, -pose.heading.log()); } @@ -330,30 +324,28 @@ public PathFactory actionBuilder(Pose2d pose) { public PathFactory setTangent(double tangent) { if (mirror) { - builder.setTangent(-tangent); + builder = builder.setTangent(-tangent); } else { - builder.setTangent(tangent); + builder = builder.setTangent(tangent); } return this; } public PathFactory splineToLinearHeading(Pose2d pose, double tangent) { - if (mirror) { - builder.splineToLinearHeading(mirrorPose(pose), -tangent); + builder = builder.splineToLinearHeading(mirrorPose(pose), -tangent); } else { - builder.splineToLinearHeading(pose, tangent); + builder = builder.splineToLinearHeading(pose, tangent); } return this; } + public PathFactory stopAndAdd(Action a) { + builder = builder.stopAndAdd(a); + return this; + } public Action build() { return builder.build(); } - - public PathFactory stopAndAdd(Action a) { - builder.stopAndAdd(a); - return this; - } } diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/utils/WilyConfig.java b/team417/src/main/java/org/firstinspires/ftc/team417/utils/WilyConfig.java index 5bed63bebff9..c04585aa12b9 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/utils/WilyConfig.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/utils/WilyConfig.java @@ -12,7 +12,7 @@ public class WilyConfig extends WilyWorks.Config { WilyConfig() { // Impersonate the DevBot when running the simulator: - deviceName = "DevBot"; + deviceName = "417-b-RC"; // Use these dimensions for the robot: robotWidth = 18.0; From 84768d315b5a4788735ebe6a4c6fb05bd89a92c0 Mon Sep 17 00:00:00 2001 From: Emmett Date: Sun, 9 Nov 2025 13:31:22 -0800 Subject: [PATCH 073/198] comments --- .../ftc/team417/ComplexDrumGlob.java | 19 ++++++++++--------- 1 file changed, 10 insertions(+), 9 deletions(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/ComplexDrumGlob.java b/team417/src/main/java/org/firstinspires/ftc/team417/ComplexDrumGlob.java index d0c9f7fa3f9d..f9187fcb2a29 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/ComplexDrumGlob.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/ComplexDrumGlob.java @@ -4,34 +4,35 @@ import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.team417.roadrunner.MecanumDrive; -enum LaunchColor { +enum LaunchColor { //an enum for different color cases for launching PURPLE, GREEN, EITHER } -class DrumGlob { +class DrumGlob { //a placeholder class encompassing all code that ISN'T for slowbot. DrumGlob(){} void intake (double intakeValue){} void launch (LaunchColor launchColor) {} + static DrumGlob create (HardwareMap hardwareMap, Telemetry telemetry){ - if (MecanumDrive.isSlowBot) { - return new ComplexDrumGlob(hardwareMap, telemetry); + if (MecanumDrive.isSlowBot) { //if the robot is slowbot, use ComplexDrumGlob. + return new ComplexDrumGlob(hardwareMap, telemetry); //Go to ComplexDrumGlob class - } else { - return new DrumGlob(); + } else { //otherwise, use DrumGlob + return new DrumGlob(); //Go to DrumGlob class } } } -public class ComplexDrumGlob extends DrumGlob{ +public class ComplexDrumGlob extends DrumGlob{ //a class encompassing all code that IS for slowbot ComplexDrumGlob(HardwareMap hardwareMap, Telemetry telemetry){} @Override - void intake (double intakeValue){} + void intake (double intakeValue){} //a class that controls the intake based on intakeValue - void launch (LaunchColor launchColor) {} + void launch (LaunchColor launchColor) {} //a class that controls the launcher and transfer } From ee4c58eafdcf43fc65e3f396af23ef377c6e59dd Mon Sep 17 00:00:00 2001 From: Hankang Zhou <136044263+nobody-particular@users.noreply.github.com> Date: Sun, 9 Nov 2025 16:15:33 -0800 Subject: [PATCH 074/198] Made AprilTags detect artifact pattern in Autonomous! Tested on DevBot! --- .../ftc/team417/CompetitionAuto.java | 45 +++++-- .../team417/apriltags/AprilTagDetector.java | 123 ++++++++++++++++++ .../team417/{ => apriltags}/AprilTagTest.java | 3 +- .../ftc/team417/apriltags/Pattern.java | 8 ++ 4 files changed, 167 insertions(+), 12 deletions(-) create mode 100644 team417/src/main/java/org/firstinspires/ftc/team417/apriltags/AprilTagDetector.java rename team417/src/main/java/org/firstinspires/ftc/team417/{ => apriltags}/AprilTagTest.java (98%) create mode 100644 team417/src/main/java/org/firstinspires/ftc/team417/apriltags/Pattern.java diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java index 183f82debbfd..a81f667ee0f9 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java @@ -4,11 +4,11 @@ import com.acmerobotics.dashboard.telemetry.TelemetryPacket; import com.acmerobotics.roadrunner.Action; import com.acmerobotics.roadrunner.Pose2d; -import com.acmerobotics.roadrunner.SleepAction; import com.acmerobotics.roadrunner.Vector2d; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; - +import org.firstinspires.ftc.team417.apriltags.AprilTagDetector; +import org.firstinspires.ftc.team417.apriltags.Pattern; import org.firstinspires.ftc.team417.javatextmenu.MenuFinishedButton; import org.firstinspires.ftc.team417.javatextmenu.MenuHeader; import org.firstinspires.ftc.team417.javatextmenu.MenuInput; @@ -22,19 +22,19 @@ */ @Autonomous(name="Auto", group="Competition", preselectTeleOp="CompetitionTeleOp") public class CompetitionAuto extends BaseOpMode { - enum Alliances { + public enum Alliance { RED, BLUE, } - enum Movements { + enum Movement { NEAR, FAR, FAR_MINIMAL, } double minWaitTime = 0.0; - double maxWaitTime = 20.0; + double maxWaitTime = 30.0; @Override public void runOpMode() { @@ -49,16 +49,21 @@ public void runOpMode() { MecanumDrive drive = new MecanumDrive(hardwareMap, telemetry, gamepad1, startPose); + // Test to make sure the camera is there, and then immediately close the detector object + try (AprilTagDetector detector = new AprilTagDetector()) { + detector.initAprilTag(hardwareMap); + } + TextMenu menu = new TextMenu(); MenuInput menuInput = new MenuInput(MenuInput.InputType.CONTROLLER); menu.add(new MenuHeader("AUTO SETUP")) .add() // empty line for spacing .add("Pick an alliance:") - .add("alliance-picker-1", Alliances.class) // enum selector shortcut + .add("alliance-picker-1", Alliance.class) // enum selector shortcut .add() .add("Pick a movement:") - .add("movement-picker-1", Movements.class) // enum selector shortcut + .add("movement-picker-1", Movement.class) // enum selector shortcut .add() .add("Wait time:") .add("wait-slider-1", new MenuSlider(minWaitTime, maxWaitTime)) @@ -78,8 +83,8 @@ public void runOpMode() { } // the first parameter is the type to return as - Alliances chosenAlliance = menu.getResult(Alliances.class, "alliance-picker-1"); - Movements chosenMovement = menu.getResult(Movements.class, "movement-picker-1"); + Alliance chosenAlliance = menu.getResult(Alliance.class, "alliance-picker-1"); + Movement chosenMovement = menu.getResult(Movement.class, "movement-picker-1"); double waitTime = menu.getResult(Double.class, "wait-slider-1"); // Red alliance auto paths @@ -202,9 +207,29 @@ public void runOpMode() { // can take multiple seconds for this operation. We wouldn't want to have to wait // as soon as the Start button is pressed! + // Assume unknown pattern unless detected otherwise. + Pattern pattern = Pattern.UNKNOWN; + // Detect the pattern with the AprilTags from the camera! // Wait for Start to be pressed on the Driver Hub! - waitForStart(); + try (AprilTagDetector detector = new AprilTagDetector()) { + detector.initAprilTag(hardwareMap); + + while (opModeInInit()) { + Pattern last = detector.detectPattern(chosenAlliance); + if (last != Pattern.UNKNOWN) { + pattern = last; + } + + telemetry.addData("Chosen alliance: ", chosenAlliance); + telemetry.addData("Chosen movement: ", chosenMovement); + telemetry.addData("Chosen wait time: ", waitTime); + telemetry.addData("Last valid pattern: ", pattern); + + telemetry.update(); + } + } + sleep((long)waitTime*1000); boolean more = true; while (opModeIsActive() && more) { diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/apriltags/AprilTagDetector.java b/team417/src/main/java/org/firstinspires/ftc/team417/apriltags/AprilTagDetector.java new file mode 100644 index 000000000000..7aacde7e97ac --- /dev/null +++ b/team417/src/main/java/org/firstinspires/ftc/team417/apriltags/AprilTagDetector.java @@ -0,0 +1,123 @@ +package org.firstinspires.ftc.team417.apriltags; + +import com.qualcomm.robotcore.hardware.HardwareMap; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.robotcore.external.hardware.camera.BuiltinCameraDirection; +import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; +import org.firstinspires.ftc.team417.CompetitionAuto; +import org.firstinspires.ftc.vision.VisionPortal; +import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; +import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor; + +import java.io.Closeable; +import java.util.Comparator; +import java.util.List; + +public class AprilTagDetector implements Closeable { + private static final boolean USE_WEBCAM = true; // true for webcam, false for phone camera + + /** + * The variable to store our instance of the AprilTag processor. + */ + private AprilTagProcessor aprilTag; + + /** + * The variable to store our instance of the vision portal. + */ + private VisionPortal visionPortal; + + /** + * Initialize the AprilTag processor. + */ + public void initAprilTag(HardwareMap hardwareMap) { + // Create the AprilTag processor the easy way. + aprilTag = AprilTagProcessor.easyCreateWithDefaults(); + + // Create the vision portal the easy way. + if (USE_WEBCAM) { + visionPortal = VisionPortal.easyCreateWithDefaults( + hardwareMap.get(WebcamName.class, "camera"), aprilTag); + } else { + visionPortal = VisionPortal.easyCreateWithDefaults( + BuiltinCameraDirection.BACK, aprilTag); + } + + } // end method initAprilTag() + + /** + * Add telemetry about AprilTag detections. + */ + public void telemetryAprilTag(Telemetry telemetry) { + List currentDetections = aprilTag.getDetections(); + telemetry.addData("# AprilTags Detected", currentDetections.size()); + + // Step through the list of detections and display info for each one. + for (AprilTagDetection detection : currentDetections) { + if (detection.metadata != null) { + telemetry.addLine(String.format("\n==== (ID %d) %s", detection.id, detection.metadata.name)); + telemetry.addLine(String.format("XYZ %6.1f %6.1f %6.1f (inch)", detection.ftcPose.x, detection.ftcPose.y, detection.ftcPose.z)); + telemetry.addLine(String.format("PRY %6.1f %6.1f %6.1f (deg)", detection.ftcPose.pitch, detection.ftcPose.roll, detection.ftcPose.yaw)); + telemetry.addLine(String.format("RBE %6.1f %6.1f %6.1f (inch, deg, deg)", detection.ftcPose.range, detection.ftcPose.bearing, detection.ftcPose.elevation)); + } else { + telemetry.addLine(String.format("\n==== (ID %d) Unknown", detection.id)); + telemetry.addLine(String.format("Center %6.0f %6.0f (pixels)", detection.center.x, detection.center.y)); + } + } // end for() loop + + // Add "key" information to telemetry + telemetry.addLine("\nkey:\nXYZ = X (Right), Y (Forward), Z (Up) dist."); + telemetry.addLine("PRY = Pitch, Roll & Yaw (XYZ Rotation)"); + telemetry.addLine("RBE = Range, Bearing & Elevation"); + + } // end method telemetryAprilTag() + + public AprilTagDetection findDetection(CompetitionAuto.Alliance alliance) { + List currentDetections = aprilTag.getDetections(); + + // Remove all AprilTags that don't have ID 21, 22, or 23 + currentDetections.removeIf(detection -> + detection.id != 21 && detection.id != 22 && detection.id != 23 + ); + + switch (alliance) { + case RED: + // Return the leftmost detection relative to the robot + // If there are no detections, return null + return currentDetections.stream() + .min(Comparator.comparingDouble(aprilTagDetection -> aprilTagDetection.ftcPose.x)).orElse(null); + case BLUE: + // Return the rightmost detection relative to the robot + // If there are no detections, return null + return currentDetections.stream() + .max(Comparator.comparingDouble(aprilTagDetection -> aprilTagDetection.ftcPose.x)).orElse(null); + } + + throw new IllegalArgumentException("Alliance must be red or blue"); + } + + public Pattern detectPattern(CompetitionAuto.Alliance alliance) { + AprilTagDetection detection = findDetection(alliance); + + // Handle the case where no valid AprilTag was found + if (detection == null) { + return Pattern.UNKNOWN; + } + + switch (detection.id) { + case 21: + return Pattern.GPP; + case 22: + return Pattern.PGP; + case 23: + return Pattern.PPG; + } + + throw new IllegalArgumentException("ID must be 21, 22, or 23"); + } + + @Override + public void close() { + visionPortal.close(); + } +} diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/AprilTagTest.java b/team417/src/main/java/org/firstinspires/ftc/team417/apriltags/AprilTagTest.java similarity index 98% rename from team417/src/main/java/org/firstinspires/ftc/team417/AprilTagTest.java rename to team417/src/main/java/org/firstinspires/ftc/team417/apriltags/AprilTagTest.java index 8b508e4c37cf..0e412aa75b9c 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/AprilTagTest.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/apriltags/AprilTagTest.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.team417; +package org.firstinspires.ftc.team417.apriltags; /* Copyright (c) 2023 FIRST. All rights reserved. * @@ -29,7 +29,6 @@ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import org.firstinspires.ftc.robotcore.external.hardware.camera.BuiltinCameraDirection; diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/apriltags/Pattern.java b/team417/src/main/java/org/firstinspires/ftc/team417/apriltags/Pattern.java new file mode 100644 index 000000000000..403b9a22e782 --- /dev/null +++ b/team417/src/main/java/org/firstinspires/ftc/team417/apriltags/Pattern.java @@ -0,0 +1,8 @@ +package org.firstinspires.ftc.team417.apriltags; + +public enum Pattern { + GPP, + PGP, + PPG, + UNKNOWN; +} From 9570e292784d945e4ba351b23fe44e1cc72d04a0 Mon Sep 17 00:00:00 2001 From: Hankang Zhou <136044263+nobody-particular@users.noreply.github.com> Date: Sun, 9 Nov 2025 16:15:33 -0800 Subject: [PATCH 075/198] Made AprilTags detect artifact pattern in Autonomous! Tested on DevBot! --- .../ftc/team417/CompetitionAuto.java | 45 +++++-- .../team417/apriltags/AprilTagDetector.java | 123 ++++++++++++++++++ .../team417/{ => apriltags}/AprilTagTest.java | 3 +- .../ftc/team417/apriltags/Pattern.java | 8 ++ 4 files changed, 168 insertions(+), 11 deletions(-) create mode 100644 team417/src/main/java/org/firstinspires/ftc/team417/apriltags/AprilTagDetector.java rename team417/src/main/java/org/firstinspires/ftc/team417/{ => apriltags}/AprilTagTest.java (98%) create mode 100644 team417/src/main/java/org/firstinspires/ftc/team417/apriltags/Pattern.java diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java index 183f82debbfd..072cc1f8ca1b 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java @@ -4,11 +4,11 @@ import com.acmerobotics.dashboard.telemetry.TelemetryPacket; import com.acmerobotics.roadrunner.Action; import com.acmerobotics.roadrunner.Pose2d; -import com.acmerobotics.roadrunner.SleepAction; import com.acmerobotics.roadrunner.Vector2d; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; - +import org.firstinspires.ftc.team417.apriltags.AprilTagDetector; +import org.firstinspires.ftc.team417.apriltags.Pattern; import org.firstinspires.ftc.team417.javatextmenu.MenuFinishedButton; import org.firstinspires.ftc.team417.javatextmenu.MenuHeader; import org.firstinspires.ftc.team417.javatextmenu.MenuInput; @@ -22,19 +22,19 @@ */ @Autonomous(name="Auto", group="Competition", preselectTeleOp="CompetitionTeleOp") public class CompetitionAuto extends BaseOpMode { - enum Alliances { + public enum Alliance { RED, BLUE, } - enum Movements { + enum Movement { NEAR, FAR, FAR_MINIMAL, } double minWaitTime = 0.0; - double maxWaitTime = 20.0; + double maxWaitTime = 30.0; @Override public void runOpMode() { @@ -49,16 +49,21 @@ public void runOpMode() { MecanumDrive drive = new MecanumDrive(hardwareMap, telemetry, gamepad1, startPose); + // Test to make sure the camera is there, and then immediately close the detector object + try (AprilTagDetector detector = new AprilTagDetector()) { + detector.initAprilTag(hardwareMap); + } + TextMenu menu = new TextMenu(); MenuInput menuInput = new MenuInput(MenuInput.InputType.CONTROLLER); menu.add(new MenuHeader("AUTO SETUP")) .add() // empty line for spacing .add("Pick an alliance:") - .add("alliance-picker-1", Alliances.class) // enum selector shortcut + .add("alliance-picker-1", Alliance.class) // enum selector shortcut .add() .add("Pick a movement:") - .add("movement-picker-1", Movements.class) // enum selector shortcut + .add("movement-picker-1", Movement.class) // enum selector shortcut .add() .add("Wait time:") .add("wait-slider-1", new MenuSlider(minWaitTime, maxWaitTime)) @@ -78,8 +83,8 @@ public void runOpMode() { } // the first parameter is the type to return as - Alliances chosenAlliance = menu.getResult(Alliances.class, "alliance-picker-1"); - Movements chosenMovement = menu.getResult(Movements.class, "movement-picker-1"); + Alliance chosenAlliance = menu.getResult(Alliance.class, "alliance-picker-1"); + Movement chosenMovement = menu.getResult(Movement.class, "movement-picker-1"); double waitTime = menu.getResult(Double.class, "wait-slider-1"); // Red alliance auto paths @@ -202,9 +207,31 @@ public void runOpMode() { // can take multiple seconds for this operation. We wouldn't want to have to wait // as soon as the Start button is pressed! + // Assume unknown pattern unless detected otherwise. + Pattern pattern = Pattern.UNKNOWN; + // Detect the pattern with the AprilTags from the camera! // Wait for Start to be pressed on the Driver Hub! + try (AprilTagDetector detector = new AprilTagDetector()) { + detector.initAprilTag(hardwareMap); + + while (!isStarted() && !isStopRequested()) { + Pattern last = detector.detectPattern(chosenAlliance); + if (last != Pattern.UNKNOWN) { + pattern = last; + } + + telemetry.addData("Chosen alliance: ", chosenAlliance); + telemetry.addData("Chosen movement: ", chosenMovement); + telemetry.addData("Chosen wait time: ", waitTime); + telemetry.addData("Last valid pattern: ", pattern); + + telemetry.update(); + } + } + waitForStart(); + sleep((long)waitTime*1000); boolean more = true; while (opModeIsActive() && more) { diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/apriltags/AprilTagDetector.java b/team417/src/main/java/org/firstinspires/ftc/team417/apriltags/AprilTagDetector.java new file mode 100644 index 000000000000..7aacde7e97ac --- /dev/null +++ b/team417/src/main/java/org/firstinspires/ftc/team417/apriltags/AprilTagDetector.java @@ -0,0 +1,123 @@ +package org.firstinspires.ftc.team417.apriltags; + +import com.qualcomm.robotcore.hardware.HardwareMap; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.robotcore.external.hardware.camera.BuiltinCameraDirection; +import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; +import org.firstinspires.ftc.team417.CompetitionAuto; +import org.firstinspires.ftc.vision.VisionPortal; +import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; +import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor; + +import java.io.Closeable; +import java.util.Comparator; +import java.util.List; + +public class AprilTagDetector implements Closeable { + private static final boolean USE_WEBCAM = true; // true for webcam, false for phone camera + + /** + * The variable to store our instance of the AprilTag processor. + */ + private AprilTagProcessor aprilTag; + + /** + * The variable to store our instance of the vision portal. + */ + private VisionPortal visionPortal; + + /** + * Initialize the AprilTag processor. + */ + public void initAprilTag(HardwareMap hardwareMap) { + // Create the AprilTag processor the easy way. + aprilTag = AprilTagProcessor.easyCreateWithDefaults(); + + // Create the vision portal the easy way. + if (USE_WEBCAM) { + visionPortal = VisionPortal.easyCreateWithDefaults( + hardwareMap.get(WebcamName.class, "camera"), aprilTag); + } else { + visionPortal = VisionPortal.easyCreateWithDefaults( + BuiltinCameraDirection.BACK, aprilTag); + } + + } // end method initAprilTag() + + /** + * Add telemetry about AprilTag detections. + */ + public void telemetryAprilTag(Telemetry telemetry) { + List currentDetections = aprilTag.getDetections(); + telemetry.addData("# AprilTags Detected", currentDetections.size()); + + // Step through the list of detections and display info for each one. + for (AprilTagDetection detection : currentDetections) { + if (detection.metadata != null) { + telemetry.addLine(String.format("\n==== (ID %d) %s", detection.id, detection.metadata.name)); + telemetry.addLine(String.format("XYZ %6.1f %6.1f %6.1f (inch)", detection.ftcPose.x, detection.ftcPose.y, detection.ftcPose.z)); + telemetry.addLine(String.format("PRY %6.1f %6.1f %6.1f (deg)", detection.ftcPose.pitch, detection.ftcPose.roll, detection.ftcPose.yaw)); + telemetry.addLine(String.format("RBE %6.1f %6.1f %6.1f (inch, deg, deg)", detection.ftcPose.range, detection.ftcPose.bearing, detection.ftcPose.elevation)); + } else { + telemetry.addLine(String.format("\n==== (ID %d) Unknown", detection.id)); + telemetry.addLine(String.format("Center %6.0f %6.0f (pixels)", detection.center.x, detection.center.y)); + } + } // end for() loop + + // Add "key" information to telemetry + telemetry.addLine("\nkey:\nXYZ = X (Right), Y (Forward), Z (Up) dist."); + telemetry.addLine("PRY = Pitch, Roll & Yaw (XYZ Rotation)"); + telemetry.addLine("RBE = Range, Bearing & Elevation"); + + } // end method telemetryAprilTag() + + public AprilTagDetection findDetection(CompetitionAuto.Alliance alliance) { + List currentDetections = aprilTag.getDetections(); + + // Remove all AprilTags that don't have ID 21, 22, or 23 + currentDetections.removeIf(detection -> + detection.id != 21 && detection.id != 22 && detection.id != 23 + ); + + switch (alliance) { + case RED: + // Return the leftmost detection relative to the robot + // If there are no detections, return null + return currentDetections.stream() + .min(Comparator.comparingDouble(aprilTagDetection -> aprilTagDetection.ftcPose.x)).orElse(null); + case BLUE: + // Return the rightmost detection relative to the robot + // If there are no detections, return null + return currentDetections.stream() + .max(Comparator.comparingDouble(aprilTagDetection -> aprilTagDetection.ftcPose.x)).orElse(null); + } + + throw new IllegalArgumentException("Alliance must be red or blue"); + } + + public Pattern detectPattern(CompetitionAuto.Alliance alliance) { + AprilTagDetection detection = findDetection(alliance); + + // Handle the case where no valid AprilTag was found + if (detection == null) { + return Pattern.UNKNOWN; + } + + switch (detection.id) { + case 21: + return Pattern.GPP; + case 22: + return Pattern.PGP; + case 23: + return Pattern.PPG; + } + + throw new IllegalArgumentException("ID must be 21, 22, or 23"); + } + + @Override + public void close() { + visionPortal.close(); + } +} diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/AprilTagTest.java b/team417/src/main/java/org/firstinspires/ftc/team417/apriltags/AprilTagTest.java similarity index 98% rename from team417/src/main/java/org/firstinspires/ftc/team417/AprilTagTest.java rename to team417/src/main/java/org/firstinspires/ftc/team417/apriltags/AprilTagTest.java index 8b508e4c37cf..0e412aa75b9c 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/AprilTagTest.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/apriltags/AprilTagTest.java @@ -1,4 +1,4 @@ -package org.firstinspires.ftc.team417; +package org.firstinspires.ftc.team417.apriltags; /* Copyright (c) 2023 FIRST. All rights reserved. * @@ -29,7 +29,6 @@ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import org.firstinspires.ftc.robotcore.external.hardware.camera.BuiltinCameraDirection; diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/apriltags/Pattern.java b/team417/src/main/java/org/firstinspires/ftc/team417/apriltags/Pattern.java new file mode 100644 index 000000000000..403b9a22e782 --- /dev/null +++ b/team417/src/main/java/org/firstinspires/ftc/team417/apriltags/Pattern.java @@ -0,0 +1,8 @@ +package org.firstinspires.ftc.team417.apriltags; + +public enum Pattern { + GPP, + PGP, + PPG, + UNKNOWN; +} From 06811396a2f1c0b817c8213263f15927ec7154f2 Mon Sep 17 00:00:00 2001 From: Emmett Date: Sun, 9 Nov 2025 16:51:03 -0800 Subject: [PATCH 076/198] made mode progress on the newly renamed ComplexMechGlob. yay. --- .../firstinspires/ftc/team417/BaseOpMode.java | 1 - .../ftc/team417/ComplexDrumGlob.java | 38 ------- .../ftc/team417/ComplexMechGlob.java | 106 ++++++++++++++++++ 3 files changed, 106 insertions(+), 39 deletions(-) delete mode 100644 team417/src/main/java/org/firstinspires/ftc/team417/ComplexDrumGlob.java create mode 100644 team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java b/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java index bf9157dd1226..409ca66c5f84 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java @@ -82,7 +82,6 @@ public enum LaunchState { public Servo drum = null; //slowbot constants - int moveOnePosition = 0; //change meeeeeeeee (should be 120 degrees ish) final double intakeslot0 = -1; final double intakeslot1 = -0.2; diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/ComplexDrumGlob.java b/team417/src/main/java/org/firstinspires/ftc/team417/ComplexDrumGlob.java deleted file mode 100644 index f9187fcb2a29..000000000000 --- a/team417/src/main/java/org/firstinspires/ftc/team417/ComplexDrumGlob.java +++ /dev/null @@ -1,38 +0,0 @@ -package org.firstinspires.ftc.team417; - -import com.qualcomm.robotcore.hardware.HardwareMap; -import org.firstinspires.ftc.robotcore.external.Telemetry; -import org.firstinspires.ftc.team417.roadrunner.MecanumDrive; - -enum LaunchColor { //an enum for different color cases for launching - PURPLE, - GREEN, - EITHER -} -class DrumGlob { //a placeholder class encompassing all code that ISN'T for slowbot. - DrumGlob(){} - void intake (double intakeValue){} - - void launch (LaunchColor launchColor) {} - - - static DrumGlob create (HardwareMap hardwareMap, Telemetry telemetry){ - if (MecanumDrive.isSlowBot) { //if the robot is slowbot, use ComplexDrumGlob. - return new ComplexDrumGlob(hardwareMap, telemetry); //Go to ComplexDrumGlob class - - } else { //otherwise, use DrumGlob - return new DrumGlob(); //Go to DrumGlob class - } - } -} - -public class ComplexDrumGlob extends DrumGlob{ //a class encompassing all code that IS for slowbot - - ComplexDrumGlob(HardwareMap hardwareMap, Telemetry telemetry){} - @Override - void intake (double intakeValue){} //a class that controls the intake based on intakeValue - - void launch (LaunchColor launchColor) {} //a class that controls the launcher and transfer - -} - diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java b/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java new file mode 100644 index 000000000000..5ff8ab4cd568 --- /dev/null +++ b/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java @@ -0,0 +1,106 @@ +package org.firstinspires.ftc.team417; + +import static org.firstinspires.ftc.robotcore.external.BlocksOpModeCompanion.gamepad2; + +import com.qualcomm.robotcore.hardware.HardwareMap; +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.team417.roadrunner.MecanumDrive; + +import java.util.ArrayList; +import java.util.Collections; + +enum LaunchColor { //an enum for different color cases for launching + PURPLE, + GREEN, + EITHER +} +enum PixelColor { + PURPLE, + GREEN, + NONE +} + +class MechGlob { //a placeholder class encompassing all code that ISN'T for slowbot. + MechGlob(){} + + //call DrumGlob.create to create a Glob object for slowbot or fastbot + static MechGlob create (HardwareMap hardwareMap, Telemetry telemetry){ + if (MecanumDrive.isSlowBot) { //if the robot is slowbot, use ComplexMechGlob. + return new ComplexMechGlob(hardwareMap, telemetry); //Go to ComplexMechGlob class + + } else { //otherwise, use MechGlob + return new MechGlob(); //Go to MechGlob class + } + } + //a method that controls the intake based on gamepad2.leftstickx + //if gamepad2.left_stick_x is > 0, intakeSpeed = 1. If negative, intakeSpeed = -1. If 0, 0. + void intake (double intakeValue){} + + //a method that determines what color to launch. Options are purple, green, or either. + void launch (LaunchColor launchColor) {} + + void update () {} + + +} + +// +public class ComplexMechGlob extends MechGlob { //a class encompassing all code that IS for slowbot + double userIntakeSpeed; + double drumServoPosition; //the last position the servo went to + ArrayList drumQueue = new ArrayList<> (); + + ArrayList slotOccupiedBy = new ArrayList<> (Collections.nCopies(3, PixelColor.NONE)); + enum WaitState { + DRUM_MOVE, //waiting for the drum to reach desired position + INTAKE, //waiting for the intake to finish + TRANSFER, //waiting for the transfer to finish + SPIN_UP, //waiting for the flywheel(s) to spin up + IDLE, //waiting for input when the drum is full + + } + + + class DrumRequest { + double position; + WaitState nextState; + } + ComplexMechGlob (HardwareMap hardwareMap, Telemetry telemetry) {} + + int findNearestSlot (LaunchColor launchColor) { + double minDistance = Double.MAX_VALUE; + int minSlot = -1; + + for (int i = 0; i <= 2; i++){ + double distance; + if (launchColor == LaunchColor.PURPLE && slotOccupiedBy.get (i) == PixelColor.PURPLE){ + return i; + } else if (launchColor == LaunchColor.GREEN && slotOccupiedBy.get (i) == PixelColor.GREEN){ + return i; + } else if (launchColor == LaunchColor.EITHER &&slotOccupiedBy.get (i) != PixelColor.NONE){ + return i; + } + } + return -1; + } + @Override + void intake (double intakeSpeed) { + + userIntakeSpeed = intakeSpeed; + } + + @Override + //a class that controls the launcher and transfer + void launch (LaunchColor launchColor) { + + + +// drumQueue.add (new DrumRequest (position, WaitState.TRANSFER)); + } + + @Override + void update () { + + } +} + From 5b7cf4551cada6a55a0716e832f637f542996c53 Mon Sep 17 00:00:00 2001 From: bharv Date: Sun, 9 Nov 2025 16:56:19 -0800 Subject: [PATCH 077/198] THe color detection and test telop, not currently fully functional --- .../ftc/team417/CoolColorDetector.java | 83 +++++++++++++++++++ .../ftc/team417/TestColorDetect.java | 16 ++++ 2 files changed, 99 insertions(+) create mode 100644 team417/src/main/java/org/firstinspires/ftc/team417/CoolColorDetector.java create mode 100644 team417/src/main/java/org/firstinspires/ftc/team417/TestColorDetect.java diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CoolColorDetector.java b/team417/src/main/java/org/firstinspires/ftc/team417/CoolColorDetector.java new file mode 100644 index 000000000000..4000330cc546 --- /dev/null +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CoolColorDetector.java @@ -0,0 +1,83 @@ +package org.firstinspires.ftc.team417; + +import com.qualcomm.robotcore.hardware.HardwareMap; + +import com.qualcomm.robotcore.hardware.ColorSensor; +import com.qualcomm.robotcore.hardware.NormalizedColorSensor; + +import android.graphics.Color; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +//import com.qualcomm.robotcore.hardware.ColorSensor; +//import com.qualcomm.robotcore.hardware.NormalizedRGBA; +//import android.graphics.Color; +//import com.qualcomm.robotcore.hardware.SwitchableLight; + +enum PixelColor { + GREEN, + PURPLE, + NONE +} + public class CoolColorDetector { + private ColorSensor sensor1; + private ColorSensor sensor2; + private float gain = 4f; // adjust for brightness + private float[] hsv = new float[3]; + public CoolColorDetector(HardwareMap map) { + sensor1 = map.get(ColorSensor.class, "cs1"); + //sensor2 = map.get(ColorSensor.class, "sensor2"); + } + // --- Convert a sensor to ONE PixelColor --- + private PixelColor detectSingle(ColorSensor sensor1) { + // Get raw values + ((NormalizedColorSensor)sensor1).setGain(gain); + //Just tried something new with the setGain + float r = sensor1.red(); + float g = sensor1.green(); + float b = sensor1.blue(); + Color.RGBToHSV((int)r, (int)g, (int)b, hsv); + float hue = hsv[0]; + // GREEN Range: 145 - 165 + if (hue >= 145 && hue <= 185) { + return PixelColor.GREEN; + } + // PURPLE Range: 215 - 245 + else if (hue >= 215 && hue <= 245) { + return PixelColor.PURPLE; + } + else { + return PixelColor.NONE; + } + + } + + // --- Use logic comparing both sensors --- + /*PixelColor detectPixelPosition() { + PixelColor s1 = detectSingle(sensor1); + PixelColor s2 = detectSingle(sensor2); + // Rule 1: If both detect something different → return sensor2 + if (s1 == s2) { + return s1; + } + // Rule 2: If sensor1 detects color and sensor2 = NONE → sensor1 wins + if ((s1 == PixelColor.GREEN || s1 == PixelColor.PURPLE) && s2 == PixelColor.NONE) { + return s1; + } + // Rule 3: If sensor2 detects color and sensor1 = NONE → sensor2 wins + if ((s2 == PixelColor.GREEN || s2 == PixelColor.PURPLE) && s1 == PixelColor.NONE) { + return s2; + } + else { + // Otherwise no decision → return none + return PixelColor.NONE; + } + }*/ + public void showTelemetry(Telemetry telemetry) { + telemetry.addData("Sensor 1", detectSingle(sensor1)); + //telemetry.addData("Sensor 1"); + //telemetry.addData("Sensor 2", detectSingle(sensor2)); + //telemetry.addData("Chosen Position", detectPixelPosition()); + telemetry.update(); + } +} + diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/TestColorDetect.java b/team417/src/main/java/org/firstinspires/ftc/team417/TestColorDetect.java new file mode 100644 index 000000000000..93ed0e7c547a --- /dev/null +++ b/team417/src/main/java/org/firstinspires/ftc/team417/TestColorDetect.java @@ -0,0 +1,16 @@ +package org.firstinspires.ftc.team417; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +@TeleOp +public class TestColorDetect extends LinearOpMode { + CoolColorDetector detector; + @Override + public void runOpMode() { + detector = new CoolColorDetector(hardwareMap); + waitForStart(); + while (opModeIsActive()) { + detector.showTelemetry(telemetry); + } + } +} + From 7e44d51860ee8c2d57319a9966d6ab44b2fc9bb8 Mon Sep 17 00:00:00 2001 From: Team <417skid@swerverobotics.club> Date: Tue, 11 Nov 2025 17:29:13 -0800 Subject: [PATCH 078/198] auto intake cycles in text menu --- .../firstinspires/ftc/team417/BaseOpMode.java | 2 +- .../ftc/team417/CompetitionAuto.java | 116 +++++++++++++++++- .../ftc/team417/CompetitionTeleOp.java | 2 +- 3 files changed, 116 insertions(+), 4 deletions(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java b/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java index 2b482073d621..a29643c92c9b 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java @@ -98,7 +98,7 @@ public enum LaunchState { final List INTAKE_POSITIONS = new ArrayList<>(Arrays.asList(-1.0, -0.2, 0.6)); int [] launcherPositions = {0, 1, 2}; - double intakeSpeed = gamepad2.left_stick_x; + //double intakeSpeed = gamepad2.left_stick_x; double Multiply = 0; //need to change, placeholder public void initHardware() { diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java index 685c242d42ca..649eff10ec8c 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java @@ -16,6 +16,8 @@ import org.firstinspires.ftc.team417.javatextmenu.TextMenu; import org.firstinspires.ftc.team417.roadrunner.MecanumDrive; +import java.nio.file.Path; + /** * This class exposes the competition version of Autonomous. As a general rule, add code to the * BaseOpMode class rather than here so that it can be shared between both TeleOp and Autonomous. @@ -35,6 +37,8 @@ enum FastBotMovements { double minWaitTime = 0.0; double maxWaitTime = 15.0; + double minIntakes = 0.0; + double maxIntakes = 3.0; @Override public void runOpMode() { @@ -82,6 +86,9 @@ public void runOpMode() { .add("Pick a movement:") .add("movement-picker-1", FastBotMovements.class) // enum selector shortcut .add() + .add("Intake Cycles:") + .add("intake-slider", new MenuSlider(minIntakes, maxIntakes)) + .add() .add("Wait time:") .add("wait-slider-1", new MenuSlider(minWaitTime, maxWaitTime)) .add() @@ -103,9 +110,11 @@ public void runOpMode() { Alliances chosenAlliance = menu.getResult(Alliances.class, "alliance-picker-1"); FastBotMovements chosenMovement = menu.getResult(FastBotMovements.class, "movement-picker-1"); double waitTime = menu.getResult(Double.class, "wait-slider-1"); + double intakeCycles = menu.getResult(Double.class, "intake-slider"); // Red alliance FastBot auto paths Action redNearFastBot = drive.actionBuilder(redFBNearStartPose) + .setTangent(Math.toRadians(-49)) .stopAndAdd(new SpinUpAction()) .stopAndAdd(new LaunchAction()) @@ -177,6 +186,85 @@ public void runOpMode() { .splineToLinearHeading(new Pose2d(48,32,Math.toRadians(180)), Math.toRadians(180)) .build(); + + + PathFactory farSlowBotIntake1 = pathFactory.actionBuilder(SBFarStartPose) + .setTangent(Math.toRadians(180)) + // 3 launch actions + //then after disp intake action + .splineToSplineHeading(new Pose2d(36,32, Math.toRadians(90)), Math.toRadians(90)) + .setTangent(Math.toRadians(90)) + .splineToConstantHeading(new Vector2d(36,60), Math.toRadians(90)) + .setTangent(Math.toRadians(-90)) + .splineToSplineHeading(new Pose2d(54,12, Math.toRadians(157.5)), Math.toRadians(-90)); + if (intakeCycles > 1) { + + + // 3 launch actions + //after disp intake action + farSlowBotIntake1 = farSlowBotIntake1.setTangent(Math.toRadians(180)) + .splineToSplineHeading(new Pose2d(12, 32, Math.toRadians(90)), Math.toRadians(90)) + .setTangent(Math.toRadians(90)) + .splineToConstantHeading(new Vector2d(12, 60), Math.toRadians(90)) + .setTangent(Math.toRadians(-90)) + .splineToSplineHeading(new Pose2d(54, 12, Math.toRadians(157.5)), Math.toRadians(-90)); + // 3 launch actions + //after disp intake action + if (intakeCycles > 2) { + farSlowBotIntake1 = farSlowBotIntake1.setTangent(Math.toRadians(180)) + .splineToSplineHeading(new Pose2d(-12,32, Math.toRadians(90)), Math.toRadians(90)) + .setTangent(Math.toRadians(90)) + .splineToConstantHeading(new Vector2d(-12,55), Math.toRadians(90)) + .setTangent(Math.toRadians(-90)) + .splineToSplineHeading(new Pose2d(54,12, Math.toRadians(157.5)), Math.toRadians(-90)); + + } + } + farSlowBotIntake1 = farSlowBotIntake1.setTangent(Math.toRadians(90)) + .splineToLinearHeading(new Pose2d(48,32,Math.toRadians(180)), Math.toRadians(180)); + Action farSlowBot = farSlowBotIntake1.build(); + + + + + PathFactory nearSlowBotPath = pathFactory.actionBuilder(SBNearStartPose) + .setTangent(Math.toRadians(-51)) + .splineToConstantHeading(new Vector2d(-36,36), Math.toRadians(-51)) + //3 launches + //after disp intake + .setTangent(Math.toRadians(0)) + .splineToSplineHeading(new Pose2d(-12,32, Math.toRadians(90)), Math.toRadians(90)) + .setTangent(Math.toRadians(90)) + .splineToConstantHeading(new Vector2d(-12,55), Math.toRadians(90)) + .setTangent(Math.toRadians(-90)) + .splineToSplineHeading(new Pose2d(-36,36, Math.toRadians(139)), Math.toRadians(180)); + if (intakeCycles > 1) { + nearSlowBotPath = nearSlowBotPath.setTangent(Math.toRadians(0)) + + + //3 launches + //after disp intake + + .splineToSplineHeading(new Pose2d(12, 32, Math.toRadians(90)), Math.toRadians(90)) + .setTangent(Math.toRadians(90)) + .splineToConstantHeading(new Vector2d(12, 60), Math.toRadians(90)) + .setTangent(Math.toRadians(-90)) + .splineToSplineHeading(new Pose2d(-36, 36, Math.toRadians(139)), Math.toRadians(180)); + //3 launches + //after disp intake + if (intakeCycles > 2) { + nearSlowBotPath = nearSlowBotPath.setTangent(Math.toRadians(0)) + .splineToSplineHeading(new Pose2d(36, 32, Math.toRadians(90)), Math.toRadians(90)) + .setTangent(Math.toRadians(90)) + .splineToConstantHeading(new Vector2d(36, 60), Math.toRadians(90)) + .setTangent(Math.toRadians(-90)) + .splineToSplineHeading(new Pose2d(-36, 36, Math.toRadians(139)), Math.toRadians(180)); + + } + } + nearSlowBotPath = nearSlowBotPath.setTangent(Math.toRadians(-90)) + .splineToSplineHeading(new Pose2d(-48, 12, Math.toRadians(180)), Math.toRadians(180)); + Action nearSlowBot = nearSlowBotPath.build(); // the first parameter is the type to return as if (MecanumDrive.isFastBot) { Action trajectoryAction = null; @@ -253,11 +341,11 @@ public void runOpMode() { case NEAR: drive.setPose(SBNearStartPose); - trajectoryAction = redNearFastBot; + trajectoryAction = nearSlowBot; break; case FAR: drive.setPose(SBFarStartPose); - trajectoryAction = redFarFastBot; + trajectoryAction = farSlowBot; break; case FAR_MINIMAL: drive.setPose(SBFarStartPose); @@ -312,6 +400,9 @@ public PathFactory(MecanumDrive drive, boolean mirror) { Pose2d mirrorPose(Pose2d pose) { return new Pose2d(pose.position.x, -pose.position.y, -pose.heading.log()); } + Vector2d mirrorVector(Vector2d vector) { + return new Vector2d(vector.x,-vector.y); + } public PathFactory actionBuilder(Pose2d pose) { if (mirror) { @@ -339,6 +430,24 @@ public PathFactory splineToLinearHeading(Pose2d pose, double tangent) { } return this; } + public PathFactory splineToSplineHeading(Pose2d pose, double tangent) { + if (mirror) { + builder = builder.splineToSplineHeading(mirrorPose(pose), -tangent); + } else { + builder = builder.splineToSplineHeading(pose, tangent); + } + return this; + } + public PathFactory splineToConstantHeading(Vector2d vector, double tangent) { + if(mirror) { + builder = builder.splineToConstantHeading(mirrorVector(vector), -tangent); + } else { + builder = builder.splineToConstantHeading(vector, tangent); + + } + return this; + } + public PathFactory stopAndAdd(Action a) { builder = builder.stopAndAdd(a); @@ -348,4 +457,7 @@ public PathFactory stopAndAdd(Action a) { public Action build() { return builder.build(); } + + + } diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java index 0587cc69ec4d..7ae3ac96f5e8 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java @@ -176,7 +176,7 @@ public void runOpMode() { } if (gamepad2.left_stick_x == 0 /*change meeeee */){ - launcher.setVelocity(intakeSpeed * Multiply); + //launcher.setVelocity(intakeSpeed * Multiply); } } From fdb670be6335c33dcc1569116ee62ead72d9f1b5 Mon Sep 17 00:00:00 2001 From: Team <417skid@swerverobotics.club> Date: Tue, 11 Nov 2025 18:30:01 -0800 Subject: [PATCH 079/198] fastbot code gone in this branch --- .../firstinspires/ftc/team417/BaseOpMode.java | 179 +----------------- .../ftc/team417/CompetitionAuto.java | 178 ++--------------- .../ftc/team417/CompetitionTeleOp.java | 137 ++++---------- 3 files changed, 60 insertions(+), 434 deletions(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java b/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java index a29643c92c9b..c579e5404964 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java @@ -103,193 +103,20 @@ public enum LaunchState { public void initHardware() { // Reversed direction of launcher for DevBot because motor is on the other side (compared to FastBot) - if (MecanumDrive.isDevBot) { - launcher.setDirection(DcMotorEx.Direction.REVERSE); - ROBOT_LENGTH = 18.5; - ROBOT_WIDTH = 18; - redLed = null; - greenLed = null; - - } - else if(MecanumDrive.isFastBot) { - ROBOT_WIDTH = 16; - ROBOT_LENGTH = 17; - //redLed = hardwareMap.get(LED.class, "redLed"); Uncomment one we get LEDs - //greenLed = hardwareMap.get(LED.class, "greenLed"); - //redLed.on(); - //greenLed.off(); - - launchState = LaunchState.IDLE; - - /* - * Initialize the hardware variables. Note that the strings used here as parameters - * to 'get' must correspond to the names assigned during the robot configuration - * step. - */ - // leftDrive = hardwareMap.get(DcMotor.class, "left_drive"); - // rightDrive = hardwareMap.get(DcMotor.class, "right_drive"); - - // initialize flywheel motor and feeder servos - launcher = hardwareMap.get(DcMotorEx.class, "motLauncher"); - leftFeeder = hardwareMap.get(CRServo.class, "servoBLaunchFeed"); - rightFeeder = hardwareMap.get(CRServo.class, "servoFLaunchFeed"); - - /* - * Here we set our launcher to the RUN_USING_ENCODER runmode. - * If you notice that you have no control over the velocity of the motor, it just jumps - * right to a number much higher than your set point, make sure that your encoders are plugged - * into the port right beside the motor itself. And that the motors polarity is consistent - * through any wiring. - */ - launcher.setMode(DcMotor.RunMode.RUN_USING_ENCODER); - - // set the flywheel to a braking behavior so it slows down faster when left trigger is pressed - launcher.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - - // set the feeder servos to an initial value to init the servo controller - leftFeeder.setPower(STOP_SPEED); - rightFeeder.setPower(STOP_SPEED); - - launcher.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, new PIDFCoefficients(300, 0, 0, 10)); - - //set the left feeder servo to rotate in reverse, so that the servos spin in the same relative direction - leftFeeder.setDirection(DcMotorSimple.Direction.REVERSE); - } else if (MecanumDrive.isSlowBot) { + + //add slowbot initialization code here drum = hardwareMap.get(Servo.class, "drum"); //launcher = hardwareMap.get(DcMotorEx.class, "motLauncher"); - } + // Tell the driver that initialization is complete. telemetry.addData("Status", "Initialized"); } - class LaunchAction extends RobotAction { - public boolean run(double ElapsedTime) { - if (ElapsedTime < 0.15) { - leftFeeder.setPower(FULL_SPEED); - rightFeeder.setPower(FULL_SPEED); - - return true; - } - else if(ElapsedTime < 1) { - leftFeeder.setPower(STOP_SPEED); - rightFeeder.setPower(STOP_SPEED); - return true; - } - else { - return false; - } - } - } - class SpinUpAction extends RobotAction { - public boolean run(double ElapsedTime) { - launcher.setVelocity(LAUNCHER_LOW_TARGET_VELOCITY); - if(ElapsedTime < 1) { - return true; - } - else { - return false; - } - } - } - - public void launch(boolean shotRequested) { - - - double launcherVelocity = launcher.getVelocity(); - switch (launchState) { - - case IDLE: - leftFeeder.setPower(SLOW_REV_SPEED); - rightFeeder.setPower(SLOW_REV_SPEED); - CURRENT_LAUNCHSTATE = "IDLE"; - break; - - case SORT: //if sorting launchmode is selected and shotRequested is true, check that the flywheel is in the correct velocity range (450 - 500 rpm) - leftFeeder.setPower(SLOW_REV_SPEED); - rightFeeder.setPower(SLOW_REV_SPEED); - if (shotRequested) { - CURRENT_LAUNCHSTATE = "SORT"; - launcher.setVelocity(LAUNCHER_SORTER_TARGET_VELOCITY); - leftFeeder.setPower(STOP_SPEED); - rightFeeder.setPower(STOP_SPEED); - if (launcherVelocity > LAUNCHER_SORTER_MIN_VELOCITY && launcherVelocity < LAUNCHER_SORTER_MAX_VELOCITY) { - launchState = LaunchState.LAUNCH; - - } - } - break; - - case LOW: //if low launchmode is selected and shotRequested is true, check that the flywheel is in the correct velocity range (1075 - 1175 rpm) - leftFeeder.setPower(SLOW_REV_SPEED); - rightFeeder.setPower(SLOW_REV_SPEED); - if (shotRequested) { - CURRENT_LAUNCHSTATE = "LOW"; - launcher.setVelocity(LAUNCHER_LOW_TARGET_VELOCITY); - if (launcherVelocity > LAUNCHER_LOW_MIN_VELOCITY && launcherVelocity < LAUNCHER_LOW_MAX_VELOCITY) { - leftFeeder.setPower(STOP_SPEED); - rightFeeder.setPower(STOP_SPEED); - if (redLed != null && greenLed != null) { - redLed.off(); - greenLed.on(); - } - launchState = LaunchState.LAUNCH; - } - } - break; - - case HIGH: //if high launchmode is selected and shotRequested is true, check that the flywheel is in the correct velocity range (1900 - 2000 rpm) - leftFeeder.setPower(SLOW_REV_SPEED); - rightFeeder.setPower(SLOW_REV_SPEED); - if (shotRequested) { - CURRENT_LAUNCHSTATE = "HIGH"; - launcher.setVelocity(LAUNCHER_HIGH_TARGET_VELOCITY); - leftFeeder.setPower(STOP_SPEED); - rightFeeder.setPower(STOP_SPEED); - if (launcherVelocity > LAUNCHER_HIGH_MIN_VELOCITY && launcherVelocity < LAUNCHER_HIGH_MAX_VELOCITY) { - if (redLed != null && greenLed != null) { - redLed.off(); - greenLed.on(); - } - launchState = LaunchState.LAUNCH; - } - } - break; - case LAUNCH: //when shotRequested, start the feeder servos to init launch - leftFeeder.setPower(FULL_SPEED); - rightFeeder.setPower(FULL_SPEED); - feederTimer.reset(); - launchState = LaunchState.LAUNCHING; - break; - case LAUNCHING: //wait until feedTimer surpasses FEED_TIME_SECONDS, then stop the feeder servos. - if (feederTimer.seconds() > FEED_TIME_SECONDS) { - leftFeeder.setPower(STOP_SPEED); - rightFeeder.setPower(STOP_SPEED); - - } - leftFeeder.setPower(SLOW_REV_SPEED); - rightFeeder.setPower(SLOW_REV_SPEED); - if (redLed != null && greenLed != null) { - redLed.off(); - greenLed.on(); - } - if (CURRENT_LAUNCHSTATE.equals("LOW") ) { - launchState = LaunchState.LOW; - } else if (CURRENT_LAUNCHSTATE.equals("HIGH")) { - launchState = LaunchState.HIGH; - - } else if (CURRENT_LAUNCHSTATE.equals("SORT")) { - launchState = LaunchState.SORT; - } else { - launchState = LaunchState.IDLE; - } - break; - } - } public void drumLogic () { diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java index 649eff10ec8c..c4e2e0113c33 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java @@ -29,7 +29,7 @@ enum Alliances { BLUE, } - enum FastBotMovements { + enum SlowBotMovements { NEAR, FAR, FAR_MINIMAL, @@ -43,16 +43,12 @@ enum FastBotMovements { @Override public void runOpMode() { initHardware(); - // different options for start positions - for both SlowBot and FastBot + Pose2d startPose = new Pose2d(0, 0, 0); - Pose2d redFBNearStartPose = new Pose2d(-60, 48, Math.toRadians(41)); - Pose2d redFBFarStartPose = new Pose2d(64, 16, Math.toRadians(0)); - Pose2d blueFBNearStartPose = new Pose2d(-50, -50.5, Math.toRadians(139)); - Pose2d blueFBFarStartPose = new Pose2d(64, -16, Math.toRadians(180)); Pose2d SBNearStartPose = new Pose2d(-60, 48, Math.toRadians(139)); - Pose2d SBFarStartPose = new Pose2d(64, 16, Math.toRadians(180)); + Pose2d SBFarStartPose = new Pose2d(60, 12, Math.toRadians(157.5)); MecanumDrive drive = new MecanumDrive(hardwareMap, telemetry, gamepad1, startPose); @@ -61,30 +57,16 @@ public void runOpMode() { MenuInput menuInput = new MenuInput(MenuInput.InputType.CONTROLLER); // Text menu for FastBot - if (MecanumDrive.isFastBot) { - menu.add(new MenuHeader("AUTO SETUP")) - .add() // empty line for spacing - .add("Pick an alliance:") - .add("alliance-picker-1", Alliances.class) // enum selector shortcut - .add() - .add("Pick a movement:") - .add("movement-picker-1", FastBotMovements.class) // enum selector shortcut - .add() - .add("Wait time:") - .add("wait-slider-1", new MenuSlider(minWaitTime, maxWaitTime)) - .add() - .add("finish-button-1", new MenuFinishedButton()); // Text menu for SlowBot - } else if (MecanumDrive.isSlowBot) { menu.add(new MenuHeader("AUTO SETUP")) .add() // empty line for spacing .add("Pick an alliance:") .add("alliance-picker-1", Alliances.class) // enum selector shortcut .add() .add("Pick a movement:") - .add("movement-picker-1", FastBotMovements.class) // enum selector shortcut + .add("movement-picker-1", SlowBotMovements.class) // enum selector shortcut .add() .add("Intake Cycles:") .add("intake-slider", new MenuSlider(minIntakes, maxIntakes)) @@ -93,7 +75,7 @@ public void runOpMode() { .add("wait-slider-1", new MenuSlider(minWaitTime, maxWaitTime)) .add() .add("finish-button-1", new MenuFinishedButton()); - } + while (!menu.isCompleted()) { // get x, y (stick) and select (A) input from controller @@ -108,65 +90,12 @@ public void runOpMode() { } Alliances chosenAlliance = menu.getResult(Alliances.class, "alliance-picker-1"); - FastBotMovements chosenMovement = menu.getResult(FastBotMovements.class, "movement-picker-1"); + SlowBotMovements chosenMovement = menu.getResult(SlowBotMovements.class, "movement-picker-1"); double waitTime = menu.getResult(Double.class, "wait-slider-1"); double intakeCycles = menu.getResult(Double.class, "intake-slider"); - // Red alliance FastBot auto paths - Action redNearFastBot = drive.actionBuilder(redFBNearStartPose) - - .setTangent(Math.toRadians(-49)) - .stopAndAdd(new SpinUpAction()) - .stopAndAdd(new LaunchAction()) - .stopAndAdd(new LaunchAction()) - .stopAndAdd(new LaunchAction()) - - .splineToLinearHeading(new Pose2d(-32, 54, Math.toRadians(0)), Math.toRadians(90)) - .build(); - - Action redFarFastBot = drive.actionBuilder(redFBFarStartPose) - .setTangent(Math.toRadians(135)) - .splineToLinearHeading(new Pose2d(-57, 36, Math.toRadians(0)), Math.toRadians(90)) - .stopAndAdd(new SpinUpAction()) - .stopAndAdd(new LaunchAction()) - .stopAndAdd(new LaunchAction()) - .stopAndAdd(new LaunchAction()) - .setTangent(Math.toRadians(-90)) - .splineToLinearHeading(new Pose2d(-56, 12, Math.toRadians(0)), Math.toRadians(-90)) - .build(); - - Action redFarMinimalFastBot = drive.actionBuilder(redFBFarStartPose) - .setTangent(Math.PI / 2) - .splineTo(new Vector2d(56, 35), Math.PI / 2) - .build(); - - // Blue alliance auto paths - Action blueNearFastBot = drive.actionBuilder(blueFBNearStartPose) - .setTangent(Math.toRadians(49)) - .stopAndAdd(new SpinUpAction()) - .stopAndAdd(new LaunchAction()) - .stopAndAdd(new LaunchAction()) - .stopAndAdd(new LaunchAction()) -// .splineTo(new Vector2d(-44, -44), Math.toRadians(49)) -// .setTangent(Math.toRadians(139)) - .splineToLinearHeading(new Pose2d(-32, -54, Math.toRadians(180)), Math.toRadians(-90)) - .build(); - Action blueFarFastBot = drive.actionBuilder(blueFBFarStartPose) - .setTangent(Math.toRadians(-135)) - .splineToLinearHeading(new Pose2d(-57, -36, Math.toRadians(180)), Math.toRadians(-90)) - .stopAndAdd(new SpinUpAction()) - .stopAndAdd(new LaunchAction()) - .stopAndAdd(new LaunchAction()) - .stopAndAdd(new LaunchAction()) - .setTangent(Math.toRadians(90)) - .splineToLinearHeading(new Pose2d(-56, -12, Math.toRadians(180)), Math.toRadians(90)) - .build(); - Action blueFarMinimalFastBot = drive.actionBuilder(blueFBFarStartPose) - .setTangent(Math.PI / 2) - .splineTo(new Vector2d(56, -35), Math.PI / 2) - .build(); PathFactory pathFactory; @@ -192,31 +121,31 @@ public void runOpMode() { .setTangent(Math.toRadians(180)) // 3 launch actions //then after disp intake action - .splineToSplineHeading(new Pose2d(36,32, Math.toRadians(90)), Math.toRadians(90)) + .splineToSplineHeading(new Pose2d(36,32, Math.toRadians(90)), Math.toRadians(90)) //go to intake farthest from goal .setTangent(Math.toRadians(90)) .splineToConstantHeading(new Vector2d(36,60), Math.toRadians(90)) .setTangent(Math.toRadians(-90)) - .splineToSplineHeading(new Pose2d(54,12, Math.toRadians(157.5)), Math.toRadians(-90)); + .splineToSplineHeading(new Pose2d(54,12, Math.toRadians(157.5)), Math.toRadians(-90)); //go to launch position if (intakeCycles > 1) { // 3 launch actions //after disp intake action farSlowBotIntake1 = farSlowBotIntake1.setTangent(Math.toRadians(180)) - .splineToSplineHeading(new Pose2d(12, 32, Math.toRadians(90)), Math.toRadians(90)) + .splineToSplineHeading(new Pose2d(12, 32, Math.toRadians(90)), Math.toRadians(90)) //go to intake middle from goal .setTangent(Math.toRadians(90)) .splineToConstantHeading(new Vector2d(12, 60), Math.toRadians(90)) .setTangent(Math.toRadians(-90)) - .splineToSplineHeading(new Pose2d(54, 12, Math.toRadians(157.5)), Math.toRadians(-90)); + .splineToSplineHeading(new Pose2d(54, 12, Math.toRadians(157.5)), Math.toRadians(-90)); //go to launch position // 3 launch actions //after disp intake action if (intakeCycles > 2) { farSlowBotIntake1 = farSlowBotIntake1.setTangent(Math.toRadians(180)) - .splineToSplineHeading(new Pose2d(-12,32, Math.toRadians(90)), Math.toRadians(90)) + .splineToSplineHeading(new Pose2d(-12,32, Math.toRadians(90)), Math.toRadians(90)) //go to intake closest to goal .setTangent(Math.toRadians(90)) .splineToConstantHeading(new Vector2d(-12,55), Math.toRadians(90)) .setTangent(Math.toRadians(-90)) - .splineToSplineHeading(new Pose2d(54,12, Math.toRadians(157.5)), Math.toRadians(-90)); + .splineToSplineHeading(new Pose2d(54,12, Math.toRadians(157.5)), Math.toRadians(-90)); //go to launch position } } @@ -233,11 +162,11 @@ public void runOpMode() { //3 launches //after disp intake .setTangent(Math.toRadians(0)) - .splineToSplineHeading(new Pose2d(-12,32, Math.toRadians(90)), Math.toRadians(90)) + .splineToSplineHeading(new Pose2d(-12,32, Math.toRadians(90)), Math.toRadians(90)) //go to intake closest from goal .setTangent(Math.toRadians(90)) .splineToConstantHeading(new Vector2d(-12,55), Math.toRadians(90)) .setTangent(Math.toRadians(-90)) - .splineToSplineHeading(new Pose2d(-36,36, Math.toRadians(139)), Math.toRadians(180)); + .splineToSplineHeading(new Pose2d(-36,36, Math.toRadians(139)), Math.toRadians(180)); //go to launch position if (intakeCycles > 1) { nearSlowBotPath = nearSlowBotPath.setTangent(Math.toRadians(0)) @@ -245,20 +174,20 @@ public void runOpMode() { //3 launches //after disp intake - .splineToSplineHeading(new Pose2d(12, 32, Math.toRadians(90)), Math.toRadians(90)) + .splineToSplineHeading(new Pose2d(12, 32, Math.toRadians(90)), Math.toRadians(90)) //go to intake middle from goal .setTangent(Math.toRadians(90)) .splineToConstantHeading(new Vector2d(12, 60), Math.toRadians(90)) .setTangent(Math.toRadians(-90)) - .splineToSplineHeading(new Pose2d(-36, 36, Math.toRadians(139)), Math.toRadians(180)); + .splineToSplineHeading(new Pose2d(-36, 36, Math.toRadians(139)), Math.toRadians(180)); //go to launch position //3 launches //after disp intake if (intakeCycles > 2) { nearSlowBotPath = nearSlowBotPath.setTangent(Math.toRadians(0)) - .splineToSplineHeading(new Pose2d(36, 32, Math.toRadians(90)), Math.toRadians(90)) + .splineToSplineHeading(new Pose2d(36, 32, Math.toRadians(90)), Math.toRadians(90)) //go to intake farthest from goal .setTangent(Math.toRadians(90)) .splineToConstantHeading(new Vector2d(36, 60), Math.toRadians(90)) .setTangent(Math.toRadians(-90)) - .splineToSplineHeading(new Pose2d(-36, 36, Math.toRadians(139)), Math.toRadians(180)); + .splineToSplineHeading(new Pose2d(-36, 36, Math.toRadians(139)), Math.toRadians(180)); //go to launch position } } @@ -266,75 +195,8 @@ public void runOpMode() { .splineToSplineHeading(new Pose2d(-48, 12, Math.toRadians(180)), Math.toRadians(180)); Action nearSlowBot = nearSlowBotPath.build(); // the first parameter is the type to return as - if (MecanumDrive.isFastBot) { - Action trajectoryAction = null; - switch (chosenAlliance) { - case RED: - switch (chosenMovement) { - case NEAR: - drive.setPose(redFBNearStartPose); - trajectoryAction = redNearFastBot; - break; - case FAR: - drive.setPose(redFBFarStartPose); - trajectoryAction = redFarFastBot; - break; - case FAR_MINIMAL: - drive.setPose(redFBFarStartPose); - trajectoryAction = redFarMinimalFastBot; - break; - } - break; - - case BLUE: - switch (chosenMovement) { - case NEAR: - drive.setPose(blueFBNearStartPose); - trajectoryAction = blueNearFastBot; - break; - case FAR: - drive.setPose(blueFBFarStartPose); - trajectoryAction = blueFarFastBot; - break; - case FAR_MINIMAL: - drive.setPose(blueFBFarStartPose); - trajectoryAction = blueFarMinimalFastBot; - break; - } - break; - } - // Get a preview of the trajectory's path: - Canvas previewCanvas = new Canvas(); - trajectoryAction.preview(previewCanvas); - - // Show the preview on FTC Dashboard now. - TelemetryPacket packet = MecanumDrive.getTelemetryPacket(); - packet.fieldOverlay().getOperations().addAll(previewCanvas.getOperations()); - MecanumDrive.sendTelemetryPacket(packet); - - // Wait for Start to be pressed on the Driver Hub! - waitForStart(); - - boolean more = true; - while (opModeIsActive() && more) { - telemetry.addLine("Running Auto!"); - - // 'packet' is the object used to send data to FTC Dashboard: - packet = MecanumDrive.getTelemetryPacket(); - - // Draw the preview and then run the next step of the trajectory on top: - packet.fieldOverlay().getOperations().addAll(previewCanvas.getOperations()); - more = trajectoryAction.run(packet); - - // Only send the packet if there's more to do in order to keep the very last - // drawing up on the field once the robot is done: - if (more) - MecanumDrive.sendTelemetryPacket(packet); - telemetry.update(); - } - } else if (MecanumDrive.isSlowBot) { Action trajectoryAction = null; switch (chosenMovement) { @@ -364,8 +226,9 @@ public void runOpMode() { MecanumDrive.sendTelemetryPacket(packet); // Wait for Start to be pressed on the Driver Hub! - waitForStart(); + waitForStart(); + sleep((long)waitTime*1000); boolean more = true; while (opModeIsActive() && more) { telemetry.addLine("Running Auto!"); @@ -385,7 +248,6 @@ public void runOpMode() { } } } -} class PathFactory { MecanumDrive drive; diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java index 7ae3ac96f5e8..8c9f36eafbff 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java @@ -23,7 +23,6 @@ public class CompetitionTeleOp extends BaseOpMode { double SLOWDRIVE_SPEED = 0.5; - ElapsedTime rightBumperTimer = new ElapsedTime(); /* @@ -76,122 +75,60 @@ public void runOpMode() { // 'packet' is the object used to send data to FTC Dashboard: TelemetryPacket packet = MecanumDrive.getTelemetryPacket(); - if (MecanumDrive.isFastBot) { - // send telemetry to FTC dashboard to graph - packet.put("FlyWheel Speed:", launcher.getVelocity()); - packet.put("Right bumper press:", gamepad2.right_bumper ? 0 : 1000); - packet.put("Feeder wheels:", rightFeeder.getPower() * 100); - - - // Do the work now for all active Road Runner actions, if any: - drive.doActionsWork(packet); - - // Draw the robot and field: - packet.fieldOverlay().setStroke("#3F51B5"); - Drawing.drawRobot(packet.fieldOverlay(), drive.pose); - MecanumDrive.sendTelemetryPacket(packet); - - - if (gamepad2.y) { //high speed - launcher.setVelocity(LAUNCHER_HIGH_TARGET_VELOCITY); - launchState = LaunchState.HIGH; - - } else if (gamepad2.a) { //slow speed - launcher.setVelocity(LAUNCHER_LOW_TARGET_VELOCITY); - launchState = LaunchState.LOW; + //add slowbot teleop controls here - } else if (gamepad2.x) { // sort speed - launcher.setVelocity(LAUNCHER_SORTER_TARGET_VELOCITY); - launchState = LaunchState.SORT; - - } else if (gamepad2.b) { //reverse - launcher.setVelocity(LAUNCHER_REV_TARGET_VELOCITY); - leftFeeder.setPower(REV_SPEED); - rightFeeder.setPower(REV_SPEED); - - } else if (gamepad2.left_bumper) { // stop flywheel - launcher.setVelocity(STOP_SPEED); - leftFeeder.setPower(STOP_SPEED); - rightFeeder.setPower(STOP_SPEED); - launchState = LaunchState.IDLE; - CURRENT_LAUNCHSTATE = "IDLE"; - } - - - /* - * Now we call our "Launch" function. - */ - if (rightBumperTimer.seconds() > 0.25) { - launch(gamepad2.rightBumperWasPressed()); - rightBumperTimer.reset(); - } - - /* - * Show the state and motor powers - */ - telemetry.addData("State", launchState); - // telemetry.addData("Motors", "left (%.2f), right (%.2f)", leftPower, rightPower); - telemetry.addData("motorSpeed", launcher.getVelocity()); - telemetry.addData("FEED_TIME_SECONDS", FEED_TIME_SECONDS); - telemetry.addData("leftFeeder", leftFeeder.getPower()); - telemetry.addData("rightFeeder", rightFeeder.getPower()); - telemetry.update(); - - } else if (MecanumDrive.isSlowBot) { - //add slowbot teleop controls here - - if (gamepad2.y) { //rotate drum to purple - - - } else if (gamepad2.a) { //rotate drum to green + } + if (gamepad2.y) { //rotate drum to purple - } + } else if (gamepad2.a) { //rotate drum to green - } else if (gamepad2.x) { // sort speed + } else if (gamepad2.x) { // sort speed // drum.setTargetPosition(moveOnePosition); // drum.setVelocity(drumVelocity); - } + } /* launcher exclusive block ----------------------------- */ - if (gamepad2.left_bumper) { // stop launcher - launcher.setVelocity(STOP_SPEED); - - } else if (gamepad2.right_bumper) { //launch + transfer - launcher.setVelocity(0/*variable*/); - //transfer stuff goes here - } else if (gamepad2.dpad_up) { - launcher.setVelocity(LAUNCHER_HIGH_TARGET_VELOCITY); - launchState = LaunchState.HIGH; - //values will need change, possibly new enum for slowbot launch - } else if (gamepad2.dpad_down) { - launcher.setVelocity(LAUNCHER_LOW_TARGET_VELOCITY); - launchState = LaunchState.LOW; - //values will need change, possibly new enum for slowbot launch - } - - if (gamepad2.left_stick_x == 0 /*change meeeee */){ - //launcher.setVelocity(intakeSpeed * Multiply); - - } - } + if (gamepad2.left_bumper) { // stop launcher + launcher.setVelocity(STOP_SPEED); + + } else if (gamepad2.right_bumper) { //launch + transfer + launcher.setVelocity(0/*variable*/); + //transfer stuff goes here + } else if (gamepad2.dpad_up) { + launcher.setVelocity(LAUNCHER_HIGH_TARGET_VELOCITY); + launchState = LaunchState.HIGH; + //values will need change, possibly new enum for slowbot launch + } else if (gamepad2.dpad_down) { + launcher.setVelocity(LAUNCHER_LOW_TARGET_VELOCITY); + launchState = LaunchState.LOW; + //values will need change, possibly new enum for slowbot launch } - public double doSLOWMODE () { - if (gamepad1.right_trigger != 0) { - return -gamepad1.right_trigger + 1.1; - } else { - return 1; - } + if (gamepad2.left_stick_x == 0 /*change meeeee */) { + //launcher.setVelocity(intakeSpeed * Multiply); + } + } + + - public static double halfLinearHalfCubic ( double input){ - return (Math.pow(input, 3) + input) / 2; + public double doSLOWMODE() { + if (gamepad1.right_trigger != 0) { + return -gamepad1.right_trigger + 1.1; + } else { + return 1; } } + public static double halfLinearHalfCubic(double input) { + return (Math.pow(input, 3) + input) / 2; + } +} + + From a818b7d8af5d787160133f83d917bda403d76e54 Mon Sep 17 00:00:00 2001 From: Emmett Date: Tue, 11 Nov 2025 19:13:34 -0800 Subject: [PATCH 080/198] finsished the launch, intake, and queueDrum methods --- .../ftc/team417/ComplexMechGlob.java | 82 +++++++++++++------ 1 file changed, 58 insertions(+), 24 deletions(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java b/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java index 5ff8ab4cd568..6ea12af181f5 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java @@ -1,6 +1,5 @@ package org.firstinspires.ftc.team417; -import static org.firstinspires.ftc.robotcore.external.BlocksOpModeCompanion.gamepad2; import com.qualcomm.robotcore.hardware.HardwareMap; import org.firstinspires.ftc.robotcore.external.Telemetry; @@ -9,10 +8,11 @@ import java.util.ArrayList; import java.util.Collections; -enum LaunchColor { //an enum for different color cases for launching +enum RequestedColor { //an enum for different color cases for launching PURPLE, GREEN, - EITHER + EITHER, + NONE } enum PixelColor { PURPLE, @@ -37,7 +37,7 @@ static MechGlob create (HardwareMap hardwareMap, Telemetry telemetry){ void intake (double intakeValue){} //a method that determines what color to launch. Options are purple, green, or either. - void launch (LaunchColor launchColor) {} + void launch (RequestedColor requestedColor) {} void update () {} @@ -59,30 +59,56 @@ enum WaitState { IDLE, //waiting for input when the drum is full } + // arrays with placeholder values for servo positions and voltages relative to intake and launch + final double [] INTAKE_POSITIONS = {0, 1, 2}; + final double [] INTAKE_VOLTS = {0, 1, 2}; + final double [] LAUNCH_POSITIONS = {0, 1, 2}; + final double [] LAUNCH_VOLTS = {0, 1, 2}; + double lastQueuedPosition; //variable remembering where the servo was told to go last + HardwareMap hardwareMap; + Telemetry telemetry; class DrumRequest { double position; WaitState nextState; + + public DrumRequest(double position, WaitState nextState) { + this.nextState = nextState; + this.position = position; + } } - ComplexMechGlob (HardwareMap hardwareMap, Telemetry telemetry) {} - - int findNearestSlot (LaunchColor launchColor) { - double minDistance = Double.MAX_VALUE; - int minSlot = -1; - - for (int i = 0; i <= 2; i++){ - double distance; - if (launchColor == LaunchColor.PURPLE && slotOccupiedBy.get (i) == PixelColor.PURPLE){ - return i; - } else if (launchColor == LaunchColor.GREEN && slotOccupiedBy.get (i) == PixelColor.GREEN){ - return i; - } else if (launchColor == LaunchColor.EITHER &&slotOccupiedBy.get (i) != PixelColor.NONE){ - return i; + ComplexMechGlob (HardwareMap hardwareMap, Telemetry telemetry) { + this.hardwareMap = hardwareMap; + this.telemetry = telemetry; + } + + //the position argument denotes whether we are using intake or launch positions + int findNearestSlot (double [] position, RequestedColor requestedColor) { + + double minDistance = Double.MAX_VALUE; //the minimum distance to a slot that has what we want + int minSlot = -1; // this will only ever be 0, 1, or 2. -1 represents a invalid value + + // a for loop that will determine what slot has the requested color. + for (int i = 0; i <= 2; i++){ //here, the integer i represents the slot we are currently checking + double distance = Math.abs(position[i] - lastQueuedPosition); + //each conditional checks if what we requested and what we have in a specific slot matches. + if (distance < minDistance){ + if (requestedColor == RequestedColor.PURPLE && slotOccupiedBy.get (i) == PixelColor.PURPLE){ + minSlot = i;// if it does, mark the current slot as the nearest slot + } else if (requestedColor == RequestedColor.GREEN && slotOccupiedBy.get (i) == PixelColor.GREEN){ + minSlot = i; + } else if (requestedColor == RequestedColor.EITHER && slotOccupiedBy.get (i) != PixelColor.NONE){ + minSlot = i; + } else if (requestedColor == RequestedColor.NONE && slotOccupiedBy.get (i) == PixelColor.NONE){ + minSlot = i; + } } + } - return -1; + return minSlot; } + @Override void intake (double intakeSpeed) { @@ -91,11 +117,19 @@ void intake (double intakeSpeed) { @Override //a class that controls the launcher and transfer - void launch (LaunchColor launchColor) { - - - -// drumQueue.add (new DrumRequest (position, WaitState.TRANSFER)); + void launch (RequestedColor requestedColor) { + + int minSlot = findNearestSlot(LAUNCH_POSITIONS, requestedColor); + if (minSlot == -1){ + telemetry.speak("bad"); + } else { + queueDrum(LAUNCH_POSITIONS[minSlot], WaitState.SPIN_UP); + slotOccupiedBy.set (minSlot, PixelColor.NONE); //marking this slot as empty so we don't accidentally try to use it again + } + } + void queueDrum (double position, WaitState waitState){ + drumQueue.add(new DrumRequest(position, waitState)); + lastQueuedPosition = position; } @Override From bef59f086217d99d4e885300b6aa56056ba43426 Mon Sep 17 00:00:00 2001 From: Hankang Zhou <136044263+nobody-particular@users.noreply.github.com> Date: Tue, 11 Nov 2025 19:36:25 -0800 Subject: [PATCH 081/198] Merge remote-tracking branch 'origin/decode' into decode # Conflicts: # team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java --- .../ftc/team417/CompetitionAuto.java | 9 +--- .../ftc/team417/ComplexMechGlob.java | 2 - .../ftc/team417/CoolColorDetector.java | 53 ++++++++++++++----- 3 files changed, 41 insertions(+), 23 deletions(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java index 953d6a912695..687abf8409d4 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java @@ -17,8 +17,6 @@ import org.firstinspires.ftc.team417.javatextmenu.TextMenu; import org.firstinspires.ftc.team417.roadrunner.MecanumDrive; -import java.nio.file.Path; - /** * This class exposes the competition version of Autonomous. As a general rule, add code to the * BaseOpMode class rather than here so that it can be shared between both TeleOp and Autonomous. @@ -96,14 +94,11 @@ public void runOpMode() { telemetry.update(); } - Alliance chosenAlliance = menu.getResult(Alliances.class, "alliance-picker-1"); - SlowBotMovement chosenMovement = menu.getResult(SlowBotMovements.class, "movement-picker-1"); + Alliance chosenAlliance = menu.getResult(Alliance.class, "alliance-picker-1"); + SlowBotMovement chosenMovement = menu.getResult(SlowBotMovement.class, "movement-picker-1"); double waitTime = menu.getResult(Double.class, "wait-slider-1"); double intakeCycles = menu.getResult(Double.class, "intake-slider"); - - - PathFactory pathFactory; switch (chosenAlliance) { diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java b/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java index 5ff8ab4cd568..38efbcebf0d9 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java @@ -1,7 +1,5 @@ package org.firstinspires.ftc.team417; -import static org.firstinspires.ftc.robotcore.external.BlocksOpModeCompanion.gamepad2; - import com.qualcomm.robotcore.hardware.HardwareMap; import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.team417.roadrunner.MecanumDrive; diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CoolColorDetector.java b/team417/src/main/java/org/firstinspires/ftc/team417/CoolColorDetector.java index 4000330cc546..5323340919dc 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CoolColorDetector.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CoolColorDetector.java @@ -1,23 +1,11 @@ package org.firstinspires.ftc.team417; -import com.qualcomm.robotcore.hardware.HardwareMap; - import com.qualcomm.robotcore.hardware.ColorSensor; +import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.NormalizedColorSensor; -import android.graphics.Color; - import org.firstinspires.ftc.robotcore.external.Telemetry; -//import com.qualcomm.robotcore.hardware.ColorSensor; -//import com.qualcomm.robotcore.hardware.NormalizedRGBA; -//import android.graphics.Color; -//import com.qualcomm.robotcore.hardware.SwitchableLight; -enum PixelColor { - GREEN, - PURPLE, - NONE -} public class CoolColorDetector { private ColorSensor sensor1; private ColorSensor sensor2; @@ -27,6 +15,7 @@ public CoolColorDetector(HardwareMap map) { sensor1 = map.get(ColorSensor.class, "cs1"); //sensor2 = map.get(ColorSensor.class, "sensor2"); } + // --- Convert a sensor to ONE PixelColor --- private PixelColor detectSingle(ColorSensor sensor1) { // Get raw values @@ -35,7 +24,7 @@ private PixelColor detectSingle(ColorSensor sensor1) { float r = sensor1.red(); float g = sensor1.green(); float b = sensor1.blue(); - Color.RGBToHSV((int)r, (int)g, (int)b, hsv); + hsv = rgbToHsv((int)r, (int)g, (int)b); float hue = hsv[0]; // GREEN Range: 145 - 165 if (hue >= 145 && hue <= 185) { @@ -51,6 +40,42 @@ else if (hue >= 215 && hue <= 245) { } + public static float[] rgbToHsv(int r, int g, int b) { + float[] hsv = new float[3]; + + // Normalize R, G, B values to the range 0-1 + float red = r / 255.0f; + float green = g / 255.0f; + float blue = b / 255.0f; + + float cmax = Math.max(red, Math.max(green, blue)); // Maximum of R, G, B + float cmin = Math.min(red, Math.min(green, blue)); // Minimum of R, G, B + float delta = cmax - cmin; // Delta of max and min + + // Calculate Hue (H) + if (delta == 0) { + hsv[0] = 0; // Hue is undefined for achromatic colors (grays) + } else if (cmax == red) { + hsv[0] = (60 * ((green - blue) / delta) + 360) % 360; + } else if (cmax == green) { + hsv[0] = (60 * ((blue - red) / delta) + 120); + } else { // cmax == blue + hsv[0] = (60 * ((red - green) / delta) + 240); + } + + // Calculate Saturation (S) + if (cmax == 0) { + hsv[1] = 0; // Saturation is 0 for black + } else { + hsv[1] = delta / cmax; + } + + // Calculate Value (V) + hsv[2] = cmax; + + return hsv; + } + // --- Use logic comparing both sensors --- /*PixelColor detectPixelPosition() { PixelColor s1 = detectSingle(sensor1); From e7df8392a4e565c9d391289660ebdbc1d7adbd06 Mon Sep 17 00:00:00 2001 From: anya-codes Date: Tue, 11 Nov 2025 20:32:53 -0800 Subject: [PATCH 082/198] Removed fastbot constants and old slowbot teleop code (because we're rewriting it anyway) --- .../firstinspires/ftc/team417/BaseOpMode.java | 53 ------------------- .../ftc/team417/CompetitionTeleOp.java | 41 -------------- 2 files changed, 94 deletions(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java b/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java index 135e72e8b396..eaa602a88e0b 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java @@ -24,62 +24,9 @@ abstract public class BaseOpMode extends LinearOpMode { //fastbot hardware variables - public DcMotorEx launcher = null; - - public CRServo leftFeeder = null; - public CRServo rightFeeder = null; - - //fastbot constants - public static final double STOP_SPEED = 0.0; //We send this power to the servos when we want them to stop. - public static double FEED_TIME_SECONDS = 0; //The feeder servos run this long when a shot is requested. - - public static double FEED_TIME_LOW = 0.15; - public static double FEED_TIME_SORT = 0.07; - - - public static double rememberVelocity = 0; - - public static double FULL_SPEED = 1.0; //We send this power to the servos when we want them to feed an artifact to the launcher - public static double SLOW_REV_SPEED = -0.15; //speed for the constant reverse rotation - public static double REV_SPEED = -1.0;//speed used for the reverse launch function - public static double LAUNCHER_HIGH_MAX_VELOCITY = 2000; //high target velocity + 50 (will need adjusting) - public static double LAUNCHER_HIGH_TARGET_VELOCITY = 1950; - public static double LAUNCHER_HIGH_MIN_VELOCITY = 1900; - - public static double LAUNCHER_LOW_MAX_VELOCITY = 1175; //low target velocity + 50 (will need adjusting) - public static double LAUNCHER_LOW_TARGET_VELOCITY = 1125; - public static double LAUNCHER_LOW_MIN_VELOCITY = 1075; - - public static double LAUNCHER_SORTER_MAX_VELOCITY = 550; //sorter target velocity + 50 (will need adjusting) - public static double LAUNCHER_SORTER_TARGET_VELOCITY = 500; - public static double LAUNCHER_SORTER_MIN_VELOCITY = 450; - - public double ROBOT_WIDTH = 0; - public double ROBOT_LENGTH = 0; - - public static double LAUNCHER_REV_TARGET_VELOCITY = -250; - - - public LED redLed; - public LED greenLed; - - ElapsedTime feederTimer = new ElapsedTime(); - - public String CURRENT_LAUNCHSTATE = "IDLE"; - public enum LaunchState { - IDLE, - HIGH, - LOW, - SORT, - LAUNCH, - LAUNCHING, - } - - public LaunchState launchState; /*---------------------------------------------------------------*/ //slowbot hardware - public Servo drum = null; //slowbot constants diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java index 8c9f36eafbff..ecab8c2ddb2c 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java @@ -19,12 +19,6 @@ @Config public class CompetitionTeleOp extends BaseOpMode { - double FASTDRIVE_SPEED = 1.0; - double SLOWDRIVE_SPEED = 0.5; - - - ElapsedTime rightBumperTimer = new ElapsedTime(); - /* * TECH TIP: State Machines * We use a "state machine" to control our launcher motor and feeder servos in this program. @@ -78,41 +72,6 @@ public void runOpMode() { //add slowbot teleop controls here - } - if (gamepad2.y) { //rotate drum to purple - - - } else if (gamepad2.a) { //rotate drum to green - - } else if (gamepad2.x) { // sort speed -// drum.setTargetPosition(moveOnePosition); -// drum.setVelocity(drumVelocity); - - - } - - /* launcher exclusive block - ----------------------------- - */ - if (gamepad2.left_bumper) { // stop launcher - launcher.setVelocity(STOP_SPEED); - - } else if (gamepad2.right_bumper) { //launch + transfer - launcher.setVelocity(0/*variable*/); - //transfer stuff goes here - } else if (gamepad2.dpad_up) { - launcher.setVelocity(LAUNCHER_HIGH_TARGET_VELOCITY); - launchState = LaunchState.HIGH; - //values will need change, possibly new enum for slowbot launch - } else if (gamepad2.dpad_down) { - launcher.setVelocity(LAUNCHER_LOW_TARGET_VELOCITY); - launchState = LaunchState.LOW; - //values will need change, possibly new enum for slowbot launch - } - - if (gamepad2.left_stick_x == 0 /*change meeeee */) { - //launcher.setVelocity(intakeSpeed * Multiply); - } } From 06ce0b54a1848748bb98e9b1fdc54b4a9ff118f5 Mon Sep 17 00:00:00 2001 From: bharv Date: Tue, 11 Nov 2025 20:49:35 -0800 Subject: [PATCH 083/198] Color detection working - redone with value for mone and hue ranges for green and purple --- .../ftc/team417/CoolColorDetector.java | 57 +++++++++++-------- .../ftc/team417/TestColorDetect.java | 4 +- 2 files changed, 34 insertions(+), 27 deletions(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CoolColorDetector.java b/team417/src/main/java/org/firstinspires/ftc/team417/CoolColorDetector.java index 4000330cc546..0bf53331faa7 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CoolColorDetector.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CoolColorDetector.java @@ -5,6 +5,7 @@ import com.qualcomm.robotcore.hardware.ColorSensor; import com.qualcomm.robotcore.hardware.NormalizedColorSensor; +import android.annotation.SuppressLint; import android.graphics.Color; import org.firstinspires.ftc.robotcore.external.Telemetry; @@ -13,46 +14,53 @@ //import android.graphics.Color; //import com.qualcomm.robotcore.hardware.SwitchableLight; -enum PixelColor { - GREEN, - PURPLE, - NONE -} +//enum PixelColor { + // GREEN, + // PURPLE, + // NONE +//} + public class CoolColorDetector { + Telemetry telemetry; private ColorSensor sensor1; private ColorSensor sensor2; - private float gain = 4f; // adjust for brightness + private float gain = 50f; // adjust for brightness private float[] hsv = new float[3]; - public CoolColorDetector(HardwareMap map) { + public CoolColorDetector(HardwareMap map, Telemetry telemetry) { sensor1 = map.get(ColorSensor.class, "cs1"); - //sensor2 = map.get(ColorSensor.class, "sensor2"); + sensor2 = map.get(ColorSensor.class, "cs2"); + this.telemetry = telemetry; } // --- Convert a sensor to ONE PixelColor --- - private PixelColor detectSingle(ColorSensor sensor1) { + @SuppressLint("DefaultLocale") + private PixelColor detectSingle(ColorSensor sensor) { // Get raw values - ((NormalizedColorSensor)sensor1).setGain(gain); + ((NormalizedColorSensor)sensor).setGain(gain); //Just tried something new with the setGain - float r = sensor1.red(); - float g = sensor1.green(); - float b = sensor1.blue(); + float r = sensor.red(); + float g = sensor.green(); + float b = sensor.blue(); Color.RGBToHSV((int)r, (int)g, (int)b, hsv); + + telemetry.addData("HSV", String.format("{%f, %f, %f}", hsv[0], hsv[1], hsv[2])); float hue = hsv[0]; + float value = hsv[2]; + // // GREEN Range: 145 - 165 - if (hue >= 145 && hue <= 185) { + if(value < 0.45) { + return PixelColor.NONE; + } + else if (hue >= 10 && hue <= 190) { return PixelColor.GREEN; } // PURPLE Range: 215 - 245 - else if (hue >= 215 && hue <= 245) { + else{ return PixelColor.PURPLE; } - else { - return PixelColor.NONE; - } - } // --- Use logic comparing both sensors --- - /*PixelColor detectPixelPosition() { + PixelColor detectPixelPosition() { PixelColor s1 = detectSingle(sensor1); PixelColor s2 = detectSingle(sensor2); // Rule 1: If both detect something different → return sensor2 @@ -71,12 +79,11 @@ else if (hue >= 215 && hue <= 245) { // Otherwise no decision → return none return PixelColor.NONE; } - }*/ - public void showTelemetry(Telemetry telemetry) { + } + public void showTelemetry() { telemetry.addData("Sensor 1", detectSingle(sensor1)); - //telemetry.addData("Sensor 1"); - //telemetry.addData("Sensor 2", detectSingle(sensor2)); - //telemetry.addData("Chosen Position", detectPixelPosition()); + telemetry.addData("Sensor 2", detectSingle(sensor2)); + telemetry.addData("Chosen Position", detectPixelPosition()); telemetry.update(); } } diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/TestColorDetect.java b/team417/src/main/java/org/firstinspires/ftc/team417/TestColorDetect.java index 93ed0e7c547a..663192140b58 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/TestColorDetect.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/TestColorDetect.java @@ -6,10 +6,10 @@ public class TestColorDetect extends LinearOpMode { CoolColorDetector detector; @Override public void runOpMode() { - detector = new CoolColorDetector(hardwareMap); + detector = new CoolColorDetector(hardwareMap, telemetry); waitForStart(); while (opModeIsActive()) { - detector.showTelemetry(telemetry); + detector.showTelemetry(); } } } From 1d5c2e8a1c398918376ffa479dd0552553c6bb15 Mon Sep 17 00:00:00 2001 From: Emmett Date: Tue, 11 Nov 2025 20:51:59 -0800 Subject: [PATCH 084/198] started work on void update. hardware intialization and many constants are now present. yay. --- .../ftc/team417/ComplexMechGlob.java | 97 ++++++++++++++++++- 1 file changed, 95 insertions(+), 2 deletions(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java b/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java index 6ea12af181f5..6d15520cd78c 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java @@ -1,9 +1,22 @@ package org.firstinspires.ftc.team417; +import com.acmerobotics.dashboard.config.Config; +import com.qualcomm.hardware.ams.AMSColorSensor; +import com.qualcomm.hardware.rev.RevColorSensorV3; +import com.qualcomm.robotcore.hardware.AnalogInput; +import com.qualcomm.robotcore.hardware.CRServo; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.NormalizedColorSensor; +import com.qualcomm.robotcore.hardware.PIDFCoefficients; +import com.qualcomm.robotcore.hardware.Servo; + import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.team417.roadrunner.MecanumDrive; +import org.swerverobotics.ftc.GoBildaPinpointDriver; import java.util.ArrayList; import java.util.Collections; @@ -44,8 +57,19 @@ void update () {} } -// +@Config public class ComplexMechGlob extends MechGlob { //a class encompassing all code that IS for slowbot + //constants for tuning via FTC Dashboard: + static double FEEDER_POWER = 1; + static double UPPER_FLYWHEEL_VELOCITY = 1500; + static double LOWER_FLYWHEEL_VELOCITY = 1500; + static double TRANSFER_INACTIVE_POSITION = 0; + static double TRANSFER_ACTIVE_POSITION = 1; + static double REVERSE_INTAKE_SPEED = -1; + static double INTAKE_SPEED = 1; + + + double userIntakeSpeed; double drumServoPosition; //the last position the servo went to ArrayList drumQueue = new ArrayList<> (); @@ -59,15 +83,31 @@ enum WaitState { IDLE, //waiting for input when the drum is full } + WaitState waitState = WaitState.IDLE; // arrays with placeholder values for servo positions and voltages relative to intake and launch final double [] INTAKE_POSITIONS = {0, 1, 2}; final double [] INTAKE_VOLTS = {0, 1, 2}; final double [] LAUNCH_POSITIONS = {0, 1, 2}; final double [] LAUNCH_VOLTS = {0, 1, 2}; - double lastQueuedPosition; //variable remembering where the servo was told to go last + double lastQueuedPosition; //where the servo was *queued* to go last. NOT THE SAME AS hwDrumPosition! + double hwDrumPosition; //where the drum was *told* to go last. NOT THE SAME AS lastQueuedPosition! + HardwareMap hardwareMap; Telemetry telemetry; + //hardware objects + Servo servoDrum; + Servo servoTransfer; + AnalogInput analogDrum; + DcMotorEx motLLauncher; + DcMotorEx motULauncher; + DcMotorEx motIntake; + CRServo servoBLaunchFeeder; + CRServo servoFLaunchFeeder; + NormalizedColorSensor sensorColor1; + NormalizedColorSensor sensorColor2; + + CoolColorDetector coolColorDetector; class DrumRequest { double position; @@ -81,6 +121,39 @@ public DrumRequest(double position, WaitState nextState) { ComplexMechGlob (HardwareMap hardwareMap, Telemetry telemetry) { this.hardwareMap = hardwareMap; this.telemetry = telemetry; + + servoDrum = hardwareMap.get(Servo.class, "servoDrum"); + servoTransfer = hardwareMap.get(Servo.class, "servoTransfer"); + analogDrum = hardwareMap.get(AnalogInput.class, "analogDrum"); + motLLauncher = hardwareMap.get(DcMotorEx.class, "motLLauncher"); + motULauncher = hardwareMap.get(DcMotorEx.class, "motULauncher"); + motIntake = hardwareMap.get(DcMotorEx.class, "motIntake"); + servoBLaunchFeeder = hardwareMap.get(CRServo.class, "servoBLaunchFeeder"); + servoFLaunchFeeder = hardwareMap.get(CRServo.class, "servoFLaunchFeeder"); + coolColorDetector = new CoolColorDetector(hardwareMap); + + /* + * Here we set our flywheels to the RUN_USING_ENCODER runmode. + * If you notice that you have no control over the velocity of the motor, it just jumps + * right to a number much higher than your set point, make sure that your encoders are plugged + * into the port right beside the motor itself. And that the motors polarity is consistent + * through any wiring. + */ + motLLauncher.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + motULauncher.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + + // set the motors to a braking behavior so it slows down faster when left trigger is pressed + motLLauncher.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + motULauncher.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + motIntake.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + + motLLauncher.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, new PIDFCoefficients(300, 0, 0, 10)); + motULauncher.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, new PIDFCoefficients(300, 0, 0, 10)); + + motLLauncher.setDirection(DcMotorSimple.Direction.REVERSE); + servoBLaunchFeeder.setDirection(CRServo.Direction.REVERSE); + + } //the position argument denotes whether we are using intake or launch positions @@ -134,7 +207,27 @@ void queueDrum (double position, WaitState waitState){ @Override void update () { + double intakePower = 0; + double transferPosition = TRANSFER_INACTIVE_POSITION; + + if (userIntakeSpeed < 0) { + intakePower = REVERSE_INTAKE_SPEED; + } else if (userIntakeSpeed > 0) { + if (waitState == WaitState.INTAKE) { + intakePower = INTAKE_SPEED; + } else if (!drumQueue.isEmpty() && drumQueue.get(0).nextState == WaitState.INTAKE) { + intakePower = INTAKE_SPEED; + } + } + + servoDrum.setPosition(hwDrumPosition); + servoTransfer.setPosition(transferPosition); + motLLauncher.setVelocity(LOWER_FLYWHEEL_VELOCITY); + motULauncher.setVelocity(UPPER_FLYWHEEL_VELOCITY); + motIntake.setPower(intakePower); + servoBLaunchFeeder.setPower(FEEDER_POWER); + servoFLaunchFeeder.setPower((FEEDER_POWER)); } } From dd857eee524718fd057f8e1055f5a32c56d70a97 Mon Sep 17 00:00:00 2001 From: bharv Date: Tue, 11 Nov 2025 20:53:39 -0800 Subject: [PATCH 085/198] Color detection working - redone with value for mone and hue ranges for green and purple --- .../org/firstinspires/ftc/team417/CoolColorDetector.java | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CoolColorDetector.java b/team417/src/main/java/org/firstinspires/ftc/team417/CoolColorDetector.java index 7c3da7fb6625..3aa3e271d33d 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CoolColorDetector.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CoolColorDetector.java @@ -1,5 +1,7 @@ package org.firstinspires.ftc.team417; +import android.annotation.SuppressLint; + import com.qualcomm.robotcore.hardware.ColorSensor; import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.NormalizedColorSensor; @@ -24,14 +26,10 @@ private PixelColor detectSingle(ColorSensor sensor) { // Get raw values ((NormalizedColorSensor)sensor).setGain(gain); //Just tried something new with the setGain - float r = sensor1.red(); - float g = sensor1.green(); - float b = sensor1.blue(); - hsv = rgbToHsv((int)r, (int)g, (int)b); float r = sensor.red(); float g = sensor.green(); float b = sensor.blue(); - Color.RGBToHSV((int)r, (int)g, (int)b, hsv); + hsv = rgbToHsv((int)r, (int)g, (int)b); telemetry.addData("HSV", String.format("{%f, %f, %f}", hsv[0], hsv[1], hsv[2])); float hue = hsv[0]; From f59dbb7bcf573c4d92bb9d8c6d30f0bfa19620b6 Mon Sep 17 00:00:00 2001 From: Emmett Date: Thu, 13 Nov 2025 20:50:34 -0800 Subject: [PATCH 086/198] finished WaitState logic, which talks to the hardware. yay! --- .../ftc/team417/ComplexMechGlob.java | 68 ++++++++++++++++--- 1 file changed, 57 insertions(+), 11 deletions(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java b/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java index 6d15520cd78c..e4d7b3ed2b5b 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java @@ -3,7 +3,6 @@ import com.acmerobotics.dashboard.config.Config; import com.qualcomm.hardware.ams.AMSColorSensor; -import com.qualcomm.hardware.rev.RevColorSensorV3; import com.qualcomm.robotcore.hardware.AnalogInput; import com.qualcomm.robotcore.hardware.CRServo; import com.qualcomm.robotcore.hardware.DcMotor; @@ -13,10 +12,10 @@ import com.qualcomm.robotcore.hardware.NormalizedColorSensor; import com.qualcomm.robotcore.hardware.PIDFCoefficients; import com.qualcomm.robotcore.hardware.Servo; +import com.qualcomm.robotcore.util.ElapsedTime; import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.team417.roadrunner.MecanumDrive; -import org.swerverobotics.ftc.GoBildaPinpointDriver; import java.util.ArrayList; import java.util.Collections; @@ -59,19 +58,21 @@ void update () {} @Config public class ComplexMechGlob extends MechGlob { //a class encompassing all code that IS for slowbot - //constants for tuning via FTC Dashboard: + // TODO tune constants via FTC Dashboard: static double FEEDER_POWER = 1; + static double TRANSFER_TIME_UP = 0.3; + static double TRANSFER_TIME_TOTAL = 0.6; //TRANSFER_TIME_TOTAL must be more than TRANSFER_TIME_UP static double UPPER_FLYWHEEL_VELOCITY = 1500; static double LOWER_FLYWHEEL_VELOCITY = 1500; static double TRANSFER_INACTIVE_POSITION = 0; static double TRANSFER_ACTIVE_POSITION = 1; static double REVERSE_INTAKE_SPEED = -1; static double INTAKE_SPEED = 1; + static double FLYWHEEL_VELOCITY_TOLERANCE = 25; - + ElapsedTime transferTimer; double userIntakeSpeed; - double drumServoPosition; //the last position the servo went to ArrayList drumQueue = new ArrayList<> (); ArrayList slotOccupiedBy = new ArrayList<> (Collections.nCopies(3, PixelColor.NONE)); @@ -92,6 +93,8 @@ enum WaitState { double lastQueuedPosition; //where the servo was *queued* to go last. NOT THE SAME AS hwDrumPosition! double hwDrumPosition; //where the drum was *told* to go last. NOT THE SAME AS lastQueuedPosition! + + HardwareMap hardwareMap; Telemetry telemetry; @@ -130,7 +133,7 @@ public DrumRequest(double position, WaitState nextState) { motIntake = hardwareMap.get(DcMotorEx.class, "motIntake"); servoBLaunchFeeder = hardwareMap.get(CRServo.class, "servoBLaunchFeeder"); servoFLaunchFeeder = hardwareMap.get(CRServo.class, "servoFLaunchFeeder"); - coolColorDetector = new CoolColorDetector(hardwareMap); + coolColorDetector = new CoolColorDetector(hardwareMap, telemetry); /* * Here we set our flywheels to the RUN_USING_ENCODER runmode. @@ -157,6 +160,7 @@ public DrumRequest(double position, WaitState nextState) { } //the position argument denotes whether we are using intake or launch positions + //position takes INTAKE_POSITIONS or LAUNCH_POSITIONS. int findNearestSlot (double [] position, RequestedColor requestedColor) { double minDistance = Double.MAX_VALUE; //the minimum distance to a slot that has what we want @@ -196,20 +200,22 @@ void launch (RequestedColor requestedColor) { if (minSlot == -1){ telemetry.speak("bad"); } else { - queueDrum(LAUNCH_POSITIONS[minSlot], WaitState.SPIN_UP); + addToDrumQueue(LAUNCH_POSITIONS[minSlot], WaitState.SPIN_UP); slotOccupiedBy.set (minSlot, PixelColor.NONE); //marking this slot as empty so we don't accidentally try to use it again } } - void queueDrum (double position, WaitState waitState){ + void addToDrumQueue(double position, WaitState waitState){ //this function adds a new drum request to the drum queue. drumQueue.add(new DrumRequest(position, waitState)); lastQueuedPosition = position; } + boolean drumAtPosition() { + return true; + // TODO: implement this + } @Override void update () { double intakePower = 0; - double transferPosition = TRANSFER_INACTIVE_POSITION; - if (userIntakeSpeed < 0) { intakePower = REVERSE_INTAKE_SPEED; } else if (userIntakeSpeed > 0) { @@ -220,7 +226,47 @@ void update () { } } - + if (waitState == WaitState.IDLE) { + if (userIntakeSpeed > 0) { + waitState = WaitState.INTAKE; + int minSlot = findNearestSlot(INTAKE_POSITIONS, RequestedColor.NONE); + if (minSlot != -1) { + addToDrumQueue(INTAKE_POSITIONS[minSlot], WaitState.INTAKE); + } + } + } + // let a firing request interrupt an intake + if (waitState == WaitState.IDLE || waitState == WaitState.INTAKE) { + if (!drumQueue.isEmpty()) { + hwDrumPosition = drumQueue.get(0).position; + waitState = WaitState.DRUM_MOVE; + } + } + if (waitState == WaitState.DRUM_MOVE) { + if (drumAtPosition()) { + waitState = drumQueue.get(0).nextState; + drumQueue.remove(0); + } + } + if (waitState == WaitState.SPIN_UP) { + if (Math.abs(motLLauncher.getVelocity() -LOWER_FLYWHEEL_VELOCITY) <= FLYWHEEL_VELOCITY_TOLERANCE && + Math.abs(motULauncher.getVelocity() - UPPER_FLYWHEEL_VELOCITY) <= FLYWHEEL_VELOCITY_TOLERANCE) { + waitState = WaitState.TRANSFER; + } + } + double transferPosition = TRANSFER_INACTIVE_POSITION; + if (waitState == WaitState.TRANSFER) { + if (transferTimer == null) { + transferTimer = new ElapsedTime(); + } + if (transferTimer.seconds() <= TRANSFER_TIME_UP) { + transferPosition = TRANSFER_ACTIVE_POSITION; + } + if (transferTimer.seconds() >= TRANSFER_TIME_TOTAL) { + waitState = WaitState.IDLE; + transferTimer = null; + } + } servoDrum.setPosition(hwDrumPosition); servoTransfer.setPosition(transferPosition); motLLauncher.setVelocity(LOWER_FLYWHEEL_VELOCITY); From 486eeb6d48826d0f04fd40fadf88de74983f7e74 Mon Sep 17 00:00:00 2001 From: Andrew Date: Sun, 16 Nov 2025 10:16:06 -0800 Subject: [PATCH 087/198] Update to latest version of Wily Works. --- .../robotcore/hardware/AnalogInput.java | 37 ++ .../com/qualcomm/robotcore/hardware/LED.java | 3 +- .../com/wilyworks/simulator/WilyCore.java | 8 +- .../wilyworks/simulator/framework/Field.java | 104 +-- .../simulator/framework/MechSim.java | 601 ++++++++++++++++++ .../simulator/framework/WilyDcMotorEx.java | 24 +- .../framework/WilyHardwareDevice.java | 6 +- .../simulator/framework/WilyHardwareMap.java | 82 ++- .../ftc/team417/ComplexMechGlob.java | 1 - 9 files changed, 738 insertions(+), 128 deletions(-) create mode 100644 WilyCore/src/main/java/com/qualcomm/robotcore/hardware/AnalogInput.java create mode 100644 WilyCore/src/main/java/com/wilyworks/simulator/framework/MechSim.java diff --git a/WilyCore/src/main/java/com/qualcomm/robotcore/hardware/AnalogInput.java b/WilyCore/src/main/java/com/qualcomm/robotcore/hardware/AnalogInput.java new file mode 100644 index 000000000000..f20a1a6e6cf0 --- /dev/null +++ b/WilyCore/src/main/java/com/qualcomm/robotcore/hardware/AnalogInput.java @@ -0,0 +1,37 @@ +package com.qualcomm.robotcore.hardware; + +import com.qualcomm.robotcore.hardware.configuration.annotations.DeviceProperties; + +/** + * Control a single analog device + */ +@DeviceProperties(name = "@string/configTypeAnalogInput", xmlTag = "AnalogInput", builtIn = true) +public class AnalogInput implements HardwareDevice { + public AnalogInput(String deviceName) { } + + @Override public Manufacturer getManufacturer() { return Manufacturer.Other; } + + public double getVoltage() { return 0; } + + public double getMaxVoltage() { return 0; } + + @Override + public String getDeviceName() { return ""; } + + @Override + public String getConnectionInfo() { return ""; } + + @Override + public int getVersion() { + return 1; + } + + @Override + public void resetDeviceConfigurationForOpMode() { + } + + @Override + public void close() { + // take no action + } +} diff --git a/WilyCore/src/main/java/com/qualcomm/robotcore/hardware/LED.java b/WilyCore/src/main/java/com/qualcomm/robotcore/hardware/LED.java index d9fe88193759..32e692391761 100644 --- a/WilyCore/src/main/java/com/qualcomm/robotcore/hardware/LED.java +++ b/WilyCore/src/main/java/com/qualcomm/robotcore/hardware/LED.java @@ -34,10 +34,11 @@ @DeviceProperties(name = "@string/configTypeLED", xmlTag = "Led", builtIn = true, description = "@string/led_description") public class LED implements HardwareDevice, SwitchableLight { + public LED(String deviceName) { } @Override public Manufacturer getManufacturer() { - return null; + return Manufacturer.Other; } @Override diff --git a/WilyCore/src/main/java/com/wilyworks/simulator/WilyCore.java b/WilyCore/src/main/java/com/wilyworks/simulator/WilyCore.java index 49297f3ab5ac..0f9027b9246e 100644 --- a/WilyCore/src/main/java/com/wilyworks/simulator/WilyCore.java +++ b/WilyCore/src/main/java/com/wilyworks/simulator/WilyCore.java @@ -14,6 +14,7 @@ import com.qualcomm.robotcore.hardware.HardwareMap; import com.wilyworks.common.Wily; import com.wilyworks.common.WilyWorks; +import com.wilyworks.simulator.framework.MechSim; import com.wilyworks.simulator.framework.InputManager; import com.wilyworks.simulator.framework.Simulation; import com.wilyworks.simulator.framework.WilyTelemetry; @@ -30,8 +31,6 @@ import java.awt.Dimension; import java.awt.Graphics2D; import java.awt.Image; -import java.awt.event.ItemEvent; -import java.awt.event.ItemListener; import java.awt.event.WindowAdapter; import java.awt.event.WindowEvent; import java.awt.image.BufferStrategy; @@ -262,6 +261,7 @@ public class WilyCore { public static Gamepad gamepad1; public static Gamepad gamepad2; public static HardwareMap hardwareMap; + public static MechSim mechSim; public static InputManager inputManager; public static DashboardWindow dashboardWindow; public static Telemetry telemetry; @@ -344,6 +344,7 @@ static public void updateSimulation(double deltaT) { // Advance the time then advance the simulation: deltaT = advanceTime(deltaT); simulation.advance(deltaT); + mechSim.advance(deltaT); // Render everything: render(); @@ -489,6 +490,9 @@ static void runOpMode(Class klass) { throw new RuntimeException(e); } + // Create this year's game simulation: + mechSim = MechSim.create(); + // We need to re-instantiate hardware map on every run: hardwareMap = new HardwareMap(); diff --git a/WilyCore/src/main/java/com/wilyworks/simulator/framework/Field.java b/WilyCore/src/main/java/com/wilyworks/simulator/framework/Field.java index d0a11b5d0759..ecf9230946e6 100644 --- a/WilyCore/src/main/java/com/wilyworks/simulator/framework/Field.java +++ b/WilyCore/src/main/java/com/wilyworks/simulator/framework/Field.java @@ -6,9 +6,6 @@ import android.annotation.SuppressLint; import com.acmerobotics.dashboard.FtcDashboard; -import com.acmerobotics.roadrunner.Pose2d; -import com.qualcomm.robotcore.hardware.HardwareMap; -import com.qualcomm.robotcore.hardware.LED; import com.wilyworks.common.WilyWorks; import com.wilyworks.simulator.WilyCore; import com.wilyworks.simulator.helpers.Point; @@ -19,19 +16,14 @@ import java.awt.GraphicsConfiguration; import java.awt.GraphicsEnvironment; import java.awt.Image; -import java.awt.Polygon; import java.awt.Rectangle; import java.awt.RenderingHints; -import java.awt.Shape; import java.awt.Transparency; import java.awt.geom.AffineTransform; -import java.awt.geom.Ellipse2D; import java.awt.geom.GeneralPath; -import java.awt.geom.Rectangle2D; import java.awt.image.BufferedImage; import java.io.IOException; import java.io.InputStream; -import java.util.ArrayList; import javax.imageio.ImageIO; @@ -133,38 +125,9 @@ private void initializeRobotImage() { round(ROBOT_IMAGE_HEIGHT * DIRECTION_LINE_HEIGHT)); } - // Render the outline of the robot's true position: - void renderRobot(Graphics2D g) { - Pose2d pose = simulation.getPose(0, true); - AffineTransform oldTransform = g.getTransform(); - g.translate(pose.position.x, pose.position.y); - g.rotate(pose.heading.log()); - g.setColor(Color.RED); - setAlpha(g, 0.5); - - g.draw(new Rectangle2D.Double( - -WilyCore.config.robotWidth / 2.0, -WilyCore.config.robotLength / 2.0, - WilyCore.config.robotWidth, WilyCore.config.robotLength)); - - // Restore the graphics state: - setAlpha(g, 1.0); - g.setTransform(oldTransform); - -// Pose2d simulationPose = simulation.getPose(0, true); -// AffineTransform imageTransform = new AffineTransform(); -// imageTransform.translate(simulationPose.position.x, simulationPose.position.y); -// imageTransform.scale(1.0 / ROBOT_IMAGE_WIDTH,1.0 / ROBOT_IMAGE_HEIGHT); -// imageTransform.rotate(simulationPose.heading.log() + Math.toRadians(90)); -// imageTransform.scale(WilyCore.config.robotWidth, WilyCore.config.robotLength); -// imageTransform.translate(-ROBOT_IMAGE_HEIGHT / 2.0, -ROBOT_IMAGE_HEIGHT / 2.0); -// setAlpha(g, 0.5); -// g.drawImage(robotImage, imageTransform, null); -// setAlpha(g, 1.0); - } - // Set the transform to use inches and have the origin at the center of field. This // returns the current transform to restore via Graphics2D.setTransform() once done: - public static AffineTransform setFieldTransform(Graphics2D g) { + public static AffineTransform setFieldViewportAndClip(Graphics2D g) { // Prime the viewport/transform and the clipping for field and overlay rendering: AffineTransform oldTransform = g.getTransform(); g.setClip(FIELD_VIEW.x, FIELD_VIEW.y, FIELD_VIEW.width, FIELD_VIEW.height); @@ -187,55 +150,6 @@ public static AffineTransform setPageFrameTransform(Graphics2D g) { return oldTransform; } - // Render the LED for REV Digital LEDs: - void renderLeds(Graphics2D g) { - HardwareMap hardwareMap = WilyCore.hardwareMap; - if (hardwareMap == null) - return; // Might not have been created yet - - final int[] colors = { 0, 0xff0000, 0x00ff00, 0xffbf00 }; // black, red, green, amber - final double radius = 2.0; // Circle radius, in inches - Pose2d pose = simulation.getPose(0, true); - - ArrayList ledArray = new ArrayList<>(); - for (LED led: hardwareMap.led) { - ledArray.add((WilyLED) led); - } - for (int i = 0; i < ledArray.size(); i++) { - WilyLED led = ledArray.get(i); - int colorIndex = 0; - colorIndex |= (led.isRed && led.enable) ? 1 : 0; - colorIndex |= (!led.isRed && led.enable) ? 2 : 0; - - // The LED actually needs two digital channels to describe all 4 possible colors. - // Assume that consecutively registered channels make a pair: - if (i + 1 < ledArray.size()) { - WilyLED nextLed = ledArray.get(i + 1); - if ((nextLed.x == led.x) && - (nextLed.y == led.y) && - (nextLed.isRed == !led.isRed)) { - - colorIndex |= (nextLed.isRed && nextLed.enable) ? 1 : 0; - colorIndex |= (!nextLed.isRed && nextLed.enable) ? 2 : 0; - i++; - } - } - - // Draw the circle at the location of the sensor on the robot, accounting for its - // current heading: - Point point = new Point(led.x, led.y) - .rotate(pose.heading.log()) - .add(new Point(pose.position)); - g.setRenderingHint(RenderingHints.KEY_ANTIALIASING, RenderingHints.VALUE_ANTIALIAS_ON); - - g.setColor(new Color(0xffffff)); - g.fill(new Ellipse2D.Double(point.x - radius - 0.5, point.y - radius - 0.5,2 * radius + 1, 2 * radius + 1)); - g.setColor(new Color(colors[colorIndex])); - g.fill(new Ellipse2D.Double(point.x - radius, point.y - radius,2 * radius, 2 * radius)); - g.setRenderingHint(RenderingHints.KEY_ANTIALIASING, RenderingHints.VALUE_ANTIALIAS_OFF); - } - } - // Render the field, the robot, and the field overlay: @SuppressLint("DefaultLocale") public void render(Graphics2D g, String caption) { @@ -246,13 +160,15 @@ public void render(Graphics2D g, String caption) { // Lay down the background image without needing a transform: g.drawImage(backgroundImage, FIELD_VIEW.x + FIELD_INSET, FIELD_VIEW.y + FIELD_INSET, null); - defaultTransform = g.getTransform(); - AffineTransform oldTransform = setFieldTransform(g); - if (FtcDashboard.fieldOverlay != null) + defaultTransform = setFieldViewportAndClip(g); + if (FtcDashboard.fieldOverlay != null) // Can't remember why this could be null FtcDashboard.fieldOverlay.render(g); - renderRobot(g); - renderLeds(g); - g.setTransform(oldTransform); + + // Render the robot via the appropriate MechSim: + if (WilyCore.mechSim != null) + WilyCore.mechSim.update(g, simulation.getPose(0, true)); + + g.setTransform(defaultTransform); } // Set the global alpha in the range [0.0, 1.0]: @@ -282,7 +198,7 @@ void renderFieldOfView(Graphics2D g, Point origin, double orientation, double fo public void renderStartScreenOverlay(Graphics2D g) { g.drawImage(compassImage, 20, 20, null); - AffineTransform oldTransform = setFieldTransform(g); + AffineTransform oldTransform = setFieldViewportAndClip(g); for (WilyWorks.Config.Camera camera: WilyCore.config.cameras) { g.setColor(new Color(0xffffff)); renderFieldOfView(g, new Point(camera.x, camera.y), camera.orientation, camera.fieldOfView); diff --git a/WilyCore/src/main/java/com/wilyworks/simulator/framework/MechSim.java b/WilyCore/src/main/java/com/wilyworks/simulator/framework/MechSim.java new file mode 100644 index 000000000000..2c6ace9ed384 --- /dev/null +++ b/WilyCore/src/main/java/com/wilyworks/simulator/framework/MechSim.java @@ -0,0 +1,601 @@ +package com.wilyworks.simulator.framework; + +import static com.wilyworks.simulator.WilyCore.time; + +import com.acmerobotics.roadrunner.Pose2d; +import com.qualcomm.robotcore.hardware.CRServo; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.HardwareDevice; +import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.hardware.LED; +import com.qualcomm.robotcore.hardware.NormalizedRGBA; +import com.qualcomm.robotcore.hardware.Servo; +import com.wilyworks.simulator.WilyCore; +import com.wilyworks.simulator.helpers.Point; + +import java.awt.AlphaComposite; +import java.awt.BasicStroke; +import java.awt.Color; +import java.awt.Graphics2D; +import java.awt.RenderingHints; +import java.awt.geom.AffineTransform; +import java.awt.geom.Arc2D; +import java.awt.geom.Ellipse2D; +import java.awt.geom.Rectangle2D; +import java.util.ArrayList; +import java.util.Collections; +import java.util.Iterator; +import java.util.LinkedList; +import java.util.List; +import java.util.stream.Collectors; +import java.util.stream.IntStream; + +// This is the base class for game simulations for particular years and robots. +public class MechSim { + // Create the game simulation appropriate to this year. + static public MechSim create() { + return new DecodeSlowBotMechSim(); + } + + // Hook the creation of a particular device. + public HardwareDevice hookDevice(String deviceName, HardwareDevice original) { + return original; + } + + // Run the simulation and render the mechanisms. + public void update(Graphics2D g, Pose2d pose) { + renderRobotBox(g, pose); + renderLeds(g, pose); + } + + // Advance the simulation time. + public void advance(double deltaT) {} + + // Render the robot box. + protected void renderRobotBox(Graphics2D g, Pose2d pose) { + AffineTransform originalTransform = g.getTransform(); + g.translate(pose.position.x, pose.position.y); + g.rotate(pose.heading.log()); + g.setColor(Color.RED); + g.setComposite(AlphaComposite.getInstance(AlphaComposite.SRC_OVER, (float) 0.5)); + // Draw a rectangle for the true position: + g.draw(new Rectangle2D.Double( + -WilyCore.config.robotWidth / 2.0, -WilyCore.config.robotLength / 2.0, + WilyCore.config.robotWidth, WilyCore.config.robotLength)); + g.setComposite(AlphaComposite.getInstance(AlphaComposite.SRC_OVER, (float) 1.0)); + g.setTransform(originalTransform); + } + + // Render the LED for REV Digital LEDs. + protected void renderLeds(Graphics2D g, Pose2d pose) { + HardwareMap hardwareMap = WilyCore.hardwareMap; + if (hardwareMap == null) + return; // Might not have been created yet + + final int[] colors = { 0, 0xff0000, 0x00ff00, 0xffbf00 }; // black, red, green, amber + final double radius = 2.0; // Circle radius, in inches + + ArrayList ledArray = new ArrayList<>(); + for (LED led: hardwareMap.led) { + ledArray.add((WilyLED) led); + } + for (int i = 0; i < ledArray.size(); i++) { + WilyLED led = ledArray.get(i); + int colorIndex = 0; + colorIndex |= (led.isRed && led.enable) ? 1 : 0; + colorIndex |= (!led.isRed && led.enable) ? 2 : 0; + + // The LED actually needs two digital channels to describe all 4 possible colors. + // Assume that consecutively registered channels make a pair: + if (i + 1 < ledArray.size()) { + WilyLED nextLed = ledArray.get(i + 1); + if ((nextLed.x == led.x) && + (nextLed.y == led.y) && + (nextLed.isRed == !led.isRed)) { + + colorIndex |= (nextLed.isRed && nextLed.enable) ? 1 : 0; + colorIndex |= (!nextLed.isRed && nextLed.enable) ? 2 : 0; + i++; + } + } + + // Draw the circle at the location of the sensor on the robot, accounting for its + // current heading: + Point point = new Point(led.x, led.y) + .rotate(pose.heading.log()) + .add(new Point(pose.position)); + g.setRenderingHint(RenderingHints.KEY_ANTIALIASING, RenderingHints.VALUE_ANTIALIAS_ON); + + g.setColor(new Color(0xffffff)); + g.fill(new Ellipse2D.Double(point.x - radius - 0.5, point.y - radius - 0.5,2 * radius + 1, 2 * radius + 1)); + g.setColor(new Color(colors[colorIndex])); + g.fill(new Ellipse2D.Double(point.x - radius, point.y - radius,2 * radius, 2 * radius)); + g.setRenderingHint(RenderingHints.KEY_ANTIALIASING, RenderingHints.VALUE_ANTIALIAS_OFF); + } + } + +} + +// Hooked class for measuring the position of the drum: +class DrumAnalogInput extends WilyAnalogInput { + DecodeSlowBotMechSim mechSim; + DrumAnalogInput(String deviceName, DecodeSlowBotMechSim mechSim) { + super(deviceName); + this.mechSim = mechSim; + } + + // Return a voltage that is proportional to the drum location, with some variation: + @Override + public double getVoltage() { + double variation = -0.1 + Math.random() * 0.2; // random() generates numbers between 0 and 1 + return 3.5 * mechSim.actualDrumPosition + variation; + } +} + +// Hooked class for determining the color of the ball once it's in the drum: +class DrumColorSensor extends WilyNormalizedColorSensor { + DecodeSlowBotMechSim mechSim; + int idMask; // Sensor 0 or 1 + DrumColorSensor(String deviceName, DecodeSlowBotMechSim mechSim, int index) { + super(deviceName); + this.mechSim = mechSim; + this.idMask = 1 << index; + } + public NormalizedRGBA getNormalizedColors() { + // Every time we get a new ball, reset our variations: + if (mechSim.colorSensorMask == -1) { + mechSim.colorSensorMask = 1 + (int)(Math.random() * 3.0); // Mask = 1, 2 or 3 + } + NormalizedRGBA color = new NormalizedRGBA(); + + // Simulate the ball holes for some reads: + if ((mechSim.colorSensorMask & idMask) != 0) { + // Figure out what slot is being input into, if any: + int slot = mechSim.findDrumSlot(mechSim.INTAKE_POSITIONS); + if (slot != -1) { + DecodeSlowBotMechSim.Ball ball = mechSim.slotBalls.get(slot); + if (ball != null) { + if (ball.color == DecodeSlowBotMechSim.BallColor.GREEN) { + color.green = 1; + } else { + color.red = 0.5f; + color.blue = 0.5f; + } + } + } + } + return color; + } +} + +// Let us ramp up the launcher motor velocity. +class LaunchMotor extends WilyDcMotorEx { + LaunchMotor(String deviceName) { super(deviceName); } + double targetVelocity; + double actualVelocity; + + @Override + public void setVelocity(double angularRate) { + targetVelocity = angularRate; + } + @Override + public double getVelocity() { + return actualVelocity; + } +} + +// Simulation for the SlowBot in the 2025/2026 Decode game. +class DecodeSlowBotMechSim extends MechSim { + enum BallColor {PURPLE, GREEN} + + final double WIDTH = 18; // Robot width + final double HEIGHT = 18; // Robot height + final double BALL_SIZE = 5; // 5 inches in diameter + final double DRUM_SERVO_SPEED = 1.0 / 0.9; // Speed of the drum servo, position/s + final Point[] INTAKE_OFFSETS = { new Point(8, -1), new Point(8, +1) }; + final double INTAKE_EPSILON = 2.5; // Epsilon for intake distance + final double INTAKE_POWER = 0.1; // Minimum power for intake + final double[] INTAKE_POSITIONS = { 0.0/6, 2.0/6, 4.0/6 }; // AKA 'launch' positions + final double[] TRANSFER_POSITIONS = { 3.0/6, 5.0/6, 1.0/6 }; // Servo positions for intaking + final double SLOT_EPSILON = 0.02; // Epsilon for determining a slot relative to a [0, 1] range + final double MIN_TRANSFER_TIME = 0.1; // Second it takes for a transfer + final double MIN_TRANSFER_POSITION = 0.1; // Minimum position to start a transfer + final double TRANSFER_SERVO_SPEED = (60.0 / 360) / 0.25; // Speed of a goBilda torque servo, position/s + final double LAUNCH_SPEED = 144; // Ball launch speed, inches per second + final Point LAUNCH_OFFSET = new Point(-4, 0); + final double FEEDER_POWER = 0.1; // Minimum power for the feeder servo + final double GOAL_EPSILON = 12; // Distance from goal center to consider a goal + final Point[] GOAL_CENTERS = { new Point(-72, 72), new Point(-72, -72) }; + final Point[] CLASSIFIER_STARTS = { new Point(-BALL_SIZE/2, 72-BALL_SIZE/2), + new Point(-BALL_SIZE/2, -72-BALL_SIZE/2) }; // Start locations, corresponding to goals + final double LAUNCH_ACCELERATION = 1000; // Increase flywheel speed by this many ticks per second + final double LAUNCH_DROP = 500; // Drop flywheel speed by this many ticks on launch + final double LAUNCH_EPSILON = 50; // Target and actual flywheel velocities must be within this amount + + // Struct for tracking ball locations: + static class Ball { + BallColor color; // GREEN or PURPLE + Point point; // Current location; not relevant if in robot mechanisms + Point velocity; // Velocity vector; null for balls lying on the field + public Ball(BallColor color, double x, double y) { + this.color = color; + this.point = new Point(x, y); + } + } + + // These preset balls are mirrored in y the constructor: + Ball[] ballPresets = { + new Ball(BallColor.GREEN, -12, 53), + new Ball(BallColor.PURPLE, -12, 48), + new Ball(BallColor.PURPLE, -12, 43), + new Ball(BallColor.PURPLE, 12, 53), + new Ball(BallColor.GREEN, 12, 48), + new Ball(BallColor.PURPLE, 12, 43), + new Ball(BallColor.PURPLE, 36, 53), + new Ball(BallColor.PURPLE, 36, 48), + new Ball(BallColor.GREEN, 36, 43), + new Ball(BallColor.PURPLE, 69.5, 69.5), + new Ball(BallColor.GREEN, 64.5, 69.5), + new Ball(BallColor.PURPLE, 59.5, 69.5) + }; + Ball[] ballPreloads = { + new Ball(BallColor.GREEN, 0, 0), + new Ball(BallColor.PURPLE, 0, 0), + new Ball(BallColor.PURPLE, 0, 0) + }; + + // Hooked devices: + LaunchMotor upperLaunchMotor; + LaunchMotor lowerLaunchMotor; + DcMotorEx intakeMotor; + Servo drumServo; + Servo transferServo; + CRServo forwardFeederServo; + CRServo backwardFeederServo; + + // State: + double accumulatedDeltaT; + Ball intakeBall; // Ball in the intake, may be null + // List slotBalls = new ArrayList<>(Arrays.asList(ballPreloads)); // Collections.nCopies(3, null)); + List slotBalls = new ArrayList<>(Collections.nCopies(3, null)); + List airBalls = new LinkedList<>(); // Balls flying through the air + List fieldBalls = new LinkedList<>(); // Pickable balls on the field + List> classifierBalls = IntStream.range(0, 2) + .mapToObj(i -> new LinkedList()).collect(Collectors.toList()); + double actualDrumPosition; // Current location of the drum, [0, 1] + double actualTransferPosition; // Current transfer servo position, [0, 1] + double transferStartTime; // Time that a transfer started, zero when not transferring + int colorSensorMask = -1; // Random 2-bit mask indicating which sensors return true data; -1 if reset + + // Initialize the beast. + DecodeSlowBotMechSim() { + // Add the presets to the field, along with the mirrored-in y versions: + for (Ball ball: ballPresets) { + fieldBalls.add(ball); + fieldBalls.add(new Ball(ball.color, ball.point.x, -ball.point.y)); + } + } + + // Compute the draw color from the ball object. + private Color ballColor(Ball ball) { + if (ball == null) + return Color.BLACK; + else if (ball.color == BallColor.PURPLE) + return new Color(128, 0, 128); + else + return Color.GREEN; + } + + // WilyHardwareMap calls this method when it creates a device, allowing us to substitute + // with a different device object. + @Override + public HardwareDevice hookDevice(String name, HardwareDevice device) { + // These are input-only devices: + if (name.equals("motIntake")) { + intakeMotor = (DcMotorEx) device; + } + if (name.equals("servoDrum")) { + drumServo = (Servo) device; + } + if (name.equals("servoTransfer")) { + transferServo = (Servo) device; + } + if (name.equals("servoFLaunchFeeder")) { + forwardFeederServo = (CRServo) device; + } + if (name.equals("servoBLaunchFeeder")) { + backwardFeederServo = (CRServo) device; + } + + // There have outputs: + if (name.equals("motULauncher")) { + device = upperLaunchMotor = new LaunchMotor(device.getDeviceName()); + } + if (name.equals("motLLauncher")) { + device = lowerLaunchMotor = new LaunchMotor(device.getDeviceName()); + } + if (name.equals("analogDrum")) { + device = new DrumAnalogInput(device.getDeviceName(), this); + } + if (name.equals("sensorColor1")) { + device = new DrumColorSensor(device.getDeviceName(), this, 0); + } + if (name.equals("sensorColor2")) { + device = new DrumColorSensor(device.getDeviceName(), this, 1); + } + return device; + } + + // Check to see if the caller created all of the expected state. + void verifyState() { + if (upperLaunchMotor == null) + throw new RuntimeException("Missing upper launch motor"); + if (lowerLaunchMotor == null) + throw new RuntimeException("Missing lower launch motor"); + if (intakeMotor == null) + throw new RuntimeException("Missing intake motor"); + if (drumServo == null) + throw new RuntimeException("Missing drum servo"); + if (transferServo == null) + throw new RuntimeException("Missing transfer servo"); + if (forwardFeederServo == null) + throw new RuntimeException("Missing forward feeder servo"); + if (backwardFeederServo == null) + throw new RuntimeException("Missing backward feeder servo"); + } + + void render(Graphics2D g, Pose2d pose) { + // Draw the balls on the field: + for (Ball ball : fieldBalls) { + g.setColor(ballColor(ball)); + g.fill(ballEllipse(ball.point.x, ball.point.y)); + } + + // Draw the classifier balls: + for (int i = 0; i < 2; i++) { + double x = CLASSIFIER_STARTS[i].x; + double y = CLASSIFIER_STARTS[i].y; + int count = 0; + for (Ball ball: classifierBalls.get(i)) { + g.setColor(ballColor(ball)); + g.fill(ballEllipse(x, y)); + x -= BALL_SIZE; + count++; + if (count >= 9) + break; // Classifier maxes out at 9 balls, more causes an overflow + } + } + + AffineTransform fieldTransform = g.getTransform(); + + // Set the transform to draw the robots in the canonical space, from (-1, -1) to (1, 1): + g.translate(pose.position.x, pose.position.y); + g.rotate(pose.heading.log()); + g.scale(WIDTH / 2, HEIGHT / 2); + + // Draw the robot outline: + g.setStroke(new BasicStroke(0.1f)); + g.setColor(Color.DARK_GRAY); + g.fill(new Rectangle2D.Double(-1, -1, 2, 2)); + + // Draw the intake wheels: + Color intakeColor = Color.GRAY; + if (intakeMotor.getPower() > 0) { + intakeColor = Color.GREEN; // Green when intaking + } else if (intakeMotor.getPower() < 0) { + intakeColor = Color.RED; // Red when spitting out + } + g.setColor(intakeColor); + final int INTAKE_WHEEL_COUNT = 4; + final double INTAKE_HEIGHT = 0.2; + final double INTAKE_SPACE = (1.6 - INTAKE_WHEEL_COUNT * INTAKE_HEIGHT) / (INTAKE_WHEEL_COUNT - 1); + for (int i = 1; i < 3; i++) { // Just draw the middle two wheels + double y = -0.8 + i * (INTAKE_HEIGHT + INTAKE_SPACE); + g.fill(new Rectangle2D.Double(0.6, y, 0.4, INTAKE_HEIGHT)); + } + + // Draw the drum: + g.setColor(Color.GRAY); + g.translate(-0.33, 0); + g.rotate(-2 * Math.PI * actualDrumPosition); // Note the negative + g.fill(new Ellipse2D.Double(-0.8, -0.8, 1.6, 1.6)); + + // Draw the dead wedge area: + g.setColor(Color.DARK_GRAY); + g.fill(new Arc2D.Double(-0.8, -0.8, 1.6, 1.6, 0.0f, -60, Arc2D.PIE)); + + // Draw each of the drum slot circles: + for (Ball ball: slotBalls) { + g.setColor(ballColor(ball)); + g.fill(new Ellipse2D.Double(0.1, -0.25, 0.6, 0.6)); + g.rotate(Math.toRadians(120)); + } + + // Draw the air balls: + g.setTransform(fieldTransform); + for (Ball ball : airBalls) { + g.setColor(ballColor(ball)); + g.fill(ballEllipse(ball.point.x, ball.point.y)); + } + + // Draw the intake ball, if there is one: + if (intakeBall != null) { + g.translate(pose.position.x, pose.position.y); + g.rotate(pose.heading.log()); + g.setColor(ballColor(intakeBall)); + g.fill(ballEllipse(7, 0)); + } + } + + // Shortcut to create an ellipse representing a ball, centered around the specified point. + Ellipse2D ballEllipse(double x, double y) { + return new Ellipse2D.Double(x - BALL_SIZE/2, y - BALL_SIZE/2, BALL_SIZE, BALL_SIZE); + } + + + // Find the slot that corresponds to the current drum angle. Returns -1 if not found. + int findDrumSlot(double[] slotPositions) { + for (int i = 0; i < 3; i++) { + if (Math.abs(actualDrumPosition - slotPositions[i]) < SLOT_EPSILON) { + return i; + } + } + return -1; + } + + // Advance the simulation: + void simulate(Pose2d pose, double deltaT) { + verifyState(); + + double heading = pose.heading.log(); + // Advance the balls flying through the air: + Iterator ballIterator = airBalls.iterator(); + while (ballIterator.hasNext()) { + Ball ball = ballIterator.next(); + // Move the ball: + ball.point = ball.point.add(ball.velocity.multiply(deltaT)); + + // See if it's scored a goal: + for (int i = 0; i < 2; i++) { // Goal index + if (Math.hypot(GOAL_CENTERS[i].x - ball.point.x, + GOAL_CENTERS[i].y - ball.point.y) < GOAL_EPSILON) { + ballIterator.remove(); + classifierBalls.get(i).add(ball); + } + } + } + + // Ramp up the launcher motors velocities: + double upperDiff = upperLaunchMotor.targetVelocity - upperLaunchMotor.actualVelocity; + double lowerDiff = lowerLaunchMotor.targetVelocity - lowerLaunchMotor.actualVelocity; + upperLaunchMotor.actualVelocity += Math.signum(upperDiff) * Math.min(Math.abs(upperDiff), deltaT * LAUNCH_ACCELERATION); + lowerLaunchMotor.actualVelocity += Math.signum(lowerDiff) * Math.min(Math.abs(lowerDiff), deltaT * LAUNCH_ACCELERATION); + + // Move the transfer servo towards the target angle: + double targetTransferPosition = transferServo.getPosition(); + double transferDiff = targetTransferPosition - actualTransferPosition; + actualTransferPosition += Math.signum(transferDiff) * Math.min(Math.abs(transferDiff), deltaT * TRANSFER_SERVO_SPEED); + + // Move the drum towards the target angle: + double targetDrumPosition = drumServo.getPosition(); + double drumDiff = targetDrumPosition - actualDrumPosition; + actualDrumPosition += Math.signum(drumDiff) * Math.min(Math.abs(drumDiff), deltaT * DRUM_SERVO_SPEED); + + // Reset the color sensor mask whenever the drum moves: + if (targetDrumPosition != actualDrumPosition) { + colorSensorMask = -1; + } + + // Find the slot if in position to transfer: + int transferSlot = findDrumSlot(TRANSFER_POSITIONS); + + // Handle load requests for the launch calibration app, signaled by running the launchers + // backwards: + if (upperLaunchMotor.getVelocity() < 0) { + if (forwardFeederServo.getPower() >= 0) { + throw new RuntimeException("That's weird, one launch motor runs backwards and the other doesn't?"); + } + if (forwardFeederServo.getPower() > 0) { + throw new RuntimeException("When running launch motors backwards, forward feeder servo must too."); + } + if (backwardFeederServo.getPower() > 0) { + throw new RuntimeException("When running launch motors backwards, backward feeder servo must too."); + } + // If the slot is empty, fill it up with a ball! + if ((transferSlot != -1) && (slotBalls.get(transferSlot) == null)) { + slotBalls.set(transferSlot, new Ball(BallColor.PURPLE, 0, 0)); + } + } + + // Handle transfer requests: + if (actualTransferPosition <= MIN_TRANSFER_POSITION) { + if (transferStartTime != 0) { + throw new RuntimeException("Didn't transfer for sufficient time."); + } + } else { + if (transferSlot == -1) { + throw new RuntimeException("A transfer is requested when drum isn't in the right spot. That will break things!"); + } + if (targetDrumPosition != actualDrumPosition) { + throw new RuntimeException("The drum is moving during a transfer. That will break things!"); + } + if (forwardFeederServo.getPower() < FEEDER_POWER) { + throw new RuntimeException("A transfer is requested when forward feeder servo isn't running. That won't work!"); + } + if (backwardFeederServo.getPower() < FEEDER_POWER) { + throw new RuntimeException("A transfer is requested when backward feeder servo isn't running. That won't work!"); + } + if (slotBalls.get(transferSlot) != null) { + if (transferStartTime == 0) { + transferStartTime = time(); + + // Check these only at the start of the transfer because we're going to + // immediately drop the speeds: + if (Math.abs(upperLaunchMotor.targetVelocity - upperLaunchMotor.actualVelocity) > LAUNCH_EPSILON) { + throw new RuntimeException("A transfer is requested when upper launcher motor isn't running. That won't work!"); + } + if (Math.abs(lowerLaunchMotor.targetVelocity - lowerLaunchMotor.actualVelocity) > LAUNCH_EPSILON) { + throw new RuntimeException("A transfer is requested when lower launcher motor isn't running. That won't work!"); + } + + upperLaunchMotor.actualVelocity -= LAUNCH_DROP; + lowerLaunchMotor.actualVelocity -= LAUNCH_DROP; + } else if (time() - transferStartTime > MIN_TRANSFER_TIME) { + // Transfer the ball from the drum to the air-balls list: + Ball ball = slotBalls.get(transferSlot); + ball.velocity = new Point(LAUNCH_SPEED, 0).rotate(heading); + ball.point = new Point(pose.position).add(LAUNCH_OFFSET.rotate(heading)); + airBalls.add(ball); + slotBalls.set(transferSlot, null); + transferStartTime = 0; + } + } + } + + // Finally, check for intake: + if (intakeMotor.getPower() > INTAKE_POWER) { + if (intakeBall == null) { + // We're intaking from the field into the intake: + for (Point intakeOffset : INTAKE_OFFSETS) { + Point intakePoint = new Point(pose.position).add(intakeOffset.rotate(pose.heading.log())); + for (Ball ball : fieldBalls) { + double distance = Math.hypot(ball.point.x - intakePoint.x, ball.point.y - intakePoint.y); + if (distance < INTAKE_EPSILON) { + intakeBall = ball; + fieldBalls.remove(ball); // I think this is okay if we terminate the loop... + break; + } + } + } + } else { + // We're intaking from the intake into a drum slot: + int slot = findDrumSlot(INTAKE_POSITIONS); + if (slot != -1) { + if (slotBalls.get(slot) == null) { + slotBalls.set(slot, intakeBall); + intakeBall = null; + } + } + } + } + } + + // Advance the Mech simulation. + @Override + public void advance(double deltaT) { + // Note that we don't update our simulation until update() time, when we also know the + // new robot pose. + this.accumulatedDeltaT += deltaT; + } + + // Render the robot and the balls. + @Override + public void update(Graphics2D g, Pose2d pose) { + // Don't call simulate() or render() for things like Configuration Tester. + if ((upperLaunchMotor != null) && (forwardFeederServo.getPower() != 0)) { + simulate(pose, accumulatedDeltaT); + render(g, pose); + } + accumulatedDeltaT = 0; + } +} diff --git a/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyDcMotorEx.java b/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyDcMotorEx.java index 2597af4657ba..5ca9cf0e1afe 100644 --- a/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyDcMotorEx.java +++ b/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyDcMotorEx.java @@ -11,6 +11,11 @@ * Wily Works DcMotorEx implementation. */ public class WilyDcMotorEx extends WilyHardwareDevice implements DcMotorEx { + WilyDcMotorEx(String deviceName) { super(deviceName); } + RunMode mode; + double velocity; + double power; + Direction direction; @Override public DcMotorController getController() { @@ -40,6 +45,7 @@ public boolean getPowerFloat() { @Override public void setTargetPosition(int position) { + } @Override @@ -59,12 +65,12 @@ public int getCurrentPosition() { @Override public void setMode(RunMode mode) { - + this.mode = mode; } @Override public RunMode getMode() { - return null; + return mode; } @Override @@ -84,7 +90,7 @@ public boolean isMotorEnabled() { @Override public void setVelocity(double angularRate) { - + velocity = angularRate; } @Override @@ -94,7 +100,7 @@ public void setVelocity(double angularRate, AngleUnit unit) { @Override public double getVelocity() { - return 0; + return velocity; } @Override @@ -143,18 +149,20 @@ public boolean isOverCurrent() { } @Override - public void setDirection(Direction direction) { } + public void setDirection(Direction direction) { + this.direction = direction; + } @Override public Direction getDirection() { - return null; + return direction; } @Override - public void setPower(double power) { } + public void setPower(double power) { this.power = power; } @Override public double getPower() { - return 0; + return power; } } diff --git a/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyHardwareDevice.java b/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyHardwareDevice.java index e53f8fa883dd..6914649e7da5 100644 --- a/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyHardwareDevice.java +++ b/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyHardwareDevice.java @@ -7,6 +7,10 @@ * Wily Works device subclass implementation. */ public class WilyHardwareDevice implements HardwareDevice { + String deviceName; + WilyHardwareDevice(String deviceName) { + this.deviceName = deviceName; + } @Override public Manufacturer getManufacturer() { return Manufacturer.Unknown; @@ -14,7 +18,7 @@ public Manufacturer getManufacturer() { @Override public String getDeviceName() { - return ""; + return deviceName; } @Override diff --git a/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyHardwareMap.java b/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyHardwareMap.java index 053526501945..e04e3432ea27 100644 --- a/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyHardwareMap.java +++ b/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyHardwareMap.java @@ -7,6 +7,7 @@ import androidx.annotation.Nullable; import com.qualcomm.hardware.sparkfun.SparkFunOTOS; +import com.qualcomm.robotcore.hardware.AnalogInput; import com.qualcomm.robotcore.hardware.CRServo; import com.qualcomm.robotcore.hardware.ColorSensor; import com.qualcomm.robotcore.hardware.DcMotor; @@ -57,6 +58,7 @@ * Wily Works simulated IMU implementation. */ class WilyIMU extends WilyHardwareDevice implements IMU { + WilyIMU(String deviceName) { super(deviceName); } double startYaw; @Override public boolean initialize(Parameters parameters) { @@ -104,6 +106,7 @@ public AngularVelocity getRobotAngularVelocity(AngleUnit angleUnit) { * Wily Works voltage sensor implementation. */ class WilyVoltageSensor extends WilyHardwareDevice implements VoltageSensor { + WilyVoltageSensor(String deviceName) { super(deviceName); } @Override public double getVoltage() { return 13.0; @@ -119,6 +122,7 @@ public String getDeviceName() { * Wily Works distance sensor implementation. */ class WilyDistanceSensor extends WilyHardwareDevice implements DistanceSensor { + WilyDistanceSensor(String deviceName) { super(deviceName); } @Override public double getDistance(DistanceUnit unit) { return unit.fromMm(65535); } // Distance when not responding } @@ -127,6 +131,7 @@ class WilyDistanceSensor extends WilyHardwareDevice implements DistanceSensor { * Wily Works normalized color sensor implementation. */ class WilyNormalizedColorSensor extends WilyHardwareDevice implements NormalizedColorSensor { + WilyNormalizedColorSensor(String deviceName) { super(deviceName); } @Override public NormalizedRGBA getNormalizedColors() { return new NormalizedRGBA(); } @@ -141,6 +146,7 @@ public void setGain(float newGain) { } * Wily Works color sensor implementation. */ class WilyColorSensor extends WilyHardwareDevice implements ColorSensor { + WilyColorSensor(String deviceName) { super(deviceName); } @Override public int red() { return 0; } @@ -174,6 +180,7 @@ class WilyWebcam extends WilyHardwareDevice implements WebcamName { WilyWorks.Config.Camera wilyCamera; WilyWebcam(String deviceName) { + super(deviceName); for (WilyWorks.Config.Camera camera: WilyCore.config.cameras) { if (camera.name.equals(deviceName)) { wilyCamera = camera; @@ -240,6 +247,7 @@ public boolean isAttached() { * Wily Works ServoController implementation. */ class WilyServoController extends WilyHardwareDevice implements ServoController { + WilyServoController(String deviceName) { super(deviceName); } @Override public void pwmEnable() { } @@ -263,11 +271,12 @@ public void close() { } * Wily Works Servo implementation. */ class WilyServo extends WilyHardwareDevice implements Servo { + WilyServo(String deviceName) { super(deviceName); } double position; @Override public ServoController getController() { - return new WilyServoController(); + return new WilyServoController(deviceName); } @Override @@ -301,10 +310,13 @@ public void scaleRange(double min, double max) { * Wily Works CRServo implementation. */ class WilyCRServo extends WilyHardwareDevice implements CRServo { + WilyCRServo(String deviceName) { super(deviceName); } + double power; + Direction direction; @Override public ServoController getController() { - return new WilyServoController(); + return new WilyServoController(deviceName); } @Override @@ -314,22 +326,22 @@ public int getPortNumber() { @Override public void setDirection(Direction direction) { - + this.direction = direction; } @Override public Direction getDirection() { - return null; + return direction; } @Override public void setPower(double power) { - + this.power = power; } @Override public double getPower() { - return 0; + return power; } } @@ -337,6 +349,7 @@ public double getPower() { * Wily Works DigitalChannel implementation. */ class WilyDigitalChannel extends WilyHardwareDevice implements DigitalChannel { + WilyDigitalChannel(String deviceName) { super(deviceName); } boolean state; @Override @@ -363,6 +376,7 @@ class WilyLED extends LED { double y; boolean isRed; WilyLED(String deviceName) { + super(deviceName); WilyWorks.Config.LEDIndicator wilyLed = null; for (WilyWorks.Config.LEDIndicator led: WilyCore.config.ledIndicators) { if (led.name.equals(deviceName)) { @@ -389,6 +403,18 @@ public boolean isLightOn() { } } +/** + * Wily Works AnalogInput implementation. + */ +class WilyAnalogInput extends AnalogInput { + WilyAnalogInput(String deviceName) { super(deviceName); } + @Override + public double getVoltage() { return 0; } + + @Override + public double getMaxVoltage() { return 0; } +} + /** * Wily Works hardware map. */ @@ -407,6 +433,8 @@ public class WilyHardwareMap implements Iterable { public DeviceMapping sparkFunOTOS = new DeviceMapping<>(SparkFunOTOS.class); public DeviceMapping goBildaPinpointDrivers = new DeviceMapping<>(GoBildaPinpointDriver.class); public DeviceMapping ultrasonicDistanceSensor = new DeviceMapping<>(UltrasonicDistanceSensor.class); + public DeviceMapping analogInput = new DeviceMapping<>(AnalogInput.class); + public DeviceMapping imu = new DeviceMapping<>(IMU.class); protected Map> allDevicesMap = new HashMap<>(); protected List allDevicesList = new ArrayList<>(); @@ -436,9 +464,6 @@ public T tryGet(Class classOrInterface, String deviceName) { return get(classOrInterface, deviceName); } public T get(Class classOrInterface, String deviceName) { - if (classOrInterface.equals(IMU.class)) { - return (T) new WilyIMU(); - } T result = wilyTryGet(classOrInterface, deviceName); if (result == null) { // Wily Works behavior is that we automatically add the device if it's not found: @@ -450,9 +475,10 @@ public T get(Class classOrInterface, String deviceName) { @Deprecated public HardwareDevice get(String deviceName) { - List list = allDevicesMap.get(deviceName.trim()); - if (list != null) { - return list.get(0); + for (HardwareDevice device: allDevicesList) { + if (device.getDeviceName().equals(deviceName)) { + return device; + } } throw new IllegalArgumentException("Use the typed version of get(), e.g. get(DcMotorEx.class, \"leftMotor\")"); @@ -468,31 +494,31 @@ public synchronized void put(String deviceName, Class klass) { } HardwareDevice device; if (CRServo.class.isAssignableFrom(klass)) { - device = new WilyCRServo(); + device = new WilyCRServo(deviceName); crservo.put(deviceName, (CRServo) device); } else if (Servo.class.isAssignableFrom(klass)) { - device = new WilyServo(); + device = new WilyServo(deviceName); servo.put(deviceName, (Servo) device); } else if (DcMotor.class.isAssignableFrom(klass)) { - device = new WilyDcMotorEx(); + device = new WilyDcMotorEx(deviceName); dcMotor.put(deviceName, (DcMotor) device); } else if (VoltageSensor.class.isAssignableFrom(klass)) { - device = new WilyVoltageSensor(); + device = new WilyVoltageSensor(deviceName); voltageSensor.put(deviceName, (VoltageSensor) device); } else if (DistanceSensor.class.isAssignableFrom(klass)) { - device = new WilyDistanceSensor(); + device = new WilyDistanceSensor(deviceName); distanceSensor.put(deviceName, (DistanceSensor) device); } else if (NormalizedColorSensor.class.isAssignableFrom(klass)) { - device = new WilyNormalizedColorSensor(); + device = new WilyNormalizedColorSensor(deviceName); normalizedColorSensor.put(deviceName, (NormalizedColorSensor) device); } else if (ColorSensor.class.isAssignableFrom(klass)) { - device = new WilyColorSensor(); + device = new WilyColorSensor(deviceName); colorSensor.put(deviceName, (ColorSensor) device); } else if (WebcamName.class.isAssignableFrom(klass)) { device = new WilyWebcam(deviceName); webcamName.put(deviceName, (WebcamName) device); } else if (DigitalChannel.class.isAssignableFrom(klass)) { - device = new WilyDigitalChannel(); + device = new WilyDigitalChannel(deviceName); digitalChannel.put(deviceName, (DigitalChannel) device); } else if (LED.class.isAssignableFrom(klass)) { device = new WilyLED(deviceName); @@ -506,9 +532,19 @@ public synchronized void put(String deviceName, Class klass) { } else if (UltrasonicDistanceSensor.class.isAssignableFrom(klass)) { device = new WilyUltrasonicDistanceSensor(deviceName); ultrasonicDistanceSensor.put(deviceName, (WilyUltrasonicDistanceSensor) device); + } else if (AnalogInput.class.isAssignableFrom(klass)) { + device = new WilyAnalogInput(deviceName); + analogInput.put(deviceName, (WilyAnalogInput) device); + } else if (IMU.class.isAssignableFrom(klass)) { + device = new WilyIMU(deviceName); + imu.put(deviceName, (WilyIMU) device); } else { throw new IllegalArgumentException("Unexpected device type for HardwareMap"); } + + // Let the game simulation change the behavior of this device: + device = WilyCore.mechSim.hookDevice(deviceName, device); + list.add(device); allDevicesList.add(device); } @@ -524,7 +560,11 @@ private void initializeMultipleDevicesIfNecessary(Iterable getAllNames(Class classOrInterface) { SortedSet result = new TreeSet<>(); - result.add("voltage_sensor"); + for (HardwareDevice device: allDevicesList) { + if (classOrInterface.isInstance(device)) { + result.add(device.getDeviceName()); + } + } return result; } diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java b/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java index e4d7b3ed2b5b..3ae5de2ff6ce 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java @@ -2,7 +2,6 @@ import com.acmerobotics.dashboard.config.Config; -import com.qualcomm.hardware.ams.AMSColorSensor; import com.qualcomm.robotcore.hardware.AnalogInput; import com.qualcomm.robotcore.hardware.CRServo; import com.qualcomm.robotcore.hardware.DcMotor; From 40c02b691a39ef4c216bf3a50481079dfe7b130d Mon Sep 17 00:00:00 2001 From: Team <417skid@swerverobotics.club> Date: Sun, 16 Nov 2025 13:14:14 -0800 Subject: [PATCH 088/198] Auto far out of way start --- .../ftc/team417/CompetitionAuto.java | 24 ++++++++++++++++++- 1 file changed, 23 insertions(+), 1 deletion(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java index c4e2e0113c33..f3e4cac32973 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java @@ -15,6 +15,7 @@ import org.firstinspires.ftc.team417.javatextmenu.MenuSlider; import org.firstinspires.ftc.team417.javatextmenu.TextMenu; import org.firstinspires.ftc.team417.roadrunner.MecanumDrive; +import org.firstinspires.ftc.team417.roadrunner.RobotAction; import java.nio.file.Path; @@ -32,6 +33,7 @@ enum Alliances { enum SlowBotMovements { NEAR, FAR, + FAR_OUT_OF_WAY, FAR_MINIMAL, } @@ -114,7 +116,17 @@ public void runOpMode() { .setTangent(Math.toRadians(90)) .splineToLinearHeading(new Pose2d(48,32,Math.toRadians(180)), Math.toRadians(180)) .build(); - + Action farOutOfWay = pathFactory.actionBuilder(SBFarStartPose) + // 3 launch actions + // after disp intake action + .setTangent(Math.toRadians(180)) + .splineToLinearHeading(new Pose2d(60,61, Math.toRadians(0)), Math.toRadians(0)) + .setTangent(Math.toRadians(-90)) + .splineToLinearHeading(new Pose2d(54,12, Math.toRadians(157.5)), Math.toRadians(-90)) + // 3 launch actions + .setTangent(Math.toRadians(90)) + .splineToLinearHeading(new Pose2d(50,32,Math.toRadians(180)), Math.toRadians(180)) + .build(); PathFactory farSlowBotIntake1 = pathFactory.actionBuilder(SBFarStartPose) @@ -209,6 +221,10 @@ public void runOpMode() { drive.setPose(SBFarStartPose); trajectoryAction = farSlowBot; break; + case FAR_OUT_OF_WAY: + drive.setPose(SBFarStartPose); + trajectoryAction = farOutOfWay; + break; case FAR_MINIMAL: drive.setPose(SBFarStartPose); trajectoryAction = farMinimalSlowBot; @@ -323,3 +339,9 @@ public Action build() { } +class LaunchAction extends RobotAction { + @Override + public boolean run(double elapsedTime) { + return false; + } +} From 35c354c7f407b2792c2bba68da96142312aa0022 Mon Sep 17 00:00:00 2001 From: bharv Date: Sun, 16 Nov 2025 13:33:35 -0800 Subject: [PATCH 089/198] Color detection redone without chatgpt stuff, not really working well yet --- .../ftc/team417/CoolColorDetector.java | 66 ++++++------------- .../ftc/team417/SensorColor.java | 5 +- 2 files changed, 23 insertions(+), 48 deletions(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CoolColorDetector.java b/team417/src/main/java/org/firstinspires/ftc/team417/CoolColorDetector.java index 3aa3e271d33d..2f5f1f12a999 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CoolColorDetector.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CoolColorDetector.java @@ -5,31 +5,41 @@ import com.qualcomm.robotcore.hardware.ColorSensor; import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.NormalizedColorSensor; +import com.qualcomm.robotcore.hardware.NormalizedRGBA; +import android.app.Activity; +import android.graphics.Color; +import android.view.View; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DistanceSensor; +import com.qualcomm.robotcore.hardware.NormalizedColorSensor; +import com.qualcomm.robotcore.hardware.NormalizedRGBA; +import com.qualcomm.robotcore.hardware.SwitchableLight; + +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; import org.firstinspires.ftc.robotcore.external.Telemetry; public class CoolColorDetector { Telemetry telemetry; - private ColorSensor sensor1; - private ColorSensor sensor2; + private NormalizedColorSensor sensor1; + private NormalizedColorSensor sensor2; private float gain = 50f; // adjust for brightness private float[] hsv = new float[3]; public CoolColorDetector(HardwareMap map, Telemetry telemetry) { - sensor1 = map.get(ColorSensor.class, "cs1"); - sensor2 = map.get(ColorSensor.class, "cs2"); + sensor1 = map.get(NormalizedColorSensor.class, "cs1"); + sensor2 = map.get(NormalizedColorSensor.class, "cs2"); this.telemetry = telemetry; } // --- Convert a sensor to ONE PixelColor --- @SuppressLint("DefaultLocale") - private PixelColor detectSingle(ColorSensor sensor) { + private PixelColor detectSingle(NormalizedColorSensor sensor) { // Get raw values - ((NormalizedColorSensor)sensor).setGain(gain); - //Just tried something new with the setGain - float r = sensor.red(); - float g = sensor.green(); - float b = sensor.blue(); - hsv = rgbToHsv((int)r, (int)g, (int)b); + sensor.setGain(gain); + NormalizedRGBA colors = sensor.getNormalizedColors(); + Color.colorToHSV(colors.toColor(), hsv); telemetry.addData("HSV", String.format("{%f, %f, %f}", hsv[0], hsv[1], hsv[2])); float hue = hsv[0]; @@ -48,41 +58,7 @@ else if (hue >= 10 && hue <= 190) { } } - public static float[] rgbToHsv(int r, int g, int b) { - float[] hsv = new float[3]; - - // Normalize R, G, B values to the range 0-1 - float red = r / 255.0f; - float green = g / 255.0f; - float blue = b / 255.0f; - - float cmax = Math.max(red, Math.max(green, blue)); // Maximum of R, G, B - float cmin = Math.min(red, Math.min(green, blue)); // Minimum of R, G, B - float delta = cmax - cmin; // Delta of max and min - - // Calculate Hue (H) - if (delta == 0) { - hsv[0] = 0; // Hue is undefined for achromatic colors (grays) - } else if (cmax == red) { - hsv[0] = (60 * ((green - blue) / delta) + 360) % 360; - } else if (cmax == green) { - hsv[0] = (60 * ((blue - red) / delta) + 120); - } else { // cmax == blue - hsv[0] = (60 * ((red - green) / delta) + 240); - } - - // Calculate Saturation (S) - if (cmax == 0) { - hsv[1] = 0; // Saturation is 0 for black - } else { - hsv[1] = delta / cmax; - } - - // Calculate Value (V) - hsv[2] = cmax; - return hsv; - } // --- Use logic comparing both sensors --- PixelColor detectPixelPosition() { diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/SensorColor.java b/team417/src/main/java/org/firstinspires/ftc/team417/SensorColor.java index ddfcc839bfba..14db3f60215a 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/SensorColor.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/SensorColor.java @@ -71,7 +71,6 @@ * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list */ @TeleOp(name = "Sensor: Color", group = "Sensor") -@Disabled public class SensorColor extends LinearOpMode { /** The colorSensor field will contain a reference to our color sensor hardware object */ @@ -123,7 +122,7 @@ protected void runSample() { // colors will report at or near 1, and you won't be able to determine what color you are // actually looking at. For this reason, it's better to err on the side of a lower gain // (but always greater than or equal to 1). - float gain = 2; + float gain = 20; // Once per loop, we will update this hsvValues array. The first element (0) will contain the // hue, the second element (1) will contain the saturation, and the third element (2) will @@ -139,7 +138,7 @@ protected void runSample() { // Get a reference to our sensor object. It's recommended to use NormalizedColorSensor over // ColorSensor, because NormalizedColorSensor consistently gives values between 0 and 1, while // the values you get from ColorSensor are dependent on the specific sensor you're using. - colorSensor = hardwareMap.get(NormalizedColorSensor.class, "sensor_color"); + colorSensor = hardwareMap.get(NormalizedColorSensor.class, "cs1"); // If possible, turn the light on in the beginning (it might already be on anyway, // we just make sure it is if we can). From e2ac7cdac100bbef7d2d176ec23f50edc16e32d3 Mon Sep 17 00:00:00 2001 From: Hankang Zhou <136044263+nobody-particular@users.noreply.github.com> Date: Sun, 16 Nov 2025 14:37:10 -0800 Subject: [PATCH 090/198] Added comments to AprilTagDetector and improved its telemetry! --- .../ftc/team417/CompetitionAuto.java | 14 +- .../team417/apriltags/AprilTagDetector.java | 169 +++++++++++++----- .../ftc/team417/apriltags/AprilTagTest.java | 4 + 3 files changed, 130 insertions(+), 57 deletions(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java index 24536ec58f26..afd5c581fcd2 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java @@ -42,6 +42,8 @@ enum SlowBotMovement { double minIntakes = 0.0; double maxIntakes = 3.0; + Pattern pattern; + @Override public void runOpMode() { initHardware(); @@ -55,11 +57,6 @@ public void runOpMode() { MecanumDrive drive = new MecanumDrive(hardwareMap, telemetry, gamepad1, startPose); - // Test to make sure the camera is there, and then immediately close the detector object - try (AprilTagDetector detector = new AprilTagDetector()) { - detector.initAprilTag(hardwareMap); - } - TextMenu menu = new TextMenu(); MenuInput menuInput = new MenuInput(MenuInput.InputType.CONTROLLER); @@ -245,12 +242,13 @@ public void runOpMode() { // Assume unknown pattern unless detected otherwise. - Pattern pattern = Pattern.UNKNOWN; + pattern = Pattern.UNKNOWN; // Detect the pattern with the AprilTags from the camera! // Wait for Start to be pressed on the Driver Hub! - try (AprilTagDetector detector = new AprilTagDetector()) { - detector.initAprilTag(hardwareMap); + // (This try-with-resources statement automatically calls detector.close() when it exits + // the try-block.) + try (AprilTagDetector detector = new AprilTagDetector(hardwareMap)) { while (!isStarted() && !isStopRequested()) { Pattern last = detector.detectPattern(chosenAlliance); diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/apriltags/AprilTagDetector.java b/team417/src/main/java/org/firstinspires/ftc/team417/apriltags/AprilTagDetector.java index 7aacde7e97ac..1e5fd2ec4214 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/apriltags/AprilTagDetector.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/apriltags/AprilTagDetector.java @@ -3,7 +3,6 @@ import com.qualcomm.robotcore.hardware.HardwareMap; import org.firstinspires.ftc.robotcore.external.Telemetry; -import org.firstinspires.ftc.robotcore.external.hardware.camera.BuiltinCameraDirection; import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; import org.firstinspires.ftc.team417.CompetitionAuto; import org.firstinspires.ftc.vision.VisionPortal; @@ -15,8 +14,6 @@ import java.util.List; public class AprilTagDetector implements Closeable { - private static final boolean USE_WEBCAM = true; // true for webcam, false for phone camera - /** * The variable to store our instance of the AprilTag processor. */ @@ -27,83 +24,154 @@ public class AprilTagDetector implements Closeable { */ private VisionPortal visionPortal; + /** + * The variable for how long ago the detection last changed. + */ + private double timeOfLastChange = System.nanoTime() / 1_000_000_000.0; + + /** + * The variable for how many detections were and what the ID was in the last cycle. + */ + private int lastDetectionsSize = -1; + private int lastId = -1; + /** * Initialize the AprilTag processor. */ - public void initAprilTag(HardwareMap hardwareMap) { + public AprilTagDetector(HardwareMap hardwareMap) { // Create the AprilTag processor the easy way. aprilTag = AprilTagProcessor.easyCreateWithDefaults(); // Create the vision portal the easy way. - if (USE_WEBCAM) { - visionPortal = VisionPortal.easyCreateWithDefaults( - hardwareMap.get(WebcamName.class, "camera"), aprilTag); - } else { - visionPortal = VisionPortal.easyCreateWithDefaults( - BuiltinCameraDirection.BACK, aprilTag); - } + visionPortal = VisionPortal.easyCreateWithDefaults( + hardwareMap.get(WebcamName.class, "camera"), aprilTag); + } - } // end method initAprilTag() + /** + * Default is no verbosity. + */ + public Pattern detectPatternAndTelemeter(CompetitionAuto.Alliance alliance, Telemetry telemetry) { + return detectPatternAndTelemeter(alliance, telemetry, false); + } /** * Add telemetry about AprilTag detections. */ - public void telemetryAprilTag(Telemetry telemetry) { - List currentDetections = aprilTag.getDetections(); - telemetry.addData("# AprilTags Detected", currentDetections.size()); - - // Step through the list of detections and display info for each one. - for (AprilTagDetection detection : currentDetections) { - if (detection.metadata != null) { - telemetry.addLine(String.format("\n==== (ID %d) %s", detection.id, detection.metadata.name)); - telemetry.addLine(String.format("XYZ %6.1f %6.1f %6.1f (inch)", detection.ftcPose.x, detection.ftcPose.y, detection.ftcPose.z)); - telemetry.addLine(String.format("PRY %6.1f %6.1f %6.1f (deg)", detection.ftcPose.pitch, detection.ftcPose.roll, detection.ftcPose.yaw)); - telemetry.addLine(String.format("RBE %6.1f %6.1f %6.1f (inch, deg, deg)", detection.ftcPose.range, detection.ftcPose.bearing, detection.ftcPose.elevation)); - } else { - telemetry.addLine(String.format("\n==== (ID %d) Unknown", detection.id)); - telemetry.addLine(String.format("Center %6.0f %6.0f (pixels)", detection.center.x, detection.center.y)); - } - } // end for() loop - - // Add "key" information to telemetry - telemetry.addLine("\nkey:\nXYZ = X (Right), Y (Forward), Z (Up) dist."); - telemetry.addLine("PRY = Pitch, Roll & Yaw (XYZ Rotation)"); - telemetry.addLine("RBE = Range, Bearing & Elevation"); + public Pattern detectPatternAndTelemeter(CompetitionAuto.Alliance alliance, Telemetry telemetry, boolean verbose) { + if (verbose) { + List currentDetections = aprilTag.getDetections(); + telemetry.addData("# AprilTags Detected", currentDetections.size()); + + // Step through the list of detections and display info for each one. + for (AprilTagDetection detection : currentDetections) { + if (detection.metadata != null) { + telemetry.addLine(String.format("\n==== (ID %d) %s", detection.id, detection.metadata.name)); + telemetry.addLine(String.format("XYZ %6.1f %6.1f %6.1f (inch)", detection.ftcPose.x, detection.ftcPose.y, detection.ftcPose.z)); + telemetry.addLine(String.format("PRY %6.1f %6.1f %6.1f (deg)", detection.ftcPose.pitch, detection.ftcPose.roll, detection.ftcPose.yaw)); + telemetry.addLine(String.format("RBE %6.1f %6.1f %6.1f (inch, deg, deg)", detection.ftcPose.range, detection.ftcPose.bearing, detection.ftcPose.elevation)); + } else { + telemetry.addLine(String.format("\n==== (ID %d) Unknown", detection.id)); + telemetry.addLine(String.format("Center %6.0f %6.0f (pixels)", detection.center.x, detection.center.y)); + } + } // end for() loop + + // Add "key" information to telemetry + telemetry.addLine("\nkey:\nXYZ = X (Right), Y (Forward), Z (Up) dist."); + telemetry.addLine("PRY = Pitch, Roll & Yaw (XYZ Rotation)"); + telemetry.addLine("RBE = Range, Bearing & Elevation"); + } + + // Get the pattern: + Pattern pattern = detectPattern(alliance); + + String patternDisplay; + + // The `\\u...` are escape sequences for green and purple circle emojis. + // \uD83D\uDFE3 -> Purple circle + // \uD83D\uDFE2 -> Green circle + // \u26AA -> White circle + switch (pattern) { + case PPG: + patternDisplay = "\uD83D\uDFE3\uD83D\uDFE3\uD83D\uDFE2"; + break; + case PGP: + patternDisplay = "\uD83D\uDFE3\uD83D\uDFE2\uD83D\uDFE3"; + break; + case GPP: + patternDisplay = "\uD83D\uDFE2\uD83D\uDFE3\uD83D\uDFE3"; + break; + default: + patternDisplay = "\u26AA\u26AA\u26AA"; + break; + } + + double elapsedTime = timeOfLastChange - (System.nanoTime() / 1_000_000_000.0); + + // Summarize the most important detection info in one line: + switch (alliance) { + case RED: + telemetry.addLine(String.format("%s (%d IDs, leftmost ID = %d for %f sec.)", + patternDisplay, lastDetectionsSize, lastId, timeOfLastChange)); + break; + case BLUE: + telemetry.addLine(String.format("%s (%d IDs, rightmost ID = %d for %f sec.)", + patternDisplay, lastDetectionsSize, lastId, timeOfLastChange)); + break; + } + return pattern; } // end method telemetryAprilTag() - public AprilTagDetection findDetection(CompetitionAuto.Alliance alliance) { + /** + * Detect the correct color pattern and return it. + */ + public Pattern detectPattern(CompetitionAuto.Alliance alliance) { List currentDetections = aprilTag.getDetections(); // Remove all AprilTags that don't have ID 21, 22, or 23 + // (This is because the obelisk only has AprilTags with IDs 21, 22, and 23) + // (The remaining IDs, 20 and 24, are for localization only) currentDetections.removeIf(detection -> detection.id != 21 && detection.id != 22 && detection.id != 23 ); + // AprilTagDetection objects contain the x (right), y (forward), and z (up) distance + // relative to the robot. When we're on the red alliance, we want the leftmost valid + // AprilTag, and when we're on the blue alliance, we want the rightmost valid AprilTag. + // This is because, in our near position, we see two AprilTags on the obelisk: the front + // AprilTag and the side AprilTag closest to our color goal. + // For more information about the info the AprilTagDetection object contains, see this link: + // https://ftc-docs.firstinspires.org/en/latest/apriltag/understanding_apriltag_detection_values/understanding-apriltag-detection-values.html + AprilTagDetection detection = null; switch (alliance) { case RED: - // Return the leftmost detection relative to the robot - // If there are no detections, return null - return currentDetections.stream() + // Set detection to the leftmost (min x) detection relative to the robot + // If there are no detections, set it to null + detection = currentDetections.stream() .min(Comparator.comparingDouble(aprilTagDetection -> aprilTagDetection.ftcPose.x)).orElse(null); + break; case BLUE: - // Return the rightmost detection relative to the robot - // If there are no detections, return null - return currentDetections.stream() + // Set detection to the rightmost (max x) detection relative to the robot + // If there are no detections, set it to null + detection = currentDetections.stream() .max(Comparator.comparingDouble(aprilTagDetection -> aprilTagDetection.ftcPose.x)).orElse(null); } - throw new IllegalArgumentException("Alliance must be red or blue"); - } - - public Pattern detectPattern(CompetitionAuto.Alliance alliance) { - AprilTagDetection detection = findDetection(alliance); - - // Handle the case where no valid AprilTag was found if (detection == null) { + lastId = -1; + timeOfLastChange = System.nanoTime() / 1_000_000_000.0; return Pattern.UNKNOWN; } + int currentDetectionsSize = currentDetections.size(); + + if (lastDetectionsSize != currentDetectionsSize || detection.id != lastId) { + timeOfLastChange = System.nanoTime() / 1_000_000_000.0; + } + + lastDetectionsSize = currentDetectionsSize; + lastId = detection.id; + switch (detection.id) { case 21: return Pattern.GPP; @@ -111,11 +179,14 @@ public Pattern detectPattern(CompetitionAuto.Alliance alliance) { return Pattern.PGP; case 23: return Pattern.PPG; + default: + return Pattern.UNKNOWN; } - - throw new IllegalArgumentException("ID must be 21, 22, or 23"); } + /** + * Release the resources taken up by the vision portal. + */ @Override public void close() { visionPortal.close(); diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/apriltags/AprilTagTest.java b/team417/src/main/java/org/firstinspires/ftc/team417/apriltags/AprilTagTest.java index 0e412aa75b9c..4e62d2c11cd1 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/apriltags/AprilTagTest.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/apriltags/AprilTagTest.java @@ -1,5 +1,7 @@ package org.firstinspires.ftc.team417.apriltags; +// LEGACY CLASS TO SHOW WHERE APRILTAG CODE CAME FROM + /* Copyright (c) 2023 FIRST. All rights reserved. * * Redistribution and use in source and binary forms, with or without modification, @@ -29,6 +31,7 @@ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ +import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import org.firstinspires.ftc.robotcore.external.hardware.camera.BuiltinCameraDirection; @@ -60,6 +63,7 @@ * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list. */ +@Disabled @TeleOp(name = "Concept: AprilTag Easy", group = "Concept") public class AprilTagTest extends LinearOpMode { From 005e6b6e7c79d9b93be1b0fefdfbb9e101d2b02d Mon Sep 17 00:00:00 2001 From: Emmett Date: Sun, 16 Nov 2025 16:42:39 -0800 Subject: [PATCH 091/198] finished glob code YAYAYAYAY!!!!!!!!!!!!!!!!!!!!! added logic that auto needs and completed mech logic. --- .../ftc/team417/ComplexMechGlob.java | 116 +++++++++++++++--- 1 file changed, 100 insertions(+), 16 deletions(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java b/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java index 3ae5de2ff6ce..7a3d9add9b5d 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java @@ -12,12 +12,15 @@ import com.qualcomm.robotcore.hardware.PIDFCoefficients; import com.qualcomm.robotcore.hardware.Servo; import com.qualcomm.robotcore.util.ElapsedTime; +import com.wilyworks.common.WilyWorks; import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.team417.roadrunner.MecanumDrive; import java.util.ArrayList; +import java.util.Arrays; import java.util.Collections; +import java.util.List; enum RequestedColor { //an enum for different color cases for launching PURPLE, @@ -30,6 +33,10 @@ enum PixelColor { GREEN, NONE } +enum LaunchDistance { + FAR, + NEAR +} class MechGlob { //a placeholder class encompassing all code that ISN'T for slowbot. MechGlob(){} @@ -52,6 +59,14 @@ void launch (RequestedColor requestedColor) {} void update () {} + boolean isDoneLaunching () { + return true; + } + + boolean preLaunch (RequestedColor requestedColor) { + return true; + } + void setLaunchVelocity (LaunchDistance launchDistance) {} } @@ -61,13 +76,17 @@ public class ComplexMechGlob extends MechGlob { //a class encompassing all code static double FEEDER_POWER = 1; static double TRANSFER_TIME_UP = 0.3; static double TRANSFER_TIME_TOTAL = 0.6; //TRANSFER_TIME_TOTAL must be more than TRANSFER_TIME_UP - static double UPPER_FLYWHEEL_VELOCITY = 1500; - static double LOWER_FLYWHEEL_VELOCITY = 1500; + static double UPPER_FAR_FLYWHEEL_VELOCITY = 1500; + static double LOWER_FAR_FLYWHEEL_VELOCITY = 1500; + static double LOWER_NEAR_FLYWHEEL_VELOCITY = 1500; + static double UPPER_NEAR_FLYWHEEL_VELOCITY = 1500; + static double TRANSFER_INACTIVE_POSITION = 0; static double TRANSFER_ACTIVE_POSITION = 1; static double REVERSE_INTAKE_SPEED = -1; static double INTAKE_SPEED = 1; - static double FLYWHEEL_VELOCITY_TOLERANCE = 25; + static double FLYWHEEL_VELOCITY_TOLERANCE = 25; //this is an epsiiiiiiiiilon + static double VOLTAGE_TOLERANCE = 0.05; //THIS IS AN EPSILON AS WELLLLLL ElapsedTime transferTimer; @@ -85,13 +104,14 @@ enum WaitState { } WaitState waitState = WaitState.IDLE; // arrays with placeholder values for servo positions and voltages relative to intake and launch - final double [] INTAKE_POSITIONS = {0, 1, 2}; - final double [] INTAKE_VOLTS = {0, 1, 2}; - final double [] LAUNCH_POSITIONS = {0, 1, 2}; - final double [] LAUNCH_VOLTS = {0, 1, 2}; + double [] INTAKE_POSITIONS = {0, 1, 2}; + double [] INTAKE_VOLTS = {0, 1, 2}; + double [] LAUNCH_POSITIONS = {0, 1, 2}; + double [] LAUNCH_VOLTS = {0, 1, 2}; double lastQueuedPosition; //where the servo was *queued* to go last. NOT THE SAME AS hwDrumPosition! double hwDrumPosition; //where the drum was *told* to go last. NOT THE SAME AS lastQueuedPosition! - + double upperLaunchVelocity; + double lowerLaunchVelocity; HardwareMap hardwareMap; @@ -121,6 +141,15 @@ public DrumRequest(double position, WaitState nextState) { } } ComplexMechGlob (HardwareMap hardwareMap, Telemetry telemetry) { + + //this changes some lists if we are using WilyWorks + if (WilyWorks.isSimulating) { + INTAKE_POSITIONS = new double[]{0 / 6.0, 2 / 6.0, 4 / 6.0}; + LAUNCH_POSITIONS = new double[]{3 / 6.0, 5 / 6.0, 1.0 / 6}; + INTAKE_VOLTS = new double[]{3.5 * 0 / 6.0, 3.5 * 2 / 6.0, 3.5 * 4 / 6.0}; + LAUNCH_VOLTS = new double[]{3.5 * 3 / 6.0, 3.5 * 5 / 6.0, 3.5 * 1.0 / 6}; + } + this.hardwareMap = hardwareMap; this.telemetry = telemetry; @@ -203,14 +232,58 @@ void launch (RequestedColor requestedColor) { slotOccupiedBy.set (minSlot, PixelColor.NONE); //marking this slot as empty so we don't accidentally try to use it again } } - void addToDrumQueue(double position, WaitState waitState){ //this function adds a new drum request to the drum queue. - drumQueue.add(new DrumRequest(position, waitState)); + //this function adds a new drum request to the drum queue. nextState is the state do use after the drum is finished moving + void addToDrumQueue(double position, WaitState nextState){ + drumQueue.add(new DrumRequest(position, nextState)); lastQueuedPosition = position; } boolean drumAtPosition() { - return true; - // TODO: implement this + + int intakeSlot = findSlotFromPosition(hwDrumPosition, INTAKE_POSITIONS); + int launchSlot = findSlotFromPosition(hwDrumPosition, LAUNCH_POSITIONS); + double expectedVolts; + + if (intakeSlot != -1) { + expectedVolts = INTAKE_VOLTS[intakeSlot]; + } else { + expectedVolts = LAUNCH_VOLTS[launchSlot]; + } + return Math.abs(analogDrum.getVoltage() - expectedVolts) <= VOLTAGE_TOLERANCE; + } + @Override + boolean isDoneLaunching () { + return drumQueue.isEmpty() && (waitState == WaitState.IDLE || waitState == WaitState.INTAKE); + } + @Override + //this function is just for auto. it rotates to the requested color but does not launch (to save time) + boolean preLaunch (RequestedColor requestedColor) { + int minSlot = findNearestSlot(LAUNCH_POSITIONS, requestedColor); + if (minSlot == -1){ + return false; + } else { + addToDrumQueue(LAUNCH_POSITIONS[minSlot], WaitState.IDLE); + return true; + } + + } + @Override + void setLaunchVelocity (LaunchDistance launchDistance) { + if (launchDistance == LaunchDistance.NEAR) { + upperLaunchVelocity = UPPER_NEAR_FLYWHEEL_VELOCITY; + lowerLaunchVelocity = LOWER_NEAR_FLYWHEEL_VELOCITY; + } else { + upperLaunchVelocity = UPPER_FAR_FLYWHEEL_VELOCITY; + lowerLaunchVelocity = LOWER_FAR_FLYWHEEL_VELOCITY; + } + } + int findSlotFromPosition (double position, double [] positions) { + for (int i = 0; i < positions.length; i++) { + if (positions [i] == position){ + return i; + } + } + return -1; } @Override void update () { @@ -248,8 +321,8 @@ void update () { } } if (waitState == WaitState.SPIN_UP) { - if (Math.abs(motLLauncher.getVelocity() -LOWER_FLYWHEEL_VELOCITY) <= FLYWHEEL_VELOCITY_TOLERANCE && - Math.abs(motULauncher.getVelocity() - UPPER_FLYWHEEL_VELOCITY) <= FLYWHEEL_VELOCITY_TOLERANCE) { + if (Math.abs(motLLauncher.getVelocity() - lowerLaunchVelocity) <= FLYWHEEL_VELOCITY_TOLERANCE && + Math.abs(motULauncher.getVelocity() - upperLaunchVelocity) <= FLYWHEEL_VELOCITY_TOLERANCE) { waitState = WaitState.TRANSFER; } } @@ -265,11 +338,22 @@ void update () { waitState = WaitState.IDLE; transferTimer = null; } + } + if (waitState == WaitState.INTAKE) { + PixelColor slotColor = coolColorDetector.detectPixelPosition(); + if (slotColor != PixelColor.NONE) { + int slot = findSlotFromPosition(hwDrumPosition, INTAKE_POSITIONS); + slotOccupiedBy.set(slot, slotColor); + waitState = WaitState.IDLE; + } + + + } servoDrum.setPosition(hwDrumPosition); servoTransfer.setPosition(transferPosition); - motLLauncher.setVelocity(LOWER_FLYWHEEL_VELOCITY); - motULauncher.setVelocity(UPPER_FLYWHEEL_VELOCITY); + motLLauncher.setVelocity(lowerLaunchVelocity); + motULauncher.setVelocity(upperLaunchVelocity); motIntake.setPower(intakePower); servoBLaunchFeeder.setPower(FEEDER_POWER); servoFLaunchFeeder.setPower((FEEDER_POWER)); From 451d2ede371e68e995d4009f944b9f8b2ee9a998 Mon Sep 17 00:00:00 2001 From: Team <417skid@swerverobotics.club> Date: Sun, 16 Nov 2025 16:45:57 -0800 Subject: [PATCH 092/198] Kashvi - color sensor code Sameer - Auto conditionals --- .../ftc/team417/CompetitionAuto.java | 14 +++++++----- .../ftc/team417/CoolColorDetector.java | 22 +++++-------------- 2 files changed, 15 insertions(+), 21 deletions(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java index 24536ec58f26..72934d3afe0d 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java @@ -131,11 +131,15 @@ public void runOpMode() { .build(); - PathFactory farSlowBotIntake1 = pathFactory.actionBuilder(SBFarStartPose) - .setTangent(Math.toRadians(180)) - // 3 launch actions - //then after disp intake action - .splineToSplineHeading(new Pose2d(36,32, Math.toRadians(90)), Math.toRadians(90)) //go to intake farthest from goal + PathFactory farSlowBotIntake1 = pathFactory.actionBuilder(SBFarStartPose); + if (intakeCycles == 0) { + farSlowBotIntake1.setTangent(Math.toRadians(180)); + // 3 launch actions + //then after disp intake action + } + + + farSlowBotIntake1.splineToSplineHeading(new Pose2d(36,32, Math.toRadians(90)), Math.toRadians(90)) //go to intake farthest from goal .setTangent(Math.toRadians(90)) .splineToConstantHeading(new Vector2d(36,60), Math.toRadians(90)) .setTangent(Math.toRadians(-90)) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CoolColorDetector.java b/team417/src/main/java/org/firstinspires/ftc/team417/CoolColorDetector.java index 2f5f1f12a999..e035ab52f55c 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CoolColorDetector.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CoolColorDetector.java @@ -2,30 +2,19 @@ import android.annotation.SuppressLint; -import com.qualcomm.robotcore.hardware.ColorSensor; import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.NormalizedColorSensor; import com.qualcomm.robotcore.hardware.NormalizedRGBA; -import android.app.Activity; -import android.graphics.Color; -import android.view.View; -import com.qualcomm.robotcore.eventloop.opmode.Disabled; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import com.qualcomm.robotcore.eventloop.opmode.TeleOp; -import com.qualcomm.robotcore.hardware.DistanceSensor; -import com.qualcomm.robotcore.hardware.NormalizedColorSensor; -import com.qualcomm.robotcore.hardware.NormalizedRGBA; -import com.qualcomm.robotcore.hardware.SwitchableLight; +import android.graphics.Color; -import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; import org.firstinspires.ftc.robotcore.external.Telemetry; public class CoolColorDetector { Telemetry telemetry; private NormalizedColorSensor sensor1; private NormalizedColorSensor sensor2; - private float gain = 50f; // adjust for brightness + private float gain = 85f; // adjust for brightness private float[] hsv = new float[3]; public CoolColorDetector(HardwareMap map, Telemetry telemetry) { sensor1 = map.get(NormalizedColorSensor.class, "cs1"); @@ -49,19 +38,20 @@ private PixelColor detectSingle(NormalizedColorSensor sensor) { if(value < 0.45) { return PixelColor.NONE; } - else if (hue >= 10 && hue <= 190) { + else if (hue >= 170 && hue <= 180) { return PixelColor.GREEN; } // PURPLE Range: 215 - 245 else{ return PixelColor.PURPLE; + } } // --- Use logic comparing both sensors --- - PixelColor detectPixelPosition() { + PixelColor detectArtifactColor() { PixelColor s1 = detectSingle(sensor1); PixelColor s2 = detectSingle(sensor2); // Rule 1: If both detect something different → return sensor2 @@ -84,7 +74,7 @@ PixelColor detectPixelPosition() { public void showTelemetry() { telemetry.addData("Sensor 1", detectSingle(sensor1)); telemetry.addData("Sensor 2", detectSingle(sensor2)); - telemetry.addData("Chosen Position", detectPixelPosition()); + telemetry.addData("Chosen Position", detectArtifactColor()); telemetry.update(); } } From 2394b2307c3c0e66ee9c3b16e73caea01c8e4ec6 Mon Sep 17 00:00:00 2001 From: anya-codes Date: Sun, 16 Nov 2025 17:35:09 -0800 Subject: [PATCH 093/198] Changed detectPixelColor function call in ComplexMechGlob to detectArtifact color because the original function name was changed --- .../java/org/firstinspires/ftc/team417/ComplexMechGlob.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java b/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java index 7a3d9add9b5d..2e5dbfef0f75 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java @@ -340,7 +340,7 @@ void update () { } } if (waitState == WaitState.INTAKE) { - PixelColor slotColor = coolColorDetector.detectPixelPosition(); + PixelColor slotColor = coolColorDetector.detectArtifactColor(); if (slotColor != PixelColor.NONE) { int slot = findSlotFromPosition(hwDrumPosition, INTAKE_POSITIONS); slotOccupiedBy.set(slot, slotColor); From fbb3170a7496ad725a76b25d52070269c2e08f8f Mon Sep 17 00:00:00 2001 From: bharv Date: Tue, 18 Nov 2025 19:56:16 -0800 Subject: [PATCH 094/198] Color detection redone for new logic --- .../ftc/team417/CoolColorDetector.java | 31 ++++++++++--------- .../ftc/team417/SensorColor.java | 15 +++++++-- 2 files changed, 29 insertions(+), 17 deletions(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CoolColorDetector.java b/team417/src/main/java/org/firstinspires/ftc/team417/CoolColorDetector.java index 2f5f1f12a999..d3d61d905c4c 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CoolColorDetector.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CoolColorDetector.java @@ -25,7 +25,7 @@ public class CoolColorDetector { Telemetry telemetry; private NormalizedColorSensor sensor1; private NormalizedColorSensor sensor2; - private float gain = 50f; // adjust for brightness + private float gain = 85f; // adjust for brightness private float[] hsv = new float[3]; public CoolColorDetector(HardwareMap map, Telemetry telemetry) { sensor1 = map.get(NormalizedColorSensor.class, "cs1"); @@ -40,27 +40,30 @@ private PixelColor detectSingle(NormalizedColorSensor sensor) { sensor.setGain(gain); NormalizedRGBA colors = sensor.getNormalizedColors(); Color.colorToHSV(colors.toColor(), hsv); + double distance = ((DistanceSensor) sensor).getDistance(DistanceUnit.MM); telemetry.addData("HSV", String.format("{%f, %f, %f}", hsv[0], hsv[1], hsv[2])); float hue = hsv[0]; - float value = hsv[2]; // - // GREEN Range: 145 - 165 - if(value < 0.45) { - return PixelColor.NONE; - } - else if (hue >= 10 && hue <= 190) { - return PixelColor.GREEN; - } - // PURPLE Range: 215 - 245 - else{ - return PixelColor.PURPLE; + if (distance <= 25) { + if (hue > 165 && hue < 180) { + return PixelColor.GREEN; + } + //Return purple based on hue value color sensor is detecting + else if (hue >= 200 && hue <= 225) { + return PixelColor.PURPLE; + } else { + return PixelColor.PURPLE; + } + } else { + return PixelColor.NONE; + } } - } - // --- Use logic comparing both sensors --- + + // --- Uswe logic comparing both sensors --- PixelColor detectPixelPosition() { PixelColor s1 = detectSingle(sensor1); PixelColor s2 = detectSingle(sensor2); diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/SensorColor.java b/team417/src/main/java/org/firstinspires/ftc/team417/SensorColor.java index 14db3f60215a..f781b535f407 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/SensorColor.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/SensorColor.java @@ -41,6 +41,7 @@ import com.qualcomm.robotcore.hardware.NormalizedRGBA; import com.qualcomm.robotcore.hardware.SwitchableLight; +import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; /* @@ -114,6 +115,7 @@ public void run() { } protected void runSample() { + telemetry.setDisplayFormat(Telemetry.DisplayFormat.HTML); // You can give the sensor a gain value, will be multiplied by the sensor's raw value before the // normalized color values are calculated. Color sensors (especially the REV Color Sensor V3) // can give very low values (depending on the lighting conditions), which only use a small part @@ -122,7 +124,7 @@ protected void runSample() { // colors will report at or near 1, and you won't be able to determine what color you are // actually looking at. For this reason, it's better to err on the side of a lower gain // (but always greater than or equal to 1). - float gain = 20; + float gain = 150; // Once per loop, we will update this hsvValues array. The first element (0) will contain the // hue, the second element (1) will contain the saturation, and the third element (2) will @@ -158,9 +160,9 @@ protected void runSample() { // Update the gain value if either of the A or B gamepad buttons is being held if (gamepad1.a) { // Only increase the gain by a small amount, since this loop will occur multiple times per second. - gain += 0.005; + gain += 0.05; } else if (gamepad1.b && gain > 1) { // A gain of less than 1 will make the values smaller, which is not helpful. - gain -= 0.005; + gain -= 0.05; } // Show the gain value via telemetry @@ -195,6 +197,7 @@ protected void runSample() { // Update the hsvValues array by passing it to Color.colorToHSV() Color.colorToHSV(colors.toColor(), hsvValues); + int hueColor = Color.HSVToColor(new float[]{hsvValues[0], 1, 1}); telemetry.addLine() .addData("Red", "%.3f", colors.red) @@ -205,6 +208,8 @@ protected void runSample() { .addData("Saturation", "%.3f", hsvValues[1]) .addData("Value", "%.3f", hsvValues[2]); telemetry.addData("Alpha", "%.3f", colors.alpha); + telemetry.addLine(colorString(colors.toColor())); + telemetry.addLine(colorString(hueColor)); /* If this color sensor also has a distance sensor, display the measured distance. * Note that the reported distance is only useful at very close range, and is impacted by @@ -223,4 +228,8 @@ public void run() { }); } } + static String colorString(int color) { + return String.format("\u25a0", + color & 0xffffff); + } } From 529d488c173c38c225ea08d120310de945528f9d Mon Sep 17 00:00:00 2001 From: bharv Date: Tue, 18 Nov 2025 20:02:23 -0800 Subject: [PATCH 095/198] Color detection redone for new logic + imports --- .../java/org/firstinspires/ftc/team417/CoolColorDetector.java | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CoolColorDetector.java b/team417/src/main/java/org/firstinspires/ftc/team417/CoolColorDetector.java index 49fa015ccaf3..ecbe0bdb50fb 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CoolColorDetector.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CoolColorDetector.java @@ -2,6 +2,7 @@ import android.annotation.SuppressLint; +import com.qualcomm.robotcore.hardware.DistanceSensor; import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.NormalizedColorSensor; import com.qualcomm.robotcore.hardware.NormalizedRGBA; @@ -9,8 +10,9 @@ import android.graphics.Color; import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; - public class CoolColorDetector { +public class CoolColorDetector { Telemetry telemetry; private NormalizedColorSensor sensor1; private NormalizedColorSensor sensor2; From daeb6c3784cf86455dcb1a51b88194500c4b4963 Mon Sep 17 00:00:00 2001 From: Emmett Date: Tue, 18 Nov 2025 20:16:43 -0800 Subject: [PATCH 096/198] teleop controls completed. ready for testing with wilyworks. Yay! --- .../ftc/team417/CompetitionTeleOp.java | 20 ++++++++++++++++--- 1 file changed, 17 insertions(+), 3 deletions(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java index ecab8c2ddb2c..06be1a57c4a4 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java @@ -6,8 +6,10 @@ import com.acmerobotics.roadrunner.PoseVelocity2d; import com.acmerobotics.roadrunner.Vector2d; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.util.ElapsedTime; +import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.team417.roadrunner.Drawing; import org.firstinspires.ftc.team417.roadrunner.MecanumDrive; @@ -40,9 +42,9 @@ public class CompetitionTeleOp extends BaseOpMode { public void runOpMode() { Pose2d beginPose = new Pose2d(0, 0, 0); MecanumDrive drive = new MecanumDrive(hardwareMap, telemetry, gamepad1, beginPose); + MechGlob mechGlob = ComplexMechGlob.create(hardwareMap, telemetry); // Initialize motors, servos, LEDs - initHardware(); // Wait for Start to be pressed on the Driver Hub! waitForStart(); @@ -70,8 +72,20 @@ public void runOpMode() { TelemetryPacket packet = MecanumDrive.getTelemetryPacket(); //add slowbot teleop controls here - - + if (gamepad2.yWasPressed()) { + mechGlob.launch(RequestedColor.EITHER); + } else if (gamepad2.bWasPressed()) { + mechGlob.launch(RequestedColor.PURPLE); + } else if (gamepad2.aWasPressed()) { + mechGlob.launch(RequestedColor.GREEN); + } + if (gamepad2.dpadUpWasPressed()) { + mechGlob.setLaunchVelocity(LaunchDistance.FAR); + } else if (gamepad2.dpadDownWasPressed()) { + mechGlob.setLaunchVelocity(LaunchDistance.NEAR); + } + mechGlob.intake(gamepad2.left_stick_x); + mechGlob.update(); } } From ab0f9ee293f0b077329eec872e9c52566b0e7998 Mon Sep 17 00:00:00 2001 From: Andrew Date: Thu, 20 Nov 2025 17:17:04 -0800 Subject: [PATCH 097/198] Update to latest version of Wily Works, mechanism simulator, and Configuration Tester. Switch from private GoBilda Pinpoint driver to version in the SDK. --- .../gobilda/GoBildaPinpointDriver.java | 489 ++++++++++ .../hardware/limelightvision/LLFieldMap.java | 251 +++++ .../hardware/limelightvision/LLResult.java | 527 +++++++++++ .../limelightvision/LLResultTypes.java | 883 ++++++++++++++++++ .../hardware/limelightvision/LLStatus.java | 247 +++++ .../hardware/limelightvision/Limelight3A.java | 733 +++++++++++++++ .../robotcore/hardware/AnalogInput.java | 2 - .../robotcore/hardware/HardwareDevice.java | 2 +- .../com/qualcomm/robotcore/hardware/LED.java | 4 +- .../qualcomm/robotcore/util/SerialNumber.java | 195 +++- .../simulator/framework/MechSim.java | 84 +- .../simulator/framework/WilyDcMotorEx.java | 1 - .../simulator/framework/WilyGamepad.java | 38 + .../framework/WilyHardwareDevice.java | 8 +- .../simulator/framework/WilyHardwareMap.java | 90 +- .../external/navigation/AngleUnit.java | 10 + .../robotcore/external/navigation/Pose3D.java | 89 ++ .../external/navigation/Position.java | 100 ++ .../navigation/UnnormalizedAngleUnit.java | 198 ++++ .../usb/EthernetOverUsbSerialNumber.java | 99 ++ .../src/main/java/org/json/JSONArray.java | 190 ++++ .../src/main/java/org/json/JSONException.java | 20 + .../src/main/java/org/json/JSONObject.java | 221 +++++ .../ftc/GoBildaPinpointDriver.java | 7 +- team417/build.gradle | 1 - .../ftc/team417/roadrunner/LoonyTune.java | 34 +- .../ftc/team417/roadrunner/MecanumDrive.java | 13 +- .../team417/utils/ConfigurationTester.java | 800 ++++++++++++---- 28 files changed, 5065 insertions(+), 271 deletions(-) create mode 100644 WilyCore/src/main/java/com/qualcomm/hardware/gobilda/GoBildaPinpointDriver.java create mode 100644 WilyCore/src/main/java/com/qualcomm/hardware/limelightvision/LLFieldMap.java create mode 100644 WilyCore/src/main/java/com/qualcomm/hardware/limelightvision/LLResult.java create mode 100644 WilyCore/src/main/java/com/qualcomm/hardware/limelightvision/LLResultTypes.java create mode 100644 WilyCore/src/main/java/com/qualcomm/hardware/limelightvision/LLStatus.java create mode 100644 WilyCore/src/main/java/com/qualcomm/hardware/limelightvision/Limelight3A.java create mode 100644 WilyCore/src/main/java/org/firstinspires/ftc/robotcore/external/navigation/Pose3D.java create mode 100644 WilyCore/src/main/java/org/firstinspires/ftc/robotcore/external/navigation/Position.java create mode 100644 WilyCore/src/main/java/org/firstinspires/ftc/robotcore/external/navigation/UnnormalizedAngleUnit.java create mode 100644 WilyCore/src/main/java/org/firstinspires/ftc/robotcore/internal/usb/EthernetOverUsbSerialNumber.java create mode 100644 WilyCore/src/main/java/org/json/JSONArray.java create mode 100644 WilyCore/src/main/java/org/json/JSONException.java create mode 100644 WilyCore/src/main/java/org/json/JSONObject.java diff --git a/WilyCore/src/main/java/com/qualcomm/hardware/gobilda/GoBildaPinpointDriver.java b/WilyCore/src/main/java/com/qualcomm/hardware/gobilda/GoBildaPinpointDriver.java new file mode 100644 index 000000000000..bf02a2732133 --- /dev/null +++ b/WilyCore/src/main/java/com/qualcomm/hardware/gobilda/GoBildaPinpointDriver.java @@ -0,0 +1,489 @@ +package com.qualcomm.hardware.gobilda; + +import com.acmerobotics.roadrunner.Pose2d; +import com.acmerobotics.roadrunner.PoseVelocity2d; +import com.qualcomm.robotcore.hardware.I2cDeviceSynchDevice; +import com.qualcomm.robotcore.hardware.I2cDeviceSynchSimple; +import com.qualcomm.robotcore.hardware.configuration.annotations.DeviceProperties; +import com.qualcomm.robotcore.hardware.configuration.annotations.I2cDeviceType; +import com.wilyworks.simulator.WilyCore; + +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.robotcore.external.navigation.Pose2D; +import org.firstinspires.ftc.robotcore.external.navigation.UnnormalizedAngleUnit; + +import java.nio.ByteBuffer; +import java.nio.ByteOrder; + +@I2cDeviceType +@DeviceProperties( + name = "Home-compiled goBILDA® Pinpoint Odometry Computer", + xmlTag = "goBILDAPinpoint", + description ="goBILDA® Pinpoint Odometry Computer (IMU Sensor Fusion for 2 Wheel Odometry)" +) + +public class GoBildaPinpointDriver extends I2cDeviceSynchDevice { + private int deviceStatus = 0; + private int loopTime = 0; + private int xEncoderValue = 0; + private int yEncoderValue = 0; + private float xPosition = 0; + private float yPosition = 0; + private float hOrientation = 0; + private float xVelocity = 0; + private float yVelocity = 0; + private float hVelocity = 0; + + private static final float goBILDA_SWINGARM_POD = 13.26291192f; //ticks-per-mm for the goBILDA Swingarm Pod + private static final float goBILDA_4_BAR_POD = 19.89436789f; //ticks-per-mm for the goBILDA 4-Bar Pod + + //i2c address of the device + public static final byte DEFAULT_ADDRESS = 0x31; + + public GoBildaPinpointDriver(I2cDeviceSynchSimple deviceClient, boolean deviceClientIsOwned) { + super(deviceClient, deviceClientIsOwned); + } + + @Override + public Manufacturer getManufacturer() { + return Manufacturer.Other; + } + + @Override + protected synchronized boolean doInitialize() { + return true; + } + + @Override + public String getDeviceName() { + return "goBILDA® Pinpoint Odometry Computer"; + } + + //Register map of the i2c device + private enum Register { + DEVICE_ID (1), + DEVICE_VERSION (2), + DEVICE_STATUS (3), + DEVICE_CONTROL (4), + LOOP_TIME (5), + X_ENCODER_VALUE (6), + Y_ENCODER_VALUE (7), + X_POSITION (8), + Y_POSITION (9), + H_ORIENTATION (10), + X_VELOCITY (11), + Y_VELOCITY (12), + H_VELOCITY (13), + TICKS_PER_MM (14), + X_POD_OFFSET (15), + Y_POD_OFFSET (16), + YAW_SCALAR (17), + BULK_READ (18); + + private final int bVal; + + Register(int bVal){ + this.bVal = bVal; + } + } + + //Device Status enum that captures the current fault condition of the device + public enum DeviceStatus{ + NOT_READY(0), + READY(1), + CALIBRATING(1 << 1), + FAULT_X_POD_NOT_DETECTED(1 << 2), + FAULT_Y_POD_NOT_DETECTED(1 << 3), + FAULT_NO_PODS_DETECTED(1 << 2 | 1 << 3), + FAULT_IMU_RUNAWAY(1 << 4), + FAULT_BAD_READ(1 << 5); + + private final int status; + + DeviceStatus(int status){ + this.status = status; + } + } + + //enum that captures the direction the encoders are set to + public enum EncoderDirection{ + FORWARD, + REVERSED; + } + + //enum that captures the kind of goBILDA odometry pods, if goBILDA pods are used + public enum GoBildaOdometryPods { + goBILDA_SWINGARM_POD, + goBILDA_4_BAR_POD; + } + //enum that captures a limited scope of read data. More options may be added in future update + public enum ReadData { + ONLY_UPDATE_HEADING, + } + + + /** Writes an int to the i2c device + @param reg the register to write the int to + @param i the integer to write to the register + */ + private void writeInt(final GoBildaPinpointDriver.Register reg, int i){ + } + + /** + * Reads an int from a register of the i2c device + * @param reg the register to read from + * @return returns an int that contains the value stored in the read register + */ + private int readInt(GoBildaPinpointDriver.Register reg){ + return 0; + } + + /** + * Converts a byte array to a float value + * @param byteArray byte array to transform + * @param byteOrder order of byte array to convert + * @return the float value stored by the byte array + */ + private float byteArrayToFloat(byte[] byteArray, ByteOrder byteOrder){ + return ByteBuffer.wrap(byteArray).order(byteOrder).getFloat(); + } + /** + * Reads a float from a register + * @param reg the register to read + * @return the float value stored in that register + */ + + private float readFloat(GoBildaPinpointDriver.Register reg){ + return 0; + } + + + /** + * Converts a float to a byte array + * @param value the float array to convert + * @return the byte array converted from the float + */ + private byte [] floatToByteArray (float value, ByteOrder byteOrder) { + return ByteBuffer.allocate(4).order(byteOrder).putFloat(value).array(); + } + + /** + * Writes a byte array to a register on the i2c device + * @param reg the register to write to + * @param bytes the byte array to write + */ + private void writeByteArray (GoBildaPinpointDriver.Register reg, byte[] bytes){ + + } + + /** + * Writes a float to a register on the i2c device + * @param reg the register to write to + * @param f the float to write + */ + private void writeFloat (GoBildaPinpointDriver.Register reg, float f){ + } + + /** + * Looks up the DeviceStatus enum corresponding with an int value + * @param s int to lookup + * @return the Odometry Computer state + */ + private GoBildaPinpointDriver.DeviceStatus lookupStatus (int s){ + if ((s & GoBildaPinpointDriver.DeviceStatus.CALIBRATING.status) != 0){ + return GoBildaPinpointDriver.DeviceStatus.CALIBRATING; + } + boolean xPodDetected = (s & GoBildaPinpointDriver.DeviceStatus.FAULT_X_POD_NOT_DETECTED.status) == 0; + boolean yPodDetected = (s & GoBildaPinpointDriver.DeviceStatus.FAULT_Y_POD_NOT_DETECTED.status) == 0; + + if(!xPodDetected && !yPodDetected){ + return GoBildaPinpointDriver.DeviceStatus.FAULT_NO_PODS_DETECTED; + } + if (!xPodDetected){ + return GoBildaPinpointDriver.DeviceStatus.FAULT_X_POD_NOT_DETECTED; + } + if (!yPodDetected){ + return GoBildaPinpointDriver.DeviceStatus.FAULT_Y_POD_NOT_DETECTED; + } + if ((s & GoBildaPinpointDriver.DeviceStatus.FAULT_IMU_RUNAWAY.status) != 0){ + return GoBildaPinpointDriver.DeviceStatus.FAULT_IMU_RUNAWAY; + } + if ((s & GoBildaPinpointDriver.DeviceStatus.READY.status) != 0){ + return GoBildaPinpointDriver.DeviceStatus.READY; + } + else { + return GoBildaPinpointDriver.DeviceStatus.NOT_READY; + } + } + + /** + * Call this once per loop to read new data from the Odometry Computer. Data will only update once this is called. + */ + public void update(){ + } + + /** + * Call this once per loop to read new data from the Odometry Computer. This is an override of the update() function + * which allows a narrower range of data to be read from the device for faster read times. Currently ONLY_UPDATE_HEADING + * is supported. + * @param data GoBildaPinpointDriver.readData.ONLY_UPDATE_HEADING + */ + public void update(ReadData data) { + if (data == ReadData.ONLY_UPDATE_HEADING) { + hOrientation = 0; + } + } + + /** + * Sets the odometry pod positions relative to the point that the odometry computer tracks around.

+ * The most common tracking position is the center of the robot.

+ * The X pod offset refers to how far sideways from the tracking point the X (forward) odometry pod is. Left of the center is a positive number, right of center is a negative number.
+ * the Y pod offset refers to how far forwards from the tracking point the Y (strafe) odometry pod is. forward of center is a positive number, backwards is a negative number.
+ * + * @param xOffset how sideways from the center of the robot is the X (forward) pod? Left increases + * @param yOffset how far forward from the center of the robot is the Y (Strafe) pod? forward increases + * @param distanceUnit the unit of distance used for offsets. + */ + public void setOffsets(double xOffset, double yOffset, DistanceUnit distanceUnit) { + writeFloat(Register.X_POD_OFFSET, (float) distanceUnit.toMm(xOffset)); + writeFloat(Register.Y_POD_OFFSET, (float) distanceUnit.toMm(yOffset)); + } + + /** + * Recalibrates the Odometry Computer's internal IMU.

+ * Robot MUST be stationary

+ * Device takes a large number of samples, and uses those as the gyroscope zero-offset. This takes approximately 0.25 seconds. + */ + public void recalibrateIMU(){writeInt(GoBildaPinpointDriver.Register.DEVICE_CONTROL,1<<0);} + + /** + * Resets the current position to 0,0,0 and recalibrates the Odometry Computer's internal IMU.

+ * Robot MUST be stationary

+ * Device takes a large number of samples, and uses those as the gyroscope zero-offset. This takes approximately 0.25 seconds. + */ + public void resetPosAndIMU(){writeInt(GoBildaPinpointDriver.Register.DEVICE_CONTROL,1<<1);} + + /** + * Can reverse the direction of each encoder. + * @param xEncoder FORWARD or REVERSED, X (forward) pod should increase when the robot is moving forward + * @param yEncoder FORWARD or REVERSED, Y (strafe) pod should increase when the robot is moving left + */ + public void setEncoderDirections(GoBildaPinpointDriver.EncoderDirection xEncoder, GoBildaPinpointDriver.EncoderDirection yEncoder){ + if (xEncoder == GoBildaPinpointDriver.EncoderDirection.FORWARD){ + writeInt(GoBildaPinpointDriver.Register.DEVICE_CONTROL,1<<5); + } + if (xEncoder == GoBildaPinpointDriver.EncoderDirection.REVERSED) { + writeInt(GoBildaPinpointDriver.Register.DEVICE_CONTROL,1<<4); + } + + if (yEncoder == GoBildaPinpointDriver.EncoderDirection.FORWARD){ + writeInt(GoBildaPinpointDriver.Register.DEVICE_CONTROL,1<<3); + } + if (yEncoder == GoBildaPinpointDriver.EncoderDirection.REVERSED){ + writeInt(GoBildaPinpointDriver.Register.DEVICE_CONTROL,1<<2); + } + } + + + /** + * This allows you to set the encoder resolution by the type of GoBilda pod you are using. + * If you aren't using a GoBilda pod, use setEncoderResolution(double) instead.

+ * + * @param pods goBILDA_SWINGARM_POD or goBILDA_4_BAR_POD + */ + public void setEncoderResolution(GoBildaOdometryPods pods) { + if (pods == GoBildaOdometryPods.goBILDA_SWINGARM_POD) { + setEncoderResolution(goBILDA_SWINGARM_POD, DistanceUnit.MM); + } + if (pods == GoBildaOdometryPods.goBILDA_4_BAR_POD) { + setEncoderResolution(goBILDA_4_BAR_POD, DistanceUnit.MM); + } + } + + /** + * Sets the encoder resolution in ticks per mm of the odometry pods.
+ * You can find this number by dividing the counts-per-revolution of your encoder by the circumference of the wheel. + * + * @param ticksPerUnit should be somewhere between 10 ticks/mm and 100 ticks/mm a goBILDA Swingarm pod is ~13.26291192 + * @param distanceUnit unit used for distance + */ + public void setEncoderResolution(double ticksPerUnit, DistanceUnit distanceUnit) { + double resolution = distanceUnit.toMm(ticksPerUnit); + writeByteArray(Register.TICKS_PER_MM, (floatToByteArray((float) resolution, ByteOrder.LITTLE_ENDIAN))); + } + + /** + * Tuning this value should be unnecessary.
+ * The goBILDA Odometry Computer has a per-device tuned yaw offset already applied when you receive it.

+ * This is a scalar that is applied to the gyro's yaw value. Increasing it will mean it will report more than one degree for every degree the sensor fusion algorithm measures.

+ * You can tune this variable by rotating the robot a large amount (10 full turns is a good starting place) and comparing the amount that the robot rotated to the amount measured. + * Rotating the robot exactly 10 times should measure 3600°. If it measures more or less, divide moved amount by the measured amount and apply that value to the Yaw Offset.

+ * If you find that to get an accurate heading number you need to apply a scalar of more than 1.05, or less than 0.95, your device may be bad. Please reach out to tech@gobilda.com + * @param yawOffset A scalar for the robot's heading. + */ + public void setYawScalar(double yawOffset){ + writeByteArray(GoBildaPinpointDriver.Register.YAW_SCALAR,(floatToByteArray((float) yawOffset, ByteOrder.LITTLE_ENDIAN))); + } + + /** + * Send a position that the Pinpoint should use to track your robot relative to. You can use this to + * update the estimated position of your robot with new external sensor data, or to run a robot + * in field coordinates.

+ * This overrides the current position.

+ * Using this feature to track your robot's position in field coordinates:
+ * When you start your code, send a Pose2D that describes the starting position on the field of your robot.
+ * Say you're on the red alliance, your robot is against the wall and closer to the audience side, + * and the front of your robot is pointing towards the center of the field. + * You can send a setPosition with something like -600mm x, -1200mm Y, and 90 degrees. The pinpoint would then always + * keep track of how far away from the center of the field you are.

+ * Using this feature to update your position with additional sensors:
+ * Some robots have a secondary way to locate their robot on the field. This is commonly + * Apriltag localization in FTC, but it can also be something like a distance sensor. + * Often these external sensors are absolute (meaning they measure something about the field) + * so their data is very accurate. But they can be slower to read, or you may need to be in a very specific + * position on the field to use them. In that case, spend most of your time relying on the Pinpoint + * to determine your location. Then when you pull a new position from your secondary sensor, + * send a setPosition command with the new position. The Pinpoint will then track your movement + * relative to that new, more accurate position. + * @param pos a Pose2D describing the robot's new position. + */ + public Pose2D setPosition(Pose2D pos){ + writeByteArray(GoBildaPinpointDriver.Register.X_POSITION,(floatToByteArray((float) pos.getX(DistanceUnit.MM), ByteOrder.LITTLE_ENDIAN))); + writeByteArray(GoBildaPinpointDriver.Register.Y_POSITION,(floatToByteArray((float) pos.getY(DistanceUnit.MM),ByteOrder.LITTLE_ENDIAN))); + writeByteArray(GoBildaPinpointDriver.Register.H_ORIENTATION,(floatToByteArray((float) pos.getHeading(AngleUnit.RADIANS),ByteOrder.LITTLE_ENDIAN))); + return pos; + } + + /** + * Checks the deviceID of the Odometry Computer. Should return 1. + * @return 1 if device is functional. + */ + public int getDeviceID(){return readInt(GoBildaPinpointDriver.Register.DEVICE_ID);} + + /** + * @return the firmware version of the Odometry Computer + */ + public int getDeviceVersion(){return readInt(GoBildaPinpointDriver.Register.DEVICE_VERSION); } + + public float getYawScalar(){return readFloat(GoBildaPinpointDriver.Register.YAW_SCALAR); } + + /** + * Device Status stores any faults the Odometry Computer may be experiencing. These faults include: + * @return one of the following states:
+ * NOT_READY - The device is currently powering up. And has not initialized yet. RED LED
+ * READY - The device is currently functioning as normal. GREEN LED
+ * CALIBRATING - The device is currently recalibrating the gyro. RED LED
+ * FAULT_NO_PODS_DETECTED - the device does not detect any pods plugged in. PURPLE LED
+ * FAULT_X_POD_NOT_DETECTED - The device does not detect an X pod plugged in. BLUE LED
+ * FAULT_Y_POD_NOT_DETECTED - The device does not detect a Y pod plugged in. ORANGE LED
+ */ + public GoBildaPinpointDriver.DeviceStatus getDeviceStatus(){return GoBildaPinpointDriver.DeviceStatus.READY; } + + /** + * Checks the Odometry Computer's most recent loop time.

+ * If values less than 500, or more than 1100 are commonly seen here, there may be something wrong with your device. Please reach out to tech@gobilda.com + * @return loop time in microseconds (1/1,000,000 seconds) + */ + public int getLoopTime(){return loopTime; } + + /** + * Checks the Odometry Computer's most recent loop frequency.

+ * If values less than 900, or more than 2000 are commonly seen here, there may be something wrong with your device. Please reach out to tech@gobilda.com + * @return Pinpoint Frequency in Hz (loops per second), + */ + public double getFrequency(){ + if (loopTime != 0){ + return 1000000.0/loopTime; + } + else { + return 0; + } + } + + /** + * @return the raw value of the X (forward) encoder in ticks + */ + public int getEncoderX(){return xEncoderValue; } + + /** + * @return the raw value of the Y (strafe) encoder in ticks + */ + public int getEncoderY(){return yEncoderValue; } + + /** + * @return the estimated X (forward) position of the robot in mm + */ + public double getPosX(){return xPosition; } + + /** + * @return the estimated Y (Strafe) position of the robot in mm + */ + public double getPosY(){return yPosition; } + + /** + * @return the estimated H (heading) position of the robot in Radians + */ + public double getHeading(AngleUnit angleUnit) { + return angleUnit.fromRadians(hOrientation); + } + + + /** + * @return the estimated X (forward) velocity of the robot in specified unit/sec + */ + public double getVelX(DistanceUnit distanceUnit) { + return distanceUnit.fromMm(xVelocity); + } + + /** + * @return the estimated Y (strafe) velocity of the robot in specified unit/sec + */ + public double getVelY(DistanceUnit distanceUnit) { + return distanceUnit.fromMm(yVelocity); + } + + /** + * @return the estimated H (heading) velocity of the robot in specified unit/sec + */ + public double getHeadingVelocity(UnnormalizedAngleUnit unnormalizedAngleUnit) { + return unnormalizedAngleUnit.fromRadians(hVelocity); + } + + /** + * This uses its own I2C read, avoid calling this every loop. + * @return the user-set offset for the X (forward) pod + */ + public float getXOffset(){return readFloat(GoBildaPinpointDriver.Register.X_POD_OFFSET);} + + /** + * This uses its own I2C read, avoid calling this every loop. + * @return the user-set offset for the Y (strafe) pod + */ + public float getYOffset(){return readFloat(GoBildaPinpointDriver.Register.Y_POD_OFFSET);} + + /** + * @return a Pose2D containing the estimated position of the robot + */ + public Pose2D getPosition(){ + Pose2d pose = WilyCore.getPose(false); // Use error-added pose + + return new Pose2D(DistanceUnit.INCH, + pose.position.x, + pose.position.y, + AngleUnit.RADIANS, + pose.heading.toDouble()); + } + + /** + * @return a Pose2D containing the estimated velocity of the robot, velocity is unit per second + */ + public Pose2D getVelocity(){ + PoseVelocity2d poseVelocity = WilyCore.getPoseVelocity(); + return new Pose2D(DistanceUnit.INCH, + poseVelocity.linearVel.x, + poseVelocity.linearVel.y, + AngleUnit.RADIANS, + poseVelocity.angVel); + } +} diff --git a/WilyCore/src/main/java/com/qualcomm/hardware/limelightvision/LLFieldMap.java b/WilyCore/src/main/java/com/qualcomm/hardware/limelightvision/LLFieldMap.java new file mode 100644 index 000000000000..6fc9dc380ad6 --- /dev/null +++ b/WilyCore/src/main/java/com/qualcomm/hardware/limelightvision/LLFieldMap.java @@ -0,0 +1,251 @@ +package com.qualcomm.hardware.limelightvision; + +import org.json.JSONArray; +import org.json.JSONObject; + +import java.util.ArrayList; +import java.util.List; + +/** + * Represents a field map containing fiducial markers / AprilTags. + */ +public class LLFieldMap { + + /** + * Represents a fiducial marker / AprilTag in the field map. + */ + public static class Fiducial { + private int id; + private double size; + private String family; + private List transform; + private boolean isUnique; + + /** + * Constructs a Fiducial with default values. + */ + public Fiducial() { + this(-1, 165.1, "apriltag3_36h11_classic", new ArrayList<>(16), true); + } + + /** + * Constructs a Fiducial with specified values. + * + * @param id The ID of the fiducial. + * @param size The size of the fiducial. + * @param family The family of the fiducial. + * @param transform The transformation of the fiducial. + * @param isUnique Whether the fiducial is unique. + */ + public Fiducial(int id, double size, String family, List transform, boolean isUnique) { + this.id = id; + this.size = size; + this.family = family; + this.transform = new ArrayList<>(transform); + this.isUnique = isUnique; + } + + /** + * Constructs a Fiducial from JSON data. Returns a default Fidcial if JSON is null or malformed + * + * @param json The JSON object containing fiducial data. + */ + protected Fiducial(JSONObject json) { + this(); //empy Fiducial + if(json != null) + { + try{ + this.id = json.optInt("id", -1); + this.size = json.optDouble("size", 165.1); + this.family = json.optString("family", "apriltag3_36h11_classic"); + this.transform = new ArrayList<>(); + JSONArray transformArray = json.optJSONArray("transform"); + if (transformArray != null) { + for (int i = 0; i < transformArray.length(); i++) { + this.transform.add(transformArray.optDouble(i, 0.0)); + } + } + this.isUnique = json.optBoolean("unique", true); + } catch (Exception e) { + + } + } + } + + /** + * Gets the ID / index of the fiducial. + * + * @return The fiducial ID. + */ + public int getId() { + return id; + } + + /** + * Gets the size of the fiducial in millimeters + * + * @return The fiducial size. + */ + public double getSize() { + return size; + } + + /** + * Gets the family of the fiducial. eg "apriltag3_36h11_classic" + * + * @return The fiducial family. + */ + public String getFamily() { + return family; + } + + /** + * Gets the 4x4 transforms matrix of the fiducial. + * + * @return A copy of the fiducial's transform. + */ + public List getTransform() { + return new ArrayList<>(transform); + } + + /** + * Checks if the fiducial is marked as unique. + * + * @return true if the fiducial is unique, false otherwise. + */ + public boolean isUnique() { + return isUnique; + } + + /** + * Converts the Fiducial to a JSONObject. + * + * @return A JSONObject representation of the Fiducial. + */ + protected JSONObject toJson() { + JSONObject json = new JSONObject(); + try { + json.put("id", id); + json.put("size", size); + json.put("family", family); + json.put("transform", new JSONArray(transform)); + json.put("unique", isUnique); + } catch (Exception e){ + + } + return json; + } + } + + private List fiducials; + private String type; + + /** + * Constructs an empty LLFieldMap. + */ + public LLFieldMap() { + this(new ArrayList<>(), ""); + } + + /** + * Constructs an LLFieldMap with specified fiducials and type. + * + * @param fiducials The list of fiducials in the field map. + * @param type The type of the field map. + */ + public LLFieldMap(List fiducials, String type) { + this.fiducials = new ArrayList<>(fiducials); + this.type = type; + } + + /** + * Constructs an LLFieldMap from JSON data. Returns an empty map if json is null or malformed + * + * @param json The JSON object containing field map data. + */ + protected LLFieldMap(JSONObject json) { + this(); + if (json != null) { + try{ + this.type = json.optString("type", ""); + JSONArray fiducialsArray = json.optJSONArray("fiducials"); + if (fiducialsArray != null) { + for (int i = 0; i < fiducialsArray.length(); i++) { + JSONObject fiducialJson = fiducialsArray.optJSONObject(i); + if (fiducialJson != null) { + this.fiducials.add(new Fiducial(fiducialJson)); + } + } + } + } + catch (Exception e){ + } + } + } + + /** + * Gets the list of fiducials in the field map. + * + * @return A copy of the list of fiducials. + */ + public List getFiducials() { + return new ArrayList<>(fiducials); + } + + /** + * Gets the type of the field map. (eg "ftc" or "frc") + * + * @return The field map type. + */ + public String getType() { + return type; + } + + /** + * Gets the number of tags (fiducials) in the field map. + * + * @return The number of tags. + */ + public int getNumberOfTags() { + return fiducials.size(); + } + + /** + * Get validity of map. Maps are valid if they have more than zero tags and have a specified type + * + * @return True if valid. False otherwise + */ + public boolean isValid() + { + if(getNumberOfTags() == 0) + { + //Likely user error. + return false; + } + if((getType() != "ftc") && (getType() != "frc")) + { + //Likely user error. + return false; + } + return true; + } + + /** + * Converts the LLFieldMap to a JSONObject. + * + * @return A JSONObject representation of the LLFieldMap. + */ + protected JSONObject toJson() { + JSONObject json = new JSONObject(); + try { + JSONArray fiducialsArray = new JSONArray(); + for (Fiducial fiducial : fiducials) { + fiducialsArray.put(fiducial.toJson()); + } + json.put("fiducials", fiducialsArray); + json.put("type", type); + } catch (Exception e) { + } + return json; + } +} \ No newline at end of file diff --git a/WilyCore/src/main/java/com/qualcomm/hardware/limelightvision/LLResult.java b/WilyCore/src/main/java/com/qualcomm/hardware/limelightvision/LLResult.java new file mode 100644 index 000000000000..2c0f7bbad275 --- /dev/null +++ b/WilyCore/src/main/java/com/qualcomm/hardware/limelightvision/LLResult.java @@ -0,0 +1,527 @@ +package com.qualcomm.hardware.limelightvision; + +/* +Copyright (c) 2024 Limelight Vision + +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, +are permitted (subject to the limitations in the disclaimer below) provided that +the following conditions are met: + +Redistributions of source code must retain the above copyright notice, this list +of conditions and the following disclaimer. + +Redistributions in binary form must reproduce the above copyright notice, this +list of conditions and the following disclaimer in the documentation and/or +other materials provided with the distribution. + +Neither the name of Limelight Vision nor the names of its contributors may be used to +endorse or promote products derived from this software without specific prior +written permission. + +NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS +LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF +THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + +import org.json.JSONArray; +import org.json.JSONException; +import org.json.JSONObject; +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.robotcore.external.navigation.Pose3D; +import org.firstinspires.ftc.robotcore.external.navigation.Position; +import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles; +import java.util.ArrayList; +import java.util.List; + +import com.qualcomm.hardware.limelightvision.LLResultTypes.*; + +/** + * Represents the result of a Limelight Pipeline. This class parses JSON data from a Limelight + * in the constructor and provides easy access to the results data. + */ +public class LLResult { + private JSONObject jsonData; + private List barcodeResults; + private List classifierResults; + private List detectorResults; + private List fiducialResults; + private List colorResults; + private double parseLatency; + private long controlHubTimeStamp; + + /** + * Constructs an LLResult object from a JSONObject. + * + * @param json The JSONObject containing Limelight vision data. + * @throws JSONException If there's an error parsing the JSON string. + */ + protected LLResult(JSONObject json) throws JSONException { + this.jsonData = json; + this.barcodeResults = new ArrayList<>(); + this.classifierResults = new ArrayList<>(); + this.detectorResults = new ArrayList<>(); + this.fiducialResults = new ArrayList<>(); + this.colorResults = new ArrayList<>(); + this.parseLatency = 0.0; + setControlHubTimeStamp(System.currentTimeMillis()); + parseResults(); + } + + /** + * Sets the timestamp from the control hub in milliseconds. + * + * @param timestamp The timestamp to set in milliseconds. + */ + void setControlHubTimeStamp(long timestamp) { + this.controlHubTimeStamp = timestamp; + } + + /** + * Gets the control hub timestamp in milliseconds. + * + * @return The control hub timestamp in milliseconds. + */ + public long getControlHubTimeStamp() { + return this.controlHubTimeStamp; + } + + /** + * Gets the control hub timestamp in nanoseconds. + * + * @return The control hub timestamp in nanoseconds. + */ + public long getControlHubTimeStampNanos() { + return this.controlHubTimeStamp * 1000000; + } + + + /** + * Calculates the staleness of the data. + * + * @return The staleness in milliseconds. + */ + public long getStaleness() { + return System.currentTimeMillis() - this.controlHubTimeStamp; + } + + /** + * Parses the JSON data into result objects. + * + * @throws JSONException If there's an error parsing the JSON data. + */ + private void parseResults() throws JSONException { + long startTime = System.nanoTime(); + + JSONArray barcodeArray = jsonData.optJSONArray("Barcode"); + if (barcodeArray != null) { + for (int i = 0; i < barcodeArray.length(); i++) { + barcodeResults.add(new BarcodeResult(barcodeArray.getJSONObject(i))); + } + } + + JSONArray classifierArray = jsonData.optJSONArray("Classifier"); + if (classifierArray != null) { + for (int i = 0; i < classifierArray.length(); i++) { + classifierResults.add(new ClassifierResult(classifierArray.getJSONObject(i))); + } + } + + JSONArray detectorArray = jsonData.optJSONArray("Detector"); + if (detectorArray != null) { + for (int i = 0; i < detectorArray.length(); i++) { + detectorResults.add(new DetectorResult(detectorArray.getJSONObject(i))); + } + } + + JSONArray fiducialArray = jsonData.optJSONArray("Fiducial"); + if (fiducialArray != null) { + for (int i = 0; i < fiducialArray.length(); i++) { + fiducialResults.add(new FiducialResult(fiducialArray.getJSONObject(i))); + } + } + + JSONArray colorArray = jsonData.optJSONArray("Retro"); + if (colorArray != null) { + for (int i = 0; i < colorArray.length(); i++) { + colorResults.add(new ColorResult(colorArray.getJSONObject(i))); + } + } + + long endTime = System.nanoTime(); + this.parseLatency = (endTime - startTime) / 1e6; // Convert to milliseconds + } + + /** + * Gets the list of barcode results. + * + * @return A list of BarcodeResult objects. + */ + public List getBarcodeResults() { + return barcodeResults; + } + + /** + * Gets the list of classifier results. + * + * @return A list of ClassifierResult objects. + */ + public List getClassifierResults() { + return classifierResults; + } + + /** + * Gets the list of detector results. + * + * @return A list of DetectorResult objects. + */ + public List getDetectorResults() { + return detectorResults; + } + + /** + * Gets the list of fiducial/apriltag results. + * + * @return A list of FiducialResult objects. + */ + public List getFiducialResults() { + return fiducialResults; + } + + /** + * Gets the list of color results. + * + * @return A list of ColorResult objects. + */ + public List getColorResults() { + return colorResults; + } + + /** + * Gets the focus metric of the image. This is only valid if the focus pipeline is enabled + * + * @return The focus metric value. + */ + public double getFocusMetric() { + return jsonData.optDouble("focus_metric", 0); + } + + /** + * Gets the 3D botpose. + * + * @return An Pose3D representing the bot pose. + */ + public Pose3D getBotpose() { + return createPose3DRobot(getDoubleArray("botpose", 6)); + } + + /** + * Gets the 3D botpose using the WPILIB Blue Alliance Coordinate System. + * + * @return An Pose3D representing the bot pose. + */ + private Pose3D getBotposeWpiblue() { + return createPose3DRobot(getDoubleArray("botpose_wpiblue", 6)); + } + + + /** + * Gets the 3D botpose using the WPILIB Red Alliance Coordinate System. + * + * @return An Pose3D representing the bot pose. + */ + private Pose3D getBotposeWpired() { + return createPose3DRobot(getDoubleArray("botpose_wpired", 6)); + } + + /** + * Gets the 3D botpose using MegaTag2. You must set the orientation of the robot with your imu for this to work. + * + * @return An Pose3D representing the MT2 botpose + */ + public Pose3D getBotpose_MT2() { + return createPose3DRobot(getDoubleArray("botpose_orb", 6)); + } + + /** + * Gets the 3D botpose using MegaTag2 and the WPILIB Blue Alliance Coordinate System. You must set the orientation of the robot with your imu for this to work. + * + * @return An Pose3D representing the MT2 botpose + */ + private Pose3D getBotposeWpiblue_MT2() { + return createPose3DRobot(getDoubleArray("botpose_orb_wpiblue", 6)); + } + + /** + * Gets the 3D botpose using MegaTag2 and the WPILIB Red Alliance Coordinate System. You must set the orientation of the robot with your imu for this to work. + * + * @return An Pose3D representing the MT2 botpose + */ + private Pose3D getBotposeOrbWpired_MT2() { + return createPose3DRobot(getDoubleArray("botpose_orb_wpired", 6)); + } + + + /** + * Gets the standard deviation metrics for MegaTag1 (x,y,z,roll,pitch,yaw) + * + * @return MegaTag1 STDevs + */ + public double[] getStddevMt1() { + return getDoubleArray("stdev_mt1", 6); + } + + /** + * Gets the standard deviation metrics for MegaTag2 (x,y,z,roll,pitch,yaw) + * + * @return MegaTag2 STDevs + */ + public double[] getStddevMt2() { + return getDoubleArray("stdev_mt2", 6); + } + + /** + * Gets the number of tags used in the botpose calculation. + * + * @return The number of tags used in the botpose calculation. + */ + public int getBotposeTagCount() { + return jsonData.optInt("botpose_tagcount", 0); + } + + /** + * Gets the span of tags used in the botpose calculation in meters. + * + * @return The span of tags used in the botpose calculation. + */ + public double getBotposeSpan() { + return jsonData.optDouble("botpose_span", 0); + } + + /** + * Gets the average distance of tags used in the botpose calculation in meters. + * + * @return The average distance of tags used in the botpose calculation. + */ + public double getBotposeAvgDist() { + return jsonData.optDouble("botpose_avgdist", 0); + } + + /** + * Gets the average area of tags used in the botpose calculation. + * + * @return The average area of tags used in the botpose calculation. + */ + public double getBotposeAvgArea() { + return jsonData.optDouble("botpose_avgarea", 0); + } + + /** + * Gets the user-specified python snapscript output data + * + * @return An array of values from the python snapscript pipeline. Returns an array of size 32. + */ + public double[] getPythonOutput() { + double[] output = getDoubleArray("PythonOut", 0); + + // Create a new array of size 32 + double[] result = new double[32]; + + // How many elements do we need to copy to the final result array? + int lengthToCopy = Math.min(output.length, 32); + + // Copy from the python result array to the result array. + System.arraycopy(output, 0, result, 0, lengthToCopy); + return result; + } + + /** + * Gets the current capture latency in milliseconds + * + * @return capture latency in millisecondss + */ + public double getCaptureLatency() { + return jsonData.optDouble("cl", 0); + } + + /** + * Gets the type of the current pipeline. + * + * @return The type of the current pipeline as a string. + */ + public String getPipelineType() { + return jsonData.optString("pipelineType", ""); + } + + /** + * Gets the current tx in degrees from the crosshair + * + * @return horizontal angular offset of the primary target in degrees from the crosshair + */ + public double getTx() { + return jsonData.optDouble("tx", 0); + } + + /** + * Gets the current ty in degrees from the crosshair + * + * @return vertical angular offset of the primary target in degrees from the crosshair + */ + public double getTy() { + return jsonData.optDouble("ty", 0); + } + + /** + * Gets the current tx in degrees from the principal pixel instead of the crosshair + * + * @return horizontal angular offset of the primary target in degrees from the principal pixel instead of the crosshair + */ + public double getTxNC() { + return jsonData.optDouble("txnc", 0); + } + + /** + * Gets the current ty in degrees from the principal pixel instead of the crosshair + * + * @return vertical angular offset of the primary target in degrees from the principal pixel instead of the crosshair + */ + public double getTyNC() { + return jsonData.optDouble("tync", 0); + } + + /** + * Gets the area of the target as a percentage of the image area + * + * @return target area (0-100) + */ + public double getTa() { + return jsonData.optDouble("ta", 0); + } + + /** + * Gets the index of the currently active pipeline + * + * @return index of the currently active pipeline + */ + public int getPipelineIndex() { + return jsonData.optInt("pID", 0); + } + + /** + * Gets the 3D camera pose in robot space as configured in the UI + * + * @return An Pose3D representing the camera pose in the robot's coordinate system. + */ + private Pose3D getCameraPose_RobotSpace() { + return createPose3DRobot(getDoubleArray("t6c_rs", 6)); + } + + /** + * Gets the targeting/pipeline latency in milliseconds. + * + * @return The targeting/pipeline latency. + */ + public double getTargetingLatency() { + return jsonData.optDouble("tl", 0); + } + + /** + * Gets the Limelight-local monotonic timestamp of the result. + * + * @return The limelight-local timestamp. + */ + public double getTimestamp() { + return jsonData.optDouble("ts", 0); + } + + /** + * Gets the validity of the result. + * + * @return Validity (0 or 1). + */ + public boolean isValid() { + int v = jsonData.optInt("v", 0); + if(v == 1) + { + return true; + } + return false; + } + + /** + * Gets the json parse latency. + * + * @return The Control Hub json parse latency in milliseconds. + */ + public double getParseLatency() { + return parseLatency; + } + + /** + * Helper method to get a double array from the JSON data. + * + * @param key The key for the array in the JSON data. + * @return A double array corresponding to the key. + */ + private double[] getDoubleArray(String key, int defaultCount) { + JSONArray array = jsonData.optJSONArray(key); + if (array == null) { + return new double[defaultCount]; + } + double[] result = new double[array.length()]; + for (int i = 0; i < array.length(); i++) { + result[i] = array.optDouble(i); + } + return result; + } + + /** + * Helper method to create an Pose3D instance from a pose array. + * + * @param pose The pose array (x, y, z, roll, pitch, yaw) + * @return An Pose3D instance without acquisition time. + */ + protected static Pose3D createPose3DRobot(double[] pose) { + if (pose.length < 6) { + return new Pose3D(new Position(), new YawPitchRollAngles(AngleUnit.DEGREES,0,0,0,0)); + } + Position position = new Position(DistanceUnit.METER, pose[0], pose[1], pose[2],0); + YawPitchRollAngles orientation = new YawPitchRollAngles(AngleUnit.DEGREES, pose[5], pose[4], pose[3],0); + return new Pose3D(position, orientation); + } + + /** + * Parses a JSONObject into an LLResult object. + * + * @param json The JSON to parse. + * @return An LLResult object, or null if parsing fails. + */ + protected static LLResult parse(JSONObject json) { + try { + return new LLResult(json); + } catch (JSONException e) { + e.printStackTrace(); + return null; + } + } + + /** + * Returns a string representation of the LLResult. + * + * @return A string representation of the JSON data. + */ + @Override + public String toString() { + return jsonData.toString(); + } +} \ No newline at end of file diff --git a/WilyCore/src/main/java/com/qualcomm/hardware/limelightvision/LLResultTypes.java b/WilyCore/src/main/java/com/qualcomm/hardware/limelightvision/LLResultTypes.java new file mode 100644 index 000000000000..05b93a69ae6e --- /dev/null +++ b/WilyCore/src/main/java/com/qualcomm/hardware/limelightvision/LLResultTypes.java @@ -0,0 +1,883 @@ +package com.qualcomm.hardware.limelightvision; + +import org.firstinspires.ftc.robotcore.external.navigation.Pose3D; +import org.json.JSONArray; +import org.json.JSONObject; +import org.json.JSONException; + +import java.util.ArrayList; +import java.util.List; + +/** + * Parent class for all Limelight Result Types + */ +public class LLResultTypes { + + /** + * Parses a JSONArray of points into a List of Lists representing 2D coordinates. + * + * @param cornersArray The JSONArray containing point data. Each element should be a JSONArray with two elements representing x and y coordinates. + * @return A List of Lists, where each inner List contains two Double values representing x and y coordinates. + * Returns an empty List if the input is null or improperly formatted. + */ + private static List> parsePoints(JSONArray cornersArray) { + List> points = new ArrayList<>(); + if (cornersArray != null) { + for (int i = 0; i < cornersArray.length(); i++) { + JSONArray point = cornersArray.optJSONArray(i); + if (point != null && point.length() == 2) { + List cornerPoint = new ArrayList<>(); + cornerPoint.add(point.optDouble(0, 0.0)); + cornerPoint.add(point.optDouble(1, 0.0)); + points.add(cornerPoint); + } + } + } + return points; + } + + /** + * Parses a JSONArray into a double array representing a robot pose. + * The array should contain 6 elements: x, y, z, roll, pitch, yaw. + * + * @param array The JSONArray to convert. + * @return A double array representing the robot pose. If the input is null or has fewer than 6 elements, + * returns an array of 6 zeros. + */ + private static double[] parsePoseArray(JSONArray array) { + double[] pose = new double[6]; // Initialize with zeros + if (array != null && array.length() >= 6) { + for (int i = 0; i < 6; i++) { + pose[i] = array.optDouble(i, 0.0); + } + } + return pose; + } + + /** + * Represents a barcode pipeline result. A barcode pipeline may generate multiple valid results. + */ + public static class BarcodeResult { + + private String family; + private String data; + private double targetXPixels; + private double targetYPixels; + private double targetXDegrees; + private double targetYDegrees; + private double targetXDegreesNoCrosshair; + private double targetYDegreesNoCrosshair; + private double targetArea; + private List> targetCorners; + + /** + * Constructs a BarcodeResult from JSON data. + * + * @param data The JSON object containing barcode data. + */ + protected BarcodeResult(JSONObject data) { + this.family = data.optString("fam", ""); + this.data = data.optString("data", ""); + this.targetXPixels = data.optDouble("txp", 0.0); + this.targetYPixels = data.optDouble("typ", 0.0); + this.targetXDegrees = data.optDouble("tx", 0.0); + this.targetYDegrees = data.optDouble("ty", 0.0); + this.targetXDegreesNoCrosshair = data.optDouble("tx_nocross", 0.0); + this.targetYDegreesNoCrosshair = data.optDouble("ty_nocross", 0.0); + this.targetArea = data.optDouble("ta", 0.0); + this.targetCorners = LLResultTypes.parsePoints(data.optJSONArray("pts")); + } + + /** + * Gets the family of the barcode. + * + * @return The barcode family. + */ + public String getFamily() { + return family; + } + + /** + * Gets the data contained in the barcode. + * + * @return The barcode data. + */ + public String getData() { + return data; + } + + /** + * Gets the horizontal offset of the barcode from the crosshair in pixels. + * + * @return The horizontal offset in pixels. + */ + public double getTargetXPixels() { + return targetXPixels; + } + + /** + * Gets the vertical offset of the barcode from the crosshair in pixels. + * + * @return The vertical offset in pixels. + */ + public double getTargetYPixels() { + return targetYPixels; + } + + /** + * Gets the horizontal offset of the barcode from the crosshair in degrees. + * + * @return The horizontal offset in degrees. + */ + public double getTargetXDegrees() { + return targetXDegrees; + } + + /** + * Gets the vertical offset of the barcode from the crosshair in degrees. + * + * @return The vertical offset in degrees. + */ + public double getTargetYDegrees() { + return targetYDegrees; + } + + /** + * Gets the horizontal offset of the barcode from the principal pixel in degrees. + * + * @return The horizontal offset in degrees. + */ + public double getTargetXDegreesNoCrosshair() { + return targetXDegreesNoCrosshair; + } + + /** + * Gets the vertical offset of the barcode from the principal pixel in degrees. + * + * @return The vertical offset in degrees. + */ + public double getTargetYDegreesNoCrosshair() { + return targetYDegreesNoCrosshair; + } + + /** + * Gets the area of the detected barcode as a percentage of the image. + * + * @return The target area (0-1). + */ + public double getTargetArea() { + return targetArea; + } + + /** + * Gets the four corner points of the detected barcode. + * + * @return A list of corner points, each point represented as a list of two doubles [x, y]. + */ + public List> getTargetCorners() { + return targetCorners; + } + + } + + /** + * Represents a classifier pipeline result. + */ + public static class ClassifierResult { + private String className; + private int classId; + private double confidence; + + /** + * Constructs a ClassifierResult from JSON data. + * + * @param data The JSON object containing classifier data. + */ + protected ClassifierResult(JSONObject data) { + this.className = data.optString("class", ""); + this.classId = data.optInt("classID", 0); + this.confidence = data.optDouble("conf", 0.0); + } + + /** + * Gets the class name of the classifier result (eg book, car, gamepiece). + * + * @return The class name + */ + public String getClassName() { + return className; + } + + /** + * Gets the class index of the classifier result. + * + * @return The class index + */ + public int getClassId() { + return classId; + } + + /** + * Gets the confidence score of the classification. + * + * @return The confidence score of the classification (0-100). + */ + public double getConfidence() { + return confidence; + } + } + + /** + * Represents a detector pipeline result. A detector pipeline may generate multiple valid results. + */ + public static class DetectorResult { + private String className; + private int classId; + private double confidence; + private double targetArea; + private double targetXPixels; + private double targetYPixels; + private double targetXDegrees; + private double targetYDegrees; + private double targetXDegreesNoCrosshair; + private double targetYDegreesNoCrosshair; + private List> targetCorners; + + /** + * Constructs a DetectorResult from JSON data. + * + * @param data The JSON object containing classifier data. + */ + protected DetectorResult(JSONObject data) { + this.className = data.optString("class", ""); + this.classId = data.optInt("classID", 0); + this.confidence = data.optDouble("conf", 0.0); + this.targetArea = data.optDouble("ta", 0.0); + this.targetXPixels = data.optDouble("txp", 0.0); + this.targetYPixels = data.optDouble("typ", 0.0); + this.targetXDegrees = data.optDouble("tx", 0.0); + this.targetYDegrees = data.optDouble("ty", 0.0); + this.targetXDegreesNoCrosshair = data.optDouble("tx_nocross", 0.0); + this.targetYDegreesNoCrosshair = data.optDouble("ty_nocross", 0.0); + this.targetCorners = LLResultTypes.parsePoints(data.optJSONArray("pts")); + } + + /** + * Gets the class name of the detector result (eg book, car, gamepiece). + * + * @return The class name + */ + public String getClassName() { + return className; + } + + + /** + * Gets the class index of the detector result. + * + * @return The class index + */ + public int getClassId() { + return classId; + } + + /** + * Gets the confidence score of the classification. + * + * @return The confidence score of the classification (0-100). + */ + public double getConfidence() { + return confidence; + } + + /** + * Gets the four corner points of the detected result. + * + * @return A list of corner points, each point represented as a list of two doubles [x, y]. + */ + public List> getTargetCorners() { + return targetCorners; + } + + /** + * Gets the undistorted area of the target as a percentage of the image area + * + * @return target area (0-100) + */ + public double getTargetArea() { + return targetArea; + } + + /** + * Gets the current tx in pixels from the crosshair + * + * @return horizontal angular offset + */ + public double getTargetXPixels() { + return targetXPixels; + } + + /** + * Gets the current ty in pixels from the crosshair + * + * @return vertical angular offset + */ + public double getTargetYPixels() { + return targetYPixels; + } + + /** + * Gets the current tx in degrees from the crosshair + * + * @return horizontal angular offset + */ + public double getTargetXDegrees() { + return targetXDegrees; + } + + /** + * Gets the current ty in degrees from the crosshair + * + * @return vertical angular offset + */ + public double getTargetYDegrees() { + return targetYDegrees; + } + + /** + * Gets the current tx in degrees from the principal pixel + * + * @return horizontal angular offset + */ + public double getTargetXDegreesNoCrosshair() { + return targetXDegreesNoCrosshair; + } + + /** + * Gets the current ty in degrees from the principal pixel + * + * @return vertical angular offset + */ + public double getTargetYDegreesNoCrosshair() { + return targetYDegreesNoCrosshair; + } + } + + /** + * Represents a Fiducial/AprilTag pipeline result. A fiducial/apriltag pipeline may generate multiple valid results. + */ + public static class FiducialResult { + private int fiducialId; + private String family; + private double skew; + private Pose3D cameraPoseTargetSpace; + private Pose3D robotPoseFieldSpace; + private Pose3D robotPoseTargetSpace; + private Pose3D targetPoseCameraSpace; + private Pose3D targetPoseRobotSpace; + private double targetArea; + private double targetXPixels; + private double targetYPixels; + private double targetXDegrees; + private double targetYDegrees; + private double targetXDegreesNoCrosshair; + private double targetYDegreesNoCrosshair; + private List> targetCorners; + + + /** + * Constructs a FiducialResult from JSON data. + * + * @param data The JSON object containing fiducial data. + */ + protected FiducialResult(JSONObject data) { + this.fiducialId = data.optInt("fID", 0); + this.family = data.optString("fam", ""); + this.skew = data.optDouble("skew", 0.0); + this.cameraPoseTargetSpace = LLResult.createPose3DRobot(LLResultTypes.parsePoseArray(data.optJSONArray("t6c_ts"))); + this.robotPoseFieldSpace = LLResult.createPose3DRobot(LLResultTypes.parsePoseArray(data.optJSONArray("t6r_fs"))); + this.robotPoseTargetSpace = LLResult.createPose3DRobot(LLResultTypes.parsePoseArray(data.optJSONArray("t6r_ts"))); + this.targetPoseCameraSpace = LLResult.createPose3DRobot(LLResultTypes.parsePoseArray(data.optJSONArray("t6t_cs"))); + this.targetPoseRobotSpace = LLResult.createPose3DRobot(LLResultTypes.parsePoseArray(data.optJSONArray("t6t_rs"))); + this.targetArea = data.optDouble("ta", 0.0); + this.targetXPixels = data.optDouble("txp", 0.0); + this.targetYPixels = data.optDouble("typ", 0.0); + this.targetXDegrees = data.optDouble("tx", 0.0); + this.targetYDegrees = data.optDouble("ty", 0.0); + this.targetXDegreesNoCrosshair = data.optDouble("tx_nocross", 0.0); + this.targetYDegreesNoCrosshair = data.optDouble("ty_nocross", 0.0); + this.targetCorners = LLResultTypes.parsePoints(data.optJSONArray("pts")); + } + + /** + * Gets the ID of the fiducial. + * + * @return The fiducial ID. + */ + public int getFiducialId() { + return fiducialId; + } + + /** + * Gets the family of the fiducial (eg 36h11). + * + * @return The fiducial family. + */ + public String getFamily() { + return family; + } + + /** + * Gets the four corner points of the detected fiducial/apriltag. + * + * @return A list of corner points, each point represented as a list of two doubles [x, y]. + */ + public List> getTargetCorners() { + return targetCorners; + } + + /** + * Gets the skew of the detected fiducial. Not recommended. + * + * @return The skew value. + */ + public double getSkew() { + return skew; + } + + /** + * Gets the camera pose in target space. + * + * @return An Pose3D representing the camera pose. + */ + public Pose3D getCameraPoseTargetSpace() { + return cameraPoseTargetSpace; + } + + /** + * Gets the robot pose in field based on this fiducial/apriltag alone. + * + * @return An Pose3D representing the robot pose. + */ + public Pose3D getRobotPoseFieldSpace() { + return robotPoseFieldSpace; + } + + /** + * Gets the robot pose in target space. + * + * @return An Pose3D representing the robot pose. + */ + public Pose3D getRobotPoseTargetSpace() { + return robotPoseTargetSpace; + } + + /** + * Gets the target pose in camera space. + * + * @return An Pose3D representing the target pose. + */ + public Pose3D getTargetPoseCameraSpace() { + return targetPoseCameraSpace; + } + + /** + * Gets the target pose in robot space. + * + * @return An Pose3D representing the target pose. + */ + public Pose3D getTargetPoseRobotSpace() { + return targetPoseRobotSpace; + } + + /** + * Gets the area of the detected fiducial as a percentage of the image. + * + * @return The target area (0-100). + */ + public double getTargetArea() { + return targetArea; + } + + /** + * Gets the current tx in pixels from the crosshair + * + * @return horizontal angular offset + */ + public double getTargetXPixels() { + return targetXPixels; + } + + /** + * Gets the current ty in pixels from the crosshair + * + * @return vertical angular offset + */ + public double getTargetYPixels() { + return targetYPixels; + } + + /** + * Gets the current tx in degrees from the crosshair + * + * @return horizontal angular offset + */ + public double getTargetXDegrees() { + return targetXDegrees; + } + + /** + * Gets the current ty in degrees from the crosshair + * + * @return vertical angular offset + */ + public double getTargetYDegrees() { + return targetYDegrees; + } + + /** + * Gets the current tx in degrees from the principal pixel + * + * @return horizontal angular offset + */ + public double getTargetXDegreesNoCrosshair() { + return targetXDegreesNoCrosshair; + } + + /** + * Gets the current ty in degrees from the principal pixel + * + * @return vertical angular offset + */ + public double getTargetYDegreesNoCrosshair() { + return targetYDegreesNoCrosshair; + } + } + + /** + * Represents a color pipeline result. A color pipeline may generate multiple valid results. + */ + public static class ColorResult { + private Pose3D cameraPoseTargetSpace; + private Pose3D robotPoseFieldSpace; + private Pose3D robotPoseTargetSpace; + private Pose3D targetPoseCameraSpace; + private Pose3D targetPoseRobotSpace; + private double targetArea; + private double targetXPixels; + private double targetYPixels; + private double targetXDegrees; + private double targetYDegrees; + private double targetXDegreesNoCrosshair; + private double targetYDegreesNoCrosshair; + private List> targetCorners; + + /** + * Constructs a Color/ColorResult from JSON data. + * + * @param data The JSON object containing color data. + */ + protected ColorResult(JSONObject data) { + this.cameraPoseTargetSpace = LLResult.createPose3DRobot(LLResultTypes.parsePoseArray(data.optJSONArray("t6c_ts"))); + this.robotPoseFieldSpace = LLResult.createPose3DRobot(LLResultTypes.parsePoseArray(data.optJSONArray("t6r_fs"))); + this.robotPoseTargetSpace = LLResult.createPose3DRobot(LLResultTypes.parsePoseArray(data.optJSONArray("t6r_ts"))); + this.targetPoseCameraSpace = LLResult.createPose3DRobot(LLResultTypes.parsePoseArray(data.optJSONArray("t6t_cs"))); + this.targetPoseRobotSpace = LLResult.createPose3DRobot( LLResultTypes.parsePoseArray(data.optJSONArray("t6t_rs"))); + this.targetArea = data.optDouble("ta", 0.0); + this.targetXPixels = data.optDouble("txp", 0.0); + this.targetYPixels = data.optDouble("typ", 0.0); + this.targetXDegrees = data.optDouble("tx", 0.0); + this.targetYDegrees = data.optDouble("ty", 0.0); + this.targetXDegreesNoCrosshair = data.optDouble("tx_nocross", 0.0); + this.targetYDegreesNoCrosshair = data.optDouble("ty_nocross", 0.0); + this.targetCorners = LLResultTypes.parsePoints(data.optJSONArray("pts")); + } + + /** + * Gets the corner points of the detected target. The number of corners is not fixed. + * + * @return A list of corner points, each point represented as a list of two doubles [x, y]. + */ + public List> getTargetCorners() { + return targetCorners; + } + + /** + * Gets the camera pose in target space. + * + * @return An Pose3D representing the camera pose. + */ + public Pose3D getCameraPoseTargetSpace() { + return cameraPoseTargetSpace; + } + + /** + * Gets the robot pose in field space based on this color target. + * + * @return An Pose3D representing the robot pose. + */ + public Pose3D getRobotPoseFieldSpace() { + return robotPoseFieldSpace; + } + + /** + * Gets the robot pose in target space. + * + * @return An Pose3D representing the robot pose. + */ + public Pose3D getRobotPoseTargetSpace() { + return robotPoseTargetSpace; + } + + /** + * Gets the target pose in camera space. + * + * @return An Pose3D representing the target pose. + */ + public Pose3D getTargetPoseCameraSpace() { + return targetPoseCameraSpace; + } + + /** + * Gets the target pose in robot space. + * + * @return An Pose3D representing the target pose. + */ + public Pose3D getTargetPoseRobotSpace() { + return targetPoseRobotSpace; + } + + /** + * Gets the area of the detected color target as a percentage of the image. + * + * @return The target area (0-100). + */ + public double getTargetArea() { + return targetArea; + } + + /** + * Gets the current tx in pixels from the crosshair + * + * @return horizontal angular offset + */ + public double getTargetXPixels() { + return targetXPixels; + } + + /** + * Gets the current ty in pixels from the crosshair + * + * @return vertical angular offset + */ + public double getTargetYPixels() { + return targetYPixels; + } + + /** + * Gets the current tx in degrees from the crosshair + * + * @return horizontal angular offset + */ + public double getTargetXDegrees() { + return targetXDegrees; + } + + /** + * Gets the current ty in degrees from the crosshair + * + * @return vertical angular offset + */ + public double getTargetYDegrees() { + return targetYDegrees; + } + + /** + * Gets the current tx in degrees from the principal pixel + * + * @return horizontal angular offset + */ + public double getTargetXDegreesNoCrosshair() { + return targetXDegreesNoCrosshair; + } + + /** + * Gets the current ty in degrees from the principal pixel + * + * @return vertical angular offset + */ + public double getTargetYDegreesNoCrosshair() { + return targetYDegreesNoCrosshair; + } + } + + + /** + * Represents a calibration result. Calibration results are generated by the user in the UI's calibration tab. + */ + public static class CalibrationResult { + private String displayName; + private double resX; + private double resY; + private double reprojectionError; + private double[] camMatVector; + private double[] distortionCoefficients; + private boolean valid; + + private static final double DEFAULT_REPORT_VAL = 0.0; + + /** + * Constructs a CalibrationResult with default values. + */ + public CalibrationResult() { + this("", DEFAULT_REPORT_VAL, DEFAULT_REPORT_VAL, DEFAULT_REPORT_VAL, + new double[9], new double[5]); + } + + /** + * Constructs a CalibrationResult with specified values. + * + * @param displayName The display name for the calibration. + * @param resX The X resolution of the calibration. + * @param resY The Y resolution of the calibration. + * @param reprojectionError The reprojection error of the calibration. + * @param camMatVector The camera matrix vector (9 elements). + * @param distortionCoefficients The distortion coefficients (5 elements). + */ + public CalibrationResult(String displayName, double resX, double resY, double reprojectionError, + double[] camMatVector, double[] distortionCoefficients) { + this.displayName = displayName; + this.resX = resX; + this.resY = resY; + this.reprojectionError = reprojectionError; + this.camMatVector = camMatVector.clone(); + this.distortionCoefficients = distortionCoefficients.clone(); + this.valid = true; + } + + /** + * Constructs a CalibrationResult from JSON data. (Package-private) + * + * @param json The JSON object containing calibration result data. + */ + protected CalibrationResult(JSONObject json) { + //default value construction + this(); + //parse json + if(json!=null) + { + try{ + this.displayName = json.optString("DISPLAY_NAME", ""); + this.reprojectionError = json.optDouble("REPROJECTION_ERROR", DEFAULT_REPORT_VAL); + this.resX = json.optDouble("RES_X", DEFAULT_REPORT_VAL); + this.resY = json.optDouble("RES_Y", DEFAULT_REPORT_VAL); + + JSONArray intrinsicsMatrix = json.optJSONArray("INTRINSICS_MATRIX"); + if (intrinsicsMatrix != null && intrinsicsMatrix.length() == 9) { + for (int i = 0; i < 9; i++) { + this.camMatVector[i] = intrinsicsMatrix.optDouble(i, 0.0); + } + } else { + this.valid = false; + } + + JSONArray distortionCoeffs = json.optJSONArray("DISTORTION_COEFFICIENTS"); + if (distortionCoeffs != null && distortionCoeffs.length() == 5) { + for (int i = 0; i < 5; i++) { + this.distortionCoefficients[i] = distortionCoeffs.optDouble(i, 0.0); + } + } else { + this.valid = false; + } + + if (this.resX == DEFAULT_REPORT_VAL || this.resY == DEFAULT_REPORT_VAL) { + this.valid = false; + } + } catch (Exception e){ } + } + } + + /** + * Checks if the calibration result is valid. + * + * @return true if the calibration result is valid, false otherwise. + */ + public boolean isValid() { + return valid && resX != 0 && resY != 0 && camMatVector[0] != 0; + } + + /** + * Gets the display name of the calibration. + * + * @return The display name. + */ + public String getDisplayName() { + return displayName; + } + + /** + * Gets the X resolution of the calibration. + * + * @return The X resolution. + */ + public double getResX() { + return resX; + } + + /** + * Gets the Y resolution of the calibration. + * + * @return The Y resolution. + */ + public double getResY() { + return resY; + } + + /** + * Gets the reprojection error of the calibration. + * + * @return The reprojection error. + */ + public double getReprojectionError() { + return reprojectionError; + } + + /** + * Gets the camera matrix vector. + * + * @return A copy of the camera matrix vector. + */ + public double[] getCamMatVector() { + return camMatVector.clone(); + } + + /** + * Gets the distortion coefficients. + * + * @return A copy of the distortion coefficients. + */ + public double[] getDistortionCoefficients() { + return distortionCoefficients.clone(); + } + + /** + * Converts a CalibrationResult to a JSONObject. + * + * @return A JSONObject representation of the CalibrationResult. + */ + protected JSONObject toJson() { + JSONObject json = new JSONObject(); + try { + json.put("DISPLAY_NAME", this.getDisplayName()); + json.put("REPROJECTION_ERROR", this.getReprojectionError()); + json.put("RES_X", this.getResX()); + json.put("RES_Y", this.getResY()); + json.put("INTRINSICS_MATRIX", new JSONArray(this.getCamMatVector())); + json.put("DISTORTION_COEFFICIENTS", new JSONArray(this.getDistortionCoefficients())); + } catch (JSONException e) { + e.printStackTrace(); + } + return json; + } + } +} \ No newline at end of file diff --git a/WilyCore/src/main/java/com/qualcomm/hardware/limelightvision/LLStatus.java b/WilyCore/src/main/java/com/qualcomm/hardware/limelightvision/LLStatus.java new file mode 100644 index 000000000000..8c3ae6c69707 --- /dev/null +++ b/WilyCore/src/main/java/com/qualcomm/hardware/limelightvision/LLStatus.java @@ -0,0 +1,247 @@ +package com.qualcomm.hardware.limelightvision; + +/* +Copyright (c) 2024 Limelight Vision + +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, +are permitted (subject to the limitations in the disclaimer below) provided that +the following conditions are met: + +Redistributions of source code must retain the above copyright notice, this list +of conditions and the following disclaimer. + +Redistributions in binary form must reproduce the above copyright notice, this +list of conditions and the following disclaimer in the documentation and/or +other materials provided with the distribution. + +Neither the name of Limelight Vision nor the names of its contributors may be used to +endorse or promote products derived from this software without specific prior +written permission. + +NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS +LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF +THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + +import org.json.JSONException; +import org.json.JSONObject; +import org.firstinspires.ftc.robotcore.external.navigation.Quaternion; +/** + * Represents the status of a Limelight. + */ +public class LLStatus { + private Quaternion cameraQuat; + private int cid; + private double cpu; + private double finalYaw; + private double fps; + private int hwType; + private int ignoreNT; + private int interfaceNeedsRefresh; + private String name; + private int pipeImgCount; + private int pipelineIndex; + private String pipelineType; + private double ram; + private int snapshotMode; + private double temp; + + /** + * Constructs an LLStatus object with default values. + */ + public LLStatus() { + this.cameraQuat = new Quaternion(1, 0, 0, 0, 0); + this.cid = 0; + this.cpu = 0.0; + this.finalYaw = 0.0; + this.fps = 0.0; + this.hwType = 0; + this.ignoreNT = 0; + this.interfaceNeedsRefresh = 0; + this.name = ""; + this.pipeImgCount = 0; + this.pipelineIndex = 0; + this.pipelineType = ""; + this.ram = 0.0; + this.snapshotMode = 0; + this.temp = 0.0; + } + + /** + * Constructs an LLStatus object from a JSON string. + * + * @param json The JSONobject containing Limelight status data. + */ + protected LLStatus(JSONObject json) { + this(); + if(json != null) + { + try { + JSONObject quatObj = json.optJSONObject("cameraQuat"); + if (quatObj != null) { + this.cameraQuat.w = (float)quatObj.optDouble("w", 1); + this.cameraQuat.x = (float)quatObj.optDouble("x", 0); + this.cameraQuat.y = (float)quatObj.optDouble("y", 0); + this.cameraQuat.z = (float)quatObj.optDouble("z", 0); + }; + this.cid = json.optInt("cid", 0); + this.cpu = json.optDouble("cpu", 0.0); + this.finalYaw = json.optDouble("finalYaw", 0.0); + this.fps = json.optDouble("fps", 0.0); + this.hwType = json.optInt("hwType", 0); + this.ignoreNT = json.optInt("ignoreNT", 0); + this.interfaceNeedsRefresh = json.optInt("interfaceNeedsRefresh", 0); + this.name = json.optString("name", ""); + this.pipeImgCount = json.optInt("pipeImgCount", 0); + this.pipelineIndex = json.optInt("pipelineIndex", 0); + this.pipelineType = json.optString("pipelineType", ""); + this.ram = json.optDouble("ram", 0.0); + this.snapshotMode = json.optInt("snapshotMode", 0); + this.temp = json.optDouble("temp", 0.0); + } catch (Exception e) { + + } + } + } + + /** + * @return The camera quaternion as a double array [w, x, y, z]. + */ + public Quaternion getCameraQuat() { + return cameraQuat; + } + + /** + * @return The camera sensor ID. + */ + public int getCid() { + return cid; + } + + /** + * @return The CPU usage as a percentage. + */ + public double getCpu() { + return cpu; + } + + /** + * @return The final yaw angle in degrees. + */ + public double getFinalYaw() { + return finalYaw; + } + + /** + * @return The frames per second being processed. + */ + public double getFps() { + return fps; + } + + /** + * @return The hardware type identifier. + */ + public int getHwType() { + return hwType; + } + + /** + * @return The ignore NetworkTables flag. + */ + private int getIgnoreNT() { + return ignoreNT; + } + + /** + * @return The interface needs refresh flag. + */ + private int getInterfaceNeedsRefresh() { + return interfaceNeedsRefresh; + } + + /** + * @return The name of the Limelight. + */ + public String getName() { + return name; + } + + /** + * @return The pipeline image count. + */ + public int getPipeImgCount() { + return pipeImgCount; + } + + /** + * @return The current pipeline index. + */ + public int getPipelineIndex() { + return pipelineIndex; + } + + /** + * @return The type of pipeline being used. + */ + public String getPipelineType() { + return pipelineType; + } + + /** + * @return The RAM usage as a percentage. + */ + public double getRam() { + return ram; + } + + /** + * @return The snapshot mode flag. + */ + public int getSnapshotMode() { + return snapshotMode; + } + + /** + * @return The temperature of the Limelight in degrees Celsius. + */ + public double getTemp() { + return temp; + } + + /** + * Returns a string representation of the LLStatus object. + * + * @return A string containing all the status parameters. + */ + @Override + public String toString() { + return "LLStatus{" + + "cameraQuat=" + cameraQuat.toString() + + ", cid=" + cid + + ", cpu=" + cpu + + ", finalYaw=" + finalYaw + + ", fps=" + fps + + ", hwType=" + hwType + + ", ignoreNT=" + ignoreNT + + ", interfaceNeedsRefresh=" + interfaceNeedsRefresh + + ", name='" + name + '\'' + + ", pipeImgCount=" + pipeImgCount + + ", pipelineIndex=" + pipelineIndex + + ", pipelineType='" + pipelineType + '\'' + + ", ram=" + ram + + ", snapshotMode=" + snapshotMode + + ", temp=" + temp + + '}'; + } +} \ No newline at end of file diff --git a/WilyCore/src/main/java/com/qualcomm/hardware/limelightvision/Limelight3A.java b/WilyCore/src/main/java/com/qualcomm/hardware/limelightvision/Limelight3A.java new file mode 100644 index 000000000000..550274b55af3 --- /dev/null +++ b/WilyCore/src/main/java/com/qualcomm/hardware/limelightvision/Limelight3A.java @@ -0,0 +1,733 @@ +package com.qualcomm.hardware.limelightvision; + +/* +Copyright (c) 2024 Limelight Vision + +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, +are permitted (subject to the limitations in the disclaimer below) provided that +the following conditions are met: + +Redistributions of source code must retain the above copyright notice, this list +of conditions and the following disclaimer. + +Redistributions in binary form must reproduce the above copyright notice, this +list of conditions and the following disclaimer in the documentation and/or +other materials provided with the distribution. + +Neither the name of Limelight Vision nor the names of its contributors may be used to +endorse or promote products derived from this software without specific prior +written permission. + +NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS +LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF +THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + +import com.qualcomm.robotcore.hardware.HardwareDevice; +import com.qualcomm.robotcore.util.SerialNumber; + +import org.firstinspires.ftc.robotcore.internal.usb.EthernetOverUsbSerialNumber; +import org.json.JSONException; +import org.json.JSONObject; +import org.json.JSONArray; +import java.io.BufferedReader; +import java.io.IOException; +import java.io.InputStreamReader; +import java.io.OutputStream; +import java.net.HttpURLConnection; +import java.net.InetAddress; +import java.net.URL; +import java.nio.charset.StandardCharsets; +import java.util.concurrent.Executors; +import java.util.concurrent.ScheduledExecutorService; +import java.util.concurrent.TimeUnit; +import com.qualcomm.hardware.limelightvision.LLResultTypes.*; +import com.qualcomm.hardware.limelightvision.LLFieldMap; +import java.util.concurrent.atomic.AtomicInteger; + + +/** + * Driver for Limelight3A Vision Sensor. + * + * {@link Limelight3A } provides support for the Limelight Vision Limelight3A Vision Sensor. + * + * @see Limelight + * + * Notes on configuration: + * + * The device presents itself, when plugged into a USB port on a Control Hub as an ethernet + * interface. A DHCP server running on the Limelight automatically assigns the Control Hub an + * ip address for the new ethernet interface. + * + * Since the Limelight is plugged into a USB port, it will be listed on the top level configuration + * activity along with the Control Hub Portal and other USB devices such as webcams. Typically + * serial numbers are displayed below the device's names. In the case of the Limelight device, the + * Control Hub's assigned ip address for that ethernet interface is used as the "serial number". + * + * Tapping the Limelight's name, transitions to a new screen where the user can rename the Limelight + * and specify the Limelight's ip address. Users should take care not to confuse the ip address of + * the Limelight itself, which can be configured through the Limelight settings page via a web browser, + * and the ip address the Limelight device assigned the Control Hub and which is displayed in small text + * below the name of the Limelight on the top level configuration screen. + */ +public class Limelight3A implements HardwareDevice +{ + private static final String TAG = "Limelight3A"; + private InetAddress inetAddress; + private EthernetOverUsbSerialNumber serialNumber; + private String name; + + private final String baseUrl; + private volatile LLResult latestResult; + private volatile ScheduledExecutorService executor; + private final Object executorLock = new Object(); + + private final AtomicInteger state = new AtomicInteger(0); + private static final int STOPPED = 0, STARTING = 1, RUNNING = 2; + + private volatile boolean isExecutorInitialized = false; + + private volatile long lastUpdateTime = 0; + private static final long ISCONNECTED_THRESHOLD = 250; + private static final int CONNECTION_TIMEOUT = 100; + private static final int GETREQUEST_TIMEOUT = 100; + private static final int POSTREQUEST_TIMEOUT = 15000; + private static final int DELETEREQUEST_TIMEOUT = 15000; + + private static final int PYTHON_INPUT_MAX_SIZE = 32; + private long pollIntervalMs = 10; // Default poll rate = 100Hz + private static final int MIN_POLL_RATE_HZ = 1; + private static final int MAX_POLL_RATE_HZ = 250; + + + public Limelight3A(SerialNumber serialNumber, String name, InetAddress ipAddress) { + this.name = name; + this.serialNumber = (EthernetOverUsbSerialNumber)serialNumber; + this.inetAddress = ipAddress; + this.baseUrl = "http://blargh:5807"; + this.executor = Executors.newSingleThreadScheduledExecutor(); + } + + /** + * Allows for re-initialization + */ + private synchronized void ensureExecutorRunning() { + synchronized (executorLock) { + //lock to make sure executor doesn't change right after we check here (bh) + if (executor == null || executor.isShutdown()) { + executor = Executors.newSingleThreadScheduledExecutor(); + isExecutorInitialized = false; + } + } + } + + /** + * Starts or resumes periodic polling of Limelight data. + */ + public synchronized void start() { + // handling rapid consecutive start calls with getandset bh + if (!state.compareAndSet(STOPPED, STARTING)) { + return; + } + + try { + ensureExecutorRunning(); + if (!isExecutorInitialized) { + executor.scheduleAtFixedRate(this::updateLatestResult, 0, pollIntervalMs, TimeUnit.MILLISECONDS); + isExecutorInitialized = true; + } + state.set(RUNNING); + } catch (Exception e) { + state.set(STOPPED); + } + } + + /** + * Pauses polling of Limelight data. + */ + public synchronized void pause() { + if (state.get() == RUNNING) { + state.set(STOPPED); + } + } + + + /** + * Stops polling of Limelight data. + */ + public synchronized void stop() { + state.set(STOPPED); + isExecutorInitialized = false; + if (!executor.isShutdown()) { + executor.shutdownNow(); + } + } + + /** + * Checks if the polling is enabled. + * + * @return true if the polling is enabled + */ + public boolean isRunning() { + return state.get() == RUNNING; + } + + /** + * Sets the poll rate in Hertz (Hz). Must be called before start() + * The rate is clamped between 1 and 250 Hz. + * + * @param rateHz The desired poll rate in Hz. + */ + public synchronized void setPollRateHz(int rateHz) { + if (state.get() == RUNNING) + { + return; // Early return if the executor is running + } + + int clampedRate = Math.max(MIN_POLL_RATE_HZ, Math.min(rateHz, MAX_POLL_RATE_HZ)); + this.pollIntervalMs = 1000 / clampedRate; + } + + /** + * Gets the time elapsed since the last update. + * + * @return The time in milliseconds since the last update. + */ + public long getTimeSinceLastUpdate() { + return System.currentTimeMillis() - lastUpdateTime; + } + + /** + * Checks if the Limelight is currently connected. + * + * @return true if connected, false otherwise. + */ + public boolean isConnected() { + return getTimeSinceLastUpdate() <= ISCONNECTED_THRESHOLD; + } + + /** + * Updates the latest result from the Limelight. + */ + private void updateLatestResult() { + if (state.get() != RUNNING) return; + + try { + + JSONObject result = sendGetRequest("/results"); + if (result != null) { + this.latestResult = LLResult.parse(result); + this.lastUpdateTime = System.currentTimeMillis(); + } + } + catch (Exception e){ + //todo + } + } + + /** + * Gets the latest result from the Limelight. + * + * @return The latest LLResult object. + */ + public LLResult getLatestResult(){ + if (this.latestResult == null) { + JSONObject fakeJSON = new JSONObject(); + this.latestResult = LLResult.parse(fakeJSON); + } + return this.latestResult; + } + + /** + * Gets the current status of the Limelight. + * + * @return A populated LLStatus object, or a default status object if request fails. + */ + public LLStatus getStatus() { + JSONObject statusJson = sendGetRequest("/status"); + if(statusJson == null) + { + return new LLStatus(); + } + return new LLStatus(statusJson); + } + + /** + * Gets the hardware report from the Limelight. The Hardware Report contains calibration data. + * This is a highly advanced feature that is not necessary for FTC Teams. Marked private for now. + * + * @return A JSONObject containing the hardware report. + */ + private JSONObject getHWReport() { + return sendGetRequest("/hwreport"); + } + + /** + * Reloads the current Limelight pipeline. + * + * @return true if successful, false otherwise. + */ + public boolean reloadPipeline() { + return sendPostRequest("/reload-pipeline", null); + } + + /** + * Gets the default pipeline. + * + * @return A JSONObject containing the default pipeline configuration. + */ + private JSONObject getPipelineDefault() { + return sendGetRequest("/pipeline-default"); + } + + /** + * Gets the pipeline at a specific index. + * + * @param index The index of the pipeline to retrieve. + * @return A JSONObject containing the pipeline configuration. + */ + private JSONObject getPipelineAtIndex(int index) { + return sendGetRequest("/pipeline-atindex?index=" + index); + } + + /** + * Switches to a pipeline at the specified index. + * + * @param index The index of the pipeline to switch to. + * @return true if successful, false otherwise. + */ + public boolean pipelineSwitch(int index) { + return sendPostRequest("/pipeline-switch?index=" + index, null); + } + + /** + * Gets the names of available snapscripts. + * This method is not necessary for FTC teams. Marked as private + * + * @return A JSONObject containing the names of available snapscripts. + */ + private JSONObject getSnapscriptNames() { + return sendGetRequest("/getsnapscriptnames"); + } + + /** + * Captures a snapshot with the given name. + * + * @param snapname The name to give to the captured snapshot. + * @return true if successful, false otherwise. + */ + public boolean captureSnapshot(String snapname) { + return sendPostRequest("/capture-snapshot?snapname=" + snapname, null); + } + + /** + * Gets the manifest of available snapshots. + * This method is not necessary for FTC teams. Marked as private + * + * @return A JSONObject containing the snapshot manifest. + */ + private JSONObject snapshotManifest() { + return sendGetRequest("/snapshotmanifest"); + } + + /** + * Deletes all snapshots. + * + * @return true if successful, false otherwise. + */ + public boolean deleteSnapshots() { + return sendDeleteRequest("/delete-snapshots"); + } + + /** + * Deletes a specific snapshot. + * + * @param snapname The name of the snapshot to delete. + * @return true if successful, false otherwise. + */ + public boolean deleteSnapshot(String snapname) { + return sendDeleteRequest("/delete-snapshot?snapname=" + snapname); + } + + /** + * Updates the current pipeline configuration. + * + * @param profileJson The new pipeline configuration as a JSONObject. + * @param flush Whether to flush the configuration to persistent storage. + * @return true if successful, false otherwise. + */ + private boolean updatePipeline(JSONObject profileJson, boolean flush) { + String url = "/update-pipeline" + (flush ? "?flush=1" : ""); + return sendPostRequest(url, profileJson.toString()); + } + + /** + * Updates the Python SnapScript inputs with 8 double values. + * + * @param input1 The first input value + * @param input2 The second input value + * @param input3 The third input value + * @param input4 The fourth input value + * @param input5 The fifth input value + * @param input6 The sixth input value + * @param input7 The seventh input value + * @param input8 The eighth input value + * @return True if successful, false otherwise. + */ + public boolean updatePythonInputs(double input1, double input2, double input3, double input4, + double input5, double input6, double input7, double input8) { + double[] inputs = new double[]{input1, input2, input3, input4, input5, input6, input7, input8}; + return updatePythonInputs(inputs); + } + + /** + * Updates the Python SnapScript inputs. + * + * @param inputs A double array containing the new Python inputs. + * @return True if successful, false otherwise. False if array is empty or contains more than 32 elements + */ + public boolean updatePythonInputs(double[] inputs) { + if (inputs == null || inputs.length == 0 || inputs.length > PYTHON_INPUT_MAX_SIZE) { + return false; // Invalid input size + } + + try { + JSONArray jsonArray = new JSONArray(inputs); + return sendPostRequest("/update-pythoninputs", jsonArray.toString()); + } catch (Exception e) { + // + return false; + } + } + + /** + * Updates the robot orientation for MegaTag2. + * + * @param yaw The yaw value of the robot, aligned with the field map + * @return true if successful, false otherwise. + */ + public boolean updateRobotOrientation(double yaw) { + double[] orientationData = {yaw, 0, 0, 0, 0, 0}; + try{ + JSONArray jsonArray = new JSONArray(orientationData); + return sendPostRequest("/update-robotorientation", jsonArray.toString()); + } catch (Exception e){ + return false; + } + } + + /** + * Uploads a pipeline to a specific slot. + * + * @param profileJson The new pipeline configuration as a JSONObject. + * @param index The index at which to upload the pipeline (null for default). + * @return true if successful, false otherwise. + */ + private boolean uploadPipeline(JSONObject profileJson, Integer index) { + String url = "/upload-pipeline" + (index != null ? "?index=" + index : ""); + return sendPostRequest(url, profileJson.toString()); + } + + /** + * Uploads a pipeline to a specific slot. + * + * @param jsonString The new pipeline configuration as a String containing JSON. + * @param index The index at which to upload the pipeline (null for default). + * @return true if successful, false otherwise. + */ + public boolean uploadPipeline(String jsonString, Integer index){ + String url = "/upload-pipeline" + (index != null ? "?index=" + index : ""); + return sendPostRequest(url, jsonString); + } + + /** + * Uploads a new fiducial field map. Early exits if map is empty or doesn't specify a type + * + * @param fieldmap The new field map configuration. + * @param index The index at which to upload the field map (null for default). + * @return true if successful, false otherwise. + */ + public boolean uploadFieldmap(LLFieldMap fieldmap, Integer index) { + if(!fieldmap.isValid()) + { + return false; + } + String url = "/upload-fieldmap" + (index != null ? "?index=" + index : ""); + return sendPostRequest(url, fieldmap.toJson().toString()); + } + + /** + * Uploads new Python code. + * + * @param pythonString The Python code as a string. + * @param index The index at which to upload the Python code (null for default). + * @return true if successful, false otherwise. + */ + public boolean uploadPython(String pythonString, Integer index) { + String url = "/upload-python" + (index != null ? "?index=" + index : ""); + return sendPostRequest(url, pythonString); + } + + /** + * Gets the default calibration data. + * + * @return A CalibrationResult containing the default calibration data. + */ + public CalibrationResult getCalDefault() { + JSONObject calData = sendGetRequest("/cal-default"); + return new CalibrationResult(calData); + } + + + /** + * Gets calibration data from the user-generated calibration file. + * + * @return A CalibrationResult containing the calibration data from the file. + */ + public CalibrationResult getCalFile() { + JSONObject calData = sendGetRequest("/cal-file"); + return new CalibrationResult(calData); + } + + /** + * Gets the calibration data from the Limelight EEPROM. + * + * @return A CalibrationResult containing the calibration data from the EEPROM. + */ + public CalibrationResult getCalEEPROM() { + JSONObject calData = sendGetRequest("/cal-eeprom"); + return new CalibrationResult(calData); + } + + /** + * Gets the latest calibration result. This result is not necessarily used by the camera in any way + * + * @return A CalibrationResult containing the latest calibration result. + */ + public CalibrationResult getCalLatest() { + JSONObject calData = sendGetRequest("/cal-latest"); + return new CalibrationResult(calData); + } + + /** + * Upload calibration data to the EEPROM + * + * @param calResult A CalibrationResult containing the new calibration data. + * @return true if successful, false otherwise. + */ + private boolean updateCalEEPROM(CalibrationResult calResult) { + return sendPostRequest("/cal-eeprom", calResult.toJson().toString()); + } + + /** + * Updates the calibration file stored in the Limelight filesystem + * + * @param calResult A CalibrationResult containing the new calibration data. + * @return true if successful, false otherwise. + */ + private boolean updateCalFile(CalibrationResult calResult) { + return sendPostRequest("/cal-file", calResult.toJson().toString()); + } + + /** + * Deletes the latest calibration data. + * + * @return true if successful, false otherwise. + */ + private boolean deleteCalLatest() { + return sendDeleteRequest("/cal-latest"); + } + + /** + * Deletes the calibration data from the EEPROM. + * + * @return true if successful, false otherwise. + */ + private boolean deleteCalEEPROM() { + return sendDeleteRequest("/cal-eeprom"); + } + + /** + * Deletes the calibration data from the file. + * + * @return true if successful, false otherwise. + */ + private boolean deleteCalFile() { + return sendDeleteRequest("/cal-file"); + } + + /** + * Sends a GET request to the specified endpoint. + * + * @param endpoint The endpoint to send the request to. + * @return A JSONObject containing the response, or null if the request fails. + */ + private JSONObject sendGetRequest(String endpoint) { + HttpURLConnection connection = null; + try { + String urlString = baseUrl + endpoint; + URL url = new URL(urlString); + connection = (HttpURLConnection) url.openConnection(); + connection.setRequestMethod("GET"); + connection.setReadTimeout(GETREQUEST_TIMEOUT); + connection.setConnectTimeout(CONNECTION_TIMEOUT); + + int responseCode = connection.getResponseCode(); + if (responseCode == HttpURLConnection.HTTP_OK) { + String response = readResponse(connection); + return new JSONObject(response); + } else { + //System.out.println("HTTP GET Error: " + responseCode); + } + } catch (Exception e) { + //e.printStackTrace(); + } finally { + if (connection != null) { + connection.disconnect(); + } + } + return null; + } + + /** + * Reads the response from an HTTP connection. + * + * @param connection The HttpURLConnection to read from. + * @return A String containing the response. + * @throws IOException If an I/O error occurs. + */ + private String readResponse(HttpURLConnection connection) throws IOException { + BufferedReader reader = new BufferedReader(new InputStreamReader(connection.getInputStream())); + StringBuilder response = new StringBuilder(); + String line; + + while ((line = reader.readLine()) != null) { + response.append(line); + } + reader.close(); + + return response.toString(); + } + + /** + * Sends a POST request to the specified endpoint. + * + * @param endpoint The endpoint to send the request to. + * @param data The data to send in the request body. + * @return true if successful, false otherwise. + */ + private boolean sendPostRequest(String endpoint, String data) { + HttpURLConnection connection = null; + try { + String urlString = baseUrl + endpoint; + URL url = new URL(urlString); + connection = (HttpURLConnection) url.openConnection(); + connection.setRequestMethod("POST"); + connection.setDoOutput(true); + connection.setRequestProperty("Content-Type", "application/json"); + connection.setReadTimeout(POSTREQUEST_TIMEOUT); + connection.setConnectTimeout(CONNECTION_TIMEOUT); + + if (data != null) { + try (OutputStream os = connection.getOutputStream()) { + byte[] input = data.getBytes(StandardCharsets.UTF_8); + os.write(input, 0, input.length); + } + } + + int responseCode = connection.getResponseCode(); + if (responseCode == HttpURLConnection.HTTP_OK) { + return true; + } else { + //System.out.println("HTTP POST Error: " + responseCode); + } + } catch (Exception e) { + //e.printStackTrace(); + } finally { + if (connection != null) { + connection.disconnect(); + } + } + return false; + } + + /** + * Sends a DELETE request to the specified endpoint. + * + * @param endpoint The endpoint to send the request to. + * @return true if successful, false otherwise. + */ + private boolean sendDeleteRequest(String endpoint) { + HttpURLConnection connection = null; + try { + String urlString = baseUrl + endpoint; + URL url = new URL(urlString); + connection = (HttpURLConnection) url.openConnection(); + connection.setRequestMethod("DELETE"); + connection.setReadTimeout(DELETEREQUEST_TIMEOUT); + connection.setConnectTimeout(CONNECTION_TIMEOUT); + + int responseCode = connection.getResponseCode(); + if (responseCode == HttpURLConnection.HTTP_OK) { + return true; + } else { + //System.out.println("HTTP DELETE Error: " + responseCode); + } + } catch (Exception e) { + //e.printStackTrace(); + } finally{ + if (connection != null) { + connection.disconnect(); + } + } + return false; + } + + /** + * Shuts down the Limelight connection and stops all ongoing processes. + */ + public void shutdown() { + stop(); + } + + @Override + public Manufacturer getManufacturer() { + return Manufacturer.LimelightVision; + } + + @Override + public String getDeviceName() { + return name; + } + + @Override + public String getConnectionInfo() { + return ""; + } + + @Override + public int getVersion() { + return 0; + } + + @Override + public void resetDeviceConfigurationForOpMode() { + } + + @Override + public void close() { + stop(); + } + +} + diff --git a/WilyCore/src/main/java/com/qualcomm/robotcore/hardware/AnalogInput.java b/WilyCore/src/main/java/com/qualcomm/robotcore/hardware/AnalogInput.java index f20a1a6e6cf0..958389666839 100644 --- a/WilyCore/src/main/java/com/qualcomm/robotcore/hardware/AnalogInput.java +++ b/WilyCore/src/main/java/com/qualcomm/robotcore/hardware/AnalogInput.java @@ -7,8 +7,6 @@ */ @DeviceProperties(name = "@string/configTypeAnalogInput", xmlTag = "AnalogInput", builtIn = true) public class AnalogInput implements HardwareDevice { - public AnalogInput(String deviceName) { } - @Override public Manufacturer getManufacturer() { return Manufacturer.Other; } public double getVoltage() { return 0; } diff --git a/WilyCore/src/main/java/com/qualcomm/robotcore/hardware/HardwareDevice.java b/WilyCore/src/main/java/com/qualcomm/robotcore/hardware/HardwareDevice.java index 0e7c9601d17d..cace5ab66d4d 100644 --- a/WilyCore/src/main/java/com/qualcomm/robotcore/hardware/HardwareDevice.java +++ b/WilyCore/src/main/java/com/qualcomm/robotcore/hardware/HardwareDevice.java @@ -37,7 +37,7 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE public interface HardwareDevice { enum Manufacturer { - Unknown, Other, Lego, HiTechnic, ModernRobotics, Adafruit, Matrix, Lynx, AMS, STMicroelectronics, Broadcom, DFRobot, SparkFun + Unknown, Other, Lego, HiTechnic, ModernRobotics, Adafruit, Matrix, Lynx, AMS, STMicroelectronics, Broadcom, DFRobot, SparkFun, LimelightVision } /** diff --git a/WilyCore/src/main/java/com/qualcomm/robotcore/hardware/LED.java b/WilyCore/src/main/java/com/qualcomm/robotcore/hardware/LED.java index 32e692391761..e0c14ea86bd4 100644 --- a/WilyCore/src/main/java/com/qualcomm/robotcore/hardware/LED.java +++ b/WilyCore/src/main/java/com/qualcomm/robotcore/hardware/LED.java @@ -34,8 +34,6 @@ @DeviceProperties(name = "@string/configTypeLED", xmlTag = "Led", builtIn = true, description = "@string/led_description") public class LED implements HardwareDevice, SwitchableLight { - public LED(String deviceName) { } - @Override public Manufacturer getManufacturer() { return Manufacturer.Other; @@ -74,6 +72,8 @@ public void enableLight(boolean enable) { public boolean isLightOn() { return false; } + + public void enable(boolean enableLed) { } /** * Turns the light on */ diff --git a/WilyCore/src/main/java/com/qualcomm/robotcore/util/SerialNumber.java b/WilyCore/src/main/java/com/qualcomm/robotcore/util/SerialNumber.java index 3ff07af8eaed..6f1cbd51cbef 100644 --- a/WilyCore/src/main/java/com/qualcomm/robotcore/util/SerialNumber.java +++ b/WilyCore/src/main/java/com/qualcomm/robotcore/util/SerialNumber.java @@ -31,5 +31,198 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE package com.qualcomm.robotcore.util; -public abstract class SerialNumber { +/* Copyright (c) 2014, 2015 Qualcomm Technologies Inc + +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, +are permitted (subject to the limitations in the disclaimer below) provided that +the following conditions are met: + +Redistributions of source code must retain the above copyright notice, this list +of conditions and the following disclaimer. + +Redistributions in binary form must reproduce the above copyright notice, this +list of conditions and the following disclaimer in the documentation and/or +other materials provided with the distribution. + +Neither the name of Qualcomm Technologies Inc nor the names of its contributors +may be used to endorse or promote products derived from this software without +specific prior written permission. + +NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS +LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ + +import androidx.annotation.Nullable; + +import com.google.gson.TypeAdapter; +import com.google.gson.annotations.JsonAdapter; +import com.google.gson.stream.JsonReader; +import com.google.gson.stream.JsonWriter; + +import org.firstinspires.ftc.robotcore.internal.system.Misc; + +import java.io.IOException; +import java.io.Serializable; +import java.util.HashMap; +import java.util.UUID; + +/** + */ +@SuppressWarnings("WeakerAccess") +@JsonAdapter(SerialNumber.GsonTypeAdapter.class) +public abstract class SerialNumber implements Serializable { + + protected SerialNumber(String serialNumberString) { + } + + public static SerialNumber createFake() { + return null; + } + + public static SerialNumber createEmbedded() { return null; } + + public static SerialNumber fromString(@Nullable String serialNumberString) { + return null; + } + + public static @Nullable SerialNumber fromStringOrNull(@Nullable String serialNumberString) { + return null; + } + + public static @Nullable SerialNumber fromUsbOrNull(@Nullable String serialNumberString) { + return null; + } + + /** Makes up a serial-number-like-thing for USB devices that internally lack a serial number. */ + public static SerialNumber fromVidPid(int vid, int pid, String connectionPath) { + return null; + } + + //------------------------------------------------------------------------------------------------ + // Gson + //------------------------------------------------------------------------------------------------ + + static class GsonTypeAdapter extends TypeAdapter { + @Override public void write(JsonWriter writer, SerialNumber serialNumber) throws IOException { + if (serialNumber==null) { + writer.nullValue(); + } else { + writer.value(serialNumber.getString()); + } + } + + @Override public SerialNumber read(JsonReader reader) throws IOException { + return SerialNumber.fromStringOrNull(reader.nextString()); + } + } + + //------------------------------------------------------------------------------------------------ + // Accessing + //------------------------------------------------------------------------------------------------ + + public boolean isVendorProduct() { + return false; + } + + /** + * Returns whether the indicated serial number is one of the legacy + * fake serial number forms or not. + * @return whether the the serial number is a legacy fake form of serial number + */ + public boolean isFake() { + return false; + } + + /** + * Returns whether the serial number is one of an actual USB device. + */ + public boolean isUsb() { + return false; + } + + /** + * Returns whether the serial number is the one used for the embedded + * Expansion Hub inside a Rev Control Hub. + */ + public boolean isEmbedded() { + return false; + } + + /** + * Returns the string contents of the serial number. Result is not intended to be + * displayed to humans. + * @see #toString() + */ + public String getString() { return ""; } + + + /** + */ + public SerialNumber getScannableDeviceSerialNumber() { + return this; + } + + //------------------------------------------------------------------------------------------------ + // Comparison + //------------------------------------------------------------------------------------------------ + + public boolean matches(Object pattern) { + return this.equals(pattern); + } + + @Override + public boolean equals(Object object) { + if (object == null) return false; + if (object == this) return true; + + if (object instanceof SerialNumber) { + return false; + } + + if (object instanceof String) { + return this.equals((String)object); + } + + return false; + } + + // separate method to avoid annoying Android Studio inspection warnings when comparing SerialNumber against String + public boolean equals(String string) { + return false; + } + + @Override + public int hashCode() { + return 0; + } + + //------------------------------------------------------------------------------------------------ + // Serial number display name management + //------------------------------------------------------------------------------------------------ + + protected static final HashMap deviceDisplayNames = new HashMap(); + + public static void noteSerialNumberType(SerialNumber serialNumber, String typeName) { + synchronized (deviceDisplayNames) { + deviceDisplayNames.put(serialNumber.getString(), Misc.formatForUser("%s [%s]", typeName, serialNumber)); + } + } + + public static String getDeviceDisplayName(SerialNumber serialNumber) { + synchronized (deviceDisplayNames) { + String result = deviceDisplayNames.get(serialNumber.getString()); + return result; + } + } + } diff --git a/WilyCore/src/main/java/com/wilyworks/simulator/framework/MechSim.java b/WilyCore/src/main/java/com/wilyworks/simulator/framework/MechSim.java index 2c6ace9ed384..bec03dd7b1f1 100644 --- a/WilyCore/src/main/java/com/wilyworks/simulator/framework/MechSim.java +++ b/WilyCore/src/main/java/com/wilyworks/simulator/framework/MechSim.java @@ -3,16 +3,20 @@ import static com.wilyworks.simulator.WilyCore.time; import com.acmerobotics.roadrunner.Pose2d; +import com.qualcomm.robotcore.hardware.AnalogInput; import com.qualcomm.robotcore.hardware.CRServo; import com.qualcomm.robotcore.hardware.DcMotorEx; import com.qualcomm.robotcore.hardware.HardwareDevice; import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.LED; +import com.qualcomm.robotcore.hardware.NormalizedColorSensor; import com.qualcomm.robotcore.hardware.NormalizedRGBA; import com.qualcomm.robotcore.hardware.Servo; import com.wilyworks.simulator.WilyCore; import com.wilyworks.simulator.helpers.Point; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; + import java.awt.AlphaComposite; import java.awt.BasicStroke; import java.awt.Color; @@ -119,8 +123,7 @@ protected void renderLeds(Graphics2D g, Pose2d pose) { // Hooked class for measuring the position of the drum: class DrumAnalogInput extends WilyAnalogInput { DecodeSlowBotMechSim mechSim; - DrumAnalogInput(String deviceName, DecodeSlowBotMechSim mechSim) { - super(deviceName); + DrumAnalogInput(DecodeSlowBotMechSim mechSim) { this.mechSim = mechSim; } @@ -136,41 +139,58 @@ public double getVoltage() { class DrumColorSensor extends WilyNormalizedColorSensor { DecodeSlowBotMechSim mechSim; int idMask; // Sensor 0 or 1 - DrumColorSensor(String deviceName, DecodeSlowBotMechSim mechSim, int index) { - super(deviceName); + DrumColorSensor(DecodeSlowBotMechSim mechSim, int index) { this.mechSim = mechSim; this.idMask = 1 << index; } - public NormalizedRGBA getNormalizedColors() { + + // Returns true if this sensor can read a ball; false if a hole in the ball is positioned + // over the sensor. + boolean sensorCanReadBall() { // Every time we get a new ball, reset our variations: if (mechSim.colorSensorMask == -1) { mechSim.colorSensorMask = 1 + (int)(Math.random() * 3.0); // Mask = 1, 2 or 3 } - NormalizedRGBA color = new NormalizedRGBA(); + return ((mechSim.colorSensorMask & idMask) != 0); + } + + @Override + public NormalizedRGBA getNormalizedColors() { + NormalizedRGBA normalizedColor = new NormalizedRGBA(); // Simulate the ball holes for some reads: - if ((mechSim.colorSensorMask & idMask) != 0) { - // Figure out what slot is being input into, if any: - int slot = mechSim.findDrumSlot(mechSim.INTAKE_POSITIONS); - if (slot != -1) { - DecodeSlowBotMechSim.Ball ball = mechSim.slotBalls.get(slot); - if (ball != null) { + int rgbColor = 0; + // Figure out what slot is being input into, if any: + int slot = mechSim.findDrumSlot(mechSim.INTAKE_POSITIONS); + if (slot != -1) { + DecodeSlowBotMechSim.Ball ball = mechSim.slotBalls.get(slot); + if (ball != null) { + // There's ball over the sensors. See if they can be read: + if (sensorCanReadBall()) { if (ball.color == DecodeSlowBotMechSim.BallColor.GREEN) { - color.green = 1; + rgbColor = android.graphics.Color.HSVToColor(new float[]{175, 1, 1}); } else { - color.red = 0.5f; - color.blue = 0.5f; + rgbColor = android.graphics.Color.HSVToColor(new float[]{210, 1, 1}); } } } } - return color; + normalizedColor.red = new Color(rgbColor).getRed() / 255.0f; + normalizedColor.green = new Color(rgbColor).getGreen() / 255.0f; + normalizedColor.blue = new Color(rgbColor).getBlue() / 255.0f; + return normalizedColor; + } + + @Override + public double getDistance(DistanceUnit unit) { + int slot = mechSim.findDrumSlot(mechSim.INTAKE_POSITIONS); + boolean ballPositionedForRead = (slot != -1) && mechSim.slotBalls.get(slot) != null; + return ballPositionedForRead && sensorCanReadBall() ? unit.fromMm(18) : unit.fromMm(70); } } // Let us ramp up the launcher motor velocity. class LaunchMotor extends WilyDcMotorEx { - LaunchMotor(String deviceName) { super(deviceName); } double targetVelocity; double actualVelocity; @@ -247,6 +267,9 @@ public Ball(BallColor color, double x, double y) { // Hooked devices: LaunchMotor upperLaunchMotor; LaunchMotor lowerLaunchMotor; + AnalogInput analogDrum; + NormalizedColorSensor sensorColor1; + NormalizedColorSensor sensorColor2; DcMotorEx intakeMotor; Servo drumServo; Servo transferServo; @@ -309,19 +332,19 @@ public HardwareDevice hookDevice(String name, HardwareDevice device) { // There have outputs: if (name.equals("motULauncher")) { - device = upperLaunchMotor = new LaunchMotor(device.getDeviceName()); + device = upperLaunchMotor = new LaunchMotor(); } if (name.equals("motLLauncher")) { - device = lowerLaunchMotor = new LaunchMotor(device.getDeviceName()); + device = lowerLaunchMotor = new LaunchMotor(); } if (name.equals("analogDrum")) { - device = new DrumAnalogInput(device.getDeviceName(), this); + device = analogDrum = new DrumAnalogInput(this); } if (name.equals("sensorColor1")) { - device = new DrumColorSensor(device.getDeviceName(), this, 0); + device = sensorColor1 = new DrumColorSensor(this, 0); } if (name.equals("sensorColor2")) { - device = new DrumColorSensor(device.getDeviceName(), this, 1); + device = sensorColor2 = new DrumColorSensor(this, 1); } return device; } @@ -342,6 +365,12 @@ void verifyState() { throw new RuntimeException("Missing forward feeder servo"); if (backwardFeederServo == null) throw new RuntimeException("Missing backward feeder servo"); + if (analogDrum == null) + throw new RuntimeException("Missing analog drum"); + if (sensorColor1 == null) + throw new RuntimeException("Missing color sensor 1"); + if (sensorColor2 == null) + throw new RuntimeException("Missing color sensor 2"); } void render(Graphics2D g, Pose2d pose) { @@ -493,12 +522,9 @@ void simulate(Pose2d pose, double deltaT) { // backwards: if (upperLaunchMotor.getVelocity() < 0) { if (forwardFeederServo.getPower() >= 0) { - throw new RuntimeException("That's weird, one launch motor runs backwards and the other doesn't?"); - } - if (forwardFeederServo.getPower() > 0) { throw new RuntimeException("When running launch motors backwards, forward feeder servo must too."); } - if (backwardFeederServo.getPower() > 0) { + if (backwardFeederServo.getPower() >= 0) { throw new RuntimeException("When running launch motors backwards, backward feeder servo must too."); } // If the slot is empty, fill it up with a ball! @@ -519,6 +545,12 @@ void simulate(Pose2d pose, double deltaT) { if (targetDrumPosition != actualDrumPosition) { throw new RuntimeException("The drum is moving during a transfer. That will break things!"); } + if (upperLaunchMotor.getVelocity() <= 0) { + throw new RuntimeException("A transfer is requested when upper launcher motor isn't running forward. That won't work!"); + } + if (lowerLaunchMotor.getVelocity() <= 0) { + throw new RuntimeException("A transfer is requested when lower launcher motor isn't running forward. That won't work!"); + } if (forwardFeederServo.getPower() < FEEDER_POWER) { throw new RuntimeException("A transfer is requested when forward feeder servo isn't running. That won't work!"); } diff --git a/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyDcMotorEx.java b/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyDcMotorEx.java index 5ca9cf0e1afe..8000e43ffa1e 100644 --- a/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyDcMotorEx.java +++ b/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyDcMotorEx.java @@ -11,7 +11,6 @@ * Wily Works DcMotorEx implementation. */ public class WilyDcMotorEx extends WilyHardwareDevice implements DcMotorEx { - WilyDcMotorEx(String deviceName) { super(deviceName); } RunMode mode; double velocity; double power; diff --git a/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyGamepad.java b/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyGamepad.java index a582fbdac6f4..429961f96acb 100644 --- a/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyGamepad.java +++ b/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyGamepad.java @@ -103,6 +103,44 @@ protected void updateAllButtons(WilyGamepad gamepad) { */ public class WilyGamepad { + + public enum Type { + // Do NOT change the order/names of existing entries, + // you will break backwards compatibility!! + UNKNOWN(LegacyType.UNKNOWN), + LOGITECH_F310(LegacyType.LOGITECH_F310), + XBOX_360(LegacyType.XBOX_360), + SONY_PS4(LegacyType.SONY_PS4), // This indicates a PS4-compatible controller that is being used through our compatibility mode + SONY_PS4_SUPPORTED_BY_KERNEL(LegacyType.SONY_PS4); // This indicates a PS4-compatible controller that is being used through the DualShock 4 Linux kernel driver. + + private final LegacyType correspondingLegacyType; + Type(LegacyType correspondingLegacyType) { + this.correspondingLegacyType = correspondingLegacyType; + } + } + + // LegacyType is necessary because robocol gamepad version 3 was written in a way that was not + // forwards-compatible, so we have to keep sending V3-compatible values. + public enum LegacyType { + // Do NOT change the order or names of existing entries, or add new entries. + // You will break backwards compatibility!! + UNKNOWN, + LOGITECH_F310, + XBOX_360, + SONY_PS4; + } + + /** + * Get the type of gamepad as a {@link Type}. This method defaults to "UNKNOWN". + * @return gamepad type + */ + public Type type() { + return type; + } + + @SuppressWarnings("UnusedAssignment") + public volatile Type type = Type.XBOX_360; // IntelliJ thinks this is redundant, but it is NOT. Must be a bug in the analyzer? + public volatile float left_stick_x = 0f; public volatile float left_stick_y = 0f; public volatile float right_stick_x = 0f; diff --git a/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyHardwareDevice.java b/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyHardwareDevice.java index 6914649e7da5..42aab53a9722 100644 --- a/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyHardwareDevice.java +++ b/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyHardwareDevice.java @@ -7,19 +7,13 @@ * Wily Works device subclass implementation. */ public class WilyHardwareDevice implements HardwareDevice { - String deviceName; - WilyHardwareDevice(String deviceName) { - this.deviceName = deviceName; - } @Override public Manufacturer getManufacturer() { return Manufacturer.Unknown; } @Override - public String getDeviceName() { - return deviceName; - } + public String getDeviceName() { return ""; } @Override public String getConnectionInfo() { diff --git a/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyHardwareMap.java b/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyHardwareMap.java index e04e3432ea27..f6c7e52b6b60 100644 --- a/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyHardwareMap.java +++ b/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyHardwareMap.java @@ -6,6 +6,8 @@ import androidx.annotation.NonNull; import androidx.annotation.Nullable; +import com.qualcomm.hardware.gobilda.GoBildaPinpointDriver; +import com.qualcomm.hardware.limelightvision.Limelight3A; import com.qualcomm.hardware.sparkfun.SparkFunOTOS; import com.qualcomm.robotcore.hardware.AnalogInput; import com.qualcomm.robotcore.hardware.CRServo; @@ -37,7 +39,6 @@ import org.firstinspires.ftc.robotcore.external.navigation.Quaternion; import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles; import org.firstinspires.ftc.robotcore.internal.system.Deadline; -import org.swerverobotics.ftc.GoBildaPinpointDriver; import org.swerverobotics.ftc.UltrasonicDistanceSensor; import java.util.ArrayList; @@ -58,7 +59,6 @@ * Wily Works simulated IMU implementation. */ class WilyIMU extends WilyHardwareDevice implements IMU { - WilyIMU(String deviceName) { super(deviceName); } double startYaw; @Override public boolean initialize(Parameters parameters) { @@ -106,7 +106,6 @@ public AngularVelocity getRobotAngularVelocity(AngleUnit angleUnit) { * Wily Works voltage sensor implementation. */ class WilyVoltageSensor extends WilyHardwareDevice implements VoltageSensor { - WilyVoltageSensor(String deviceName) { super(deviceName); } @Override public double getVoltage() { return 13.0; @@ -122,7 +121,6 @@ public String getDeviceName() { * Wily Works distance sensor implementation. */ class WilyDistanceSensor extends WilyHardwareDevice implements DistanceSensor { - WilyDistanceSensor(String deviceName) { super(deviceName); } @Override public double getDistance(DistanceUnit unit) { return unit.fromMm(65535); } // Distance when not responding } @@ -130,8 +128,7 @@ class WilyDistanceSensor extends WilyHardwareDevice implements DistanceSensor { /** * Wily Works normalized color sensor implementation. */ -class WilyNormalizedColorSensor extends WilyHardwareDevice implements NormalizedColorSensor { - WilyNormalizedColorSensor(String deviceName) { super(deviceName); } +class WilyNormalizedColorSensor extends WilyHardwareDevice implements NormalizedColorSensor, DistanceSensor { @Override public NormalizedRGBA getNormalizedColors() { return new NormalizedRGBA(); } @@ -140,14 +137,17 @@ class WilyNormalizedColorSensor extends WilyHardwareDevice implements Normalized @Override public void setGain(float newGain) { } + + @Override + public double getDistance(DistanceUnit unit) { + return 0; + } } /** * Wily Works color sensor implementation. */ class WilyColorSensor extends WilyHardwareDevice implements ColorSensor { - WilyColorSensor(String deviceName) { super(deviceName); } - @Override public int red() { return 0; } @@ -180,7 +180,6 @@ class WilyWebcam extends WilyHardwareDevice implements WebcamName { WilyWorks.Config.Camera wilyCamera; WilyWebcam(String deviceName) { - super(deviceName); for (WilyWorks.Config.Camera camera: WilyCore.config.cameras) { if (camera.name.equals(deviceName)) { wilyCamera = camera; @@ -247,7 +246,6 @@ public boolean isAttached() { * Wily Works ServoController implementation. */ class WilyServoController extends WilyHardwareDevice implements ServoController { - WilyServoController(String deviceName) { super(deviceName); } @Override public void pwmEnable() { } @@ -271,12 +269,11 @@ public void close() { } * Wily Works Servo implementation. */ class WilyServo extends WilyHardwareDevice implements Servo { - WilyServo(String deviceName) { super(deviceName); } double position; @Override public ServoController getController() { - return new WilyServoController(deviceName); + return new WilyServoController(); } @Override @@ -310,13 +307,12 @@ public void scaleRange(double min, double max) { * Wily Works CRServo implementation. */ class WilyCRServo extends WilyHardwareDevice implements CRServo { - WilyCRServo(String deviceName) { super(deviceName); } double power; Direction direction; @Override public ServoController getController() { - return new WilyServoController(deviceName); + return new WilyServoController(); } @Override @@ -349,7 +345,6 @@ public double getPower() { * Wily Works DigitalChannel implementation. */ class WilyDigitalChannel extends WilyHardwareDevice implements DigitalChannel { - WilyDigitalChannel(String deviceName) { super(deviceName); } boolean state; @Override @@ -376,7 +371,6 @@ class WilyLED extends LED { double y; boolean isRed; WilyLED(String deviceName) { - super(deviceName); WilyWorks.Config.LEDIndicator wilyLed = null; for (WilyWorks.Config.LEDIndicator led: WilyCore.config.ledIndicators) { if (led.name.equals(deviceName)) { @@ -393,9 +387,10 @@ class WilyLED extends LED { } @Override - public void enableLight(boolean enable) { - this.enable = enable; - } + public void enable(boolean enableLed) { this.enable = enableLed; } + + @Override + public void enableLight(boolean enable) { enable(enable); } @Override public boolean isLightOn() { @@ -407,7 +402,6 @@ public boolean isLightOn() { * Wily Works AnalogInput implementation. */ class WilyAnalogInput extends AnalogInput { - WilyAnalogInput(String deviceName) { super(deviceName); } @Override public double getVoltage() { return 0; } @@ -435,10 +429,13 @@ public class WilyHardwareMap implements Iterable { public DeviceMapping ultrasonicDistanceSensor = new DeviceMapping<>(UltrasonicDistanceSensor.class); public DeviceMapping analogInput = new DeviceMapping<>(AnalogInput.class); public DeviceMapping imu = new DeviceMapping<>(IMU.class); + public DeviceMapping limelight = new DeviceMapping<>(Limelight3A.class); protected Map> allDevicesMap = new HashMap<>(); protected List allDevicesList = new ArrayList<>(); public WilyHardwareMap() { + // Road Runner expects this object to be already created becaues it references + // hardwareMap.voltageSensor.iterator().next() directly: put("voltage_sensor", VoltageSensor.class); } @@ -473,14 +470,15 @@ public T get(Class classOrInterface, String deviceName) { return result; } - @Deprecated public HardwareDevice get(String deviceName) { - for (HardwareDevice device: allDevicesList) { - if (device.getDeviceName().equals(deviceName)) { + deviceName = deviceName.trim(); + List list = allDevicesMap.get(deviceName); + if (list != null) { + for (HardwareDevice device : list) { + initializeDeviceIfNecessary(device); return device; } } - throw new IllegalArgumentException("Use the typed version of get(), e.g. get(DcMotorEx.class, \"leftMotor\")"); } @@ -494,50 +492,55 @@ public synchronized void put(String deviceName, Class klass) { } HardwareDevice device; if (CRServo.class.isAssignableFrom(klass)) { - device = new WilyCRServo(deviceName); + device = new WilyCRServo(); crservo.put(deviceName, (CRServo) device); } else if (Servo.class.isAssignableFrom(klass)) { - device = new WilyServo(deviceName); + device = new WilyServo(); servo.put(deviceName, (Servo) device); } else if (DcMotor.class.isAssignableFrom(klass)) { - device = new WilyDcMotorEx(deviceName); + device = new WilyDcMotorEx(); dcMotor.put(deviceName, (DcMotor) device); } else if (VoltageSensor.class.isAssignableFrom(klass)) { - device = new WilyVoltageSensor(deviceName); + device = new WilyVoltageSensor(); voltageSensor.put(deviceName, (VoltageSensor) device); } else if (DistanceSensor.class.isAssignableFrom(klass)) { - device = new WilyDistanceSensor(deviceName); + device = new WilyDistanceSensor(); distanceSensor.put(deviceName, (DistanceSensor) device); } else if (NormalizedColorSensor.class.isAssignableFrom(klass)) { - device = new WilyNormalizedColorSensor(deviceName); + device = new WilyNormalizedColorSensor(); normalizedColorSensor.put(deviceName, (NormalizedColorSensor) device); } else if (ColorSensor.class.isAssignableFrom(klass)) { - device = new WilyColorSensor(deviceName); + device = new WilyColorSensor(); colorSensor.put(deviceName, (ColorSensor) device); } else if (WebcamName.class.isAssignableFrom(klass)) { device = new WilyWebcam(deviceName); webcamName.put(deviceName, (WebcamName) device); } else if (DigitalChannel.class.isAssignableFrom(klass)) { - device = new WilyDigitalChannel(deviceName); + device = new WilyDigitalChannel(); digitalChannel.put(deviceName, (DigitalChannel) device); } else if (LED.class.isAssignableFrom(klass)) { device = new WilyLED(deviceName); led.put(deviceName, (LED) device); + } else if (AnalogInput.class.isAssignableFrom(klass)) { + device = new WilyAnalogInput(); + analogInput.put(deviceName, (WilyAnalogInput) device); + } else if (IMU.class.isAssignableFrom(klass)) { + device = new WilyIMU(); + imu.put(deviceName, (WilyIMU) device); + + // Not actually built into HardwareMap.java: + } else if (UltrasonicDistanceSensor.class.isAssignableFrom(klass)) { + device = new WilyUltrasonicDistanceSensor(deviceName); + ultrasonicDistanceSensor.put(deviceName, (WilyUltrasonicDistanceSensor) device); } else if (SparkFunOTOS.class.isAssignableFrom(klass)) { device = new SparkFunOTOS(null); sparkFunOTOS.put(deviceName, (SparkFunOTOS) device); } else if (GoBildaPinpointDriver.class.isAssignableFrom(klass)) { device = new GoBildaPinpointDriver(null, false); goBildaPinpointDrivers.put(deviceName, (GoBildaPinpointDriver) device); - } else if (UltrasonicDistanceSensor.class.isAssignableFrom(klass)) { - device = new WilyUltrasonicDistanceSensor(deviceName); - ultrasonicDistanceSensor.put(deviceName, (WilyUltrasonicDistanceSensor) device); - } else if (AnalogInput.class.isAssignableFrom(klass)) { - device = new WilyAnalogInput(deviceName); - analogInput.put(deviceName, (WilyAnalogInput) device); - } else if (IMU.class.isAssignableFrom(klass)) { - device = new WilyIMU(deviceName); - imu.put(deviceName, (WilyIMU) device); + } else if (Limelight3A.class.isAssignableFrom(klass)) { + device = new Limelight3A(null, "", null); + limelight.put(deviceName, (Limelight3A) device); } else { throw new IllegalArgumentException("Unexpected device type for HardwareMap"); } @@ -560,9 +563,10 @@ private void initializeMultipleDevicesIfNecessary(Iterable getAllNames(Class classOrInterface) { SortedSet result = new TreeSet<>(); - for (HardwareDevice device: allDevicesList) { + for (String userName: allDevicesMap.keySet()) { + HardwareDevice device = allDevicesMap.get(userName).get(0); if (classOrInterface.isInstance(device)) { - result.add(device.getDeviceName()); + result.add(userName); } } return result; diff --git a/WilyCore/src/main/java/org/firstinspires/ftc/robotcore/external/navigation/AngleUnit.java b/WilyCore/src/main/java/org/firstinspires/ftc/robotcore/external/navigation/AngleUnit.java index 0e1d55ab76ee..3a266370f70c 100644 --- a/WilyCore/src/main/java/org/firstinspires/ftc/robotcore/external/navigation/AngleUnit.java +++ b/WilyCore/src/main/java/org/firstinspires/ftc/robotcore/external/navigation/AngleUnit.java @@ -223,4 +223,14 @@ public static float normalizeRadians(float radians) { return (float)normalizeRadians((double)radians); } + + public UnnormalizedAngleUnit getUnnormalized() + { + switch (this) + { + default: + case RADIANS: return UnnormalizedAngleUnit.RADIANS; + case DEGREES: return UnnormalizedAngleUnit.DEGREES; + } + } } diff --git a/WilyCore/src/main/java/org/firstinspires/ftc/robotcore/external/navigation/Pose3D.java b/WilyCore/src/main/java/org/firstinspires/ftc/robotcore/external/navigation/Pose3D.java new file mode 100644 index 000000000000..ad88ccbabc06 --- /dev/null +++ b/WilyCore/src/main/java/org/firstinspires/ftc/robotcore/external/navigation/Pose3D.java @@ -0,0 +1,89 @@ +package org.firstinspires.ftc.robotcore.external.navigation; + +/* + * Copyright (c) 2024 Dryw Wade + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to + * endorse or promote products derived from this software without specific prior + * written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR + * TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF + * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +import androidx.annotation.NonNull; + +import org.firstinspires.ftc.robotcore.external.navigation.Position; +import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles; + +import java.util.Locale; + +/** + * Pose3D represents the position and orientation of an object in 3D space. + */ +public class Pose3D +{ + protected final Position position; + protected final YawPitchRollAngles orientation; + + public Pose3D(Position position, YawPitchRollAngles orientation) + { + this.position = position; + this.orientation = orientation; + } + + //---------------------------------------------------------------------------------------------- + // Formatting + //---------------------------------------------------------------------------------------------- + + @NonNull + @Override public String toString() + { + return String.format(Locale.getDefault(), + "position=%s, orientation=%s", + position.toString(), + orientation.toString()); + } + + /** + * A 3D orientation. + * + * The axis mapping is defined by the code that creates objects from this class. One should not assume that + * pitch, for example, is along the x axis. Consult the documentation of the API that returns + * a Pose3D for its axis mapping. + */ + public YawPitchRollAngles getOrientation() + { + return orientation; + } + + /** + * A 3D position. + */ + public Position getPosition() + { + return position; + } +} diff --git a/WilyCore/src/main/java/org/firstinspires/ftc/robotcore/external/navigation/Position.java b/WilyCore/src/main/java/org/firstinspires/ftc/robotcore/external/navigation/Position.java new file mode 100644 index 000000000000..feb87ba7c310 --- /dev/null +++ b/WilyCore/src/main/java/org/firstinspires/ftc/robotcore/external/navigation/Position.java @@ -0,0 +1,100 @@ +package org.firstinspires.ftc.robotcore.external.navigation; + +/* +Copyright (c) 2016 Robert Atkinson + +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, +are permitted (subject to the limitations in the disclaimer below) provided that +the following conditions are met: + +Redistributions of source code must retain the above copyright notice, this list +of conditions and the following disclaimer. + +Redistributions in binary form must reproduce the above copyright notice, this +list of conditions and the following disclaimer in the documentation and/or +other materials provided with the distribution. + +Neither the name of Robert Atkinson nor the names of his contributors may be used to +endorse or promote products derived from this software without specific prior +written permission. + +NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS +LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF +THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ +import java.util.Locale; + +/** + * Instances of {@link Position} represent a three-dimensional distance in a particular distance unit. + * + * @see Position + */ +public class Position +{ + //---------------------------------------------------------------------------------------------- + // State + //---------------------------------------------------------------------------------------------- + + public DistanceUnit unit; + + public double x; + public double y; + public double z; + + /** + * the time on the System.nanoTime() clock at which the data was acquired. If no + * timestamp is associated with this particular set of data, this value is zero. + */ + public long acquisitionTime; + + //---------------------------------------------------------------------------------------------- + // Construction + //---------------------------------------------------------------------------------------------- + + public Position() + { + this(DistanceUnit.MM, 0, 0, 0, 0); + } + + public Position(DistanceUnit unit, double x, double y, double z, long acquisitionTime) + { + this.unit = unit; + this.x = x; + this.y = y; + this.z = z; + this.acquisitionTime = acquisitionTime; + } + + public Position toUnit(DistanceUnit distanceUnit) + { + if (distanceUnit != this.unit) + { + return new Position(distanceUnit, + distanceUnit.fromUnit(this.unit, x), + distanceUnit.fromUnit(this.unit, y), + distanceUnit.fromUnit(this.unit, z), + this.acquisitionTime); + } + else + return this; + } + + //---------------------------------------------------------------------------------------------- + // Formatting + //---------------------------------------------------------------------------------------------- + + @Override public String toString() + { + return String.format(Locale.getDefault(), "(%.3f %.3f %.3f)%s", x, y, z, unit.toString()); + } +} diff --git a/WilyCore/src/main/java/org/firstinspires/ftc/robotcore/external/navigation/UnnormalizedAngleUnit.java b/WilyCore/src/main/java/org/firstinspires/ftc/robotcore/external/navigation/UnnormalizedAngleUnit.java new file mode 100644 index 000000000000..724c144de3c7 --- /dev/null +++ b/WilyCore/src/main/java/org/firstinspires/ftc/robotcore/external/navigation/UnnormalizedAngleUnit.java @@ -0,0 +1,198 @@ +package org.firstinspires.ftc.robotcore.external.navigation; + +/* +Copyright (c) 2016 Robert Atkinson + +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, +are permitted (subject to the limitations in the disclaimer below) provided that +the following conditions are met: + +Redistributions of source code must retain the above copyright notice, this list +of conditions and the following disclaimer. + +Redistributions in binary form must reproduce the above copyright notice, this +list of conditions and the following disclaimer in the documentation and/or +other materials provided with the distribution. + +Neither the name of Robert Atkinson nor the names of his contributors may be used to +endorse or promote products derived from this software without specific prior +written permission. + +NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS +LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF +THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + +/** + * An {@link UnnormalizedAngleUnit} represents angles in different units of measure and + * provides utility methods to convert across units. {@link UnnormalizedAngleUnit} does not + * maintain angle information internally, but only helps organize + * and use angle measures that may be maintained separately across various contexts. + *

+ * Angles can be distinguished along (at least) two axes: + * Normalized angles are of most utility when dealing with physical angles, as normalization + * removes ambiguity of representation. In particular, two angles can be compared for equality + * by subtracting them, normalizing, and testing whether the absolute value of the result is + * smaller than some tolerance threshold. This approach neatly handles all cases of cyclical + * wrapping without unexpected discontinuities. + *

+ *

+ * Unnormalized angles can be handy when the angular quantity is not a physical angle but some + * related quantity such as an angular velocity or acceleration, where the + * quantity in question lacks the 360-degree cyclical equivalence of a physical angle. + *

+ *

+ * {@link AngleUnit} expresses normalized angles, while {@link UnnormalizedAngleUnit} expresses unnormalized ones + *

+ */ +@SuppressWarnings("WeakerAccess") +public enum UnnormalizedAngleUnit +{ + DEGREES(0), RADIANS(1); + public final byte bVal; + + UnnormalizedAngleUnit(int i) + { + bVal = (byte) i; + } + + //---------------------------------------------------------------------------------------------- + // Primitive operations + //---------------------------------------------------------------------------------------------- + + public double fromDegrees(double degrees) + { + switch (this) + { + default: + case RADIANS: return (degrees / 180.0 * Math.PI); + case DEGREES: return (degrees); + } + } + + public float fromDegrees(float degrees) + { + switch (this) + { + default: + case RADIANS: return (degrees / 180.0f * AngleUnit.Pif); + case DEGREES: return (degrees); + } + } + + public double fromRadians(double radians) + { + switch (this) + { + default: + case RADIANS: return (radians); + case DEGREES: return (radians / Math.PI * 180.0); + } + } + + public float fromRadians(float radians) + { + switch (this) + { + default: + case RADIANS: return (radians); + case DEGREES: return (radians / AngleUnit.Pif * 180.0f); + } + } + + public double fromUnit(UnnormalizedAngleUnit them, double theirs) + { + switch (them) + { + default: + case RADIANS: return this.fromRadians(theirs); + case DEGREES: return this.fromDegrees(theirs); + } + } + + public float fromUnit(UnnormalizedAngleUnit them, float theirs) + { + switch (them) + { + default: + case RADIANS: return this.fromRadians(theirs); + case DEGREES: return this.fromDegrees(theirs); + } + } + + public double fromUnit(AngleUnit them, double theirs) { + return this.fromUnit(them.getUnnormalized(), theirs); + } + + public float fromUnit(AngleUnit them, float theirs) { + return this.fromUnit(them.getUnnormalized(), theirs); + } + + //---------------------------------------------------------------------------------------------- + // Derived operations + //---------------------------------------------------------------------------------------------- + + public double toDegrees(double inOurUnits) + { + switch (this) + { + default: + case RADIANS: return DEGREES.fromRadians(inOurUnits); + case DEGREES: return DEGREES.fromDegrees(inOurUnits); + } + } + + public float toDegrees(float inOurUnits) + { + switch (this) + { + default: + case RADIANS: return DEGREES.fromRadians(inOurUnits); + case DEGREES: return DEGREES.fromDegrees(inOurUnits); + } + } + + public double toRadians(double inOurUnits) + { + switch (this) + { + default: + case RADIANS: return RADIANS.fromRadians(inOurUnits); + case DEGREES: return RADIANS.fromDegrees(inOurUnits); + } + } + + public float toRadians(float inOurUnits) + { + switch (this) + { + default: + case RADIANS: return RADIANS.fromRadians(inOurUnits); + case DEGREES: return RADIANS.fromDegrees(inOurUnits); + } + } + + //---------------------------------------------------------------------------------------------- + // Normalization + //---------------------------------------------------------------------------------------------- + + public AngleUnit getNormalized() + { + switch (this) + { + default: + case RADIANS: return AngleUnit.RADIANS; + case DEGREES: return AngleUnit.DEGREES; + } + } +} diff --git a/WilyCore/src/main/java/org/firstinspires/ftc/robotcore/internal/usb/EthernetOverUsbSerialNumber.java b/WilyCore/src/main/java/org/firstinspires/ftc/robotcore/internal/usb/EthernetOverUsbSerialNumber.java new file mode 100644 index 000000000000..c5ac2f84592c --- /dev/null +++ b/WilyCore/src/main/java/org/firstinspires/ftc/robotcore/internal/usb/EthernetOverUsbSerialNumber.java @@ -0,0 +1,99 @@ +package org.firstinspires.ftc.robotcore.internal.usb; + +/* +Copyright (c) 2024 FIRST + +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, +are permitted (subject to the limitations in the disclaimer below) provided that +the following conditions are met: + +Redistributions of source code must retain the above copyright notice, this list +of conditions and the following disclaimer. + +Redistributions in binary form must reproduce the above copyright notice, this +list of conditions and the following disclaimer in the documentation and/or +other materials provided with the distribution. + +Neither the name of FIRST nor the names of their contributors may be used to +endorse or promote products derived from this software without specific prior +written permission. + +NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS +LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF +THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ +import com.qualcomm.robotcore.util.SerialNumber; + +@SuppressWarnings("WeakerAccess") +public class EthernetOverUsbSerialNumber extends SerialNumber +{ + protected final String ipAddress; + protected final String iface; + private final static String SERIAL_NUMBER_PREFIX = "EthernetOverUsb"; + + //---------------------------------------------------------------------------------------------- + // Construction + //---------------------------------------------------------------------------------------------- + + public static EthernetOverUsbSerialNumber fromIpAddress(String ipAddress, String iface) + { + String serialNumber = SERIAL_NUMBER_PREFIX + ":" + iface + ":" + ipAddress; + return new EthernetOverUsbSerialNumber(serialNumber); + } + + public static EthernetOverUsbSerialNumber fromSerialNumber(String serialNumber) + { + return new EthernetOverUsbSerialNumber(serialNumber); + } + + private EthernetOverUsbSerialNumber(String serialNumber) + { + super(serialNumber.trim()); + this.iface = serialNumber.substring(serialNumber.indexOf(':') + 1, serialNumber.lastIndexOf(':')); + this.ipAddress = serialNumber.substring(serialNumber.lastIndexOf(':') + 1); + } + + //---------------------------------------------------------------------------------------------- + // Accessing + //---------------------------------------------------------------------------------------------- + + public String getIpAddress() + { + return ipAddress; + } + + @Override + public String toString() + { + return iface + ':' + ipAddress; + } + + @Override + public boolean isFake() + { + return false; + } + + /** + * Returns whether the indicated serial number initialization string is one of the legacy + * fake serial number forms or not. + * + * @param initializer the serial number initialization string to test + * @return whether the serial number initialization string is a legacy fake form of serial number + */ + public static boolean isLegacyFake(String initializer) + { + return false; + } + +} diff --git a/WilyCore/src/main/java/org/json/JSONArray.java b/WilyCore/src/main/java/org/json/JSONArray.java new file mode 100644 index 000000000000..c71410ee5e7f --- /dev/null +++ b/WilyCore/src/main/java/org/json/JSONArray.java @@ -0,0 +1,190 @@ +package org.json; + +// +// Source code recreated from a .class file by IntelliJ IDEA +// (powered by FernFlower decompiler) +// + +import java.util.Collection; + +public class JSONArray { + public JSONArray() { + throw new RuntimeException("Stub!"); + } + + public JSONArray(Collection copyFrom) { + throw new RuntimeException("Stub!"); + } + +// public JSONArray(JSONTokener readFrom) throws JSONException { +// throw new RuntimeException("Stub!"); +// } + + public JSONArray(String json) throws JSONException { + throw new RuntimeException("Stub!"); + } + + public JSONArray(Object array) throws JSONException { + throw new RuntimeException("Stub!"); + } + + public int length() { + throw new RuntimeException("Stub!"); + } + + public JSONArray put(boolean value) { + throw new RuntimeException("Stub!"); + } + + public JSONArray put(double value) throws JSONException { + throw new RuntimeException("Stub!"); + } + + public JSONArray put(int value) { + throw new RuntimeException("Stub!"); + } + + public JSONArray put(long value) { + throw new RuntimeException("Stub!"); + } + + public JSONArray put(Object value) { + throw new RuntimeException("Stub!"); + } + + public JSONArray put(int index, boolean value) throws JSONException { + throw new RuntimeException("Stub!"); + } + + public JSONArray put(int index, double value) throws JSONException { + throw new RuntimeException("Stub!"); + } + + public JSONArray put(int index, int value) throws JSONException { + throw new RuntimeException("Stub!"); + } + + public JSONArray put(int index, long value) throws JSONException { + throw new RuntimeException("Stub!"); + } + + public JSONArray put(int index, Object value) throws JSONException { + throw new RuntimeException("Stub!"); + } + + public boolean isNull(int index) { + throw new RuntimeException("Stub!"); + } + + public Object get(int index) throws JSONException { + throw new RuntimeException("Stub!"); + } + + public Object opt(int index) { + throw new RuntimeException("Stub!"); + } + + public Object remove(int index) { + throw new RuntimeException("Stub!"); + } + + public boolean getBoolean(int index) throws JSONException { + throw new RuntimeException("Stub!"); + } + + public boolean optBoolean(int index) { + throw new RuntimeException("Stub!"); + } + + public boolean optBoolean(int index, boolean fallback) { + throw new RuntimeException("Stub!"); + } + + public double getDouble(int index) throws JSONException { + throw new RuntimeException("Stub!"); + } + + public double optDouble(int index) { + throw new RuntimeException("Stub!"); + } + + public double optDouble(int index, double fallback) { + throw new RuntimeException("Stub!"); + } + + public int getInt(int index) throws JSONException { + throw new RuntimeException("Stub!"); + } + + public int optInt(int index) { + throw new RuntimeException("Stub!"); + } + + public int optInt(int index, int fallback) { + throw new RuntimeException("Stub!"); + } + + public long getLong(int index) throws JSONException { + throw new RuntimeException("Stub!"); + } + + public long optLong(int index) { + throw new RuntimeException("Stub!"); + } + + public long optLong(int index, long fallback) { + throw new RuntimeException("Stub!"); + } + + public String getString(int index) throws JSONException { + throw new RuntimeException("Stub!"); + } + + public String optString(int index) { + throw new RuntimeException("Stub!"); + } + + public String optString(int index, String fallback) { + throw new RuntimeException("Stub!"); + } + + public JSONArray getJSONArray(int index) throws JSONException { + throw new RuntimeException("Stub!"); + } + + public JSONArray optJSONArray(int index) { + throw new RuntimeException("Stub!"); + } + + public JSONObject getJSONObject(int index) throws JSONException { + throw new RuntimeException("Stub!"); + } + + public JSONObject optJSONObject(int index) { + throw new RuntimeException("Stub!"); + } + + public JSONObject toJSONObject(JSONArray names) throws JSONException { + throw new RuntimeException("Stub!"); + } + + public String join(String separator) throws JSONException { + throw new RuntimeException("Stub!"); + } + + public String toString() { + throw new RuntimeException("Stub!"); + } + + public String toString(int indentSpaces) throws JSONException { + throw new RuntimeException("Stub!"); + } + + public boolean equals(Object o) { + throw new RuntimeException("Stub!"); + } + + public int hashCode() { + throw new RuntimeException("Stub!"); + } +} diff --git a/WilyCore/src/main/java/org/json/JSONException.java b/WilyCore/src/main/java/org/json/JSONException.java new file mode 100644 index 000000000000..8831ae5063ce --- /dev/null +++ b/WilyCore/src/main/java/org/json/JSONException.java @@ -0,0 +1,20 @@ +package org.json; + +// +// Source code recreated from a .class file by IntelliJ IDEA +// (powered by FernFlower decompiler) +// + +public class JSONException extends Exception { + public JSONException(String s) { + throw new RuntimeException("Stub!"); + } + + public JSONException(String message, Throwable cause) { + throw new RuntimeException("Stub!"); + } + + public JSONException(Throwable cause) { + throw new RuntimeException("Stub!"); + } +} diff --git a/WilyCore/src/main/java/org/json/JSONObject.java b/WilyCore/src/main/java/org/json/JSONObject.java new file mode 100644 index 000000000000..e16c68fd98db --- /dev/null +++ b/WilyCore/src/main/java/org/json/JSONObject.java @@ -0,0 +1,221 @@ +package org.json; + +// +// Source code recreated from a .class file by IntelliJ IDEA +// (powered by FernFlower decompiler) +// + +import androidx.annotation.RecentlyNonNull; +import androidx.annotation.RecentlyNullable; +import java.util.Iterator; +import java.util.Map; + +public class JSONObject { + @RecentlyNonNull + public static final Object NULL = null; + + public JSONObject() { + throw new RuntimeException("Stub!"); + } + + public JSONObject(@RecentlyNonNull Map copyFrom) { + throw new RuntimeException("Stub!"); + } + +// public JSONObject(@RecentlyNonNull JSONTokener readFrom) throws JSONException { +// throw new RuntimeException("Stub!"); +// } + + public JSONObject(@RecentlyNonNull String json) throws JSONException { + throw new RuntimeException("Stub!"); + } + + public JSONObject(@RecentlyNonNull JSONObject copyFrom, @RecentlyNonNull String[] names) throws JSONException { + throw new RuntimeException("Stub!"); + } + + public int length() { + throw new RuntimeException("Stub!"); + } + + @RecentlyNonNull + public JSONObject put(@RecentlyNonNull String name, boolean value) throws JSONException { + throw new RuntimeException("Stub!"); + } + + @RecentlyNonNull + public JSONObject put(@RecentlyNonNull String name, double value) throws JSONException { + throw new RuntimeException("Stub!"); + } + + @RecentlyNonNull + public JSONObject put(@RecentlyNonNull String name, int value) throws JSONException { + throw new RuntimeException("Stub!"); + } + + @RecentlyNonNull + public JSONObject put(@RecentlyNonNull String name, long value) throws JSONException { + throw new RuntimeException("Stub!"); + } + + @RecentlyNonNull + public JSONObject put(@RecentlyNonNull String name, @RecentlyNullable Object value) throws JSONException { + throw new RuntimeException("Stub!"); + } + + @RecentlyNonNull + public JSONObject putOpt(@RecentlyNullable String name, @RecentlyNullable Object value) throws JSONException { + throw new RuntimeException("Stub!"); + } + + @RecentlyNonNull + public JSONObject accumulate(@RecentlyNonNull String name, @RecentlyNullable Object value) throws JSONException { + throw new RuntimeException("Stub!"); + } + + @RecentlyNullable + public Object remove(@RecentlyNullable String name) { + throw new RuntimeException("Stub!"); + } + + public boolean isNull(@RecentlyNullable String name) { + throw new RuntimeException("Stub!"); + } + + public boolean has(@RecentlyNullable String name) { + throw new RuntimeException("Stub!"); + } + + @RecentlyNonNull + public Object get(@RecentlyNonNull String name) throws JSONException { + throw new RuntimeException("Stub!"); + } + + @RecentlyNullable + public Object opt(@RecentlyNullable String name) { + throw new RuntimeException("Stub!"); + } + + public boolean getBoolean(@RecentlyNonNull String name) throws JSONException { + throw new RuntimeException("Stub!"); + } + + public boolean optBoolean(@RecentlyNullable String name) { + throw new RuntimeException("Stub!"); + } + + public boolean optBoolean(@RecentlyNullable String name, boolean fallback) { + throw new RuntimeException("Stub!"); + } + + public double getDouble(@RecentlyNonNull String name) throws JSONException { + throw new RuntimeException("Stub!"); + } + + public double optDouble(@RecentlyNullable String name) { + throw new RuntimeException("Stub!"); + } + + public double optDouble(@RecentlyNullable String name, double fallback) { + throw new RuntimeException("Stub!"); + } + + public int getInt(@RecentlyNonNull String name) throws JSONException { + throw new RuntimeException("Stub!"); + } + + public int optInt(@RecentlyNullable String name) { + throw new RuntimeException("Stub!"); + } + + public int optInt(@RecentlyNullable String name, int fallback) { + throw new RuntimeException("Stub!"); + } + + public long getLong(@RecentlyNonNull String name) throws JSONException { + throw new RuntimeException("Stub!"); + } + + public long optLong(@RecentlyNullable String name) { + throw new RuntimeException("Stub!"); + } + + public long optLong(@RecentlyNullable String name, long fallback) { + throw new RuntimeException("Stub!"); + } + + @RecentlyNonNull + public String getString(@RecentlyNonNull String name) throws JSONException { + throw new RuntimeException("Stub!"); + } + + @RecentlyNonNull + public String optString(@RecentlyNullable String name) { + throw new RuntimeException("Stub!"); + } + + @RecentlyNonNull + public String optString(@RecentlyNullable String name, @RecentlyNonNull String fallback) { + throw new RuntimeException("Stub!"); + } + + @RecentlyNonNull + public JSONArray getJSONArray(@RecentlyNonNull String name) throws JSONException { + throw new RuntimeException("Stub!"); + } + + @RecentlyNullable + public JSONArray optJSONArray(@RecentlyNullable String name) { + throw new RuntimeException("Stub!"); + } + + @RecentlyNonNull + public JSONObject getJSONObject(@RecentlyNonNull String name) throws JSONException { + throw new RuntimeException("Stub!"); + } + + @RecentlyNullable + public JSONObject optJSONObject(@RecentlyNullable String name) { + throw new RuntimeException("Stub!"); + } + + @RecentlyNullable + public JSONArray toJSONArray(@RecentlyNullable JSONArray names) throws JSONException { + throw new RuntimeException("Stub!"); + } + + @RecentlyNonNull + public Iterator keys() { + throw new RuntimeException("Stub!"); + } + + @RecentlyNullable + public JSONArray names() { + throw new RuntimeException("Stub!"); + } + + @RecentlyNonNull + public String toString() { + throw new RuntimeException("Stub!"); + } + + @RecentlyNonNull + public String toString(int indentSpaces) throws JSONException { + throw new RuntimeException("Stub!"); + } + + @RecentlyNonNull + public static String numberToString(@RecentlyNonNull Number number) throws JSONException { + throw new RuntimeException("Stub!"); + } + + @RecentlyNonNull + public static String quote(@RecentlyNullable String data) { + throw new RuntimeException("Stub!"); + } + + @RecentlyNullable + public static Object wrap(@RecentlyNullable Object o) { + throw new RuntimeException("Stub!"); + } +} diff --git a/WilyCore/src/main/java/org/swerverobotics/ftc/GoBildaPinpointDriver.java b/WilyCore/src/main/java/org/swerverobotics/ftc/GoBildaPinpointDriver.java index f37c1c38c76d..bdbed77f7717 100644 --- a/WilyCore/src/main/java/org/swerverobotics/ftc/GoBildaPinpointDriver.java +++ b/WilyCore/src/main/java/org/swerverobotics/ftc/GoBildaPinpointDriver.java @@ -1,15 +1,11 @@ package org.swerverobotics.ftc; -import static com.qualcomm.robotcore.util.TypeConversion.byteArrayToInt; - import com.acmerobotics.roadrunner.Pose2d; import com.acmerobotics.roadrunner.PoseVelocity2d; -import com.qualcomm.robotcore.hardware.I2cAddr; import com.qualcomm.robotcore.hardware.I2cDeviceSynchDevice; import com.qualcomm.robotcore.hardware.I2cDeviceSynchSimple; import com.qualcomm.robotcore.hardware.configuration.annotations.DeviceProperties; import com.qualcomm.robotcore.hardware.configuration.annotations.I2cDeviceType; -import com.qualcomm.robotcore.util.TypeConversion; import com.wilyworks.simulator.WilyCore; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; @@ -18,11 +14,10 @@ import java.nio.ByteBuffer; import java.nio.ByteOrder; -import java.util.Arrays; @I2cDeviceType @DeviceProperties( - name = "goBILDA® Pinpoint Odometry Computer", + name = "Home-compiled goBILDA® Pinpoint Odometry Computer", xmlTag = "goBILDAPinpoint", description ="goBILDA® Pinpoint Odometry Computer (IMU Sensor Fusion for 2 Wheel Odometry)" ) diff --git a/team417/build.gradle b/team417/build.gradle index c7861fbf694c..dbb3e6627b7c 100644 --- a/team417/build.gradle +++ b/team417/build.gradle @@ -49,5 +49,4 @@ dependencies { implementation "com.acmerobotics.roadrunner:actions:1.0.0" implementation "com.acmerobotics.dashboard:dashboard:0.4.16" implementation 'org.team11260:fast-load:0.1.2' - implementation 'com.github.SwerveRobotics:FtcDrivers:0.0.2' } \ No newline at end of file diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/roadrunner/LoonyTune.java b/team417/src/main/java/org/firstinspires/ftc/team417/roadrunner/LoonyTune.java index 3faf204b6842..c2fad068dc03 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/roadrunner/LoonyTune.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/roadrunner/LoonyTune.java @@ -7,10 +7,6 @@ import static com.acmerobotics.roadrunner.Profiles.constantProfile; -import static org.swerverobotics.ftc.GoBildaPinpointDriver.DeviceStatus.FAULT_NO_PODS_DETECTED; -import static org.swerverobotics.ftc.GoBildaPinpointDriver.DeviceStatus.FAULT_X_POD_NOT_DETECTED; -import static org.swerverobotics.ftc.GoBildaPinpointDriver.DeviceStatus.FAULT_Y_POD_NOT_DETECTED; -import static org.swerverobotics.ftc.GoBildaPinpointDriver.DeviceStatus.NOT_READY; import static java.lang.System.nanoTime; import android.annotation.SuppressLint; @@ -36,6 +32,7 @@ import com.acmerobotics.roadrunner.TurnConstraints; import com.acmerobotics.roadrunner.Vector2d; import com.google.gson.Gson; +import com.qualcomm.hardware.gobilda.GoBildaPinpointDriver; import com.qualcomm.hardware.lynx.LynxModule; import com.qualcomm.hardware.sparkfun.SparkFunOTOS; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; @@ -54,7 +51,6 @@ import org.firstinspires.ftc.robotcore.external.matrices.VectorF; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; -import org.swerverobotics.ftc.GoBildaPinpointDriver; import java.io.BufferedReader; import java.io.BufferedWriter; @@ -899,11 +895,11 @@ enum Tuner { // Constants: final Pose2d zeroPose = new Pose2d(0, 0, 0); - // Get the current heading from the tracking sensor: + // Get the current heading from the tracking sensor, in radians: double getSensorHeading() { if (usePinpoint) { - drive.pinpointDriver.update(GoBildaPinpointDriver.readData.ONLY_UPDATE_HEADING); - return drive.pinpointDriver.getHeading(); + drive.pinpointDriver.update(GoBildaPinpointDriver.ReadData.ONLY_UPDATE_HEADING); + return drive.pinpointDriver.getHeading(AngleUnit.RADIANS); } else { return drive.otosDriver.getPosition().h; } @@ -914,12 +910,10 @@ enum Tuner { Pose2D getSensorVelocity() { if (usePinpoint) { drive.pinpointDriver.update(); - org.firstinspires.ftc.robotcore.external.navigation.Pose2D velocity - = drive.pinpointDriver.getVelocity(); return new Pose2D( - velocity.getX(DistanceUnit.INCH), - velocity.getY(DistanceUnit.INCH), - velocity.getHeading(AngleUnit.RADIANS)); + drive.pinpointDriver.getVelX(DistanceUnit.INCH), + drive.pinpointDriver.getVelY(DistanceUnit.INCH), + drive.pinpointDriver.getHeading(AngleUnit.RADIANS)); } else { return drive.otosDriver.getVelocity(); } @@ -1898,11 +1892,11 @@ else if (status == GoBildaPinpointDriver.DeviceStatus.NOT_READY) currentStatus = "not ready"; else if (status == GoBildaPinpointDriver.DeviceStatus.CALIBRATING) currentStatus = "calibrating"; - else if (status == FAULT_X_POD_NOT_DETECTED) + else if (status == GoBildaPinpointDriver.DeviceStatus.FAULT_X_POD_NOT_DETECTED) currentStatus = "X pod not detected"; - else if (status == FAULT_Y_POD_NOT_DETECTED) + else if (status == GoBildaPinpointDriver.DeviceStatus.FAULT_Y_POD_NOT_DETECTED) currentStatus = "Y pod not detected"; - else if (status == FAULT_NO_PODS_DETECTED) + else if (status == GoBildaPinpointDriver.DeviceStatus.FAULT_NO_PODS_DETECTED) currentStatus = "no pods detected"; else if (status == GoBildaPinpointDriver.DeviceStatus.FAULT_IMU_RUNAWAY) currentStatus = "IMU runaway"; @@ -4231,14 +4225,14 @@ public void runOpMode() { drive.pinpointDriver.update(); // Call update() to read the status register GoBildaPinpointDriver.DeviceStatus status = drive.pinpointDriver.getDeviceStatus(); - if (status == NOT_READY) { + if (status == GoBildaPinpointDriver.DeviceStatus.NOT_READY) { dialog.warning("The Pinpoint computer isn't responding. Check your I2C wiring."); return; // ====> } - if ((status == FAULT_X_POD_NOT_DETECTED) || - (status == FAULT_Y_POD_NOT_DETECTED) || - (status == FAULT_NO_PODS_DETECTED)) { + if ((status == GoBildaPinpointDriver.DeviceStatus.FAULT_X_POD_NOT_DETECTED) || + (status == GoBildaPinpointDriver.DeviceStatus.FAULT_Y_POD_NOT_DETECTED) || + (status == GoBildaPinpointDriver.DeviceStatus.FAULT_NO_PODS_DETECTED)) { dialog.warning("The Pinpoint computer indicates that pods aren't connected. " + "Check your encoder wiring."); return; // ====> diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/roadrunner/MecanumDrive.java b/team417/src/main/java/org/firstinspires/ftc/team417/roadrunner/MecanumDrive.java index 00644adb7208..f9ac1e9590b3 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/roadrunner/MecanumDrive.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/roadrunner/MecanumDrive.java @@ -48,6 +48,7 @@ import com.acmerobotics.roadrunner.ftc.OverflowEncoder; import com.acmerobotics.roadrunner.ftc.PositionVelocityPair; import com.acmerobotics.roadrunner.ftc.RawEncoder; +import com.qualcomm.hardware.gobilda.GoBildaPinpointDriver; import com.qualcomm.hardware.lynx.LynxModule; import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; import com.qualcomm.hardware.sparkfun.SparkFunOTOS; @@ -70,7 +71,6 @@ import org.firstinspires.ftc.team417.roadrunner.messages.MecanumLocalizerInputsMessage; import org.firstinspires.ftc.team417.roadrunner.messages.PoseMessage; import org.firstinspires.inspection.InspectionState; -import org.swerverobotics.ftc.GoBildaPinpointDriver; import java.util.Arrays; import java.util.LinkedList; @@ -394,8 +394,8 @@ public void initializePinpointDriver() { if (pinpointDriver == null) return; // ====> - pinpointDriver.setOffsets(PARAMS.pinpoint.xOffset, PARAMS.pinpoint.yOffset); - pinpointDriver.setEncoderResolution(PARAMS.pinpoint.ticksPerMm); + pinpointDriver.setOffsets(PARAMS.pinpoint.xOffset, PARAMS.pinpoint.yOffset, DistanceUnit.MM); + pinpointDriver.setEncoderResolution(PARAMS.pinpoint.ticksPerMm, DistanceUnit.MM); pinpointDriver.setEncoderDirections( PARAMS.pinpoint.xReversed ? GoBildaPinpointDriver.EncoderDirection.REVERSED : GoBildaPinpointDriver.EncoderDirection.FORWARD, PARAMS.pinpoint.yReversed ? GoBildaPinpointDriver.EncoderDirection.REVERSED : GoBildaPinpointDriver.EncoderDirection.FORWARD); @@ -828,7 +828,12 @@ public PoseVelocity2d updatePoseEstimate() { // Query the driver for position and velocity: pinpointDriver.update(); Pose2D pose2D = pinpointDriver.getPosition(); - Pose2D poseVelocity2D = pinpointDriver.getVelocity(); + Pose2D poseVelocity2D = new Pose2D( + DistanceUnit.INCH, + pinpointDriver.getVelX(DistanceUnit.INCH), + pinpointDriver.getVelY(DistanceUnit.INCH), + AngleUnit.RADIANS, + pinpointDriver.getHeading(AngleUnit.RADIANS)); // Convert to Road Runner format, remembering that the Pinpoint tracking sensor // reports velocity as field-relative but Road Runner wants it robot-relative: diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/utils/ConfigurationTester.java b/team417/src/main/java/org/firstinspires/ftc/team417/utils/ConfigurationTester.java index 5005fa1edf8f..6e27b1a6390d 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/utils/ConfigurationTester.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/utils/ConfigurationTester.java @@ -2,51 +2,121 @@ import static java.lang.System.nanoTime; -import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import android.annotation.SuppressLint; +import android.graphics.Color; + +import com.qualcomm.hardware.gobilda.GoBildaPinpointDriver; +import com.qualcomm.hardware.limelightvision.LLResult; +import com.qualcomm.hardware.limelightvision.LLStatus; +import com.qualcomm.hardware.limelightvision.Limelight3A; +import com.qualcomm.hardware.sparkfun.SparkFunOTOS; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.AnalogInput; import com.qualcomm.robotcore.hardware.CRServo; import com.qualcomm.robotcore.hardware.DcMotor; -import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.DigitalChannel; import com.qualcomm.robotcore.hardware.DistanceSensor; import com.qualcomm.robotcore.hardware.Gamepad; import com.qualcomm.robotcore.hardware.HardwareDevice; import com.qualcomm.robotcore.hardware.IMU; +import com.qualcomm.robotcore.hardware.LED; +import com.qualcomm.robotcore.hardware.NormalizedColorSensor; +import com.qualcomm.robotcore.hardware.NormalizedRGBA; import com.qualcomm.robotcore.hardware.Servo; -import com.qualcomm.robotcore.hardware.ServoController; +import com.qualcomm.robotcore.hardware.VoltageSensor; +import com.wilyworks.common.WilyWorks; +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles; +import org.firstinspires.ftc.vision.VisionPortal; +import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor; + import java.util.ArrayList; +import java.util.Arrays; +import java.util.Comparator; +import java.util.LinkedList; import java.util.List; +import java.util.Map; +import java.util.function.Consumer; +import java.util.regex.Pattern; @TeleOp(name="Configuration Tester", group="Utility") +@SuppressLint("DefaultLocale") public class ConfigurationTester extends LinearOpMode { - ArrayList deviceNames = new ArrayList<>(); - /** - * - */ - int menu(String header, List options, int current, boolean topmost) { + double nextAdvanceTime; // Time, relative to time(), at which an auto-repeat happens + + // Show and process input for a menu. + // + // header - a text string that is drawn before the menu list. + // scrollLine - the lowest line in the menu list that's guaranteed to be visible on the DS. + // options - the menu list. + // current - the currently highlighted option index. + // topmost - true if the B button won't cancel; false if the B button can cancel and return -1 + /** @noinspection SameParameterValue*/ + int menu(String header, int scrollLine, List options, int current, boolean topmost) { + final double INITIAL_DELAY = 0.6; // Seconds after initial press before starting to repeat + final double REPEAT_DELAY = 0.15; // Seconds after any repeat to repeat again while (isActive()) { + // If a gamepad1 hasn't been connected before, the user will have to press A + // while holding start. That can result in phantom A button presses, so account + // for that here: + if (gamepad1.start) { + gamepad1.aWasPressed(); + gamepad1.bWasPressed(); + continue; + } if (header != null) { - telemetry.addLine(header); + ui.line(header); + } + + // The list can be so long that it's not all visible on the screen at once, so we + // scroll the list. Unfortunately, we can't be sure how many lines are visible + // because some top lines can be reserved by the OS to give error messages, and + // because the DS can be in either landscape or portrait mode, and we have no good + // way to determine either. So we just scroll the top of the list, reserving the + // top line for an arrow-up indicator. + // + // This method assumes that menu lines don't exceed the width of the display (which + // we can't really tell, either), but that should be true almost always here. + int firstDisplayLine = 0; + if (current > scrollLine) { + firstDisplayLine = current - scrollLine + 1; + ui.line("  \u25b2"); // "black up-pointing triangle" } - for (int i = 0; i < options.size(); i++) { - String cursor = (i == current) ? ">" : " "; - telemetry.addLine(cursor + options.get(i)); + for (int i = firstDisplayLine; i < options.size(); i++) { + if (i == current) { + ui.line(htmlBackground(Ui.HIGHLIGHT_COLOR, + "\u25c6 " + options.get(i))); // Solid circle + } else { + ui.line("\u25c7 " + options.get(i)); // Hollow circle + } } - telemetry.update(); + ui.update(); + int advance = 0; if (gamepad1.dpadUpWasPressed()) { - current--; - if (current < 0) - current = options.size() - 1; + advance = -1; + nextAdvanceTime = time() + INITIAL_DELAY; } if (gamepad1.dpadDownWasPressed()) { - current++; - if (current == options.size()) - current = 0; + advance = 1; + nextAdvanceTime = time() + INITIAL_DELAY; + } + // Automatically repeat if held long enough: + if ((gamepad1.dpad_up) && (time() > nextAdvanceTime)) { + advance = -1; + nextAdvanceTime = time() + REPEAT_DELAY; } + if ((gamepad1.dpad_down) && (time() > nextAdvanceTime)) { + advance = 1; + nextAdvanceTime = time() + REPEAT_DELAY; + } + current = Math.max(0, Math.min(options.size() - 1, current + advance)); + if (gamepad1.bWasPressed() && !topmost) // Cancel return -1; if (gamepad1.aWasPressed()) // Select @@ -54,177 +124,593 @@ int menu(String header, List options, int current, boolean topmost) { } return topmost ? 0 : -1; } + + // This class is for registering tests. + static class Test { + Class klass; + Consumer test; + public Test(Class klass, Consumer test) { + this.klass = klass; + this.test = test; + } + } + + // Give more precision at slow speeds: + double shapeStick(double input){ + return (Math.pow(input, 3) + input) / 2; + } + + // This routine inputs a double value from the gamepad's right thumbstick. It scales between + // the specified minimum and maximum values. + double previousTime; + double INPUT_RATE = 1.0/2; // 2 seconds at full speed to span entire range + double stickValue(double stickInput, double oldValue, double min, double max) { + double time = time(); + double deltaT = Math.min(time - previousTime, 0.03); // Max delta-t of 30ms + double deltaValue = (max - min) * INPUT_RATE * deltaT; + previousTime = time; + return Math.max(min, Math.min(max, oldValue - shapeStick(stickInput) * deltaValue)); + } + + // Time, in seconds: + static double time() { + return nanoTime() * 1e-9; + } + + // Unusually, we do all our work before Start is pressed, rather than after. That's so that + // we can support camera previews, which the Driver Station can only do before Start is + // pressed. boolean isActive() { - return opModeIsActive(); - } - boolean notCancelled() { - telemetry.update(); - return isActive() && !gamepad1.bWasPressed(); - } - void commonHeader(String deviceName, HardwareDevice device) { - telemetry.addLine(String.format("Name: %s", deviceName)); - telemetry.addLine(device.getDeviceName()); - telemetry.addLine(device.getConnectionInfo()); - telemetry.addLine(String.format("Version: %d", device.getVersion())); - telemetry.addLine(String.format("Manufacturer: %s", device.getManufacturer().name())); - } - void testMotor(String deviceName) { - int previousTicks = 0; - DcMotor motor = (DcMotor) hardwareMap.get(deviceName); + return !this.isStopRequested() && !isStarted(); + } + + // Set the foreground font color for a string. Color must be in the format" #dc3545". + static String htmlForeground(String color, String string, Object... args) { + return String.format("%s", color, String.format(string, args)); + } + + // Set the background color for a string. Color must be in the format" #dc3545". + static String htmlBackground(String backgroundColor, String string, Object... args) { + return String.format("%s", backgroundColor, String.format(string, args)); + } + + // Set the foreground and background colors for a string. Colors must be in the format" #dc3545". + /** @noinspection unused*/ + static String htmlColors(String foregroundColor, String backgroundColor, String string, Object... args) { + return String.format("%s", foregroundColor, backgroundColor, String.format(string, args)); + } + + // Make a string big according to the specified size: 1.5^size. + static String htmlBig(int size, String string, Object... args) { + StringBuilder result = new StringBuilder(); + for (int i = 0; i < size; i++) { + result.append(""); + } + result.append(String.format(string, args)); + for (int i = 0; i < size; i++) { + result.append(""); + } + return result.toString(); + } + + // Make a string bold. + static String htmlBold(String string, Object... args) { + return "" + String.format(string, args) + ""; + } + + // Convert a string into a red error message. + static String error(String string, Object... args) { + return htmlForeground("#DC3545", string, args); + } + + // Style for displaying gamepad control names. + static String buttonName(String button) { + return htmlBackground("#404040", button); + } + + String format(String format, Object... args) { + String text = String.format(format, args); + // Replace A, B, X Y with the appropriate Xbox or PS4 symbols: + final Map xboxReplacements = Map.ofEntries( + Map.entry("A", "\ud83c\udd50"), // Round A + Map.entry("B", "\ud83c\udd51"), // Round B + Map.entry("X", "\ud83c\udd67"), // Round X + Map.entry("Y", "\ud83c\udd68") // Round Y + ); + final Map ps4Replacements = Map.ofEntries( + Map.entry("A", "\u2715"), // Cross + Map.entry("B", "\u2b58"), // Circle + Map.entry("X", "\u2b1C"), // Square + Map.entry("Y", "\u25b2") // Triangle + ); + final Map commonReplacements = Map.ofEntries( + Map.entry("LS", buttonName("LS")), + Map.entry("RS", buttonName("RS")), + Map.entry("LT", buttonName("LT")), + Map.entry("RT", buttonName("RT")), + Map.entry("LB", buttonName("LB")), + Map.entry("RB", buttonName("RB")), + Map.entry("dpad", buttonName("DPAD")) + ); + + Map abxyReplacements = xboxReplacements; + if ((gamepad1.type == Gamepad.Type.SONY_PS4) || + (gamepad1.type == Gamepad.Type.SONY_PS4_SUPPORTED_BY_KERNEL)) { + abxyReplacements = ps4Replacements; + } + for (Map.Entry entry : abxyReplacements.entrySet()) { + text = text.replaceAll("\\b" + Pattern.quote(entry.getKey()) + "\\b", entry.getValue()); + } + for (Map.Entry entry : commonReplacements.entrySet()) { + text = text.replaceAll("\\b" + Pattern.quote(entry.getKey()) + "\\b", entry.getValue()); + } + return text; + } + + // Every test has to call this loop. It returns false when the test should terminate. + boolean prompt() { return prompt(""); } + boolean prompt(String text) { + text = format((text.isEmpty() ? "\n" : "\n" + text + ", ") + "B to exit"); + + ui.out(text); // Add the prompt text + ui.update(); // Send all output to the driver station + if (!isActive() || gamepad1.bWasPressed()) { + return false; + } + String gray = "#808080"; + ui.line(htmlBig(2, htmlBold("\"%s\"", testDescriptor.deviceName))); + ui.line(htmlForeground(gray, "Description: %s", testDescriptor.hardwareDevice.getDeviceName())); + ui.line(htmlForeground(gray, "Connection: %s", testDescriptor.hardwareDevice.getConnectionInfo())); + ui.line(htmlForeground(gray, "Loop I/O performance: %s", loopTimer.get())); + return true; + } + + // Exclude the following device names from the enumeration because they're for built-in + // devices that are boring. + ArrayList EXCLUDE_DEVICE_NAMES = new ArrayList<>(Arrays.asList( + "Control Hub Portal", + "Control Hub", + "Expansion Hub 2" + )); + + // Helper class for doing formatted output to the Driver Station. + static class Ui { + static final String HIGHLIGHT_COLOR = "#9090c0"; + + Telemetry telemetry; + StringBuilder buffer; + + Ui(Telemetry telemetry) { + this.telemetry = telemetry; + this.buffer = new StringBuilder(); + // Enable our extensive use of HTML: + telemetry.setDisplayFormat(Telemetry.DisplayFormat.HTML); + // Change the update interval from 250ms to 50ms for a more responsive UI: + telemetry.setMsTransmissionInterval(50); + } + + // Output without a newline: + void out(String format, Object... args) { + buffer.append(String.format(format, args)); + } + // Output with a newline: + void line(String format, Object... args) { + out(format + "\n", args); + } + // Send the output to the Driver Station: + void update() { + telemetry.addLine(buffer.toString()); + telemetry.update(); + buffer = new StringBuilder(); + } + } + + Ui ui; // Class for outputting UI to the Driver Station + TestDescriptor testDescriptor; // Descriptor of currently executing test + LoopTimer loopTimer; // Loop timer for the current test + + // Structure for registering tests. + static class TestDescriptor { + String deviceName; // User's name for the device + String className; // Friendly name of the device object's class + HardwareDevice hardwareDevice; // The device object + Consumer testMethod; // The method that does the test + public TestDescriptor(String deviceName, String className, HardwareDevice hardwareDevice, Consumer testMethod) { + this.deviceName = deviceName; + this.className = className; + this.hardwareDevice = hardwareDevice; + this.testMethod = testMethod; + } + String getClassName() { return className; } + String getDeviceName() { return deviceName; } + } + + // Class to time loops. + static class LoopTimer { + final double DURATION = 1.0; // Update every second + int loopCount; // Count of loops so far + double timePerLoop; // Result from the last interval + double startTime; // Start time of this interval + LoopTimer() { + startTime = time(); + } + public String get() { + loopCount++; + if (time() - startTime > DURATION) { + timePerLoop = (time() - startTime) / loopCount; + startTime = time(); + loopCount = 0; + } + // Return the time in milliseconds: + return (timePerLoop == 0) ? "-" : String.format("%.1fms", timePerLoop * 1000.0f); + } + } + + @Override + public void runOpMode() { + ui = new Ui(telemetry); + + // Show a splash screen while we initialize: + double splashTime = time(); + ui.line(htmlBig(5, htmlForeground(Ui.HIGHLIGHT_COLOR, htmlBold("Configuration Tester!")))); + ui.line(htmlBig(2, "By Swerve Robotics, Woodinville\n")); + ui.line("Initializing..."); + ui.update(); + + // If running Wily Works, register all of the potential classes: + if (WilyWorks.isSimulating) { + for (int i = 0; i < TESTS.length; i++) { + hardwareMap.get(TESTS[i].klass, String.valueOf(i)); + } + } + + // Query the hardwareMap for all of the registered device, instantiate them, and create + // corresponding test entries: + List testList = new LinkedList<>(); + for (String name: hardwareMap.getAllNames(HardwareDevice.class)) { + // Exclude some (boring built-in) devices based on their names: + if (!EXCLUDE_DEVICE_NAMES.contains(name)) { + HardwareDevice device = hardwareMap.get(name); + // System.out.println(String.format("\"%s\": %s", name, device.getClass().getName())); + + // Find a test for this device type: + int i; + for (i = 0; i < TESTS.length; i++) { + Test test = TESTS[i]; + if (test.klass.isAssignableFrom(device.getClass())) { + testList.add(new TestDescriptor(name, test.klass.getSimpleName(), device, test.test)); + break; + } + } + // If we couldn't find a test that's appropriate for this type, use a generic one: + if (i == TESTS.length) { + testList.add(new TestDescriptor(name, device.getClass().getSimpleName(), device, this::testGeneric)); + } + + // CRServos annoyingly default to a power of -1. Set it to zero here. + if (device instanceof CRServo) { + ((CRServo) device).setPower(0); + } + } + } + + // Major sort on class name, then minor sort on device name: + testList.sort(Comparator.comparing(TestDescriptor::getClassName).thenComparing(TestDescriptor::getDeviceName)); + + // Convert into a menu: + ArrayList options = new ArrayList<>(); + for (TestDescriptor testDescriptor : testList) { + options.add(String.format("%s: \"%s\"", testDescriptor.className, testDescriptor.deviceName)); + } + + // Make sure the splash screen has been visible for a couple of seconds: + while (isActive()) + if (time() - splashTime > 2.0) + break; + + int selection = 0; + while (isActive()) { + String header = format("Here's your entire configuration. dpad to navigate, A to select. Tap %s to quit.", + htmlForeground("#05BD05", "\u25B6")); + + selection = menu(header + "\n", 6, options, selection, true); + testDescriptor = testList.get(selection); + loopTimer = new LoopTimer(); + // Invoke the test method: + testDescriptor.testMethod.accept(testDescriptor.hardwareDevice); + } + ui.line("Configuration Tester is done!"); + ui.update(); + } + + // This method is for devices that don't have a specific test. + void testGeneric(HardwareDevice device) { + do { + ui.line("Sorry, no test exists for %s. Please add one!", testDescriptor.className); + } while (prompt()); + } + + // Test the built-in IMU. + void testIMU(HardwareDevice device) { + IMU imu = (IMU) device; + do { + YawPitchRollAngles angles = imu.getRobotYawPitchRollAngles(); + ui.line("Yaw: " + htmlBig(2, "%.2f\u00b0", angles.getYaw(AngleUnit.DEGREES)) + + ", Pitch: " + htmlBig(2, "%.2f\u00b0", angles.getPitch(AngleUnit.DEGREES)) + + ", Roll: " + htmlBig(2, "%.2f\u00b0", angles.getRoll(AngleUnit.DEGREES))); + } while (prompt()); + } + + // Test the voltage module. + void testVoltage(HardwareDevice device) { + VoltageSensor voltage = (VoltageSensor) device; + do { + ui.line(htmlBig(3, "Voltage: %.2f", voltage.getVoltage())); + } while (prompt()); + } + + // Test a motor. + void testMotor(HardwareDevice device) { + DcMotorEx motor = (DcMotorEx) device; + double power = motor.getPower(); String encoderStatus = ""; - double previousTime = nanoTime()*1e-9; + motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); do { - commonHeader(deviceName, motor); - telemetry.addLine("Right trigger to spin forward, left to spin backward."); - double power = gamepad1.right_trigger - gamepad1.left_trigger; - telemetry.addLine(String.format("Power: %.2f", power)); + ui.line(htmlBig(3, "Power: %.2f", power)); + power = stickValue(gamepad1.right_stick_y, power, -1, 1); + if (gamepad1.xWasPressed()) + power = 0; motor.setPower(power); + int currentTicks = motor.getCurrentPosition(); - int deltaTicks = currentTicks - previousTicks; - double currentTime = nanoTime()*1e-9; - double deltaTime = currentTime - previousTime; + double velocity = motor.getVelocity(); if (power != 0) { - if ((currentTicks == 0) && (deltaTicks == 0)) { + if ((currentTicks == 0) && (velocity == 0)) { encoderStatus = "No encoder detected."; - } else if (((deltaTicks < 0) && (power > 0)) || ((deltaTicks > 0) && (power < 0))) { - encoderStatus = "ERROR: Encoder turns opposite of motor; is motor wiring wrong?"; + } else if (((velocity < 0) && (power > 0)) || ((velocity > 0) && (power < 0))) { + encoderStatus = error("ERROR: Encoder turns opposite of motor; is motor wiring wrong?"); } else { encoderStatus = "Encoder detected."; } } if (!encoderStatus.isEmpty()) { - telemetry.addLine(encoderStatus); - if ((currentTicks != 0) || (deltaTicks != 0)) { - telemetry.addLine(String.format("Position: %d", currentTicks)); - telemetry.addLine(String.format("Velocity: %.0f", deltaTicks / deltaTime)); + ui.line(encoderStatus); + if ((currentTicks != 0) || (velocity != 0)) { + ui.line("Position: %d", currentTicks); + ui.line("Velocity: %.0f", velocity); } } - previousTicks = currentTicks; - previousTime = currentTime; - } while (notCancelled()); + } while (prompt("RS for power, X to stop")); } - void testCRServo(String deviceName) { - CRServo crServo = (CRServo) hardwareMap.get(deviceName); + + // Test a Continuous Rotation servo. + void testCRServo(HardwareDevice device) { + CRServo crServo = (CRServo) device; + double power = crServo.getPower(); do { - commonHeader(deviceName, crServo); - telemetry.addLine("Right trigger to spin forward, left to spin backward."); - double power = gamepad1.right_trigger - gamepad1.left_trigger; - telemetry.addLine(String.format("Power: %.2f", power)); + ui.line(htmlBig(3, "Power: %.2f", power)); + power = stickValue(gamepad1.right_stick_y, power, -1, 1); + if (gamepad1.xWasPressed()) + power = 0; crServo.setPower(power); - } while (notCancelled()); + } while (prompt("RS for power, X to stop")); } - void testServo(String deviceName) { - Servo servo = (Servo) hardwareMap.get(deviceName); - ServoController controller = servo.getController(); + + // Test a servo. + void testServo(HardwareDevice device) { + Servo servo = (Servo) device; + double position = servo.getPosition(); boolean enabled = false; do { - commonHeader(deviceName, servo); - telemetry.addLine("Right trigger to control amount of rotation."); - double position = gamepad1.right_trigger - gamepad1.left_trigger; - telemetry.addLine(String.format("Position: %.2f, Status: %s", position, controller.getPwmStatus().toString())); - if (position != 0) - enabled = true; // Don't enable until there's a non-zero input + ui.line(htmlBig(3, "Position: %.2f", position)); + position = stickValue(gamepad1.right_stick_y, position, 0, 1); if (enabled) servo.setPosition(position); - } while (notCancelled()); + else + ui.line(format("\nA to activate servo. Be prepared for the servo to jump to its position!")); + if (gamepad1.aWasPressed()) + enabled = true; + } while (prompt("RS for position")); } - void testDistance(String deviceName) { - DistanceSensor distance = (DistanceSensor) hardwareMap.get(deviceName); + + // Test a distance sensor. + void testDistance(HardwareDevice device) { + DistanceSensor distance = (DistanceSensor) device; do { - commonHeader(deviceName, distance); - telemetry.addLine(String.format("Distance CM: %.2f", distance.getDistance(DistanceUnit.CM))); - } while (notCancelled()); + ui.line(htmlBig(2, "Distance: %.2fcm", distance.getDistance(DistanceUnit.CM))); + } while (prompt()); } - void testGeneric(String deviceName) { - HardwareDevice device = hardwareMap.get(deviceName); + + // Digital channels are configurable for input and output. + void testDigitalChannel(HardwareDevice device) { + DigitalChannel channel = (DigitalChannel) device; + DigitalChannel.Mode mode = channel.getMode(); + boolean outputValue = true; + + String promptText; do { - commonHeader(deviceName, device); - } while (notCancelled()); + if (mode == DigitalChannel.Mode.INPUT) { + ui.line(htmlBig(2, "Input: %s", channel.getState())); + promptText = "Y to switch output mode"; + } else { + ui.line(htmlBig(2, "Output: %s", outputValue)); + promptText = "A to toggle value, Y to switch output mode"; + if (gamepad1.aWasPressed()) + outputValue = !outputValue; + } + if (gamepad1.yWasPressed()) { + mode = (mode == DigitalChannel.Mode.INPUT) + ? DigitalChannel.Mode.OUTPUT + : DigitalChannel.Mode.INPUT; + channel.setMode(mode); + } + } while (prompt(promptText)); } - void testIMU(String deviceName) { - IMU imu = (IMU) hardwareMap.get(deviceName); + + // Test webcams. + void testCamera(HardwareDevice device) { + WebcamName camera = (WebcamName) device; + AprilTagProcessor aprilTag = AprilTagProcessor.easyCreateWithDefaults(); + VisionPortal visionPortal = VisionPortal.easyCreateWithDefaults(camera, aprilTag); do { - commonHeader(deviceName, imu); - YawPitchRollAngles angles = imu.getRobotYawPitchRollAngles(); - telemetry.addLine(String.format("Yaw: %.2f, Pitch: %.2f, Roll: %.2f (degrees)", - angles.getYaw(AngleUnit.DEGREES), - angles.getPitch(AngleUnit.DEGREES), - angles.getRoll(AngleUnit.DEGREES))); - } while (notCancelled()); + ui.out("Press ··· on the Driver Station and then select 'Camera Stream'. "); + ui.line("That will show a snapshot from the camera. Tap the DS screen to update.\n"); + ui.line("IMPORTANT: Select 'Camera Stream' again when you want to return to this app!"); + } while (prompt()); + visionPortal.close(); } - // @@@ Deprecated: - void addDeviceNames(Class classType) { - for (String name: hardwareMap.getAllNames(DcMotor.class)) { - if (!deviceNames.contains(name)) - deviceNames.add(name); - } + + // Test the Control Hub's analog input. + void testAnalogInput(HardwareDevice device) { + AnalogInput input = (AnalogInput) device; + do { + ui.line("Max voltage: %.2f", input.getMaxVoltage()); + ui.line(htmlBig(2, "Voltage: %.2f", input.getVoltage())); + } while (prompt()); } - @Override - public void runOpMode() { - telemetry.addLine("Press START to test the current configuration."); - telemetry.addLine(""); - telemetry.addLine("All devices set via 'Configure Robot' will be listed. Use touch " + - "to scroll if they extend below the bottom of the screen."); - telemetry.addLine(""); - telemetry.addLine("Use A to select, B to cancel, dpad to navigate."); - telemetry.update(); - for (String name: hardwareMap.getAllNames(DcMotor.class)) { - if (!deviceNames.contains(name)) - deviceNames.add(name); - } - for (String name: hardwareMap.getAllNames(CRServo.class)) { - if (!deviceNames.contains(name)) - deviceNames.add(name); - } - for (String name: hardwareMap.getAllNames(Servo.class)) { - if (!deviceNames.contains(name)) - deviceNames.add(name); - } - for (String name: hardwareMap.getAllNames(IMU.class)) { - if (!deviceNames.contains(name)) - deviceNames.add(name); - } - for (String name: hardwareMap.getAllNames(DistanceSensor.class)) { - if (!deviceNames.contains(name)) - deviceNames.add(name); - } - for (HardwareDevice device: hardwareMap) { - for (String name: hardwareMap.getAllNames(device.getClass())) { - if (!deviceNames.contains(name)) - deviceNames.add(name); + + // Test the SparkFun Optical Tracking Odometry Sensor. + void testOtos(HardwareDevice device) { + SparkFunOTOS otos = (SparkFunOTOS) device; + SparkFunOTOS.Version hwVersion = new SparkFunOTOS.Version(); + SparkFunOTOS.Version fwVersion = new SparkFunOTOS.Version(); + + do { + otos.getVersionInfo(hwVersion, fwVersion); + ui.line("Hardware version: %d.%d, firmware version: %d.%d", + hwVersion.major, hwVersion.minor, fwVersion.major, fwVersion.minor); + ui.line("Is connected: %s", otos.isConnected()); + + SparkFunOTOS.Status status = otos.getStatus(); + ui.line("Tilt angle warning: %s", status.warnTiltAngle); + + SparkFunOTOS.Pose2D pose = otos.getPosition(); + ui.line("x: %.2f\", y: %.2f\", heading: %.2f°", pose.x, pose.y, pose.h); + ui.line(htmlBig(2, "Status: %s", otos.selfTest() ? "Good" : "Bad")); + } while (prompt()); + } + + // Test the GoBilda Pinpoint odometry computer. + void testPinpoint(HardwareDevice device) { + GoBildaPinpointDriver pinpoint = (GoBildaPinpointDriver) device; + do { + int loopTime = pinpoint.getLoopTime(); + double frequency = pinpoint.getFrequency(); + ui.line("Loop time: %d, frequency: %.1f", loopTime, frequency); + // The GoBilda driver code says to contact tech support if the following + // conditions are consistently seen: + if ((loopTime < 500) || (loopTime > 1100)) { + ui.line(error("Bad loop time, contact tech@gobilda.com")); } - } - waitForStart(); - if (gamepad1.getUser() == null) { - while (isActive() && !gamepad1.aWasPressed()) { - telemetry.addLine("Please configure Gamepad #1 and press A to continue"); - telemetry.update(); + if ((frequency < 900) || (frequency > 2000)) { + ui.line(error("Bad frequency, contact tech@gobilda.com")); } - } - ArrayList options = new ArrayList<>(); - for (String name: deviceNames) { - HardwareDevice device = hardwareMap.get(name); - options.add(String.format("%s: %s", device.getClass().getSimpleName(), name)); - } - int selection = 0; - while (isActive()) { - selection = menu("", options, selection, true); - String deviceName = deviceNames.get(selection); - HardwareDevice device = hardwareMap.get(deviceName); - if (device instanceof DcMotor) { - testMotor(deviceName); - } else if (device instanceof CRServo) { - testCRServo(deviceName); - } else if (device instanceof Servo) { - testServo(deviceName); - } else if (device instanceof DistanceSensor) { - testDistance(deviceName); - } else if (device instanceof IMU) { - testIMU(deviceName); - } else { - testGeneric(deviceName); + pinpoint.update(); + ui.line("X encoder: %d, y encoder: %d", pinpoint.getEncoderX(), pinpoint.getEncoderY()); + + GoBildaPinpointDriver.DeviceStatus status = pinpoint.getDeviceStatus(); + if (status == GoBildaPinpointDriver.DeviceStatus.READY) + ui.line(htmlBig(2, "Status: Good")); + else if (status == GoBildaPinpointDriver.DeviceStatus.FAULT_BAD_READ) + ui.line(htmlBig(2, "Status: Ok") + "(bad read)"); + else { + String error = "Unknown error"; + switch (status) { + case NOT_READY: + error = "Not ready"; + break; + case CALIBRATING: + error = "Calibrating"; + break; + case FAULT_X_POD_NOT_DETECTED: + error = "X pod not detected"; + break; + case FAULT_Y_POD_NOT_DETECTED: + error = "Y pod not detected"; + break; + case FAULT_NO_PODS_DETECTED: + error = "No pods detected"; + break; + case FAULT_IMU_RUNAWAY: + error = "IMU runaway"; + break; + } + ui.line(error("Error: " + error)); } - } + } while (prompt()); + } + + // Test a color sensor. + void testNormalizedColorSensor(HardwareDevice device) { + final float[] hsv = new float[3]; + NormalizedColorSensor sensor = (NormalizedColorSensor) device; + double gain = sensor.getGain(); + do { + gain = stickValue(gamepad1.right_stick_y, gain, 1, 255); + ui.line("Gain: %.2f", gain); + sensor.setGain((float) gain); + + NormalizedRGBA rgba = sensor.getNormalizedColors(); + Color.colorToHSV(rgba.toColor(), hsv); + String color = String.format("#%06x", rgba.toColor() & 0xffffff); // Color in hex + ui.line("Color: %s", htmlBig(3, htmlForeground(color, "\u25a0"))); // Box + ui.line("Normalized ARGB: (%.2f, %.2f, %.2f)", rgba.red, rgba.green, rgba.blue); + ui.line("HSV: (%.2f, %.2f, %.2f)", hsv[0], hsv[1], hsv[2]); + + if (sensor instanceof DistanceSensor) { + ui.line("Distance: %.2f\"", ((DistanceSensor) sensor).getDistance(DistanceUnit.INCH)); + } + } while (prompt("RS to adjust gain")); + } + + // Test an LED. + void testLED(HardwareDevice device) { + LED led = (LED) device; + boolean enable = true; + do { + ui.line("Enable: %s", enable); + if (gamepad1.aWasPressed()) + enable = !enable; + led.enable(enable); + } while (prompt("A to toggle enable")); + } + + // Test a Limelight 3A. + void testLimelight(HardwareDevice device) { + Limelight3A limelight = (Limelight3A) device; + limelight.setPollRateHz(100); // This sets how often we ask Limelight for data (100 times per second) + limelight.start(); // This tells Limelight to start looking! + limelight.pipelineSwitch(0); // Switch to pipeline number 0 + + do { + limelight.getLatestResult(); + + LLStatus status = limelight.getStatus(); + ui.line("Connected: %s", limelight.isConnected()); + ui.line("Name: %s", status.getName()); + ui.line("Temperature: %.1f°", status.getTemp()); + sleep(10); + } while (prompt()); } + + // Register your test here. Note that the order can be important if a device supports + // multiple device objects (e.g., many color sensors support both NormalizedColorSensor + // and DistanceSensor). + final Test[] TESTS = { + new Test(IMU.class, this::testIMU), + new Test(VoltageSensor.class, this::testVoltage), + new Test(CRServo.class, this::testCRServo), + new Test(Servo.class, this::testServo), + new Test(DcMotor.class, this::testMotor), + new Test(NormalizedColorSensor.class, this::testNormalizedColorSensor), + new Test(DistanceSensor.class, this::testDistance), + new Test(DigitalChannel.class, this::testDigitalChannel), + new Test(WebcamName.class, this::testCamera), + new Test(AnalogInput.class, this::testAnalogInput), + new Test(SparkFunOTOS.class, this::testOtos), + new Test(GoBildaPinpointDriver.class, this::testPinpoint), + new Test(LED.class, this::testLED), + new Test(Limelight3A.class, this::testLimelight), + }; } From b4da5ff465f7071ba2f8d89b1cbe001a861913cf Mon Sep 17 00:00:00 2001 From: crest0 Date: Thu, 20 Nov 2025 19:26:41 -0800 Subject: [PATCH 098/198] Changed the hardware name to match the document --- .../java/org/firstinspires/ftc/team417/CoolColorDetector.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CoolColorDetector.java b/team417/src/main/java/org/firstinspires/ftc/team417/CoolColorDetector.java index e035ab52f55c..78330862b082 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CoolColorDetector.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CoolColorDetector.java @@ -17,8 +17,8 @@ public class CoolColorDetector { private float gain = 85f; // adjust for brightness private float[] hsv = new float[3]; public CoolColorDetector(HardwareMap map, Telemetry telemetry) { - sensor1 = map.get(NormalizedColorSensor.class, "cs1"); - sensor2 = map.get(NormalizedColorSensor.class, "cs2"); + sensor1 = map.get(NormalizedColorSensor.class, "sensorColor1"); + sensor2 = map.get(NormalizedColorSensor.class, "sensorColor2"); this.telemetry = telemetry; } From d5fe7f1eaa7243d50b04f9c61e1053986b85332f Mon Sep 17 00:00:00 2001 From: Emmett Date: Thu, 20 Nov 2025 20:42:25 -0800 Subject: [PATCH 099/198] added some initialization features back to teleop that were removed added a boolean to complexmechglob that determines if we are in auto or not. removed fastbot code from baseopmode, it remains intact in the main decode branch --- .../firstinspires/ftc/team417/BaseOpMode.java | 59 +------------------ .../ftc/team417/CompetitionTeleOp.java | 18 +++++- .../ftc/team417/ComplexMechGlob.java | 6 +- 3 files changed, 20 insertions(+), 63 deletions(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java b/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java index eaa602a88e0b..f874ba6b0a23 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java @@ -1,21 +1,6 @@ package org.firstinspires.ftc.team417; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import com.qualcomm.robotcore.hardware.CRServo; -import com.qualcomm.robotcore.hardware.DcMotor; -import com.qualcomm.robotcore.hardware.DcMotorEx; -import com.qualcomm.robotcore.hardware.DcMotorSimple; -import com.qualcomm.robotcore.hardware.LED; -import com.qualcomm.robotcore.hardware.PIDFCoefficients; -import com.qualcomm.robotcore.hardware.Servo; -import com.qualcomm.robotcore.util.ElapsedTime; - -import org.firstinspires.ftc.team417.roadrunner.MecanumDrive; -import org.firstinspires.ftc.team417.roadrunner.RobotAction; - -import java.util.ArrayList; -import java.util.Arrays; -import java.util.List; /** * This class contains all of the base logic that is shared between all of the TeleOp and @@ -23,49 +8,7 @@ */ abstract public class BaseOpMode extends LinearOpMode { - //fastbot hardware variables - - /*---------------------------------------------------------------*/ - //slowbot hardware - public Servo drum = null; - - //slowbot constants - - final double intakeslot0 = -1; - final double intakeslot1 = -0.2; - final double intakeslot2 = 0.6; - final double launcherslot0 = 0.2; - final double launcherslot1 = 1; - final double launcherslot2 = -0.6; - - int [] intakePositions = {0, 1, 2}; - - ArrayList blah; - final List INTAKE_POSITIONS - = new ArrayList<>(Arrays.asList(-1.0, -0.2, 0.6)); - int [] launcherPositions = {0, 1, 2}; - //double intakeSpeed = gamepad2.left_stick_x; - double Multiply = 0; //need to change, placeholder - public void initHardware() { - - // Reversed direction of launcher for DevBot because motor is on the other side (compared to FastBot) - - //add slowbot initialization code here - drum = hardwareMap.get(Servo.class, "drum"); - //launcher = hardwareMap.get(DcMotorEx.class, "motLauncher"); - - - - - // Tell the driver that initialization is complete. - telemetry.addData("Status", "Initialized"); - } - - - - public void drumLogic () { - - } } + diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java index 06be1a57c4a4..e0b6d966a942 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java @@ -42,7 +42,7 @@ public class CompetitionTeleOp extends BaseOpMode { public void runOpMode() { Pose2d beginPose = new Pose2d(0, 0, 0); MecanumDrive drive = new MecanumDrive(hardwareMap, telemetry, gamepad1, beginPose); - MechGlob mechGlob = ComplexMechGlob.create(hardwareMap, telemetry); + MechGlob mechGlob = ComplexMechGlob.create(hardwareMap, telemetry, false); // Initialize motors, servos, LEDs @@ -64,13 +64,26 @@ public void runOpMode() { )); + + // Update the current pose: drive.updatePoseEstimate(); + + // 'packet' is the object used to send data to FTC Dashboard: TelemetryPacket packet = MecanumDrive.getTelemetryPacket(); + // Do the work now for all active Road Runner actions, if any: + drive.doActionsWork(packet); + + // Draw the robot and field: + packet.fieldOverlay().setStroke("#3F51B5"); + Drawing.drawRobot(packet.fieldOverlay(), drive.pose); + MecanumDrive.sendTelemetryPacket(packet); + + //add slowbot teleop controls here if (gamepad2.yWasPressed()) { mechGlob.launch(RequestedColor.EITHER); @@ -86,6 +99,9 @@ public void runOpMode() { } mechGlob.intake(gamepad2.left_stick_x); mechGlob.update(); + + MecanumDrive.sendTelemetryPacket(packet); + telemetry.update(); } } diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java b/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java index 2e5dbfef0f75..627725151c3e 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java @@ -42,7 +42,7 @@ class MechGlob { //a placeholder class encompassing all code that ISN'T for slow MechGlob(){} //call DrumGlob.create to create a Glob object for slowbot or fastbot - static MechGlob create (HardwareMap hardwareMap, Telemetry telemetry){ + static MechGlob create (HardwareMap hardwareMap, Telemetry telemetry, boolean runningAuto){ if (MecanumDrive.isSlowBot) { //if the robot is slowbot, use ComplexMechGlob. return new ComplexMechGlob(hardwareMap, telemetry); //Go to ComplexMechGlob class @@ -225,9 +225,7 @@ void intake (double intakeSpeed) { void launch (RequestedColor requestedColor) { int minSlot = findNearestSlot(LAUNCH_POSITIONS, requestedColor); - if (minSlot == -1){ - telemetry.speak("bad"); - } else { + if (minSlot != -1){ addToDrumQueue(LAUNCH_POSITIONS[minSlot], WaitState.SPIN_UP); slotOccupiedBy.set (minSlot, PixelColor.NONE); //marking this slot as empty so we don't accidentally try to use it again } From 34f0f3efc9213157d66775b9a280511373809032 Mon Sep 17 00:00:00 2001 From: Emmett Date: Sun, 23 Nov 2025 12:14:29 -0800 Subject: [PATCH 100/198] inithardware removed from auto --- .../main/java/org/firstinspires/ftc/team417/CompetitionAuto.java | 1 - 1 file changed, 1 deletion(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java index 29dcf2a61a04..7106391aca60 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java @@ -46,7 +46,6 @@ enum SlowBotMovement { @Override public void runOpMode() { - initHardware(); Pose2d startPose = new Pose2d(0, 0, 0); From aaac3d23044923c8f3833e29d63ab27bd2fb783f Mon Sep 17 00:00:00 2001 From: Andrew Date: Tue, 25 Nov 2025 19:20:20 -0800 Subject: [PATCH 101/198] Update FTC Dashboard for Decode image. --- team417/build.gradle | 2 +- .../org/firstinspires/ftc/team417/roadrunner/MecanumDrive.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/team417/build.gradle b/team417/build.gradle index dbb3e6627b7c..4af9428b2251 100644 --- a/team417/build.gradle +++ b/team417/build.gradle @@ -47,6 +47,6 @@ dependencies { implementation "com.acmerobotics.roadrunner:ftc:0.1.13" implementation "com.acmerobotics.roadrunner:core:1.0.0" implementation "com.acmerobotics.roadrunner:actions:1.0.0" - implementation "com.acmerobotics.dashboard:dashboard:0.4.16" + implementation "com.acmerobotics.dashboard:dashboard:0.5.1" implementation 'org.team11260:fast-load:0.1.2' } \ No newline at end of file diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/roadrunner/MecanumDrive.java b/team417/src/main/java/org/firstinspires/ftc/team417/roadrunner/MecanumDrive.java index f9ac1e9590b3..3d25de386e61 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/roadrunner/MecanumDrive.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/roadrunner/MecanumDrive.java @@ -1027,7 +1027,7 @@ public static TelemetryPacket getTelemetryPacket(boolean showField) { // rendering. Canvas canvas = packet.fieldOverlay(); if (showField) { - canvas.drawImage("/dash/into-the-deep.png", 0, 0, 144, 144, + canvas.drawImage("/dash/decode.webp", 0, 0, 144, 144, Math.toRadians(90), 0, 144, true); } else { canvas.setFill("#000000"); From 8b59982c89eb11da21afb8f0e055dc42e3989482 Mon Sep 17 00:00:00 2001 From: crest0 Date: Tue, 25 Nov 2025 20:03:10 -0800 Subject: [PATCH 102/198] Shortened logic to include distance and added telemetry --- .../ftc/team417/CoolColorDetector.java | 81 ++++++++----------- .../ftc/team417/TestColorDetect.java | 2 +- 2 files changed, 36 insertions(+), 47 deletions(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CoolColorDetector.java b/team417/src/main/java/org/firstinspires/ftc/team417/CoolColorDetector.java index 5c3200338b0c..0a3f0f2e48b1 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CoolColorDetector.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CoolColorDetector.java @@ -13,70 +13,59 @@ import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; public class CoolColorDetector { - Telemetry telemetry; + Telemetry telemetry; private NormalizedColorSensor sensor1; private NormalizedColorSensor sensor2; - private float gain = 85f; // adjust for brightness - private float[] hsv = new float[3]; + private final float GAIN = 85f; // adjust for brightness + public CoolColorDetector(HardwareMap map, Telemetry telemetry) { sensor1 = map.get(NormalizedColorSensor.class, "sensorColor1"); sensor2 = map.get(NormalizedColorSensor.class, "sensorColor2"); + sensor1.setGain(GAIN); + sensor2.setGain(GAIN); this.telemetry = telemetry; } // --- Convert a sensor to ONE PixelColor --- @SuppressLint("DefaultLocale") - private PixelColor detectSingle(NormalizedColorSensor sensor) { - // Get raw values - sensor.setGain(gain); - NormalizedRGBA colors = sensor.getNormalizedColors(); - Color.colorToHSV(colors.toColor(), hsv); - double distance = ((DistanceSensor) sensor).getDistance(DistanceUnit.MM); + // --- Use logic comparing both sensors --- + PixelColor detectArtifactColor() { + final double MINIMUM_DISTANCE = 25; //25mm + double distance1 = ((DistanceSensor) sensor1).getDistance(DistanceUnit.MM); + double distance2 = ((DistanceSensor) sensor2).getDistance(DistanceUnit.MM); + NormalizedColorSensor sensor; - telemetry.addData("HSV", String.format("{%f, %f, %f}", hsv[0], hsv[1], hsv[2])); - float hue = hsv[0]; - // - if (distance <= 25) { - if (hue > 165 && hue < 180) { - return PixelColor.GREEN; - } - //Return purple based on hue value color sensor is detecting - else if (hue >= 200 && hue <= 225) { - return PixelColor.PURPLE; - } else { - return PixelColor.PURPLE; - } + if (distance1 < MINIMUM_DISTANCE) { + sensor = sensor1; + } else if (distance2 < MINIMUM_DISTANCE) { + sensor = sensor2; } else { - return PixelColor.NONE; - } + telemetry.addLine(String.format(" %.2f\", %.2f\"", distance1, distance2)); + return PixelColor.NONE; } + NormalizedRGBA colors = sensor.getNormalizedColors(); + float[] hsv = new float[3]; + Color.colorToHSV(colors.toColor(), hsv); + float hue = hsv[0]; + String colorCube = String.format("\u25a0", + colors.toColor() & 0xffffff); + telemetry.addLine(String.format("Color Detect: %.2f\", %.2f\" %s, Hue: %.1f", + distance1, distance2, colorCube, hue)); - // --- Use logic comparing both sensors --- - PixelColor detectArtifactColor() { - PixelColor s1 = detectSingle(sensor1); - PixelColor s2 = detectSingle(sensor2); - // Rule 1: If both detect something different → return sensor2 - if (s1 == s2) { - return s1; - } - // Rule 2: If sensor1 detects color and sensor2 = NONE → sensor1 wins - if ((s1 == PixelColor.GREEN || s1 == PixelColor.PURPLE) && s2 == PixelColor.NONE) { - return s1; - } - // Rule 3: If sensor2 detects color and sensor1 = NONE → sensor2 wins - if ((s2 == PixelColor.GREEN || s2 == PixelColor.PURPLE) && s1 == PixelColor.NONE) { - return s2; - } - else { - // Otherwise no decision → return none - return PixelColor.NONE; + if (hue > 165 && hue < 180) { //range determined from testing + return PixelColor.GREEN; + } else if (hue >= 200 && hue <= 225) { //range determined from testing + return PixelColor.PURPLE; + } else { + //error case use the most likely color + return PixelColor.PURPLE; } } - public void showTelemetry() { - telemetry.addData("Sensor 1", detectSingle(sensor1)); - telemetry.addData("Sensor 2", detectSingle(sensor2)); + + public void testTelemetry() { + telemetry.addData("Chosen Position", detectArtifactColor()); telemetry.update(); } diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/TestColorDetect.java b/team417/src/main/java/org/firstinspires/ftc/team417/TestColorDetect.java index 663192140b58..a0f2c5ff7954 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/TestColorDetect.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/TestColorDetect.java @@ -9,7 +9,7 @@ public void runOpMode() { detector = new CoolColorDetector(hardwareMap, telemetry); waitForStart(); while (opModeIsActive()) { - detector.showTelemetry(); + detector.testTelemetry(); } } } From 061d9f87d05ee25b583db39ef1e4f8cc6da9a396 Mon Sep 17 00:00:00 2001 From: anya-codes Date: Tue, 25 Nov 2025 20:09:11 -0800 Subject: [PATCH 103/198] Created new program to test launcher, feeder, and transfer without intake and drum for easier testing --- .../ftc/team417/LauncherTest.java | 81 +++++++++++++++++++ 1 file changed, 81 insertions(+) create mode 100644 team417/src/main/java/org/firstinspires/ftc/team417/LauncherTest.java diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/LauncherTest.java b/team417/src/main/java/org/firstinspires/ftc/team417/LauncherTest.java new file mode 100644 index 000000000000..6453e749fa55 --- /dev/null +++ b/team417/src/main/java/org/firstinspires/ftc/team417/LauncherTest.java @@ -0,0 +1,81 @@ +package org.firstinspires.ftc.team417; + +import com.qualcomm.robotcore.hardware.CRServo; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.hardware.Servo; +import com.qualcomm.robotcore.util.ElapsedTime; + +// Teleop without the intake and drum logic so we can just test for launcher speeds +public class LauncherTest extends CompetitionTeleOp { + Servo servoTransfer; + DcMotorEx motLLauncher; + DcMotorEx motULauncher; + CRServo servoBLaunchFeeder; + CRServo servoFLaunchFeeder; + ElapsedTime transferTimer; + TransferState transferState; + + + enum TransferState { + WAIT, + DONE + } + public void initHardware() { + // Initializing only the transfer, feeder, and launcher motors/servos + servoTransfer = hardwareMap.get(Servo.class, "servoTransfer"); + motLLauncher = hardwareMap.get(DcMotorEx.class, "motLLauncher"); + motULauncher = hardwareMap.get(DcMotorEx.class, "motULauncher"); + servoBLaunchFeeder = hardwareMap.get(CRServo.class, "servoBLaunchFeeder"); + servoFLaunchFeeder = hardwareMap.get(CRServo.class, "servoFLaunchFeeder"); + + motLLauncher.setDirection(DcMotorSimple.Direction.REVERSE); + servoBLaunchFeeder.setDirection(CRServo.Direction.REVERSE); + + } + + public void runOpMode() { + + // Spin up launcher flywheels to set flywheel velocities and start feeder wheels + if (gamepad2.dpadUpWasPressed()) { + motULauncher.setVelocity(ComplexMechGlob.UPPER_FAR_FLYWHEEL_VELOCITY); + motLLauncher.setVelocity(ComplexMechGlob.LOWER_FAR_FLYWHEEL_VELOCITY); + servoBLaunchFeeder.setPower(ComplexMechGlob.FEEDER_POWER); + servoFLaunchFeeder.setPower(ComplexMechGlob.FEEDER_POWER); + + } else if (gamepad2.dpadDownWasPressed()) { + motULauncher.setVelocity(ComplexMechGlob.UPPER_NEAR_FLYWHEEL_VELOCITY); + motLLauncher.setVelocity(ComplexMechGlob.UPPER_NEAR_FLYWHEEL_VELOCITY); + servoBLaunchFeeder.setPower(ComplexMechGlob.FEEDER_POWER); + servoFLaunchFeeder.setPower(ComplexMechGlob.FEEDER_POWER); + } + + + // When y is pressed, start the transfer, run for TRANSFER_TIME_UP, then stop it + if (gamepad2.yWasPressed()) { + if (transferTimer == null) { + transferTimer = new ElapsedTime(); + } + if (transferTimer.seconds() <= ComplexMechGlob.TRANSFER_TIME_UP) { + servoTransfer.setPosition(ComplexMechGlob.TRANSFER_ACTIVE_POSITION); + transferState = TransferState.WAIT; + } + if (transferTimer.seconds() >= ComplexMechGlob.TRANSFER_TIME_TOTAL) { + servoTransfer.setPosition(ComplexMechGlob.TRANSFER_INACTIVE_POSITION); + transferState = TransferState.DONE; + transferTimer = null; + } + } + + + // Stop all motors and feeders + if (gamepad2.dpadRightWasPressed()) { + servoBLaunchFeeder.setPower(0); + servoFLaunchFeeder.setPower(0); + motULauncher.setVelocity(0); + motLLauncher.setVelocity(0); + } + + } + +} From d6c2d9c9b39f6bb000979de2614418df69f9d544 Mon Sep 17 00:00:00 2001 From: anya-codes Date: Tue, 25 Nov 2025 20:17:18 -0800 Subject: [PATCH 104/198] Added a constant for top spin that can now be modified in FTC dashboard as well. By default it is 0 --- .../ftc/team417/ComplexMechGlob.java | 16 +++++++--------- .../firstinspires/ftc/team417/LauncherTest.java | 8 ++++---- 2 files changed, 11 insertions(+), 13 deletions(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java b/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java index 627725151c3e..cd0e132a1082 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java @@ -76,11 +76,9 @@ public class ComplexMechGlob extends MechGlob { //a class encompassing all code static double FEEDER_POWER = 1; static double TRANSFER_TIME_UP = 0.3; static double TRANSFER_TIME_TOTAL = 0.6; //TRANSFER_TIME_TOTAL must be more than TRANSFER_TIME_UP - static double UPPER_FAR_FLYWHEEL_VELOCITY = 1500; - static double LOWER_FAR_FLYWHEEL_VELOCITY = 1500; - static double LOWER_NEAR_FLYWHEEL_VELOCITY = 1500; - static double UPPER_NEAR_FLYWHEEL_VELOCITY = 1500; - + static double FAR_FLYWHEEL_VELOCITY = 1500; + static double NEAR_FLYWHEEL_VELOCITY = 1500; + static double FLYWHEEL_TOP_SPIN = 0; static double TRANSFER_INACTIVE_POSITION = 0; static double TRANSFER_ACTIVE_POSITION = 1; static double REVERSE_INTAKE_SPEED = -1; @@ -268,11 +266,11 @@ boolean preLaunch (RequestedColor requestedColor) { @Override void setLaunchVelocity (LaunchDistance launchDistance) { if (launchDistance == LaunchDistance.NEAR) { - upperLaunchVelocity = UPPER_NEAR_FLYWHEEL_VELOCITY; - lowerLaunchVelocity = LOWER_NEAR_FLYWHEEL_VELOCITY; + upperLaunchVelocity = NEAR_FLYWHEEL_VELOCITY + (0.5 * FLYWHEEL_TOP_SPIN); + lowerLaunchVelocity = NEAR_FLYWHEEL_VELOCITY - (0.5 * FLYWHEEL_TOP_SPIN); } else { - upperLaunchVelocity = UPPER_FAR_FLYWHEEL_VELOCITY; - lowerLaunchVelocity = LOWER_FAR_FLYWHEEL_VELOCITY; + upperLaunchVelocity = FAR_FLYWHEEL_VELOCITY + (0.5 * FLYWHEEL_TOP_SPIN); + lowerLaunchVelocity = FAR_FLYWHEEL_VELOCITY - (0.5 * FLYWHEEL_TOP_SPIN); } } int findSlotFromPosition (double position, double [] positions) { diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/LauncherTest.java b/team417/src/main/java/org/firstinspires/ftc/team417/LauncherTest.java index 6453e749fa55..d395509732f2 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/LauncherTest.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/LauncherTest.java @@ -38,14 +38,14 @@ public void runOpMode() { // Spin up launcher flywheels to set flywheel velocities and start feeder wheels if (gamepad2.dpadUpWasPressed()) { - motULauncher.setVelocity(ComplexMechGlob.UPPER_FAR_FLYWHEEL_VELOCITY); - motLLauncher.setVelocity(ComplexMechGlob.LOWER_FAR_FLYWHEEL_VELOCITY); + motULauncher.setVelocity(ComplexMechGlob.FAR_FLYWHEEL_VELOCITY + (0.5 * ComplexMechGlob.FLYWHEEL_TOP_SPIN)); + motLLauncher.setVelocity(ComplexMechGlob.FAR_FLYWHEEL_VELOCITY - (0.5 * ComplexMechGlob.FLYWHEEL_TOP_SPIN)); servoBLaunchFeeder.setPower(ComplexMechGlob.FEEDER_POWER); servoFLaunchFeeder.setPower(ComplexMechGlob.FEEDER_POWER); } else if (gamepad2.dpadDownWasPressed()) { - motULauncher.setVelocity(ComplexMechGlob.UPPER_NEAR_FLYWHEEL_VELOCITY); - motLLauncher.setVelocity(ComplexMechGlob.UPPER_NEAR_FLYWHEEL_VELOCITY); + motULauncher.setVelocity(ComplexMechGlob.NEAR_FLYWHEEL_VELOCITY + (0.5 * ComplexMechGlob.FLYWHEEL_TOP_SPIN)); + motLLauncher.setVelocity(ComplexMechGlob.NEAR_FLYWHEEL_VELOCITY - (0.5 * ComplexMechGlob.FLYWHEEL_TOP_SPIN)); servoBLaunchFeeder.setPower(ComplexMechGlob.FEEDER_POWER); servoFLaunchFeeder.setPower(ComplexMechGlob.FEEDER_POWER); } From 5b652f384d33b08638dd89a62ef6f58948bcd29b Mon Sep 17 00:00:00 2001 From: crest0 Date: Tue, 25 Nov 2025 20:42:02 -0800 Subject: [PATCH 105/198] changed inches to mm and html telemtry --- .../java/org/firstinspires/ftc/team417/CoolColorDetector.java | 3 ++- .../java/org/firstinspires/ftc/team417/TestColorDetect.java | 4 ++++ 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CoolColorDetector.java b/team417/src/main/java/org/firstinspires/ftc/team417/CoolColorDetector.java index 0a3f0f2e48b1..296784ff5b70 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CoolColorDetector.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CoolColorDetector.java @@ -51,7 +51,8 @@ PixelColor detectArtifactColor() { String colorCube = String.format("\u25a0", colors.toColor() & 0xffffff); - telemetry.addLine(String.format("Color Detect: %.2f\", %.2f\" %s, Hue: %.1f", + + telemetry.addLine(String.format("Color Detect: %.2fmm, %.2fmm %s, Hue: %.1f", distance1, distance2, colorCube, hue)); if (hue > 165 && hue < 180) { //range determined from testing diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/TestColorDetect.java b/team417/src/main/java/org/firstinspires/ftc/team417/TestColorDetect.java index a0f2c5ff7954..1d1fe0920cfb 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/TestColorDetect.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/TestColorDetect.java @@ -1,12 +1,16 @@ package org.firstinspires.ftc.team417; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import org.firstinspires.ftc.robotcore.external.Telemetry; + @TeleOp public class TestColorDetect extends LinearOpMode { CoolColorDetector detector; @Override public void runOpMode() { detector = new CoolColorDetector(hardwareMap, telemetry); + telemetry.setDisplayFormat(Telemetry.DisplayFormat.HTML); waitForStart(); while (opModeIsActive()) { detector.testTelemetry(); From aefb52bcc84f7975a05f87ecc89a0403e06ab027 Mon Sep 17 00:00:00 2001 From: anya-codes Date: Tue, 25 Nov 2025 20:54:36 -0800 Subject: [PATCH 106/198] Made some changes to LauncherTest so it shows up on driver station and runs --- .../ftc/team417/LauncherTest.java | 75 ++++++++++--------- 1 file changed, 41 insertions(+), 34 deletions(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/LauncherTest.java b/team417/src/main/java/org/firstinspires/ftc/team417/LauncherTest.java index d395509732f2..5de2a3f03698 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/LauncherTest.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/LauncherTest.java @@ -1,5 +1,6 @@ package org.firstinspires.ftc.team417; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.CRServo; import com.qualcomm.robotcore.hardware.DcMotorEx; import com.qualcomm.robotcore.hardware.DcMotorSimple; @@ -7,6 +8,7 @@ import com.qualcomm.robotcore.util.ElapsedTime; // Teleop without the intake and drum logic so we can just test for launcher speeds +@TeleOp (name = "LauncherTest", group = "Competition") public class LauncherTest extends CompetitionTeleOp { Servo servoTransfer; DcMotorEx motLLauncher; @@ -34,48 +36,53 @@ public void initHardware() { } + @Override public void runOpMode() { + initHardware(); + waitForStart(); - // Spin up launcher flywheels to set flywheel velocities and start feeder wheels - if (gamepad2.dpadUpWasPressed()) { - motULauncher.setVelocity(ComplexMechGlob.FAR_FLYWHEEL_VELOCITY + (0.5 * ComplexMechGlob.FLYWHEEL_TOP_SPIN)); - motLLauncher.setVelocity(ComplexMechGlob.FAR_FLYWHEEL_VELOCITY - (0.5 * ComplexMechGlob.FLYWHEEL_TOP_SPIN)); - servoBLaunchFeeder.setPower(ComplexMechGlob.FEEDER_POWER); - servoFLaunchFeeder.setPower(ComplexMechGlob.FEEDER_POWER); - - } else if (gamepad2.dpadDownWasPressed()) { - motULauncher.setVelocity(ComplexMechGlob.NEAR_FLYWHEEL_VELOCITY + (0.5 * ComplexMechGlob.FLYWHEEL_TOP_SPIN)); - motLLauncher.setVelocity(ComplexMechGlob.NEAR_FLYWHEEL_VELOCITY - (0.5 * ComplexMechGlob.FLYWHEEL_TOP_SPIN)); - servoBLaunchFeeder.setPower(ComplexMechGlob.FEEDER_POWER); - servoFLaunchFeeder.setPower(ComplexMechGlob.FEEDER_POWER); - } + while (opModeIsActive()) { + // Spin up launcher flywheels to set flywheel velocities and start feeder wheels + if (gamepad2.dpadUpWasPressed()) { + motULauncher.setVelocity(ComplexMechGlob.FAR_FLYWHEEL_VELOCITY + (0.5 * ComplexMechGlob.FLYWHEEL_TOP_SPIN)); + motLLauncher.setVelocity(ComplexMechGlob.FAR_FLYWHEEL_VELOCITY - (0.5 * ComplexMechGlob.FLYWHEEL_TOP_SPIN)); + servoBLaunchFeeder.setPower(ComplexMechGlob.FEEDER_POWER); + servoFLaunchFeeder.setPower(ComplexMechGlob.FEEDER_POWER); - // When y is pressed, start the transfer, run for TRANSFER_TIME_UP, then stop it - if (gamepad2.yWasPressed()) { - if (transferTimer == null) { - transferTimer = new ElapsedTime(); - } - if (transferTimer.seconds() <= ComplexMechGlob.TRANSFER_TIME_UP) { - servoTransfer.setPosition(ComplexMechGlob.TRANSFER_ACTIVE_POSITION); - transferState = TransferState.WAIT; + } else if (gamepad2.dpadDownWasPressed()) { + motULauncher.setVelocity(ComplexMechGlob.NEAR_FLYWHEEL_VELOCITY + (0.5 * ComplexMechGlob.FLYWHEEL_TOP_SPIN)); + motLLauncher.setVelocity(ComplexMechGlob.NEAR_FLYWHEEL_VELOCITY - (0.5 * ComplexMechGlob.FLYWHEEL_TOP_SPIN)); + servoBLaunchFeeder.setPower(ComplexMechGlob.FEEDER_POWER); + servoFLaunchFeeder.setPower(ComplexMechGlob.FEEDER_POWER); } - if (transferTimer.seconds() >= ComplexMechGlob.TRANSFER_TIME_TOTAL) { - servoTransfer.setPosition(ComplexMechGlob.TRANSFER_INACTIVE_POSITION); - transferState = TransferState.DONE; - transferTimer = null; + + + // When y is pressed, start the transfer, run for TRANSFER_TIME_UP, then stop it + if (gamepad2.yWasPressed()) { + if (transferTimer == null) { + transferTimer = new ElapsedTime(); + } + if (transferTimer.seconds() <= ComplexMechGlob.TRANSFER_TIME_UP) { + servoTransfer.setPosition(ComplexMechGlob.TRANSFER_ACTIVE_POSITION); + transferState = TransferState.WAIT; + } + if (transferTimer.seconds() >= ComplexMechGlob.TRANSFER_TIME_TOTAL) { + servoTransfer.setPosition(ComplexMechGlob.TRANSFER_INACTIVE_POSITION); + transferState = TransferState.DONE; + transferTimer = null; + } } - } - // Stop all motors and feeders - if (gamepad2.dpadRightWasPressed()) { - servoBLaunchFeeder.setPower(0); - servoFLaunchFeeder.setPower(0); - motULauncher.setVelocity(0); - motLLauncher.setVelocity(0); - } + // Stop all motors and feeders + if (gamepad2.dpadRightWasPressed()) { + servoBLaunchFeeder.setPower(0); + servoFLaunchFeeder.setPower(0); + motULauncher.setVelocity(0); + motLLauncher.setVelocity(0); + } + } } - } From 761567c97e6afdf77c0604b69e877643b01f58a2 Mon Sep 17 00:00:00 2001 From: Team <417skid@swerverobotics.club> Date: Mon, 17 Nov 2025 19:45:21 -0800 Subject: [PATCH 107/198] Auto far out of way start --- .../main/java/org/firstinspires/ftc/team417/CompetitionAuto.java | 1 - 1 file changed, 1 deletion(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java index 29dcf2a61a04..7106391aca60 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java @@ -46,7 +46,6 @@ enum SlowBotMovement { @Override public void runOpMode() { - initHardware(); Pose2d startPose = new Pose2d(0, 0, 0); From 53da66a83e375cf6c3d7dc7a1c68a5f7c6c331aa Mon Sep 17 00:00:00 2001 From: Team <417skid@swerverobotics.club> Date: Sun, 30 Nov 2025 14:17:30 -0800 Subject: [PATCH 108/198] Auto not working, paths get lost --- .../ftc/team417/CompetitionAuto.java | 237 ++++++++++-------- .../ftc/team417/ComplexMechGlob.java | 14 +- .../ftc/team417/LauncherTest.java | 10 +- 3 files changed, 142 insertions(+), 119 deletions(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java index 7106391aca60..0b50a20d4bbd 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java @@ -4,6 +4,8 @@ import com.acmerobotics.dashboard.telemetry.TelemetryPacket; import com.acmerobotics.roadrunner.Action; import com.acmerobotics.roadrunner.Pose2d; +import com.acmerobotics.roadrunner.Pose2dDual; +import com.acmerobotics.roadrunner.PoseMap; import com.acmerobotics.roadrunner.TrajectoryActionBuilder; import com.acmerobotics.roadrunner.Vector2d; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; @@ -41,23 +43,144 @@ enum SlowBotMovement { double minIntakes = 0.0; double maxIntakes = 3.0; - + TextMenu menu = new TextMenu(); + MenuInput menuInput = new MenuInput(MenuInput.InputType.CONTROLLER); Pattern pattern; + Alliance chosenAlliance; + SlowBotMovement chosenMovement; + double intakeCycles; + public Action getPath(SlowBotMovement chosenMovement, Alliance chosenAlliance, double intakeCycles) { + Pose2d startPose = new Pose2d(0, 0, 0); + + + Pose2d SBNearStartPose = new Pose2d(-60, 48, Math.toRadians(139)); + Pose2d SBFarStartPose = new Pose2d(60, 12, Math.toRadians(157.5)); + MecanumDrive drive = new MecanumDrive(hardwareMap, telemetry, gamepad1, startPose); + + + PoseMap poseMap = pose -> new Pose2dDual<>( + pose.position.x, + pose.position.y, + pose.heading); + if (chosenAlliance == Alliance.BLUE) { + poseMap = pose -> new Pose2dDual<>( + pose.position.x, + pose.position.y.unaryMinus(), + pose.heading.inverse()); + } + TrajectoryActionBuilder trajectoryAction = drive.actionBuilder(SBFarStartPose, poseMap); + switch (chosenMovement) { + case NEAR: + trajectoryAction.setTangent(Math.toRadians(-51)) + .splineToConstantHeading(new Vector2d(-36,36), Math.toRadians(-51)) + //3 launches + //after disp intake + .setTangent(Math.toRadians(0)) + .splineToSplineHeading(new Pose2d(-12,32, Math.toRadians(90)), Math.toRadians(90)) //go to intake closest from goal + .setTangent(Math.toRadians(90)) + .splineToConstantHeading(new Vector2d(-12,55), Math.toRadians(90)) + .setTangent(Math.toRadians(-90)) + .splineToSplineHeading(new Pose2d(-36,36, Math.toRadians(139)), Math.toRadians(180)); //go to launch position + if (intakeCycles > 1) { + trajectoryAction = trajectoryAction.setTangent(Math.toRadians(0)) + + + //3 launches + //after disp intake + + .splineToSplineHeading(new Pose2d(12, 32, Math.toRadians(90)), Math.toRadians(90)) //go to intake middle from goal + .setTangent(Math.toRadians(90)) + .splineToConstantHeading(new Vector2d(12, 60), Math.toRadians(90)) + .setTangent(Math.toRadians(-90)) + .splineToSplineHeading(new Pose2d(-36, 36, Math.toRadians(139)), Math.toRadians(180)); //go to launch position + //3 launches + //after disp intake + if (intakeCycles > 2) { + trajectoryAction = trajectoryAction.setTangent(Math.toRadians(0)) + .splineToSplineHeading(new Pose2d(36, 32, Math.toRadians(90)), Math.toRadians(90)) //go to intake farthest from goal + .setTangent(Math.toRadians(90)) + .splineToConstantHeading(new Vector2d(36, 60), Math.toRadians(90)) + .setTangent(Math.toRadians(-90)) + .splineToSplineHeading(new Pose2d(-36, 36, Math.toRadians(139)), Math.toRadians(180)); //go to launch position + + } + } + break; + case FAR: + if (intakeCycles == 0) { + trajectoryAction.setTangent(Math.toRadians(180)); + // 3 launch actions + //then after disp intake action + } + + + trajectoryAction = trajectoryAction.splineToSplineHeading(new Pose2d(36,32, Math.toRadians(90)), Math.toRadians(90)) //go to intake farthest from goal + .setTangent(Math.toRadians(90)) + .splineToConstantHeading(new Vector2d(36,60), Math.toRadians(90)) + .setTangent(Math.toRadians(-90)) + .splineToSplineHeading(new Pose2d(54,12, Math.toRadians(157.5)), Math.toRadians(-90)); //go to launch position + if (intakeCycles > 1) { + + + // 3 launch actions + //after disp intake action + trajectoryAction = trajectoryAction.setTangent(Math.toRadians(180)) + .splineToSplineHeading(new Pose2d(12, 32, Math.toRadians(90)), Math.toRadians(90)) //go to intake middle from goal + .setTangent(Math.toRadians(90)) + .splineToConstantHeading(new Vector2d(12, 60), Math.toRadians(90)) + .setTangent(Math.toRadians(-90)) + .splineToSplineHeading(new Pose2d(54, 12, Math.toRadians(157.5)), Math.toRadians(-90)); //go to launch position + // 3 launch actions + //after disp intake action + if (intakeCycles > 2) { + trajectoryAction = trajectoryAction.setTangent(Math.toRadians(180)) + .splineToSplineHeading(new Pose2d(-12,32, Math.toRadians(90)), Math.toRadians(90)) //go to intake closest to goal + .setTangent(Math.toRadians(90)) + .splineToConstantHeading(new Vector2d(-12,55), Math.toRadians(90)) + .setTangent(Math.toRadians(-90)) + .splineToSplineHeading(new Pose2d(54,12, Math.toRadians(157.5)), Math.toRadians(-90)); //go to launch position + + } + } + break; + case FAR_OUT_OF_WAY: + // 3 launch actions + // after disp intake action + trajectoryAction.setTangent(Math.toRadians(180)) + .splineToLinearHeading(new Pose2d(60,61, Math.toRadians(0)), Math.toRadians(0)) + .setTangent(Math.toRadians(-90)) + .splineToLinearHeading(new Pose2d(54,12, Math.toRadians(157.5)), Math.toRadians(-90)) + // 3 launch actions + .setTangent(Math.toRadians(90)) + .splineToLinearHeading(new Pose2d(50,32,Math.toRadians(180)), Math.toRadians(180)); + break; + case FAR_MINIMAL: + trajectoryAction.setTangent(Math.toRadians(90)) + .splineToLinearHeading(new Pose2d(48,32,Math.toRadians(180)), Math.toRadians(180)) + .build(); + break; + } + return trajectoryAction.build(); + + + + + } @Override public void runOpMode() { + + + Pose2d startPose = new Pose2d(0, 0, 0); Pose2d SBNearStartPose = new Pose2d(-60, 48, Math.toRadians(139)); Pose2d SBFarStartPose = new Pose2d(60, 12, Math.toRadians(157.5)); - - MecanumDrive drive = new MecanumDrive(hardwareMap, telemetry, gamepad1, startPose); - TextMenu menu = new TextMenu(); - MenuInput menuInput = new MenuInput(MenuInput.InputType.CONTROLLER); + // Text menu for FastBot @@ -110,104 +233,7 @@ public void runOpMode() { throw new IllegalArgumentException("Alliance must be red or blue"); } - Action farMinimalSlowBot = pathFactory.actionBuilder(SBFarStartPose) - .setTangent(Math.toRadians(90)) - .splineToLinearHeading(new Pose2d(48,32,Math.toRadians(180)), Math.toRadians(180)) - .build(); - Action farOutOfWay = pathFactory.actionBuilder(SBFarStartPose) - // 3 launch actions - // after disp intake action - .setTangent(Math.toRadians(180)) - .splineToLinearHeading(new Pose2d(60,61, Math.toRadians(0)), Math.toRadians(0)) - .setTangent(Math.toRadians(-90)) - .splineToLinearHeading(new Pose2d(54,12, Math.toRadians(157.5)), Math.toRadians(-90)) - // 3 launch actions - .setTangent(Math.toRadians(90)) - .splineToLinearHeading(new Pose2d(50,32,Math.toRadians(180)), Math.toRadians(180)) - .build(); - - - PathFactory farSlowBotIntake1 = pathFactory.actionBuilder(SBFarStartPose); - if (intakeCycles == 0) { - farSlowBotIntake1.setTangent(Math.toRadians(180)); - // 3 launch actions - //then after disp intake action - } - - - farSlowBotIntake1.splineToSplineHeading(new Pose2d(36,32, Math.toRadians(90)), Math.toRadians(90)) //go to intake farthest from goal - .setTangent(Math.toRadians(90)) - .splineToConstantHeading(new Vector2d(36,60), Math.toRadians(90)) - .setTangent(Math.toRadians(-90)) - .splineToSplineHeading(new Pose2d(54,12, Math.toRadians(157.5)), Math.toRadians(-90)); //go to launch position - if (intakeCycles > 1) { - - - // 3 launch actions - //after disp intake action - farSlowBotIntake1 = farSlowBotIntake1.setTangent(Math.toRadians(180)) - .splineToSplineHeading(new Pose2d(12, 32, Math.toRadians(90)), Math.toRadians(90)) //go to intake middle from goal - .setTangent(Math.toRadians(90)) - .splineToConstantHeading(new Vector2d(12, 60), Math.toRadians(90)) - .setTangent(Math.toRadians(-90)) - .splineToSplineHeading(new Pose2d(54, 12, Math.toRadians(157.5)), Math.toRadians(-90)); //go to launch position - // 3 launch actions - //after disp intake action - if (intakeCycles > 2) { - farSlowBotIntake1 = farSlowBotIntake1.setTangent(Math.toRadians(180)) - .splineToSplineHeading(new Pose2d(-12,32, Math.toRadians(90)), Math.toRadians(90)) //go to intake closest to goal - .setTangent(Math.toRadians(90)) - .splineToConstantHeading(new Vector2d(-12,55), Math.toRadians(90)) - .setTangent(Math.toRadians(-90)) - .splineToSplineHeading(new Pose2d(54,12, Math.toRadians(157.5)), Math.toRadians(-90)); //go to launch position - - } - } - farSlowBotIntake1 = farSlowBotIntake1.setTangent(Math.toRadians(90)) - .splineToLinearHeading(new Pose2d(48,32,Math.toRadians(180)), Math.toRadians(180)); - Action farSlowBot = farSlowBotIntake1.build(); - - - - PathFactory nearSlowBotPath = pathFactory.actionBuilder(SBNearStartPose) - .setTangent(Math.toRadians(-51)) - .splineToConstantHeading(new Vector2d(-36,36), Math.toRadians(-51)) - //3 launches - //after disp intake - .setTangent(Math.toRadians(0)) - .splineToSplineHeading(new Pose2d(-12,32, Math.toRadians(90)), Math.toRadians(90)) //go to intake closest from goal - .setTangent(Math.toRadians(90)) - .splineToConstantHeading(new Vector2d(-12,55), Math.toRadians(90)) - .setTangent(Math.toRadians(-90)) - .splineToSplineHeading(new Pose2d(-36,36, Math.toRadians(139)), Math.toRadians(180)); //go to launch position - if (intakeCycles > 1) { - nearSlowBotPath = nearSlowBotPath.setTangent(Math.toRadians(0)) - - - //3 launches - //after disp intake - - .splineToSplineHeading(new Pose2d(12, 32, Math.toRadians(90)), Math.toRadians(90)) //go to intake middle from goal - .setTangent(Math.toRadians(90)) - .splineToConstantHeading(new Vector2d(12, 60), Math.toRadians(90)) - .setTangent(Math.toRadians(-90)) - .splineToSplineHeading(new Pose2d(-36, 36, Math.toRadians(139)), Math.toRadians(180)); //go to launch position - //3 launches - //after disp intake - if (intakeCycles > 2) { - nearSlowBotPath = nearSlowBotPath.setTangent(Math.toRadians(0)) - .splineToSplineHeading(new Pose2d(36, 32, Math.toRadians(90)), Math.toRadians(90)) //go to intake farthest from goal - .setTangent(Math.toRadians(90)) - .splineToConstantHeading(new Vector2d(36, 60), Math.toRadians(90)) - .setTangent(Math.toRadians(-90)) - .splineToSplineHeading(new Pose2d(-36, 36, Math.toRadians(139)), Math.toRadians(180)); //go to launch position - - } - } - nearSlowBotPath = nearSlowBotPath.setTangent(Math.toRadians(-90)) - .splineToSplineHeading(new Pose2d(-48, 12, Math.toRadians(180)), Math.toRadians(180)); - Action nearSlowBot = nearSlowBotPath.build(); // the first parameter is the type to return as @@ -217,22 +243,19 @@ public void runOpMode() { case NEAR: drive.setPose(SBNearStartPose); - trajectoryAction = nearSlowBot; + break; case FAR: drive.setPose(SBFarStartPose); - trajectoryAction = farSlowBot; break; case FAR_OUT_OF_WAY: drive.setPose(SBFarStartPose); - trajectoryAction = farOutOfWay; break; case FAR_MINIMAL: drive.setPose(SBFarStartPose); - trajectoryAction = farMinimalSlowBot; break; } - + trajectoryAction = getPath(chosenMovement, chosenAlliance, intakeCycles); // Get a preview of the trajectory's path: Canvas previewCanvas = new Canvas(); diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java b/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java index cd0e132a1082..7826f6ec6d72 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java @@ -78,7 +78,7 @@ public class ComplexMechGlob extends MechGlob { //a class encompassing all code static double TRANSFER_TIME_TOTAL = 0.6; //TRANSFER_TIME_TOTAL must be more than TRANSFER_TIME_UP static double FAR_FLYWHEEL_VELOCITY = 1500; static double NEAR_FLYWHEEL_VELOCITY = 1500; - static double FLYWHEEL_TOP_SPIN = 0; + static double FLYWHEEL_BACK_SPIN = 300; static double TRANSFER_INACTIVE_POSITION = 0; static double TRANSFER_ACTIVE_POSITION = 1; static double REVERSE_INTAKE_SPEED = -1; @@ -172,8 +172,8 @@ public DrumRequest(double position, WaitState nextState) { motULauncher.setMode(DcMotor.RunMode.RUN_USING_ENCODER); // set the motors to a braking behavior so it slows down faster when left trigger is pressed - motLLauncher.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - motULauncher.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + motLLauncher.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); + motULauncher.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); motIntake.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); motLLauncher.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, new PIDFCoefficients(300, 0, 0, 10)); @@ -266,11 +266,11 @@ boolean preLaunch (RequestedColor requestedColor) { @Override void setLaunchVelocity (LaunchDistance launchDistance) { if (launchDistance == LaunchDistance.NEAR) { - upperLaunchVelocity = NEAR_FLYWHEEL_VELOCITY + (0.5 * FLYWHEEL_TOP_SPIN); - lowerLaunchVelocity = NEAR_FLYWHEEL_VELOCITY - (0.5 * FLYWHEEL_TOP_SPIN); + upperLaunchVelocity = NEAR_FLYWHEEL_VELOCITY - (0.5 * FLYWHEEL_BACK_SPIN); + lowerLaunchVelocity = NEAR_FLYWHEEL_VELOCITY + (0.5 * FLYWHEEL_BACK_SPIN); } else { - upperLaunchVelocity = FAR_FLYWHEEL_VELOCITY + (0.5 * FLYWHEEL_TOP_SPIN); - lowerLaunchVelocity = FAR_FLYWHEEL_VELOCITY - (0.5 * FLYWHEEL_TOP_SPIN); + upperLaunchVelocity = FAR_FLYWHEEL_VELOCITY - (0.5 * FLYWHEEL_BACK_SPIN); + lowerLaunchVelocity = FAR_FLYWHEEL_VELOCITY + (0.5 * FLYWHEEL_BACK_SPIN); } } int findSlotFromPosition (double position, double [] positions) { diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/LauncherTest.java b/team417/src/main/java/org/firstinspires/ftc/team417/LauncherTest.java index 5de2a3f03698..c00a783fe520 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/LauncherTest.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/LauncherTest.java @@ -31,7 +31,7 @@ public void initHardware() { servoBLaunchFeeder = hardwareMap.get(CRServo.class, "servoBLaunchFeeder"); servoFLaunchFeeder = hardwareMap.get(CRServo.class, "servoFLaunchFeeder"); - motLLauncher.setDirection(DcMotorSimple.Direction.REVERSE); + servoBLaunchFeeder.setDirection(CRServo.Direction.REVERSE); } @@ -45,14 +45,14 @@ public void runOpMode() { // Spin up launcher flywheels to set flywheel velocities and start feeder wheels if (gamepad2.dpadUpWasPressed()) { - motULauncher.setVelocity(ComplexMechGlob.FAR_FLYWHEEL_VELOCITY + (0.5 * ComplexMechGlob.FLYWHEEL_TOP_SPIN)); - motLLauncher.setVelocity(ComplexMechGlob.FAR_FLYWHEEL_VELOCITY - (0.5 * ComplexMechGlob.FLYWHEEL_TOP_SPIN)); + motULauncher.setVelocity(ComplexMechGlob.FAR_FLYWHEEL_VELOCITY - (0.5 * ComplexMechGlob.FLYWHEEL_BACK_SPIN)); + motLLauncher.setVelocity(ComplexMechGlob.FAR_FLYWHEEL_VELOCITY + (0.5 * ComplexMechGlob.FLYWHEEL_BACK_SPIN)); servoBLaunchFeeder.setPower(ComplexMechGlob.FEEDER_POWER); servoFLaunchFeeder.setPower(ComplexMechGlob.FEEDER_POWER); } else if (gamepad2.dpadDownWasPressed()) { - motULauncher.setVelocity(ComplexMechGlob.NEAR_FLYWHEEL_VELOCITY + (0.5 * ComplexMechGlob.FLYWHEEL_TOP_SPIN)); - motLLauncher.setVelocity(ComplexMechGlob.NEAR_FLYWHEEL_VELOCITY - (0.5 * ComplexMechGlob.FLYWHEEL_TOP_SPIN)); + motULauncher.setVelocity(ComplexMechGlob.NEAR_FLYWHEEL_VELOCITY - (0.5 * ComplexMechGlob.FLYWHEEL_BACK_SPIN)); + motLLauncher.setVelocity(ComplexMechGlob.NEAR_FLYWHEEL_VELOCITY + (0.5 * ComplexMechGlob.FLYWHEEL_BACK_SPIN)); servoBLaunchFeeder.setPower(ComplexMechGlob.FEEDER_POWER); servoFLaunchFeeder.setPower(ComplexMechGlob.FEEDER_POWER); } From 519246a729d0d7aabfd3050c178f8ccc16220cc2 Mon Sep 17 00:00:00 2001 From: Team <417skid@swerverobotics.club> Date: Sun, 30 Nov 2025 14:37:52 -0800 Subject: [PATCH 109/198] we found the paths, mechglob update in loop --- .../ftc/team417/CompetitionAuto.java | 135 ++++-------------- 1 file changed, 24 insertions(+), 111 deletions(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java index 0b50a20d4bbd..40a7d9f03c2b 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java @@ -49,13 +49,14 @@ enum SlowBotMovement { Alliance chosenAlliance; SlowBotMovement chosenMovement; double intakeCycles; - public Action getPath(SlowBotMovement chosenMovement, Alliance chosenAlliance, double intakeCycles) { + public Action getPath(SlowBotMovement chosenMovement, Alliance chosenAlliance, double intakeCycles, MecanumDrive drive) { Pose2d startPose = new Pose2d(0, 0, 0); Pose2d SBNearStartPose = new Pose2d(-60, 48, Math.toRadians(139)); Pose2d SBFarStartPose = new Pose2d(60, 12, Math.toRadians(157.5)); - MecanumDrive drive = new MecanumDrive(hardwareMap, telemetry, gamepad1, startPose); + + PoseMap poseMap = pose -> new Pose2dDual<>( @@ -68,10 +69,11 @@ public Action getPath(SlowBotMovement chosenMovement, Alliance chosenAlliance, d pose.position.y.unaryMinus(), pose.heading.inverse()); } - TrajectoryActionBuilder trajectoryAction = drive.actionBuilder(SBFarStartPose, poseMap); + TrajectoryActionBuilder trajectoryAction = null; switch (chosenMovement) { case NEAR: - trajectoryAction.setTangent(Math.toRadians(-51)) + trajectoryAction = drive.actionBuilder(SBNearStartPose, poseMap); + trajectoryAction = trajectoryAction.setTangent(Math.toRadians(-51)) .splineToConstantHeading(new Vector2d(-36,36), Math.toRadians(-51)) //3 launches //after disp intake @@ -81,13 +83,11 @@ public Action getPath(SlowBotMovement chosenMovement, Alliance chosenAlliance, d .splineToConstantHeading(new Vector2d(-12,55), Math.toRadians(90)) .setTangent(Math.toRadians(-90)) .splineToSplineHeading(new Pose2d(-36,36, Math.toRadians(139)), Math.toRadians(180)); //go to launch position + if (intakeCycles > 1) { trajectoryAction = trajectoryAction.setTangent(Math.toRadians(0)) - - //3 launches //after disp intake - .splineToSplineHeading(new Pose2d(12, 32, Math.toRadians(90)), Math.toRadians(90)) //go to intake middle from goal .setTangent(Math.toRadians(90)) .splineToConstantHeading(new Vector2d(12, 60), Math.toRadians(90)) @@ -95,6 +95,7 @@ public Action getPath(SlowBotMovement chosenMovement, Alliance chosenAlliance, d .splineToSplineHeading(new Pose2d(-36, 36, Math.toRadians(139)), Math.toRadians(180)); //go to launch position //3 launches //after disp intake + if (intakeCycles > 2) { trajectoryAction = trajectoryAction.setTangent(Math.toRadians(0)) .splineToSplineHeading(new Pose2d(36, 32, Math.toRadians(90)), Math.toRadians(90)) //go to intake farthest from goal @@ -106,22 +107,21 @@ public Action getPath(SlowBotMovement chosenMovement, Alliance chosenAlliance, d } } break; + case FAR: + trajectoryAction = drive.actionBuilder(SBFarStartPose, poseMap); if (intakeCycles == 0) { - trajectoryAction.setTangent(Math.toRadians(180)); + trajectoryAction = trajectoryAction.setTangent(Math.toRadians(180)); // 3 launch actions //then after disp intake action } - - trajectoryAction = trajectoryAction.splineToSplineHeading(new Pose2d(36,32, Math.toRadians(90)), Math.toRadians(90)) //go to intake farthest from goal .setTangent(Math.toRadians(90)) .splineToConstantHeading(new Vector2d(36,60), Math.toRadians(90)) .setTangent(Math.toRadians(-90)) .splineToSplineHeading(new Pose2d(54,12, Math.toRadians(157.5)), Math.toRadians(-90)); //go to launch position + if (intakeCycles > 1) { - - // 3 launch actions //after disp intake action trajectoryAction = trajectoryAction.setTangent(Math.toRadians(180)) @@ -132,6 +132,7 @@ public Action getPath(SlowBotMovement chosenMovement, Alliance chosenAlliance, d .splineToSplineHeading(new Pose2d(54, 12, Math.toRadians(157.5)), Math.toRadians(-90)); //go to launch position // 3 launch actions //after disp intake action + if (intakeCycles > 2) { trajectoryAction = trajectoryAction.setTangent(Math.toRadians(180)) .splineToSplineHeading(new Pose2d(-12,32, Math.toRadians(90)), Math.toRadians(90)) //go to intake closest to goal @@ -139,14 +140,15 @@ public Action getPath(SlowBotMovement chosenMovement, Alliance chosenAlliance, d .splineToConstantHeading(new Vector2d(-12,55), Math.toRadians(90)) .setTangent(Math.toRadians(-90)) .splineToSplineHeading(new Pose2d(54,12, Math.toRadians(157.5)), Math.toRadians(-90)); //go to launch position - } } break; + case FAR_OUT_OF_WAY: // 3 launch actions // after disp intake action - trajectoryAction.setTangent(Math.toRadians(180)) + trajectoryAction = drive.actionBuilder(SBFarStartPose, poseMap); + trajectoryAction = trajectoryAction.setTangent(Math.toRadians(180)) .splineToLinearHeading(new Pose2d(60,61, Math.toRadians(0)), Math.toRadians(0)) .setTangent(Math.toRadians(-90)) .splineToLinearHeading(new Pose2d(54,12, Math.toRadians(157.5)), Math.toRadians(-90)) @@ -155,9 +157,10 @@ public Action getPath(SlowBotMovement chosenMovement, Alliance chosenAlliance, d .splineToLinearHeading(new Pose2d(50,32,Math.toRadians(180)), Math.toRadians(180)); break; case FAR_MINIMAL: - trajectoryAction.setTangent(Math.toRadians(90)) - .splineToLinearHeading(new Pose2d(48,32,Math.toRadians(180)), Math.toRadians(180)) - .build(); + trajectoryAction = drive.actionBuilder(SBFarStartPose, poseMap); + trajectoryAction = trajectoryAction.setTangent(Math.toRadians(90)) + .splineToLinearHeading(new Pose2d(48,32,Math.toRadians(180)), Math.toRadians(180)); + break; } return trajectoryAction.build(); @@ -179,7 +182,7 @@ public void runOpMode() { Pose2d SBNearStartPose = new Pose2d(-60, 48, Math.toRadians(139)); Pose2d SBFarStartPose = new Pose2d(60, 12, Math.toRadians(157.5)); MecanumDrive drive = new MecanumDrive(hardwareMap, telemetry, gamepad1, startPose); - + MechGlob mechGlob = ComplexMechGlob.create(hardwareMap, telemetry, false); // Text menu for FastBot @@ -220,24 +223,13 @@ public void runOpMode() { double waitTime = menu.getResult(Double.class, "wait-slider-1"); double intakeCycles = menu.getResult(Double.class, "intake-slider"); - PathFactory pathFactory; - switch (chosenAlliance) { - case RED: - pathFactory = new PathFactory(drive, false); - break; - case BLUE: - pathFactory = new PathFactory(drive, true); - break; - default: - throw new IllegalArgumentException("Alliance must be red or blue"); - } // the first parameter is the type to return as - Action trajectoryAction = null; + Action trajectoryAction; switch (chosenMovement) { case NEAR: @@ -255,7 +247,7 @@ public void runOpMode() { drive.setPose(SBFarStartPose); break; } - trajectoryAction = getPath(chosenMovement, chosenAlliance, intakeCycles); + trajectoryAction = getPath(chosenMovement, chosenAlliance, intakeCycles, drive); // Get a preview of the trajectory's path: Canvas previewCanvas = new Canvas(); @@ -302,7 +294,7 @@ public void runOpMode() { // Draw the preview and then run the next step of the trajectory on top: packet.fieldOverlay().getOperations().addAll(previewCanvas.getOperations()); more = trajectoryAction.run(packet); - + mechGlob.update(); // Only send the packet if there's more to do in order to keep the very last // drawing up on the field once the robot is done: if (more) @@ -312,83 +304,4 @@ public void runOpMode() { } } -class PathFactory { - MecanumDrive drive; - TrajectoryActionBuilder builder; - boolean mirror; - public PathFactory(MecanumDrive drive, boolean mirror) { - this.drive = drive; - this.mirror = mirror; - } - - Pose2d mirrorPose(Pose2d pose) { - return new Pose2d(pose.position.x, -pose.position.y, -pose.heading.log()); - } - Vector2d mirrorVector(Vector2d vector) { - return new Vector2d(vector.x,-vector.y); - } - - public PathFactory actionBuilder(Pose2d pose) { - if (mirror) { - builder = drive.actionBuilder(mirrorPose(pose)); - } else { - builder = drive.actionBuilder(pose); - } - return this; - } - - public PathFactory setTangent(double tangent) { - if (mirror) { - builder = builder.setTangent(-tangent); - } else { - builder = builder.setTangent(tangent); - } - return this; - } - - public PathFactory splineToLinearHeading(Pose2d pose, double tangent) { - if (mirror) { - builder = builder.splineToLinearHeading(mirrorPose(pose), -tangent); - } else { - builder = builder.splineToLinearHeading(pose, tangent); - } - return this; - } - public PathFactory splineToSplineHeading(Pose2d pose, double tangent) { - if (mirror) { - builder = builder.splineToSplineHeading(mirrorPose(pose), -tangent); - } else { - builder = builder.splineToSplineHeading(pose, tangent); - } - return this; - } - public PathFactory splineToConstantHeading(Vector2d vector, double tangent) { - if(mirror) { - builder = builder.splineToConstantHeading(mirrorVector(vector), -tangent); - } else { - builder = builder.splineToConstantHeading(vector, tangent); - - } - return this; - } - - - public PathFactory stopAndAdd(Action a) { - builder = builder.stopAndAdd(a); - return this; - } - - public Action build() { - return builder.build(); - } - - - -} -class LaunchAction extends RobotAction { - @Override - public boolean run(double elapsedTime) { - return false; - } -} From da87bcdb9c923306e0bb540c8c837bea6ab31c61 Mon Sep 17 00:00:00 2001 From: Hankang Zhou <136044263+nobody-particular@users.noreply.github.com> Date: Sun, 16 Nov 2025 15:56:27 -0800 Subject: [PATCH 110/198] Added Limelight3ATest, currently concept code does not work --- .../ftc/team417/apriltags/AprilTagTest.java | 3 +- .../team417/apriltags/Limelight3ATest.java | 157 ++++++++++++++++++ 2 files changed, 159 insertions(+), 1 deletion(-) create mode 100644 team417/src/main/java/org/firstinspires/ftc/team417/apriltags/Limelight3ATest.java diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/apriltags/AprilTagTest.java b/team417/src/main/java/org/firstinspires/ftc/team417/apriltags/AprilTagTest.java index 4e62d2c11cd1..75a7daa7e89c 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/apriltags/AprilTagTest.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/apriltags/AprilTagTest.java @@ -34,6 +34,7 @@ import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + import org.firstinspires.ftc.robotcore.external.hardware.camera.BuiltinCameraDirection; import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; import org.firstinspires.ftc.vision.VisionPortal; @@ -63,8 +64,8 @@ * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list. */ -@Disabled @TeleOp(name = "Concept: AprilTag Easy", group = "Concept") +@Disabled public class AprilTagTest extends LinearOpMode { private static final boolean USE_WEBCAM = true; // true for webcam, false for phone camera diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/apriltags/Limelight3ATest.java b/team417/src/main/java/org/firstinspires/ftc/team417/apriltags/Limelight3ATest.java new file mode 100644 index 000000000000..1256be0f649f --- /dev/null +++ b/team417/src/main/java/org/firstinspires/ftc/team417/apriltags/Limelight3ATest.java @@ -0,0 +1,157 @@ +/* +Copyright (c) 2024 Limelight Vision + +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, +are permitted (subject to the limitations in the disclaimer below) provided that +the following conditions are met: + +Redistributions of source code must retain the above copyright notice, this list +of conditions and the following disclaimer. + +Redistributions in binary form must reproduce the above copyright notice, this +list of conditions and the following disclaimer in the documentation and/or +other materials provided with the distribution. + +Neither the name of FIRST nor the names of its contributors may be used to +endorse or promote products derived from this software without specific prior +written permission. + +NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS +LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF +THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ +package org.firstinspires.ftc.team417.apriltags; + +import com.qualcomm.hardware.limelightvision.LLResult; +import com.qualcomm.hardware.limelightvision.LLResultTypes; +import com.qualcomm.hardware.limelightvision.LLStatus; +import com.qualcomm.hardware.limelightvision.Limelight3A; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import org.firstinspires.ftc.robotcore.external.navigation.Pose3D; + +import java.util.List; + +/* + * This OpMode illustrates how to use the Limelight3A Vision Sensor. + * + * @see Limelight + * + * Notes on configuration: + * + * The device presents itself, when plugged into a USB port on a Control Hub as an ethernet + * interface. A DHCP server running on the Limelight automatically assigns the Control Hub an + * ip address for the new ethernet interface. + * + * Since the Limelight is plugged into a USB port, it will be listed on the top level configuration + * activity along with the Control Hub Portal and other USB devices such as webcams. Typically + * serial numbers are displayed below the device's names. In the case of the Limelight device, the + * Control Hub's assigned ip address for that ethernet interface is used as the "serial number". + * + * Tapping the Limelight's name, transitions to a new screen where the user can rename the Limelight + * and specify the Limelight's ip address. Users should take care not to confuse the ip address of + * the Limelight itself, which can be configured through the Limelight settings page via a web browser, + * and the ip address the Limelight device assigned the Control Hub and which is displayed in small text + * below the name of the Limelight on the top level configuration screen. + */ +@TeleOp(name = "Sensor: Limelight3A", group = "Sensor") +@Disabled +public class Limelight3ATest extends LinearOpMode { + + private Limelight3A limelight; + + @Override + public void runOpMode() throws InterruptedException + { + limelight = hardwareMap.get(Limelight3A.class, "limelight"); + + telemetry.setMsTransmissionInterval(11); + + limelight.pipelineSwitch(0); + + /* + * Starts polling for data. If you neglect to call start(), getLatestResult() will return null. + */ + limelight.start(); + + telemetry.addData(">", "Robot Ready. Press Play."); + telemetry.update(); + waitForStart(); + + while (opModeIsActive()) { + LLStatus status = limelight.getStatus(); + telemetry.addData("Name", "%s", + status.getName()); + telemetry.addData("LL", "Temp: %.1fC, CPU: %.1f%%, FPS: %d", + status.getTemp(), status.getCpu(),(int)status.getFps()); + telemetry.addData("Pipeline", "Index: %d, Type: %s", + status.getPipelineIndex(), status.getPipelineType()); + + LLResult result = limelight.getLatestResult(); + if (result.isValid()) { + // Access general information + Pose3D botpose = result.getBotpose(); + double captureLatency = result.getCaptureLatency(); + double targetingLatency = result.getTargetingLatency(); + double parseLatency = result.getParseLatency(); + telemetry.addData("LL Latency", captureLatency + targetingLatency); + telemetry.addData("Parse Latency", parseLatency); + telemetry.addData("PythonOutput", java.util.Arrays.toString(result.getPythonOutput())); + + telemetry.addData("tx", result.getTx()); + telemetry.addData("txnc", result.getTxNC()); + telemetry.addData("ty", result.getTy()); + telemetry.addData("tync", result.getTyNC()); + + telemetry.addData("Botpose", botpose.toString()); + + // Access barcode results + List barcodeResults = result.getBarcodeResults(); + for (LLResultTypes.BarcodeResult br : barcodeResults) { + telemetry.addData("Barcode", "Data: %s", br.getData()); + } + + // Access classifier results + List classifierResults = result.getClassifierResults(); + for (LLResultTypes.ClassifierResult cr : classifierResults) { + telemetry.addData("Classifier", "Class: %s, Confidence: %.2f", cr.getClassName(), cr.getConfidence()); + } + + // Access detector results + List detectorResults = result.getDetectorResults(); + for (LLResultTypes.DetectorResult dr : detectorResults) { + telemetry.addData("Detector", "Class: %s, Area: %.2f", dr.getClassName(), dr.getTargetArea()); + } + + // Access fiducial results + List fiducialResults = result.getFiducialResults(); + for (LLResultTypes.FiducialResult fr : fiducialResults) { + telemetry.addData("Fiducial", "ID: %d, Family: %s, X: %.2f, Y: %.2f", fr.getFiducialId(), fr.getFamily(), fr.getTargetXDegrees(), fr.getTargetYDegrees()); + } + + // Access color results + List colorResults = result.getColorResults(); + for (LLResultTypes.ColorResult cr : colorResults) { + telemetry.addData("Color", "X: %.2f, Y: %.2f", cr.getTargetXDegrees(), cr.getTargetYDegrees()); + } + } else { + telemetry.addData("Limelight", "No data available"); + } + + telemetry.update(); + } + limelight.stop(); + } +} From c50cbd9a500cb549785b012e71f4160afa51cdf9 Mon Sep 17 00:00:00 2001 From: Hankang Zhou <136044263+nobody-particular@users.noreply.github.com> Date: Sun, 30 Nov 2025 15:38:19 -0800 Subject: [PATCH 111/198] Finished LimelightDetector for Pattern, tested on robot --- .../ftc/team417/CompetitionAuto.java | 15 +- .../team417/apriltags/Limelight3ATest.java | 4 +- .../team417/apriltags/LimelightDetector.java | 278 ++++++++++++++++++ 3 files changed, 288 insertions(+), 9 deletions(-) create mode 100644 team417/src/main/java/org/firstinspires/ftc/team417/apriltags/LimelightDetector.java diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java index 0b50a20d4bbd..843f9b8048aa 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java @@ -10,7 +10,7 @@ import com.acmerobotics.roadrunner.Vector2d; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -import org.firstinspires.ftc.team417.apriltags.AprilTagDetector; +import org.firstinspires.ftc.team417.apriltags.LimelightDetector; import org.firstinspires.ftc.team417.apriltags.Pattern; import org.firstinspires.ftc.team417.javatextmenu.MenuFinishedButton; import org.firstinspires.ftc.team417.javatextmenu.MenuHeader; @@ -203,7 +203,7 @@ public void runOpMode() { .add("finish-button-1", new MenuFinishedButton()); - while (!menu.isCompleted()) { + while (!menu.isCompleted() && !isStopRequested()) { // get x, y (stick) and select (A) input from controller // on Wily Works, this is x, y (wasd) and select (enter) on the keyboard menuInput.update(gamepad1.left_stick_x, -gamepad1.left_stick_y, gamepad1.a); @@ -274,10 +274,10 @@ public void runOpMode() { // Wait for Start to be pressed on the Driver Hub! // (This try-with-resources statement automatically calls detector.close() when it exits // the try-block.) - try (AprilTagDetector detector = new AprilTagDetector(hardwareMap)) { + try (LimelightDetector detector = new LimelightDetector(hardwareMap)) { - while (!isStarted() && !isStopRequested()) { - Pattern last = detector.detectPattern(chosenAlliance); + while (opModeInInit()) { + Pattern last = detector.detectPatternAndTelemeter(chosenAlliance, telemetry); if (last != Pattern.UNKNOWN) { pattern = last; } @@ -285,9 +285,12 @@ public void runOpMode() { telemetry.addData("Chosen alliance: ", chosenAlliance); telemetry.addData("Chosen movement: ", chosenMovement); telemetry.addData("Chosen wait time: ", waitTime); - telemetry.addData("Last valid pattern: ", pattern); telemetry.update(); + + if (isStopRequested()) { + break; + } } } diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/apriltags/Limelight3ATest.java b/team417/src/main/java/org/firstinspires/ftc/team417/apriltags/Limelight3ATest.java index 1256be0f649f..2e9140126e4e 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/apriltags/Limelight3ATest.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/apriltags/Limelight3ATest.java @@ -36,7 +36,6 @@ are permitted (subject to the limitations in the disclaimer below) provided that import com.qualcomm.hardware.limelightvision.LLResultTypes; import com.qualcomm.hardware.limelightvision.LLStatus; import com.qualcomm.hardware.limelightvision.Limelight3A; -import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; @@ -67,7 +66,6 @@ are permitted (subject to the limitations in the disclaimer below) provided that * below the name of the Limelight on the top level configuration screen. */ @TeleOp(name = "Sensor: Limelight3A", group = "Sensor") -@Disabled public class Limelight3ATest extends LinearOpMode { private Limelight3A limelight; @@ -79,7 +77,7 @@ public void runOpMode() throws InterruptedException telemetry.setMsTransmissionInterval(11); - limelight.pipelineSwitch(0); + limelight.pipelineSwitch(7); /* * Starts polling for data. If you neglect to call start(), getLatestResult() will return null. diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/apriltags/LimelightDetector.java b/team417/src/main/java/org/firstinspires/ftc/team417/apriltags/LimelightDetector.java new file mode 100644 index 000000000000..dd24c51655f6 --- /dev/null +++ b/team417/src/main/java/org/firstinspires/ftc/team417/apriltags/LimelightDetector.java @@ -0,0 +1,278 @@ +/* +Copyright (c) 2024 Limelight Vision + +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, +are permitted (subject to the limitations in the disclaimer below) provided that +the following conditions are met: + +Redistributions of source code must retain the above copyright notice, this list +of conditions and the following disclaimer. + +Redistributions in binary form must reproduce the above copyright notice, this +list of conditions and the following disclaimer in the documentation and/or +other materials provided with the distribution. + +Neither the name of FIRST nor the names of its contributors may be used to +endorse or promote products derived from this software without specific prior +written permission. + +NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS +LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF +THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ +package org.firstinspires.ftc.team417.apriltags; + +import com.qualcomm.hardware.limelightvision.LLResult; +import com.qualcomm.hardware.limelightvision.LLResultTypes; +import com.qualcomm.hardware.limelightvision.LLStatus; +import com.qualcomm.hardware.limelightvision.Limelight3A; +import com.qualcomm.robotcore.hardware.HardwareMap; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.robotcore.external.navigation.Pose3D; +import org.firstinspires.ftc.team417.CompetitionAuto; + +import java.io.Closeable; +import java.util.ArrayList; +import java.util.Comparator; +import java.util.List; + +/* + * This class is used to detect AprilTags using the Limelight3A Vision Sensor. + * + * @see Limelight + * + * Notes on configuration: + * + * The device presents itself, when plugged into a USB port on a Control Hub as an ethernet + * interface. A DHCP server running on the Limelight automatically assigns the Control Hub an + * ip address for the new ethernet interface. + * + * Since the Limelight is plugged into a USB port, it will be listed on the top level configuration + * activity along with the Control Hub Portal and other USB devices such as webcams. Typically + * serial numbers are displayed below the device's names. In the case of the Limelight device, the + * Control Hub's assigned ip address for that ethernet interface is used as the "serial number". + * + * Tapping the Limelight's name, transitions to a new screen where the user can rename the Limelight + * and specify the Limelight's ip address. Users should take care not to confuse the ip address of + * the Limelight itself, which can be configured through the Limelight settings page via a web browser, + * and the ip address the Limelight device assigned the Control Hub and which is displayed in small text + * below the name of the Limelight on the top level configuration screen. + */ +public class LimelightDetector implements Closeable { + /** + * The variable to store our instance of the AprilTag processor. + */ + private Limelight3A limelight; + + /** + * The variable for how long ago the detection last changed. + */ + private double timeOfLastChange; + + /** + * The variable for how many detections were and what the ID was in the last cycle. + */ + private int lastDetectionsSize = -1; + private int lastId = -1; + + /** + * Initialize the AprilTag processor. + */ + public LimelightDetector(HardwareMap hardwareMap) { + // Create the AprilTag processor. + limelight = hardwareMap.get(Limelight3A.class, "limelight"); + + limelight.pipelineSwitch(7); + + limelight.start(); + } + + /** + * Default is no verbosity. + */ + public Pattern detectPatternAndTelemeter(CompetitionAuto.Alliance alliance, Telemetry telemetry) { + return detectPatternAndTelemeter(alliance, telemetry, false); + } + + /** + * Add telemetry about AprilTag detections. + */ + public Pattern detectPatternAndTelemeter(CompetitionAuto.Alliance alliance, Telemetry telemetry, boolean verbose) { + LLResult result = limelight.getLatestResult(); + + if (verbose) { + LLStatus status = limelight.getStatus(); + telemetry.addData("Name", "%s", + status.getName()); + telemetry.addData("LL", "Temp: %.1fC, CPU: %.1f%%, FPS: %d", + status.getTemp(), status.getCpu(),(int)status.getFps()); + telemetry.addData("Pipeline", "Index: %d, Type: %s", + status.getPipelineIndex(), status.getPipelineType()); + + if (result.isValid()) { + // Access general information + Pose3D botpose = result.getBotpose(); + double captureLatency = result.getCaptureLatency(); + double targetingLatency = result.getTargetingLatency(); + double parseLatency = result.getParseLatency(); + telemetry.addData("LL Latency", captureLatency + targetingLatency); + telemetry.addData("Parse Latency", parseLatency); + telemetry.addData("PythonOutput", java.util.Arrays.toString(result.getPythonOutput())); + + telemetry.addData("tx", result.getTx()); + telemetry.addData("txnc", result.getTxNC()); + telemetry.addData("ty", result.getTy()); + telemetry.addData("tync", result.getTyNC()); + + telemetry.addData("Botpose", botpose.toString()); + + + // Access fiducial results + List fiducialResults = result.getFiducialResults(); + telemetry.addData("# AprilTags Detected", fiducialResults.size()); + + for (LLResultTypes.FiducialResult fr : fiducialResults) { + telemetry.addData("AprilTags", "ID: %d, Family: %s, X: %.2f, Y: %.2f, Pose: %s", fr.getFiducialId(), fr.getFamily(), fr.getTargetXDegrees(), fr.getTargetYDegrees(), fr.getRobotPoseFieldSpace()); + } + } else { + telemetry.addData("Limelight", "No data available"); + } + } + + // Get the pattern: + Pattern pattern = detectPattern(alliance, result); + + String patternDisplay; + + // The `\\u...` are escape sequences for green and purple circle emojis. + // \uD83D\uDFE3 -> Purple circle + // \uD83D\uDFE2 -> Green circle + // \u26AA -> White circle + switch (pattern) { + case PPG: + patternDisplay = "\uD83D\uDFE3\uD83D\uDFE3\uD83D\uDFE2"; + break; + case PGP: + patternDisplay = "\uD83D\uDFE3\uD83D\uDFE2\uD83D\uDFE3"; + break; + case GPP: + patternDisplay = "\uD83D\uDFE2\uD83D\uDFE3\uD83D\uDFE3"; + break; + default: + patternDisplay = "\u26AA\u26AA\u26AA"; + break; + } + + double elapsedTime = (System.currentTimeMillis() / 1000.0) - timeOfLastChange; + + // Summarize the most important detection info in one line: + switch (alliance) { + case RED: + telemetry.addLine(String.format("%s (%d IDs, leftmost ID = %d for %f sec.)", + patternDisplay, lastDetectionsSize, lastId, elapsedTime)); + break; + case BLUE: + telemetry.addLine(String.format("%s (%d IDs, rightmost ID = %d for %f sec.)", + patternDisplay, lastDetectionsSize, lastId, elapsedTime)); + break; + } + + return pattern; + } // end method telemetryAprilTag() + + /** + * We want the Limelight to get its own result as default. + */ + public Pattern detectPattern(CompetitionAuto.Alliance alliance) { + return detectPattern(alliance, null); + } + + /** + * Detect the correct color pattern and return it. + */ + public Pattern detectPattern(CompetitionAuto.Alliance alliance, LLResult result) { + if (result == null) + result = limelight.getLatestResult(); + + List currentDetections = new ArrayList<>(); + + if (result.isValid()) + currentDetections = result.getFiducialResults(); + + // Remove all AprilTags that don't have ID 21, 22, or 23 + // (This is because the obelisk only has AprilTags with IDs 21, 22, and 23) + // (The remaining IDs, 20 and 24, are for localization only) + currentDetections.removeIf(detection -> + detection.getFiducialId() != 21 + && detection.getFiducialId() != 22 + && detection.getFiducialId() != 23 + ); + + // FiducialResult objects contain the x (left) and y (up) degrees relative to the robot + // When we're on the red alliance, we want the leftmost valid + // AprilTag, and when we're on the blue alliance, we want the rightmost valid AprilTag. + // This is because, in our near position, we see two AprilTags on the obelisk: the front + // AprilTag and the side AprilTag closest to our color goal. + // For more information about the info the AprilTagDetection object contains, see this link: + // https://ftc-docs.firstinspires.org/en/latest/apriltag/understanding_apriltag_detection_values/understanding-apriltag-detection-values.html + LLResultTypes.FiducialResult detection = null; + switch (alliance) { + case RED: + // Set detection to the leftmost (min x degrees) detection relative to the robot + // If there are no detections, set it to null + detection = currentDetections.stream() + .min(Comparator.comparingDouble(aprilTagDetection -> aprilTagDetection.getTargetXDegrees())).orElse(null); + break; + case BLUE: + // Set detection to the rightmost (max x degrees) detection relative to the robot + // If there are no detections, set it to null + detection = currentDetections.stream() + .max(Comparator.comparingDouble(aprilTagDetection -> aprilTagDetection.getTargetXDegrees())).orElse(null); + } + + if (detection == null) { + lastId = -1; + timeOfLastChange = System.currentTimeMillis() / 1_000.0; + return Pattern.UNKNOWN; + } + + int currentDetectionsSize = currentDetections.size(); + + if (lastDetectionsSize != currentDetectionsSize || detection.getFiducialId() != lastId) { + timeOfLastChange = System.currentTimeMillis() / 1_000.0; + } + + lastDetectionsSize = currentDetectionsSize; + lastId = detection.getFiducialId(); + + switch (detection.getFiducialId()) { + case 21: + return Pattern.GPP; + case 22: + return Pattern.PGP; + case 23: + return Pattern.PPG; + default: + return Pattern.UNKNOWN; + } + } + + /** + * Release the resources taken up by the vision portal. + */ + @Override + public void close() { + limelight.stop(); + } +} From 49823742863dc95fd830acfff9a9da03a2d1168e Mon Sep 17 00:00:00 2001 From: Team <417skid@swerverobotics.club> Date: Sun, 30 Nov 2025 16:02:55 -0800 Subject: [PATCH 112/198] launch in auto --- .../ftc/team417/CompetitionAuto.java | 277 ++++++++++-------- 1 file changed, 160 insertions(+), 117 deletions(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java index 40a7d9f03c2b..15bffdf1fa06 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java @@ -46,10 +46,10 @@ enum SlowBotMovement { TextMenu menu = new TextMenu(); MenuInput menuInput = new MenuInput(MenuInput.InputType.CONTROLLER); Pattern pattern; - Alliance chosenAlliance; - SlowBotMovement chosenMovement; - double intakeCycles; - public Action getPath(SlowBotMovement chosenMovement, Alliance chosenAlliance, double intakeCycles, MecanumDrive drive) { + CountBalls countBalls = new CountBalls(); + + + public Action getPath(SlowBotMovement chosenMovement, Alliance chosenAlliance, double intakeCycles, MecanumDrive drive, MechGlob mechGlob) { Pose2d startPose = new Pose2d(0, 0, 0); @@ -57,8 +57,6 @@ public Action getPath(SlowBotMovement chosenMovement, Alliance chosenAlliance, d Pose2d SBFarStartPose = new Pose2d(60, 12, Math.toRadians(157.5)); - - PoseMap poseMap = pose -> new Pose2dDual<>( pose.position.x, pose.position.y, @@ -69,21 +67,20 @@ public Action getPath(SlowBotMovement chosenMovement, Alliance chosenAlliance, d pose.position.y.unaryMinus(), pose.heading.inverse()); } - TrajectoryActionBuilder trajectoryAction = null; + TrajectoryActionBuilder trajectoryAction = null; switch (chosenMovement) { case NEAR: trajectoryAction = drive.actionBuilder(SBNearStartPose, poseMap); trajectoryAction = trajectoryAction.setTangent(Math.toRadians(-51)) - .splineToConstantHeading(new Vector2d(-36,36), Math.toRadians(-51)) - //3 launches - //after disp intake - .setTangent(Math.toRadians(0)) - .splineToSplineHeading(new Pose2d(-12,32, Math.toRadians(90)), Math.toRadians(90)) //go to intake closest from goal - .setTangent(Math.toRadians(90)) - .splineToConstantHeading(new Vector2d(-12,55), Math.toRadians(90)) - .setTangent(Math.toRadians(-90)) - .splineToSplineHeading(new Pose2d(-36,36, Math.toRadians(139)), Math.toRadians(180)); //go to launch position - + .splineToConstantHeading(new Vector2d(-36, 36), Math.toRadians(-51)) + .stopAndAdd(new LaunchAction(mechGlob, pattern, countBalls)) + .setTangent(Math.toRadians(0)) + .splineToSplineHeading(new Pose2d(-12, 32, Math.toRadians(90)), Math.toRadians(90)) //go to intake closest from goal + .setTangent(Math.toRadians(90)) + .splineToConstantHeading(new Vector2d(-12, 55), Math.toRadians(90)) + .setTangent(Math.toRadians(-90)) + .splineToSplineHeading(new Pose2d(-36, 36, Math.toRadians(139)), Math.toRadians(180)); //go to launch position + if (intakeCycles > 1) { trajectoryAction = trajectoryAction.setTangent(Math.toRadians(0)) //3 launches @@ -95,7 +92,7 @@ public Action getPath(SlowBotMovement chosenMovement, Alliance chosenAlliance, d .splineToSplineHeading(new Pose2d(-36, 36, Math.toRadians(139)), Math.toRadians(180)); //go to launch position //3 launches //after disp intake - + if (intakeCycles > 2) { trajectoryAction = trajectoryAction.setTangent(Math.toRadians(0)) .splineToSplineHeading(new Pose2d(36, 32, Math.toRadians(90)), Math.toRadians(90)) //go to intake farthest from goal @@ -107,7 +104,7 @@ public Action getPath(SlowBotMovement chosenMovement, Alliance chosenAlliance, d } } break; - + case FAR: trajectoryAction = drive.actionBuilder(SBFarStartPose, poseMap); if (intakeCycles == 0) { @@ -115,12 +112,12 @@ public Action getPath(SlowBotMovement chosenMovement, Alliance chosenAlliance, d // 3 launch actions //then after disp intake action } - trajectoryAction = trajectoryAction.splineToSplineHeading(new Pose2d(36,32, Math.toRadians(90)), Math.toRadians(90)) //go to intake farthest from goal + trajectoryAction = trajectoryAction.splineToSplineHeading(new Pose2d(36, 32, Math.toRadians(90)), Math.toRadians(90)) //go to intake farthest from goal .setTangent(Math.toRadians(90)) - .splineToConstantHeading(new Vector2d(36,60), Math.toRadians(90)) + .splineToConstantHeading(new Vector2d(36, 60), Math.toRadians(90)) .setTangent(Math.toRadians(-90)) - .splineToSplineHeading(new Pose2d(54,12, Math.toRadians(157.5)), Math.toRadians(-90)); //go to launch position - + .splineToSplineHeading(new Pose2d(54, 12, Math.toRadians(157.5)), Math.toRadians(-90)); //go to launch position + if (intakeCycles > 1) { // 3 launch actions //after disp intake action @@ -132,50 +129,46 @@ public Action getPath(SlowBotMovement chosenMovement, Alliance chosenAlliance, d .splineToSplineHeading(new Pose2d(54, 12, Math.toRadians(157.5)), Math.toRadians(-90)); //go to launch position // 3 launch actions //after disp intake action - + if (intakeCycles > 2) { trajectoryAction = trajectoryAction.setTangent(Math.toRadians(180)) - .splineToSplineHeading(new Pose2d(-12,32, Math.toRadians(90)), Math.toRadians(90)) //go to intake closest to goal + .splineToSplineHeading(new Pose2d(-12, 32, Math.toRadians(90)), Math.toRadians(90)) //go to intake closest to goal .setTangent(Math.toRadians(90)) - .splineToConstantHeading(new Vector2d(-12,55), Math.toRadians(90)) + .splineToConstantHeading(new Vector2d(-12, 55), Math.toRadians(90)) .setTangent(Math.toRadians(-90)) - .splineToSplineHeading(new Pose2d(54,12, Math.toRadians(157.5)), Math.toRadians(-90)); //go to launch position + .splineToSplineHeading(new Pose2d(54, 12, Math.toRadians(157.5)), Math.toRadians(-90)); //go to launch position } } break; - + case FAR_OUT_OF_WAY: // 3 launch actions // after disp intake action trajectoryAction = drive.actionBuilder(SBFarStartPose, poseMap); trajectoryAction = trajectoryAction.setTangent(Math.toRadians(180)) - .splineToLinearHeading(new Pose2d(60,61, Math.toRadians(0)), Math.toRadians(0)) - .setTangent(Math.toRadians(-90)) - .splineToLinearHeading(new Pose2d(54,12, Math.toRadians(157.5)), Math.toRadians(-90)) - // 3 launch actions - .setTangent(Math.toRadians(90)) - .splineToLinearHeading(new Pose2d(50,32,Math.toRadians(180)), Math.toRadians(180)); + .splineToLinearHeading(new Pose2d(60, 61, Math.toRadians(0)), Math.toRadians(0)) + .setTangent(Math.toRadians(-90)) + .splineToLinearHeading(new Pose2d(54, 12, Math.toRadians(157.5)), Math.toRadians(-90)) + // 3 launch actions + .setTangent(Math.toRadians(90)) + .splineToLinearHeading(new Pose2d(50, 32, Math.toRadians(180)), Math.toRadians(180)); break; case FAR_MINIMAL: trajectoryAction = drive.actionBuilder(SBFarStartPose, poseMap); trajectoryAction = trajectoryAction.setTangent(Math.toRadians(90)) - .splineToLinearHeading(new Pose2d(48,32,Math.toRadians(180)), Math.toRadians(180)); + .splineToLinearHeading(new Pose2d(48, 32, Math.toRadians(180)), Math.toRadians(180)); break; } return trajectoryAction.build(); - - - } + @Override public void runOpMode() { - - Pose2d startPose = new Pose2d(0, 0, 0); @@ -188,22 +181,22 @@ public void runOpMode() { // Text menu for FastBot - // Text menu for SlowBot - menu.add(new MenuHeader("AUTO SETUP")) - .add() // empty line for spacing - .add("Pick an alliance:") - .add("alliance-picker-1", Alliance.class) // enum selector shortcut - .add() - .add("Pick a movement:") - .add("movement-picker-1", SlowBotMovement.class) // enum selector shortcut - .add() - .add("Intake Cycles:") - .add("intake-slider", new MenuSlider(minIntakes, maxIntakes)) - .add() - .add("Wait time:") - .add("wait-slider-1", new MenuSlider(minWaitTime, maxWaitTime)) - .add() - .add("finish-button-1", new MenuFinishedButton()); + // Text menu for SlowBot + menu.add(new MenuHeader("AUTO SETUP")) + .add() // empty line for spacing + .add("Pick an alliance:") + .add("alliance-picker-1", Alliance.class) // enum selector shortcut + .add() + .add("Pick a movement:") + .add("movement-picker-1", SlowBotMovement.class) // enum selector shortcut + .add() + .add("Intake Cycles:") + .add("intake-slider", new MenuSlider(minIntakes, maxIntakes)) + .add() + .add("Wait time:") + .add("wait-slider-1", new MenuSlider(minWaitTime, maxWaitTime)) + .add() + .add("finish-button-1", new MenuFinishedButton()); while (!menu.isCompleted()) { @@ -224,84 +217,134 @@ public void runOpMode() { double intakeCycles = menu.getResult(Double.class, "intake-slider"); - - // the first parameter is the type to return as - Action trajectoryAction; - - switch (chosenMovement) { - case NEAR: + Action trajectoryAction; - drive.setPose(SBNearStartPose); - - break; - case FAR: - drive.setPose(SBFarStartPose); - break; - case FAR_OUT_OF_WAY: - drive.setPose(SBFarStartPose); - break; - case FAR_MINIMAL: - drive.setPose(SBFarStartPose); - break; - } - trajectoryAction = getPath(chosenMovement, chosenAlliance, intakeCycles, drive); + switch (chosenMovement) { + case NEAR: - // Get a preview of the trajectory's path: - Canvas previewCanvas = new Canvas(); - trajectoryAction.preview(previewCanvas); + drive.setPose(SBNearStartPose); - // Show the preview on FTC Dashboard now. - TelemetryPacket packet = MecanumDrive.getTelemetryPacket(); - packet.fieldOverlay().getOperations().addAll(previewCanvas.getOperations()); - MecanumDrive.sendTelemetryPacket(packet); + break; + case FAR: + drive.setPose(SBFarStartPose); + break; + case FAR_OUT_OF_WAY: + drive.setPose(SBFarStartPose); + break; + case FAR_MINIMAL: + drive.setPose(SBFarStartPose); + break; + } + trajectoryAction = getPath(chosenMovement, chosenAlliance, intakeCycles, drive, mechGlob); + // Get a preview of the trajectory's path: + Canvas previewCanvas = new Canvas(); + trajectoryAction.preview(previewCanvas); - // Assume unknown pattern unless detected otherwise. - pattern = Pattern.UNKNOWN; + // Show the preview on FTC Dashboard now. + TelemetryPacket packet = MecanumDrive.getTelemetryPacket(); + packet.fieldOverlay().getOperations().addAll(previewCanvas.getOperations()); + MecanumDrive.sendTelemetryPacket(packet); - // Detect the pattern with the AprilTags from the camera! - // Wait for Start to be pressed on the Driver Hub! - // (This try-with-resources statement automatically calls detector.close() when it exits - // the try-block.) - try (AprilTagDetector detector = new AprilTagDetector(hardwareMap)) { - while (!isStarted() && !isStopRequested()) { - Pattern last = detector.detectPattern(chosenAlliance); - if (last != Pattern.UNKNOWN) { - pattern = last; - } + // Assume unknown pattern unless detected otherwise. + pattern = Pattern.UNKNOWN; - telemetry.addData("Chosen alliance: ", chosenAlliance); - telemetry.addData("Chosen movement: ", chosenMovement); - telemetry.addData("Chosen wait time: ", waitTime); - telemetry.addData("Last valid pattern: ", pattern); + // Detect the pattern with the AprilTags from the camera! + // Wait for Start to be pressed on the Driver Hub! + // (This try-with-resources statement automatically calls detector.close() when it exits + // the try-block.) + try (AprilTagDetector detector = new AprilTagDetector(hardwareMap)) { - telemetry.update(); + while (!isStarted() && !isStopRequested()) { + Pattern last = detector.detectPattern(chosenAlliance); + if (last != Pattern.UNKNOWN) { + pattern = last; } - } - sleep((long)waitTime*1000); - boolean more = true; - while (opModeIsActive() && more) { - telemetry.addLine("Running Auto!"); - - // 'packet' is the object used to send data to FTC Dashboard: - packet = MecanumDrive.getTelemetryPacket(); - - // Draw the preview and then run the next step of the trajectory on top: - packet.fieldOverlay().getOperations().addAll(previewCanvas.getOperations()); - more = trajectoryAction.run(packet); - mechGlob.update(); - // Only send the packet if there's more to do in order to keep the very last - // drawing up on the field once the robot is done: - if (more) - MecanumDrive.sendTelemetryPacket(packet); + telemetry.addData("Chosen alliance: ", chosenAlliance); + telemetry.addData("Chosen movement: ", chosenMovement); + telemetry.addData("Chosen wait time: ", waitTime); + telemetry.addData("Last valid pattern: ", pattern); + telemetry.update(); } } + if(chosenMovement == SlowBotMovement.NEAR) { + mechGlob.setLaunchVelocity(LaunchDistance.NEAR); + } else { + mechGlob.setLaunchVelocity(LaunchDistance.FAR); + } + sleep((long) waitTime * 1000); + boolean more = true; + while (opModeIsActive() && more) { + telemetry.addLine("Running Auto!"); + + // 'packet' is the object used to send data to FTC Dashboard: + packet = MecanumDrive.getTelemetryPacket(); + + // Draw the preview and then run the next step of the trajectory on top: + packet.fieldOverlay().getOperations().addAll(previewCanvas.getOperations()); + more = trajectoryAction.run(packet); + mechGlob.update(); + // Only send the packet if there's more to do in order to keep the very last + // drawing up on the field once the robot is done: + if (more) + MecanumDrive.sendTelemetryPacket(packet); + telemetry.update(); + } + } +} +class LaunchAction extends RobotAction { + MechGlob mechGlob; + Pattern pattern; + CountBalls orderCount; + RequestedColor[] array; + public LaunchAction(MechGlob mechGlob, Pattern pattern, CountBalls orderCount) { + this.mechGlob = mechGlob; + this.pattern = pattern; + this.orderCount = orderCount; + if (pattern == Pattern.GPP) { + array = new RequestedColor[] {RequestedColor.GREEN, RequestedColor.PURPLE, RequestedColor.PURPLE}; + } else if (pattern == Pattern.PGP) { + array = new RequestedColor[] {RequestedColor.PURPLE, RequestedColor.GREEN, RequestedColor.PURPLE}; + } else { + array = new RequestedColor[] {RequestedColor.PURPLE, RequestedColor.PURPLE, RequestedColor.GREEN}; + } + + + + } + @Override + public boolean run(double elapsedTime) { + if (elapsedTime == 0) { + RequestedColor requestedColor = array[orderCount.orderCount]; + mechGlob.launch(requestedColor); + orderCount.increment(); + requestedColor = array[orderCount.orderCount]; + mechGlob.launch(requestedColor); + orderCount.increment(); + requestedColor = array[orderCount.orderCount]; + mechGlob.launch(requestedColor); + orderCount.increment(); + } + return !mechGlob.isDoneLaunching(); //we are done + } + +} +class CountBalls { + public int orderCount; // 0, 1 or 2 to find color pattern + public void increment() { + if (orderCount == 2) { + orderCount = 0; + } else { + orderCount++; + } + } +} From 8833a02028b24368267317909463b64149d841d2 Mon Sep 17 00:00:00 2001 From: Team <417skid@swerverobotics.club> Date: Sun, 30 Nov 2025 16:20:39 -0800 Subject: [PATCH 113/198] intake in auto --- .../ftc/team417/CompetitionAuto.java | 24 +++++++++++++++---- 1 file changed, 20 insertions(+), 4 deletions(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java index 15bffdf1fa06..35d452952acc 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java @@ -76,15 +76,16 @@ public Action getPath(SlowBotMovement chosenMovement, Alliance chosenAlliance, d .stopAndAdd(new LaunchAction(mechGlob, pattern, countBalls)) .setTangent(Math.toRadians(0)) .splineToSplineHeading(new Pose2d(-12, 32, Math.toRadians(90)), Math.toRadians(90)) //go to intake closest from goal + .stopAndAdd(new IntakeAction(mechGlob, 1)) .setTangent(Math.toRadians(90)) .splineToConstantHeading(new Vector2d(-12, 55), Math.toRadians(90)) + .stopAndAdd(new IntakeAction(mechGlob, 0)) .setTangent(Math.toRadians(-90)) - .splineToSplineHeading(new Pose2d(-36, 36, Math.toRadians(139)), Math.toRadians(180)); //go to launch position - + .splineToSplineHeading(new Pose2d(-36, 36, Math.toRadians(139)), Math.toRadians(180)) //go to launch position + .stopAndAdd(new LaunchAction(mechGlob, pattern, countBalls)); if (intakeCycles > 1) { trajectoryAction = trajectoryAction.setTangent(Math.toRadians(0)) - //3 launches - //after disp intake + .splineToSplineHeading(new Pose2d(12, 32, Math.toRadians(90)), Math.toRadians(90)) //go to intake middle from goal .setTangent(Math.toRadians(90)) .splineToConstantHeading(new Vector2d(12, 60), Math.toRadians(90)) @@ -335,6 +336,21 @@ public boolean run(double elapsedTime) { } } +class IntakeAction extends RobotAction { + double intakeSpeed; + MechGlob mechGlob; + public IntakeAction(MechGlob mechGlob, double intakeSpeed) { + this.intakeSpeed = intakeSpeed; + this.mechGlob = mechGlob; + + } + + @Override + public boolean run(double elapsedTime) { + mechGlob.intake(intakeSpeed); + return false; + } +} class CountBalls { public int orderCount; // 0, 1 or 2 to find color pattern public void increment() { From 44f3628dc41e2967dd01e6660036c36ab482bc8f Mon Sep 17 00:00:00 2001 From: Andrew Date: Sun, 30 Nov 2025 16:36:02 -0800 Subject: [PATCH 114/198] Hankang is calling a new OpMode member function. --- .../java/com/wilyworks/simulator/framework/WilyOpMode.java | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyOpMode.java b/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyOpMode.java index f6fc6abe1994..650fd54f8440 100644 --- a/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyOpMode.java +++ b/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyOpMode.java @@ -19,6 +19,10 @@ public final void idle() { Thread.yield(); } + public final boolean opModeInInit() { + return !isStarted() && !isStopRequested(); + } + public void waitForStart() { WilyCore.render(); while (!isStarted()) From 6e8258ee69564ae6d4547a3b32f0c263f90a6d57 Mon Sep 17 00:00:00 2001 From: Emmett Date: Sun, 30 Nov 2025 16:51:42 -0800 Subject: [PATCH 115/198] added drum positions & voltages, and edited the epsilon reversed some motors --- .../ftc/team417/ComplexMechGlob.java | 17 +++++++++-------- .../ftc/team417/roadrunner/MecanumDrive.java | 3 +++ 2 files changed, 12 insertions(+), 8 deletions(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java b/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java index cd0e132a1082..e7cab205e7b0 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java @@ -84,7 +84,7 @@ public class ComplexMechGlob extends MechGlob { //a class encompassing all code static double REVERSE_INTAKE_SPEED = -1; static double INTAKE_SPEED = 1; static double FLYWHEEL_VELOCITY_TOLERANCE = 25; //this is an epsiiiiiiiiilon - static double VOLTAGE_TOLERANCE = 0.05; //THIS IS AN EPSILON AS WELLLLLL + static double VOLTAGE_TOLERANCE = 0.01; //THIS IS AN EPSILON AS WELLLLLL ElapsedTime transferTimer; @@ -102,10 +102,10 @@ enum WaitState { } WaitState waitState = WaitState.IDLE; // arrays with placeholder values for servo positions and voltages relative to intake and launch - double [] INTAKE_POSITIONS = {0, 1, 2}; - double [] INTAKE_VOLTS = {0, 1, 2}; - double [] LAUNCH_POSITIONS = {0, 1, 2}; - double [] LAUNCH_VOLTS = {0, 1, 2}; + double [] INTAKE_POSITIONS = {0.067, 0.44, 0.803}; + double [] INTAKE_VOLTS = {2.94, 1.83, 0.74}; + double [] LAUNCH_POSITIONS = {0.258, 0.627, 1}; + double [] LAUNCH_VOLTS = {2.37, 1.27, 0.155}; double lastQueuedPosition; //where the servo was *queued* to go last. NOT THE SAME AS hwDrumPosition! double hwDrumPosition; //where the drum was *told* to go last. NOT THE SAME AS lastQueuedPosition! double upperLaunchVelocity; @@ -179,8 +179,9 @@ public DrumRequest(double position, WaitState nextState) { motLLauncher.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, new PIDFCoefficients(300, 0, 0, 10)); motULauncher.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, new PIDFCoefficients(300, 0, 0, 10)); - motLLauncher.setDirection(DcMotorSimple.Direction.REVERSE); - servoBLaunchFeeder.setDirection(CRServo.Direction.REVERSE); + motULauncher.setDirection(DcMotorSimple.Direction.REVERSE); + + setLaunchVelocity(LaunchDistance.NEAR); } @@ -243,7 +244,7 @@ boolean drumAtPosition() { if (intakeSlot != -1) { expectedVolts = INTAKE_VOLTS[intakeSlot]; } else { - expectedVolts = LAUNCH_VOLTS[launchSlot]; + expectedVolts = LAUNCH_VOLTS[launchSlot]; } return Math.abs(analogDrum.getVoltage() - expectedVolts) <= VOLTAGE_TOLERANCE; } diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/roadrunner/MecanumDrive.java b/team417/src/main/java/org/firstinspires/ftc/team417/roadrunner/MecanumDrive.java index 3d25de386e61..946d4a407a61 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/roadrunner/MecanumDrive.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/roadrunner/MecanumDrive.java @@ -256,6 +256,9 @@ public DriveLocalizer() { leftBack = new OverflowEncoder(new RawEncoder(MecanumDrive.this.leftBack)); rightBack = new OverflowEncoder(new RawEncoder(MecanumDrive.this.rightBack)); rightFront = new OverflowEncoder(new RawEncoder(MecanumDrive.this.rightFront)); + leftFront.setDirection(DcMotorEx.Direction.REVERSE); + leftBack.setDirection(DcMotorEx.Direction.REVERSE); + // TODO: reverse encoders if needed // leftFront.setDirection(DcMotorSimple.Direction.REVERSE); From 544baf22edd1a07e22e80a304bd9e22a47472c1d Mon Sep 17 00:00:00 2001 From: Andrew Date: Sun, 30 Nov 2025 21:02:06 -0800 Subject: [PATCH 116/198] Update to latest drop of Wily Works and Configuration Tester. --- WilyCore/build.gradle | 1 + .../hardware/limelightvision/LLResult.java | 15 +- .../limelightvision/LLResultTypes.java | 2 +- .../hardware/limelightvision/LLStatus.java | 4 +- .../hardware/limelightvision/Limelight3A.java | 7 +- .../simulator/framework/MechSim.java | 27 ++- .../src/main/java/org/json/JSONArray.java | 190 --------------- .../src/main/java/org/json/JSONException.java | 20 -- .../src/main/java/org/json/JSONObject.java | 221 ------------------ .../ftc/team417/roadrunner/MecanumDrive.java | 5 +- .../team417/utils/ConfigurationTester.java | 177 ++++++++------ 11 files changed, 153 insertions(+), 516 deletions(-) delete mode 100644 WilyCore/src/main/java/org/json/JSONArray.java delete mode 100644 WilyCore/src/main/java/org/json/JSONException.java delete mode 100644 WilyCore/src/main/java/org/json/JSONObject.java diff --git a/WilyCore/build.gradle b/WilyCore/build.gradle index fead464319dc..d575434548bb 100644 --- a/WilyCore/build.gradle +++ b/WilyCore/build.gradle @@ -63,6 +63,7 @@ dependencies { implementation "com.google.code.gson:gson:2.11.0" // For serializing Java objects to JSON implementation 'net.bytebuddy:byte-buddy:1.15.10' // For proxying classes rather than interfaces implementation 'org.objenesis:objenesis:3.2' // Instantiate class without invoking a constructor + implementation 'org.json:json:20250107' // Or any recent version // Lwjgl for gamepad input: implementation platform("org.lwjgl:lwjgl-bom:$lwjglVersion") diff --git a/WilyCore/src/main/java/com/qualcomm/hardware/limelightvision/LLResult.java b/WilyCore/src/main/java/com/qualcomm/hardware/limelightvision/LLResult.java index 2c0f7bbad275..d53848613ed1 100644 --- a/WilyCore/src/main/java/com/qualcomm/hardware/limelightvision/LLResult.java +++ b/WilyCore/src/main/java/com/qualcomm/hardware/limelightvision/LLResult.java @@ -33,19 +33,24 @@ are permitted (subject to the limitations in the disclaimer below) provided that THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -import org.json.JSONArray; -import org.json.JSONException; -import org.json.JSONObject; +import com.qualcomm.hardware.limelightvision.LLResultTypes.BarcodeResult; +import com.qualcomm.hardware.limelightvision.LLResultTypes.ClassifierResult; +import com.qualcomm.hardware.limelightvision.LLResultTypes.ColorResult; +import com.qualcomm.hardware.limelightvision.LLResultTypes.DetectorResult; +import com.qualcomm.hardware.limelightvision.LLResultTypes.FiducialResult; + import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; import org.firstinspires.ftc.robotcore.external.navigation.Pose3D; import org.firstinspires.ftc.robotcore.external.navigation.Position; import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles; +import org.json.JSONArray; +import org.json.JSONException; +import org.json.JSONObject; + import java.util.ArrayList; import java.util.List; -import com.qualcomm.hardware.limelightvision.LLResultTypes.*; - /** * Represents the result of a Limelight Pipeline. This class parses JSON data from a Limelight * in the constructor and provides easy access to the results data. diff --git a/WilyCore/src/main/java/com/qualcomm/hardware/limelightvision/LLResultTypes.java b/WilyCore/src/main/java/com/qualcomm/hardware/limelightvision/LLResultTypes.java index 05b93a69ae6e..ea4910573607 100644 --- a/WilyCore/src/main/java/com/qualcomm/hardware/limelightvision/LLResultTypes.java +++ b/WilyCore/src/main/java/com/qualcomm/hardware/limelightvision/LLResultTypes.java @@ -2,8 +2,8 @@ import org.firstinspires.ftc.robotcore.external.navigation.Pose3D; import org.json.JSONArray; -import org.json.JSONObject; import org.json.JSONException; +import org.json.JSONObject; import java.util.ArrayList; import java.util.List; diff --git a/WilyCore/src/main/java/com/qualcomm/hardware/limelightvision/LLStatus.java b/WilyCore/src/main/java/com/qualcomm/hardware/limelightvision/LLStatus.java index 8c3ae6c69707..23f4bb9e3e7b 100644 --- a/WilyCore/src/main/java/com/qualcomm/hardware/limelightvision/LLStatus.java +++ b/WilyCore/src/main/java/com/qualcomm/hardware/limelightvision/LLStatus.java @@ -33,9 +33,9 @@ are permitted (subject to the limitations in the disclaimer below) provided that THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ -import org.json.JSONException; -import org.json.JSONObject; import org.firstinspires.ftc.robotcore.external.navigation.Quaternion; +import org.json.JSONObject; + /** * Represents the status of a Limelight. */ diff --git a/WilyCore/src/main/java/com/qualcomm/hardware/limelightvision/Limelight3A.java b/WilyCore/src/main/java/com/qualcomm/hardware/limelightvision/Limelight3A.java index 550274b55af3..23ad79a6265d 100644 --- a/WilyCore/src/main/java/com/qualcomm/hardware/limelightvision/Limelight3A.java +++ b/WilyCore/src/main/java/com/qualcomm/hardware/limelightvision/Limelight3A.java @@ -33,13 +33,14 @@ are permitted (subject to the limitations in the disclaimer below) provided that THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ +import com.qualcomm.hardware.limelightvision.LLResultTypes.CalibrationResult; import com.qualcomm.robotcore.hardware.HardwareDevice; import com.qualcomm.robotcore.util.SerialNumber; import org.firstinspires.ftc.robotcore.internal.usb.EthernetOverUsbSerialNumber; -import org.json.JSONException; -import org.json.JSONObject; import org.json.JSONArray; +import org.json.JSONObject; + import java.io.BufferedReader; import java.io.IOException; import java.io.InputStreamReader; @@ -51,8 +52,6 @@ are permitted (subject to the limitations in the disclaimer below) provided that import java.util.concurrent.Executors; import java.util.concurrent.ScheduledExecutorService; import java.util.concurrent.TimeUnit; -import com.qualcomm.hardware.limelightvision.LLResultTypes.*; -import com.qualcomm.hardware.limelightvision.LLFieldMap; import java.util.concurrent.atomic.AtomicInteger; diff --git a/WilyCore/src/main/java/com/wilyworks/simulator/framework/MechSim.java b/WilyCore/src/main/java/com/wilyworks/simulator/framework/MechSim.java index bec03dd7b1f1..51f224f131ae 100644 --- a/WilyCore/src/main/java/com/wilyworks/simulator/framework/MechSim.java +++ b/WilyCore/src/main/java/com/wilyworks/simulator/framework/MechSim.java @@ -27,6 +27,7 @@ import java.awt.geom.Ellipse2D; import java.awt.geom.Rectangle2D; import java.util.ArrayList; +import java.util.Arrays; import java.util.Collections; import java.util.Iterator; import java.util.LinkedList; @@ -206,7 +207,7 @@ public double getVelocity() { // Simulation for the SlowBot in the 2025/2026 Decode game. class DecodeSlowBotMechSim extends MechSim { - enum BallColor {PURPLE, GREEN} + enum BallColor {PURPLE, GREEN, GOLD} final double WIDTH = 18; // Robot width final double HEIGHT = 18; // Robot height @@ -259,9 +260,9 @@ public Ball(BallColor color, double x, double y) { new Ball(BallColor.PURPLE, 59.5, 69.5) }; Ball[] ballPreloads = { - new Ball(BallColor.GREEN, 0, 0), - new Ball(BallColor.PURPLE, 0, 0), - new Ball(BallColor.PURPLE, 0, 0) + new Ball(BallColor.GOLD, 0, 0), + new Ball(BallColor.GOLD, 0, 0), + new Ball(BallColor.GOLD, 0, 0) }; // Hooked devices: @@ -279,7 +280,6 @@ public Ball(BallColor color, double x, double y) { // State: double accumulatedDeltaT; Ball intakeBall; // Ball in the intake, may be null - // List slotBalls = new ArrayList<>(Arrays.asList(ballPreloads)); // Collections.nCopies(3, null)); List slotBalls = new ArrayList<>(Collections.nCopies(3, null)); List airBalls = new LinkedList<>(); // Balls flying through the air List fieldBalls = new LinkedList<>(); // Pickable balls on the field @@ -289,6 +289,8 @@ public Ball(BallColor color, double x, double y) { double actualTransferPosition; // Current transfer servo position, [0, 1] double transferStartTime; // Time that a transfer started, zero when not transferring int colorSensorMask = -1; // Random 2-bit mask indicating which sensors return true data; -1 if reset + boolean hasIntaken = false; // True once any ball has been taken into the drum + boolean hasLaunched = false; // True once any ball has been launched // Initialize the beast. DecodeSlowBotMechSim() { @@ -305,8 +307,10 @@ private Color ballColor(Ball ball) { return Color.BLACK; else if (ball.color == BallColor.PURPLE) return new Color(128, 0, 128); - else + else if (ball.color == BallColor.GREEN) return Color.GREEN; + else + return new Color(255, 215, 0); // Gold } // WilyHardwareMap calls this method when it creates a device, allowing us to substitute @@ -557,6 +561,15 @@ void simulate(Pose2d pose, double deltaT) { if (backwardFeederServo.getPower() < FEEDER_POWER) { throw new RuntimeException("A transfer is requested when backward feeder servo isn't running. That won't work!"); } + if (slotBalls.get(transferSlot) == null) { + if (!hasIntaken && !hasLaunched) { + // A transfer has been requested there was no intake done. Assume that this + // is Auto with preloads and populate the drum with three golden balls. We + // make them gold balls so that we don't have to worry about getting the + // ordering correct. + slotBalls = new ArrayList<>(Arrays.asList(ballPreloads)); + } + } if (slotBalls.get(transferSlot) != null) { if (transferStartTime == 0) { transferStartTime = time(); @@ -580,6 +593,7 @@ void simulate(Pose2d pose, double deltaT) { airBalls.add(ball); slotBalls.set(transferSlot, null); transferStartTime = 0; + hasLaunched = true; } } } @@ -606,6 +620,7 @@ void simulate(Pose2d pose, double deltaT) { if (slotBalls.get(slot) == null) { slotBalls.set(slot, intakeBall); intakeBall = null; + hasIntaken = true; } } } diff --git a/WilyCore/src/main/java/org/json/JSONArray.java b/WilyCore/src/main/java/org/json/JSONArray.java deleted file mode 100644 index c71410ee5e7f..000000000000 --- a/WilyCore/src/main/java/org/json/JSONArray.java +++ /dev/null @@ -1,190 +0,0 @@ -package org.json; - -// -// Source code recreated from a .class file by IntelliJ IDEA -// (powered by FernFlower decompiler) -// - -import java.util.Collection; - -public class JSONArray { - public JSONArray() { - throw new RuntimeException("Stub!"); - } - - public JSONArray(Collection copyFrom) { - throw new RuntimeException("Stub!"); - } - -// public JSONArray(JSONTokener readFrom) throws JSONException { -// throw new RuntimeException("Stub!"); -// } - - public JSONArray(String json) throws JSONException { - throw new RuntimeException("Stub!"); - } - - public JSONArray(Object array) throws JSONException { - throw new RuntimeException("Stub!"); - } - - public int length() { - throw new RuntimeException("Stub!"); - } - - public JSONArray put(boolean value) { - throw new RuntimeException("Stub!"); - } - - public JSONArray put(double value) throws JSONException { - throw new RuntimeException("Stub!"); - } - - public JSONArray put(int value) { - throw new RuntimeException("Stub!"); - } - - public JSONArray put(long value) { - throw new RuntimeException("Stub!"); - } - - public JSONArray put(Object value) { - throw new RuntimeException("Stub!"); - } - - public JSONArray put(int index, boolean value) throws JSONException { - throw new RuntimeException("Stub!"); - } - - public JSONArray put(int index, double value) throws JSONException { - throw new RuntimeException("Stub!"); - } - - public JSONArray put(int index, int value) throws JSONException { - throw new RuntimeException("Stub!"); - } - - public JSONArray put(int index, long value) throws JSONException { - throw new RuntimeException("Stub!"); - } - - public JSONArray put(int index, Object value) throws JSONException { - throw new RuntimeException("Stub!"); - } - - public boolean isNull(int index) { - throw new RuntimeException("Stub!"); - } - - public Object get(int index) throws JSONException { - throw new RuntimeException("Stub!"); - } - - public Object opt(int index) { - throw new RuntimeException("Stub!"); - } - - public Object remove(int index) { - throw new RuntimeException("Stub!"); - } - - public boolean getBoolean(int index) throws JSONException { - throw new RuntimeException("Stub!"); - } - - public boolean optBoolean(int index) { - throw new RuntimeException("Stub!"); - } - - public boolean optBoolean(int index, boolean fallback) { - throw new RuntimeException("Stub!"); - } - - public double getDouble(int index) throws JSONException { - throw new RuntimeException("Stub!"); - } - - public double optDouble(int index) { - throw new RuntimeException("Stub!"); - } - - public double optDouble(int index, double fallback) { - throw new RuntimeException("Stub!"); - } - - public int getInt(int index) throws JSONException { - throw new RuntimeException("Stub!"); - } - - public int optInt(int index) { - throw new RuntimeException("Stub!"); - } - - public int optInt(int index, int fallback) { - throw new RuntimeException("Stub!"); - } - - public long getLong(int index) throws JSONException { - throw new RuntimeException("Stub!"); - } - - public long optLong(int index) { - throw new RuntimeException("Stub!"); - } - - public long optLong(int index, long fallback) { - throw new RuntimeException("Stub!"); - } - - public String getString(int index) throws JSONException { - throw new RuntimeException("Stub!"); - } - - public String optString(int index) { - throw new RuntimeException("Stub!"); - } - - public String optString(int index, String fallback) { - throw new RuntimeException("Stub!"); - } - - public JSONArray getJSONArray(int index) throws JSONException { - throw new RuntimeException("Stub!"); - } - - public JSONArray optJSONArray(int index) { - throw new RuntimeException("Stub!"); - } - - public JSONObject getJSONObject(int index) throws JSONException { - throw new RuntimeException("Stub!"); - } - - public JSONObject optJSONObject(int index) { - throw new RuntimeException("Stub!"); - } - - public JSONObject toJSONObject(JSONArray names) throws JSONException { - throw new RuntimeException("Stub!"); - } - - public String join(String separator) throws JSONException { - throw new RuntimeException("Stub!"); - } - - public String toString() { - throw new RuntimeException("Stub!"); - } - - public String toString(int indentSpaces) throws JSONException { - throw new RuntimeException("Stub!"); - } - - public boolean equals(Object o) { - throw new RuntimeException("Stub!"); - } - - public int hashCode() { - throw new RuntimeException("Stub!"); - } -} diff --git a/WilyCore/src/main/java/org/json/JSONException.java b/WilyCore/src/main/java/org/json/JSONException.java deleted file mode 100644 index 8831ae5063ce..000000000000 --- a/WilyCore/src/main/java/org/json/JSONException.java +++ /dev/null @@ -1,20 +0,0 @@ -package org.json; - -// -// Source code recreated from a .class file by IntelliJ IDEA -// (powered by FernFlower decompiler) -// - -public class JSONException extends Exception { - public JSONException(String s) { - throw new RuntimeException("Stub!"); - } - - public JSONException(String message, Throwable cause) { - throw new RuntimeException("Stub!"); - } - - public JSONException(Throwable cause) { - throw new RuntimeException("Stub!"); - } -} diff --git a/WilyCore/src/main/java/org/json/JSONObject.java b/WilyCore/src/main/java/org/json/JSONObject.java deleted file mode 100644 index e16c68fd98db..000000000000 --- a/WilyCore/src/main/java/org/json/JSONObject.java +++ /dev/null @@ -1,221 +0,0 @@ -package org.json; - -// -// Source code recreated from a .class file by IntelliJ IDEA -// (powered by FernFlower decompiler) -// - -import androidx.annotation.RecentlyNonNull; -import androidx.annotation.RecentlyNullable; -import java.util.Iterator; -import java.util.Map; - -public class JSONObject { - @RecentlyNonNull - public static final Object NULL = null; - - public JSONObject() { - throw new RuntimeException("Stub!"); - } - - public JSONObject(@RecentlyNonNull Map copyFrom) { - throw new RuntimeException("Stub!"); - } - -// public JSONObject(@RecentlyNonNull JSONTokener readFrom) throws JSONException { -// throw new RuntimeException("Stub!"); -// } - - public JSONObject(@RecentlyNonNull String json) throws JSONException { - throw new RuntimeException("Stub!"); - } - - public JSONObject(@RecentlyNonNull JSONObject copyFrom, @RecentlyNonNull String[] names) throws JSONException { - throw new RuntimeException("Stub!"); - } - - public int length() { - throw new RuntimeException("Stub!"); - } - - @RecentlyNonNull - public JSONObject put(@RecentlyNonNull String name, boolean value) throws JSONException { - throw new RuntimeException("Stub!"); - } - - @RecentlyNonNull - public JSONObject put(@RecentlyNonNull String name, double value) throws JSONException { - throw new RuntimeException("Stub!"); - } - - @RecentlyNonNull - public JSONObject put(@RecentlyNonNull String name, int value) throws JSONException { - throw new RuntimeException("Stub!"); - } - - @RecentlyNonNull - public JSONObject put(@RecentlyNonNull String name, long value) throws JSONException { - throw new RuntimeException("Stub!"); - } - - @RecentlyNonNull - public JSONObject put(@RecentlyNonNull String name, @RecentlyNullable Object value) throws JSONException { - throw new RuntimeException("Stub!"); - } - - @RecentlyNonNull - public JSONObject putOpt(@RecentlyNullable String name, @RecentlyNullable Object value) throws JSONException { - throw new RuntimeException("Stub!"); - } - - @RecentlyNonNull - public JSONObject accumulate(@RecentlyNonNull String name, @RecentlyNullable Object value) throws JSONException { - throw new RuntimeException("Stub!"); - } - - @RecentlyNullable - public Object remove(@RecentlyNullable String name) { - throw new RuntimeException("Stub!"); - } - - public boolean isNull(@RecentlyNullable String name) { - throw new RuntimeException("Stub!"); - } - - public boolean has(@RecentlyNullable String name) { - throw new RuntimeException("Stub!"); - } - - @RecentlyNonNull - public Object get(@RecentlyNonNull String name) throws JSONException { - throw new RuntimeException("Stub!"); - } - - @RecentlyNullable - public Object opt(@RecentlyNullable String name) { - throw new RuntimeException("Stub!"); - } - - public boolean getBoolean(@RecentlyNonNull String name) throws JSONException { - throw new RuntimeException("Stub!"); - } - - public boolean optBoolean(@RecentlyNullable String name) { - throw new RuntimeException("Stub!"); - } - - public boolean optBoolean(@RecentlyNullable String name, boolean fallback) { - throw new RuntimeException("Stub!"); - } - - public double getDouble(@RecentlyNonNull String name) throws JSONException { - throw new RuntimeException("Stub!"); - } - - public double optDouble(@RecentlyNullable String name) { - throw new RuntimeException("Stub!"); - } - - public double optDouble(@RecentlyNullable String name, double fallback) { - throw new RuntimeException("Stub!"); - } - - public int getInt(@RecentlyNonNull String name) throws JSONException { - throw new RuntimeException("Stub!"); - } - - public int optInt(@RecentlyNullable String name) { - throw new RuntimeException("Stub!"); - } - - public int optInt(@RecentlyNullable String name, int fallback) { - throw new RuntimeException("Stub!"); - } - - public long getLong(@RecentlyNonNull String name) throws JSONException { - throw new RuntimeException("Stub!"); - } - - public long optLong(@RecentlyNullable String name) { - throw new RuntimeException("Stub!"); - } - - public long optLong(@RecentlyNullable String name, long fallback) { - throw new RuntimeException("Stub!"); - } - - @RecentlyNonNull - public String getString(@RecentlyNonNull String name) throws JSONException { - throw new RuntimeException("Stub!"); - } - - @RecentlyNonNull - public String optString(@RecentlyNullable String name) { - throw new RuntimeException("Stub!"); - } - - @RecentlyNonNull - public String optString(@RecentlyNullable String name, @RecentlyNonNull String fallback) { - throw new RuntimeException("Stub!"); - } - - @RecentlyNonNull - public JSONArray getJSONArray(@RecentlyNonNull String name) throws JSONException { - throw new RuntimeException("Stub!"); - } - - @RecentlyNullable - public JSONArray optJSONArray(@RecentlyNullable String name) { - throw new RuntimeException("Stub!"); - } - - @RecentlyNonNull - public JSONObject getJSONObject(@RecentlyNonNull String name) throws JSONException { - throw new RuntimeException("Stub!"); - } - - @RecentlyNullable - public JSONObject optJSONObject(@RecentlyNullable String name) { - throw new RuntimeException("Stub!"); - } - - @RecentlyNullable - public JSONArray toJSONArray(@RecentlyNullable JSONArray names) throws JSONException { - throw new RuntimeException("Stub!"); - } - - @RecentlyNonNull - public Iterator keys() { - throw new RuntimeException("Stub!"); - } - - @RecentlyNullable - public JSONArray names() { - throw new RuntimeException("Stub!"); - } - - @RecentlyNonNull - public String toString() { - throw new RuntimeException("Stub!"); - } - - @RecentlyNonNull - public String toString(int indentSpaces) throws JSONException { - throw new RuntimeException("Stub!"); - } - - @RecentlyNonNull - public static String numberToString(@RecentlyNonNull Number number) throws JSONException { - throw new RuntimeException("Stub!"); - } - - @RecentlyNonNull - public static String quote(@RecentlyNullable String data) { - throw new RuntimeException("Stub!"); - } - - @RecentlyNullable - public static Object wrap(@RecentlyNullable Object o) { - throw new RuntimeException("Stub!"); - } -} diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/roadrunner/MecanumDrive.java b/team417/src/main/java/org/firstinspires/ftc/team417/roadrunner/MecanumDrive.java index 946d4a407a61..4c50ea7b23e4 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/roadrunner/MecanumDrive.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/roadrunner/MecanumDrive.java @@ -164,7 +164,7 @@ public static class Params { public double inPerTick; // Inches-per-tick for encoders (set to 1.0 if using Optical Tracking) public double lateralInPerTick; // Lateral inches-per-tick for encoders - public double trackWidthTicks; // Diameter of the circle that a wheel travels to turn the robot 360 degrees, in ticks + public double trackWidthTicks; // Radius of the circle that a wheel travels to turn the robot 360 degrees, in ticks public double kS; // Feed-forward voltage to overcome static friction public double kV; // Feed-forward voltage factor to achieve target velocity, in tick units @@ -950,6 +950,9 @@ static public Vector2d rotateVector(Vector2d vector, double theta) { // Override the current pose for Road Runner and the optical tracking sensor: public void setPose(Pose2d pose) { + // Tell Wily Works about the new pose: + WilyWorks.setStartPose(pose, new PoseVelocity2d(new Vector2d(0, 0), 0)); + // Set the Road Runner pose: this.pose = pose; this.targetPose = pose; diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/utils/ConfigurationTester.java b/team417/src/main/java/org/firstinspires/ftc/team417/utils/ConfigurationTester.java index 6e27b1a6390d..1444cd865b8d 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/utils/ConfigurationTester.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/utils/ConfigurationTester.java @@ -6,7 +6,6 @@ import android.graphics.Color; import com.qualcomm.hardware.gobilda.GoBildaPinpointDriver; -import com.qualcomm.hardware.limelightvision.LLResult; import com.qualcomm.hardware.limelightvision.LLStatus; import com.qualcomm.hardware.limelightvision.Limelight3A; import com.qualcomm.hardware.sparkfun.SparkFunOTOS; @@ -45,6 +44,71 @@ import java.util.function.Consumer; import java.util.regex.Pattern; +// Helpers for using HTML with the FTC Driver Station. +/** @noinspection unused*/ +class Html { + // Showing a less-than or greater-than sign requires special encodings when HTML is enabled: + public final static String LESS_THAN = "<"; // String to show a "<" + public final static String GREATER_THAN = ">"; // String to show a ">" + + // Enable the display on telemetry for HTML. + public static void initialize(Telemetry telemetry) { initialize(telemetry, false); } + public static void initialize(Telemetry telemetry, boolean monospace) { + telemetry.setDisplayFormat(Telemetry.DisplayFormat.HTML); + if (monospace) { + telemetry.addLine(""); + } + } + + // Repeat the string for the specified count. + private static String repeat(int count, String string) { + StringBuilder result = new StringBuilder(); + for (int i = 0; i < count; i++) { + result.append(string); + } + return result.toString(); + } + + // Set the foreground font color for a string. Color must be in the format" #dc3545". + public static String color(String color, String string) { + return "" + string + ""; + } + + // Set the background color for a string. Color must be in the format" #dc3545". + public static String background(String backgroundColor, String string) { + return "" + string + ""; + } + + // Set the foreground and background colors for a string. Colors must be in the format" #dc3545". + public static String colors(String foregroundColor, String backgroundColor, String string) { + return "" + string + ""; + } + + // Make a string big according to the specified count: 1.25^count times bigger. + public static String big(int count, String string) { + return repeat(count, "") + string + repeat(count, ""); + } + + // Make a string smaller according to the specified count: 0.8^count times smaller. + public static String small(int count, String string) { + return repeat(count, "") + string + repeat(count, ""); + } + + // Leading spaces on a line will be trimmed unless this is used: + public static String spaces(int count) { + return repeat(count / 4, " ") + repeat(count % 4, " "); + } + + // One-liners: + public static String bold(String string) { return "" + string + ""; } + public static String italic(String string) { return "" + string + ""; } + public static String monospace(String string) { return "" + string + ""; } + public static String underline(String string) { return "" + string + ""; } + public static String superscript(String string) { return "" + string + ""; } + public static String subscript(String string) { return "" + string + ""; } + public static String strikethrough(String string) { return "" + string + ""; } +} + @TeleOp(name="Configuration Tester", group="Utility") @SuppressLint("DefaultLocale") public class ConfigurationTester extends LinearOpMode { @@ -90,7 +154,7 @@ int menu(String header, int scrollLine, List options, int current, boole } for (int i = firstDisplayLine; i < options.size(); i++) { if (i == current) { - ui.line(htmlBackground(Ui.HIGHLIGHT_COLOR, + ui.line(Html.background(Ui.HIGHLIGHT_COLOR, "\u25c6 " + options.get(i))); // Solid circle } else { ui.line("\u25c7 " + options.get(i)); // Hollow circle @@ -154,7 +218,7 @@ public Test(Class klass, Consumer test) { // Time, in seconds: static double time() { - return nanoTime() * 1e-9; + return nanoTime() * 1e-9; } // Unusually, we do all our work before Start is pressed, rather than after. That's so that @@ -164,48 +228,14 @@ boolean isActive() { return !this.isStopRequested() && !isStarted(); } - // Set the foreground font color for a string. Color must be in the format" #dc3545". - static String htmlForeground(String color, String string, Object... args) { - return String.format("%s", color, String.format(string, args)); - } - - // Set the background color for a string. Color must be in the format" #dc3545". - static String htmlBackground(String backgroundColor, String string, Object... args) { - return String.format("%s", backgroundColor, String.format(string, args)); - } - - // Set the foreground and background colors for a string. Colors must be in the format" #dc3545". - /** @noinspection unused*/ - static String htmlColors(String foregroundColor, String backgroundColor, String string, Object... args) { - return String.format("%s", foregroundColor, backgroundColor, String.format(string, args)); - } - - // Make a string big according to the specified size: 1.5^size. - static String htmlBig(int size, String string, Object... args) { - StringBuilder result = new StringBuilder(); - for (int i = 0; i < size; i++) { - result.append(""); - } - result.append(String.format(string, args)); - for (int i = 0; i < size; i++) { - result.append(""); - } - return result.toString(); - } - - // Make a string bold. - static String htmlBold(String string, Object... args) { - return "" + String.format(string, args) + ""; - } - // Convert a string into a red error message. static String error(String string, Object... args) { - return htmlForeground("#DC3545", string, args); + return Html.color("#DC3545", String.format(string, args)); } // Style for displaying gamepad control names. static String buttonName(String button) { - return htmlBackground("#404040", button); + return Html.background("#404040", button); } String format(String format, Object... args) { @@ -258,10 +288,10 @@ boolean prompt(String text) { return false; } String gray = "#808080"; - ui.line(htmlBig(2, htmlBold("\"%s\"", testDescriptor.deviceName))); - ui.line(htmlForeground(gray, "Description: %s", testDescriptor.hardwareDevice.getDeviceName())); - ui.line(htmlForeground(gray, "Connection: %s", testDescriptor.hardwareDevice.getConnectionInfo())); - ui.line(htmlForeground(gray, "Loop I/O performance: %s", loopTimer.get())); + ui.line(Html.big(2,Html.bold("\"%s\"")), testDescriptor.deviceName); + ui.line(Html.color(gray, "Description: %s"), testDescriptor.hardwareDevice.getDeviceName()); + ui.line(Html.color(gray, "Connection: %s"), testDescriptor.hardwareDevice.getConnectionInfo()); + ui.line(Html.color(gray, "Loop I/O performance: %s"), loopTimer.get()); return true; } @@ -284,7 +314,7 @@ static class Ui { this.telemetry = telemetry; this.buffer = new StringBuilder(); // Enable our extensive use of HTML: - telemetry.setDisplayFormat(Telemetry.DisplayFormat.HTML); + Html.initialize(telemetry); // Change the update interval from 250ms to 50ms for a more responsive UI: telemetry.setMsTransmissionInterval(50); } @@ -352,8 +382,8 @@ public void runOpMode() { // Show a splash screen while we initialize: double splashTime = time(); - ui.line(htmlBig(5, htmlForeground(Ui.HIGHLIGHT_COLOR, htmlBold("Configuration Tester!")))); - ui.line(htmlBig(2, "By Swerve Robotics, Woodinville\n")); + ui.line(Html.big(5, Html.color(Ui.HIGHLIGHT_COLOR, Html.bold("Configuration Tester!")))); + ui.line(Html.big(2, "By Swerve Robotics, Woodinville\n")); ui.line("Initializing..."); ui.update(); @@ -411,7 +441,7 @@ public void runOpMode() { int selection = 0; while (isActive()) { String header = format("Here's your entire configuration. dpad to navigate, A to select. Tap %s to quit.", - htmlForeground("#05BD05", "\u25B6")); + Html.color("#05BD05", "\u25B6")); selection = menu(header + "\n", 6, options, selection, true); testDescriptor = testList.get(selection); @@ -435,9 +465,12 @@ void testIMU(HardwareDevice device) { IMU imu = (IMU) device; do { YawPitchRollAngles angles = imu.getRobotYawPitchRollAngles(); - ui.line("Yaw: " + htmlBig(2, "%.2f\u00b0", angles.getYaw(AngleUnit.DEGREES)) + - ", Pitch: " + htmlBig(2, "%.2f\u00b0", angles.getPitch(AngleUnit.DEGREES)) + - ", Roll: " + htmlBig(2, "%.2f\u00b0", angles.getRoll(AngleUnit.DEGREES))); + ui.line("Yaw: " + Html.big(2, "%.2f\u00b0") + + ", Pitch: " + Html.big(2, "%.2f\u00b0") + + ", Roll: " + Html.big(2, "%.2f\u00b0"), + angles.getYaw(AngleUnit.DEGREES), + angles.getPitch(AngleUnit.DEGREES), + angles.getRoll(AngleUnit.DEGREES)); } while (prompt()); } @@ -445,7 +478,7 @@ void testIMU(HardwareDevice device) { void testVoltage(HardwareDevice device) { VoltageSensor voltage = (VoltageSensor) device; do { - ui.line(htmlBig(3, "Voltage: %.2f", voltage.getVoltage())); + ui.line(Html.big(3, "Voltage: %.2f"), voltage.getVoltage()); } while (prompt()); } @@ -456,7 +489,7 @@ void testMotor(HardwareDevice device) { String encoderStatus = ""; motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); do { - ui.line(htmlBig(3, "Power: %.2f", power)); + ui.line(Html.big(3, "Power: %.2f"), power); power = stickValue(gamepad1.right_stick_y, power, -1, 1); if (gamepad1.xWasPressed()) power = 0; @@ -488,7 +521,7 @@ void testCRServo(HardwareDevice device) { CRServo crServo = (CRServo) device; double power = crServo.getPower(); do { - ui.line(htmlBig(3, "Power: %.2f", power)); + ui.line(Html.big(3, "Power: %.2f"), power); power = stickValue(gamepad1.right_stick_y, power, -1, 1); if (gamepad1.xWasPressed()) power = 0; @@ -502,7 +535,7 @@ void testServo(HardwareDevice device) { double position = servo.getPosition(); boolean enabled = false; do { - ui.line(htmlBig(3, "Position: %.2f", position)); + ui.line(Html.big(3, "Position: %.2f"), position); position = stickValue(gamepad1.right_stick_y, position, 0, 1); if (enabled) servo.setPosition(position); @@ -517,7 +550,7 @@ void testServo(HardwareDevice device) { void testDistance(HardwareDevice device) { DistanceSensor distance = (DistanceSensor) device; do { - ui.line(htmlBig(2, "Distance: %.2fcm", distance.getDistance(DistanceUnit.CM))); + ui.line(Html.big(2, "Distance: %.2fcm"), distance.getDistance(DistanceUnit.CM)); } while (prompt()); } @@ -530,10 +563,10 @@ void testDigitalChannel(HardwareDevice device) { String promptText; do { if (mode == DigitalChannel.Mode.INPUT) { - ui.line(htmlBig(2, "Input: %s", channel.getState())); + ui.line(Html.big(2, "Input: %s"), channel.getState()); promptText = "Y to switch output mode"; } else { - ui.line(htmlBig(2, "Output: %s", outputValue)); + ui.line(Html.big(2, "Output: %s"), outputValue); promptText = "A to toggle value, Y to switch output mode"; if (gamepad1.aWasPressed()) outputValue = !outputValue; @@ -565,7 +598,7 @@ void testAnalogInput(HardwareDevice device) { AnalogInput input = (AnalogInput) device; do { ui.line("Max voltage: %.2f", input.getMaxVoltage()); - ui.line(htmlBig(2, "Voltage: %.2f", input.getVoltage())); + ui.line(Html.big(2, "Voltage: %.2f"), input.getVoltage()); } while (prompt()); } @@ -586,17 +619,32 @@ void testOtos(HardwareDevice device) { SparkFunOTOS.Pose2D pose = otos.getPosition(); ui.line("x: %.2f\", y: %.2f\", heading: %.2f°", pose.x, pose.y, pose.h); - ui.line(htmlBig(2, "Status: %s", otos.selfTest() ? "Good" : "Bad")); + ui.line(Html.big(2, "Status: %s"), otos.selfTest() ? "Good" : "Bad"); } while (prompt()); } // Test the GoBilda Pinpoint odometry computer. void testPinpoint(HardwareDevice device) { GoBildaPinpointDriver pinpoint = (GoBildaPinpointDriver) device; + int xOr = 0; + int yOr = 0; do { + pinpoint.update(); + int xEncoder = pinpoint.getEncoderX(); + int yEncoder = pinpoint.getEncoderY(); + xOr |= xEncoder; + yOr |= yEncoder; + + ui.line("X encoder: %d, Y encoder: %d", xEncoder, yEncoder); + if (xOr == 0 || yOr == 0) { + ui.line(error("Turn both pod wheels manually to verify wiring. " + + "The encoder values shouldn't stay at zero.")); + } + int loopTime = pinpoint.getLoopTime(); double frequency = pinpoint.getFrequency(); ui.line("Loop time: %d, frequency: %.1f", loopTime, frequency); + // The GoBilda driver code says to contact tech support if the following // conditions are consistently seen: if ((loopTime < 500) || (loopTime > 1100)) { @@ -606,14 +654,11 @@ void testPinpoint(HardwareDevice device) { ui.line(error("Bad frequency, contact tech@gobilda.com")); } - pinpoint.update(); - ui.line("X encoder: %d, y encoder: %d", pinpoint.getEncoderX(), pinpoint.getEncoderY()); - GoBildaPinpointDriver.DeviceStatus status = pinpoint.getDeviceStatus(); if (status == GoBildaPinpointDriver.DeviceStatus.READY) - ui.line(htmlBig(2, "Status: Good")); + ui.line(Html.big(0, "Reported status: Good")); else if (status == GoBildaPinpointDriver.DeviceStatus.FAULT_BAD_READ) - ui.line(htmlBig(2, "Status: Ok") + "(bad read)"); + ui.line(Html.big(0, "Reported status: Ok") + "(bad read)"); else { String error = "Unknown error"; switch (status) { @@ -636,7 +681,7 @@ else if (status == GoBildaPinpointDriver.DeviceStatus.FAULT_BAD_READ) error = "IMU runaway"; break; } - ui.line(error("Error: " + error)); + ui.line(error("Status error: " + error)); } } while (prompt()); } @@ -654,7 +699,7 @@ void testNormalizedColorSensor(HardwareDevice device) { NormalizedRGBA rgba = sensor.getNormalizedColors(); Color.colorToHSV(rgba.toColor(), hsv); String color = String.format("#%06x", rgba.toColor() & 0xffffff); // Color in hex - ui.line("Color: %s", htmlBig(3, htmlForeground(color, "\u25a0"))); // Box + ui.line("Color: %s", Html.big(3, Html.color(color, "\u25a0"))); // Box ui.line("Normalized ARGB: (%.2f, %.2f, %.2f)", rgba.red, rgba.green, rgba.blue); ui.line("HSV: (%.2f, %.2f, %.2f)", hsv[0], hsv[1], hsv[2]); From 9b60d94ad6fb228d288c860a7d4bc8ba4abae72a Mon Sep 17 00:00:00 2001 From: Hankang Zhou <136044263+nobody-particular@users.noreply.github.com> Date: Tue, 2 Dec 2025 20:08:07 -0800 Subject: [PATCH 117/198] Coded LimelightDetector.detectRobotPose --- .../team417/apriltags/LimelightDetector.java | 40 +++++++++++++++++-- 1 file changed, 37 insertions(+), 3 deletions(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/apriltags/LimelightDetector.java b/team417/src/main/java/org/firstinspires/ftc/team417/apriltags/LimelightDetector.java index dd24c51655f6..e451973ccf1f 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/apriltags/LimelightDetector.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/apriltags/LimelightDetector.java @@ -39,6 +39,8 @@ are permitted (subject to the limitations in the disclaimer below) provided that import com.qualcomm.robotcore.hardware.HardwareMap; import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.Pose2D; import org.firstinspires.ftc.robotcore.external.navigation.Pose3D; import org.firstinspires.ftc.team417.CompetitionAuto; @@ -219,7 +221,7 @@ public Pattern detectPattern(CompetitionAuto.Alliance alliance, LLResult result) && detection.getFiducialId() != 23 ); - // FiducialResult objects contain the x (left) and y (up) degrees relative to the robot + // FiducialResult objects contain the x (left) and y (up) degrees relative to the robot. // When we're on the red alliance, we want the leftmost valid // AprilTag, and when we're on the blue alliance, we want the rightmost valid AprilTag. // This is because, in our near position, we see two AprilTags on the obelisk: the front @@ -232,13 +234,13 @@ public Pattern detectPattern(CompetitionAuto.Alliance alliance, LLResult result) // Set detection to the leftmost (min x degrees) detection relative to the robot // If there are no detections, set it to null detection = currentDetections.stream() - .min(Comparator.comparingDouble(aprilTagDetection -> aprilTagDetection.getTargetXDegrees())).orElse(null); + .min(Comparator.comparingDouble(LLResultTypes.FiducialResult::getTargetXDegrees)).orElse(null); break; case BLUE: // Set detection to the rightmost (max x degrees) detection relative to the robot // If there are no detections, set it to null detection = currentDetections.stream() - .max(Comparator.comparingDouble(aprilTagDetection -> aprilTagDetection.getTargetXDegrees())).orElse(null); + .max(Comparator.comparingDouble(LLResultTypes.FiducialResult::getTargetXDegrees)).orElse(null); } if (detection == null) { @@ -268,6 +270,38 @@ public Pattern detectPattern(CompetitionAuto.Alliance alliance, LLResult result) } } + /** + * Detect the pose of the robot with the AprilTag. + */ + public Pose2D detectRobotPose() { + LLResult result = limelight.getLatestResult(); + + if (result.isValid()) { + List currentDetections = result.getFiducialResults(); + + // FiducialResult objects contain the x (left) and y (up) degrees relative to the robot. + // We want the AprilTag that is as straight on as possible, + // that is, the lowest absolute value x. + // For more information about the info the AprilTagDetection object contains, see this link: + // https://ftc-docs.firstinspires.org/en/latest/apriltag/understanding_apriltag_detection_values/understanding-apriltag-detection-values.html + + LLResultTypes.FiducialResult detection = currentDetections.stream() + .min(Comparator.comparingDouble(aprilTagDetection -> + Math.abs(aprilTagDetection.getTargetXDegrees()))).orElse(null); + + Pose3D pose = detection.getRobotPoseFieldSpace(); + + return new Pose2D( + pose.getPosition().unit, + pose.getPosition().x, + pose.getPosition().y, + AngleUnit.RADIANS, + pose.getOrientation().getYaw(AngleUnit.RADIANS)); + } + + return null; + }; + /** * Release the resources taken up by the vision portal. */ From 603ffbe0cb717aea75d2cff425cf78ca7a26cfee Mon Sep 17 00:00:00 2001 From: Hankang Zhou <136044263+nobody-particular@users.noreply.github.com> Date: Tue, 2 Dec 2025 20:14:42 -0800 Subject: [PATCH 118/198] Removed extraneous code --- .../ftc/team417/CompetitionAuto.java | 90 ------------------- 1 file changed, 90 deletions(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java index 843f9b8048aa..0183bfdd0df5 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java @@ -220,20 +220,6 @@ public void runOpMode() { double waitTime = menu.getResult(Double.class, "wait-slider-1"); double intakeCycles = menu.getResult(Double.class, "intake-slider"); - PathFactory pathFactory; - - switch (chosenAlliance) { - case RED: - pathFactory = new PathFactory(drive, false); - break; - case BLUE: - pathFactory = new PathFactory(drive, true); - break; - default: - throw new IllegalArgumentException("Alliance must be red or blue"); - } - - // the first parameter is the type to return as @@ -241,9 +227,7 @@ public void runOpMode() { switch (chosenMovement) { case NEAR: - drive.setPose(SBNearStartPose); - break; case FAR: drive.setPose(SBFarStartPose); @@ -315,80 +299,6 @@ public void runOpMode() { } } -class PathFactory { - MecanumDrive drive; - TrajectoryActionBuilder builder; - boolean mirror; - - public PathFactory(MecanumDrive drive, boolean mirror) { - this.drive = drive; - this.mirror = mirror; - } - - Pose2d mirrorPose(Pose2d pose) { - return new Pose2d(pose.position.x, -pose.position.y, -pose.heading.log()); - } - Vector2d mirrorVector(Vector2d vector) { - return new Vector2d(vector.x,-vector.y); - } - - public PathFactory actionBuilder(Pose2d pose) { - if (mirror) { - builder = drive.actionBuilder(mirrorPose(pose)); - } else { - builder = drive.actionBuilder(pose); - } - return this; - } - - public PathFactory setTangent(double tangent) { - if (mirror) { - builder = builder.setTangent(-tangent); - } else { - builder = builder.setTangent(tangent); - } - return this; - } - - public PathFactory splineToLinearHeading(Pose2d pose, double tangent) { - if (mirror) { - builder = builder.splineToLinearHeading(mirrorPose(pose), -tangent); - } else { - builder = builder.splineToLinearHeading(pose, tangent); - } - return this; - } - public PathFactory splineToSplineHeading(Pose2d pose, double tangent) { - if (mirror) { - builder = builder.splineToSplineHeading(mirrorPose(pose), -tangent); - } else { - builder = builder.splineToSplineHeading(pose, tangent); - } - return this; - } - public PathFactory splineToConstantHeading(Vector2d vector, double tangent) { - if(mirror) { - builder = builder.splineToConstantHeading(mirrorVector(vector), -tangent); - } else { - builder = builder.splineToConstantHeading(vector, tangent); - - } - return this; - } - - - public PathFactory stopAndAdd(Action a) { - builder = builder.stopAndAdd(a); - return this; - } - - public Action build() { - return builder.build(); - } - - - -} class LaunchAction extends RobotAction { @Override public boolean run(double elapsedTime) { From 00788f782700a2575ee68a61a40660bf105172be Mon Sep 17 00:00:00 2001 From: Team <417skid@swerverobotics.club> Date: Tue, 2 Dec 2025 20:16:14 -0800 Subject: [PATCH 119/198] intake in auto --- .../org/firstinspires/ftc/team417/CompetitionAuto.java | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java index b03a3554b119..42094685a426 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java @@ -76,10 +76,10 @@ public Action getPath(SlowBotMovement chosenMovement, Alliance chosenAlliance, d .stopAndAdd(new LaunchAction(mechGlob, pattern, countBalls)) .setTangent(Math.toRadians(0)) .splineToSplineHeading(new Pose2d(-12, 32, Math.toRadians(90)), Math.toRadians(90)) //go to intake closest from goal - .stopAndAdd(new IntakeAction(mechGlob, 1)) + .afterDisp(0, new IntakeAction(mechGlob, 1)) .setTangent(Math.toRadians(90)) - .splineToConstantHeading(new Vector2d(-12, 55), Math.toRadians(90)) - .stopAndAdd(new IntakeAction(mechGlob, 0)) + .splineToConstantHeading(new Vector2d(-12, 59), Math.toRadians(90)) + .afterDisp(0, new IntakeAction(mechGlob, 0)) .setTangent(Math.toRadians(-90)) .splineToSplineHeading(new Pose2d(-36, 36, Math.toRadians(139)), Math.toRadians(180)) //go to launch position .stopAndAdd(new LaunchAction(mechGlob, pattern, countBalls)); @@ -350,8 +350,9 @@ public IntakeAction(MechGlob mechGlob, double intakeSpeed) { @Override public boolean run(double elapsedTime) { + mechGlob.intake(intakeSpeed); - return false; + return elapsedTime < 3; } } class CountBalls { From e69a55945cbf0b54a904830b3f715b73b2089632 Mon Sep 17 00:00:00 2001 From: Hankang Zhou <136044263+nobody-particular@users.noreply.github.com> Date: Tue, 2 Dec 2025 20:46:38 -0800 Subject: [PATCH 120/198] Conceptually implemented BaseOpMode.tryResetRobotPose. --- .../firstinspires/ftc/team417/BaseOpMode.java | 21 ++ .../ftc/team417/CompetitionAuto.java | 212 +++++++++--------- .../ftc/team417/CompetitionTeleOp.java | 15 +- .../ftc/team417/ComplexMechGlob.java | 9 +- .../team417/apriltags/LimelightDetector.java | 37 +-- 5 files changed, 162 insertions(+), 132 deletions(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java b/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java index f874ba6b0a23..92530aca17f3 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java @@ -1,14 +1,35 @@ package org.firstinspires.ftc.team417; +import com.acmerobotics.roadrunner.Pose2d; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import org.firstinspires.ftc.team417.apriltags.LimelightDetector; +import org.firstinspires.ftc.team417.roadrunner.MecanumDrive; + /** * This class contains all of the base logic that is shared between all of the TeleOp and * Autonomous logic. All TeleOp and Autonomous classes should derive from this class. */ abstract public class BaseOpMode extends LinearOpMode { + // Resets the robot pose only if the robot is not moving + public static void tryResetRobotPose(LimelightDetector detector, MecanumDrive drive) { + if (isZero(drive.rightBack.getVelocity()) + && isZero(drive.rightFront.getVelocity()) + && isZero(drive.leftBack.getVelocity()) + && isZero(drive.leftFront.getVelocity())) { + + Pose2d pose = detector.detectRobotPose(); + if (pose != null) { + drive.setPose(pose); + } + } + } + // Sees if a number is within one one-thousandths of zero + private static boolean isZero(double x) { + return Math.abs(x) < 0.001; + } } diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java index 0183bfdd0df5..1f5f92987ad5 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java @@ -44,18 +44,21 @@ enum SlowBotMovement { double minIntakes = 0.0; double maxIntakes = 3.0; TextMenu menu = new TextMenu(); + LimelightDetector detector = new LimelightDetector(hardwareMap); MenuInput menuInput = new MenuInput(MenuInput.InputType.CONTROLLER); Pattern pattern; Alliance chosenAlliance; SlowBotMovement chosenMovement; double intakeCycles; + MecanumDrive drive; + public Action getPath(SlowBotMovement chosenMovement, Alliance chosenAlliance, double intakeCycles) { Pose2d startPose = new Pose2d(0, 0, 0); Pose2d SBNearStartPose = new Pose2d(-60, 48, Math.toRadians(139)); Pose2d SBFarStartPose = new Pose2d(60, 12, Math.toRadians(157.5)); - MecanumDrive drive = new MecanumDrive(hardwareMap, telemetry, gamepad1, startPose); + drive = new MecanumDrive(hardwareMap, telemetry, gamepad1, startPose); PoseMap poseMap = pose -> new Pose2dDual<>( @@ -72,15 +75,15 @@ public Action getPath(SlowBotMovement chosenMovement, Alliance chosenAlliance, d switch (chosenMovement) { case NEAR: trajectoryAction.setTangent(Math.toRadians(-51)) - .splineToConstantHeading(new Vector2d(-36,36), Math.toRadians(-51)) - //3 launches - //after disp intake - .setTangent(Math.toRadians(0)) - .splineToSplineHeading(new Pose2d(-12,32, Math.toRadians(90)), Math.toRadians(90)) //go to intake closest from goal - .setTangent(Math.toRadians(90)) - .splineToConstantHeading(new Vector2d(-12,55), Math.toRadians(90)) - .setTangent(Math.toRadians(-90)) - .splineToSplineHeading(new Pose2d(-36,36, Math.toRadians(139)), Math.toRadians(180)); //go to launch position + .splineToConstantHeading(new Vector2d(-36, 36), Math.toRadians(-51)) + //3 launches + //after disp intake + .setTangent(Math.toRadians(0)) + .splineToSplineHeading(new Pose2d(-12, 32, Math.toRadians(90)), Math.toRadians(90)) //go to intake closest from goal + .setTangent(Math.toRadians(90)) + .splineToConstantHeading(new Vector2d(-12, 55), Math.toRadians(90)) + .setTangent(Math.toRadians(-90)) + .splineToSplineHeading(new Pose2d(-36, 36, Math.toRadians(139)), Math.toRadians(180)); //go to launch position if (intakeCycles > 1) { trajectoryAction = trajectoryAction.setTangent(Math.toRadians(0)) @@ -114,11 +117,11 @@ public Action getPath(SlowBotMovement chosenMovement, Alliance chosenAlliance, d } - trajectoryAction = trajectoryAction.splineToSplineHeading(new Pose2d(36,32, Math.toRadians(90)), Math.toRadians(90)) //go to intake farthest from goal + trajectoryAction = trajectoryAction.splineToSplineHeading(new Pose2d(36, 32, Math.toRadians(90)), Math.toRadians(90)) //go to intake farthest from goal .setTangent(Math.toRadians(90)) - .splineToConstantHeading(new Vector2d(36,60), Math.toRadians(90)) + .splineToConstantHeading(new Vector2d(36, 60), Math.toRadians(90)) .setTangent(Math.toRadians(-90)) - .splineToSplineHeading(new Pose2d(54,12, Math.toRadians(157.5)), Math.toRadians(-90)); //go to launch position + .splineToSplineHeading(new Pose2d(54, 12, Math.toRadians(157.5)), Math.toRadians(-90)); //go to launch position if (intakeCycles > 1) { @@ -134,11 +137,11 @@ public Action getPath(SlowBotMovement chosenMovement, Alliance chosenAlliance, d //after disp intake action if (intakeCycles > 2) { trajectoryAction = trajectoryAction.setTangent(Math.toRadians(180)) - .splineToSplineHeading(new Pose2d(-12,32, Math.toRadians(90)), Math.toRadians(90)) //go to intake closest to goal + .splineToSplineHeading(new Pose2d(-12, 32, Math.toRadians(90)), Math.toRadians(90)) //go to intake closest to goal .setTangent(Math.toRadians(90)) - .splineToConstantHeading(new Vector2d(-12,55), Math.toRadians(90)) + .splineToConstantHeading(new Vector2d(-12, 55), Math.toRadians(90)) .setTangent(Math.toRadians(-90)) - .splineToSplineHeading(new Pose2d(54,12, Math.toRadians(157.5)), Math.toRadians(-90)); //go to launch position + .splineToSplineHeading(new Pose2d(54, 12, Math.toRadians(157.5)), Math.toRadians(-90)); //go to launch position } } @@ -147,32 +150,28 @@ public Action getPath(SlowBotMovement chosenMovement, Alliance chosenAlliance, d // 3 launch actions // after disp intake action trajectoryAction.setTangent(Math.toRadians(180)) - .splineToLinearHeading(new Pose2d(60,61, Math.toRadians(0)), Math.toRadians(0)) - .setTangent(Math.toRadians(-90)) - .splineToLinearHeading(new Pose2d(54,12, Math.toRadians(157.5)), Math.toRadians(-90)) - // 3 launch actions - .setTangent(Math.toRadians(90)) - .splineToLinearHeading(new Pose2d(50,32,Math.toRadians(180)), Math.toRadians(180)); + .splineToLinearHeading(new Pose2d(60, 61, Math.toRadians(0)), Math.toRadians(0)) + .setTangent(Math.toRadians(-90)) + .splineToLinearHeading(new Pose2d(54, 12, Math.toRadians(157.5)), Math.toRadians(-90)) + // 3 launch actions + .setTangent(Math.toRadians(90)) + .splineToLinearHeading(new Pose2d(50, 32, Math.toRadians(180)), Math.toRadians(180)); break; case FAR_MINIMAL: trajectoryAction.setTangent(Math.toRadians(90)) - .splineToLinearHeading(new Pose2d(48,32,Math.toRadians(180)), Math.toRadians(180)) - .build(); + .splineToLinearHeading(new Pose2d(48, 32, Math.toRadians(180)), Math.toRadians(180)) + .build(); break; } return trajectoryAction.build(); - - - } + @Override public void runOpMode() { - - Pose2d startPose = new Pose2d(0, 0, 0); @@ -181,26 +180,25 @@ public void runOpMode() { MecanumDrive drive = new MecanumDrive(hardwareMap, telemetry, gamepad1, startPose); - // Text menu for FastBot - // Text menu for SlowBot - menu.add(new MenuHeader("AUTO SETUP")) - .add() // empty line for spacing - .add("Pick an alliance:") - .add("alliance-picker-1", Alliance.class) // enum selector shortcut - .add() - .add("Pick a movement:") - .add("movement-picker-1", SlowBotMovement.class) // enum selector shortcut - .add() - .add("Intake Cycles:") - .add("intake-slider", new MenuSlider(minIntakes, maxIntakes)) - .add() - .add("Wait time:") - .add("wait-slider-1", new MenuSlider(minWaitTime, maxWaitTime)) - .add() - .add("finish-button-1", new MenuFinishedButton()); + // Text menu for SlowBot + menu.add(new MenuHeader("AUTO SETUP")) + .add() // empty line for spacing + .add("Pick an alliance:") + .add("alliance-picker-1", Alliance.class) // enum selector shortcut + .add() + .add("Pick a movement:") + .add("movement-picker-1", SlowBotMovement.class) // enum selector shortcut + .add() + .add("Intake Cycles:") + .add("intake-slider", new MenuSlider(minIntakes, maxIntakes)) + .add() + .add("Wait time:") + .add("wait-slider-1", new MenuSlider(minWaitTime, maxWaitTime)) + .add() + .add("finish-button-1", new MenuFinishedButton()); while (!menu.isCompleted() && !isStopRequested()) { @@ -223,85 +221,89 @@ public void runOpMode() { // the first parameter is the type to return as - Action trajectoryAction = null; - - switch (chosenMovement) { - case NEAR: - drive.setPose(SBNearStartPose); - break; - case FAR: - drive.setPose(SBFarStartPose); - break; - case FAR_OUT_OF_WAY: - drive.setPose(SBFarStartPose); - break; - case FAR_MINIMAL: - drive.setPose(SBFarStartPose); - break; - } - trajectoryAction = getPath(chosenMovement, chosenAlliance, intakeCycles); + Action trajectoryAction = null; - // Get a preview of the trajectory's path: - Canvas previewCanvas = new Canvas(); - trajectoryAction.preview(previewCanvas); + switch (chosenMovement) { + case NEAR: + drive.setPose(SBNearStartPose); + break; + case FAR: + drive.setPose(SBFarStartPose); + break; + case FAR_OUT_OF_WAY: + drive.setPose(SBFarStartPose); + break; + case FAR_MINIMAL: + drive.setPose(SBFarStartPose); + break; + } + trajectoryAction = getPath(chosenMovement, chosenAlliance, intakeCycles); - // Show the preview on FTC Dashboard now. - TelemetryPacket packet = MecanumDrive.getTelemetryPacket(); - packet.fieldOverlay().getOperations().addAll(previewCanvas.getOperations()); - MecanumDrive.sendTelemetryPacket(packet); + // Get a preview of the trajectory's path: + Canvas previewCanvas = new Canvas(); + trajectoryAction.preview(previewCanvas); + // Show the preview on FTC Dashboard now. + TelemetryPacket packet = MecanumDrive.getTelemetryPacket(); + packet.fieldOverlay().getOperations().addAll(previewCanvas.getOperations()); + MecanumDrive.sendTelemetryPacket(packet); - // Assume unknown pattern unless detected otherwise. - pattern = Pattern.UNKNOWN; - // Detect the pattern with the AprilTags from the camera! - // Wait for Start to be pressed on the Driver Hub! - // (This try-with-resources statement automatically calls detector.close() when it exits - // the try-block.) - try (LimelightDetector detector = new LimelightDetector(hardwareMap)) { + // Assume unknown pattern unless detected otherwise. + pattern = Pattern.UNKNOWN; - while (opModeInInit()) { - Pattern last = detector.detectPatternAndTelemeter(chosenAlliance, telemetry); - if (last != Pattern.UNKNOWN) { - pattern = last; - } + // Detect the pattern with the AprilTags from the camera! + // Wait for Start to be pressed on the Driver Hub! + while (opModeInInit()) { + Pattern last = detector.detectPatternAndTelemeter(chosenAlliance, telemetry); + if (last != Pattern.UNKNOWN) { + pattern = last; + } - telemetry.addData("Chosen alliance: ", chosenAlliance); - telemetry.addData("Chosen movement: ", chosenMovement); - telemetry.addData("Chosen wait time: ", waitTime); + telemetry.addData("Chosen alliance: ", chosenAlliance); + telemetry.addData("Chosen movement: ", chosenMovement); + telemetry.addData("Chosen wait time: ", waitTime); - telemetry.update(); + telemetry.update(); - if (isStopRequested()) { - break; - } - } + if (isStopRequested()) { + break; } + } - sleep((long)waitTime*1000); - boolean more = true; - while (opModeIsActive() && more) { - telemetry.addLine("Running Auto!"); - // 'packet' is the object used to send data to FTC Dashboard: - packet = MecanumDrive.getTelemetryPacket(); + sleep((long) waitTime * 1000); + boolean more = true; + while (opModeIsActive() && more) { + telemetry.addLine("Running Auto!"); - // Draw the preview and then run the next step of the trajectory on top: - packet.fieldOverlay().getOperations().addAll(previewCanvas.getOperations()); - more = trajectoryAction.run(packet); + // 'packet' is the object used to send data to FTC Dashboard: + packet = MecanumDrive.getTelemetryPacket(); - // Only send the packet if there's more to do in order to keep the very last - // drawing up on the field once the robot is done: - if (more) - MecanumDrive.sendTelemetryPacket(packet); - telemetry.update(); - } + // Draw the preview and then run the next step of the trajectory on top: + packet.fieldOverlay().getOperations().addAll(previewCanvas.getOperations()); + more = trajectoryAction.run(packet); + + // Only send the packet if there's more to do in order to keep the very last + // drawing up on the field once the robot is done: + if (more) + MecanumDrive.sendTelemetryPacket(packet); + telemetry.update(); } } +} class LaunchAction extends RobotAction { + CompetitionAuto opMode; + + // Pass `this` into here + public LaunchAction(CompetitionAuto opMode) { + this.opMode = opMode; + } + @Override public boolean run(double elapsedTime) { + BaseOpMode.tryResetRobotPose(opMode.detector, opMode.drive); // Resets the robot pose only if the robot is not moving return false; } } diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java index e0b6d966a942..a3a451514e36 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java @@ -6,10 +6,8 @@ import com.acmerobotics.roadrunner.PoseVelocity2d; import com.acmerobotics.roadrunner.Vector2d; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; -import com.qualcomm.robotcore.hardware.HardwareMap; -import com.qualcomm.robotcore.util.ElapsedTime; -import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.team417.apriltags.LimelightDetector; import org.firstinspires.ftc.team417.roadrunner.Drawing; import org.firstinspires.ftc.team417.roadrunner.MecanumDrive; @@ -38,10 +36,13 @@ public class CompetitionTeleOp extends BaseOpMode { * functions and autonomous routines in a way that avoids loops within loops, and "waits". */ + LimelightDetector detector = new LimelightDetector(hardwareMap); + MecanumDrive drive; + @Override public void runOpMode() { Pose2d beginPose = new Pose2d(0, 0, 0); - MecanumDrive drive = new MecanumDrive(hardwareMap, telemetry, gamepad1, beginPose); + drive = new MecanumDrive(hardwareMap, telemetry, gamepad1, beginPose); MechGlob mechGlob = ComplexMechGlob.create(hardwareMap, telemetry, false); // Initialize motors, servos, LEDs @@ -86,11 +87,11 @@ public void runOpMode() { //add slowbot teleop controls here if (gamepad2.yWasPressed()) { - mechGlob.launch(RequestedColor.EITHER); + mechGlob.launch(RequestedColor.EITHER, this); } else if (gamepad2.bWasPressed()) { - mechGlob.launch(RequestedColor.PURPLE); + mechGlob.launch(RequestedColor.PURPLE, this); } else if (gamepad2.aWasPressed()) { - mechGlob.launch(RequestedColor.GREEN); + mechGlob.launch(RequestedColor.GREEN, this); } if (gamepad2.dpadUpWasPressed()) { mechGlob.setLaunchVelocity(LaunchDistance.FAR); diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java b/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java index d79a19469fab..5c80c5ef875d 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java @@ -18,9 +18,7 @@ import org.firstinspires.ftc.team417.roadrunner.MecanumDrive; import java.util.ArrayList; -import java.util.Arrays; import java.util.Collections; -import java.util.List; enum RequestedColor { //an enum for different color cases for launching PURPLE, @@ -55,7 +53,9 @@ static MechGlob create (HardwareMap hardwareMap, Telemetry telemetry, boolean ru void intake (double intakeValue){} //a method that determines what color to launch. Options are purple, green, or either. - void launch (RequestedColor requestedColor) {} + void launch (RequestedColor requestedColor, CompetitionTeleOp opMode) { + BaseOpMode.tryResetRobotPose(opMode.detector, opMode.drive); + } void update () {} @@ -221,13 +221,14 @@ void intake (double intakeSpeed) { @Override //a class that controls the launcher and transfer - void launch (RequestedColor requestedColor) { + void launch (RequestedColor requestedColor, CompetitionTeleOp opMode) { int minSlot = findNearestSlot(LAUNCH_POSITIONS, requestedColor); if (minSlot != -1){ addToDrumQueue(LAUNCH_POSITIONS[minSlot], WaitState.SPIN_UP); slotOccupiedBy.set (minSlot, PixelColor.NONE); //marking this slot as empty so we don't accidentally try to use it again } + BaseOpMode.tryResetRobotPose(opMode.detector, opMode.drive); // Resets the robot pose only if the robot is not moving } //this function adds a new drum request to the drum queue. nextState is the state do use after the drum is finished moving void addToDrumQueue(double position, WaitState nextState){ diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/apriltags/LimelightDetector.java b/team417/src/main/java/org/firstinspires/ftc/team417/apriltags/LimelightDetector.java index e451973ccf1f..13805b518408 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/apriltags/LimelightDetector.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/apriltags/LimelightDetector.java @@ -32,6 +32,9 @@ are permitted (subject to the limitations in the disclaimer below) provided that */ package org.firstinspires.ftc.team417.apriltags; +import android.annotation.SuppressLint; + +import com.acmerobotics.roadrunner.Pose2d; import com.qualcomm.hardware.limelightvision.LLResult; import com.qualcomm.hardware.limelightvision.LLResultTypes; import com.qualcomm.hardware.limelightvision.LLStatus; @@ -40,7 +43,6 @@ are permitted (subject to the limitations in the disclaimer below) provided that import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; -import org.firstinspires.ftc.robotcore.external.navigation.Pose2D; import org.firstinspires.ftc.robotcore.external.navigation.Pose3D; import org.firstinspires.ftc.team417.CompetitionAuto; @@ -49,22 +51,22 @@ are permitted (subject to the limitations in the disclaimer below) provided that import java.util.Comparator; import java.util.List; -/* +/** * This class is used to detect AprilTags using the Limelight3A Vision Sensor. * * @see Limelight - * + *

* Notes on configuration: - * + *

* The device presents itself, when plugged into a USB port on a Control Hub as an ethernet * interface. A DHCP server running on the Limelight automatically assigns the Control Hub an * ip address for the new ethernet interface. - * + *

* Since the Limelight is plugged into a USB port, it will be listed on the top level configuration * activity along with the Control Hub Portal and other USB devices such as webcams. Typically * serial numbers are displayed below the device's names. In the case of the Limelight device, the * Control Hub's assigned ip address for that ethernet interface is used as the "serial number". - * + *

* Tapping the Limelight's name, transitions to a new screen where the user can rename the Limelight * and specify the Limelight's ip address. Users should take care not to confuse the ip address of * the Limelight itself, which can be configured through the Limelight settings page via a web browser, @@ -75,7 +77,7 @@ public class LimelightDetector implements Closeable { /** * The variable to store our instance of the AprilTag processor. */ - private Limelight3A limelight; + private final Limelight3A limelight; /** * The variable for how long ago the detection last changed. @@ -110,6 +112,7 @@ public Pattern detectPatternAndTelemeter(CompetitionAuto.Alliance alliance, Tele /** * Add telemetry about AprilTag detections. */ + @SuppressLint("DefaultLocale") public Pattern detectPatternAndTelemeter(CompetitionAuto.Alliance alliance, Telemetry telemetry, boolean verbose) { LLResult result = limelight.getLatestResult(); @@ -124,7 +127,7 @@ public Pattern detectPatternAndTelemeter(CompetitionAuto.Alliance alliance, Tele if (result.isValid()) { // Access general information - Pose3D botpose = result.getBotpose(); + Pose3D botPose = result.getBotpose(); double captureLatency = result.getCaptureLatency(); double targetingLatency = result.getTargetingLatency(); double parseLatency = result.getParseLatency(); @@ -137,7 +140,7 @@ public Pattern detectPatternAndTelemeter(CompetitionAuto.Alliance alliance, Tele telemetry.addData("ty", result.getTy()); telemetry.addData("tync", result.getTyNC()); - telemetry.addData("Botpose", botpose.toString()); + telemetry.addData("Botpose", botPose.toString()); // Access fiducial results @@ -160,7 +163,7 @@ public Pattern detectPatternAndTelemeter(CompetitionAuto.Alliance alliance, Tele // The `\\u...` are escape sequences for green and purple circle emojis. // \uD83D\uDFE3 -> Purple circle // \uD83D\uDFE2 -> Green circle - // \u26AA -> White circle + // ⚪ -> White circle switch (pattern) { case PPG: patternDisplay = "\uD83D\uDFE3\uD83D\uDFE3\uD83D\uDFE2"; @@ -172,7 +175,7 @@ public Pattern detectPatternAndTelemeter(CompetitionAuto.Alliance alliance, Tele patternDisplay = "\uD83D\uDFE2\uD83D\uDFE3\uD83D\uDFE3"; break; default: - patternDisplay = "\u26AA\u26AA\u26AA"; + patternDisplay = "⚪⚪⚪"; break; } @@ -273,7 +276,7 @@ public Pattern detectPattern(CompetitionAuto.Alliance alliance, LLResult result) /** * Detect the pose of the robot with the AprilTag. */ - public Pose2D detectRobotPose() { + public Pose2d detectRobotPose() { LLResult result = limelight.getLatestResult(); if (result.isValid()) { @@ -289,18 +292,20 @@ public Pose2D detectRobotPose() { .min(Comparator.comparingDouble(aprilTagDetection -> Math.abs(aprilTagDetection.getTargetXDegrees()))).orElse(null); + if (detection == null) { + return null; + } + Pose3D pose = detection.getRobotPoseFieldSpace(); - return new Pose2D( - pose.getPosition().unit, + return new Pose2d( pose.getPosition().x, pose.getPosition().y, - AngleUnit.RADIANS, pose.getOrientation().getYaw(AngleUnit.RADIANS)); } return null; - }; + } /** * Release the resources taken up by the vision portal. From 9fb3e963aa8eb1838d416bcab8cac50b9625348f Mon Sep 17 00:00:00 2001 From: Andrew Date: Wed, 3 Dec 2025 15:29:09 -0800 Subject: [PATCH 121/198] Update Wily Works to latest. --- WilyCore/src/main/java/com/wilyworks/simulator/WilyCore.java | 1 + 1 file changed, 1 insertion(+) diff --git a/WilyCore/src/main/java/com/wilyworks/simulator/WilyCore.java b/WilyCore/src/main/java/com/wilyworks/simulator/WilyCore.java index 0f9027b9246e..6efb1a0b9ef4 100644 --- a/WilyCore/src/main/java/com/wilyworks/simulator/WilyCore.java +++ b/WilyCore/src/main/java/com/wilyworks/simulator/WilyCore.java @@ -366,6 +366,7 @@ static public void runTo(Pose2d pose, PoseVelocity2d velocity) { // If the user didn't explicitly call the simulation update() API, do it now: double deltaT = advanceTime(0); simulation.runTo(deltaT, pose, velocity); + mechSim.advance(deltaT); simulationUpdated = true; render(); } From 1460d7c92297aa823f7f7cc3efd4fe9d71d97d0e Mon Sep 17 00:00:00 2001 From: anya-codes Date: Wed, 3 Dec 2025 17:06:43 -0800 Subject: [PATCH 122/198] Disabled transfer in teleop. Will enable again once we have tuned values for the transfer constants to avoid breaking it. --- .../org/firstinspires/ftc/team417/ComplexMechGlob.java | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java b/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java index d79a19469fab..d064b8428f8f 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java @@ -348,7 +348,12 @@ void update () { } servoDrum.setPosition(hwDrumPosition); - servoTransfer.setPosition(transferPosition); + //servoTransfer.setPosition(transferPosition); + if (WilyWorks.isSimulating) { + // Enable on real hardware once transfer parameters are tuned + servoTransfer.setPosition(transferPosition); + } + motLLauncher.setVelocity(lowerLaunchVelocity); motULauncher.setVelocity(upperLaunchVelocity); motIntake.setPower(intakePower); From 167a7a36522ded2ccf9238fe9187833595841178 Mon Sep 17 00:00:00 2001 From: anya-codes Date: Wed, 3 Dec 2025 17:35:08 -0800 Subject: [PATCH 123/198] Changed robot wifi name from 417-b-RC to 417-B-RC --- .../org/firstinspires/ftc/team417/roadrunner/MecanumDrive.java | 2 +- .../java/org/firstinspires/ftc/team417/utils/WilyConfig.java | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/roadrunner/MecanumDrive.java b/team417/src/main/java/org/firstinspires/ftc/team417/roadrunner/MecanumDrive.java index 4c50ea7b23e4..9e2ee55fe76a 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/roadrunner/MecanumDrive.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/roadrunner/MecanumDrive.java @@ -207,7 +207,7 @@ public static String getBotName() { } public static boolean isDevBot = getBotName().equals("DevBot"); public static boolean isFastBot = getBotName().equals("417-RC"); - public static boolean isSlowBot = getBotName().equals("417-b-RC"); + public static boolean isSlowBot = getBotName().equals("417-B-RC"); public static Params PARAMS = new Params(); public MecanumKinematics kinematics; // Initialized by initializeKinematics() diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/utils/WilyConfig.java b/team417/src/main/java/org/firstinspires/ftc/team417/utils/WilyConfig.java index c04585aa12b9..d2160337de0f 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/utils/WilyConfig.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/utils/WilyConfig.java @@ -12,7 +12,7 @@ public class WilyConfig extends WilyWorks.Config { WilyConfig() { // Impersonate the DevBot when running the simulator: - deviceName = "417-b-RC"; + deviceName = "417-B-RC"; // Use these dimensions for the robot: robotWidth = 18.0; From 2e185824e936726d504cad9e8f84b0faa773844e Mon Sep 17 00:00:00 2001 From: anya-codes Date: Thu, 4 Dec 2025 17:27:35 -0800 Subject: [PATCH 124/198] Moved intake from right_stick_x to right_stick_y --- .../org/firstinspires/ftc/team417/CompetitionTeleOp.java | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java index e0b6d966a942..e5530a25f8f1 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java @@ -2,14 +2,17 @@ import com.acmerobotics.dashboard.config.Config; import com.acmerobotics.dashboard.telemetry.TelemetryPacket; +import com.acmerobotics.roadrunner.AngularVelConstraint; import com.acmerobotics.roadrunner.Pose2d; import com.acmerobotics.roadrunner.PoseVelocity2d; import com.acmerobotics.roadrunner.Vector2d; +import com.qualcomm.hardware.bosch.BNO055IMU; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.util.ElapsedTime; import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.firstinspires.ftc.team417.roadrunner.Drawing; import org.firstinspires.ftc.team417.roadrunner.MecanumDrive; @@ -59,7 +62,7 @@ public void runOpMode() { halfLinearHalfCubic(-gamepad1.left_stick_x * doSLOWMODE()) ), - halfLinearHalfCubic(-gamepad1.right_stick_x) + halfLinearHalfCubic(-gamepad1.right_stick_y) )); From b44f639fb548617e4ef30cab3c11d241112eb4bb Mon Sep 17 00:00:00 2001 From: Team <417skid@swerverobotics.club> Date: Thu, 4 Dec 2025 19:46:27 -0800 Subject: [PATCH 125/198] changes --- .../ftc/team417/ComplexMechGlob.java | 31 ++++++++++--------- 1 file changed, 17 insertions(+), 14 deletions(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java b/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java index d064b8428f8f..102a809af10f 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java @@ -74,18 +74,20 @@ void setLaunchVelocity (LaunchDistance launchDistance) {} public class ComplexMechGlob extends MechGlob { //a class encompassing all code that IS for slowbot // TODO tune constants via FTC Dashboard: static double FEEDER_POWER = 1; - static double TRANSFER_TIME_UP = 0.3; - static double TRANSFER_TIME_TOTAL = 0.6; //TRANSFER_TIME_TOTAL must be more than TRANSFER_TIME_UP - static double FAR_FLYWHEEL_VELOCITY = 1500; - static double NEAR_FLYWHEEL_VELOCITY = 1500; - static double FLYWHEEL_BACK_SPIN = 300; - static double TRANSFER_INACTIVE_POSITION = 0; - static double TRANSFER_ACTIVE_POSITION = 1; + static double TRANSFER_TIME_UP = 2; + static double TRANSFER_TIME_TOTAL = 5; //TRANSFER_TIME_TOTAL must be more than TRANSFER_TIME_UP + static double FAR_FLYWHEEL_VELOCITY = 933; //was 1500 + static double NEAR_FLYWHEEL_VELOCITY = 933; //was 1500 + static double FLYWHEEL_BACK_SPIN = 150; //was 300 + static double TRANSFER_INACTIVE_POSITION = 0.45; + static double TRANSFER_ACTIVE_POSITION = 0.7; static double REVERSE_INTAKE_SPEED = -1; static double INTAKE_SPEED = 1; static double FLYWHEEL_VELOCITY_TOLERANCE = 25; //this is an epsiiiiiiiiilon static double VOLTAGE_TOLERANCE = 0.01; //THIS IS AN EPSILON AS WELLLLLL + static double MOTOR_D_VALUE = 1; + ElapsedTime transferTimer; double userIntakeSpeed; @@ -176,10 +178,11 @@ public DrumRequest(double position, WaitState nextState) { motULauncher.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.FLOAT); motIntake.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); - motLLauncher.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, new PIDFCoefficients(300, 0, 0, 10)); - motULauncher.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, new PIDFCoefficients(300, 0, 0, 10)); + motLLauncher.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, new PIDFCoefficients(300, 0, MOTOR_D_VALUE, 10)); + motULauncher.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, new PIDFCoefficients(300, 0, MOTOR_D_VALUE, 10)); motULauncher.setDirection(DcMotorSimple.Direction.REVERSE); + motLLauncher.setDirection(DcMotorSimple.Direction.REVERSE); setLaunchVelocity(LaunchDistance.NEAR); @@ -349,16 +352,16 @@ void update () { } servoDrum.setPosition(hwDrumPosition); //servoTransfer.setPosition(transferPosition); - if (WilyWorks.isSimulating) { + // Enable on real hardware once transfer parameters are tuned - servoTransfer.setPosition(transferPosition); - } + servoTransfer.setPosition(transferPosition); + motLLauncher.setVelocity(lowerLaunchVelocity); motULauncher.setVelocity(upperLaunchVelocity); motIntake.setPower(intakePower); - servoBLaunchFeeder.setPower(FEEDER_POWER); - servoFLaunchFeeder.setPower((FEEDER_POWER)); + servoBLaunchFeeder.setPower(-FEEDER_POWER); + servoFLaunchFeeder.setPower(FEEDER_POWER); } } From 95f257c229530a17d1ac36cb9c91eadc7b8d94fa Mon Sep 17 00:00:00 2001 From: anya-codes Date: Thu, 4 Dec 2025 20:00:53 -0800 Subject: [PATCH 126/198] Added a new method to ComplexMechGlob that returns the color of the artifact stored at a slot index and added telemetry to print that to CompTeleOp. Also added code for drum gate to ComplexMechGlob in intake function and changed intaking to x and turning the robot to x --- .../ftc/team417/CompetitionTeleOp.java | 20 ++++++++++++--- .../ftc/team417/ComplexMechGlob.java | 25 ++++++++++++++++--- 2 files changed, 38 insertions(+), 7 deletions(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java index e5530a25f8f1..262ab050ba3f 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java @@ -6,7 +6,6 @@ import com.acmerobotics.roadrunner.Pose2d; import com.acmerobotics.roadrunner.PoseVelocity2d; import com.acmerobotics.roadrunner.Vector2d; -import com.qualcomm.hardware.bosch.BNO055IMU; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.util.ElapsedTime; @@ -47,6 +46,8 @@ public void runOpMode() { MecanumDrive drive = new MecanumDrive(hardwareMap, telemetry, gamepad1, beginPose); MechGlob mechGlob = ComplexMechGlob.create(hardwareMap, telemetry, false); + telemetry.setDisplayFormat(Telemetry.DisplayFormat.HTML); + // Initialize motors, servos, LEDs // Wait for Start to be pressed on the Driver Hub! @@ -54,6 +55,7 @@ public void runOpMode() { while (opModeIsActive()) { telemetry.addLine("Running TeleOp!"); + telemetry.addLine("I want bold text"); // Set the drive motor powers according to the gamepad input: drive.setDrivePowers(new PoseVelocity2d( @@ -62,7 +64,7 @@ public void runOpMode() { halfLinearHalfCubic(-gamepad1.left_stick_x * doSLOWMODE()) ), - halfLinearHalfCubic(-gamepad1.right_stick_y) + halfLinearHalfCubic(-gamepad1.right_stick_x) )); @@ -100,9 +102,19 @@ public void runOpMode() { } else if (gamepad2.dpadDownWasPressed()) { mechGlob.setLaunchVelocity(LaunchDistance.NEAR); } - mechGlob.intake(gamepad2.left_stick_x); + mechGlob.intake(gamepad2.left_stick_y); mechGlob.update(); - + + String slot0 = mechGlob.getSlotColor(0); + String slot1 = mechGlob.getSlotColor(1); + String slot2 = mechGlob.getSlotColor(2); + + telemetry.addData("Slot0: ", slot0); + telemetry.addData("Slot1: ", slot1); + telemetry.addData("Slot2: ", slot2); + + + MecanumDrive.sendTelemetryPacket(packet); telemetry.update(); } diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java b/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java index d064b8428f8f..ec69126a54fb 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java @@ -68,14 +68,19 @@ boolean preLaunch (RequestedColor requestedColor) { } void setLaunchVelocity (LaunchDistance launchDistance) {} + public String getSlotColor(int slotIndex) { + return "NONE"; + } + + } @Config public class ComplexMechGlob extends MechGlob { //a class encompassing all code that IS for slowbot // TODO tune constants via FTC Dashboard: static double FEEDER_POWER = 1; - static double TRANSFER_TIME_UP = 0.3; - static double TRANSFER_TIME_TOTAL = 0.6; //TRANSFER_TIME_TOTAL must be more than TRANSFER_TIME_UP + static double TRANSFER_TIME_UP = 2; + static double TRANSFER_TIME_TOTAL = 5; //TRANSFER_TIME_TOTAL must be more than TRANSFER_TIME_UP static double FAR_FLYWHEEL_VELOCITY = 1500; static double NEAR_FLYWHEEL_VELOCITY = 1500; static double FLYWHEEL_BACK_SPIN = 300; @@ -85,6 +90,8 @@ public class ComplexMechGlob extends MechGlob { //a class encompassing all code static double INTAKE_SPEED = 1; static double FLYWHEEL_VELOCITY_TOLERANCE = 25; //this is an epsiiiiiiiiilon static double VOLTAGE_TOLERANCE = 0.01; //THIS IS AN EPSILON AS WELLLLLL + static double DRUM_GATE_OPEN_POSITION = 1; + static double DRUM_GATE_CLOSED_POSITION = 0.7; ElapsedTime transferTimer; @@ -124,6 +131,7 @@ enum WaitState { DcMotorEx motIntake; CRServo servoBLaunchFeeder; CRServo servoFLaunchFeeder; + Servo servoDrumGate; NormalizedColorSensor sensorColor1; NormalizedColorSensor sensorColor2; @@ -159,6 +167,7 @@ public DrumRequest(double position, WaitState nextState) { motIntake = hardwareMap.get(DcMotorEx.class, "motIntake"); servoBLaunchFeeder = hardwareMap.get(CRServo.class, "servoBLaunchFeeder"); servoFLaunchFeeder = hardwareMap.get(CRServo.class, "servoFLaunchFeeder"); + servoDrumGate = hardwareMap.get(Servo.class, "servoDrumGate"); coolColorDetector = new CoolColorDetector(hardwareMap, telemetry); /* @@ -215,8 +224,12 @@ int findNearestSlot (double [] position, RequestedColor requestedColor) { @Override void intake (double intakeSpeed) { - userIntakeSpeed = intakeSpeed; + if (userIntakeSpeed != 0) { + servoDrumGate.setPosition(DRUM_GATE_OPEN_POSITION); + } else { + servoDrumGate.setPosition(DRUM_GATE_CLOSED_POSITION); + } } @Override @@ -282,6 +295,12 @@ int findSlotFromPosition (double position, double [] positions) { } return -1; } + + public String getSlotColor(int slotIndex) { + PixelColor artifactColor = slotOccupiedBy.get(slotIndex); + return artifactColor.toString(); + } + @Override void update () { double intakePower = 0; From e60692ff1d33b4995babe49a10a5f58134163786 Mon Sep 17 00:00:00 2001 From: anya-codes Date: Thu, 4 Dec 2025 20:57:19 -0800 Subject: [PATCH 127/198] Added telemetry and a manual control to turn the flywheel off. Manual control NEEDS to be tested --- .../org/firstinspires/ftc/team417/CompetitionTeleOp.java | 6 ++++-- .../org/firstinspires/ftc/team417/ComplexMechGlob.java | 8 ++++++-- .../org/firstinspires/ftc/team417/CoolColorDetector.java | 2 ++ 3 files changed, 12 insertions(+), 4 deletions(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java index 262ab050ba3f..bcc9fb4777ec 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java @@ -55,7 +55,6 @@ public void runOpMode() { while (opModeIsActive()) { telemetry.addLine("Running TeleOp!"); - telemetry.addLine("I want bold text"); // Set the drive motor powers according to the gamepad input: drive.setDrivePowers(new PoseVelocity2d( @@ -101,7 +100,11 @@ public void runOpMode() { mechGlob.setLaunchVelocity(LaunchDistance.FAR); } else if (gamepad2.dpadDownWasPressed()) { mechGlob.setLaunchVelocity(LaunchDistance.NEAR); + } else if (gamepad2.dpadRightWasPressed()) { + // turns off the flywheels + mechGlob.setLaunchVelocity(LaunchDistance.OFF); } + mechGlob.intake(gamepad2.left_stick_y); mechGlob.update(); @@ -114,7 +117,6 @@ public void runOpMode() { telemetry.addData("Slot2: ", slot2); - MecanumDrive.sendTelemetryPacket(packet); telemetry.update(); } diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java b/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java index 1a336d4d7123..c4ed7f5dc034 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java @@ -35,7 +35,8 @@ enum PixelColor { } enum LaunchDistance { FAR, - NEAR + NEAR, + OFF //turns the flywheel off } class MechGlob { //a placeholder class encompassing all code that ISN'T for slowbot. @@ -284,9 +285,12 @@ void setLaunchVelocity (LaunchDistance launchDistance) { if (launchDistance == LaunchDistance.NEAR) { upperLaunchVelocity = NEAR_FLYWHEEL_VELOCITY - (0.5 * FLYWHEEL_BACK_SPIN); lowerLaunchVelocity = NEAR_FLYWHEEL_VELOCITY + (0.5 * FLYWHEEL_BACK_SPIN); - } else { + } else if (launchDistance == LaunchDistance.FAR){ upperLaunchVelocity = FAR_FLYWHEEL_VELOCITY - (0.5 * FLYWHEEL_BACK_SPIN); lowerLaunchVelocity = FAR_FLYWHEEL_VELOCITY + (0.5 * FLYWHEEL_BACK_SPIN); + } else { + upperLaunchVelocity = 0; + lowerLaunchVelocity = 0; } } int findSlotFromPosition (double position, double [] positions) { diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CoolColorDetector.java b/team417/src/main/java/org/firstinspires/ftc/team417/CoolColorDetector.java index 296784ff5b70..c916af672948 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CoolColorDetector.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CoolColorDetector.java @@ -54,6 +54,8 @@ PixelColor detectArtifactColor() { telemetry.addLine(String.format("Color Detect: %.2fmm, %.2fmm %s, Hue: %.1f", distance1, distance2, colorCube, hue)); + telemetry.addLine(String.format(" %.2f\", %.2f\"", distance1, distance2)); + if (hue > 165 && hue < 180) { //range determined from testing return PixelColor.GREEN; From 222608d523dfef66b875a142247de526d0595376 Mon Sep 17 00:00:00 2001 From: anya-codes Date: Thu, 4 Dec 2025 21:38:26 -0800 Subject: [PATCH 128/198] Started auto aim code and added a class for auto aim and a PID controller class. Code is not finished --- .../ftc/team417/CompetitionTeleOp.java | 103 ++++++++++++++++++ 1 file changed, 103 insertions(+) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java index bcc9fb4777ec..4addeaa7d947 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java @@ -137,4 +137,107 @@ public static double halfLinearHalfCubic(double input) { } } +class AmazingAutoAim { + Telemetry telemetry = null; + // Constants to tune in FTC dashboard + public static double KP = 1.5; + public static double KI = 0; + public static double KD = 0.1; + double targetX; + double targetY; + PIDController pid; + + AmazingAutoAim(Telemetry telemetry, CompetitionAuto.Alliance alliance) { + this.telemetry = telemetry; + + if (alliance == CompetitionAuto.Alliance.RED) { + targetX = -65; + targetY = 55; + } else { + targetX = -65; + targetY = -55; + } + pid = new PIDController(KP, KI, KD); + + } + + public double get(Pose2d pose) { + double deltaY = targetY - pose.position.y; + double deltaX = targetX - pose.position.x; + + double beta = Math.atan2(deltaY, deltaX); + double alpha = pose.heading.toDouble(); + double angle = beta - alpha; + double normalizedAngle = AngleUnit.normalizeRadians(angle); + + return pid.calculate(normalizedAngle); + + } + +} + +class PIDController { + + private double kP; + private double kI; + private double kD; + + private double setpoint; + private double previousError = 0; + private double integral = 0; + private double outputMin = Double.NEGATIVE_INFINITY; + private double outputMax = Double.POSITIVE_INFINITY; + + private long lastTimestamp = System.nanoTime(); + + public PIDController(double kP, double kI, double kD) { + this.kP = kP; + this.kI = kI; + this.kD = kD; + } + + public void setSetpoint(double setpoint) { + this.setpoint = setpoint; + } + + public void setOutputLimits(double min, double max) { + this.outputMin = min; + this.outputMax = max; + } + + /** + * Calculates the PID output based on the current process variable. + */ + public double calculate(double currentValue) { + long now = System.nanoTime(); + double dt = (now - lastTimestamp) / 1e9; // seconds + lastTimestamp = now; + + double error = setpoint - currentValue; + + // Integral with basic anti-windup + integral += error * dt; + + // Derivative + double derivative = (error - previousError) / dt; + + // PID Output + double output = (kP * error) + (kI * integral) + (kD * derivative); + + // Clamp output + output = Math.max(outputMin, Math.min(outputMax, output)); + + previousError = error; + + return output; + } + + public void reset() { + integral = 0; + previousError = 0; + lastTimestamp = System.nanoTime(); + } +} + + From aa0fdb4111016d1aa762c911d7fdc572fce1f881 Mon Sep 17 00:00:00 2001 From: Sameer Date: Sat, 6 Dec 2025 13:56:07 -0800 Subject: [PATCH 129/198] Created PreLaunchAction and SpinUpAction for auto, builds but not tested in wily works --- .../ftc/team417/CompetitionAuto.java | 31 +++++++++++++++++++ 1 file changed, 31 insertions(+) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java index 42094685a426..17e9f1e12b13 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java @@ -9,6 +9,7 @@ import com.acmerobotics.roadrunner.TrajectoryActionBuilder; import com.acmerobotics.roadrunner.Vector2d; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.robot.Robot; import org.firstinspires.ftc.team417.apriltags.LimelightDetector; import org.firstinspires.ftc.team417.apriltags.Pattern; @@ -322,6 +323,7 @@ public LaunchAction(MechGlob mechGlob, Pattern pattern, CountBalls orderCount) { } + @Override public boolean run(double elapsedTime) { if (elapsedTime == 0) { @@ -339,6 +341,35 @@ public boolean run(double elapsedTime) { } } +class SpinUpAction extends RobotAction { + MechGlob mechGlob; + LaunchDistance launchDistance; + public SpinUpAction(MechGlob mechglob, LaunchDistance launchDistance) { + this.mechGlob = mechglob; + this.launchDistance = launchDistance; + } + + @Override + public boolean run(double elapsedTime) { + mechGlob.setLaunchVelocity(launchDistance); + return false; + } +} +class PreLaunchAction extends RobotAction { + MechGlob mechGlob; + RequestedColor requestedColor; + public PreLaunchAction(MechGlob mechGlob, RequestedColor requestedColor) { + this.requestedColor = requestedColor; + this.mechGlob = mechGlob; + } + + @Override + public boolean run(double elapsedTime) { + return mechGlob.preLaunch(requestedColor); + } +} + + class IntakeAction extends RobotAction { double intakeSpeed; MechGlob mechGlob; From b109fa8081f857161708118e491f33aa2fe47eda Mon Sep 17 00:00:00 2001 From: Andrew Date: Sat, 6 Dec 2025 20:16:35 -0800 Subject: [PATCH 130/198] Update simulator. --- .../main/java/com/wilyworks/simulator/framework/MechSim.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/WilyCore/src/main/java/com/wilyworks/simulator/framework/MechSim.java b/WilyCore/src/main/java/com/wilyworks/simulator/framework/MechSim.java index 51f224f131ae..100e5c0aff5e 100644 --- a/WilyCore/src/main/java/com/wilyworks/simulator/framework/MechSim.java +++ b/WilyCore/src/main/java/com/wilyworks/simulator/framework/MechSim.java @@ -220,7 +220,7 @@ enum BallColor {PURPLE, GREEN, GOLD} final double[] TRANSFER_POSITIONS = { 3.0/6, 5.0/6, 1.0/6 }; // Servo positions for intaking final double SLOT_EPSILON = 0.02; // Epsilon for determining a slot relative to a [0, 1] range final double MIN_TRANSFER_TIME = 0.1; // Second it takes for a transfer - final double MIN_TRANSFER_POSITION = 0.1; // Minimum position to start a transfer + final double MIN_TRANSFER_POSITION = 0.6; // Minimum position to start a transfer final double TRANSFER_SERVO_SPEED = (60.0 / 360) / 0.25; // Speed of a goBilda torque servo, position/s final double LAUNCH_SPEED = 144; // Ball launch speed, inches per second final Point LAUNCH_OFFSET = new Point(-4, 0); @@ -558,7 +558,7 @@ void simulate(Pose2d pose, double deltaT) { if (forwardFeederServo.getPower() < FEEDER_POWER) { throw new RuntimeException("A transfer is requested when forward feeder servo isn't running. That won't work!"); } - if (backwardFeederServo.getPower() < FEEDER_POWER) { + if (Math.abs(backwardFeederServo.getPower()) < FEEDER_POWER) { throw new RuntimeException("A transfer is requested when backward feeder servo isn't running. That won't work!"); } if (slotBalls.get(transferSlot) == null) { From c80f7e85e3bccdd205b58d0e18f7f537a88df07a Mon Sep 17 00:00:00 2001 From: anya-codes Date: Sat, 6 Dec 2025 23:14:57 -0800 Subject: [PATCH 131/198] Added @override annotation to getSlotColor that was missing before --- .../main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java | 1 + 1 file changed, 1 insertion(+) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java b/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java index c4ed7f5dc034..c72d8d12f8e8 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java @@ -302,6 +302,7 @@ int findSlotFromPosition (double position, double [] positions) { return -1; } + @Override public String getSlotColor(int slotIndex) { PixelColor artifactColor = slotOccupiedBy.get(slotIndex); return artifactColor.toString(); From 2432711f2b65460fc254730396820dbbb051885c Mon Sep 17 00:00:00 2001 From: Andrew Date: Sun, 7 Dec 2025 11:49:36 -0800 Subject: [PATCH 132/198] Update to latest drops of Wily Works and Loony Tune. --- .../main/java/android/content/Context.java | 2 + .../android/content/res/AssetManager.java | 13 + .../main/java/android/graphics/Bitmap.java | 235 ++++++++ .../java/android/graphics/BitmapFactory.java | 9 + .../main/java/android/graphics/Canvas.java | 239 ++++++++ .../src/main/java/android/graphics/Color.java | 106 ++++ .../src/main/java/android/graphics/Paint.java | 509 ++++++++++++++++++ team417/build.gradle | 2 +- .../ftc/team417/roadrunner/LoonyTune.java | 477 +++++++--------- .../ftc/team417/roadrunner/MecanumDrive.java | 14 +- .../team417/utils/ConfigurationTester.java | 6 +- 11 files changed, 1335 insertions(+), 277 deletions(-) create mode 100644 WilyCore/src/main/java/android/content/res/AssetManager.java create mode 100644 WilyCore/src/main/java/android/graphics/BitmapFactory.java create mode 100644 WilyCore/src/main/java/android/graphics/Paint.java diff --git a/WilyCore/src/main/java/android/content/Context.java b/WilyCore/src/main/java/android/content/Context.java index 7abce111a1bf..94849d3cdb5e 100644 --- a/WilyCore/src/main/java/android/content/Context.java +++ b/WilyCore/src/main/java/android/content/Context.java @@ -1,6 +1,7 @@ package android.content; import android.app.Activity; +import android.content.res.AssetManager; import java.io.File; @@ -13,4 +14,5 @@ public String getPackageName() { } public static final int MODE_PRIVATE = 0; public File getDir(String var1, int var2) { return null; } + public AssetManager getAssets() { return null; } } diff --git a/WilyCore/src/main/java/android/content/res/AssetManager.java b/WilyCore/src/main/java/android/content/res/AssetManager.java new file mode 100644 index 000000000000..620a5840785a --- /dev/null +++ b/WilyCore/src/main/java/android/content/res/AssetManager.java @@ -0,0 +1,13 @@ +package android.content.res; + +import android.annotation.NonNull; + +import java.io.IOException; +import java.io.InputStream; + +public class AssetManager { + @NonNull + public InputStream open(@NonNull String fileName) throws IOException { + throw new RuntimeException("Wily Works Stub!"); + } +} diff --git a/WilyCore/src/main/java/android/graphics/Bitmap.java b/WilyCore/src/main/java/android/graphics/Bitmap.java index a6d567772595..6fe95dfc63a6 100644 --- a/WilyCore/src/main/java/android/graphics/Bitmap.java +++ b/WilyCore/src/main/java/android/graphics/Bitmap.java @@ -1,4 +1,239 @@ package android.graphics; +import android.annotation.NonNull; + +import java.io.OutputStream; +import java.nio.Buffer; + public class Bitmap { + @NonNull + public static final int DENSITY_NONE = 0; + + Bitmap() { + throw new RuntimeException("Stub!"); + } + + public int getDensity() { + throw new RuntimeException("Stub!"); + } + + public void setDensity(int density) { + throw new RuntimeException("Stub!"); + } + + public void reconfigure(int width, int height, Config config) { + throw new RuntimeException("Stub!"); + } + + public void setWidth(int width) { + throw new RuntimeException("Stub!"); + } + + public void setHeight(int height) { + throw new RuntimeException("Stub!"); + } + + public void setConfig(Config config) { + throw new RuntimeException("Stub!"); + } + + public void recycle() { + throw new RuntimeException("Stub!"); + } + + public boolean isRecycled() { + throw new RuntimeException("Stub!"); + } + + public int getGenerationId() { + throw new RuntimeException("Stub!"); + } + + public void copyPixelsToBuffer(Buffer dst) { + throw new RuntimeException("Stub!"); + } + + public void copyPixelsFromBuffer(Buffer src) { + throw new RuntimeException("Stub!"); + } + + public Bitmap copy(Config config, boolean isMutable) { + throw new RuntimeException("Stub!"); + } + + + public static Bitmap createScaledBitmap(@NonNull Bitmap src, int dstWidth, int dstHeight, boolean filter) { + throw new RuntimeException("Stub!"); + } + + public static Bitmap createBitmap(@NonNull Bitmap src) { + throw new RuntimeException("Stub!"); + } + + public static Bitmap createBitmap(@NonNull Bitmap source, int x, int y, int width, int height) { + throw new RuntimeException("Stub!"); + } + + public static Bitmap createBitmap(int width, int height, @NonNull Config config) { + throw new RuntimeException("Stub!"); + } + + public static Bitmap createBitmap(int width, int height, @NonNull Config config, boolean hasAlpha) { + throw new RuntimeException("Stub!"); + } + + public static Bitmap createBitmap(@NonNull int[] colors, int offset, int stride, int width, int height, @NonNull Config config) { + throw new RuntimeException("Stub!"); + } + + public static Bitmap createBitmap(@NonNull int[] colors, int width, int height, Config config) { + throw new RuntimeException("Stub!"); + } + + public byte[] getNinePatchChunk() { + throw new RuntimeException("Stub!"); + } + + public boolean compress(CompressFormat format, int quality, OutputStream stream) { + throw new RuntimeException("Stub!"); + } + + public boolean isMutable() { + throw new RuntimeException("Stub!"); + } + + public boolean isPremultiplied() { + throw new RuntimeException("Stub!"); + } + + public void setPremultiplied(boolean premultiplied) { + throw new RuntimeException("Stub!"); + } + + public int getWidth() { + throw new RuntimeException("Stub!"); + } + + public int getHeight() { + throw new RuntimeException("Stub!"); + } + + public int getScaledWidth(Canvas canvas) { + throw new RuntimeException("Stub!"); + } + + public int getScaledHeight(Canvas canvas) { + throw new RuntimeException("Stub!"); + } + + public int getScaledWidth(int targetDensity) { + throw new RuntimeException("Stub!"); + } + + public int getScaledHeight(int targetDensity) { + throw new RuntimeException("Stub!"); + } + + public int getRowBytes() { + throw new RuntimeException("Stub!"); + } + + public int getByteCount() { + throw new RuntimeException("Stub!"); + } + + public int getAllocationByteCount() { + throw new RuntimeException("Stub!"); + } + + public Config getConfig() { + throw new RuntimeException("Stub!"); + } + + public boolean hasAlpha() { + throw new RuntimeException("Stub!"); + } + + public void setHasAlpha(boolean hasAlpha) { + throw new RuntimeException("Stub!"); + } + + public boolean hasMipMap() { + throw new RuntimeException("Stub!"); + } + + public void setHasMipMap(boolean hasMipMap) { + throw new RuntimeException("Stub!"); + } + + public void eraseColor(int c) { + throw new RuntimeException("Stub!"); + } + + public void eraseColor(long color) { + throw new RuntimeException("Stub!"); + } + + public int getPixel(int x, int y) { + throw new RuntimeException("Stub!"); + } + + @NonNull + public Color getColor(int x, int y) { + throw new RuntimeException("Stub!"); + } + + public void getPixels(int[] pixels, int offset, int stride, int x, int y, int width, int height) { + throw new RuntimeException("Stub!"); + } + + public void setPixel(int x, int y, int color) { + throw new RuntimeException("Stub!"); + } + + public void setPixels(int[] pixels, int offset, int stride, int x, int y, int width, int height) { + throw new RuntimeException("Stub!"); + } + + public int describeContents() { + throw new RuntimeException("Stub!"); + } + + public Bitmap extractAlpha() { + throw new RuntimeException("Stub!"); + } + + public Bitmap extractAlpha(Paint paint, int[] offsetXY) { + throw new RuntimeException("Stub!"); + } + + public boolean sameAs(Bitmap other) { + throw new RuntimeException("Stub!"); + } + + public void prepareToDraw() { + throw new RuntimeException("Stub!"); + } + + public static enum CompressFormat { + JPEG, + PNG, + /** @deprecated */ + @Deprecated + WEBP, + WEBP_LOSSY, + WEBP_LOSSLESS; + } + + public static enum Config { + ALPHA_8, + RGB_565, + /** @deprecated */ + @Deprecated + ARGB_4444, + ARGB_8888, + RGBA_F16, + HARDWARE; + } + } diff --git a/WilyCore/src/main/java/android/graphics/BitmapFactory.java b/WilyCore/src/main/java/android/graphics/BitmapFactory.java new file mode 100644 index 000000000000..043fb7891a33 --- /dev/null +++ b/WilyCore/src/main/java/android/graphics/BitmapFactory.java @@ -0,0 +1,9 @@ +package android.graphics; + +import java.io.InputStream; + +public class BitmapFactory { + public static Bitmap decodeStream(InputStream is) { + throw new RuntimeException("Not Wily Works implemented!"); + } +} diff --git a/WilyCore/src/main/java/android/graphics/Canvas.java b/WilyCore/src/main/java/android/graphics/Canvas.java index 006df0645204..880ac9d08639 100644 --- a/WilyCore/src/main/java/android/graphics/Canvas.java +++ b/WilyCore/src/main/java/android/graphics/Canvas.java @@ -1,4 +1,243 @@ package android.graphics; +import android.annotation.NonNull; +import android.annotation.Nullable; + public class Canvas { + public Canvas(@NonNull Bitmap bitmap) { + throw new RuntimeException("Wily Works Stub!"); + } + + public static final int ALL_SAVE_FLAG = 31; + + public Canvas() { + throw new RuntimeException("Stub!"); + } + + public boolean isHardwareAccelerated() { + throw new RuntimeException("Stub!"); + } + + public void setBitmap(@Nullable Bitmap bitmap) { + throw new RuntimeException("Stub!"); + } + + public void enableZ() { + throw new RuntimeException("Stub!"); + } + + public void disableZ() { + throw new RuntimeException("Stub!"); + } + + public boolean isOpaque() { + throw new RuntimeException("Stub!"); + } + + public int getWidth() { + throw new RuntimeException("Stub!"); + } + + public int getHeight() { + throw new RuntimeException("Stub!"); + } + + public int getDensity() { + throw new RuntimeException("Stub!"); + } + + public void setDensity(int density) { + throw new RuntimeException("Stub!"); + } + + public int getMaximumBitmapWidth() { + throw new RuntimeException("Stub!"); + } + + public int getMaximumBitmapHeight() { + throw new RuntimeException("Stub!"); + } + + public int save() { + throw new RuntimeException("Stub!"); + } + + /** @deprecated */ + @Deprecated + public int saveLayer(float left, float top, float right, float bottom, @Nullable Paint paint, int saveFlags) { + throw new RuntimeException("Stub!"); + } + + public int saveLayer(float left, float top, float right, float bottom, @Nullable Paint paint) { + throw new RuntimeException("Stub!"); + } + /** @deprecated */ + @Deprecated + public int saveLayerAlpha(float left, float top, float right, float bottom, int alpha, int saveFlags) { + throw new RuntimeException("Stub!"); + } + + public int saveLayerAlpha(float left, float top, float right, float bottom, int alpha) { + throw new RuntimeException("Stub!"); + } + + public void restore() { + throw new RuntimeException("Stub!"); + } + + public int getSaveCount() { + throw new RuntimeException("Stub!"); + } + + public void restoreToCount(int saveCount) { + throw new RuntimeException("Stub!"); + } + + public void translate(float dx, float dy) { + throw new RuntimeException("Stub!"); + } + + public void scale(float sx, float sy) { + throw new RuntimeException("Stub!"); + } + + public final void scale(float sx, float sy, float px, float py) { + throw new RuntimeException("Stub!"); + } + + public void rotate(float degrees) { + throw new RuntimeException("Stub!"); + } + + public final void rotate(float degrees, float px, float py) { + throw new RuntimeException("Stub!"); + } + + public void skew(float sx, float sy) { + throw new RuntimeException("Stub!"); + } + + public boolean clipRect(float left, float top, float right, float bottom) { + throw new RuntimeException("Stub!"); + } + + public boolean clipOutRect(float left, float top, float right, float bottom) { + throw new RuntimeException("Stub!"); + } + + public boolean clipRect(int left, int top, int right, int bottom) { + throw new RuntimeException("Stub!"); + } + + public boolean clipOutRect(int left, int top, int right, int bottom) { + throw new RuntimeException("Stub!"); + } + + + /** @deprecated */ + @Deprecated + public void drawBitmap(@NonNull int[] colors, int offset, int stride, float x, float y, int width, int height, boolean hasAlpha, @Nullable Paint paint) { + throw new RuntimeException("Stub!"); + } + + /** @deprecated */ + @Deprecated + public void drawBitmap(@NonNull int[] colors, int offset, int stride, int x, int y, int width, int height, boolean hasAlpha, @Nullable Paint paint) { + throw new RuntimeException("Stub!"); + } + + public void drawBitmapMesh(@NonNull Bitmap bitmap, int meshWidth, int meshHeight, @NonNull float[] verts, int vertOffset, @Nullable int[] colors, int colorOffset, @Nullable Paint paint) { + throw new RuntimeException("Stub!"); + } + + public void drawCircle(float cx, float cy, float radius, @NonNull Paint paint) { + throw new RuntimeException("Stub!"); + } + + public void drawColor(int color) { + throw new RuntimeException("Stub!"); + } + + public void drawColor(long color) { + throw new RuntimeException("Stub!"); + } + + public void drawLine(float startX, float startY, float stopX, float stopY, @NonNull Paint paint) { + throw new RuntimeException("Stub!"); + } + + public void drawLines(@NonNull float[] pts, int offset, int count, @NonNull Paint paint) { + throw new RuntimeException("Stub!"); + } + + public void drawLines(@NonNull float[] pts, @NonNull Paint paint) { + throw new RuntimeException("Stub!"); + } + public void drawOval(float left, float top, float right, float bottom, @NonNull Paint paint) { + throw new RuntimeException("Stub!"); + } + + public void drawPaint(@NonNull Paint paint) { + throw new RuntimeException("Stub!"); + } + + public void drawPoint(float x, float y, @NonNull Paint paint) { + throw new RuntimeException("Stub!"); + } + + public void drawPoints(float[] pts, int offset, int count, @NonNull Paint paint) { + throw new RuntimeException("Stub!"); + } + + public void drawPoints(@NonNull float[] pts, @NonNull Paint paint) { + throw new RuntimeException("Stub!"); + } + + /** @deprecated */ + @Deprecated + public void drawPosText(@NonNull char[] text, int index, int count, @NonNull float[] pos, @NonNull Paint paint) { + throw new RuntimeException("Stub!"); + } + + /** @deprecated */ + @Deprecated + public void drawPosText(@NonNull String text, @NonNull float[] pos, @NonNull Paint paint) { + throw new RuntimeException("Stub!"); + } + + public void drawRect(float left, float top, float right, float bottom, @NonNull Paint paint) { + throw new RuntimeException("Stub!"); + } + + public void drawRGB(int r, int g, int b) { + throw new RuntimeException("Stub!"); + } + public void drawRoundRect(float left, float top, float right, float bottom, float rx, float ry, @NonNull Paint paint) { + throw new RuntimeException("Stub!"); + } + + public void drawText(@NonNull char[] text, int index, int count, float x, float y, @NonNull Paint paint) { + throw new RuntimeException("Stub!"); + } + + public void drawText(@NonNull String text, float x, float y, @NonNull Paint paint) { + throw new RuntimeException("Stub!"); + } + + public void drawText(@NonNull String text, int start, int end, float x, float y, @NonNull Paint paint) { + throw new RuntimeException("Stub!"); + } + + public void drawText(@NonNull CharSequence text, int start, int end, float x, float y, @NonNull Paint paint) { + throw new RuntimeException("Stub!"); + } + + public void drawTextRun(@NonNull char[] text, int index, int count, int contextIndex, int contextCount, float x, float y, boolean isRtl, @NonNull Paint paint) { + throw new RuntimeException("Stub!"); + } + + public void drawTextRun(@NonNull CharSequence text, int start, int end, int contextStart, int contextEnd, float x, float y, boolean isRtl, @NonNull Paint paint) { + throw new RuntimeException("Stub!"); + } + } diff --git a/WilyCore/src/main/java/android/graphics/Color.java b/WilyCore/src/main/java/android/graphics/Color.java index 6eb7ef13a2e1..2703c8777385 100644 --- a/WilyCore/src/main/java/android/graphics/Color.java +++ b/WilyCore/src/main/java/android/graphics/Color.java @@ -1,5 +1,7 @@ package android.graphics; +import android.annotation.NonNull; + public class Color { public static final int BLACK = -16777216; public static final int BLUE = -16776961; @@ -108,4 +110,108 @@ public static int parseColor(String colorString) { value |= 0xff000000; return value; } + + public static float red(long color) { + throw new RuntimeException("Stub!"); + } + + public static float green(long color) { + throw new RuntimeException("Stub!"); + } + + public static float blue(long color) { + throw new RuntimeException("Stub!"); + } + + public static float alpha(long color) { + throw new RuntimeException("Stub!"); + } + + public static boolean isSrgb(long color) { + throw new RuntimeException("Stub!"); + } + + public static boolean isWideGamut(long color) { + throw new RuntimeException("Stub!"); + } + + public static int toArgb(long color) { + throw new RuntimeException("Stub!"); + } + + @NonNull + public static Color valueOf(int color) { + throw new RuntimeException("Stub!"); + } + + @NonNull + public static Color valueOf(long color) { + throw new RuntimeException("Stub!"); + } + + @NonNull + public static Color valueOf(float r, float g, float b) { + throw new RuntimeException("Stub!"); + } + + @NonNull + public static Color valueOf(float r, float g, float b, float a) { + throw new RuntimeException("Stub!"); + } + + public static long pack(int color) { + throw new RuntimeException("Stub!"); + } + + public static long pack(float red, float green, float blue) { + throw new RuntimeException("Stub!"); + } + + public static long pack(float red, float green, float blue, float alpha) { + throw new RuntimeException("Stub!"); + } + + public static float luminance(long color) { + throw new RuntimeException("Stub!"); + } + + public static int alpha(int color) { + throw new RuntimeException("Stub!"); + } + + public static int red(int color) { + throw new RuntimeException("Stub!"); + } + + public static int green(int color) { + throw new RuntimeException("Stub!"); + } + + public static int blue(int color) { + throw new RuntimeException("Stub!"); + } + + public static int rgb(int red, int green, int blue) { + throw new RuntimeException("Stub!"); + } + + public static int rgb(float red, float green, float blue) { + throw new RuntimeException("Stub!"); + } + + public static int argb(float alpha, float red, float green, float blue) { + throw new RuntimeException("Stub!"); + } + + public static float luminance(int color) { + throw new RuntimeException("Stub!"); + } + + public static void RGBToHSV(int red, int green, int blue, float[] hsv) { + throw new RuntimeException("Stub!"); + } + + public static int HSVToColor(int alpha, float[] hsv) { + throw new RuntimeException("Stub!"); + } } diff --git a/WilyCore/src/main/java/android/graphics/Paint.java b/WilyCore/src/main/java/android/graphics/Paint.java new file mode 100644 index 000000000000..6268ac2ab7ef --- /dev/null +++ b/WilyCore/src/main/java/android/graphics/Paint.java @@ -0,0 +1,509 @@ +package android.graphics; + +import android.annotation.NonNull; +import android.annotation.Nullable; + +import java.util.Locale; + +public class Paint { + public static final int ANTI_ALIAS_FLAG = 1; + public static final int CURSOR_AFTER = 0; + public static final int CURSOR_AT = 4; + public static final int CURSOR_AT_OR_AFTER = 1; + public static final int CURSOR_AT_OR_BEFORE = 3; + public static final int CURSOR_BEFORE = 2; + public static final int DEV_KERN_TEXT_FLAG = 256; + public static final int DITHER_FLAG = 4; + public static final int EMBEDDED_BITMAP_TEXT_FLAG = 1024; + public static final int END_HYPHEN_EDIT_INSERT_ARMENIAN_HYPHEN = 3; + public static final int END_HYPHEN_EDIT_INSERT_HYPHEN = 2; + public static final int END_HYPHEN_EDIT_INSERT_MAQAF = 4; + public static final int END_HYPHEN_EDIT_INSERT_UCAS_HYPHEN = 5; + public static final int END_HYPHEN_EDIT_INSERT_ZWJ_AND_HYPHEN = 6; + public static final int END_HYPHEN_EDIT_NO_EDIT = 0; + public static final int END_HYPHEN_EDIT_REPLACE_WITH_HYPHEN = 1; + public static final int FAKE_BOLD_TEXT_FLAG = 32; + public static final int FILTER_BITMAP_FLAG = 2; + public static final int HINTING_OFF = 0; + public static final int HINTING_ON = 1; + public static final int LINEAR_TEXT_FLAG = 64; + public static final int START_HYPHEN_EDIT_INSERT_HYPHEN = 1; + public static final int START_HYPHEN_EDIT_INSERT_ZWJ = 2; + public static final int START_HYPHEN_EDIT_NO_EDIT = 0; + public static final int STRIKE_THRU_TEXT_FLAG = 16; + public static final int SUBPIXEL_TEXT_FLAG = 128; + public static final int UNDERLINE_TEXT_FLAG = 8; + + public Paint() { + throw new RuntimeException("Stub!"); + } + + public Paint(int flags) { + throw new RuntimeException("Stub!"); + } + + public Paint(Paint paint) { + throw new RuntimeException("Stub!"); + } + + public void reset() { + throw new RuntimeException("Stub!"); + } + + public void set(Paint src) { + throw new RuntimeException("Stub!"); + } + + public int getFlags() { + throw new RuntimeException("Stub!"); + } + + public void setFlags(int flags) { + throw new RuntimeException("Stub!"); + } + + public int getHinting() { + throw new RuntimeException("Stub!"); + } + + public void setHinting(int mode) { + throw new RuntimeException("Stub!"); + } + + public final boolean isAntiAlias() { + throw new RuntimeException("Stub!"); + } + + public void setAntiAlias(boolean aa) { + throw new RuntimeException("Stub!"); + } + + public final boolean isDither() { + throw new RuntimeException("Stub!"); + } + + public void setDither(boolean dither) { + throw new RuntimeException("Stub!"); + } + + public final boolean isLinearText() { + throw new RuntimeException("Stub!"); + } + + public void setLinearText(boolean linearText) { + throw new RuntimeException("Stub!"); + } + + public final boolean isSubpixelText() { + throw new RuntimeException("Stub!"); + } + + public void setSubpixelText(boolean subpixelText) { + throw new RuntimeException("Stub!"); + } + + public final boolean isUnderlineText() { + throw new RuntimeException("Stub!"); + } + + public float getUnderlinePosition() { + throw new RuntimeException("Stub!"); + } + + public float getUnderlineThickness() { + throw new RuntimeException("Stub!"); + } + + public void setUnderlineText(boolean underlineText) { + throw new RuntimeException("Stub!"); + } + + public final boolean isStrikeThruText() { + throw new RuntimeException("Stub!"); + } + + public float getStrikeThruPosition() { + throw new RuntimeException("Stub!"); + } + + public float getStrikeThruThickness() { + throw new RuntimeException("Stub!"); + } + + public void setStrikeThruText(boolean strikeThruText) { + throw new RuntimeException("Stub!"); + } + + public final boolean isFakeBoldText() { + throw new RuntimeException("Stub!"); + } + + public void setFakeBoldText(boolean fakeBoldText) { + throw new RuntimeException("Stub!"); + } + + public final boolean isFilterBitmap() { + throw new RuntimeException("Stub!"); + } + + public void setFilterBitmap(boolean filter) { + throw new RuntimeException("Stub!"); + } + + public Style getStyle() { + throw new RuntimeException("Stub!"); + } + + public void setStyle(Style style) { + throw new RuntimeException("Stub!"); + } + + public int getColor() { + throw new RuntimeException("Stub!"); + } + + public long getColorLong() { + throw new RuntimeException("Stub!"); + } + + public void setColor(int color) { + throw new RuntimeException("Stub!"); + } + + public void setColor(long color) { + throw new RuntimeException("Stub!"); + } + + public int getAlpha() { + throw new RuntimeException("Stub!"); + } + + public void setAlpha(int a) { + throw new RuntimeException("Stub!"); + } + + public void setARGB(int a, int r, int g, int b) { + throw new RuntimeException("Stub!"); + } + + public float getStrokeWidth() { + throw new RuntimeException("Stub!"); + } + + public void setStrokeWidth(float width) { + throw new RuntimeException("Stub!"); + } + + public float getStrokeMiter() { + throw new RuntimeException("Stub!"); + } + + public void setStrokeMiter(float miter) { + throw new RuntimeException("Stub!"); + } + + public Cap getStrokeCap() { + throw new RuntimeException("Stub!"); + } + + public void setStrokeCap(Cap cap) { + throw new RuntimeException("Stub!"); + } + + public Join getStrokeJoin() { + throw new RuntimeException("Stub!"); + } + + public void setStrokeJoin(Join join) { + throw new RuntimeException("Stub!"); + } + + public void setShadowLayer(float radius, float dx, float dy, int shadowColor) { + throw new RuntimeException("Stub!"); + } + + public void setShadowLayer(float radius, float dx, float dy, long shadowColor) { + throw new RuntimeException("Stub!"); + } + + public void clearShadowLayer() { + throw new RuntimeException("Stub!"); + } + + public float getShadowLayerRadius() { + throw new RuntimeException("Stub!"); + } + + public float getShadowLayerDx() { + throw new RuntimeException("Stub!"); + } + + public float getShadowLayerDy() { + throw new RuntimeException("Stub!"); + } + + public int getShadowLayerColor() { + throw new RuntimeException("Stub!"); + } + + public long getShadowLayerColorLong() { + throw new RuntimeException("Stub!"); + } + + public Align getTextAlign() { + throw new RuntimeException("Stub!"); + } + + public void setTextAlign(Align align) { + throw new RuntimeException("Stub!"); + } + + @NonNull + public Locale getTextLocale() { + throw new RuntimeException("Stub!"); + } + + public void setTextLocale(@NonNull Locale locale) { + throw new RuntimeException("Stub!"); + } + + public boolean isElegantTextHeight() { + throw new RuntimeException("Stub!"); + } + + public void setElegantTextHeight(boolean elegant) { + throw new RuntimeException("Stub!"); + } + + public float getTextSize() { + throw new RuntimeException("Stub!"); + } + + public void setTextSize(float textSize) { + throw new RuntimeException("Stub!"); + } + + public float getTextScaleX() { + throw new RuntimeException("Stub!"); + } + + public void setTextScaleX(float scaleX) { + throw new RuntimeException("Stub!"); + } + + public float getTextSkewX() { + throw new RuntimeException("Stub!"); + } + + public void setTextSkewX(float skewX) { + throw new RuntimeException("Stub!"); + } + + public float getLetterSpacing() { + throw new RuntimeException("Stub!"); + } + + public void setLetterSpacing(float letterSpacing) { + throw new RuntimeException("Stub!"); + } + + public float getWordSpacing() { + throw new RuntimeException("Stub!"); + } + + public void setWordSpacing(float wordSpacing) { + throw new RuntimeException("Stub!"); + } + + public String getFontFeatureSettings() { + throw new RuntimeException("Stub!"); + } + + public void setFontFeatureSettings(String settings) { + throw new RuntimeException("Stub!"); + } + + public String getFontVariationSettings() { + throw new RuntimeException("Stub!"); + } + + public boolean setFontVariationSettings(String fontVariationSettings) { + throw new RuntimeException("Stub!"); + } + + public int getStartHyphenEdit() { + throw new RuntimeException("Stub!"); + } + + public int getEndHyphenEdit() { + throw new RuntimeException("Stub!"); + } + + public void setStartHyphenEdit(int startHyphen) { + throw new RuntimeException("Stub!"); + } + + public void setEndHyphenEdit(int endHyphen) { + throw new RuntimeException("Stub!"); + } + + public float ascent() { + throw new RuntimeException("Stub!"); + } + + public float descent() { + throw new RuntimeException("Stub!"); + } + + public float getFontMetrics(FontMetrics metrics) { + throw new RuntimeException("Stub!"); + } + + public FontMetrics getFontMetrics() { + throw new RuntimeException("Stub!"); + } + + public int getFontMetricsInt(FontMetricsInt fmi) { + throw new RuntimeException("Stub!"); + } + + public FontMetricsInt getFontMetricsInt() { + throw new RuntimeException("Stub!"); + } + + public float getFontSpacing() { + throw new RuntimeException("Stub!"); + } + + public float measureText(char[] text, int index, int count) { + throw new RuntimeException("Stub!"); + } + + public float measureText(String text, int start, int end) { + throw new RuntimeException("Stub!"); + } + + public float measureText(String text) { + throw new RuntimeException("Stub!"); + } + + public float measureText(CharSequence text, int start, int end) { + throw new RuntimeException("Stub!"); + } + + public int breakText(char[] text, int index, int count, float maxWidth, float[] measuredWidth) { + throw new RuntimeException("Stub!"); + } + + public int breakText(CharSequence text, int start, int end, boolean measureForwards, float maxWidth, float[] measuredWidth) { + throw new RuntimeException("Stub!"); + } + + public int breakText(String text, boolean measureForwards, float maxWidth, float[] measuredWidth) { + throw new RuntimeException("Stub!"); + } + + public int getTextWidths(char[] text, int index, int count, float[] widths) { + throw new RuntimeException("Stub!"); + } + + public int getTextWidths(CharSequence text, int start, int end, float[] widths) { + throw new RuntimeException("Stub!"); + } + + public int getTextWidths(String text, int start, int end, float[] widths) { + throw new RuntimeException("Stub!"); + } + + public int getTextWidths(String text, float[] widths) { + throw new RuntimeException("Stub!"); + } + + public float getTextRunAdvances(@NonNull char[] chars, int index, int count, int contextIndex, int contextCount, boolean isRtl, @Nullable float[] advances, int advancesIndex) { + throw new RuntimeException("Stub!"); + } + + public int getTextRunCursor(@NonNull char[] text, int contextStart, int contextLength, boolean isRtl, int offset, int cursorOpt) { + throw new RuntimeException("Stub!"); + } + + public int getTextRunCursor(@NonNull CharSequence text, int contextStart, int contextEnd, boolean isRtl, int offset, int cursorOpt) { + throw new RuntimeException("Stub!"); + } + + public boolean hasGlyph(String string) { + throw new RuntimeException("Stub!"); + } + + public float getRunAdvance(char[] text, int start, int end, int contextStart, int contextEnd, boolean isRtl, int offset) { + throw new RuntimeException("Stub!"); + } + + public float getRunAdvance(CharSequence text, int start, int end, int contextStart, int contextEnd, boolean isRtl, int offset) { + throw new RuntimeException("Stub!"); + } + + public int getOffsetForAdvance(char[] text, int start, int end, int contextStart, int contextEnd, boolean isRtl, float advance) { + throw new RuntimeException("Stub!"); + } + + public int getOffsetForAdvance(CharSequence text, int start, int end, int contextStart, int contextEnd, boolean isRtl, float advance) { + throw new RuntimeException("Stub!"); + } + + public boolean equalsForTextMeasurement(@NonNull Paint other) { + throw new RuntimeException("Stub!"); + } + + public static enum Align { + LEFT, + CENTER, + RIGHT; + } + + public static enum Cap { + BUTT, + ROUND, + SQUARE; + } + + public static class FontMetrics { + public float ascent; + public float bottom; + public float descent; + public float leading; + public float top; + + public FontMetrics() { + throw new RuntimeException("Stub!"); + } + } + + public static class FontMetricsInt { + public int ascent; + public int bottom; + public int descent; + public int leading; + public int top; + + public FontMetricsInt() { + throw new RuntimeException("Stub!"); + } + + public String toString() { + throw new RuntimeException("Stub!"); + } + } + + public static enum Join { + MITER, + ROUND, + BEVEL; + } + + public static enum Style { + FILL, + STROKE, + FILL_AND_STROKE; + } +} diff --git a/team417/build.gradle b/team417/build.gradle index 4af9428b2251..253b6fbefaee 100644 --- a/team417/build.gradle +++ b/team417/build.gradle @@ -48,5 +48,5 @@ dependencies { implementation "com.acmerobotics.roadrunner:core:1.0.0" implementation "com.acmerobotics.roadrunner:actions:1.0.0" implementation "com.acmerobotics.dashboard:dashboard:0.5.1" - implementation 'org.team11260:fast-load:0.1.2' + implementation 'org.team11260:fast-load:0.1.4-beta1' } \ No newline at end of file diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/roadrunner/LoonyTune.java b/team417/src/main/java/org/firstinspires/ftc/team417/roadrunner/LoonyTune.java index c2fad068dc03..c7d92c737c65 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/roadrunner/LoonyTune.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/roadrunner/LoonyTune.java @@ -6,8 +6,8 @@ package org.firstinspires.ftc.team417.roadrunner; import static com.acmerobotics.roadrunner.Profiles.constantProfile; - import static java.lang.System.nanoTime; +import static java.lang.System.out; import android.annotation.SuppressLint; import android.util.Log; @@ -28,25 +28,22 @@ import com.acmerobotics.roadrunner.TimeProfile; import com.acmerobotics.roadrunner.TimeTrajectory; import com.acmerobotics.roadrunner.TimeTurn; -import com.acmerobotics.roadrunner.TrajectoryActionBuilder; import com.acmerobotics.roadrunner.TurnConstraints; import com.acmerobotics.roadrunner.Vector2d; import com.google.gson.Gson; import com.qualcomm.hardware.gobilda.GoBildaPinpointDriver; import com.qualcomm.hardware.lynx.LynxModule; import com.qualcomm.hardware.sparkfun.SparkFunOTOS; +import com.qualcomm.hardware.sparkfun.SparkFunOTOS.Pose2D; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorEx; import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.Gamepad; -import com.qualcomm.hardware.sparkfun.SparkFunOTOS.Pose2D; import com.qualcomm.robotcore.util.RobotLog; import com.wilyworks.common.WilyWorks; -import static java.lang.System.out; - import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.robotcore.external.matrices.VectorF; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; @@ -402,20 +399,12 @@ static void clearDashboardTelemetry() { */ class TuneParameters { MecanumDrive.Params params; - boolean passedWheelTest; - boolean passedTrackingTest; - boolean passedCompletionTest; // Get the tuning parameters from the current MecanumDrive object but the 'passed' state // from the saved state, if any: public TuneParameters(MecanumDrive drive) { this(drive, null); } public TuneParameters(MecanumDrive drive, TuneParameters savedParameters) { params = drive.PARAMS; - if (savedParameters != null) { - passedWheelTest = savedParameters.passedWheelTest; - passedTrackingTest = savedParameters.passedTrackingTest; - passedCompletionTest = savedParameters.passedCompletionTest; - } } // Return a deep-copy clone of the current Settings object: @@ -844,9 +833,6 @@ static String buttonString(String button) { static final String DPAD_UP_DOWN = buttonString(" DPAD \u2195 "); static final String GUIDE = buttonString("HOME \u2302"); - // Types of interactive PiD tuners: - enum PidTunerType { AXIAL, LATERAL, HEADING, ALL } - // Menu widgets for each of the tuners: enum Tuner { NONE(0), @@ -858,13 +844,14 @@ enum Tuner { SPIN(5), TRACKING_TEST(6), LATERAL_MULTIPLIER(7), - AXIAL_GAIN(8), - LATERAL_GAIN(9), - HEADING_GAIN(10), - COMPLETION_TEST(11), - RETUNE(12), + GAINS(8), +// AXIAL_GAIN(9), +// LATERAL_GAIN(10), +// HEADING_GAIN(11), + COMPLETION_TEST(9), + RETUNE(10), - COUNT(13); // Count of tuners + COUNT(11); // Count of tuners final int index; Tuner(int index) { this.index = index; } @@ -890,7 +877,7 @@ enum Tuner { AcceleratingStraightLineTuner acceleratingTuner = new AcceleratingStraightLineTuner(); InteractiveFeedForwardTuner feedForwardTuner = new InteractiveFeedForwardTuner(); LateralMultiplierTuner lateralMultiplierTuner = new LateralMultiplierTuner(); - InteractivePidTuner pidTuner = new InteractivePidTuner(); + GainsTuner gainsTuner = new GainsTuner(); // Constants: final Pose2d zeroPose = new Pose2d(0, 0, 0); @@ -963,9 +950,7 @@ void updateTunerDependencies(Tuner completedTuner) { } Tuner firstFailure; - if (!currentParameters.passedWheelTest) - firstFailure = Tuner.WHEEL_TEST; - else if (pushFailure) + if (pushFailure) firstFailure = Tuner.PUSH; else if (params.kS == 0 || params.kV == 0) firstFailure = Tuner.ACCELERATING; @@ -973,18 +958,10 @@ else if (params.kA == 0) firstFailure = Tuner.FEED_FORWARD; else if (spinFailure) firstFailure = Tuner.SPIN; - else if (!currentParameters.passedTrackingTest) - firstFailure = Tuner.TRACKING_TEST; else if (params.lateralInPerTick == 0 || params.lateralInPerTick == 1) firstFailure = Tuner.LATERAL_MULTIPLIER; - else if (params.axialGain == 0) - firstFailure = Tuner.AXIAL_GAIN; - else if (params.lateralGain == 0) - firstFailure = Tuner.LATERAL_GAIN; - else if (params.headingGain == 0) - firstFailure = Tuner.HEADING_GAIN; - else if (!currentParameters.passedCompletionTest) - firstFailure = Tuner.COMPLETION_TEST; + else if ((params.axialGain == 0) || (params.lateralGain == 0) || (params.headingGain == 0)) + firstFailure = Tuner.GAINS; else firstFailure = Tuner.COUNT; // Don't star the completion test if it already passed @@ -1132,7 +1109,7 @@ Pose2d getTargetPose() { void update() { Canvas canvas = io.canvas(Io.Background.GRID); canvas.setFill("#a0a0a0"); - canvas.fillText("Preview", -19, 5, "", 0, false); + canvas.fillText("Preview", -66, -20, "36px Arial", 0, false); sequentialAction.preview(canvas); canvas.setStroke("#000000"); // Black double time = time(); @@ -1216,8 +1193,9 @@ String testDistance(int distance) { // Return a string that represents the amount of clearance needed: /** @noinspection SameParameterValue*/ - String clearanceDistance(int distance) { - return String.format("%.0f tiles", Math.ceil(distance / 24.0)); + String clearanceDistance(double distance) { + int tiles = (int) Math.ceil(distance / 24.0); + return tiles == 1 ? "1 tile" : String.format("%d tiles", tiles); } // Run an Action but end it early if Cancel is pressed. @@ -1790,8 +1768,7 @@ void wheelTest() { "Test 'leftBack' wheel", // 2 "Test 'rightBack' wheel", // 3 "Test 'rightFront' wheel", // 4 - "Test all wheels by driving"}, // 5 - (passed)->currentParameters.passedWheelTest = passed); + "Test all wheels by driving"}); // 5 while (opModeIsActive()) { if (!screens.update()) @@ -1869,8 +1846,7 @@ void trackingTest() { double maxRotationalSpeed = 0; // Max rotational speed seen, radians/s Screens screens = new Screens(Tuner.TRACKING_TEST, - new String[]{"Preview", "Free drive", "Measure error", "Stats"}, - (passed)->currentParameters.passedTrackingTest = passed); + new String[]{"Preview", "Free drive", "Measure error", "Stats"}); while (opModeIsActive()) { if (!screens.update()) break; // ====> @@ -2100,7 +2076,7 @@ class PushTuner { // goBilda's odometry pods are 13.3 and 19.9 ticks-per-mm which works out to // 336.9 and 505.3 ticks-per-inch. The REV thru-bore encoder is 1891.9 inches- // per-tick using the OpenOdometry design. - final int MIN_TICKS_PER_INCH = 100; + final int MIN_TICKS_PER_INCH = 50; // Structure used to encapsulate an OTOS result from push tuning: class OtosPushResult extends Result { @@ -2294,9 +2270,6 @@ void tuneOtos() { } } } - - // Set/restore the hardware settings: - initializeTrackerHardware(); } // Push either forward or left, as requested by the caller: @@ -2458,6 +2431,9 @@ void tune() { tunePinpoint(); else tuneOtos(); + + // Set/restore the hardware settings: + initializeTrackerHardware(); } } @@ -3794,264 +3770,228 @@ void tune() { /** * Class to encapsulate all interactive PID tuning logic. */ - class InteractivePidTuner { - final int DISTANCE = 48; // Test distance in inches - String errorSummary; // String describing the current amount of error - double maxAxialError; // Maximum error accumulated over the current robot run - double maxLateralError; - double maxHeadingError; - - // Run the tuning update: - boolean run(PidTunerType type) { - // Execute the trajectory: - boolean more = drive.doActionsWork(io.packet); - if (!more) - io.abortCanvas(); // Do this to keep the last frame shown - - // Update the error summary if we're actively running a trajectory, or if it's - // previously been updated: - if ((more) || !errorSummary.isEmpty()) { - // Compute the errors: - Point errorVector = new Point(drive.pose.position.x, drive.pose.position.y).subtract( - new Point(drive.targetPose.position.x, drive.targetPose.position.y)); - double errorTheta = errorVector.atan2() - drive.targetPose.heading.toDouble(); - double errorLength = errorVector.length(); - double axialError = errorLength * Math.cos(errorTheta); - double lateralError = errorLength * Math.sin(errorTheta); - double headingError = normalizeAngle(drive.pose.heading.toDouble() - - drive.targetPose.heading.toDouble()); - - maxAxialError = Math.max(maxAxialError, axialError); - maxLateralError = Math.max(maxLateralError, lateralError); - maxHeadingError = Math.max(maxHeadingError, headingError); - - errorSummary = "Max gain error: "; - if (type == PidTunerType.AXIAL) - errorSummary += String.format("%.2f\"", maxAxialError); - else if (type == PidTunerType.LATERAL) - errorSummary += String.format("%.2f\"", maxLateralError); - else if (type == PidTunerType.HEADING) - errorSummary += String.format("%.2f\u00b0", Math.toDegrees(maxHeadingError)); - else - errorSummary += String.format("%.2f\", %.2f\", %.2f\u00b0", - maxAxialError, maxLateralError, Math.toDegrees(maxHeadingError)); - errorSummary += ""; - - if (!more) { - errorSummary += (type == PidTunerType.ALL) ? "\n" : ", "; - errorSummary += "End error: "; - if (type == PidTunerType.AXIAL) - errorSummary += String.format("%.2f\"", axialError); - else if (type == PidTunerType.LATERAL) - errorSummary += String.format("%.2f\"", lateralError); - else if (type == PidTunerType.HEADING) - errorSummary += String.format("%.2f\u00b0", Math.toDegrees(headingError)); - else - errorSummary += String.format("%.2f\", %.2f\", %.2f\u00b0", - axialError, lateralError, Math.toDegrees(headingError)); - errorSummary += ""; - } - errorSummary += "\n\n"; - } - return more; - } + class GainsTuner { + final double LENGTH = 48; // Tuning test length in inches + final double ERROR_DISTANCE = 6; // Tuning initial displacement // Copy the relevant fields for this test when saving the state: - void sync(PidTunerType type, TuneParameters destination, TuneParameters source) { - if (type == PidTunerType.AXIAL) { - destination.params.axialGain = source.params.axialGain; - destination.params.axialVelGain = source.params.axialVelGain; - } else if (type == PidTunerType.LATERAL) { - destination.params.lateralGain = source.params.lateralGain; - destination.params.lateralVelGain = source.params.lateralVelGain; - } else if (type == PidTunerType.HEADING) { - destination.params.headingGain = source.params.headingGain; - destination.params.headingVelGain = source.params.headingVelGain; - } else { // All case - destination.params.axialGain = source.params.axialGain; - destination.params.axialVelGain = source.params.axialVelGain; - destination.params.lateralGain = source.params.lateralGain; - destination.params.lateralVelGain = source.params.lateralVelGain; - destination.params.headingGain = source.params.headingGain; - destination.params.headingVelGain = source.params.headingVelGain; - } + void sync(TuneParameters destination, TuneParameters source) { + destination.params.axialGain = source.params.axialGain; + destination.params.lateralGain = source.params.lateralGain; + destination.params.headingGain = source.params.headingGain; + destination.params.axialVelGain = source.params.axialVelGain; + destination.params.lateralVelGain = source.params.lateralVelGain; + destination.params.headingVelGain = source.params.headingVelGain; } - void tune(PidTunerType type) { + // Tune the gains: + void tune() { configureToDrive(true); // Do use MecanumDrive - errorSummary = ""; - TuneParameters testParameters = currentParameters.createClone(); + + // Set the current params to our clone and add 30 seconds to every trajectory + // duration: MecanumDrive.PARAMS = testParameters.params; - String overview; - String clearance; - String[] gainNames; - Tuner tuner; - - TrajectoryActionBuilder trajectory = drive.actionBuilder(zeroPose); - Action preview; - String adjective; - if (type == PidTunerType.AXIAL) { - overview = "The robot will drive forward then backward " + testDistance(DISTANCE) + ". " - + "Tune these gains to reduce the forward/backward error between target and actual position:\n" - + "\n" - + "\u2022 axialGain sets the magnitude of response to the error. " - + "A higher value more aggressively corrects but can cause overshoot.\n" - + "\u2022 axialVelGain is a damper and can reduce overshoot and oscillation.\n"; - clearance = "ensure "+ clearanceDistance(DISTANCE) + " forward clearance"; - adjective = "axially "; - gainNames = new String[] { "axialGain", "axialVelGain" }; - tuner = Tuner.AXIAL_GAIN; - trajectory = trajectory.lineToX(DISTANCE).lineToX(0); - preview = drive.actionBuilder(new Pose2d(-DISTANCE / 2.0, 0, 0)) - .lineToX(DISTANCE / 2.0) - .lineToX(-DISTANCE / 2.0) - .build(); - - } else if (type == PidTunerType.LATERAL) { - overview = "The robot will strafe left and then right " + testDistance(DISTANCE) + ". " - + "Tune these gains to reduce the lateral error between target and actual positions:\n" - + "\n" - + "\u2022 lateralGain sets the magnitude of response to the error. " - + "A higher value more aggressively corrects but can cause overshoot.\n" - + "\u2022 lateralVelGain is a damper and can reduce overshoot and oscillation.\n"; - clearance = "ensure " + clearanceDistance(DISTANCE) + " clearance to the left"; - adjective = "laterally "; - gainNames = new String[] { "lateralGain", "lateralVelGain" }; - tuner = Tuner.LATERAL_GAIN; - trajectory = trajectory.strafeTo(new Vector2d(0, DISTANCE)).strafeTo(new Vector2d(0, 0)); - preview = drive.actionBuilder(new Pose2d(0, -DISTANCE / 2.0, 0)) - .strafeTo(new Vector2d(0, DISTANCE / 2.0)) - .strafeTo(new Vector2d(0, -DISTANCE / 2.0)) - .build(); - - } else if (type == PidTunerType.HEADING) { - overview = "The robot will rotate in place 180\u00b0. Tune these gains to reduce the " - + "error between target and actual headings:\n" - + "\n" - + "\u2022 headingGain sets the magnitude of response to the error. " - + "A higher value more aggressively corrects but can cause overshoot.\n" - + "\u2022 headingVelGain is a damper and can reduce overshoot and oscillation.\n"; - clearance = "ensure enough clearance to spin"; - adjective = "rotationally "; - gainNames = new String[] { "headingGain", "headingVelGain" }; - tuner = Tuner.HEADING_GAIN; - trajectory = trajectory.turn(Math.PI).turn(-Math.PI); - preview = drive.actionBuilder(zeroPose) - .turn(Math.PI) - .turn(-Math.PI) - .build(); - } else { - overview = "The robot will drive forward then backward " + testDistance(DISTANCE) - + " while turning. Tune the gains as appropriate.\n"; - clearance = "ensure sufficient clearance"; - adjective = ""; - gainNames = new String[] { "axialGain", "axialVelGain", "lateralGain", "lateralVelGain", "headingGain", "headingVelGain" }; - tuner = Tuner.NONE; - trajectory = trajectory - .lineToXLinearHeading(DISTANCE, Math.PI) - .endTrajectory() // Stop at the end of the line - .setTangent(Math.PI) // When we start again, go in the direction of 180 degrees - .lineToXLinearHeading(0, 0); - preview = trajectory.build(); - } + Screens screens = new Screens(Tuner.GAINS, new String[]{"Preview", "Lateral", "Axial", "Heading", "Test"}, this::sync); ArrayList inputs = new ArrayList<>(); - ArrayList screenNames = new ArrayList<>(); - screenNames.add("Preview"); - for (String gainName: gainNames) { - screenNames.add(String.format("Tune %s", gainName)); - inputs.add(new NumericInput(drive.PARAMS, gainName, -1, 2, 0, 20)); + for (String gain: new String[]{"lateralGain", "lateralVelGain", "axialGain", "axialVelGain", "headingGain", "headingVelGain"}) { + inputs.add(new NumericInput(drive.PARAMS, gain, -1, 2, 0, 20)); } - TrajectoryPreviewer previewer = new TrajectoryPreviewer(io, preview); + // Create the test trajectory as a lambda to allow easy recreation: + final int TEST_DIMENSION = 24; + Pose2d testPose = new Pose2d(0, 0, Math.PI/2); + Supplier testSupplier = () -> drive.actionBuilder(testPose) + .strafeTo(new Vector2d(0, TEST_DIMENSION)) + .strafeTo(new Vector2d(-TEST_DIMENSION, TEST_DIMENSION)) + .turnTo(-Math.PI/2) + .strafeTo(new Vector2d(-TEST_DIMENSION, 0)) + .strafeTo(new Vector2d(0, 0)) + .turnTo(Math.PI/2) + .build(); + Action testTrajectory = testSupplier.get(); + TrajectoryPreviewer testPreviewer = null; + + // Tuning parameters: + boolean running = false; + boolean returning = false; + boolean tuningVelGain = false; + Action tuningTrajectory = null; + Pose2d tuningStartPose = null; + TrajectoryPreviewer tuningPreviewer = null; + String clearance = null; - int queuedStarts = 0; - Screens screens = new Screens(tuner, screenNames.toArray(new String[0]), (a, b)->sync(type, a, b)); while (opModeIsActive()) { if (!screens.update()) break; // ====> io.begin(); io.out(screens.header); + + if (screens.switched) { + drive.abortActions(); + stopMotors(); + returning = false; + tuningTrajectory = null; // Reset the tuning trajectory + testPreviewer = null; // Reset the preview + } + if (screens.index == 0) { // Preview screen - previewer.update(); // Animate the trajectory preview - updateGamepadDriving(); // Let the user drive - io.out(overview); - io.out("\n" - + "These are essentially the P and D (proportional and " - + "differential) terms for a PID control system.\n" - + "\n" - + "Press " + screens.buttons + "."); + updateGamepadDriving(); + io.out("This tuner sets the lateral, axial and header gains. " + + "The gains control how hard the robot tries to " + + "correct its movement when the odometry indicates that it's " + + "off-course. The gains are basically P components of three different " + + "PID controllers.\n\n" + + "Each screen will tune a different gain, plus there is a screen " + + "for testing the gains with a sample trajectory.\n\n" + + "Pay attention to the clearance needed on each screen.\n\n"); + io.out("Press " + screens.buttons + "."); + io.end(); + } else if (screens.index == 4) { + drive.setDurationExtension(0); // Disable extension + if (testPreviewer == null) { + testPreviewer = new TrajectoryPreviewer(io, testTrajectory); + } + testPreviewer.update(); + updateGamepadDriving(); + io.out("This screen tests the gains with a sample trajectory.\n\n"); + io.out("Ensure there's enough clearance for the robot to drive "); + io.out("one full tile forward and one full tile to the left.\n\n"); + io.out("Press " + A + " to start the robot, " + screens.buttons + "."); io.end(); - } else { // Tune screens - if (screens.switched) - errorSummary = ""; - io.canvas(Io.Background.GRID); // Clear the field - int index = screens.index - 1; - NumericInput input = inputs.get(index); - io.out(" %s: %s\n", input.fieldName, input.update()); - - if ((index & 1) == 0) { // Tuning a proportional gain - io.out("\n"+ errorSummary); - io.out("Increase the gain to make the circles %scoincident and to minimize " - + "the maximum and final error. ", adjective); - io.out("Green is target, blue is actual. "); - io.out("Don't increase so much that the robot has " - + "significant shaking or oscillation. "); - io.out("(A small amount can be corrected by adjusting the corresponding " - + "velocity gain.) "); - } else { // Tuning a derivative gain - io.out(" Don't exceed %.2f (\u2153 the other gain)\n", // One third - 0.33 * inputs.get(index ^1).value); - io.out("\n"+ errorSummary + ""); - io.out("Increase the velocity gain to dampen oscillation " - + "and shaking, but not so much that it makes it worse. "); + if (io.ok()) { + stopMotors(); + drive.setPose(testPose); + runCancelableAction(screens.header, testTrajectory); + // Road Runner requires us to regenerate the trajectory: + testTrajectory = testSupplier.get(); + } + } else { + drive.setDurationExtension(5); // Enable extension + + int test = screens.index - 1; // 0 = axial, 1 = lateral, 2 = heading + if (test <= 1) { + io.out("The robot will start out displaced from its intended trajectory. " + + "Increase the positional gain until the robot moves to its intended trajectory " + + "in a timely fashion, as shown in FTC Dashboard. " + + "Don't increase the gain so much that it causes " + + "excessive overshoot and oscillation.\n\n" + + "Increase the velocity gain if the positional gain is too high " + + "at the start.\n\n"); + } else { + io.out("The robot will start out at a displaced angle from its intended " + + "trajectory. Increase the gain until the blue (actual) heading " + + "indicator catches up with the green (target) heading. " + + "Don't increase the gain so much that it causes " + + "excessive overshoot and oscillation.\n\n"); } - io.out("Press "+ DPAD_UP_DOWN + " to change the value, " + DPAD_LEFT_RIGHT - + " to move the cursor.\n\n"); - if (io.start()) - queuedStarts++; + if (!tuningVelGain) { + io.out(X + ": Positional gain: %s\n", inputs.get(2*test).update()); + io.out(Y + ": Velocity gain: %.2f\n\n", inputs.get(2*test + 1).value); + } else { + io.out(X + ": Positional gain: %.2f\n", inputs.get(2*test).value); + io.out(Y + ": Velocity gain: %s\n\n", inputs.get(2*test + 1).update()); + } + + if (io.x()) { + tuningVelGain = false; + } else if (io.y()) { + tuningVelGain = true; + } - // Continue the trajectory, if any: - if (run(type)) { - io.out("Press " + B + " to cancel and stop the robot."); + if (running) { + io.canvas(Io.Background.BLANK); // Clear the field + running = drive.doActionsWork(io.packet); + if (!running) { + io.abortCanvas(); // Do this to keep the last frame shown + tuningTrajectory = null; + } + + io.out("Press " + B + " when the robot has stopped moving, or to cancel."); io.end(); if (io.cancel()) { // Cancel the current cycle but remain in this test: drive.abortActions(); - queuedStarts = 0; } } else { updateGamepadDriving(); // Let the user drive + if (tuningTrajectory == null) { + if (test == 0) { + // Forward/up, with the robot to the right of the intended line: + if (!returning) { + tuningStartPose = new Pose2d(ERROR_DISTANCE, -LENGTH / 2, Math.PI / 2); + tuningTrajectory = drive.actionBuilder( + new Pose2d(0, -LENGTH / 2, Math.PI / 2)) + .lineToY(LENGTH / 2).build(); + clearance = clearanceDistance(LENGTH) + " clearance ahead, " + + clearanceDistance(ERROR_DISTANCE) + " clearance to left"; + } else { + tuningStartPose = new Pose2d(-ERROR_DISTANCE, LENGTH / 2, Math.PI / 2); + tuningTrajectory = drive.actionBuilder( + new Pose2d(0, LENGTH / 2, Math.PI / 2)) + .lineToY(-LENGTH / 2).build(); + clearance = clearanceDistance(LENGTH) + " clearance behind, " + + clearanceDistance(ERROR_DISTANCE) + " clearance to right"; + } + } else if (test == 1) { + if (!returning) { + tuningStartPose = new Pose2d(LENGTH / 2, -ERROR_DISTANCE, Math.PI / 2); + tuningTrajectory = drive.actionBuilder( + new Pose2d(LENGTH / 2, 0, Math.PI / 2)) + .strafeTo(new Vector2d(-LENGTH / 2, 0)).build(); + clearance = clearanceDistance(LENGTH) + " clearance to left, " + + clearanceDistance(ERROR_DISTANCE) + " clearance ahead"; + } else { + tuningStartPose = new Pose2d(-LENGTH / 2, ERROR_DISTANCE, Math.PI / 2); + tuningTrajectory = drive.actionBuilder( + new Pose2d(-LENGTH / 2, 0, Math.PI / 2)) + .strafeTo(new Vector2d(LENGTH / 2, 0)).build(); + clearance = clearanceDistance(LENGTH) + " clearance to right, " + + clearanceDistance(ERROR_DISTANCE) + " clearance behind"; + } + } else { + double heading = drive.pose.heading.toDouble(); + tuningStartPose = new Pose2d(0, 0, heading); + clearance = "clearance for the robot to rotate in place"; + if (!returning) { + tuningTrajectory = drive.actionBuilder( + new Pose2d(0, 0, heading + Math.PI / 2)) + .turnTo(heading + Math.PI).build(); + } else { + tuningTrajectory = drive.actionBuilder( + new Pose2d(0, 0, heading - Math.PI / 2)) + .turnTo(heading - Math.PI).build(); + } + } - io.out("Press "+ A + " to start the robot (%s), %s.", clearance, screens.buttons); + tuningPreviewer = new TrajectoryPreviewer(io, tuningTrajectory); + } + io.out("Ensure " + clearance + ".\n\n"); + io.out("Press " + A + " to start the robot, %s.", screens.buttons); + tuningPreviewer.update(); io.end(); - if (io.ok()) - queuedStarts++; - - if (queuedStarts > 0) { + if (io.ok()) { // Kick off a new run: - queuedStarts--; stopMotors(); // Stop the user's driving - drive.setPose(zeroPose); - if (type == PidTunerType.HEADING) { - // An apparent Road Runner bug prevents a turn trajectory from being reused: - drive.runParallel(drive.actionBuilder(zeroPose).turn(Math.PI).turn(-Math.PI).build()); - } else { - drive.runParallel(trajectory.build()); - } - maxAxialError = 0; - maxHeadingError = 0; - maxLateralError = 0; + + // Start the action: + drive.setPose(tuningStartPose); + drive.runParallel(tuningTrajectory); + running = true; + returning = !returning; } } } } + + // Restore the state: MecanumDrive.PARAMS = currentParameters.params; + drive.setDurationExtension(0); } } @@ -4071,7 +4011,6 @@ void completionTest() { .splineToLinearHeading(new Pose2d(0, 0, Math.toRadians(-0.0001)), Math.toRadians(-180)) .build(), message, clearance); - currentParameters.passedCompletionTest = true; currentParameters.save(); updateTunerDependencies(Tuner.COMPLETION_TEST); } @@ -4288,9 +4227,7 @@ public void runOpMode() { addTuner(Tuner.SPIN, spinTuner::tune, spinDescription); addTuner(Tuner.TRACKING_TEST, this::trackingTest, "Tracking test (sensor verification)"); addTuner(Tuner.LATERAL_MULTIPLIER, lateralMultiplierTuner::tune, "Lateral tuner (lateralInPerTick)"); - addTuner(Tuner.AXIAL_GAIN, ()-> pidTuner.tune(PidTunerType.AXIAL), "Interactive PiD tuner (axial gains)"); - addTuner(Tuner.LATERAL_GAIN, ()-> pidTuner.tune(PidTunerType.LATERAL), "Interactive PiD tuner (lateral gains)"); - addTuner(Tuner.HEADING_GAIN, ()-> pidTuner.tune(PidTunerType.HEADING), "Interactive PiD tuner (heading gains)"); + addTuner(Tuner.GAINS, gainsTuner::tune, "Gains tuner (axial, lateral, heading)"); addTuner(Tuner.COMPLETION_TEST, this::completionTest, "Completion test (overall verification)"); addTuner(Tuner.RETUNE, this::retuneDialog, "Re-tune"); @@ -4304,7 +4241,6 @@ public void runOpMode() { updateTunerDependencies(Tuner.NONE); // Add more options if tuning is complete: - addUnlockable(() -> pidTuner.tune(PidTunerType.ALL), "More::Interactive PiD tuner (all gains)"); addUnlockable(this::rotationTest, "More::Rotation test (verify trackWidthTicks)"); addUnlockable(this::splineExample, "Examples::Spline"); addUnlockable(this::lineToTurnExample, "Examples::LineTo/Turn example"); @@ -4315,4 +4251,7 @@ public void runOpMode() { io.message(menu.update()); } } + + // @@@ Override lateralInPerTick and trackWidthTicks if zero + // @@@ Enforce inPerTick = 1.0000 } \ No newline at end of file diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/roadrunner/MecanumDrive.java b/team417/src/main/java/org/firstinspires/ftc/team417/roadrunner/MecanumDrive.java index 9e2ee55fe76a..2f35e8b8f33f 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/roadrunner/MecanumDrive.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/roadrunner/MecanumDrive.java @@ -21,9 +21,9 @@ import com.acmerobotics.roadrunner.MecanumKinematics; import com.acmerobotics.roadrunner.MinVelConstraint; import com.acmerobotics.roadrunner.MotorFeedforward; -import com.acmerobotics.roadrunner.PoseMap; import com.acmerobotics.roadrunner.Pose2d; import com.acmerobotics.roadrunner.Pose2dDual; +import com.acmerobotics.roadrunner.PoseMap; import com.acmerobotics.roadrunner.PoseVelocity2d; import com.acmerobotics.roadrunner.PoseVelocity2dDual; import com.acmerobotics.roadrunner.ProfileAccelConstraint; @@ -58,7 +58,6 @@ import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.IMU; import com.qualcomm.robotcore.hardware.VoltageSensor; - import com.wilyworks.common.WilyWorks; import org.firstinspires.ftc.robotcore.external.Telemetry; @@ -229,6 +228,7 @@ public static String getBotName() { public Pose2d targetPose; // Target pose when actively traversing a trajectory public SparkFunOTOS otosDriver; // Can be null which means no OTOS public GoBildaPinpointDriver pinpointDriver; // Can be null which means no Pinpoint + public double durationExtension; // Seconds to extend the duration of a trajectory, usually 0 public double lastLinearGainError = 0; // Most recent gain error in inches and radians public double lastHeadingGainError = 0; @@ -639,7 +639,7 @@ public boolean run(@NonNull TelemetryPacket p) { t = Actions.now() - beginTs; } - if (t >= timeTrajectory.duration) { + if (t >= timeTrajectory.duration + durationExtension) { leftFront.setPower(0); leftBack.setPower(0); rightBack.setPower(0); @@ -738,7 +738,7 @@ public boolean run(@NonNull TelemetryPacket p) { t = Actions.now() - beginTs; } - if (t >= turn.duration) { + if (t >= turn.duration + durationExtension) { leftFront.setPower(0); leftBack.setPower(0); rightBack.setPower(0); @@ -1055,4 +1055,10 @@ public static TelemetryPacket getTelemetryPacket(boolean showField) { public static void sendTelemetryPacket(TelemetryPacket packet) { FtcDashboard.getInstance().sendTelemetryPacket(packet); } + + // Give extra time at the end of the trajectory for the PID to get the robot into exactly + // the right position: + public void setDurationExtension(double seconds) { + durationExtension = seconds; + } } diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/utils/ConfigurationTester.java b/team417/src/main/java/org/firstinspires/ftc/team417/utils/ConfigurationTester.java index 1444cd865b8d..f4fa3c5f565a 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/utils/ConfigurationTester.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/utils/ConfigurationTester.java @@ -69,17 +69,17 @@ private static String repeat(int count, String string) { return result.toString(); } - // Set the foreground font color for a string. Color must be in the format" #dc3545". + // Set the foreground font color for a string. Color must be in the format "#00ff00". public static String color(String color, String string) { return "" + string + ""; } - // Set the background color for a string. Color must be in the format" #dc3545". + // Set the background color for a string. Color must be in the format "#ff0000". public static String background(String backgroundColor, String string) { return "" + string + ""; } - // Set the foreground and background colors for a string. Colors must be in the format" #dc3545". + // Set the foreground and background colors for a string. Colors must be in the format "#0000ff". public static String colors(String foregroundColor, String backgroundColor, String string) { return "" + string + ""; } From aa6b07cf3a44a88e62500906cf64548985131f62 Mon Sep 17 00:00:00 2001 From: Emmett Date: Sun, 7 Dec 2025 13:27:21 -0800 Subject: [PATCH 133/198] changed transfer positions --- .../java/org/firstinspires/ftc/team417/ComplexMechGlob.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java b/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java index d79a19469fab..63d78b6b775e 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java @@ -79,8 +79,8 @@ public class ComplexMechGlob extends MechGlob { //a class encompassing all code static double FAR_FLYWHEEL_VELOCITY = 1500; static double NEAR_FLYWHEEL_VELOCITY = 1500; static double FLYWHEEL_BACK_SPIN = 300; - static double TRANSFER_INACTIVE_POSITION = 0; - static double TRANSFER_ACTIVE_POSITION = 1; + static double TRANSFER_INACTIVE_POSITION = 0.45; + static double TRANSFER_ACTIVE_POSITION = 0.71; static double REVERSE_INTAKE_SPEED = -1; static double INTAKE_SPEED = 1; static double FLYWHEEL_VELOCITY_TOLERANCE = 25; //this is an epsiiiiiiiiilon From 3f307a909d89de3b03c9a1944f3db8e7b231a3d2 Mon Sep 17 00:00:00 2001 From: Sameer Date: Sun, 7 Dec 2025 14:10:27 -0800 Subject: [PATCH 134/198] rename countballs to getcolor, Spin up action removed and put at start. --- .../ftc/team417/CompetitionAuto.java | 83 +++++++++---------- 1 file changed, 41 insertions(+), 42 deletions(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java index 17e9f1e12b13..790590d02273 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java @@ -9,7 +9,7 @@ import com.acmerobotics.roadrunner.TrajectoryActionBuilder; import com.acmerobotics.roadrunner.Vector2d; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -import com.qualcomm.robotcore.robot.Robot; + import org.firstinspires.ftc.team417.apriltags.LimelightDetector; import org.firstinspires.ftc.team417.apriltags.Pattern; @@ -47,10 +47,9 @@ enum SlowBotMovement { TextMenu menu = new TextMenu(); MenuInput menuInput = new MenuInput(MenuInput.InputType.CONTROLLER); Pattern pattern; - CountBalls countBalls = new CountBalls(); - public Action getPath(SlowBotMovement chosenMovement, Alliance chosenAlliance, double intakeCycles, MecanumDrive drive, MechGlob mechGlob) { + public Action getPath(SlowBotMovement chosenMovement, Alliance chosenAlliance, double intakeCycles, MecanumDrive drive, MechGlob mechGlob, GetColor countBalls) { Pose2d startPose = new Pose2d(0, 0, 0); @@ -74,7 +73,7 @@ public Action getPath(SlowBotMovement chosenMovement, Alliance chosenAlliance, d trajectoryAction = drive.actionBuilder(SBNearStartPose, poseMap); trajectoryAction = trajectoryAction.setTangent(Math.toRadians(-51)) .splineToConstantHeading(new Vector2d(-36, 36), Math.toRadians(-51)) - .stopAndAdd(new LaunchAction(mechGlob, pattern, countBalls)) + .stopAndAdd(new LaunchAction(mechGlob, countBalls)) .setTangent(Math.toRadians(0)) .splineToSplineHeading(new Pose2d(-12, 32, Math.toRadians(90)), Math.toRadians(90)) //go to intake closest from goal .afterDisp(0, new IntakeAction(mechGlob, 1)) @@ -83,7 +82,7 @@ public Action getPath(SlowBotMovement chosenMovement, Alliance chosenAlliance, d .afterDisp(0, new IntakeAction(mechGlob, 0)) .setTangent(Math.toRadians(-90)) .splineToSplineHeading(new Pose2d(-36, 36, Math.toRadians(139)), Math.toRadians(180)) //go to launch position - .stopAndAdd(new LaunchAction(mechGlob, pattern, countBalls)); + .stopAndAdd(new LaunchAction(mechGlob, countBalls)); if (intakeCycles > 1) { trajectoryAction = trajectoryAction.setTangent(Math.toRadians(0)) @@ -180,6 +179,7 @@ public void runOpMode() { MechGlob mechGlob = ComplexMechGlob.create(hardwareMap, telemetry, false); + // Text menu for FastBot @@ -213,10 +213,16 @@ public void runOpMode() { telemetry.update(); } + GetColor countBalls = new GetColor(pattern); Alliance chosenAlliance = menu.getResult(Alliance.class, "alliance-picker-1"); SlowBotMovement chosenMovement = menu.getResult(SlowBotMovement.class, "movement-picker-1"); double waitTime = menu.getResult(Double.class, "wait-slider-1"); double intakeCycles = menu.getResult(Double.class, "intake-slider"); + if (chosenMovement == SlowBotMovement.NEAR) { + mechGlob.setLaunchVelocity(LaunchDistance.NEAR); + } else { + mechGlob.setLaunchVelocity(LaunchDistance.FAR); + } // the first parameter is the type to return as @@ -240,7 +246,7 @@ public void runOpMode() { drive.setPose(SBFarStartPose); break; } - trajectoryAction = getPath(chosenMovement, chosenAlliance, intakeCycles, drive, mechGlob); + trajectoryAction = getPath(chosenMovement, chosenAlliance, intakeCycles, drive, mechGlob, countBalls); // Get a preview of the trajectory's path: Canvas previewCanvas = new Canvas(); @@ -254,7 +260,7 @@ public void runOpMode() { // Assume unknown pattern unless detected otherwise. pattern = Pattern.UNKNOWN; - + pattern = Pattern.PPG; //temporary until hankang limelight // Detect the pattern with the AprilTags from the camera! // Wait for Start to be pressed on the Driver Hub! // (This try-with-resources statement automatically calls detector.close() when it exits @@ -306,19 +312,12 @@ public void runOpMode() { class LaunchAction extends RobotAction { MechGlob mechGlob; Pattern pattern; - CountBalls orderCount; - RequestedColor[] array; - public LaunchAction(MechGlob mechGlob, Pattern pattern, CountBalls orderCount) { + GetColor orderCount; + + public LaunchAction(MechGlob mechGlob, GetColor orderCount) { this.mechGlob = mechGlob; - this.pattern = pattern; + this.pattern = Pattern.PPG; this.orderCount = orderCount; - if (pattern == Pattern.GPP) { - array = new RequestedColor[] {RequestedColor.GREEN, RequestedColor.PURPLE, RequestedColor.PURPLE}; - } else if (pattern == Pattern.PGP) { - array = new RequestedColor[] {RequestedColor.PURPLE, RequestedColor.GREEN, RequestedColor.PURPLE}; - } else { - array = new RequestedColor[] {RequestedColor.PURPLE, RequestedColor.PURPLE, RequestedColor.GREEN}; - } @@ -327,45 +326,29 @@ public LaunchAction(MechGlob mechGlob, Pattern pattern, CountBalls orderCount) { @Override public boolean run(double elapsedTime) { if (elapsedTime == 0) { - RequestedColor requestedColor = array[orderCount.orderCount]; - mechGlob.launch(requestedColor); + mechGlob.launch(orderCount.getColor()); orderCount.increment(); - requestedColor = array[orderCount.orderCount]; - mechGlob.launch(requestedColor); + mechGlob.launch(orderCount.getColor()); orderCount.increment(); - requestedColor = array[orderCount.orderCount]; - mechGlob.launch(requestedColor); + mechGlob.launch(orderCount.getColor()); orderCount.increment(); } return !mechGlob.isDoneLaunching(); //we are done } } -class SpinUpAction extends RobotAction { - MechGlob mechGlob; - LaunchDistance launchDistance; - public SpinUpAction(MechGlob mechglob, LaunchDistance launchDistance) { - this.mechGlob = mechglob; - this.launchDistance = launchDistance; - } - @Override - public boolean run(double elapsedTime) { - mechGlob.setLaunchVelocity(launchDistance); - return false; - } -} class PreLaunchAction extends RobotAction { MechGlob mechGlob; - RequestedColor requestedColor; - public PreLaunchAction(MechGlob mechGlob, RequestedColor requestedColor) { - this.requestedColor = requestedColor; + GetColor orderCount; + public PreLaunchAction(MechGlob mechGlob, GetColor orderCount) { + this.orderCount = orderCount; this.mechGlob = mechGlob; } @Override public boolean run(double elapsedTime) { - return mechGlob.preLaunch(requestedColor); + return mechGlob.preLaunch(orderCount.getColor()); } } @@ -386,8 +369,20 @@ public boolean run(double elapsedTime) { return elapsedTime < 3; } } -class CountBalls { +class GetColor { public int orderCount; // 0, 1 or 2 to find color pattern + public RequestedColor[] array; + public GetColor(Pattern pattern) { + if (pattern == Pattern.GPP) { + array = new RequestedColor[] {RequestedColor.GREEN, RequestedColor.PURPLE, RequestedColor.PURPLE}; + } else if (pattern == Pattern.PGP) { + array = new RequestedColor[] {RequestedColor.PURPLE, RequestedColor.GREEN, RequestedColor.PURPLE}; + } else { + array = new RequestedColor[] {RequestedColor.PURPLE, RequestedColor.PURPLE, RequestedColor.GREEN}; + } + orderCount = 0; + + } public void increment() { if (orderCount == 2) { orderCount = 0; @@ -396,6 +391,10 @@ public void increment() { } } + + public RequestedColor getColor() { + return array[orderCount]; + } } From d78b8235cec91658ccaad1b4154d6c8d09fdf66b Mon Sep 17 00:00:00 2001 From: anya-codes Date: Sun, 7 Dec 2025 15:54:53 -0800 Subject: [PATCH 135/198] Removed SensorColor because it was just a sample. Finished auto aim code and it works! Updated distance value for CoolColorDetector --- .../ftc/team417/CompetitionTeleOp.java | 31 ++- .../ftc/team417/ComplexMechGlob.java | 11 +- .../ftc/team417/CoolColorDetector.java | 16 +- .../ftc/team417/SensorColor.java | 235 ------------------ 4 files changed, 48 insertions(+), 245 deletions(-) delete mode 100644 team417/src/main/java/org/firstinspires/ftc/team417/SensorColor.java diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java index 4addeaa7d947..027e26bb439c 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java @@ -45,17 +45,30 @@ public void runOpMode() { Pose2d beginPose = new Pose2d(0, 0, 0); MecanumDrive drive = new MecanumDrive(hardwareMap, telemetry, gamepad1, beginPose); MechGlob mechGlob = ComplexMechGlob.create(hardwareMap, telemetry, false); + AmazingAutoAim amazingAutoAim = null; telemetry.setDisplayFormat(Telemetry.DisplayFormat.HTML); - + //Variable for auto aim + double amountToTurn; // Initialize motors, servos, LEDs // Wait for Start to be pressed on the Driver Hub! waitForStart(); + while (opModeIsActive()) { telemetry.addLine("Running TeleOp!"); + if (gamepad1.rightBumperWasPressed()) { + amazingAutoAim = new AmazingAutoAim(telemetry, CompetitionAuto.Alliance.BLUE); + } + + if (gamepad1.right_bumper) { + amountToTurn = -amazingAutoAim.get(drive.pose); + } else { + amountToTurn = halfLinearHalfCubic(-gamepad1.right_stick_x); + } + // Set the drive motor powers according to the gamepad input: drive.setDrivePowers(new PoseVelocity2d( new Vector2d( @@ -63,8 +76,7 @@ public void runOpMode() { halfLinearHalfCubic(-gamepad1.left_stick_x * doSLOWMODE()) ), - halfLinearHalfCubic(-gamepad1.right_stick_x) - + amountToTurn )); @@ -170,12 +182,23 @@ public double get(Pose2d pose) { double angle = beta - alpha; double normalizedAngle = AngleUnit.normalizeRadians(angle); - return pid.calculate(normalizedAngle); + double pidOutput = pid.calculate(normalizedAngle); + + if (pidOutput <= -1) { + return -1; + } else if (pidOutput >= 1){ + return 1; + } else { + return pidOutput; + } + + } } + class PIDController { private double kP; diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java b/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java index c72d8d12f8e8..95ab7d00dd5e 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java @@ -119,6 +119,7 @@ enum WaitState { double hwDrumPosition; //where the drum was *told* to go last. NOT THE SAME AS lastQueuedPosition! double upperLaunchVelocity; double lowerLaunchVelocity; + double feederPower; HardwareMap hardwareMap; @@ -192,6 +193,7 @@ public DrumRequest(double position, WaitState nextState) { motULauncher.setDirection(DcMotorSimple.Direction.REVERSE); motLLauncher.setDirection(DcMotorSimple.Direction.REVERSE); + servoBLaunchFeeder.setDirection(DcMotorSimple.Direction.REVERSE); setLaunchVelocity(LaunchDistance.NEAR); @@ -285,12 +287,17 @@ void setLaunchVelocity (LaunchDistance launchDistance) { if (launchDistance == LaunchDistance.NEAR) { upperLaunchVelocity = NEAR_FLYWHEEL_VELOCITY - (0.5 * FLYWHEEL_BACK_SPIN); lowerLaunchVelocity = NEAR_FLYWHEEL_VELOCITY + (0.5 * FLYWHEEL_BACK_SPIN); + feederPower = FEEDER_POWER; } else if (launchDistance == LaunchDistance.FAR){ upperLaunchVelocity = FAR_FLYWHEEL_VELOCITY - (0.5 * FLYWHEEL_BACK_SPIN); lowerLaunchVelocity = FAR_FLYWHEEL_VELOCITY + (0.5 * FLYWHEEL_BACK_SPIN); + feederPower = FEEDER_POWER; } else { upperLaunchVelocity = 0; lowerLaunchVelocity = 0; + servoBLaunchFeeder.setPower(0); + servoFLaunchFeeder.setPower(0); + feederPower = 0; } } int findSlotFromPosition (double position, double [] positions) { @@ -383,8 +390,8 @@ void update () { motLLauncher.setVelocity(lowerLaunchVelocity); motULauncher.setVelocity(upperLaunchVelocity); motIntake.setPower(intakePower); - servoBLaunchFeeder.setPower(-FEEDER_POWER); - servoFLaunchFeeder.setPower(FEEDER_POWER); + servoBLaunchFeeder.setPower(feederPower); + servoFLaunchFeeder.setPower(feederPower); } } diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CoolColorDetector.java b/team417/src/main/java/org/firstinspires/ftc/team417/CoolColorDetector.java index c916af672948..12a721323d06 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CoolColorDetector.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CoolColorDetector.java @@ -2,6 +2,7 @@ import android.annotation.SuppressLint; +import com.acmerobotics.dashboard.config.Config; import com.qualcomm.robotcore.hardware.DistanceSensor; import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.NormalizedColorSensor; @@ -11,8 +12,16 @@ import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; - +@Config public class CoolColorDetector { + public static double MINIMUM_DISTANCE = 60; //25mm + public static double PURPLE_MIN_HUE = 200; + public static double PURPLE_MAX_HUE = 225; + public static double GREEN_MIN_HUE = 165; + + public static double GREEN_MAX_HUE = 180; + + Telemetry telemetry; private NormalizedColorSensor sensor1; private NormalizedColorSensor sensor2; @@ -30,7 +39,6 @@ public CoolColorDetector(HardwareMap map, Telemetry telemetry) { @SuppressLint("DefaultLocale") // --- Use logic comparing both sensors --- PixelColor detectArtifactColor() { - final double MINIMUM_DISTANCE = 25; //25mm double distance1 = ((DistanceSensor) sensor1).getDistance(DistanceUnit.MM); double distance2 = ((DistanceSensor) sensor2).getDistance(DistanceUnit.MM); NormalizedColorSensor sensor; @@ -57,9 +65,9 @@ PixelColor detectArtifactColor() { telemetry.addLine(String.format(" %.2f\", %.2f\"", distance1, distance2)); - if (hue > 165 && hue < 180) { //range determined from testing + if (hue > GREEN_MIN_HUE && hue < GREEN_MAX_HUE) { //range determined from testing return PixelColor.GREEN; - } else if (hue >= 200 && hue <= 225) { //range determined from testing + } else if (hue >= PURPLE_MIN_HUE && hue <= PURPLE_MAX_HUE) { //range determined from testing return PixelColor.PURPLE; } else { //error case use the most likely color diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/SensorColor.java b/team417/src/main/java/org/firstinspires/ftc/team417/SensorColor.java deleted file mode 100644 index f781b535f407..000000000000 --- a/team417/src/main/java/org/firstinspires/ftc/team417/SensorColor.java +++ /dev/null @@ -1,235 +0,0 @@ -/* Copyright (c) 2017-2020 FIRST. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without modification, - * are permitted (subject to the limitations in the disclaimer below) provided that - * the following conditions are met: - * - * Redistributions of source code must retain the above copyright notice, this list - * of conditions and the following disclaimer. - * - * Redistributions in binary form must reproduce the above copyright notice, this - * list of conditions and the following disclaimer in the documentation and/or - * other materials provided with the distribution. - * - * Neither the name of FIRST nor the names of its contributors may be used to endorse or - * promote products derived from this software without specific prior written permission. - * - * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS - * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, - * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - */ - -package org.firstinspires.ftc.team417; - -import android.app.Activity; -import android.graphics.Color; -import android.view.View; - -import com.qualcomm.robotcore.eventloop.opmode.Disabled; -import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; -import com.qualcomm.robotcore.eventloop.opmode.TeleOp; -import com.qualcomm.robotcore.hardware.DistanceSensor; -import com.qualcomm.robotcore.hardware.NormalizedColorSensor; -import com.qualcomm.robotcore.hardware.NormalizedRGBA; -import com.qualcomm.robotcore.hardware.SwitchableLight; - -import org.firstinspires.ftc.robotcore.external.Telemetry; -import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; - -/* - * This OpMode shows how to use a color sensor in a generic - * way, regardless of which particular make or model of color sensor is used. The OpMode - * assumes that the color sensor is configured with a name of "sensor_color". - * - * There will be some variation in the values measured depending on the specific sensor you are using. - * - * If the color sensor supports adjusting the gain, you can increase the gain (a multiplier to make - * the sensor report higher values) by holding down the A button on the gamepad, and decrease the - * gain by holding down the B button on the gamepad. The AndyMark Proximity & Color Sensor does not - * support this. - * - * If the color sensor has a light which is controllable from software, you can use the X button on - * the gamepad to toggle the light on and off. The REV sensors don't support this, but instead have - * a physical switch on them to turn the light on and off, beginning with REV Color Sensor V2. The - * AndyMark Proximity & Color Sensor does not support this. - * - * If the color sensor also supports short-range distance measurements (usually via an infrared - * proximity sensor), the reported distance will be written to telemetry. As of September 2025, - * the only color sensors that support this are the ones from REV Robotics and the AndyMark - * Proximity & Color Sensor. These infrared proximity sensor measurements are only useful at very - * small distances, and are sensitive to ambient light and surface reflectivity. You should use a - * different sensor if you need precise distance measurements. - * - * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. - * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list - */ -@TeleOp(name = "Sensor: Color", group = "Sensor") -public class SensorColor extends LinearOpMode { - - /** The colorSensor field will contain a reference to our color sensor hardware object */ - NormalizedColorSensor colorSensor; - - /** The relativeLayout field is used to aid in providing interesting visual feedback - * in this sample application; you probably *don't* need this when you use a color sensor on your - * robot. Note that you won't see anything change on the Driver Station, only on the Robot Controller. */ - View relativeLayout; - - /* - * The runOpMode() method is the root of this OpMode, as it is in all LinearOpModes. - * Our implementation here, though is a bit unusual: we've decided to put all the actual work - * in the runSample() method rather than directly in runOpMode() itself. The reason we do that is - * that in this sample we're changing the background color of the robot controller screen as the - * OpMode runs, and we want to be able to *guarantee* that we restore it to something reasonable - * and palatable when the OpMode ends. The simplest way to do that is to use a try...finally - * block around the main, core logic, and an easy way to make that all clear was to separate - * the former from the latter in separate methods. - */ - @Override public void runOpMode() { - - // Get a reference to the RelativeLayout so we can later change the background - // color of the Robot Controller app to match the hue detected by the RGB sensor. - int relativeLayoutId = hardwareMap.appContext.getResources().getIdentifier("RelativeLayout", "id", hardwareMap.appContext.getPackageName()); - relativeLayout = ((Activity) hardwareMap.appContext).findViewById(relativeLayoutId); - - try { - runSample(); // actually execute the sample - } finally { - // On the way out, *guarantee* that the background is reasonable. It doesn't actually start off - // as pure white, but it's too much work to dig out what actually was used, and this is good - // enough to at least make the screen reasonable again. - // Set the panel back to the default color - relativeLayout.post(new Runnable() { - public void run() { - relativeLayout.setBackgroundColor(Color.WHITE); - } - }); - } - } - - protected void runSample() { - telemetry.setDisplayFormat(Telemetry.DisplayFormat.HTML); - // You can give the sensor a gain value, will be multiplied by the sensor's raw value before the - // normalized color values are calculated. Color sensors (especially the REV Color Sensor V3) - // can give very low values (depending on the lighting conditions), which only use a small part - // of the 0-1 range that is available for the red, green, and blue values. In brighter conditions, - // you should use a smaller gain than in dark conditions. If your gain is too high, all of the - // colors will report at or near 1, and you won't be able to determine what color you are - // actually looking at. For this reason, it's better to err on the side of a lower gain - // (but always greater than or equal to 1). - float gain = 150; - - // Once per loop, we will update this hsvValues array. The first element (0) will contain the - // hue, the second element (1) will contain the saturation, and the third element (2) will - // contain the value. See http://web.archive.org/web/20190311170843/https://infohost.nmt.edu/tcc/help/pubs/colortheory/web/hsv.html - // for an explanation of HSV color. - final float[] hsvValues = new float[3]; - - // xButtonPreviouslyPressed and xButtonCurrentlyPressed keep track of the previous and current - // state of the X button on the gamepad - boolean xButtonPreviouslyPressed = false; - boolean xButtonCurrentlyPressed = false; - - // Get a reference to our sensor object. It's recommended to use NormalizedColorSensor over - // ColorSensor, because NormalizedColorSensor consistently gives values between 0 and 1, while - // the values you get from ColorSensor are dependent on the specific sensor you're using. - colorSensor = hardwareMap.get(NormalizedColorSensor.class, "cs1"); - - // If possible, turn the light on in the beginning (it might already be on anyway, - // we just make sure it is if we can). - if (colorSensor instanceof SwitchableLight) { - ((SwitchableLight)colorSensor).enableLight(true); - } - - // Wait for the start button to be pressed. - waitForStart(); - - // Loop until we are asked to stop - while (opModeIsActive()) { - // Explain basic gain information via telemetry - telemetry.addLine("Hold the A button on gamepad 1 to increase gain, or B to decrease it.\n"); - telemetry.addLine("Higher gain values mean that the sensor will report larger numbers for Red, Green, and Blue, and Value\n"); - - // Update the gain value if either of the A or B gamepad buttons is being held - if (gamepad1.a) { - // Only increase the gain by a small amount, since this loop will occur multiple times per second. - gain += 0.05; - } else if (gamepad1.b && gain > 1) { // A gain of less than 1 will make the values smaller, which is not helpful. - gain -= 0.05; - } - - // Show the gain value via telemetry - telemetry.addData("Gain", gain); - - // Tell the sensor our desired gain value (normally you would do this during initialization, - // not during the loop) - colorSensor.setGain(gain); - - // Check the status of the X button on the gamepad - xButtonCurrentlyPressed = gamepad1.x; - - // If the button state is different than what it was, then act - if (xButtonCurrentlyPressed != xButtonPreviouslyPressed) { - // If the button is (now) down, then toggle the light - if (xButtonCurrentlyPressed) { - if (colorSensor instanceof SwitchableLight) { - SwitchableLight light = (SwitchableLight)colorSensor; - light.enableLight(!light.isLightOn()); - } - } - } - xButtonPreviouslyPressed = xButtonCurrentlyPressed; - - // Get the normalized colors from the sensor - NormalizedRGBA colors = colorSensor.getNormalizedColors(); - - /* Use telemetry to display feedback on the driver station. We show the red, green, and blue - * normalized values from the sensor (in the range of 0 to 1), as well as the equivalent - * HSV (hue, saturation and value) values. See http://web.archive.org/web/20190311170843/https://infohost.nmt.edu/tcc/help/pubs/colortheory/web/hsv.html - * for an explanation of HSV color. */ - - // Update the hsvValues array by passing it to Color.colorToHSV() - Color.colorToHSV(colors.toColor(), hsvValues); - int hueColor = Color.HSVToColor(new float[]{hsvValues[0], 1, 1}); - - telemetry.addLine() - .addData("Red", "%.3f", colors.red) - .addData("Green", "%.3f", colors.green) - .addData("Blue", "%.3f", colors.blue); - telemetry.addLine() - .addData("Hue", "%.3f", hsvValues[0]) - .addData("Saturation", "%.3f", hsvValues[1]) - .addData("Value", "%.3f", hsvValues[2]); - telemetry.addData("Alpha", "%.3f", colors.alpha); - telemetry.addLine(colorString(colors.toColor())); - telemetry.addLine(colorString(hueColor)); - - /* If this color sensor also has a distance sensor, display the measured distance. - * Note that the reported distance is only useful at very close range, and is impacted by - * ambient light and surface reflectivity. */ - if (colorSensor instanceof DistanceSensor) { - telemetry.addData("Distance (cm)", "%.3f", ((DistanceSensor) colorSensor).getDistance(DistanceUnit.CM)); - } - - telemetry.update(); - - // Change the Robot Controller's background color to match the color detected by the color sensor. - relativeLayout.post(new Runnable() { - public void run() { - relativeLayout.setBackgroundColor(Color.HSVToColor(hsvValues)); - } - }); - } - } - static String colorString(int color) { - return String.format("\u25a0", - color & 0xffffff); - } -} From 90cfa4a531e83304913d08c7246230355357c994 Mon Sep 17 00:00:00 2001 From: Sameer Date: Sun, 7 Dec 2025 16:08:41 -0800 Subject: [PATCH 136/198] Auto works, roadrunner builds after start, 12 ball 8 seconds over. --- .../ftc/team417/CompetitionAuto.java | 62 +++++++++++-------- 1 file changed, 37 insertions(+), 25 deletions(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java index 790590d02273..56cbd5751765 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java @@ -7,8 +7,10 @@ import com.acmerobotics.roadrunner.Pose2dDual; import com.acmerobotics.roadrunner.PoseMap; import com.acmerobotics.roadrunner.TrajectoryActionBuilder; +import com.acmerobotics.roadrunner.TranslationalVelConstraint; import com.acmerobotics.roadrunner.Vector2d; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.wilyworks.common.WilyWorks; import org.firstinspires.ftc.team417.apriltags.LimelightDetector; @@ -21,6 +23,10 @@ import org.firstinspires.ftc.team417.roadrunner.MecanumDrive; import org.firstinspires.ftc.team417.roadrunner.RobotAction; +import java.lang.reflect.Array; +import java.util.ArrayList; +import java.util.Arrays; + /** * This class exposes the competition version of Autonomous. As a general rule, add code to the * BaseOpMode class rather than here so that it can be shared between both TeleOp and Autonomous. @@ -72,35 +78,37 @@ public Action getPath(SlowBotMovement chosenMovement, Alliance chosenAlliance, d case NEAR: trajectoryAction = drive.actionBuilder(SBNearStartPose, poseMap); trajectoryAction = trajectoryAction.setTangent(Math.toRadians(-51)) - .splineToConstantHeading(new Vector2d(-36, 36), Math.toRadians(-51)) + .splineToConstantHeading(new Vector2d(-12, 12), Math.toRadians(-51)) .stopAndAdd(new LaunchAction(mechGlob, countBalls)) .setTangent(Math.toRadians(0)) .splineToSplineHeading(new Pose2d(-12, 32, Math.toRadians(90)), Math.toRadians(90)) //go to intake closest from goal .afterDisp(0, new IntakeAction(mechGlob, 1)) .setTangent(Math.toRadians(90)) - .splineToConstantHeading(new Vector2d(-12, 59), Math.toRadians(90)) + .splineToConstantHeading(new Vector2d(-12, 50), Math.toRadians(90),new TranslationalVelConstraint(15)) .afterDisp(0, new IntakeAction(mechGlob, 0)) .setTangent(Math.toRadians(-90)) - .splineToSplineHeading(new Pose2d(-36, 36, Math.toRadians(139)), Math.toRadians(180)) //go to launch position + .splineToSplineHeading(new Pose2d(-12, 12, Math.toRadians(139)), Math.toRadians(180)) //go to launch position .stopAndAdd(new LaunchAction(mechGlob, countBalls)); if (intakeCycles > 1) { trajectoryAction = trajectoryAction.setTangent(Math.toRadians(0)) .splineToSplineHeading(new Pose2d(12, 32, Math.toRadians(90)), Math.toRadians(90)) //go to intake middle from goal + .afterDisp(0,new IntakeAction(mechGlob, 1)) .setTangent(Math.toRadians(90)) - .splineToConstantHeading(new Vector2d(12, 60), Math.toRadians(90)) + .splineToConstantHeading(new Vector2d(12, 50), Math.toRadians(90),new TranslationalVelConstraint(15)) .setTangent(Math.toRadians(-90)) - .splineToSplineHeading(new Pose2d(-36, 36, Math.toRadians(139)), Math.toRadians(180)); //go to launch position - //3 launches - //after disp intake + .splineToSplineHeading(new Pose2d(-12, 12, Math.toRadians(139)), Math.toRadians(180)) //go to launch position + .stopAndAdd(new LaunchAction(mechGlob, countBalls)); if (intakeCycles > 2) { trajectoryAction = trajectoryAction.setTangent(Math.toRadians(0)) .splineToSplineHeading(new Pose2d(36, 32, Math.toRadians(90)), Math.toRadians(90)) //go to intake farthest from goal + .afterDisp(0,new IntakeAction(mechGlob, 1)) .setTangent(Math.toRadians(90)) - .splineToConstantHeading(new Vector2d(36, 60), Math.toRadians(90)) + .splineToConstantHeading(new Vector2d(36, 50), Math.toRadians(90),new TranslationalVelConstraint(15)) .setTangent(Math.toRadians(-90)) - .splineToSplineHeading(new Pose2d(-36, 36, Math.toRadians(139)), Math.toRadians(180)); //go to launch position + .splineToSplineHeading(new Pose2d(-12, 12, Math.toRadians(139)), Math.toRadians(180)) //go to launch position + .stopAndAdd(new LaunchAction(mechGlob, countBalls)); } } @@ -109,35 +117,35 @@ public Action getPath(SlowBotMovement chosenMovement, Alliance chosenAlliance, d case FAR: trajectoryAction = drive.actionBuilder(SBFarStartPose, poseMap); if (intakeCycles == 0) { - trajectoryAction = trajectoryAction.setTangent(Math.toRadians(180)); - // 3 launch actions - //then after disp intake action + trajectoryAction = trajectoryAction.setTangent(Math.toRadians(180)) + .stopAndAdd(new LaunchAction(mechGlob, countBalls)); } trajectoryAction = trajectoryAction.splineToSplineHeading(new Pose2d(36, 32, Math.toRadians(90)), Math.toRadians(90)) //go to intake farthest from goal + .afterDisp(0,new IntakeAction(mechGlob, 1)) .setTangent(Math.toRadians(90)) - .splineToConstantHeading(new Vector2d(36, 60), Math.toRadians(90)) + .splineToConstantHeading(new Vector2d(36, 60), Math.toRadians(90), new TranslationalVelConstraint(10)) .setTangent(Math.toRadians(-90)) - .splineToSplineHeading(new Pose2d(54, 12, Math.toRadians(157.5)), Math.toRadians(-90)); //go to launch position - + .splineToSplineHeading(new Pose2d(54, 12, Math.toRadians(157.5)), Math.toRadians(-90)) //go to launch position + .stopAndAdd(new LaunchAction(mechGlob, countBalls)); if (intakeCycles > 1) { - // 3 launch actions - //after disp intake action trajectoryAction = trajectoryAction.setTangent(Math.toRadians(180)) .splineToSplineHeading(new Pose2d(12, 32, Math.toRadians(90)), Math.toRadians(90)) //go to intake middle from goal + .afterDisp(0,new IntakeAction(mechGlob, 1)) .setTangent(Math.toRadians(90)) - .splineToConstantHeading(new Vector2d(12, 60), Math.toRadians(90)) + .splineToConstantHeading(new Vector2d(12, 60), Math.toRadians(90), new TranslationalVelConstraint(10)) .setTangent(Math.toRadians(-90)) - .splineToSplineHeading(new Pose2d(54, 12, Math.toRadians(157.5)), Math.toRadians(-90)); //go to launch position - // 3 launch actions - //after disp intake action + .splineToSplineHeading(new Pose2d(54, 12, Math.toRadians(157.5)), Math.toRadians(-90)) //go to launch position + .stopAndAdd(new LaunchAction(mechGlob, countBalls)); if (intakeCycles > 2) { trajectoryAction = trajectoryAction.setTangent(Math.toRadians(180)) .splineToSplineHeading(new Pose2d(-12, 32, Math.toRadians(90)), Math.toRadians(90)) //go to intake closest to goal + .afterDisp(0,new IntakeAction(mechGlob, 1)) .setTangent(Math.toRadians(90)) - .splineToConstantHeading(new Vector2d(-12, 55), Math.toRadians(90)) + .splineToConstantHeading(new Vector2d(-12, 55), Math.toRadians(90), new TranslationalVelConstraint(10)) .setTangent(Math.toRadians(-90)) - .splineToSplineHeading(new Pose2d(54, 12, Math.toRadians(157.5)), Math.toRadians(-90)); //go to launch position + .splineToSplineHeading(new Pose2d(54, 12, Math.toRadians(157.5)), Math.toRadians(-90)) //go to launch position + .stopAndAdd(new LaunchAction(mechGlob, countBalls)); } } break; @@ -150,7 +158,7 @@ public Action getPath(SlowBotMovement chosenMovement, Alliance chosenAlliance, d .splineToLinearHeading(new Pose2d(60, 61, Math.toRadians(0)), Math.toRadians(0)) .setTangent(Math.toRadians(-90)) .splineToLinearHeading(new Pose2d(54, 12, Math.toRadians(157.5)), Math.toRadians(-90)) - // 3 launch actions + .stopAndAdd(new LaunchAction(mechGlob, countBalls)) .setTangent(Math.toRadians(90)) .splineToLinearHeading(new Pose2d(50, 32, Math.toRadians(180)), Math.toRadians(180)); break; @@ -176,7 +184,9 @@ public void runOpMode() { Pose2d SBNearStartPose = new Pose2d(-60, 48, Math.toRadians(139)); Pose2d SBFarStartPose = new Pose2d(60, 12, Math.toRadians(157.5)); MecanumDrive drive = new MecanumDrive(hardwareMap, telemetry, gamepad1, startPose); - MechGlob mechGlob = ComplexMechGlob.create(hardwareMap, telemetry, false); + PixelColor[] preloads = new PixelColor[]{PixelColor.PURPLE, PixelColor.GREEN, PixelColor.PURPLE}; + MechGlob mechGlob = ComplexMechGlob.create(hardwareMap, telemetry,preloads ); + @@ -218,6 +228,7 @@ public void runOpMode() { SlowBotMovement chosenMovement = menu.getResult(SlowBotMovement.class, "movement-picker-1"); double waitTime = menu.getResult(Double.class, "wait-slider-1"); double intakeCycles = menu.getResult(Double.class, "intake-slider"); + if (chosenMovement == SlowBotMovement.NEAR) { mechGlob.setLaunchVelocity(LaunchDistance.NEAR); } else { @@ -301,6 +312,7 @@ public void runOpMode() { packet.fieldOverlay().getOperations().addAll(previewCanvas.getOperations()); more = trajectoryAction.run(packet); mechGlob.update(); + WilyWorks.updateSimulation(0); // Advance the simulation when not driving // Only send the packet if there's more to do in order to keep the very last // drawing up on the field once the robot is done: if (more) From 354218259eab9e0c7fcc04d7dd154ca3fc1a3937 Mon Sep 17 00:00:00 2001 From: Hankang Zhou <136044263+nobody-particular@users.noreply.github.com> Date: Sun, 7 Dec 2025 16:31:45 -0800 Subject: [PATCH 137/198] Revised BaseOpMode.tryResetRobotPose to be non-static. --- .../firstinspires/ftc/team417/BaseOpMode.java | 19 +++++++----- .../ftc/team417/CompetitionAuto.java | 7 +++-- .../ftc/team417/CompetitionTeleOp.java | 17 +++++----- .../ftc/team417/ComplexMechGlob.java | 4 +-- .../team417/apriltags/LimelightDetector.java | 31 +++++++------------ 5 files changed, 35 insertions(+), 43 deletions(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java b/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java index 92530aca17f3..9eaefee20240 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/BaseOpMode.java @@ -11,12 +11,15 @@ * Autonomous logic. All TeleOp and Autonomous classes should derive from this class. */ abstract public class BaseOpMode extends LinearOpMode { + LimelightDetector detector; + MecanumDrive drive; + // Resets the robot pose only if the robot is not moving - public static void tryResetRobotPose(LimelightDetector detector, MecanumDrive drive) { - if (isZero(drive.rightBack.getVelocity()) - && isZero(drive.rightFront.getVelocity()) - && isZero(drive.leftBack.getVelocity()) - && isZero(drive.leftFront.getVelocity())) { + public void tryResetRobotPose() { + if (isZero(drive.poseVelocity.linearVel.x) + && isZero(drive.poseVelocity.linearVel.y) + && isZero(drive.poseVelocity.angVel) + ) { Pose2d pose = detector.detectRobotPose(); @@ -26,9 +29,9 @@ && isZero(drive.leftFront.getVelocity())) { } } - // Sees if a number is within one one-thousandths of zero - private static boolean isZero(double x) { - return Math.abs(x) < 0.001; + // Sees if a number is within one one-hundredths of zero + private static boolean isZero(double z) { + return Math.abs(z) < 0.01; } } diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java index 1f5f92987ad5..c27068c5c4fa 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java @@ -44,13 +44,11 @@ enum SlowBotMovement { double minIntakes = 0.0; double maxIntakes = 3.0; TextMenu menu = new TextMenu(); - LimelightDetector detector = new LimelightDetector(hardwareMap); MenuInput menuInput = new MenuInput(MenuInput.InputType.CONTROLLER); Pattern pattern; Alliance chosenAlliance; SlowBotMovement chosenMovement; double intakeCycles; - MecanumDrive drive; public Action getPath(SlowBotMovement chosenMovement, Alliance chosenAlliance, double intakeCycles) { Pose2d startPose = new Pose2d(0, 0, 0); @@ -178,6 +176,7 @@ public void runOpMode() { Pose2d SBNearStartPose = new Pose2d(-60, 48, Math.toRadians(139)); Pose2d SBFarStartPose = new Pose2d(60, 12, Math.toRadians(157.5)); MecanumDrive drive = new MecanumDrive(hardwareMap, telemetry, gamepad1, startPose); + detector = new LimelightDetector(hardwareMap); // Text menu for FastBot @@ -290,6 +289,8 @@ public void runOpMode() { MecanumDrive.sendTelemetryPacket(packet); telemetry.update(); } + + detector.close(); } } @@ -303,7 +304,7 @@ public LaunchAction(CompetitionAuto opMode) { @Override public boolean run(double elapsedTime) { - BaseOpMode.tryResetRobotPose(opMode.detector, opMode.drive); // Resets the robot pose only if the robot is not moving + opMode.tryResetRobotPose(); // Resets the robot pose only if the robot is not moving return false; } } diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java index a3a451514e36..1bc84f0f28bf 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java @@ -36,12 +36,10 @@ public class CompetitionTeleOp extends BaseOpMode { * functions and autonomous routines in a way that avoids loops within loops, and "waits". */ - LimelightDetector detector = new LimelightDetector(hardwareMap); - MecanumDrive drive; - @Override public void runOpMode() { Pose2d beginPose = new Pose2d(0, 0, 0); + detector = new LimelightDetector(hardwareMap); drive = new MecanumDrive(hardwareMap, telemetry, gamepad1, beginPose); MechGlob mechGlob = ComplexMechGlob.create(hardwareMap, telemetry, false); @@ -61,17 +59,12 @@ public void runOpMode() { ), halfLinearHalfCubic(-gamepad1.right_stick_x) - - )); - - // Update the current pose: - drive.updatePoseEstimate(); - + detector.updateRobotYaw(drive.pose.heading.log()); // 'packet' is the object used to send data to FTC Dashboard: TelemetryPacket packet = MecanumDrive.getTelemetryPacket(); @@ -84,7 +77,6 @@ public void runOpMode() { Drawing.drawRobot(packet.fieldOverlay(), drive.pose); MecanumDrive.sendTelemetryPacket(packet); - //add slowbot teleop controls here if (gamepad2.yWasPressed()) { mechGlob.launch(RequestedColor.EITHER, this); @@ -102,8 +94,13 @@ public void runOpMode() { mechGlob.update(); MecanumDrive.sendTelemetryPacket(packet); + + detector.detectPatternAndTelemeter(CompetitionAuto.Alliance.BLUE, telemetry, true); + telemetry.update(); } + + detector.close(); } diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java b/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java index 5c80c5ef875d..4f2157b87330 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java @@ -54,7 +54,7 @@ void intake (double intakeValue){} //a method that determines what color to launch. Options are purple, green, or either. void launch (RequestedColor requestedColor, CompetitionTeleOp opMode) { - BaseOpMode.tryResetRobotPose(opMode.detector, opMode.drive); + opMode.tryResetRobotPose(); } void update () {} @@ -228,7 +228,7 @@ void launch (RequestedColor requestedColor, CompetitionTeleOp opMode) { addToDrumQueue(LAUNCH_POSITIONS[minSlot], WaitState.SPIN_UP); slotOccupiedBy.set (minSlot, PixelColor.NONE); //marking this slot as empty so we don't accidentally try to use it again } - BaseOpMode.tryResetRobotPose(opMode.detector, opMode.drive); // Resets the robot pose only if the robot is not moving + opMode.tryResetRobotPose(); // Resets the robot pose only if the robot is not moving } //this function adds a new drum request to the drum queue. nextState is the state do use after the drum is finished moving void addToDrumQueue(double position, WaitState nextState){ diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/apriltags/LimelightDetector.java b/team417/src/main/java/org/firstinspires/ftc/team417/apriltags/LimelightDetector.java index 13805b518408..45c8e938e6c3 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/apriltags/LimelightDetector.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/apriltags/LimelightDetector.java @@ -125,7 +125,7 @@ public Pattern detectPatternAndTelemeter(CompetitionAuto.Alliance alliance, Tele telemetry.addData("Pipeline", "Index: %d, Type: %s", status.getPipelineIndex(), status.getPipelineType()); - if (result.isValid()) { + if (result != null && result.isValid()) { // Access general information Pose3D botPose = result.getBotpose(); double captureLatency = result.getCaptureLatency(); @@ -212,7 +212,7 @@ public Pattern detectPattern(CompetitionAuto.Alliance alliance, LLResult result) List currentDetections = new ArrayList<>(); - if (result.isValid()) + if (result != null && result.isValid()) currentDetections = result.getFiducialResults(); // Remove all AprilTags that don't have ID 21, 22, or 23 @@ -279,24 +279,8 @@ public Pattern detectPattern(CompetitionAuto.Alliance alliance, LLResult result) public Pose2d detectRobotPose() { LLResult result = limelight.getLatestResult(); - if (result.isValid()) { - List currentDetections = result.getFiducialResults(); - - // FiducialResult objects contain the x (left) and y (up) degrees relative to the robot. - // We want the AprilTag that is as straight on as possible, - // that is, the lowest absolute value x. - // For more information about the info the AprilTagDetection object contains, see this link: - // https://ftc-docs.firstinspires.org/en/latest/apriltag/understanding_apriltag_detection_values/understanding-apriltag-detection-values.html - - LLResultTypes.FiducialResult detection = currentDetections.stream() - .min(Comparator.comparingDouble(aprilTagDetection -> - Math.abs(aprilTagDetection.getTargetXDegrees()))).orElse(null); - - if (detection == null) { - return null; - } - - Pose3D pose = detection.getRobotPoseFieldSpace(); + if (result != null && result.isValid()) { + Pose3D pose = result.getBotpose_MT2(); return new Pose2d( pose.getPosition().x, @@ -307,6 +291,13 @@ public Pose2d detectRobotPose() { return null; } + /** + * Feed in the yaw from the IMU for MT2. + */ + public void updateRobotYaw(double yaw) { + limelight.updateRobotOrientation(Math.toDegrees(yaw)); + } + /** * Release the resources taken up by the vision portal. */ From 89036722f54cba5663ac761c74c123d02389ad38 Mon Sep 17 00:00:00 2001 From: Sameer Date: Sun, 7 Dec 2025 16:34:52 -0800 Subject: [PATCH 138/198] Auto works, roadrunner builds after start, 12 ball 8 seconds over. --- .../org/firstinspires/ftc/team417/CompetitionTeleOp.java | 3 ++- .../org/firstinspires/ftc/team417/ComplexMechGlob.java | 9 +++++---- 2 files changed, 7 insertions(+), 5 deletions(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java index 027e26bb439c..f55b0dca8042 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java @@ -44,7 +44,8 @@ public class CompetitionTeleOp extends BaseOpMode { public void runOpMode() { Pose2d beginPose = new Pose2d(0, 0, 0); MecanumDrive drive = new MecanumDrive(hardwareMap, telemetry, gamepad1, beginPose); - MechGlob mechGlob = ComplexMechGlob.create(hardwareMap, telemetry, false); + PixelColor[] preloads = new PixelColor[]{PixelColor.NONE, PixelColor.NONE, PixelColor.NONE}; + MechGlob mechGlob = ComplexMechGlob.create(hardwareMap, telemetry, preloads); AmazingAutoAim amazingAutoAim = null; telemetry.setDisplayFormat(Telemetry.DisplayFormat.HTML); diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java b/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java index 95ab7d00dd5e..0ff2911e4e24 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java @@ -43,9 +43,10 @@ class MechGlob { //a placeholder class encompassing all code that ISN'T for slow MechGlob(){} //call DrumGlob.create to create a Glob object for slowbot or fastbot - static MechGlob create (HardwareMap hardwareMap, Telemetry telemetry, boolean runningAuto){ + static MechGlob create (HardwareMap hardwareMap, Telemetry telemetry, PixelColor[] preloads){ + if (MecanumDrive.isSlowBot) { //if the robot is slowbot, use ComplexMechGlob. - return new ComplexMechGlob(hardwareMap, telemetry); //Go to ComplexMechGlob class + return new ComplexMechGlob(hardwareMap, telemetry, preloads); //Go to ComplexMechGlob class } else { //otherwise, use MechGlob return new MechGlob(); //Go to MechGlob class @@ -149,7 +150,7 @@ public DrumRequest(double position, WaitState nextState) { this.position = position; } } - ComplexMechGlob (HardwareMap hardwareMap, Telemetry telemetry) { + ComplexMechGlob (HardwareMap hardwareMap, Telemetry telemetry, PixelColor[] preloads) { //this changes some lists if we are using WilyWorks if (WilyWorks.isSimulating) { @@ -172,7 +173,7 @@ public DrumRequest(double position, WaitState nextState) { servoFLaunchFeeder = hardwareMap.get(CRServo.class, "servoFLaunchFeeder"); servoDrumGate = hardwareMap.get(Servo.class, "servoDrumGate"); coolColorDetector = new CoolColorDetector(hardwareMap, telemetry); - + slotOccupiedBy = new ArrayList<>(Arrays.asList(preloads)); /* * Here we set our flywheels to the RUN_USING_ENCODER runmode. * If you notice that you have no control over the velocity of the motor, it just jumps From 1f7548a505d893d46fee63b669a3d65f0b64fff5 Mon Sep 17 00:00:00 2001 From: anya-codes Date: Sun, 7 Dec 2025 16:58:53 -0800 Subject: [PATCH 139/198] Added LoonyTune config for SlowBot . The parameters are wrong and need to be tuned. Also created a new class called TransferState that stores the variables that need to transfer from auto to teleop. --- .../ftc/team417/CompetitionAuto.java | 46 +++++++++++-------- .../ftc/team417/CompetitionTeleOp.java | 32 ++++++++++--- .../ftc/team417/ComplexMechGlob.java | 8 ++-- .../ftc/team417/TransferState.java | 10 ++++ .../ftc/team417/roadrunner/MecanumDrive.java | 34 +++++++++++++- 5 files changed, 99 insertions(+), 31 deletions(-) create mode 100644 team417/src/main/java/org/firstinspires/ftc/team417/TransferState.java diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java index 56cbd5751765..a0a520e7f5f0 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java @@ -185,7 +185,7 @@ public void runOpMode() { Pose2d SBFarStartPose = new Pose2d(60, 12, Math.toRadians(157.5)); MecanumDrive drive = new MecanumDrive(hardwareMap, telemetry, gamepad1, startPose); PixelColor[] preloads = new PixelColor[]{PixelColor.PURPLE, PixelColor.GREEN, PixelColor.PURPLE}; - MechGlob mechGlob = ComplexMechGlob.create(hardwareMap, telemetry,preloads ); + MechGlob mechGlob = ComplexMechGlob.create(hardwareMap, telemetry, preloads); @@ -272,28 +272,28 @@ public void runOpMode() { // Assume unknown pattern unless detected otherwise. pattern = Pattern.UNKNOWN; pattern = Pattern.PPG; //temporary until hankang limelight - // Detect the pattern with the AprilTags from the camera! - // Wait for Start to be pressed on the Driver Hub! - // (This try-with-resources statement automatically calls detector.close() when it exits - // the try-block.) - try (LimelightDetector detector = new LimelightDetector(hardwareMap)) { - - while (opModeInInit()) { - Pattern last = detector.detectPatternAndTelemeter(chosenAlliance, telemetry); - if (last != Pattern.UNKNOWN) { - pattern = last; - } + // Detect the pattern with the AprilTags from the camera! + // Wait for Start to be pressed on the Driver Hub! + // (This try-with-resources statement automatically calls detector.close() when it exits + // the try-block.) + try (LimelightDetector detector = new LimelightDetector(hardwareMap)) { + + while (opModeInInit()) { + Pattern last = detector.detectPatternAndTelemeter(chosenAlliance, telemetry); + if (last != Pattern.UNKNOWN) { + pattern = last; + } - telemetry.addData("Chosen alliance: ", chosenAlliance); - telemetry.addData("Chosen movement: ", chosenMovement); - telemetry.addData("Chosen wait time: ", waitTime); - telemetry.addData("Last valid pattern: ", pattern); + telemetry.addData("Chosen alliance: ", chosenAlliance); + telemetry.addData("Chosen movement: ", chosenMovement); + telemetry.addData("Chosen wait time: ", waitTime); + telemetry.addData("Last valid pattern: ", pattern); - telemetry.update(); - if (isStopRequested()) { - break; - } + telemetry.update(); + if (isStopRequested()) { + break; } + } } if(chosenMovement == SlowBotMovement.NEAR) { mechGlob.setLaunchVelocity(LaunchDistance.NEAR); @@ -319,6 +319,12 @@ public void runOpMode() { MecanumDrive.sendTelemetryPacket(packet); telemetry.update(); } + + // Stores these so they can be transferred to teleop + TransferState.chosenAlliance = chosenAlliance; + TransferState.storedColors = new PixelColor[] {mechGlob.getSlotColor(0), mechGlob.getSlotColor(1), mechGlob.getSlotColor(2)}; + TransferState.pose = drive.pose; + } } class LaunchAction extends RobotAction { diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java index f55b0dca8042..203037078ea0 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java @@ -42,10 +42,30 @@ public class CompetitionTeleOp extends BaseOpMode { @Override public void runOpMode() { - Pose2d beginPose = new Pose2d(0, 0, 0); + Pose2d beginPose; + if (TransferState.pose != null) { + beginPose = TransferState.pose; + } else { + beginPose = new Pose2d(0, 0, 0); + } + + CompetitionAuto.Alliance alliance; + if (TransferState.chosenAlliance != null) { + alliance = TransferState.chosenAlliance; + } else { + alliance = CompetitionAuto.Alliance.BLUE; + } + + PixelColor[] storedColors; + if (TransferState.storedColors != null) { + storedColors = TransferState.storedColors; + } else { + storedColors = new PixelColor[] {PixelColor.NONE, PixelColor.NONE, PixelColor.NONE}; + } + MecanumDrive drive = new MecanumDrive(hardwareMap, telemetry, gamepad1, beginPose); PixelColor[] preloads = new PixelColor[]{PixelColor.NONE, PixelColor.NONE, PixelColor.NONE}; - MechGlob mechGlob = ComplexMechGlob.create(hardwareMap, telemetry, preloads); + MechGlob mechGlob = ComplexMechGlob.create(hardwareMap, telemetry, storedColors); AmazingAutoAim amazingAutoAim = null; telemetry.setDisplayFormat(Telemetry.DisplayFormat.HTML); @@ -61,7 +81,7 @@ public void runOpMode() { telemetry.addLine("Running TeleOp!"); if (gamepad1.rightBumperWasPressed()) { - amazingAutoAim = new AmazingAutoAim(telemetry, CompetitionAuto.Alliance.BLUE); + amazingAutoAim = new AmazingAutoAim(telemetry, alliance); } if (gamepad1.right_bumper) { @@ -121,9 +141,9 @@ public void runOpMode() { mechGlob.intake(gamepad2.left_stick_y); mechGlob.update(); - String slot0 = mechGlob.getSlotColor(0); - String slot1 = mechGlob.getSlotColor(1); - String slot2 = mechGlob.getSlotColor(2); + PixelColor slot0 = mechGlob.getSlotColor(0); + PixelColor slot1 = mechGlob.getSlotColor(1); + PixelColor slot2 = mechGlob.getSlotColor(2); telemetry.addData("Slot0: ", slot0); telemetry.addData("Slot1: ", slot1); diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java b/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java index 0ff2911e4e24..7e9fc6ab3c9f 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java @@ -70,8 +70,8 @@ boolean preLaunch (RequestedColor requestedColor) { } void setLaunchVelocity (LaunchDistance launchDistance) {} - public String getSlotColor(int slotIndex) { - return "NONE"; + public PixelColor getSlotColor(int slotIndex) { + return PixelColor.NONE; } @@ -311,9 +311,9 @@ int findSlotFromPosition (double position, double [] positions) { } @Override - public String getSlotColor(int slotIndex) { + public PixelColor getSlotColor(int slotIndex) { PixelColor artifactColor = slotOccupiedBy.get(slotIndex); - return artifactColor.toString(); + return artifactColor; } @Override diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/TransferState.java b/team417/src/main/java/org/firstinspires/ftc/team417/TransferState.java new file mode 100644 index 000000000000..cd9cfd5283c8 --- /dev/null +++ b/team417/src/main/java/org/firstinspires/ftc/team417/TransferState.java @@ -0,0 +1,10 @@ +package org.firstinspires.ftc.team417; + +import com.acmerobotics.roadrunner.Pose2d; + +// Stores the values that need to be transferred from auto to teleop +public class TransferState { + public static CompetitionAuto.Alliance chosenAlliance; + public static PixelColor[] storedColors; + public static Pose2d pose; +} diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/roadrunner/MecanumDrive.java b/team417/src/main/java/org/firstinspires/ftc/team417/roadrunner/MecanumDrive.java index 2f35e8b8f33f..abf7f8ee6311 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/roadrunner/MecanumDrive.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/roadrunner/MecanumDrive.java @@ -117,7 +117,39 @@ public static class Params { pinpoint.yReversed = false; pinpoint.xOffset = -199.4; pinpoint.yOffset = -120.2; - } else { + } else if (isFastBot) { + // Your competition robot Loony Tune configuration is here: + logoFacingDirection = RevHubOrientationOnRobot.LogoFacingDirection.UP; + usbFacingDirection = RevHubOrientationOnRobot.UsbFacingDirection.FORWARD; + + inPerTick = 1.0; + lateralInPerTick = 0.798; + trackWidthTicks = 13.82; + + kS = 0.625; + kV = 0.183; + kA = 0.0110; + + axialGain = 2.0; + axialVelGain = 0.55; + lateralGain = 9.0; + lateralVelGain = 2.0; + headingGain = 9.4; + headingVelGain = 0.0; + + otos.offset.x = 0; + otos.offset.y = 0; + otos.offset.h = Math.toRadians(0); + otos.linearScalar = 0; + otos.angularScalar = 0; + + pinpoint.ticksPerMm = 19.589; + pinpoint.xReversed = false; + pinpoint.yReversed = true; + pinpoint.xOffset = 119.9; + pinpoint.yOffset = 5.4; + } + else { // Your competition robot Loony Tune configuration is here: logoFacingDirection = RevHubOrientationOnRobot.LogoFacingDirection.UP; usbFacingDirection = RevHubOrientationOnRobot.UsbFacingDirection.FORWARD; From ef0324d0e96e706a38906908c52995f3ba623374 Mon Sep 17 00:00:00 2001 From: Emmett Date: Sun, 7 Dec 2025 18:26:46 -0800 Subject: [PATCH 140/198] added manual drum rotation --- .../ftc/team417/CompetitionAuto.java | 2 +- .../ftc/team417/CompetitionTeleOp.java | 2 ++ .../ftc/team417/ComplexMechGlob.java | 17 +++++++++++++++++ 3 files changed, 20 insertions(+), 1 deletion(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java index 17e9f1e12b13..1c203055f4f0 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java @@ -9,7 +9,7 @@ import com.acmerobotics.roadrunner.TrajectoryActionBuilder; import com.acmerobotics.roadrunner.Vector2d; import com.qualcomm.robotcore.eventloop.opmode.Autonomous; -import com.qualcomm.robotcore.robot.Robot; + import org.firstinspires.ftc.team417.apriltags.LimelightDetector; import org.firstinspires.ftc.team417.apriltags.Pattern; diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java index 4addeaa7d947..fada4f12df80 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java @@ -103,6 +103,8 @@ public void runOpMode() { } else if (gamepad2.dpadRightWasPressed()) { // turns off the flywheels mechGlob.setLaunchVelocity(LaunchDistance.OFF); + } else if (gamepad2.rightBumperWasPressed()) { + mechGlob.controlDrumManually(); } mechGlob.intake(gamepad2.left_stick_y); diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java b/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java index c72d8d12f8e8..73fd4b2358ed 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java @@ -69,6 +69,8 @@ boolean preLaunch (RequestedColor requestedColor) { } void setLaunchVelocity (LaunchDistance launchDistance) {} + void controlDrumManually () {} + public String getSlotColor(int slotIndex) { return "NONE"; } @@ -172,6 +174,7 @@ public DrumRequest(double position, WaitState nextState) { servoDrumGate = hardwareMap.get(Servo.class, "servoDrumGate"); coolColorDetector = new CoolColorDetector(hardwareMap, telemetry); + /* * Here we set our flywheels to the RUN_USING_ENCODER runmode. * If you notice that you have no control over the velocity of the motor, it just jumps @@ -302,6 +305,20 @@ int findSlotFromPosition (double position, double [] positions) { return -1; } + void controlDrumManually () { + int currentSlot = findSlotFromPosition(hwDrumPosition, INTAKE_POSITIONS); + if (currentSlot != -1) { + slotOccupiedBy.set(currentSlot, PixelColor.PURPLE); + } + int minSlot = findNearestSlot(INTAKE_POSITIONS, RequestedColor.NONE); + if (minSlot != -1) { + addToDrumQueue(INTAKE_POSITIONS[minSlot], WaitState.INTAKE); + } + telemetry.addData("LastQueuedPosition", lastQueuedPosition); + telemetry.addData("DrumPosition", hwDrumPosition); + telemetry.update(); + + } @Override public String getSlotColor(int slotIndex) { PixelColor artifactColor = slotOccupiedBy.get(slotIndex); From ec093e94dc17e0ae5b96f3e0611d8f2180547289 Mon Sep 17 00:00:00 2001 From: Emmett Date: Mon, 8 Dec 2025 08:38:42 -0800 Subject: [PATCH 141/198] fixed merge conflicts --- .../java/org/firstinspires/ftc/team417/ComplexMechGlob.java | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java b/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java index a308ed5ec222..28dac7f23fa1 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java @@ -72,11 +72,10 @@ void setLaunchVelocity (LaunchDistance launchDistance) {} public PixelColor getSlotColor(int slotIndex) { return PixelColor.NONE; + } void controlDrumManually () {} - public String getSlotColor(int slotIndex) { - return "NONE"; - } + } From 2564c87283058304565f2f5d46d90ca4360cd605 Mon Sep 17 00:00:00 2001 From: Sameer Date: Mon, 8 Dec 2025 17:16:45 -0800 Subject: [PATCH 142/198] Handle failure cases, prelaunch. 9 ball near auto in 28.2 seconds including weird pause at beginning(with guessed transfer values which are not being pushed). Far Actions are not implemented yet. --- .../ftc/team417/CompetitionAuto.java | 83 ++++++++++++++----- 1 file changed, 60 insertions(+), 23 deletions(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java index 56cbd5751765..f3717802d0e9 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java @@ -22,10 +22,7 @@ import org.firstinspires.ftc.team417.javatextmenu.TextMenu; import org.firstinspires.ftc.team417.roadrunner.MecanumDrive; import org.firstinspires.ftc.team417.roadrunner.RobotAction; - -import java.lang.reflect.Array; import java.util.ArrayList; -import java.util.Arrays; /** * This class exposes the competition version of Autonomous. As a general rule, add code to the @@ -78,6 +75,7 @@ public Action getPath(SlowBotMovement chosenMovement, Alliance chosenAlliance, d case NEAR: trajectoryAction = drive.actionBuilder(SBNearStartPose, poseMap); trajectoryAction = trajectoryAction.setTangent(Math.toRadians(-51)) + .afterDisp(0,new PreLaunchAction(mechGlob, countBalls)) .splineToConstantHeading(new Vector2d(-12, 12), Math.toRadians(-51)) .stopAndAdd(new LaunchAction(mechGlob, countBalls)) .setTangent(Math.toRadians(0)) @@ -86,6 +84,7 @@ public Action getPath(SlowBotMovement chosenMovement, Alliance chosenAlliance, d .setTangent(Math.toRadians(90)) .splineToConstantHeading(new Vector2d(-12, 50), Math.toRadians(90),new TranslationalVelConstraint(15)) .afterDisp(0, new IntakeAction(mechGlob, 0)) + .afterDisp(1, new PreLaunchAction(mechGlob, countBalls)) .setTangent(Math.toRadians(-90)) .splineToSplineHeading(new Pose2d(-12, 12, Math.toRadians(139)), Math.toRadians(180)) //go to launch position .stopAndAdd(new LaunchAction(mechGlob, countBalls)); @@ -96,6 +95,8 @@ public Action getPath(SlowBotMovement chosenMovement, Alliance chosenAlliance, d .afterDisp(0,new IntakeAction(mechGlob, 1)) .setTangent(Math.toRadians(90)) .splineToConstantHeading(new Vector2d(12, 50), Math.toRadians(90),new TranslationalVelConstraint(15)) + .afterDisp(0, new IntakeAction(mechGlob, 0)) + .afterDisp(1, new PreLaunchAction(mechGlob, countBalls)) .setTangent(Math.toRadians(-90)) .splineToSplineHeading(new Pose2d(-12, 12, Math.toRadians(139)), Math.toRadians(180)) //go to launch position .stopAndAdd(new LaunchAction(mechGlob, countBalls)); @@ -106,6 +107,8 @@ public Action getPath(SlowBotMovement chosenMovement, Alliance chosenAlliance, d .afterDisp(0,new IntakeAction(mechGlob, 1)) .setTangent(Math.toRadians(90)) .splineToConstantHeading(new Vector2d(36, 50), Math.toRadians(90),new TranslationalVelConstraint(15)) + .afterDisp(0, new IntakeAction(mechGlob, 0)) + .afterDisp(1, new PreLaunchAction(mechGlob, countBalls)) .setTangent(Math.toRadians(-90)) .splineToSplineHeading(new Pose2d(-12, 12, Math.toRadians(139)), Math.toRadians(180)) //go to launch position .stopAndAdd(new LaunchAction(mechGlob, countBalls)); @@ -124,6 +127,8 @@ public Action getPath(SlowBotMovement chosenMovement, Alliance chosenAlliance, d .afterDisp(0,new IntakeAction(mechGlob, 1)) .setTangent(Math.toRadians(90)) .splineToConstantHeading(new Vector2d(36, 60), Math.toRadians(90), new TranslationalVelConstraint(10)) + .afterDisp(0, new IntakeAction(mechGlob, 0)) + .afterDisp(1, new PreLaunchAction(mechGlob, countBalls)) .setTangent(Math.toRadians(-90)) .splineToSplineHeading(new Pose2d(54, 12, Math.toRadians(157.5)), Math.toRadians(-90)) //go to launch position .stopAndAdd(new LaunchAction(mechGlob, countBalls)); @@ -133,6 +138,8 @@ public Action getPath(SlowBotMovement chosenMovement, Alliance chosenAlliance, d .afterDisp(0,new IntakeAction(mechGlob, 1)) .setTangent(Math.toRadians(90)) .splineToConstantHeading(new Vector2d(12, 60), Math.toRadians(90), new TranslationalVelConstraint(10)) + .afterDisp(0, new IntakeAction(mechGlob, 0)) + .afterDisp(1, new PreLaunchAction(mechGlob, countBalls)) .setTangent(Math.toRadians(-90)) .splineToSplineHeading(new Pose2d(54, 12, Math.toRadians(157.5)), Math.toRadians(-90)) //go to launch position .stopAndAdd(new LaunchAction(mechGlob, countBalls)); @@ -143,6 +150,8 @@ public Action getPath(SlowBotMovement chosenMovement, Alliance chosenAlliance, d .afterDisp(0,new IntakeAction(mechGlob, 1)) .setTangent(Math.toRadians(90)) .splineToConstantHeading(new Vector2d(-12, 55), Math.toRadians(90), new TranslationalVelConstraint(10)) + .afterDisp(0, new IntakeAction(mechGlob, 0)) + .afterDisp(1, new PreLaunchAction(mechGlob, countBalls)) .setTangent(Math.toRadians(-90)) .splineToSplineHeading(new Pose2d(54, 12, Math.toRadians(157.5)), Math.toRadians(-90)) //go to launch position .stopAndAdd(new LaunchAction(mechGlob, countBalls)); @@ -192,7 +201,6 @@ public void runOpMode() { // Text menu for FastBot - // Text menu for SlowBot menu.add(new MenuHeader("AUTO SETUP")) .add() // empty line for spacing @@ -210,7 +218,6 @@ public void runOpMode() { .add() .add("finish-button-1", new MenuFinishedButton()); - while (!menu.isCompleted() && !isStopRequested()) { // get x, y (stick) and select (A) input from controller // on Wily Works, this is x, y (wasd) and select (enter) on the keyboard @@ -222,13 +229,14 @@ public void runOpMode() { } telemetry.update(); } - GetColor countBalls = new GetColor(pattern); Alliance chosenAlliance = menu.getResult(Alliance.class, "alliance-picker-1"); SlowBotMovement chosenMovement = menu.getResult(SlowBotMovement.class, "movement-picker-1"); double waitTime = menu.getResult(Double.class, "wait-slider-1"); double intakeCycles = menu.getResult(Double.class, "intake-slider"); + + if (chosenMovement == SlowBotMovement.NEAR) { mechGlob.setLaunchVelocity(LaunchDistance.NEAR); } else { @@ -258,8 +266,6 @@ public void runOpMode() { break; } trajectoryAction = getPath(chosenMovement, chosenAlliance, intakeCycles, drive, mechGlob, countBalls); - - // Get a preview of the trajectory's path: Canvas previewCanvas = new Canvas(); trajectoryAction.preview(previewCanvas); @@ -269,6 +275,10 @@ public void runOpMode() { MecanumDrive.sendTelemetryPacket(packet); + // Get a preview of the trajectory's path: + + + // Assume unknown pattern unless detected otherwise. pattern = Pattern.UNKNOWN; pattern = Pattern.PPG; //temporary until hankang limelight @@ -295,11 +305,7 @@ public void runOpMode() { } } } - if(chosenMovement == SlowBotMovement.NEAR) { - mechGlob.setLaunchVelocity(LaunchDistance.NEAR); - } else { - mechGlob.setLaunchVelocity(LaunchDistance.FAR); - } + sleep((long) waitTime * 1000); boolean more = true; while (opModeIsActive() && more) { @@ -330,20 +336,39 @@ public LaunchAction(MechGlob mechGlob, GetColor orderCount) { this.mechGlob = mechGlob; this.pattern = Pattern.PPG; this.orderCount = orderCount; - - - + } + public boolean hasColor(RequestedColor requestedColor) { + ArrayList array = new ArrayList<>(); + array.add(mechGlob.getSlotColor(0)); + array.add(mechGlob.getSlotColor(1)); + array.add(mechGlob.getSlotColor(2)); + return array.contains(requestedColor.toString()); } @Override public boolean run(double elapsedTime) { if (elapsedTime == 0) { - mechGlob.launch(orderCount.getColor()); - orderCount.increment(); - mechGlob.launch(orderCount.getColor()); - orderCount.increment(); - mechGlob.launch(orderCount.getColor()); - orderCount.increment(); + if (hasColor(orderCount.getColor())) { + mechGlob.launch(orderCount.getColor()); + orderCount.increment(); + } else if (hasColor(RequestedColor.EITHER)) { + mechGlob.launch(RequestedColor.EITHER); + orderCount.increment(); + } + if (hasColor(orderCount.getColor())) { + mechGlob.launch(orderCount.getColor()); + orderCount.increment(); + } else if (hasColor(RequestedColor.EITHER)) { + mechGlob.launch(RequestedColor.EITHER); + orderCount.increment(); + } + if (hasColor(orderCount.getColor())) { + mechGlob.launch(orderCount.getColor()); + orderCount.increment(); + } else if (hasColor(RequestedColor.EITHER)) { + mechGlob.launch(RequestedColor.EITHER); + orderCount.increment(); + } } return !mechGlob.isDoneLaunching(); //we are done } @@ -357,10 +382,22 @@ public PreLaunchAction(MechGlob mechGlob, GetColor orderCount) { this.orderCount = orderCount; this.mechGlob = mechGlob; } + public boolean hasColor(RequestedColor requestedColor) { + ArrayList array = new ArrayList<>(); + array.add(mechGlob.getSlotColor(0)); + array.add(mechGlob.getSlotColor(1)); + array.add(mechGlob.getSlotColor(2)); + return array.contains(requestedColor.toString()); + } @Override public boolean run(double elapsedTime) { - return mechGlob.preLaunch(orderCount.getColor()); + if (hasColor(orderCount.getColor())) { + mechGlob.preLaunch(orderCount.getColor()); + } else if (hasColor(RequestedColor.EITHER)) { + mechGlob.preLaunch(RequestedColor.EITHER); + } + return false; } } From b76abcc66046608f9f977ac7ed7d13661a0d13bb Mon Sep 17 00:00:00 2001 From: anya-codes Date: Mon, 8 Dec 2025 19:15:22 -0800 Subject: [PATCH 143/198] Changed String arrays in CompetitionAuto to PixelColor arrays because of the change made to the return type of getSlotColor. Also changed telemetry display to HTML in auto --- .../org/firstinspires/ftc/team417/CompetitionAuto.java | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java index 011d1e4edbdb..bb50f6db0004 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java @@ -14,6 +14,7 @@ import com.wilyworks.common.WilyWorks; +import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.team417.apriltags.LimelightDetector; import org.firstinspires.ftc.team417.apriltags.Pattern; import org.firstinspires.ftc.team417.javatextmenu.MenuFinishedButton; @@ -187,10 +188,10 @@ public Action getPath(SlowBotMovement chosenMovement, Alliance chosenAlliance, d @Override public void runOpMode() { + telemetry.setDisplayFormat(Telemetry.DisplayFormat.HTML); Pose2d startPose = new Pose2d(0, 0, 0); - Pose2d SBNearStartPose = new Pose2d(-60, 48, Math.toRadians(139)); Pose2d SBFarStartPose = new Pose2d(60, 12, Math.toRadians(157.5)); MecanumDrive drive = new MecanumDrive(hardwareMap, telemetry, gamepad1, startPose); @@ -345,7 +346,7 @@ public LaunchAction(MechGlob mechGlob, GetColor orderCount) { this.orderCount = orderCount; } public boolean hasColor(RequestedColor requestedColor) { - ArrayList array = new ArrayList<>(); + ArrayList array = new ArrayList<>(); array.add(mechGlob.getSlotColor(0)); array.add(mechGlob.getSlotColor(1)); array.add(mechGlob.getSlotColor(2)); @@ -390,7 +391,7 @@ public PreLaunchAction(MechGlob mechGlob, GetColor orderCount) { this.mechGlob = mechGlob; } public boolean hasColor(RequestedColor requestedColor) { - ArrayList array = new ArrayList<>(); + ArrayList array = new ArrayList<>(); array.add(mechGlob.getSlotColor(0)); array.add(mechGlob.getSlotColor(1)); array.add(mechGlob.getSlotColor(2)); From 857b2092144619a2cb51d3c302c52a55f6201f1b Mon Sep 17 00:00:00 2001 From: Sameer Date: Mon, 8 Dec 2025 19:48:31 -0800 Subject: [PATCH 144/198] symettry problem fixed, error cases handled better --- .../ftc/team417/CompetitionAuto.java | 40 +++++-------------- 1 file changed, 9 insertions(+), 31 deletions(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java index bb50f6db0004..c354c78b7b96 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java @@ -345,36 +345,23 @@ public LaunchAction(MechGlob mechGlob, GetColor orderCount) { this.pattern = Pattern.PPG; this.orderCount = orderCount; } - public boolean hasColor(RequestedColor requestedColor) { - ArrayList array = new ArrayList<>(); - array.add(mechGlob.getSlotColor(0)); - array.add(mechGlob.getSlotColor(1)); - array.add(mechGlob.getSlotColor(2)); - return array.contains(requestedColor.toString()); - } @Override public boolean run(double elapsedTime) { if (elapsedTime == 0) { - if (hasColor(orderCount.getColor())) { - mechGlob.launch(orderCount.getColor()); + if (mechGlob.launch(orderCount.getColor())) { orderCount.increment(); - } else if (hasColor(RequestedColor.EITHER)) { - mechGlob.launch(RequestedColor.EITHER); + } else if (mechGlob.launch(RequestedColor.EITHER)) { orderCount.increment(); } - if (hasColor(orderCount.getColor())) { - mechGlob.launch(orderCount.getColor()); + if (mechGlob.launch(orderCount.getColor())) { orderCount.increment(); - } else if (hasColor(RequestedColor.EITHER)) { - mechGlob.launch(RequestedColor.EITHER); + } else if (mechGlob.launch(RequestedColor.EITHER)) { orderCount.increment(); } - if (hasColor(orderCount.getColor())) { - mechGlob.launch(orderCount.getColor()); + if (mechGlob.launch(orderCount.getColor())) { orderCount.increment(); - } else if (hasColor(RequestedColor.EITHER)) { - mechGlob.launch(RequestedColor.EITHER); + } else if (mechGlob.launch(RequestedColor.EITHER)) { orderCount.increment(); } } @@ -390,21 +377,11 @@ public PreLaunchAction(MechGlob mechGlob, GetColor orderCount) { this.orderCount = orderCount; this.mechGlob = mechGlob; } - public boolean hasColor(RequestedColor requestedColor) { - ArrayList array = new ArrayList<>(); - array.add(mechGlob.getSlotColor(0)); - array.add(mechGlob.getSlotColor(1)); - array.add(mechGlob.getSlotColor(2)); - return array.contains(requestedColor.toString()); - } + @Override public boolean run(double elapsedTime) { - if (hasColor(orderCount.getColor())) { - mechGlob.preLaunch(orderCount.getColor()); - } else if (hasColor(RequestedColor.EITHER)) { - mechGlob.preLaunch(RequestedColor.EITHER); - } + mechGlob.preLaunch(orderCount.getColor()); return false; } } @@ -449,6 +426,7 @@ public void increment() { } + public RequestedColor getColor() { return array[orderCount]; } From fb377b8b4171ffd8531583f95d67e32c9d186282 Mon Sep 17 00:00:00 2001 From: Sameer Date: Mon, 8 Dec 2025 20:33:48 -0800 Subject: [PATCH 145/198] check in CMC --- .../ftc/team417/CompetitionAuto.java | 24 ++++++++++++------- .../ftc/team417/ComplexMechGlob.java | 8 +++++-- 2 files changed, 22 insertions(+), 10 deletions(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java index c354c78b7b96..ed6db27b9192 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java @@ -55,11 +55,7 @@ enum SlowBotMovement { public Action getPath(SlowBotMovement chosenMovement, Alliance chosenAlliance, double intakeCycles, MecanumDrive drive, MechGlob mechGlob, GetColor countBalls) { - Pose2d startPose = new Pose2d(0, 0, 0); - - Pose2d SBNearStartPose = new Pose2d(-60, 48, Math.toRadians(139)); - Pose2d SBFarStartPose = new Pose2d(60, 12, Math.toRadians(157.5)); PoseMap poseMap = pose -> new Pose2dDual<>( @@ -75,7 +71,7 @@ public Action getPath(SlowBotMovement chosenMovement, Alliance chosenAlliance, d TrajectoryActionBuilder trajectoryAction = null; switch (chosenMovement) { case NEAR: - trajectoryAction = drive.actionBuilder(SBNearStartPose, poseMap); + trajectoryAction = drive.actionBuilder(drive.pose, poseMap); trajectoryAction = trajectoryAction.setTangent(Math.toRadians(-51)) .afterDisp(0,new PreLaunchAction(mechGlob, countBalls)) .splineToConstantHeading(new Vector2d(-12, 12), Math.toRadians(-51)) @@ -120,7 +116,7 @@ public Action getPath(SlowBotMovement chosenMovement, Alliance chosenAlliance, d break; case FAR: - trajectoryAction = drive.actionBuilder(SBFarStartPose, poseMap); + trajectoryAction = drive.actionBuilder(drive.pose, poseMap); if (intakeCycles == 0) { trajectoryAction = trajectoryAction.setTangent(Math.toRadians(180)) .stopAndAdd(new LaunchAction(mechGlob, countBalls)); @@ -164,7 +160,7 @@ public Action getPath(SlowBotMovement chosenMovement, Alliance chosenAlliance, d case FAR_OUT_OF_WAY: // 3 launch actions // after disp intake action - trajectoryAction = drive.actionBuilder(SBFarStartPose, poseMap); + trajectoryAction = drive.actionBuilder(drive.pose, poseMap); trajectoryAction = trajectoryAction.setTangent(Math.toRadians(180)) .splineToLinearHeading(new Pose2d(60, 61, Math.toRadians(0)), Math.toRadians(0)) .setTangent(Math.toRadians(-90)) @@ -174,7 +170,7 @@ public Action getPath(SlowBotMovement chosenMovement, Alliance chosenAlliance, d .splineToLinearHeading(new Pose2d(50, 32, Math.toRadians(180)), Math.toRadians(180)); break; case FAR_MINIMAL: - trajectoryAction = drive.actionBuilder(SBFarStartPose, poseMap); + trajectoryAction = drive.actionBuilder(drive.pose, poseMap); trajectoryAction = trajectoryAction.setTangent(Math.toRadians(90)) .splineToLinearHeading(new Pose2d(48, 32, Math.toRadians(180)), Math.toRadians(180)); @@ -267,6 +263,18 @@ public void runOpMode() { drive.setPose(SBFarStartPose); break; } + + // this lets us move the robot to see the obelisk before start and after init + while (opModeIsActive()) { + telemetry.addLine("Ok to move \n A to start"); + telemetry.addData("Last valid pattern: ", pattern); + telemetry.update(); + if (gamepad1.aWasPressed()) { + break; + } + + } + trajectoryAction = getPath(chosenMovement, chosenAlliance, intakeCycles, drive, mechGlob, countBalls); Canvas previewCanvas = new Canvas(); trajectoryAction.preview(previewCanvas); diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java b/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java index 28dac7f23fa1..6be7722f86d9 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java @@ -57,7 +57,9 @@ static MechGlob create (HardwareMap hardwareMap, Telemetry telemetry, PixelColor void intake (double intakeValue){} //a method that determines what color to launch. Options are purple, green, or either. - void launch (RequestedColor requestedColor) {} + boolean launch (RequestedColor requestedColor) { + return true; + } void update () {} @@ -245,13 +247,15 @@ void intake (double intakeSpeed) { @Override //a class that controls the launcher and transfer - void launch (RequestedColor requestedColor) { + boolean launch (RequestedColor requestedColor) { int minSlot = findNearestSlot(LAUNCH_POSITIONS, requestedColor); if (minSlot != -1){ addToDrumQueue(LAUNCH_POSITIONS[minSlot], WaitState.SPIN_UP); slotOccupiedBy.set (minSlot, PixelColor.NONE); //marking this slot as empty so we don't accidentally try to use it again + return true; } + return false; } //this function adds a new drum request to the drum queue. nextState is the state do use after the drum is finished moving void addToDrumQueue(double position, WaitState nextState){ From efbb111120effa2d961d56daea2755d5a7511a5d Mon Sep 17 00:00:00 2001 From: Sameer Date: Mon, 8 Dec 2025 20:47:06 -0800 Subject: [PATCH 146/198] check in CMC --- .../java/org/firstinspires/ftc/team417/CompetitionAuto.java | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java index ed6db27b9192..15adf2494d84 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionAuto.java @@ -263,11 +263,9 @@ public void runOpMode() { drive.setPose(SBFarStartPose); break; } - // this lets us move the robot to see the obelisk before start and after init while (opModeIsActive()) { telemetry.addLine("Ok to move \n A to start"); - telemetry.addData("Last valid pattern: ", pattern); telemetry.update(); if (gamepad1.aWasPressed()) { break; @@ -290,8 +288,7 @@ public void runOpMode() { // Assume unknown pattern unless detected otherwise. - pattern = Pattern.UNKNOWN; - pattern = Pattern.PPG; //temporary until hankang limelight + // Detect the pattern with the AprilTags from the camera! // Wait for Start to be pressed on the Driver Hub! // (This try-with-resources statement automatically calls detector.close() when it exits From 31a3baf0c66c1564d6140c2f004178e8e76a35f2 Mon Sep 17 00:00:00 2001 From: anya-codes Date: Mon, 8 Dec 2025 21:01:15 -0800 Subject: [PATCH 147/198] Added code to log last 9 color sensor readings in telemetry. Also tuned some of the teleop constants. Added a new wait state DrumMoveWait but code for that needs to be added --- .../ftc/team417/ComplexMechGlob.java | 31 ++++++++++--------- .../ftc/team417/CoolColorDetector.java | 12 ++++--- 2 files changed, 23 insertions(+), 20 deletions(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java b/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java index 28dac7f23fa1..99d52d1676c0 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java @@ -83,21 +83,21 @@ void controlDrumManually () {} @Config public class ComplexMechGlob extends MechGlob { //a class encompassing all code that IS for slowbot // TODO tune constants via FTC Dashboard: - static double FEEDER_POWER = 1; - static double TRANSFER_TIME_UP = 2; - static double TRANSFER_TIME_TOTAL = 5; //TRANSFER_TIME_TOTAL must be more than TRANSFER_TIME_UP - static double FAR_FLYWHEEL_VELOCITY = 933; //was 1500 - static double NEAR_FLYWHEEL_VELOCITY = 933; //was 1500 - static double FLYWHEEL_BACK_SPIN = 150; //was 300 - static double TRANSFER_INACTIVE_POSITION = 0.45; - static double TRANSFER_ACTIVE_POSITION = 0.7; - static double REVERSE_INTAKE_SPEED = -1; - static double INTAKE_SPEED = 1; - static double FLYWHEEL_VELOCITY_TOLERANCE = 25; //this is an epsiiiiiiiiilon - static double VOLTAGE_TOLERANCE = 0.01; //THIS IS AN EPSILON AS WELLLLLL - static double DRUM_GATE_OPEN_POSITION = 1; - static double DRUM_GATE_CLOSED_POSITION = 0.7; - static double MOTOR_D_VALUE = 1; + public static double FEEDER_POWER = 1; + public static double TRANSFER_TIME_UP = 0.5; + public static double TRANSFER_TIME_TOTAL = 1; //TRANSFER_TIME_TOTAL must be more than TRANSFER_TIME_UP + public static double FAR_FLYWHEEL_VELOCITY = 933; //was 1500 + public static double NEAR_FLYWHEEL_VELOCITY = 933; //was 1500 + public static double FLYWHEEL_BACK_SPIN = 150; //was 300 + public static double TRANSFER_INACTIVE_POSITION = 0.45; + public static double TRANSFER_ACTIVE_POSITION = 0.7; + public static double REVERSE_INTAKE_SPEED = -1; + public static double INTAKE_SPEED = 1; + public static double FLYWHEEL_VELOCITY_TOLERANCE = 25; //this is an epsiiiiiiiiilon + public static double VOLTAGE_TOLERANCE = 0.01; //THIS IS AN EPSILON AS WELLLLLL + public static double DRUM_GATE_OPEN_POSITION = 1; + public static double DRUM_GATE_CLOSED_POSITION = 0.6555; + public static double MOTOR_D_VALUE = 1; ElapsedTime transferTimer; @@ -106,6 +106,7 @@ public class ComplexMechGlob extends MechGlob { //a class encompassing all code ArrayList slotOccupiedBy = new ArrayList<> (Collections.nCopies(3, PixelColor.NONE)); enum WaitState { + DRUM_MOVE_WAIT, //waiting for the ball to fully enter the slot before moving the drum DRUM_MOVE, //waiting for the drum to reach desired position INTAKE, //waiting for the intake to finish TRANSFER, //waiting for the transfer to finish diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CoolColorDetector.java b/team417/src/main/java/org/firstinspires/ftc/team417/CoolColorDetector.java index 12a721323d06..6ecd9f2e411f 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CoolColorDetector.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CoolColorDetector.java @@ -17,8 +17,7 @@ public class CoolColorDetector { public static double MINIMUM_DISTANCE = 60; //25mm public static double PURPLE_MIN_HUE = 200; public static double PURPLE_MAX_HUE = 225; - public static double GREEN_MIN_HUE = 165; - + public static double GREEN_MIN_HUE = 155; public static double GREEN_MAX_HUE = 180; @@ -60,9 +59,10 @@ PixelColor detectArtifactColor() { String colorCube = String.format("\u25a0", colors.toColor() & 0xffffff); - telemetry.addLine(String.format("Color Detect: %.2fmm, %.2fmm %s, Hue: %.1f", - distance1, distance2, colorCube, hue)); - telemetry.addLine(String.format(" %.2f\", %.2f\"", distance1, distance2)); + String string = String.format("Color Detect: %.2fmm, %.2fmm %s, Hue: %.1f", + distance1, distance2, colorCube, hue); + telemetry.log().add(string); + telemetry.addLine(string); if (hue > GREEN_MIN_HUE && hue < GREEN_MAX_HUE) { //range determined from testing @@ -73,6 +73,8 @@ PixelColor detectArtifactColor() { //error case use the most likely color return PixelColor.PURPLE; } + + } public void testTelemetry() { From 0437726a674147de61eb07670a5981c10f66364b Mon Sep 17 00:00:00 2001 From: Andrew Date: Tue, 9 Dec 2025 13:43:15 -0800 Subject: [PATCH 148/198] Update to latest drop of Wily Works. --- .../external/samples/ConceptAprilTagEasy.java | 42 +------ .../com/wilyworks/simulator/WilyCore.java | 38 +++--- .../simulator/framework/MechSim.java | 14 ++- .../simulator/framework/WilyTelemetry.java | 88 ++++++++++--- .../ftc/teamcode/wilyworks/WilyTelemetry.java | 116 ------------------ .../java/com/wilyworks/common/WilyWorks.java | 3 +- 6 files changed, 110 insertions(+), 191 deletions(-) delete mode 100644 WilyCore/src/main/java/org/firstinspires/ftc/teamcode/wilyworks/WilyTelemetry.java diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagEasy.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagEasy.java index f7c3733c13d9..12dcf6e99d21 100644 --- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagEasy.java +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagEasy.java @@ -29,8 +29,6 @@ package org.firstinspires.ftc.robotcontroller.external.samples; -import android.util.Size; - import com.qualcomm.robotcore.eventloop.opmode.Disabled; import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; @@ -123,45 +121,15 @@ private void initAprilTag() { // Create the AprilTag processor the easy way. aprilTag = AprilTagProcessor.easyCreateWithDefaults(); -// // Create the vision portal the easy way. -// if (USE_WEBCAM) { -// visionPortal = VisionPortal.easyCreateWithDefaults( -// hardwareMap.get(WebcamName.class, "Webcam 1"), aprilTag); -// } else { -// visionPortal = VisionPortal.easyCreateWithDefaults( -// BuiltinCameraDirection.BACK, aprilTag); -// } - - VisionPortal.Builder builder = new VisionPortal.Builder(); - - // Set the camera (webcam vs. built-in RC phone camera). + // Create the vision portal the easy way. if (USE_WEBCAM) { - builder.setCamera(hardwareMap.get(WebcamName.class, "Webcam 1")); + visionPortal = VisionPortal.easyCreateWithDefaults( + hardwareMap.get(WebcamName.class, "Webcam 1"), aprilTag); } else { - builder.setCamera(BuiltinCameraDirection.BACK); + visionPortal = VisionPortal.easyCreateWithDefaults( + BuiltinCameraDirection.BACK, aprilTag); } - // Choose a camera resolution. Not all cameras support all resolutions. - builder.setCameraResolution(new Size(1920, 1080)); - - // Enable the RC preview (LiveView). Set "false" to omit camera monitoring. - //builder.enableLiveView(true); - - // Set the stream format; MJPEG uses less bandwidth than default YUY2. - //builder.setStreamFormat(VisionPortal.StreamFormat.YUY2); - - // Choose whether or not LiveView stops if no processors are enabled. - // If set "true", monitor shows solid orange screen if no processors enabled. - // If set "false", monitor shows camera view without annotations. - //builder.setAutoStopLiveView(false); - - // Set and enable the processor. - builder.addProcessor(aprilTag); - - // Build the Vision Portal, using the above settings. - visionPortal = builder.build(); - - } // end method initAprilTag() /** diff --git a/WilyCore/src/main/java/com/wilyworks/simulator/WilyCore.java b/WilyCore/src/main/java/com/wilyworks/simulator/WilyCore.java index 6efb1a0b9ef4..8af65337e3e0 100644 --- a/WilyCore/src/main/java/com/wilyworks/simulator/WilyCore.java +++ b/WilyCore/src/main/java/com/wilyworks/simulator/WilyCore.java @@ -11,16 +11,15 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.OpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.Gamepad; import com.qualcomm.robotcore.hardware.HardwareMap; import com.wilyworks.common.Wily; import com.wilyworks.common.WilyWorks; -import com.wilyworks.simulator.framework.MechSim; +import com.wilyworks.simulator.framework.Field; import com.wilyworks.simulator.framework.InputManager; +import com.wilyworks.simulator.framework.MechSim; import com.wilyworks.simulator.framework.Simulation; import com.wilyworks.simulator.framework.WilyTelemetry; -import com.qualcomm.robotcore.hardware.Gamepad; - -import com.wilyworks.simulator.framework.Field; import org.firstinspires.ftc.robotcore.external.Telemetry; import org.reflections.Reflections; @@ -170,6 +169,7 @@ public void windowClosing(WindowEvent e) { case STARTED: WilyCore.opModeThread.interrupt(); WilyCore.status = new WilyCore.Status(WilyCore.State.STOPPED, null, null); + WilyCore.terminateOpMode(); button.setText("Init"); dropDown.setMaximumSize(new Dimension(400, 100)); dropDown.setVisible(true); @@ -318,7 +318,7 @@ static public void render(boolean startScreenOverlay) { dashboardWindow.errorLabel.setText(simulation.getErrorLabel()); } - // Advance the time: + // Advance the time. Zero means to use the realtime clock. static double advanceTime(double deltaT) { if (deltaT <= 0) { deltaT = nanoTime() * 1e-9 - lastUpdateWallClockTime; @@ -339,24 +339,22 @@ static double advanceTime(double deltaT) { // Callbacks provided to the guest. These are all called via reflection from the WilyWorks // class. - // The guest can specify the delta-t (which is handy when single stepping): + // Set the robot to a given pose and (optional) velocity in the simulation. The + // localizer will not register a move. + static public void setStartPose(Pose2d pose, PoseVelocity2d velocity) { + lastUpdateWallClockTime = nanoTime() * 1e-9; // Reset the detla-t calculations + simulation.setStartPose(pose, velocity); + } + + // The guest can specify the delta-t (which is handy when single stepping). Zero means to + // use the real-time clock. static public void updateSimulation(double deltaT) { // Advance the time then advance the simulation: deltaT = advanceTime(deltaT); simulation.advance(deltaT); mechSim.advance(deltaT); - - // Render everything: - render(); - simulationUpdated = true; - } - - // Set the robot to a given pose and (optional) velocity in the simulation. The - // localizer will not register a move. - static public void setStartPose(Pose2d pose, PoseVelocity2d velocity) { - lastUpdateWallClockTime = nanoTime() * 1e-9; // Reset the detla-t calculations - simulation.setStartPose(pose, velocity); + render(); } // MecanumDrive uses this while running a trajectory to update the simulator as to its @@ -561,6 +559,12 @@ public static Image getImage(String imagePath, int width, int height) { return image; } + // Do some cleanup when an OpMode is terminated: + public static void terminateOpMode() { + telemetry.clearAll(); + telemetry.log().clear(); + } + //////////////////////////////////////////////////////////////////////////////////////////////// // This is the application entry point that starts up all of Wily Works! public static void main(String[] args) diff --git a/WilyCore/src/main/java/com/wilyworks/simulator/framework/MechSim.java b/WilyCore/src/main/java/com/wilyworks/simulator/framework/MechSim.java index 100e5c0aff5e..1afe1cc16372 100644 --- a/WilyCore/src/main/java/com/wilyworks/simulator/framework/MechSim.java +++ b/WilyCore/src/main/java/com/wilyworks/simulator/framework/MechSim.java @@ -219,7 +219,7 @@ enum BallColor {PURPLE, GREEN, GOLD} final double[] INTAKE_POSITIONS = { 0.0/6, 2.0/6, 4.0/6 }; // AKA 'launch' positions final double[] TRANSFER_POSITIONS = { 3.0/6, 5.0/6, 1.0/6 }; // Servo positions for intaking final double SLOT_EPSILON = 0.02; // Epsilon for determining a slot relative to a [0, 1] range - final double MIN_TRANSFER_TIME = 0.1; // Second it takes for a transfer + final double MIN_TRANSFER_TIME = 0.020; // Second it takes for a transfer final double MIN_TRANSFER_POSITION = 0.6; // Minimum position to start a transfer final double TRANSFER_SERVO_SPEED = (60.0 / 360) / 0.25; // Speed of a goBilda torque servo, position/s final double LAUNCH_SPEED = 144; // Ball launch speed, inches per second @@ -232,6 +232,7 @@ enum BallColor {PURPLE, GREEN, GOLD} final double LAUNCH_ACCELERATION = 1000; // Increase flywheel speed by this many ticks per second final double LAUNCH_DROP = 500; // Drop flywheel speed by this many ticks on launch final double LAUNCH_EPSILON = 50; // Target and actual flywheel velocities must be within this amount + final double INTAKE_ERROR_PROBABILITY = 0.2; // Probability that a ball is missed on intake // Struct for tracking ball locations: static class Ball { @@ -607,8 +608,15 @@ void simulate(Pose2d pose, double deltaT) { for (Ball ball : fieldBalls) { double distance = Math.hypot(ball.point.x - intakePoint.x, ball.point.y - intakePoint.y); if (distance < INTAKE_EPSILON) { - intakeBall = ball; - fieldBalls.remove(ball); // I think this is okay if we terminate the loop... + // Remove the ball from the field. I think this is okay so long as we + // terminate the loop... + fieldBalls.remove(ball); + + // Move the ball into the intake position, unless random error says + // to toss it: + if ((!WilyCore.enableSensorError) || (Math.random() > INTAKE_ERROR_PROBABILITY)) { + intakeBall = ball; + } break; } } diff --git a/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyTelemetry.java b/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyTelemetry.java index 6ef7048ef057..b5aac6ef48f8 100644 --- a/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyTelemetry.java +++ b/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyTelemetry.java @@ -1,18 +1,17 @@ package com.wilyworks.simulator.framework; +import static org.firstinspires.ftc.robotcore.external.Telemetry.DisplayFormat; import static java.awt.Font.MONOSPACED; import static java.awt.font.TextAttribute.POSTURE_REGULAR; import static java.awt.font.TextAttribute.WEIGHT_REGULAR; -import org.firstinspires.ftc.robotcore.external.Func; -import org.firstinspires.ftc.robotcore.external.Telemetry; - -import static org.firstinspires.ftc.robotcore.external.Telemetry.DisplayFormat; - import androidx.annotation.Nullable; import com.qualcomm.robotcore.robocol.TelemetryMessage; +import org.firstinspires.ftc.robotcore.external.Func; +import org.firstinspires.ftc.robotcore.external.Telemetry; + import java.awt.Canvas; import java.awt.Color; import java.awt.Graphics2D; @@ -517,12 +516,66 @@ public Telemetry.Item addData(String caption, String format, Func valuePr } } +class WilyLog implements Telemetry.Log { + LinkedList lines = new LinkedList<>(); + DisplayOrder displayOrder = DisplayOrder.OLDEST_FIRST; + int capacity = 9; + + @Override + public int getCapacity() { + return capacity; + } + + @Override + public void setCapacity(int capacity) { + this.capacity = capacity; + } + + @Override + public DisplayOrder getDisplayOrder() { + return displayOrder; + } + + @Override + public void setDisplayOrder(DisplayOrder displayOrder) { + this.displayOrder = displayOrder; + } + + @Override + public void add(String entry) { + lines.add(entry); + if (lines.size() > capacity) + lines.removeFirst(); + } + + @Override + public void add(String format, Object... args) { + add(String.format(format, args)); + } + + @Override + public void clear() { + lines.clear(); + } + + // This is a private function called by Telemetry.update() to add all of the lines + // of the log to the telemetry: + public void get(List telemetry) { + if (displayOrder == DisplayOrder.OLDEST_FIRST) { + telemetry.addAll(lines); + } else { + Iterator iterator = lines.descendingIterator(); + while (iterator.hasNext()) { + telemetry.add(iterator.next()); + } + } + } +} + /** * This class implements a lightweight emulation of FTC Telemetry that can run on the PC. */ public class WilyTelemetry implements Telemetry { - final int MAX_LINES = 36; // It's 18 with a regular sized font - // Global state: public static WilyTelemetry instance; @@ -532,6 +585,7 @@ public class WilyTelemetry implements Telemetry { ArrayList lineList = new ArrayList<>(); DisplayFormat displayFormat = DisplayFormat.CLASSIC; // HTML vs. monospace modes Layout layout = new Layout(); + WilyLog log = new WilyLog(); // Wily Works constructor for a Telemetry object: public WilyTelemetry(java.awt.Image icon) { @@ -544,15 +598,13 @@ public WilyTelemetry(java.awt.Image icon) { } public Line addLine(String string) { - if (lineList.size() <= MAX_LINES) { - int newLineIndex; - while ((newLineIndex = string.indexOf("\n")) != -1) { - String line = string.substring(0, newLineIndex); - lineList.add(line); - string = string.substring(newLineIndex + 1); - } - lineList.add(string); + int newLineIndex; + while ((newLineIndex = string.indexOf("\n")) != -1) { + String line = string.substring(0, newLineIndex); + lineList.add(line); + string = string.substring(newLineIndex + 1); } + lineList.add(string); return new WilyLine(this); } @@ -600,11 +652,12 @@ public void setDisplayFormat(DisplayFormat displayFormat) { @Override public Log log() { - return null; + return log; } public Item addData(String caption, Object value) { - addLine(String.format("%s : %s", caption, value.toString())); + String string = (value == null) ? "null" : value.toString(); + addLine(String.format("%s : %s", caption, string)); return null; // ### } @@ -651,6 +704,7 @@ public void speak(String text, String languageCode, String countryCode) { } public boolean update() { Graphics2D g = (Graphics2D) canvas.getBufferStrategy().getDrawGraphics(); g.clearRect(0, 0, canvas.getWidth(), canvas.getHeight()); + log.get(lineList); // Append the log to the line list layout.parseAndRender(g, displayFormat, lineList); g.dispose(); diff --git a/WilyCore/src/main/java/org/firstinspires/ftc/teamcode/wilyworks/WilyTelemetry.java b/WilyCore/src/main/java/org/firstinspires/ftc/teamcode/wilyworks/WilyTelemetry.java deleted file mode 100644 index cbb121228e26..000000000000 --- a/WilyCore/src/main/java/org/firstinspires/ftc/teamcode/wilyworks/WilyTelemetry.java +++ /dev/null @@ -1,116 +0,0 @@ -package org.firstinspires.ftc.teamcode.wilyworks; - -import org.firstinspires.ftc.robotcore.external.Func; -import org.firstinspires.ftc.robotcore.external.Telemetry; - -/** - * This is the Wily Works implementation of the standard FTC Telemetry object. - */ -public class WilyTelemetry implements Telemetry { - @Override - public Item addData(String caption, String format, Object... args) { - return null; - } - - @Override - public Item addData(String caption, Object value) { - return null; - } - - @Override - public Item addData(String caption, Func valueProducer) { - return null; - } - - @Override - public Item addData(String caption, String format, Func valueProducer) { - return null; - } - - @Override - public boolean removeItem(Item item) { - return false; - } - - @Override - public void clear() { } - - @Override - public void clearAll() { } - - @Override - public Object addAction(Runnable action) { - return null; - } - - @Override - public boolean removeAction(Object token) { - return false; - } - - @Override - public void speak(String text) { } - - @Override - public void speak(String text, String languageCode, String countryCode) { } - - @Override - public boolean update() { - return false; - } - - @Override - public Line addLine() { - return null; - } - - @Override - public Line addLine(String lineCaption) { - return null; - } - - @Override - public boolean removeLine(Line line) { - return false; - } - - @Override - public boolean isAutoClear() { - return false; - } - - @Override - public void setAutoClear(boolean autoClear) { } - - @Override - public int getMsTransmissionInterval() { - return 0; - } - - @Override - public void setMsTransmissionInterval(int msTransmissionInterval) { } - - @Override - public String getItemSeparator() { - return null; - } - - @Override - public void setItemSeparator(String itemSeparator) { } - - @Override - public String getCaptionValueSeparator() { - return null; - } - - @Override - public void setCaptionValueSeparator(String captionValueSeparator) { } - - @Override - public void setDisplayFormat(DisplayFormat displayFormat) { } - - @Override - public Log log() { - return null; - } -} diff --git a/WilyWorks/src/main/java/com/wilyworks/common/WilyWorks.java b/WilyWorks/src/main/java/com/wilyworks/common/WilyWorks.java index 2988d8f3e235..43394ddc6738 100644 --- a/WilyWorks/src/main/java/com/wilyworks/common/WilyWorks.java +++ b/WilyWorks/src/main/java/com/wilyworks/common/WilyWorks.java @@ -277,7 +277,8 @@ static public Twist2dDual

+ * Example: + * 

{@code
+ *  public abstract void setTextColor(@ColorInt int color);
+ * }
+ */ +@Documented +@Retention(CLASS) +@Target({PARAMETER, METHOD, LOCAL_VARIABLE, FIELD}) +public @interface ColorInt { +} diff --git a/WilyCore/src/main/java/com/qualcomm/ftccommon/FtcEventLoop.java b/WilyCore/src/main/java/com/qualcomm/ftccommon/FtcEventLoop.java index ec625044b355..1f0ba0255db0 100644 --- a/WilyCore/src/main/java/com/qualcomm/ftccommon/FtcEventLoop.java +++ b/WilyCore/src/main/java/com/qualcomm/ftccommon/FtcEventLoop.java @@ -4,6 +4,6 @@ public class FtcEventLoop { public OpModeManagerImpl getOpModeManager() { - return null; + return new OpModeManagerImpl(); } } diff --git a/WilyCore/src/main/java/com/qualcomm/hardware/limelightvision/Limelight3A.java b/WilyCore/src/main/java/com/qualcomm/hardware/limelightvision/Limelight3A.java index 0f01e571c2c1..f75a655c2f5c 100644 --- a/WilyCore/src/main/java/com/qualcomm/hardware/limelightvision/Limelight3A.java +++ b/WilyCore/src/main/java/com/qualcomm/hardware/limelightvision/Limelight3A.java @@ -44,11 +44,9 @@ are permitted (subject to the limitations in the disclaimer below) provided that import java.io.BufferedReader; import java.io.IOException; import java.io.InputStreamReader; -import java.io.OutputStream; import java.net.HttpURLConnection; import java.net.InetAddress; import java.net.URL; -import java.nio.charset.StandardCharsets; import java.util.concurrent.Executors; import java.util.concurrent.ScheduledExecutorService; import java.util.concurrent.TimeUnit; diff --git a/WilyCore/src/main/java/com/qualcomm/hardware/sparkfun/SparkFunLEDStick.java b/WilyCore/src/main/java/com/qualcomm/hardware/sparkfun/SparkFunLEDStick.java new file mode 100644 index 000000000000..330449ec8e50 --- /dev/null +++ b/WilyCore/src/main/java/com/qualcomm/hardware/sparkfun/SparkFunLEDStick.java @@ -0,0 +1,195 @@ +package com.qualcomm.hardware.sparkfun; + +import android.graphics.Color; + +import androidx.annotation.ColorInt; + +import com.qualcomm.robotcore.hardware.I2cAddr; +import com.qualcomm.robotcore.hardware.I2cDeviceSynchDevice; +import com.qualcomm.robotcore.hardware.I2cDeviceSynchSimple; +import com.qualcomm.robotcore.hardware.configuration.annotations.DeviceProperties; +import com.qualcomm.robotcore.hardware.configuration.annotations.I2cDeviceType; + +/** + * Support for the Sparkfun QWIIC LED Stick + * To connect it directly, you need this cable + */ +@I2cDeviceType() +@DeviceProperties(name = "@string/sparkfun_led_stick_name", + description = "@string/sparkfun_led_stick_description", + xmlTag = "QWIIC_LED_STICK") +public class SparkFunLEDStick extends I2cDeviceSynchDevice { + private final static boolean LIMIT_TO_ONE_STICK = true; + private final static int MAX_SEGMENT_LENGTH = 12; + private enum Commands { + CHANGE_LED_LENGTH(0x70), + WRITE_SINGLE_LED_COLOR(0x71), + WRITE_ALL_LED_COLOR(0x72), + WRITE_RED_ARRAY(0x73), + WRITE_GREEN_ARRAY(0x74), + WRITE_BLUE_ARRAY(0x75), + WRITE_SINGLE_LED_BRIGHTNESS(0x76), + WRITE_ALL_LED_BRIGHTNESS(0x77), + WRITE_ALL_LED_OFF(0x78); + final int bVal; + + Commands(int bVal) { + this.bVal = bVal; + } + } + + /** + * Change the color of a specific LED + * + * @param position which LED to change (1 - 255) + * @param color what color to set it to + */ + public void setColor(int position, @ColorInt int color) { + byte[] data = new byte[4]; + data[0] = (byte) position; + data[1] = (byte) Color.red(color); + data[2] = (byte) Color.green(color); + data[3] = (byte) Color.blue(color); + writeI2C(Commands.WRITE_SINGLE_LED_COLOR, data); + } + + /** + * Change the color of all LEDs to a single color + * + * @param color what the color should be + */ + public void setColor(@ColorInt int color) { + byte[] data = new byte[3]; + data[0] = (byte) Color.red(color); + data[1] = (byte) Color.green(color); + data[2] = (byte) Color.blue(color); + writeI2C(Commands.WRITE_ALL_LED_COLOR, data); + } + + /** + * Send a segment of the LED array + * + * @param cmd command to send + * @param array the values (limited from 0..255) + * @param offset the starting value (LED only, array starts at 0) + * @param length the length to send + */ + private void sendSegment(Commands cmd, int[] array, int offset, int length) { + byte[] data = new byte[length + 2]; + data[0] = (byte) length; + data[1] = (byte) offset; + + for (int i = 0; i < length; i++) { + data[2 + i] = (byte) array[i]; + } + writeI2C(cmd, data); + } + + /** + * Change the color of an LED color segment + * + * @param colors what the colors should be + * @param offset where in the array to start + * @param length length to send (limited to 12) + */ + private void setLEDColorSegment(@ColorInt int[] colors, int offset, int length) { + // Copy a segment of elements from the colors array into separate arrays for red, green, and blue. + int[] redArray = new int[length]; + int[] greenArray = new int[length]; + int[] blueArray = new int[length]; + + // Here we iterate [0, length) because we are filling the red, green, and blue arrays. + for (int i = 0; i < length; i++) { + // Use the offset when indexing into the colors array so we get the appropriate segement of elements. + redArray[i] = Color.red(colors[i + offset]); + greenArray[i] = Color.green(colors[i + offset]); + blueArray[i] = Color.blue(colors[i + offset]); + } + sendSegment(Commands.WRITE_RED_ARRAY, redArray, offset, length); + sendSegment(Commands.WRITE_GREEN_ARRAY, greenArray, offset, length); + sendSegment(Commands.WRITE_BLUE_ARRAY, blueArray, offset, length); + } + + /** + * Change the color of all LEDs using arrays + * + * @param colors array of colors to set lights to + */ + public void setColors(@ColorInt int[] colors) { + int length = colors.length; + + if (LIMIT_TO_ONE_STICK && length > 10) { + length = 10; + } + + int numInLastSegment = length % MAX_SEGMENT_LENGTH; + int numSegments = length / MAX_SEGMENT_LENGTH; + for (int i = 0; i < numSegments; i++) { + setLEDColorSegment(colors, i * MAX_SEGMENT_LENGTH, MAX_SEGMENT_LENGTH); + } + if (numInLastSegment > 0) { + setLEDColorSegment(colors, numSegments * MAX_SEGMENT_LENGTH, numInLastSegment); + } + } + + /** + * Set the brightness of an individual LED + * + * @param position which LED to change (1-255) + * @param brightness brightness level (0-31) + */ + public void setBrightness(int position, int brightness) { + byte[] data = new byte[2]; + data[0] = (byte) position; + data[1] = (byte) brightness; + writeI2C(Commands.WRITE_SINGLE_LED_BRIGHTNESS, data); + } + + /** + * Set the brightness of all LEDs + * + * @param brightness brightness level (0-31) + */ + public void setBrightness(int brightness) { + byte[] data = new byte[1]; + data[0] = (byte) brightness; + writeI2C(Commands.WRITE_ALL_LED_BRIGHTNESS, data); + } + + /** + * Turn all LEDS off... + */ + public void turnAllOff() { + setColor(0); + } + + private void writeI2C(Commands cmd, byte[] data) {} +// { +// deviceClient.write(cmd.bVal, data); +// } + + @Override + public Manufacturer getManufacturer() { + return Manufacturer.SparkFun; + } + + @Override + protected synchronized boolean doInitialize() { + return true; + } + + @Override + public String getDeviceName() { + return "SparkFun Qwiic LED Strip"; + } + + private final static I2cAddr ADDRESS_I2C_DEFAULT = I2cAddr.create7bit(0x23); + + public SparkFunLEDStick(I2cDeviceSynchSimple deviceClient, boolean deviceClientIsOwned) { + super(deviceClient, deviceClientIsOwned); + +// deviceClient.setI2cAddress(ADDRESS_I2C_DEFAULT); +// super.registerArmingStateCallback(false); + } + +} diff --git a/WilyCore/src/main/java/com/qualcomm/robotcore/eventloop/EventLoopManager.java b/WilyCore/src/main/java/com/qualcomm/robotcore/eventloop/EventLoopManager.java new file mode 100644 index 000000000000..6dd1455ca25d --- /dev/null +++ b/WilyCore/src/main/java/com/qualcomm/robotcore/eventloop/EventLoopManager.java @@ -0,0 +1,15 @@ +package com.qualcomm.robotcore.eventloop; + +// public class EventLoopManager implements RecvLoopRunnable.RecvLoopCallback, NetworkConnection.NetworkConnectionCallback, PeerStatusCallback, SyncdDevice.Manager { +public class EventLoopManager implements SyncdDevice.Manager { + + @Override + public void registerSyncdDevice(SyncdDevice device) { + + } + + @Override + public void unregisterSyncdDevice(SyncdDevice device) { + + } +} diff --git a/WilyCore/src/main/java/com/qualcomm/robotcore/eventloop/SyncdDevice.java b/WilyCore/src/main/java/com/qualcomm/robotcore/eventloop/SyncdDevice.java new file mode 100644 index 000000000000..bf2743f648ec --- /dev/null +++ b/WilyCore/src/main/java/com/qualcomm/robotcore/eventloop/SyncdDevice.java @@ -0,0 +1,9 @@ +package com.qualcomm.robotcore.eventloop; + +public interface SyncdDevice { + + interface Manager { + void registerSyncdDevice(SyncdDevice device); + void unregisterSyncdDevice(SyncdDevice device); + } +} diff --git a/WilyCore/src/main/java/com/qualcomm/robotcore/eventloop/opmode/OpModeManagerImpl.java b/WilyCore/src/main/java/com/qualcomm/robotcore/eventloop/opmode/OpModeManagerImpl.java index 698f94cc0afe..c4f3a7c1f6a2 100644 --- a/WilyCore/src/main/java/com/qualcomm/robotcore/eventloop/opmode/OpModeManagerImpl.java +++ b/WilyCore/src/main/java/com/qualcomm/robotcore/eventloop/opmode/OpModeManagerImpl.java @@ -2,8 +2,13 @@ import androidx.annotation.Nullable; -public class OpModeManagerImpl { +public class OpModeManagerImpl implements OpModeManagerNotifier { public @Nullable OpMode registerListener(OpModeManagerNotifier.Notifications listener) { return null; } + + @Override + public void unregisterListener(Notifications listener) { + + } } diff --git a/WilyCore/src/main/java/com/qualcomm/robotcore/hardware/HardwareMap.java b/WilyCore/src/main/java/com/qualcomm/robotcore/hardware/HardwareMap.java index 4535f06a70b6..246943823f1a 100644 --- a/WilyCore/src/main/java/com/qualcomm/robotcore/hardware/HardwareMap.java +++ b/WilyCore/src/main/java/com/qualcomm/robotcore/hardware/HardwareMap.java @@ -1,6 +1,15 @@ package com.qualcomm.robotcore.hardware; +import android.content.Context; + +import com.qualcomm.robotcore.eventloop.opmode.OpModeManagerNotifier; import com.wilyworks.simulator.framework.WilyHardwareMap; public class HardwareMap extends WilyHardwareMap { + public HardwareMap() { + super(null, null); + } + public HardwareMap(Context appContext, OpModeManagerNotifier notifier) { + super(appContext, notifier); + } } diff --git a/WilyCore/src/main/java/com/wilyworks/simulator/WilyCore.java b/WilyCore/src/main/java/com/wilyworks/simulator/WilyCore.java index 8af65337e3e0..9c214b87cf6b 100644 --- a/WilyCore/src/main/java/com/wilyworks/simulator/WilyCore.java +++ b/WilyCore/src/main/java/com/wilyworks/simulator/WilyCore.java @@ -1,5 +1,6 @@ package com.wilyworks.simulator; +import static java.lang.ClassLoader.getSystemClassLoader; import static java.lang.System.nanoTime; import static java.lang.Thread.currentThread; import static java.lang.Thread.sleep; @@ -17,9 +18,9 @@ import com.wilyworks.common.WilyWorks; import com.wilyworks.simulator.framework.Field; import com.wilyworks.simulator.framework.InputManager; -import com.wilyworks.simulator.framework.MechSim; import com.wilyworks.simulator.framework.Simulation; import com.wilyworks.simulator.framework.WilyTelemetry; +import com.wilyworks.simulator.framework.mechsim.MechSim; import org.firstinspires.ftc.robotcore.external.Telemetry; import org.reflections.Reflections; @@ -39,6 +40,7 @@ import java.io.StringWriter; import java.lang.reflect.Constructor; import java.lang.reflect.InvocationTargetException; +import java.lang.reflect.Method; import java.util.ArrayList; import java.util.Comparator; import java.util.HashSet; @@ -103,7 +105,7 @@ public void windowClosing(WindowEvent e) { }); setSize(WINDOW_WIDTH, WINDOW_HEIGHT); - setLocation(400, 0); + setLocation(250, 0); setResizable(false); Choice dropDown = new Choice(); @@ -146,10 +148,13 @@ public void windowClosing(WindowEvent e) { button.addActionListener(actionEvent -> { switch (WilyCore.status.state) { - case STOPPED: - // Inform the main thread of the choice and save the preference: + case STOPPED: // It was in STOPPED state, now transition to INITIALIZED OpModeChoice opModeChoice = opModeChoices.get(dropDown.getSelectedIndex()); - WilyCore.status = new WilyCore.Status(WilyCore.State.INITIALIZED, opModeChoice.klass, button); + OpMode opMode = WilyCore.createOpMode(opModeChoice.klass); + + // Inform the main thread of the choice and save the preference: + WilyCore.opModeNotification("onOpModePreInit", opMode); + WilyCore.status = new WilyCore.Status(WilyCore.State.INITIALIZED, opMode, button); WilyCore.startTime = 0; dropDown.setMaximumSize(new Dimension(0, 0)); dropDown.setVisible(false); // Needed for long opMode names, for whatever reason @@ -160,15 +165,20 @@ public void windowClosing(WindowEvent e) { label.setText(opModeName); break; - case INITIALIZED: - WilyCore.status = new WilyCore.Status(WilyCore.State.STARTED, WilyCore.status.klass, button); + case INITIALIZED: // It was in INITIALIZED state, now transition to STARTED + WilyCore.opModeNotification("onOpModePreStart", WilyCore.status.opMode); + WilyCore.status = new WilyCore.Status(WilyCore.State.STARTED, null, button); WilyCore.startTime = WilyCore.wallClockTime(); button.setText("Stop"); break; - case STARTED: - WilyCore.opModeThread.interrupt(); + case STARTED: // It was in STARTED state, now transition to STOPPED + // I used to call WilyCore.opModeThread.interrupt(); here, but that prevented + // any user code from running after the Stop button was pressed. But maybe + // that's how it works on the robot, too? + // WilyCore.opModeThread.interrupt(); WilyCore.status = new WilyCore.Status(WilyCore.State.STOPPED, null, null); + WilyCore.opModeNotification("onOpModePostStop", WilyCore.status.opMode); WilyCore.terminateOpMode(); button.setText("Init"); dropDown.setMaximumSize(new Dimension(400, 100)); @@ -291,11 +301,11 @@ public static double time() { */ public enum State { STOPPED, INITIALIZED, STARTED } public static class Status { - public Class klass; + public OpMode opMode; public State state; public JButton stopButton; - public Status(State state, Class klass, JButton button) { - this.state = state; this.klass = klass; this.stopButton = button; + public Status(State state, OpMode opMode, JButton button) { + this.state = state; this.opMode = opMode; this.stopButton = button; } } @@ -479,28 +489,34 @@ static WilyWorks.Config getConfig(Class configKlass) { return new WilyWorks.Config(); } - // Call from the window manager to invoke the user's chosen "runOpMode" method: - static void runOpMode(Class klass) { + // Create the opMode from the user's choice of class. + static OpMode createOpMode(Class opModeClass) { OpMode opMode; try { //noinspection deprecation - opMode = (OpMode) klass.newInstance(); - } catch (InstantiationException|IllegalAccessException e) { + opMode = (OpMode) opModeClass.newInstance(); + } catch (InstantiationException | IllegalAccessException e) { throw new RuntimeException(e); } - // Create this year's game simulation: + // Before we initialize the HardwareMap, create this year's game simulation to go with + // the new opMode. It may override some of the HardwareMap. mechSim = MechSim.create(); // We need to re-instantiate hardware map on every run: hardwareMap = new HardwareMap(); - opMode.hardwareMap = hardwareMap; opMode.gamepad1 = gamepad1; opMode.gamepad2 = gamepad2; opMode.telemetry = telemetry; - if (LinearOpMode.class.isAssignableFrom(klass)) { + return opMode; + } + + // Call from the window manager to invoke the user's chosen "runOpMode" method. Instantiates + // the user's chosen opMode and invokes it. + static void runOpMode(OpMode opMode) { + if (LinearOpMode.class.isAssignableFrom(opMode.getClass())) { LinearOpMode linearOpMode = (LinearOpMode) opMode; try { linearOpMode.runOpMode(); @@ -530,15 +546,15 @@ static void runOpMode(Class klass) { // Thread dedicated to running the user's opMode: static class OpModeThread extends Thread { - Class opModeClass; - OpModeThread(Class opModeClass) { - this.opModeClass = opModeClass; + OpMode opMode; + OpModeThread(OpMode opMode) { + this.opMode = opMode; setName("Wily OpMode thread"); start(); } @Override public void run() { - WilyCore.runOpMode(status.klass); + WilyCore.runOpMode(opMode); } } @@ -565,6 +581,24 @@ public static void terminateOpMode() { telemetry.log().clear(); } + // Call Sidekick Core's "onOpModePreInit", "onOpModePreStart", "onOpModePostStop" notifications. + public static void opModeNotification(String method, OpMode opMode) { + Class sidekickCore; + try { + sidekickCore = getSystemClassLoader().loadClass("com.loonybot.sidekick.Sidekick"); + } catch (ClassNotFoundException e) { + return; + } + try { + Method getInstance = sidekickCore.getMethod("getWilyWorksInstance"); + Object core = getInstance.invoke(null); + Method notification = sidekickCore.getMethod(method, OpMode.class); + notification.invoke(core, opMode); + } catch (IllegalAccessException | InvocationTargetException | NoSuchMethodException e) { + throw new RuntimeException(e); + } + } + //////////////////////////////////////////////////////////////////////////////////////////////// // This is the application entry point that starts up all of Wily Works! public static void main(String[] args) @@ -602,7 +636,7 @@ public static void main(String[] args) // noinspection InfiniteLoopStatement while (true) { // Wait for the DashboardWindow UI to tell us what opMode to run: - while (status.state == State.STOPPED) { + while ((status.state == State.STOPPED) || (status.opMode == null)) { try { //noinspection BusyWait sleep(30); @@ -614,7 +648,7 @@ public static void main(String[] args) // The user has selected an opMode and pressed Init! Run the opMode on a dedicated // thread so that it can be interrupted as necessary: simulation.totalDistance = 0; - opModeThread = new OpModeThread(status.klass); + opModeThread = new OpModeThread(status.opMode); try { // Wait for the opMode thread to complete: opModeThread.join(); diff --git a/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyAnalogInput.java b/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyAnalogInput.java new file mode 100644 index 000000000000..f5a9aea48b89 --- /dev/null +++ b/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyAnalogInput.java @@ -0,0 +1,18 @@ +package com.wilyworks.simulator.framework; + +import com.qualcomm.robotcore.hardware.AnalogInput; + +/** + * Wily Works AnalogInput implementation. + */ +public class WilyAnalogInput extends AnalogInput { + @Override + public double getVoltage() { + return 0; + } + + @Override + public double getMaxVoltage() { + return 0; + } +} diff --git a/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyCRServo.java b/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyCRServo.java new file mode 100644 index 000000000000..c53e7c3b55db --- /dev/null +++ b/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyCRServo.java @@ -0,0 +1,42 @@ +package com.wilyworks.simulator.framework; + +import com.qualcomm.robotcore.hardware.CRServo; +import com.qualcomm.robotcore.hardware.ServoController; + +/** + * Wily Works CRServo implementation. + */ +public class WilyCRServo extends WilyHardwareDevice implements CRServo { + double power; + Direction direction; + + @Override + public ServoController getController() { + return new WilyServoController(); + } + + @Override + public int getPortNumber() { + return 0; + } + + @Override + public void setDirection(Direction direction) { + this.direction = direction; + } + + @Override + public Direction getDirection() { + return direction; + } + + @Override + public void setPower(double power) { + this.power = power; + } + + @Override + public double getPower() { + return power; + } +} diff --git a/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyColorSensor.java b/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyColorSensor.java new file mode 100644 index 000000000000..8faab7df9acc --- /dev/null +++ b/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyColorSensor.java @@ -0,0 +1,47 @@ +package com.wilyworks.simulator.framework; + +import com.qualcomm.robotcore.hardware.ColorSensor; +import com.qualcomm.robotcore.hardware.I2cAddr; + +/** + * Wily Works color sensor implementation. + */ +public class WilyColorSensor extends WilyHardwareDevice implements ColorSensor { + @Override + public int red() { + return 0; + } + + @Override + public int green() { + return 0; + } + + @Override + public int blue() { + return 0; + } + + @Override + public int alpha() { + return 0; + } + + @Override + public int argb() { + return 0; + } + + @Override + public void enableLed(boolean enable) { + } + + @Override + public void setI2cAddress(I2cAddr newAddress) { + } + + @Override + public I2cAddr getI2cAddress() { + return null; + } +} diff --git a/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyDigitalChannel.java b/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyDigitalChannel.java new file mode 100644 index 000000000000..13c4752bb0a4 --- /dev/null +++ b/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyDigitalChannel.java @@ -0,0 +1,29 @@ +package com.wilyworks.simulator.framework; + +import com.qualcomm.robotcore.hardware.DigitalChannel; + +/** + * Wily Works DigitalChannel implementation. + */ +public class WilyDigitalChannel extends WilyHardwareDevice implements DigitalChannel { + boolean state; + + @Override + public Mode getMode() { + return null; + } + + @Override + public void setMode(Mode mode) { + } + + @Override + public boolean getState() { + return state; + } + + @Override + public void setState(boolean state) { + this.state = state; + } +} diff --git a/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyDistanceSensor.java b/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyDistanceSensor.java new file mode 100644 index 000000000000..05471c831737 --- /dev/null +++ b/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyDistanceSensor.java @@ -0,0 +1,15 @@ +package com.wilyworks.simulator.framework; + +import com.qualcomm.robotcore.hardware.DistanceSensor; + +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; + +/** + * Wily Works distance sensor implementation. + */ +public class WilyDistanceSensor extends WilyHardwareDevice implements DistanceSensor { + @Override + public double getDistance(DistanceUnit unit) { + return unit.fromMm(65535); + } // Distance when not responding +} diff --git a/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyHardwareMap.java b/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyHardwareMap.java index f6c7e52b6b60..64bc601d1bf5 100644 --- a/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyHardwareMap.java +++ b/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyHardwareMap.java @@ -8,7 +8,9 @@ import com.qualcomm.hardware.gobilda.GoBildaPinpointDriver; import com.qualcomm.hardware.limelightvision.Limelight3A; +import com.qualcomm.hardware.sparkfun.SparkFunLEDStick; import com.qualcomm.hardware.sparkfun.SparkFunOTOS; +import com.qualcomm.robotcore.eventloop.opmode.OpModeManagerNotifier; import com.qualcomm.robotcore.hardware.AnalogInput; import com.qualcomm.robotcore.hardware.CRServo; import com.qualcomm.robotcore.hardware.ColorSensor; @@ -16,29 +18,15 @@ import com.qualcomm.robotcore.hardware.DigitalChannel; import com.qualcomm.robotcore.hardware.DistanceSensor; import com.qualcomm.robotcore.hardware.HardwareDevice; -import com.qualcomm.robotcore.hardware.I2cAddr; import com.qualcomm.robotcore.hardware.IMU; import com.qualcomm.robotcore.hardware.LED; import com.qualcomm.robotcore.hardware.NormalizedColorSensor; -import com.qualcomm.robotcore.hardware.NormalizedRGBA; import com.qualcomm.robotcore.hardware.Servo; -import com.qualcomm.robotcore.hardware.ServoController; import com.qualcomm.robotcore.hardware.VoltageSensor; import com.qualcomm.robotcore.util.SerialNumber; -import com.wilyworks.common.WilyWorks; import com.wilyworks.simulator.WilyCore; -import org.firstinspires.ftc.robotcore.external.hardware.camera.CameraCharacteristics; import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; -import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; -import org.firstinspires.ftc.robotcore.external.navigation.AngularVelocity; -import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder; -import org.firstinspires.ftc.robotcore.external.navigation.AxesReference; -import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; -import org.firstinspires.ftc.robotcore.external.navigation.Orientation; -import org.firstinspires.ftc.robotcore.external.navigation.Quaternion; -import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles; -import org.firstinspires.ftc.robotcore.internal.system.Deadline; import org.swerverobotics.ftc.UltrasonicDistanceSensor; import java.util.ArrayList; @@ -51,363 +39,6 @@ import java.util.Set; import java.util.SortedSet; import java.util.TreeSet; -import java.util.function.Consumer; - -import kotlin.coroutines.Continuation; - -/** - * Wily Works simulated IMU implementation. - */ -class WilyIMU extends WilyHardwareDevice implements IMU { - double startYaw; - @Override - public boolean initialize(Parameters parameters) { - resetYaw(); - return true; - } - - @Override - public void resetYaw() { - startYaw = WilyWorks.getPose().heading.log(); - } - - @Override - public YawPitchRollAngles getRobotYawPitchRollAngles() { - return new YawPitchRollAngles( - AngleUnit.RADIANS, - WilyWorks.getPose().heading.log() - startYaw, - 0, - 0, - 0); - } - - @Override - public Orientation getRobotOrientation(AxesReference reference, AxesOrder order, AngleUnit angleUnit) { - return new Orientation(); - } - - @Override - public Quaternion getRobotOrientationAsQuaternion() { - return new Quaternion(); - } - - @Override - public AngularVelocity getRobotAngularVelocity(AngleUnit angleUnit) { - return new AngularVelocity( - angleUnit, - (float) WilyWorks.getPoseVelocity().angVel, // ### transformedAngularVelocityVector.get(0), - 0, // ### transformedAngularVelocityVector.get(1), - 0, // ### transformedAngularVelocityVector.get(2), - 0); // ### rawAngularVelocity.acquisitionTime); - } -} - -/** - * Wily Works voltage sensor implementation. - */ -class WilyVoltageSensor extends WilyHardwareDevice implements VoltageSensor { - @Override - public double getVoltage() { - return 13.0; - } - - @Override - public String getDeviceName() { - return "Voltage Sensor"; - } -} - -/** - * Wily Works distance sensor implementation. - */ -class WilyDistanceSensor extends WilyHardwareDevice implements DistanceSensor { - @Override - public double getDistance(DistanceUnit unit) { return unit.fromMm(65535); } // Distance when not responding -} - -/** - * Wily Works normalized color sensor implementation. - */ -class WilyNormalizedColorSensor extends WilyHardwareDevice implements NormalizedColorSensor, DistanceSensor { - @Override - public NormalizedRGBA getNormalizedColors() { return new NormalizedRGBA(); } - - @Override - public float getGain() { return 0; } - - @Override - public void setGain(float newGain) { } - - @Override - public double getDistance(DistanceUnit unit) { - return 0; - } -} - -/** - * Wily Works color sensor implementation. - */ -class WilyColorSensor extends WilyHardwareDevice implements ColorSensor { - @Override - public int red() { return 0; } - - @Override - public int green() { return 0; } - - @Override - public int blue() { return 0; } - - @Override - public int alpha() { return 0; } - - @Override - public int argb() { return 0; } - - @Override - public void enableLed(boolean enable) { } - - @Override - public void setI2cAddress(I2cAddr newAddress) { } - - @Override - public I2cAddr getI2cAddress() { return null; } -} - -/** - * Wily Works named webcam implementation. - */ -class WilyWebcam extends WilyHardwareDevice implements WebcamName { - WilyWorks.Config.Camera wilyCamera; - - WilyWebcam(String deviceName) { - for (WilyWorks.Config.Camera camera: WilyCore.config.cameras) { - if (camera.name.equals(deviceName)) { - wilyCamera = camera; - } - } - if (wilyCamera == null) { - System.out.printf("WilyWorks: Couldn't find configuration data for camera '%s'", deviceName); - } - } - - @Override - public boolean isWebcam() { - return true; - } - - @Override - public boolean isCameraDirection() { - return false; - } - - @Override - public boolean isSwitchable() { - return false; - } - - @Override - public boolean isUnknown() { - return false; - } - - @Override - public void asyncRequestCameraPermission(Context context, Deadline deadline, Continuation> continuation) { - - } - - @Override - public boolean requestCameraPermission(Deadline deadline) { - return false; - } - - @Override - public CameraCharacteristics getCameraCharacteristics() { - return null; - } - - @Override - public SerialNumber getSerialNumber() { - return null; - } - - @Nullable - @Override - public String getUsbDeviceNameIfAttached() { - return null; - } - - @Override - public boolean isAttached() { - return false; - } -} - -/** - * Wily Works ServoController implementation. - */ -class WilyServoController extends WilyHardwareDevice implements ServoController { - @Override - public void pwmEnable() { } - - @Override - public void pwmDisable() { } - - @Override - public PwmStatus getPwmStatus() { return PwmStatus.DISABLED; } - - @Override - public void setServoPosition(int servo, double position) { } - - @Override - public double getServoPosition(int servo) { return 0; } - - @Override - public void close() { } -} - -/** - * Wily Works Servo implementation. - */ -class WilyServo extends WilyHardwareDevice implements Servo { - double position; - - @Override - public ServoController getController() { - return new WilyServoController(); - } - - @Override - public int getPortNumber() { - return 0; - } - - @Override - public void setDirection(Direction direction) { - - } - - @Override - public Direction getDirection() { - return null; - } - - @Override - public void setPosition(double position) { this.position = Math.max(0, Math.min(1, position)); } - - @Override - public double getPosition() {return position; } - - @Override - public void scaleRange(double min, double max) { - - } -} - -/** - * Wily Works CRServo implementation. - */ -class WilyCRServo extends WilyHardwareDevice implements CRServo { - double power; - Direction direction; - - @Override - public ServoController getController() { - return new WilyServoController(); - } - - @Override - public int getPortNumber() { - return 0; - } - - @Override - public void setDirection(Direction direction) { - this.direction = direction; - } - - @Override - public Direction getDirection() { - return direction; - } - - @Override - public void setPower(double power) { - this.power = power; - } - - @Override - public double getPower() { - return power; - } -} - -/** - * Wily Works DigitalChannel implementation. - */ -class WilyDigitalChannel extends WilyHardwareDevice implements DigitalChannel { - boolean state; - - @Override - public Mode getMode() { return null; } - - @Override - public void setMode(Mode mode) {} - - @Override - public boolean getState() { return state; } - - @Override - public void setState(boolean state) { this.state = state; } -} - -/** - * Wily Works LED implementation. - */ -class WilyLED extends LED { - // Assume that every digital channels is a REV LED indicator. Doesn't hurt if that's not - // the case: - boolean enable = true; // They're always on by default - double x; - double y; - boolean isRed; - WilyLED(String deviceName) { - WilyWorks.Config.LEDIndicator wilyLed = null; - for (WilyWorks.Config.LEDIndicator led: WilyCore.config.ledIndicators) { - if (led.name.equals(deviceName)) { - wilyLed = led; - } - } - if (wilyLed != null) { - x = wilyLed.x; - y = wilyLed.y; - isRed = wilyLed.isRed; - } else { - isRed = !(deviceName.toLowerCase().contains("green")); - } - } - - @Override - public void enable(boolean enableLed) { this.enable = enableLed; } - - @Override - public void enableLight(boolean enable) { enable(enable); } - - @Override - public boolean isLightOn() { - return enable; - } -} - -/** - * Wily Works AnalogInput implementation. - */ -class WilyAnalogInput extends AnalogInput { - @Override - public double getVoltage() { return 0; } - - @Override - public double getMaxVoltage() { return 0; } -} /** * Wily Works hardware map. @@ -430,11 +61,14 @@ public class WilyHardwareMap implements Iterable { public DeviceMapping analogInput = new DeviceMapping<>(AnalogInput.class); public DeviceMapping imu = new DeviceMapping<>(IMU.class); public DeviceMapping limelight = new DeviceMapping<>(Limelight3A.class); + public DeviceMapping sparkFunLEDStick = new DeviceMapping<>(SparkFunLEDStick.class); protected Map> allDevicesMap = new HashMap<>(); protected List allDevicesList = new ArrayList<>(); - public WilyHardwareMap() { - // Road Runner expects this object to be already created becaues it references + public final List> allDeviceMappings = new ArrayList<>(); + + public WilyHardwareMap(Context appContext, OpModeManagerNotifier notifier) { + // Road Runner expects this object to be already created because it references // hardwareMap.voltageSensor.iterator().next() directly: put("voltage_sensor", VoltageSensor.class); } @@ -541,6 +175,9 @@ public synchronized void put(String deviceName, Class klass) { } else if (Limelight3A.class.isAssignableFrom(klass)) { device = new Limelight3A(null, "", null); limelight.put(deviceName, (Limelight3A) device); + } else if (SparkFunLEDStick.class.isAssignableFrom(klass)) { + device = new SparkFunLEDStick(null, false); + sparkFunLEDStick.put(deviceName, (SparkFunLEDStick) device); } else { throw new IllegalArgumentException("Unexpected device type for HardwareMap"); } diff --git a/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyIMU.java b/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyIMU.java new file mode 100644 index 000000000000..b26da584b708 --- /dev/null +++ b/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyIMU.java @@ -0,0 +1,60 @@ +package com.wilyworks.simulator.framework; + +import com.qualcomm.robotcore.hardware.IMU; +import com.wilyworks.common.WilyWorks; + +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.AngularVelocity; +import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder; +import org.firstinspires.ftc.robotcore.external.navigation.AxesReference; +import org.firstinspires.ftc.robotcore.external.navigation.Orientation; +import org.firstinspires.ftc.robotcore.external.navigation.Quaternion; +import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles; + +/** + * Wily Works simulated IMU implementation. + */ +public class WilyIMU extends WilyHardwareDevice implements IMU { + double startYaw; + + @Override + public boolean initialize(Parameters parameters) { + resetYaw(); + return true; + } + + @Override + public void resetYaw() { + startYaw = WilyWorks.getPose().heading.log(); + } + + @Override + public YawPitchRollAngles getRobotYawPitchRollAngles() { + return new YawPitchRollAngles( + AngleUnit.RADIANS, + WilyWorks.getPose().heading.log() - startYaw, + 0, + 0, + 0); + } + + @Override + public Orientation getRobotOrientation(AxesReference reference, AxesOrder order, AngleUnit angleUnit) { + return new Orientation(); + } + + @Override + public Quaternion getRobotOrientationAsQuaternion() { + return new Quaternion(); + } + + @Override + public AngularVelocity getRobotAngularVelocity(AngleUnit angleUnit) { + return new AngularVelocity( + angleUnit, + (float) WilyWorks.getPoseVelocity().angVel, // ### transformedAngularVelocityVector.get(0), + 0, // ### transformedAngularVelocityVector.get(1), + 0, // ### transformedAngularVelocityVector.get(2), + 0); // ### rawAngularVelocity.acquisitionTime); + } +} diff --git a/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyLED.java b/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyLED.java new file mode 100644 index 000000000000..1907266f4471 --- /dev/null +++ b/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyLED.java @@ -0,0 +1,44 @@ +package com.wilyworks.simulator.framework; + +import com.qualcomm.robotcore.hardware.LED; +import com.wilyworks.common.WilyWorks; +import com.wilyworks.simulator.WilyCore; + +/** + * Wily Works LED implementation. + */ +public class WilyLED extends LED { + // Assume that every digital channels is a REV LED indicator. Doesn't hurt if that's not + // the case: + public boolean enable = true; // They're always on by default + public double x; + public double y; + public boolean isRed; + public WilyLED(String deviceName) { + WilyWorks.Config.LEDIndicator wilyLed = null; + for (WilyWorks.Config.LEDIndicator led: WilyCore.config.ledIndicators) { + if (led.name.equals(deviceName)) { + wilyLed = led; + } + } + if (wilyLed != null) { + x = wilyLed.x; + y = wilyLed.y; + isRed = wilyLed.isRed; + } else { + isRed = !(deviceName.toLowerCase().contains("green")); + } + } + + @Override + public void enable(boolean enableLed) { this.enable = enableLed; } + + @Override + public void enableLight(boolean enable) { enable(enable); } + + @Override + public boolean isLightOn() { + return enable; + } +} + diff --git a/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyNormalizedColorSensor.java b/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyNormalizedColorSensor.java new file mode 100644 index 000000000000..f37d08b78294 --- /dev/null +++ b/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyNormalizedColorSensor.java @@ -0,0 +1,31 @@ +package com.wilyworks.simulator.framework; + +import com.qualcomm.robotcore.hardware.DistanceSensor; +import com.qualcomm.robotcore.hardware.NormalizedColorSensor; +import com.qualcomm.robotcore.hardware.NormalizedRGBA; + +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; + +/** + * Wily Works normalized color sensor implementation. + */ +public class WilyNormalizedColorSensor extends WilyHardwareDevice implements NormalizedColorSensor, DistanceSensor { + @Override + public NormalizedRGBA getNormalizedColors() { + return new NormalizedRGBA(); + } + + @Override + public float getGain() { + return 0; + } + + @Override + public void setGain(float newGain) { + } + + @Override + public double getDistance(DistanceUnit unit) { + return 0; + } +} diff --git a/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyServo.java b/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyServo.java new file mode 100644 index 000000000000..44022174de2c --- /dev/null +++ b/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyServo.java @@ -0,0 +1,46 @@ +package com.wilyworks.simulator.framework; + +import com.qualcomm.robotcore.hardware.Servo; +import com.qualcomm.robotcore.hardware.ServoController; + +/** + * Wily Works Servo implementation. + */ +public class WilyServo extends WilyHardwareDevice implements Servo { + double position; + + @Override + public ServoController getController() { + return new WilyServoController(); + } + + @Override + public int getPortNumber() { + return 0; + } + + @Override + public void setDirection(Direction direction) { + + } + + @Override + public Direction getDirection() { + return null; + } + + @Override + public void setPosition(double position) { + this.position = Math.max(0, Math.min(1, position)); + } + + @Override + public double getPosition() { + return position; + } + + @Override + public void scaleRange(double min, double max) { + + } +} diff --git a/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyServoController.java b/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyServoController.java new file mode 100644 index 000000000000..b6ee70013097 --- /dev/null +++ b/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyServoController.java @@ -0,0 +1,34 @@ +package com.wilyworks.simulator.framework; + +import com.qualcomm.robotcore.hardware.ServoController; + +/** + * Wily Works ServoController implementation. + */ +public class WilyServoController extends WilyHardwareDevice implements ServoController { + @Override + public void pwmEnable() { + } + + @Override + public void pwmDisable() { + } + + @Override + public PwmStatus getPwmStatus() { + return PwmStatus.DISABLED; + } + + @Override + public void setServoPosition(int servo, double position) { + } + + @Override + public double getServoPosition(int servo) { + return 0; + } + + @Override + public void close() { + } +} diff --git a/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyVoltageSensor.java b/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyVoltageSensor.java new file mode 100644 index 000000000000..9d846f19b514 --- /dev/null +++ b/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyVoltageSensor.java @@ -0,0 +1,18 @@ +package com.wilyworks.simulator.framework; + +import com.qualcomm.robotcore.hardware.VoltageSensor; + +/** + * Wily Works voltage sensor implementation. + */ +public class WilyVoltageSensor extends WilyHardwareDevice implements VoltageSensor { + @Override + public double getVoltage() { + return 13.0; + } + + @Override + public String getDeviceName() { + return "Voltage Sensor"; + } +} diff --git a/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyWebcam.java b/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyWebcam.java new file mode 100644 index 000000000000..8aa242fa397b --- /dev/null +++ b/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyWebcam.java @@ -0,0 +1,86 @@ +package com.wilyworks.simulator.framework; + +import android.content.Context; + +import androidx.annotation.Nullable; + +import com.qualcomm.robotcore.util.SerialNumber; +import com.wilyworks.common.WilyWorks; +import com.wilyworks.simulator.WilyCore; + +import org.firstinspires.ftc.robotcore.external.hardware.camera.CameraCharacteristics; +import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; +import org.firstinspires.ftc.robotcore.internal.system.Deadline; + +import java.util.function.Consumer; + +import kotlin.coroutines.Continuation; + +/** + * Wily Works named webcam implementation. + */ +public class WilyWebcam extends WilyHardwareDevice implements WebcamName { + WilyWorks.Config.Camera wilyCamera; + + WilyWebcam(String deviceName) { + for (WilyWorks.Config.Camera camera : WilyCore.config.cameras) { + if (camera.name.equals(deviceName)) { + wilyCamera = camera; + } + } + if (wilyCamera == null) { + System.out.printf("WilyWorks: Couldn't find configuration data for camera '%s'", deviceName); + } + } + + @Override + public boolean isWebcam() { + return true; + } + + @Override + public boolean isCameraDirection() { + return false; + } + + @Override + public boolean isSwitchable() { + return false; + } + + @Override + public boolean isUnknown() { + return false; + } + + @Override + public void asyncRequestCameraPermission(Context context, Deadline deadline, Continuation> continuation) { + + } + + @Override + public boolean requestCameraPermission(Deadline deadline) { + return false; + } + + @Override + public CameraCharacteristics getCameraCharacteristics() { + return null; + } + + @Override + public SerialNumber getSerialNumber() { + return null; + } + + @Nullable + @Override + public String getUsbDeviceNameIfAttached() { + return null; + } + + @Override + public boolean isAttached() { + return false; + } +} diff --git a/WilyCore/src/main/java/com/wilyworks/simulator/framework/mechsim/DrumAnalogInput.java b/WilyCore/src/main/java/com/wilyworks/simulator/framework/mechsim/DrumAnalogInput.java new file mode 100644 index 000000000000..6947273e92da --- /dev/null +++ b/WilyCore/src/main/java/com/wilyworks/simulator/framework/mechsim/DrumAnalogInput.java @@ -0,0 +1,19 @@ +package com.wilyworks.simulator.framework.mechsim; + +import com.wilyworks.simulator.framework.WilyAnalogInput; + +// Hooked class for measuring the position of the drum: +public class DrumAnalogInput extends WilyAnalogInput { + DecodeSlowBotMechSim mechSim; + + DrumAnalogInput(DecodeSlowBotMechSim mechSim) { + this.mechSim = mechSim; + } + + // Return a voltage that is proportional to the drum location, with some variation: + @Override + public double getVoltage() { + double variation = -0.1 + Math.random() * 0.2; // random() generates numbers between 0 and 1 + return 3.5 * mechSim.actualDrumPosition + variation; + } +} diff --git a/WilyCore/src/main/java/com/wilyworks/simulator/framework/mechsim/DrumColorSensor.java b/WilyCore/src/main/java/com/wilyworks/simulator/framework/mechsim/DrumColorSensor.java new file mode 100644 index 000000000000..ec1a42bc95d9 --- /dev/null +++ b/WilyCore/src/main/java/com/wilyworks/simulator/framework/mechsim/DrumColorSensor.java @@ -0,0 +1,63 @@ +package com.wilyworks.simulator.framework.mechsim; + +import com.qualcomm.robotcore.hardware.NormalizedRGBA; +import com.wilyworks.simulator.framework.WilyNormalizedColorSensor; + +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; + +import java.awt.Color; + +// Hooked class for determining the color of the ball once it's in the drum: +public class DrumColorSensor extends WilyNormalizedColorSensor { + DecodeSlowBotMechSim mechSim; + int idMask; // Sensor 0 or 1 + + DrumColorSensor(DecodeSlowBotMechSim mechSim, int index) { + this.mechSim = mechSim; + this.idMask = 1 << index; + } + + // Returns true if this sensor can read a ball; false if a hole in the ball is positioned + // over the sensor. + boolean sensorCanReadBall() { + // Every time we get a new ball, reset our variations: + if (mechSim.colorSensorMask == -1) { + mechSim.colorSensorMask = 1 + (int) (Math.random() * 3.0); // Mask = 1, 2 or 3 + } + return ((mechSim.colorSensorMask & idMask) != 0); + } + + @Override + public NormalizedRGBA getNormalizedColors() { + NormalizedRGBA normalizedColor = new NormalizedRGBA(); + + // Simulate the ball holes for some reads: + int rgbColor = 0; + // Figure out what slot is being input into, if any: + int slot = mechSim.findDrumSlot(mechSim.INTAKE_POSITIONS); + if (slot != -1) { + DecodeSlowBotMechSim.Ball ball = mechSim.slotBalls.get(slot); + if (ball != null) { + // There's ball over the sensors. See if they can be read: + if (sensorCanReadBall()) { + if (ball.color == DecodeSlowBotMechSim.BallColor.GREEN) { + rgbColor = android.graphics.Color.HSVToColor(new float[]{175, 1, 1}); + } else { + rgbColor = android.graphics.Color.HSVToColor(new float[]{210, 1, 1}); + } + } + } + } + normalizedColor.red = new Color(rgbColor).getRed() / 255.0f; + normalizedColor.green = new Color(rgbColor).getGreen() / 255.0f; + normalizedColor.blue = new Color(rgbColor).getBlue() / 255.0f; + return normalizedColor; + } + + @Override + public double getDistance(DistanceUnit unit) { + int slot = mechSim.findDrumSlot(mechSim.INTAKE_POSITIONS); + boolean ballPositionedForRead = (slot != -1) && mechSim.slotBalls.get(slot) != null; + return ballPositionedForRead && sensorCanReadBall() ? unit.fromMm(18) : unit.fromMm(70); + } +} diff --git a/WilyCore/src/main/java/com/wilyworks/simulator/framework/mechsim/DrumDigitalChannel.java b/WilyCore/src/main/java/com/wilyworks/simulator/framework/mechsim/DrumDigitalChannel.java new file mode 100644 index 000000000000..daabafd7594d --- /dev/null +++ b/WilyCore/src/main/java/com/wilyworks/simulator/framework/mechsim/DrumDigitalChannel.java @@ -0,0 +1,18 @@ +package com.wilyworks.simulator.framework.mechsim; + +import com.wilyworks.simulator.framework.WilyDigitalChannel; + +public class DrumDigitalChannel extends WilyDigitalChannel { + DecodeSlowBotMechSim mechSim; + int index; + + DrumDigitalChannel(DecodeSlowBotMechSim mechSim, int index) { + this.mechSim = mechSim; + this.index = index; + } + + @Override + public boolean getState() { + return super.getState(); + } +} diff --git a/WilyCore/src/main/java/com/wilyworks/simulator/framework/mechsim/LaunchMotor.java b/WilyCore/src/main/java/com/wilyworks/simulator/framework/mechsim/LaunchMotor.java new file mode 100644 index 000000000000..c1fc627bb3cc --- /dev/null +++ b/WilyCore/src/main/java/com/wilyworks/simulator/framework/mechsim/LaunchMotor.java @@ -0,0 +1,19 @@ +package com.wilyworks.simulator.framework.mechsim; + +import com.wilyworks.simulator.framework.WilyDcMotorEx; + +// Let us ramp up the launcher motor velocity. +public class LaunchMotor extends WilyDcMotorEx { + double targetVelocity; + double actualVelocity; + + @Override + public void setVelocity(double angularRate) { + targetVelocity = angularRate; + } + + @Override + public double getVelocity() { + return actualVelocity; + } +} diff --git a/WilyCore/src/main/java/com/wilyworks/simulator/framework/MechSim.java b/WilyCore/src/main/java/com/wilyworks/simulator/framework/mechsim/MechSim.java similarity index 87% rename from WilyCore/src/main/java/com/wilyworks/simulator/framework/MechSim.java rename to WilyCore/src/main/java/com/wilyworks/simulator/framework/mechsim/MechSim.java index 1afe1cc16372..b946ec3809b5 100644 --- a/WilyCore/src/main/java/com/wilyworks/simulator/framework/MechSim.java +++ b/WilyCore/src/main/java/com/wilyworks/simulator/framework/mechsim/MechSim.java @@ -1,4 +1,4 @@ -package com.wilyworks.simulator.framework; +package com.wilyworks.simulator.framework.mechsim; import static com.wilyworks.simulator.WilyCore.time; @@ -6,17 +6,16 @@ import com.qualcomm.robotcore.hardware.AnalogInput; import com.qualcomm.robotcore.hardware.CRServo; import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.hardware.DigitalChannel; import com.qualcomm.robotcore.hardware.HardwareDevice; import com.qualcomm.robotcore.hardware.HardwareMap; import com.qualcomm.robotcore.hardware.LED; import com.qualcomm.robotcore.hardware.NormalizedColorSensor; -import com.qualcomm.robotcore.hardware.NormalizedRGBA; import com.qualcomm.robotcore.hardware.Servo; import com.wilyworks.simulator.WilyCore; +import com.wilyworks.simulator.framework.WilyLED; import com.wilyworks.simulator.helpers.Point; -import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; - import java.awt.AlphaComposite; import java.awt.BasicStroke; import java.awt.Color; @@ -121,90 +120,6 @@ protected void renderLeds(Graphics2D g, Pose2d pose) { } -// Hooked class for measuring the position of the drum: -class DrumAnalogInput extends WilyAnalogInput { - DecodeSlowBotMechSim mechSim; - DrumAnalogInput(DecodeSlowBotMechSim mechSim) { - this.mechSim = mechSim; - } - - // Return a voltage that is proportional to the drum location, with some variation: - @Override - public double getVoltage() { - double variation = -0.1 + Math.random() * 0.2; // random() generates numbers between 0 and 1 - return 3.5 * mechSim.actualDrumPosition + variation; - } -} - -// Hooked class for determining the color of the ball once it's in the drum: -class DrumColorSensor extends WilyNormalizedColorSensor { - DecodeSlowBotMechSim mechSim; - int idMask; // Sensor 0 or 1 - DrumColorSensor(DecodeSlowBotMechSim mechSim, int index) { - this.mechSim = mechSim; - this.idMask = 1 << index; - } - - // Returns true if this sensor can read a ball; false if a hole in the ball is positioned - // over the sensor. - boolean sensorCanReadBall() { - // Every time we get a new ball, reset our variations: - if (mechSim.colorSensorMask == -1) { - mechSim.colorSensorMask = 1 + (int)(Math.random() * 3.0); // Mask = 1, 2 or 3 - } - return ((mechSim.colorSensorMask & idMask) != 0); - } - - @Override - public NormalizedRGBA getNormalizedColors() { - NormalizedRGBA normalizedColor = new NormalizedRGBA(); - - // Simulate the ball holes for some reads: - int rgbColor = 0; - // Figure out what slot is being input into, if any: - int slot = mechSim.findDrumSlot(mechSim.INTAKE_POSITIONS); - if (slot != -1) { - DecodeSlowBotMechSim.Ball ball = mechSim.slotBalls.get(slot); - if (ball != null) { - // There's ball over the sensors. See if they can be read: - if (sensorCanReadBall()) { - if (ball.color == DecodeSlowBotMechSim.BallColor.GREEN) { - rgbColor = android.graphics.Color.HSVToColor(new float[]{175, 1, 1}); - } else { - rgbColor = android.graphics.Color.HSVToColor(new float[]{210, 1, 1}); - } - } - } - } - normalizedColor.red = new Color(rgbColor).getRed() / 255.0f; - normalizedColor.green = new Color(rgbColor).getGreen() / 255.0f; - normalizedColor.blue = new Color(rgbColor).getBlue() / 255.0f; - return normalizedColor; - } - - @Override - public double getDistance(DistanceUnit unit) { - int slot = mechSim.findDrumSlot(mechSim.INTAKE_POSITIONS); - boolean ballPositionedForRead = (slot != -1) && mechSim.slotBalls.get(slot) != null; - return ballPositionedForRead && sensorCanReadBall() ? unit.fromMm(18) : unit.fromMm(70); - } -} - -// Let us ramp up the launcher motor velocity. -class LaunchMotor extends WilyDcMotorEx { - double targetVelocity; - double actualVelocity; - - @Override - public void setVelocity(double angularRate) { - targetVelocity = angularRate; - } - @Override - public double getVelocity() { - return actualVelocity; - } -} - // Simulation for the SlowBot in the 2025/2026 Decode game. class DecodeSlowBotMechSim extends MechSim { enum BallColor {PURPLE, GREEN, GOLD} @@ -272,6 +187,8 @@ public Ball(BallColor color, double x, double y) { AnalogInput analogDrum; NormalizedColorSensor sensorColor1; NormalizedColorSensor sensorColor2; + DigitalChannel digitalChannel1; + DigitalChannel digitalChannel2; DcMotorEx intakeMotor; Servo drumServo; Servo transferServo; @@ -351,6 +268,12 @@ public HardwareDevice hookDevice(String name, HardwareDevice device) { if (name.equals("sensorColor2")) { device = sensorColor2 = new DrumColorSensor(this, 1); } + if (name.equals("breakBeam1")) { + device = digitalChannel1 = new DrumDigitalChannel(this, 1); + } + if (name.equals("breakBeam2")) { + device = digitalChannel2 = new DrumDigitalChannel(this, 2); + } return device; } @@ -612,7 +535,7 @@ void simulate(Pose2d pose, double deltaT) { // terminate the loop... fieldBalls.remove(ball); - // Move the ball into the intake position, unless random error says + // Move the ball into an intake position, unless random error says // to toss it: if ((!WilyCore.enableSensorError) || (Math.random() > INTAKE_ERROR_PROBABILITY)) { intakeBall = ball; @@ -625,8 +548,17 @@ void simulate(Pose2d pose, double deltaT) { // We're intaking from the intake into a drum slot: int slot = findDrumSlot(INTAKE_POSITIONS); if (slot != -1) { - if (slotBalls.get(slot) == null) { - slotBalls.set(slot, intakeBall); + // Accumulate the empty slots: + LinkedList emptySlots = new LinkedList<>(); + for (int i = 0; i < 3; i++) { + if (slotBalls.get(i) == null) { + emptySlots.add(i); + } + } + // Assign the ball to an empty slot: + if (!emptySlots.isEmpty()) { + int emptySlot = emptySlots.get((int) (Math.random() * emptySlots.size())); + slotBalls.set(emptySlot, intakeBall); intakeBall = null; hasIntaken = true; } diff --git a/WilyCore/src/main/java/org/swerverobotics/ftc/GoBildaPinpointDriver.java b/WilyCore/src/main/java/org/swerverobotics/ftc/GoBildaPinpointDriver.java deleted file mode 100644 index bdbed77f7717..000000000000 --- a/WilyCore/src/main/java/org/swerverobotics/ftc/GoBildaPinpointDriver.java +++ /dev/null @@ -1,470 +0,0 @@ -package org.swerverobotics.ftc; - -import com.acmerobotics.roadrunner.Pose2d; -import com.acmerobotics.roadrunner.PoseVelocity2d; -import com.qualcomm.robotcore.hardware.I2cDeviceSynchDevice; -import com.qualcomm.robotcore.hardware.I2cDeviceSynchSimple; -import com.qualcomm.robotcore.hardware.configuration.annotations.DeviceProperties; -import com.qualcomm.robotcore.hardware.configuration.annotations.I2cDeviceType; -import com.wilyworks.simulator.WilyCore; - -import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; -import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; -import org.firstinspires.ftc.robotcore.external.navigation.Pose2D; - -import java.nio.ByteBuffer; -import java.nio.ByteOrder; - -@I2cDeviceType -@DeviceProperties( - name = "Home-compiled goBILDA® Pinpoint Odometry Computer", - xmlTag = "goBILDAPinpoint", - description ="goBILDA® Pinpoint Odometry Computer (IMU Sensor Fusion for 2 Wheel Odometry)" -) - -public class GoBildaPinpointDriver extends I2cDeviceSynchDevice { - private int deviceStatus = 0; - private int loopTime = 0; - private int xEncoderValue = 0; - private int yEncoderValue = 0; - private float xPosition = 0; - private float yPosition = 0; - private float hOrientation = 0; - private float xVelocity = 0; - private float yVelocity = 0; - private float hVelocity = 0; - - private static final float goBILDA_SWINGARM_POD = 13.26291192f; //ticks-per-mm for the goBILDA Swingarm Pod - private static final float goBILDA_4_BAR_POD = 19.89436789f; //ticks-per-mm for the goBILDA 4-Bar Pod - - //i2c address of the device - public static final byte DEFAULT_ADDRESS = 0x31; - - public GoBildaPinpointDriver(I2cDeviceSynchSimple deviceClient, boolean deviceClientIsOwned) { - super(deviceClient, deviceClientIsOwned); - } - - @Override - public Manufacturer getManufacturer() { - return Manufacturer.Other; - } - - @Override - protected synchronized boolean doInitialize() { - return true; - } - - @Override - public String getDeviceName() { - return "goBILDA® Pinpoint Odometry Computer"; - } - - //Register map of the i2c device - private enum Register { - DEVICE_ID (1), - DEVICE_VERSION (2), - DEVICE_STATUS (3), - DEVICE_CONTROL (4), - LOOP_TIME (5), - X_ENCODER_VALUE (6), - Y_ENCODER_VALUE (7), - X_POSITION (8), - Y_POSITION (9), - H_ORIENTATION (10), - X_VELOCITY (11), - Y_VELOCITY (12), - H_VELOCITY (13), - MM_PER_TICK (14), - X_POD_OFFSET (15), - Y_POD_OFFSET (16), - YAW_SCALAR (17), - BULK_READ (18); - - private final int bVal; - - Register(int bVal){ - this.bVal = bVal; - } - } - - //Device Status enum that captures the current fault condition of the device - public enum DeviceStatus{ - NOT_READY (0), - READY (1), - CALIBRATING (1 << 1), - FAULT_X_POD_NOT_DETECTED (1 << 2), - FAULT_Y_POD_NOT_DETECTED (1 << 3), - FAULT_NO_PODS_DETECTED (1 << 2 | 1 << 3), - FAULT_IMU_RUNAWAY (1 << 4); - - private final int status; - - DeviceStatus(int status){ - this.status = status; - } - } - - //enum that captures the direction the encoders are set to - public enum EncoderDirection{ - FORWARD, - REVERSED; - } - - //enum that captures the kind of goBILDA odometry pods, if goBILDA pods are used - public enum GoBildaOdometryPods { - goBILDA_SWINGARM_POD, - goBILDA_4_BAR_POD; - } - //enum that captures a limited scope of read data. More options may be added in future update - public enum readData { - ONLY_UPDATE_HEADING, - } - - - /** Writes an int to the i2c device - @param reg the register to write the int to - @param i the integer to write to the register - */ - private void writeInt(final Register reg, int i){ - } - - /** - * Reads an int from a register of the i2c device - * @param reg the register to read from - * @return returns an int that contains the value stored in the read register - */ - private int readInt(Register reg){ - return 0; - } - - /** - * Converts a byte array to a float value - * @param byteArray byte array to transform - * @param byteOrder order of byte array to convert - * @return the float value stored by the byte array - */ - private float byteArrayToFloat(byte[] byteArray, ByteOrder byteOrder){ - return ByteBuffer.wrap(byteArray).order(byteOrder).getFloat(); - } - /** - * Reads a float from a register - * @param reg the register to read - * @return the float value stored in that register - */ - - private float readFloat(Register reg){ - return 0; - } - - - /** - * Converts a float to a byte array - * @param value the float array to convert - * @return the byte array converted from the float - */ - private byte [] floatToByteArray (float value, ByteOrder byteOrder) { - return ByteBuffer.allocate(4).order(byteOrder).putFloat(value).array(); - } - - /** - * Writes a byte array to a register on the i2c device - * @param reg the register to write to - * @param bytes the byte array to write - */ - private void writeByteArray (Register reg, byte[] bytes){ - - } - - /** - * Writes a float to a register on the i2c device - * @param reg the register to write to - * @param f the float to write - */ - private void writeFloat (Register reg, float f){ - } - - /** - * Looks up the DeviceStatus enum corresponding with an int value - * @param s int to lookup - * @return the Odometry Computer state - */ - private DeviceStatus lookupStatus (int s){ - if ((s & DeviceStatus.CALIBRATING.status) != 0){ - return DeviceStatus.CALIBRATING; - } - boolean xPodDetected = (s & DeviceStatus.FAULT_X_POD_NOT_DETECTED.status) == 0; - boolean yPodDetected = (s & DeviceStatus.FAULT_Y_POD_NOT_DETECTED.status) == 0; - - if(!xPodDetected && !yPodDetected){ - return DeviceStatus.FAULT_NO_PODS_DETECTED; - } - if (!xPodDetected){ - return DeviceStatus.FAULT_X_POD_NOT_DETECTED; - } - if (!yPodDetected){ - return DeviceStatus.FAULT_Y_POD_NOT_DETECTED; - } - if ((s & DeviceStatus.FAULT_IMU_RUNAWAY.status) != 0){ - return DeviceStatus.FAULT_IMU_RUNAWAY; - } - if ((s & DeviceStatus.READY.status) != 0){ - return DeviceStatus.READY; - } - else { - return DeviceStatus.NOT_READY; - } - } - - /** - * Call this once per loop to read new data from the Odometry Computer. Data will only update once this is called. - */ - public void update(){ - } - - /** - * Call this once per loop to read new data from the Odometry Computer. This is an override of the update() function - * which allows a narrower range of data to be read from the device for faster read times. Currently ONLY_UPDATE_HEADING - * is supported. - * @param data GoBildaPinpointDriver.readData.ONLY_UPDATE_HEADING - */ - public void update(readData data) { - if (data == readData.ONLY_UPDATE_HEADING) { - hOrientation = 0; - } - } - - /** - * Sets the odometry pod positions relative to the point that the odometry computer tracks around.

- * The most common tracking position is the center of the robot.

- * The X pod offset refers to how far sideways (in mm) from the tracking point the X (forward) odometry pod is. Left of the center is a positive number, right of center is a negative number.
- * the Y pod offset refers to how far forwards (in mm) from the tracking point the Y (strafe) odometry pod is. forward of center is a positive number, backwards is a negative number.
- * @param xOffset how sideways from the center of the robot is the X (forward) pod? Left increases - * @param yOffset how far forward from the center of the robot is the Y (Strafe) pod? forward increases - */ - public void setOffsets(double xOffset, double yOffset){ - writeFloat(Register.X_POD_OFFSET, (float) xOffset); - writeFloat(Register.Y_POD_OFFSET, (float) yOffset); - } - - /** - * Recalibrates the Odometry Computer's internal IMU.

- * Robot MUST be stationary

- * Device takes a large number of samples, and uses those as the gyroscope zero-offset. This takes approximately 0.25 seconds. - */ - public void recalibrateIMU(){writeInt(Register.DEVICE_CONTROL,1<<0);} - - /** - * Resets the current position to 0,0,0 and recalibrates the Odometry Computer's internal IMU.

- * Robot MUST be stationary

- * Device takes a large number of samples, and uses those as the gyroscope zero-offset. This takes approximately 0.25 seconds. - */ - public void resetPosAndIMU(){writeInt(Register.DEVICE_CONTROL,1<<1);} - - /** - * Can reverse the direction of each encoder. - * @param xEncoder FORWARD or REVERSED, X (forward) pod should increase when the robot is moving forward - * @param yEncoder FORWARD or REVERSED, Y (strafe) pod should increase when the robot is moving left - */ - public void setEncoderDirections(EncoderDirection xEncoder, EncoderDirection yEncoder){ - if (xEncoder == EncoderDirection.FORWARD){ - writeInt(Register.DEVICE_CONTROL,1<<5); - } - if (xEncoder == EncoderDirection.REVERSED) { - writeInt(Register.DEVICE_CONTROL,1<<4); - } - - if (yEncoder == EncoderDirection.FORWARD){ - writeInt(Register.DEVICE_CONTROL,1<<3); - } - if (yEncoder == EncoderDirection.REVERSED){ - writeInt(Register.DEVICE_CONTROL,1<<2); - } - } - - /** - * If you're using goBILDA odometry pods, the ticks-per-mm values are stored here for easy access.

- * @param pods goBILDA_SWINGARM_POD or goBILDA_4_BAR_POD - */ - public void setEncoderResolution(GoBildaOdometryPods pods){ - if (pods == GoBildaOdometryPods.goBILDA_SWINGARM_POD) { - writeByteArray(Register.MM_PER_TICK, (floatToByteArray(goBILDA_SWINGARM_POD, ByteOrder.LITTLE_ENDIAN))); - } - if (pods == GoBildaOdometryPods.goBILDA_4_BAR_POD){ - writeByteArray(Register.MM_PER_TICK,(floatToByteArray(goBILDA_4_BAR_POD, ByteOrder.LITTLE_ENDIAN))); - } - } - - /** - * Sets the encoder resolution in ticks per mm of the odometry pods.
- * You can find this number by dividing the counts-per-revolution of your encoder by the circumference of the wheel. - * @param ticks_per_mm should be somewhere between 10 ticks/mm and 100 ticks/mm a goBILDA Swingarm pod is ~13.26291192 - */ - public void setEncoderResolution(double ticks_per_mm){ - writeByteArray(Register.MM_PER_TICK,(floatToByteArray((float) ticks_per_mm,ByteOrder.LITTLE_ENDIAN))); - } - - /** - * Tuning this value should be unnecessary.
- * The goBILDA Odometry Computer has a per-device tuned yaw offset already applied when you receive it.

- * This is a scalar that is applied to the gyro's yaw value. Increasing it will mean it will report more than one degree for every degree the sensor fusion algorithm measures.

- * You can tune this variable by rotating the robot a large amount (10 full turns is a good starting place) and comparing the amount that the robot rotated to the amount measured. - * Rotating the robot exactly 10 times should measure 3600°. If it measures more or less, divide moved amount by the measured amount and apply that value to the Yaw Offset.

- * If you find that to get an accurate heading number you need to apply a scalar of more than 1.05, or less than 0.95, your device may be bad. Please reach out to tech@gobilda.com - * @param yawOffset A scalar for the robot's heading. - */ - public void setYawScalar(double yawOffset){ - writeByteArray(Register.YAW_SCALAR,(floatToByteArray((float) yawOffset, ByteOrder.LITTLE_ENDIAN))); - } - - /** - * Send a position that the Pinpoint should use to track your robot relative to. You can use this to - * update the estimated position of your robot with new external sensor data, or to run a robot - * in field coordinates.

- * This overrides the current position.

- * Using this feature to track your robot's position in field coordinates:
- * When you start your code, send a Pose2D that describes the starting position on the field of your robot.
- * Say you're on the red alliance, your robot is against the wall and closer to the audience side, - * and the front of your robot is pointing towards the center of the field. - * You can send a setPosition with something like -600mm x, -1200mm Y, and 90 degrees. The pinpoint would then always - * keep track of how far away from the center of the field you are.

- * Using this feature to update your position with additional sensors:
- * Some robots have a secondary way to locate their robot on the field. This is commonly - * Apriltag localization in FTC, but it can also be something like a distance sensor. - * Often these external sensors are absolute (meaning they measure something about the field) - * so their data is very accurate. But they can be slower to read, or you may need to be in a very specific - * position on the field to use them. In that case, spend most of your time relying on the Pinpoint - * to determine your location. Then when you pull a new position from your secondary sensor, - * send a setPosition command with the new position. The Pinpoint will then track your movement - * relative to that new, more accurate position. - * @param pos a Pose2D describing the robot's new position. - */ - public Pose2D setPosition(Pose2D pos){ - writeByteArray(Register.X_POSITION,(floatToByteArray((float) pos.getX(DistanceUnit.MM), ByteOrder.LITTLE_ENDIAN))); - writeByteArray(Register.Y_POSITION,(floatToByteArray((float) pos.getY(DistanceUnit.MM),ByteOrder.LITTLE_ENDIAN))); - writeByteArray(Register.H_ORIENTATION,(floatToByteArray((float) pos.getHeading(AngleUnit.RADIANS),ByteOrder.LITTLE_ENDIAN))); - return pos; - } - - /** - * Checks the deviceID of the Odometry Computer. Should return 1. - * @return 1 if device is functional. - */ - public int getDeviceID(){return readInt(Register.DEVICE_ID);} - - /** - * @return the firmware version of the Odometry Computer - */ - public int getDeviceVersion(){return readInt(Register.DEVICE_VERSION); } - - public float getYawScalar(){return readFloat(Register.YAW_SCALAR); } - - /** - * Device Status stores any faults the Odometry Computer may be experiencing. These faults include: - * @return one of the following states:
- * NOT_READY - The device is currently powering up. And has not initialized yet. RED LED
- * READY - The device is currently functioning as normal. GREEN LED
- * CALIBRATING - The device is currently recalibrating the gyro. RED LED
- * FAULT_NO_PODS_DETECTED - the device does not detect any pods plugged in. PURPLE LED
- * FAULT_X_POD_NOT_DETECTED - The device does not detect an X pod plugged in. BLUE LED
- * FAULT_Y_POD_NOT_DETECTED - The device does not detect a Y pod plugged in. ORANGE LED
- */ - public DeviceStatus getDeviceStatus(){return DeviceStatus.READY; } - - /** - * Checks the Odometry Computer's most recent loop time.

- * If values less than 500, or more than 1100 are commonly seen here, there may be something wrong with your device. Please reach out to tech@gobilda.com - * @return loop time in microseconds (1/1,000,000 seconds) - */ - public int getLoopTime(){return loopTime; } - - /** - * Checks the Odometry Computer's most recent loop frequency.

- * If values less than 900, or more than 2000 are commonly seen here, there may be something wrong with your device. Please reach out to tech@gobilda.com - * @return Pinpoint Frequency in Hz (loops per second), - */ - public double getFrequency(){ - if (loopTime != 0){ - return 1000000.0/loopTime; - } - else { - return 0; - } - } - - /** - * @return the raw value of the X (forward) encoder in ticks - */ - public int getEncoderX(){return xEncoderValue; } - - /** - * @return the raw value of the Y (strafe) encoder in ticks - */ - public int getEncoderY(){return yEncoderValue; } - - /** - * @return the estimated X (forward) position of the robot in mm - */ - public double getPosX(){return xPosition; } - - /** - * @return the estimated Y (Strafe) position of the robot in mm - */ - public double getPosY(){return yPosition; } - - /** - * @return the estimated H (heading) position of the robot in Radians - */ - public double getHeading(){return hOrientation;} - - /** - * @return the estimated X (forward) velocity of the robot in mm/sec - */ - public double getVelX(){return xVelocity; } - - /** - * @return the estimated Y (strafe) velocity of the robot in mm/sec - */ - public double getVelY(){return yVelocity; } - - /** - * @return the estimated H (heading) velocity of the robot in radians/sec - */ - public double getHeadingVelocity(){return hVelocity; } - - /** - * This uses its own I2C read, avoid calling this every loop. - * @return the user-set offset for the X (forward) pod - */ - public float getXOffset(){return readFloat(Register.X_POD_OFFSET);} - - /** - * This uses its own I2C read, avoid calling this every loop. - * @return the user-set offset for the Y (strafe) pod - */ - public float getYOffset(){return readFloat(Register.Y_POD_OFFSET);} - - /** - * @return a Pose2D containing the estimated position of the robot - */ - public Pose2D getPosition(){ - Pose2d pose = WilyCore.getPose(false); // Use error-added pose - - return new Pose2D(DistanceUnit.INCH, - pose.position.x, - pose.position.y, - AngleUnit.RADIANS, - pose.heading.toDouble()); - } - - /** - * @return a Pose2D containing the estimated velocity of the robot, velocity is unit per second - */ - public Pose2D getVelocity(){ - PoseVelocity2d poseVelocity = WilyCore.getPoseVelocity(); - return new Pose2D(DistanceUnit.INCH, - poseVelocity.linearVel.x, - poseVelocity.linearVel.y, - AngleUnit.RADIANS, - poseVelocity.angVel); - } -} diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/roadrunner/LoonyTune.java b/team417/src/main/java/org/firstinspires/ftc/team417/roadrunner/LoonyTune.java index c7d92c737c65..e45ba10cdd62 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/roadrunner/LoonyTune.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/roadrunner/LoonyTune.java @@ -41,6 +41,7 @@ import com.qualcomm.robotcore.hardware.DcMotorEx; import com.qualcomm.robotcore.hardware.DcMotorSimple; import com.qualcomm.robotcore.hardware.Gamepad; +import com.qualcomm.robotcore.hardware.IMU; import com.qualcomm.robotcore.util.RobotLog; import com.wilyworks.common.WilyWorks; @@ -865,6 +866,7 @@ enum Tuner { Poll poll; Dialog dialog; MecanumDrive drive; + IMU imu; // Built-in IMU TuneParameters currentParameters; TuneParameters originalParameters; boolean usePinpoint; // True if using Pinpoint odometry, false if using OTOS @@ -1840,6 +1842,7 @@ void trackingTest() { String lastSeenStatus = ""; // Most recently reported non-zero status from the sensor double lastSeenTime = 0; // Time at which lastSeenStatus was set double goldenStartTime = 0; // Start time of the golden pose test, in seconds + double startImuYaw = imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS); Pose2d baselinePose = null; // Position where the baseline was set double maxLinearSpeed = 0; // Max linear speed seen, inches/s @@ -1979,9 +1982,10 @@ else if (status == GoBildaPinpointDriver.DeviceStatus.FAULT_IMU_RUNAWAY) double dx = pose.position.x - baselinePose.position.x; double dy = pose.position.y - baselinePose.position.y; - double otosTheta = normalizeAngle(pose.heading.toDouble() - baselinePose.heading.toDouble()); + double odoTheta = normalizeAngle(pose.heading.toDouble() - baselinePose.heading.toDouble()); + double imuTheta = normalizeAngle(imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS) - startImuYaw); - io.out(" Sensor error: (%.2f\", %.2f\"), %.2f\u00b0\n", dx, dy, Math.toDegrees(otosTheta)); + io.out(" Sensor error: (%.2f\", %.2f\"), %.2f\u00b0\n", dx, dy, Math.toDegrees(odoTheta)); if ((totalDistance != 0) && (totalRotation != 0)) { double distancePercent = 100 * Math.abs(dx) / totalDistance; @@ -1990,10 +1994,14 @@ else if (status == GoBildaPinpointDriver.DeviceStatus.FAULT_IMU_RUNAWAY) // Round the minutes so that the rotation error updates every 6 seconds // rather than constantly: double roundedMinutes = Math.round(exactMinutes * 10.0) / 10.0; - double degreesPerMinute = (roundedMinutes == 0) ? 0 : - Math.abs(Math.toDegrees(otosTheta)) / roundedMinutes; + double odoDegreesPerMinute = (roundedMinutes == 0) ? 0 : + Math.abs(Math.toDegrees(odoTheta) / roundedMinutes); + double imuDegreesPerMinute = (roundedMinutes == 0) ? 0 : + Math.abs(Math.toDegrees(imuTheta) / roundedMinutes); + io.out(" Positional error: %.2f%%\n", distancePercent); - io.out(" Rotational error: %.2f\u00b0/minute\n", degreesPerMinute); + io.out(" Rotational error: %.2f\u00b0/minute\n", odoDegreesPerMinute); + io.out(" (Compare to built-in IMU: %.2f\u00b0/minute)\n", imuDegreesPerMinute); io.out("\nGood error results are less than 1% positional and 1\u00b0/minute rotational.\n"); } } @@ -2005,6 +2013,7 @@ else if (status == GoBildaPinpointDriver.DeviceStatus.FAULT_IMU_RUNAWAY) if (io.x()) { // Set golden pose at home drive.setPose(homePose); + startImuYaw = imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS); previousPose = homePose; baselinePose = drive.pose; totalDistance = 0; @@ -4139,6 +4148,7 @@ public void runOpMode() { poll = new Poll(); dialog = new Dialog(); drive = new MecanumDrive(hardwareMap, telemetry, gamepad1, zeroPose); + imu = hardwareMap.get(IMU.class, "imu"); usePinpoint = drive.pinpointDriver != null; currentParameters = new TuneParameters(drive, TuneParameters.getSavedParameters()); originalParameters = currentParameters.createClone(); From 80a9f5b639a147770af472d014ac47aa69aaad78 Mon Sep 17 00:00:00 2001 From: Andrew Date: Tue, 20 Jan 2026 19:23:29 -0800 Subject: [PATCH 191/198] Update MechSim to be visible even when launch motor is off. --- .../java/com/wilyworks/simulator/framework/mechsim/MechSim.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/WilyCore/src/main/java/com/wilyworks/simulator/framework/mechsim/MechSim.java b/WilyCore/src/main/java/com/wilyworks/simulator/framework/mechsim/MechSim.java index b946ec3809b5..fd9568b61f80 100644 --- a/WilyCore/src/main/java/com/wilyworks/simulator/framework/mechsim/MechSim.java +++ b/WilyCore/src/main/java/com/wilyworks/simulator/framework/mechsim/MechSim.java @@ -579,7 +579,7 @@ public void advance(double deltaT) { @Override public void update(Graphics2D g, Pose2d pose) { // Don't call simulate() or render() for things like Configuration Tester. - if ((upperLaunchMotor != null) && (forwardFeederServo.getPower() != 0)) { + if (upperLaunchMotor != null) { simulate(pose, accumulatedDeltaT); render(g, pose); } From 2f4ac897b783a9e1b9481080300127898d33a045 Mon Sep 17 00:00:00 2001 From: anya-codes Date: Tue, 3 Feb 2026 19:14:58 -0800 Subject: [PATCH 192/198] Modified ComplexMechGlob logic for change to drum. Added scan function, scan state, and manual launch override. Removed presence detection from color sensor code --- .../ftc/team417/CompetitionTeleOp.java | 4 +- .../ftc/team417/ComplexMechGlob.java | 107 +++++++++++++++--- .../ftc/team417/CoolColorDetector.java | 78 +++++++------ 3 files changed, 135 insertions(+), 54 deletions(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java index cd0f35f8dc47..d76b8501bb84 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java @@ -153,9 +153,11 @@ public void runOpMode() { } else if (gamepad2.dpadRightWasPressed()) { // turns off the flywheels mechGlob.setLaunchVelocity(LaunchDistance.OFF); + //Manually stop the transfer process } else if (gamepad2.rightBumperWasPressed()) { - mechGlob.controlDrumManually(); + mechGlob.manualLaunchOverride(); } + mechGlob.ohCrap(gamepad2.right_trigger > 0); if (gamepad2.backWasPressed()) { telemetry.log().clear(); diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java b/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java index 1e0c8061ac95..5157b8ce42e9 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java @@ -78,20 +78,22 @@ boolean preLaunch (RequestedColor requestedColor) { } void setLaunchVelocity (LaunchDistance launchDistance) {} - public PixelColor getSlotColor(int slotIndex) { + public PixelColor getSlotColor (int slotIndex) { return PixelColor.NONE; } void controlDrumManually () {} - void ohCrap(boolean engage) {} + void ohCrap (boolean engage) {} + //If endgame or auto, rotate through all 3 drum slots and detect color in each + void scan () {} + void manualLaunchOverride () {} } @Config public class ComplexMechGlob extends MechGlob { //a class encompassing all code that IS for slowbot - // TODO tune constants via FTC Dashboard: public static double FEEDER_POWER = 1; public static double TRANSFER_TIME_UP = 0.6; public static double TRANSFER_TIME_DOWN = 0.25; @@ -114,11 +116,16 @@ public class ComplexMechGlob extends MechGlob { //a class encompassing all code public static double INTAKE_BACK_TIME = 0.25; public static double NEAR_AUTO_VELOCTIY = 835; public static double FAR_AUTO_VELOCITY = 1040; - + //Todo: Update these constants with the values Caden has + public static double LEFT_PADDLE_INACTIVE = 10; + public static double RIGHT_PADDLE_INACTIVE = 10; + public static double LEFT_PADDLE_ACTIVE = 10; + public static double RIGHT_PADDLE_ACTIVE = 10; ElapsedTime transferTimer; ElapsedTime intakeTimer; ElapsedTime intakeBackTimer; + ElapsedTime scanTimer; double userIntakeSpeed; ArrayList drumQueue = new ArrayList<> (); @@ -127,6 +134,7 @@ enum WaitState { DRUM_MOVE_WAIT, //waiting for the ball to fully enter the slot before moving the drum DRUM_MOVE, //waiting for the drum to reach desired position INTAKE, //waiting for the intake to finish + SCAN, //check the contents of the drum TRANSFER, //waiting for the transfer to finish SPIN_UP, //waiting for the flywheel(s) to spin up IDLE, //waiting for input when the drum is full @@ -144,6 +152,8 @@ enum WaitState { double lowerLaunchVelocity; double feederPower; boolean engageOhCrap; + boolean scanningOn; //only in auto and endgame, scan after intaking to score pattern points + boolean scanRequired; //When the intake runs, assume we have a new ball and allow scanning again LaunchDistance launchDistance = LaunchDistance.OFF; @@ -161,6 +171,8 @@ enum WaitState { Servo servoDrumGate; NormalizedColorSensor sensorColor1; NormalizedColorSensor sensorColor2; + Servo servoLeftPaddle; + Servo servoRightPaddle; CoolColorDetector coolColorDetector; @@ -197,6 +209,8 @@ public DrumRequest(double newPosition, double oldPosition, WaitState nextState) servoBLaunchFeeder = hardwareMap.get(CRServo.class, "servoBLaunchFeeder"); servoFLaunchFeeder = hardwareMap.get(CRServo.class, "servoFLaunchFeeder"); servoDrumGate = hardwareMap.get(Servo.class, "servoDrumGate"); + servoRightPaddle = hardwareMap.get(Servo.class, "servoRightPaddle"); + servoLeftPaddle = hardwareMap.get(Servo.class, "servoLeftPaddle"); coolColorDetector = new CoolColorDetector(hardwareMap, telemetry); slotOccupiedBy = new ArrayList<>(Arrays.asList(preloads)); @@ -225,6 +239,7 @@ public DrumRequest(double newPosition, double oldPosition, WaitState nextState) + } //the position argument denotes whether we are using intake or launch positions @@ -265,20 +280,46 @@ void intake (double intakeSpeed) { } @Override - //a class that controls the launcher and transfer + // controls the launcher and transfer boolean launch (RequestedColor requestedColor, LimelightDetector detector) { detector.tryResetRobotPose(telemetry); // Resets the robot pose only if the robot is not moving if (launchDistance == LaunchDistance.OFF) { launchDistance = LaunchDistance.NEAR; } + + if (requestedColor != RequestedColor.EITHER) { + addToDrumQueue(INTAKE_POSITIONS[0], WaitState.SCAN); + addToDrumQueue(INTAKE_POSITIONS[1], WaitState.SCAN); + addToDrumQueue(INTAKE_POSITIONS[2], WaitState.SCAN); + } + int minSlot = findNearestSlot(LAUNCH_POSITIONS, requestedColor); - if (minSlot != -1){ + if (minSlot != -1) { addToDrumQueue(LAUNCH_POSITIONS[minSlot], WaitState.SPIN_UP); - slotOccupiedBy.set (minSlot, PixelColor.NONE); //marking this slot as empty so we don't accidentally try to use it again + slotOccupiedBy.set(minSlot, PixelColor.NONE); //marking this slot as empty so we don't accidentally try to use it again return true; } + return false; + + + + // If we want to score pattern points and have not already scanned after last intake, we want to scan the contents of the drum +// if (requestedColor != RequestedColor.EITHER && scanRequired) { +// addToDrumQueue(INTAKE_POSITIONS[0], WaitState.SCAN); +// addToDrumQueue(INTAKE_POSITIONS[1], WaitState.SCAN); +// addToDrumQueue(INTAKE_POSITIONS[2], WaitState.SCAN); +// +// } else { +// int minSlot = findNearestSlot(LAUNCH_POSITIONS, requestedColor); +// if (minSlot != -1) { +// addToDrumQueue(LAUNCH_POSITIONS[minSlot], WaitState.SPIN_UP); +// slotOccupiedBy.set(minSlot, PixelColor.NONE); //marking this slot as empty so we don't accidentally try to use it again +// return true; +// } +// } +// return false; } //this function adds a new drum request to the drum queue. nextState is the state do use after the drum is finished moving void addToDrumQueue(double position, WaitState nextState){ @@ -367,7 +408,7 @@ void controlDrumManually () { } //if the gate is stuck, move the drum back to get the stuck ball out, along with opening gate @Override - void ohCrap(boolean engage) { + void ohCrap (boolean engage) { engageOhCrap = engage; if (engage) { if (drumQueue.size() == 1) { @@ -379,11 +420,32 @@ void ohCrap(boolean engage) { } @Override - public PixelColor getSlotColor(int slotIndex) { + public PixelColor getSlotColor (int slotIndex) { PixelColor artifactColor = slotOccupiedBy.get(slotIndex); return artifactColor; } + @Override + void scan () { + PixelColor slotColor = coolColorDetector.detectArtifactColor(); + int slot = findSlotFromPosition(hwDrumPosition, INTAKE_POSITIONS); + slotOccupiedBy.set(slot, slotColor); + } + + @Override + void manualLaunchOverride () { + //TODO: Can I set transfer position here? Does it have to be at the bottom? + servoTransfer.setPosition(TRANSFER_INACTIVE_POSITION); + ElapsedTime timer = new ElapsedTime(); + int slot = findSlotFromPosition(hwDrumPosition, LAUNCH_POSITIONS); + while (timer.seconds() < TRANSFER_TIME_DOWN) { + slotOccupiedBy.set(slot, PixelColor.NONE); + } + //TODO: Is this the correct state to move into after stopping launch? + waitState = WaitState.DRUM_MOVE_WAIT; + + } + @Override void update () { double intakePower = 0; @@ -416,6 +478,7 @@ void update () { if (intakePower > 0 && waitState != WaitState.DRUM_MOVE) { gatePosition = DRUM_GATE_OPEN_POSITION; + scanRequired = true; } else { gatePosition = DRUM_GATE_CLOSED_POSITION; } @@ -439,6 +502,17 @@ void update () { } } + if (waitState == WaitState.SCAN) { +// int i = 0; + if (drumAtPosition()) { + scan(); +// i += 1; + } + + waitState = WaitState.DRUM_MOVE; + //TODO: Add to drum queue the driver's launch request with requested color that was not done because scan was req + } + // let a firing request interrupt an intake if (waitState == WaitState.IDLE || waitState == WaitState.INTAKE) { if (!drumQueue.isEmpty()) { @@ -473,15 +547,14 @@ void update () { } if (waitState == WaitState.INTAKE) { launchDistance = LaunchDistance.OFF; - PixelColor slotColor = coolColorDetector.detectArtifactColor(); - if (slotColor != PixelColor.NONE) { - int slot = findSlotFromPosition(hwDrumPosition, INTAKE_POSITIONS); - slotOccupiedBy.set(slot, slotColor); - waitState = WaitState.DRUM_MOVE_WAIT; - intakeTimer = new ElapsedTime(); - intakeBackTimer = new ElapsedTime(); - } + // Because slot colors don't matter right now + // so we assume all 3 slots are purple + slotOccupiedBy.set(0, PixelColor.PURPLE); + slotOccupiedBy.set(1, PixelColor.PURPLE); + slotOccupiedBy.set(2, PixelColor.PURPLE); } + + //TODO remove? if (waitState == WaitState.DRUM_MOVE_WAIT) { if (intakeTimer.seconds() >= INTAKE_TIMER) { waitState = WaitState.IDLE; diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CoolColorDetector.java b/team417/src/main/java/org/firstinspires/ftc/team417/CoolColorDetector.java index 64d3bb56beea..fba0db5d1db8 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CoolColorDetector.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CoolColorDetector.java @@ -47,45 +47,51 @@ PixelColor detectArtifactColor() { double minDistance; NormalizedColorSensor sensor; - if (distance1 < MAXIMUM_DISTANCE && distance1 > MINIMUM_DISTANCE && distance2 > MINIMUM_DISTANCE) { - sensor = sensor1; - minDistance = distance1; - } else { - sensor = sensor2; - minDistance = distance2; - } - if (minDistance > MAXIMUM_DISTANCE || minDistance < MINIMUM_DISTANCE) { - String string = String.format(" %.1f, %.1f\"", distance1, distance2); - telemetry.addLine(string); - Log.d("CoolColorDetector", string); - return PixelColor.NONE; - } - - NormalizedRGBA colors = sensor.getNormalizedColors(); - float[] hsv = new float[3]; - Color.colorToHSV(colors.toColor(), hsv); - float hue = hsv[0]; - float value = hsv[2]; - - String colorCube = String.format("\u25a0\u25a0\u25a0", - colors.toColor() & 0xffffff); - - PixelColor result = PixelColor.NONE; - if (value > MIN_VALUE) { - if (hue > GREEN_MIN_HUE && hue < GREEN_MAX_HUE) { //range determined from testing +// if (distance1 < MAXIMUM_DISTANCE && distance1 > MINIMUM_DISTANCE && distance2 > MINIMUM_DISTANCE) { +// sensor = sensor1; +// minDistance = distance1; +// } else { +// sensor = sensor2; +// minDistance = distance2; +// } + +// if (minDistance > MAXIMUM_DISTANCE || minDistance < MINIMUM_DISTANCE) { +// String string = String.format(" %.1f, %.1f\"", distance1, distance2); +// telemetry.addLine(string); +// Log.d("CoolColorDetector", string); +// return PixelColor.NONE; +// } + NormalizedRGBA sensor1Color = sensor1.getNormalizedColors(); + NormalizedRGBA sensor2Color = sensor2.getNormalizedColors(); + + float[] hsv1 = new float[3]; + Color.colorToHSV(sensor1Color.toColor(), hsv1); + float hue1 = hsv1[0]; + float value1 = hsv1[2]; + + float[] hsv2 = new float[3]; + Color.colorToHSV(sensor2Color.toColor(), hsv2); + float hue2 = hsv2[0]; + float value2 = hsv2[2]; + + float averagedHue = (hue1 + hue2)/2; + +// String colorCube = String.format("\u25a0\u25a0\u25a0", +// averagedColor.toColor() & 0xffffff); + + PixelColor result = PixelColor.PURPLE; + if (averagedHue > GREEN_MIN_HUE && averagedHue < GREEN_MAX_HUE) { //range determined from testing result = PixelColor.GREEN; - } else if (hue >= PURPLE_MIN_HUE && hue <= PURPLE_MAX_HUE) { //range determined from testing + } else if (averagedHue >= PURPLE_MIN_HUE && averagedHue <= PURPLE_MAX_HUE) { //range determined from testing result = PixelColor.PURPLE; } - } - - String string = String.format("%.1f/%.1fmm %s H: %.1f V: %.2f %s", - distance1, distance2, colorCube, hue, value, result); - if (result != PixelColor.NONE) { - telemetry.log().add(string); - } - telemetry.addLine(string); - Log.d("CoolColorDetector", string); + +// String string = String.format("%.1f/%.1fmm %s H: %.1f V: %.2f %s", +// distance1, distance2, colorCube, hue, value, result); + +// telemetry.log().add(string); +// telemetry.addLine(string); +// Log.d("CoolColorDetector", string); // Return the result that was decided in the if statements above return result; From d7e03c8c8e24ef4ae1195e89ffd7fb1ef0cd4ce2 Mon Sep 17 00:00:00 2001 From: anya-codes Date: Thu, 5 Feb 2026 21:29:50 -0800 Subject: [PATCH 193/198] Added paddle transfer logic, fixed stopLaunch, changed some of the state machine logic after debugging in WilyWorks, changed color range values --- .../ftc/team417/CompetitionTeleOp.java | 2 +- .../ftc/team417/ComplexMechGlob.java | 202 +++++++++++------- .../ftc/team417/CoolColorDetector.java | 4 +- 3 files changed, 123 insertions(+), 85 deletions(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java index d76b8501bb84..9ff76f0f0c2d 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CompetitionTeleOp.java @@ -155,7 +155,7 @@ public void runOpMode() { mechGlob.setLaunchVelocity(LaunchDistance.OFF); //Manually stop the transfer process } else if (gamepad2.rightBumperWasPressed()) { - mechGlob.manualLaunchOverride(); + mechGlob.stopLaunch(); } mechGlob.ohCrap(gamepad2.right_trigger > 0); diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java b/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java index 5157b8ce42e9..10b388aa2d03 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java @@ -84,10 +84,7 @@ public PixelColor getSlotColor (int slotIndex) { void controlDrumManually () {} void ohCrap (boolean engage) {} - //If endgame or auto, rotate through all 3 drum slots and detect color in each - void scan () {} - - void manualLaunchOverride () {} + void stopLaunch() {} } @@ -97,6 +94,8 @@ public class ComplexMechGlob extends MechGlob { //a class encompassing all code public static double FEEDER_POWER = 1; public static double TRANSFER_TIME_UP = 0.6; public static double TRANSFER_TIME_DOWN = 0.25; + //TODO will need to tune the time for paddle transfer + public static double PADDLE_TRANSFER_TIME_UP = 0.25; // How long we wait before bringing the paddles down (we don't need time for down because they don't interfere with drum) // how long we wait before continuing after the color detector // detects. this is 0 because it will likely become obsolete @@ -110,22 +109,24 @@ public class ComplexMechGlob extends MechGlob { //a class encompassing all code public static double INTAKE_SPEED = 1; public static double FLYWHEEL_VELOCITY_TOLERANCE = 22; //this is an epsiiiiiiiiilon was 10 public static double VOLTAGE_TOLERANCE = 0.04; //THIS IS AN EPSILON AS WELLLLLL - public static double DRUM_GATE_OPEN_POSITION = .8; - public static double DRUM_GATE_CLOSED_POSITION = 0.59; + public static double DRUM_GATE_OPEN_POSITION = 1; + public static double DRUM_GATE_CLOSED_POSITION = 0.7; public static double MOTOR_D_VALUE = 1; public static double INTAKE_BACK_TIME = 0.25; public static double NEAR_AUTO_VELOCTIY = 835; public static double FAR_AUTO_VELOCITY = 1040; //Todo: Update these constants with the values Caden has - public static double LEFT_PADDLE_INACTIVE = 10; - public static double RIGHT_PADDLE_INACTIVE = 10; - public static double LEFT_PADDLE_ACTIVE = 10; - public static double RIGHT_PADDLE_ACTIVE = 10; + public static double LEFT_PADDLE_INACTIVE_POSITION = -1; + public static double RIGHT_PADDLE_INACTIVE_POSITION = -1; + public static double LEFT_PADDLE_ACTIVE_POSITION = 0.6; + public static double RIGHT_PADDLE_ACTIVE_POSITION = 0.6; ElapsedTime transferTimer; + ElapsedTime transferDownTimer; ElapsedTime intakeTimer; ElapsedTime intakeBackTimer; - ElapsedTime scanTimer; + ElapsedTime paddleTransferTimer; + ElapsedTime scanTimer; //wait this long after drum rotates to slot before scanning double userIntakeSpeed; ArrayList drumQueue = new ArrayList<> (); @@ -136,6 +137,8 @@ enum WaitState { INTAKE, //waiting for the intake to finish SCAN, //check the contents of the drum TRANSFER, //waiting for the transfer to finish + TRANSFER_DOWN, //when stopLaunch is called, waits for transfer to go down + LAUNCH_AFTER_SCAN, //launch the color that was requested before scan SPIN_UP, //waiting for the flywheel(s) to spin up IDLE, //waiting for input when the drum is full @@ -152,7 +155,6 @@ enum WaitState { double lowerLaunchVelocity; double feederPower; boolean engageOhCrap; - boolean scanningOn; //only in auto and endgame, scan after intaking to score pattern points boolean scanRequired; //When the intake runs, assume we have a new ball and allow scanning again LaunchDistance launchDistance = LaunchDistance.OFF; @@ -180,11 +182,13 @@ class DrumRequest { double newPosition; double oldPosition; WaitState nextState; + RequestedColor requestedColor; - public DrumRequest(double newPosition, double oldPosition, WaitState nextState) { + public DrumRequest(double newPosition, double oldPosition, WaitState nextState, RequestedColor requestedColor) { this.nextState = nextState; this.newPosition = newPosition; this.oldPosition = oldPosition; + this.requestedColor = requestedColor; } } ComplexMechGlob (HardwareMap hardwareMap, Telemetry telemetry, PixelColor[] preloads) { @@ -206,13 +210,15 @@ public DrumRequest(double newPosition, double oldPosition, WaitState nextState) motLLauncher = hardwareMap.get(DcMotorEx.class, "motLLauncher"); motULauncher = hardwareMap.get(DcMotorEx.class, "motULauncher"); motIntake = hardwareMap.get(DcMotorEx.class, "motIntake"); - servoBLaunchFeeder = hardwareMap.get(CRServo.class, "servoBLaunchFeeder"); - servoFLaunchFeeder = hardwareMap.get(CRServo.class, "servoFLaunchFeeder"); +// servoBLaunchFeeder = hardwareMap.get(CRServo.class, "servoBLaunchFeeder"); +// servoFLaunchFeeder = hardwareMap.get(CRServo.class, "servoFLaunchFeeder"); servoDrumGate = hardwareMap.get(Servo.class, "servoDrumGate"); servoRightPaddle = hardwareMap.get(Servo.class, "servoRightPaddle"); servoLeftPaddle = hardwareMap.get(Servo.class, "servoLeftPaddle"); coolColorDetector = new CoolColorDetector(hardwareMap, telemetry); slotOccupiedBy = new ArrayList<>(Arrays.asList(preloads)); + servoRightPaddle.setPosition(RIGHT_PADDLE_INACTIVE_POSITION); + servoLeftPaddle.setPosition(LEFT_PADDLE_INACTIVE_POSITION); /* @@ -235,7 +241,7 @@ public DrumRequest(double newPosition, double oldPosition, WaitState nextState) motULauncher.setDirection(DcMotorSimple.Direction.REVERSE); motLLauncher.setDirection(DcMotorSimple.Direction.REVERSE); - servoBLaunchFeeder.setDirection(DcMotorSimple.Direction.REVERSE); + servoRightPaddle.setDirection(Servo.Direction.REVERSE); @@ -288,42 +294,33 @@ boolean launch (RequestedColor requestedColor, LimelightDetector detector) { launchDistance = LaunchDistance.NEAR; } - if (requestedColor != RequestedColor.EITHER) { + if (requestedColor != RequestedColor.EITHER && scanRequired) { addToDrumQueue(INTAKE_POSITIONS[0], WaitState.SCAN); addToDrumQueue(INTAKE_POSITIONS[1], WaitState.SCAN); addToDrumQueue(INTAKE_POSITIONS[2], WaitState.SCAN); - } - - int minSlot = findNearestSlot(LAUNCH_POSITIONS, requestedColor); - if (minSlot != -1) { - addToDrumQueue(LAUNCH_POSITIONS[minSlot], WaitState.SPIN_UP); - slotOccupiedBy.set(minSlot, PixelColor.NONE); //marking this slot as empty so we don't accidentally try to use it again - return true; + addToDrumQueue(INTAKE_POSITIONS[0], WaitState.LAUNCH_AFTER_SCAN, requestedColor); + scanRequired = false; + } else { + int minSlot = findNearestSlot(LAUNCH_POSITIONS, requestedColor); + if (minSlot != -1) { + addToDrumQueue(LAUNCH_POSITIONS[minSlot], WaitState.SPIN_UP); + slotOccupiedBy.set(minSlot, PixelColor.NONE); //marking this slot as empty so we don't accidentally try to use it again + return true; + } } return false; + } - - - // If we want to score pattern points and have not already scanned after last intake, we want to scan the contents of the drum -// if (requestedColor != RequestedColor.EITHER && scanRequired) { -// addToDrumQueue(INTAKE_POSITIONS[0], WaitState.SCAN); -// addToDrumQueue(INTAKE_POSITIONS[1], WaitState.SCAN); -// addToDrumQueue(INTAKE_POSITIONS[2], WaitState.SCAN); -// -// } else { -// int minSlot = findNearestSlot(LAUNCH_POSITIONS, requestedColor); -// if (minSlot != -1) { -// addToDrumQueue(LAUNCH_POSITIONS[minSlot], WaitState.SPIN_UP); -// slotOccupiedBy.set(minSlot, PixelColor.NONE); //marking this slot as empty so we don't accidentally try to use it again -// return true; -// } -// } -// return false; + // Adds new drum request to the queue when we don't need to specify requested color + void addToDrumQueue(double position, WaitState nextState) { + addToDrumQueue(position, nextState, RequestedColor.EITHER); } - //this function adds a new drum request to the drum queue. nextState is the state do use after the drum is finished moving - void addToDrumQueue(double position, WaitState nextState){ - drumQueue.add(new DrumRequest(position, lastQueuedPosition, nextState)); + + // This function adds a new drum request to the drum queue when we need to specify requested color. + // nextState is the state do use after the drum is finished moving + void addToDrumQueue(double position, WaitState nextState, RequestedColor requestedColor){ + drumQueue.add(new DrumRequest(position, lastQueuedPosition, nextState, requestedColor)); lastQueuedPosition = position; } @@ -381,8 +378,8 @@ void calculateLaunchVelocity () { else { upperLaunchVelocity = 0; lowerLaunchVelocity = 0; - servoBLaunchFeeder.setPower(0); - servoFLaunchFeeder.setPower(0); +// servoBLaunchFeeder.setPower(0); +// servoFLaunchFeeder.setPower(0); feederPower = 0; } } @@ -425,7 +422,6 @@ public PixelColor getSlotColor (int slotIndex) { return artifactColor; } - @Override void scan () { PixelColor slotColor = coolColorDetector.detectArtifactColor(); int slot = findSlotFromPosition(hwDrumPosition, INTAKE_POSITIONS); @@ -433,17 +429,10 @@ void scan () { } @Override - void manualLaunchOverride () { - //TODO: Can I set transfer position here? Does it have to be at the bottom? - servoTransfer.setPosition(TRANSFER_INACTIVE_POSITION); - ElapsedTime timer = new ElapsedTime(); - int slot = findSlotFromPosition(hwDrumPosition, LAUNCH_POSITIONS); - while (timer.seconds() < TRANSFER_TIME_DOWN) { - slotOccupiedBy.set(slot, PixelColor.NONE); + void stopLaunch() { + if (waitState == WaitState.TRANSFER) { + waitState = WaitState.TRANSFER_DOWN; } - //TODO: Is this the correct state to move into after stopping launch? - waitState = WaitState.DRUM_MOVE_WAIT; - } @Override @@ -459,12 +448,13 @@ void update () { // allow the intake to run if the driver wants it to intakePower = REVERSE_INTAKE_SPEED; } else if (userIntakeSpeed > 0) { + intakePower= INTAKE_SPEED; // if we are in the intake waitState, allow the intake to run - if (waitState == WaitState.INTAKE) { - intakePower = INTAKE_SPEED; - } else if (!drumQueue.isEmpty() && drumQueue.get(0).nextState == WaitState.INTAKE) { - intakePower = INTAKE_SPEED; - } +// if (waitState == WaitState.INTAKE) { +// intakePower = INTAKE_SPEED; +// } else if (!drumQueue.isEmpty() && drumQueue.get(0).nextState == WaitState.INTAKE) { +// intakePower = INTAKE_SPEED; +// } } // whenever we see a ball in the drum then we move the intake back so the ball immediately // doesn't get stuck in the drum @@ -490,27 +480,35 @@ void update () { if (waitState == WaitState.IDLE) { if (userIntakeSpeed > 0) { - int minSlot = findNearestSlot(INTAKE_POSITIONS, RequestedColor.NONE); - if (minSlot != -1) { - addToDrumQueue(INTAKE_POSITIONS[minSlot], WaitState.INTAKE); - waitState = WaitState.INTAKE; + int minSlot = findNearestSlot(INTAKE_POSITIONS, RequestedColor.PURPLE); + int a = findSlotFromPosition(hwDrumPosition, INTAKE_POSITIONS); + if (a == -1) { + addToDrumQueue(minSlot, WaitState.INTAKE); } + +// if (minSlot != -1) { +// addToDrumQueue(INTAKE_POSITIONS[minSlot], WaitState.INTAKE); + waitState = WaitState.INTAKE; +// } } // this makes it so that after we are done launching the drum goes to intake position - if (drumQueue.isEmpty() && slotOccupiedBy.stream().allMatch(e -> e == PixelColor.NONE)) { - addToDrumQueue(INTAKE_POSITIONS[0], WaitState.INTAKE); - } +// if (drumQueue.isEmpty() && slotOccupiedBy.stream().allMatch(e -> e == PixelColor.NONE)) { +// addToDrumQueue(INTAKE_POSITIONS[0], WaitState.INTAKE); +// } } - if (waitState == WaitState.SCAN) { -// int i = 0; if (drumAtPosition()) { - scan(); -// i += 1; + if (scanTimer == null) { + scanTimer = new ElapsedTime(); + } + if (scanTimer.seconds() >= 1) { + PixelColor slotCo = coolColorDetector.detectArtifactColor(); + telemetry.addData("slotco", slotCo); + scan(); + scanTimer = null; + waitState = WaitState.IDLE; + } } - - waitState = WaitState.DRUM_MOVE; - //TODO: Add to drum queue the driver's launch request with requested color that was not done because scan was req } // let a firing request interrupt an intake @@ -522,8 +520,19 @@ void update () { } if (waitState == WaitState.DRUM_MOVE) { if (drumAtPosition()) { - waitState = drumQueue.get(0).nextState; + DrumRequest request = drumQueue.get(0); + waitState = request.nextState; + RequestedColor requestedColor = request.requestedColor; drumQueue.remove(0); + if (waitState == WaitState.LAUNCH_AFTER_SCAN) { + int minSlot = findNearestSlot(LAUNCH_POSITIONS, requestedColor); + if (minSlot != -1) { + addToDrumQueue(LAUNCH_POSITIONS[minSlot], WaitState.SPIN_UP); + slotOccupiedBy.set(minSlot, PixelColor.NONE); //marking this slot as empty so we don't accidentally try to use it again + } + waitState = WaitState.IDLE; + + } } } if (waitState == WaitState.SPIN_UP) { @@ -533,6 +542,8 @@ void update () { } } double transferPosition = TRANSFER_INACTIVE_POSITION; + double leftPaddlePosition = LEFT_PADDLE_INACTIVE_POSITION; + double rightPaddlePosition = RIGHT_PADDLE_INACTIVE_POSITION; if (waitState == WaitState.TRANSFER) { if (transferTimer == null) { transferTimer = new ElapsedTime(); @@ -541,10 +552,20 @@ void update () { transferPosition = TRANSFER_ACTIVE_POSITION; } if (transferTimer.seconds() >= TRANSFER_TIME_UP + TRANSFER_TIME_DOWN) { + if (paddleTransferTimer == null) { + paddleTransferTimer = new ElapsedTime(); + } + if (paddleTransferTimer.seconds() < PADDLE_TRANSFER_TIME_UP) { + leftPaddlePosition = LEFT_PADDLE_ACTIVE_POSITION; + rightPaddlePosition = RIGHT_PADDLE_ACTIVE_POSITION; + } + waitState = WaitState.IDLE; transferTimer = null; + paddleTransferTimer = null; } } + if (waitState == WaitState.INTAKE) { launchDistance = LaunchDistance.OFF; // Because slot colors don't matter right now @@ -552,6 +573,24 @@ void update () { slotOccupiedBy.set(0, PixelColor.PURPLE); slotOccupiedBy.set(1, PixelColor.PURPLE); slotOccupiedBy.set(2, PixelColor.PURPLE); + waitState = WaitState.IDLE; + } + if (waitState == WaitState.TRANSFER_DOWN) { + if (transferDownTimer == null) { + transferDownTimer = new ElapsedTime(); + } + while (transferDownTimer.seconds() <= TRANSFER_TIME_DOWN) { + int currSlot = findSlotFromPosition(hwDrumPosition, LAUNCH_POSITIONS); + slotOccupiedBy.set(currSlot, PixelColor.NONE); + transferPosition = TRANSFER_INACTIVE_POSITION; + leftPaddlePosition = LEFT_PADDLE_INACTIVE_POSITION; + rightPaddlePosition = RIGHT_PADDLE_INACTIVE_POSITION; + } + if (transferTimer.seconds() > TRANSFER_TIME_DOWN) { + waitState = WaitState.IDLE; + transferTimer = null; + transferDownTimer = null; + } } //TODO remove? @@ -562,16 +601,15 @@ void update () { } servoDrum.setPosition(hwDrumPosition); - //servoTransfer.setPosition(transferPosition); - - // Enable on real hardware once transfer parameters are tuned + servoLeftPaddle.setPosition(leftPaddlePosition); + servoRightPaddle.setPosition(rightPaddlePosition); servoTransfer.setPosition(transferPosition); servoDrumGate.setPosition(gatePosition); motLLauncher.setVelocity(lowerLaunchVelocity); motULauncher.setVelocity(upperLaunchVelocity); motIntake.setPower(intakePower); - servoBLaunchFeeder.setPower(feederPower); - servoFLaunchFeeder.setPower(feederPower); +// servoBLaunchFeeder.setPower(feederPower); +// servoFLaunchFeeder.setPower(feederPower); telemetry.addData("hwDrumPos", hwDrumPosition); telemetry.addData("currVoltage ", "%.2f", analogDrum.getVoltage()); } diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CoolColorDetector.java b/team417/src/main/java/org/firstinspires/ftc/team417/CoolColorDetector.java index fba0db5d1db8..0ba114cc6a39 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CoolColorDetector.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CoolColorDetector.java @@ -19,8 +19,8 @@ public class CoolColorDetector { public static double MINIMUM_DISTANCE = 21; public static double PURPLE_MIN_HUE = 200; - public static double PURPLE_MAX_HUE = 235; - public static double GREEN_MIN_HUE = 150; + public static double PURPLE_MAX_HUE = 320; //increased from 235 to loosen requirement + public static double GREEN_MIN_HUE = 100; //decreased from 150 to loosen requirement public static double GREEN_MAX_HUE = 180; public static double MIN_VALUE = 0.25; public static float GAIN = 85f; // adjust for brightness From b352dec9c1d6b7f75e9d7183cddd1dcb636f53fb Mon Sep 17 00:00:00 2001 From: Andrew Date: Tue, 10 Feb 2026 19:22:41 -0800 Subject: [PATCH 194/198] Remove the feeders from the Mech Sim. --- .../simulator/framework/mechsim/MechSim.java | 25 ------------------- 1 file changed, 25 deletions(-) diff --git a/WilyCore/src/main/java/com/wilyworks/simulator/framework/mechsim/MechSim.java b/WilyCore/src/main/java/com/wilyworks/simulator/framework/mechsim/MechSim.java index fd9568b61f80..56fe0e8e1c13 100644 --- a/WilyCore/src/main/java/com/wilyworks/simulator/framework/mechsim/MechSim.java +++ b/WilyCore/src/main/java/com/wilyworks/simulator/framework/mechsim/MechSim.java @@ -139,7 +139,6 @@ enum BallColor {PURPLE, GREEN, GOLD} final double TRANSFER_SERVO_SPEED = (60.0 / 360) / 0.25; // Speed of a goBilda torque servo, position/s final double LAUNCH_SPEED = 144; // Ball launch speed, inches per second final Point LAUNCH_OFFSET = new Point(-4, 0); - final double FEEDER_POWER = 0.1; // Minimum power for the feeder servo final double GOAL_EPSILON = 12; // Distance from goal center to consider a goal final Point[] GOAL_CENTERS = { new Point(-72, 72), new Point(-72, -72) }; final Point[] CLASSIFIER_STARTS = { new Point(-BALL_SIZE/2, 72-BALL_SIZE/2), @@ -192,8 +191,6 @@ public Ball(BallColor color, double x, double y) { DcMotorEx intakeMotor; Servo drumServo; Servo transferServo; - CRServo forwardFeederServo; - CRServo backwardFeederServo; // State: double accumulatedDeltaT; @@ -245,12 +242,6 @@ public HardwareDevice hookDevice(String name, HardwareDevice device) { if (name.equals("servoTransfer")) { transferServo = (Servo) device; } - if (name.equals("servoFLaunchFeeder")) { - forwardFeederServo = (CRServo) device; - } - if (name.equals("servoBLaunchFeeder")) { - backwardFeederServo = (CRServo) device; - } // There have outputs: if (name.equals("motULauncher")) { @@ -289,10 +280,6 @@ void verifyState() { throw new RuntimeException("Missing drum servo"); if (transferServo == null) throw new RuntimeException("Missing transfer servo"); - if (forwardFeederServo == null) - throw new RuntimeException("Missing forward feeder servo"); - if (backwardFeederServo == null) - throw new RuntimeException("Missing backward feeder servo"); if (analogDrum == null) throw new RuntimeException("Missing analog drum"); if (sensorColor1 == null) @@ -449,12 +436,6 @@ void simulate(Pose2d pose, double deltaT) { // Handle load requests for the launch calibration app, signaled by running the launchers // backwards: if (upperLaunchMotor.getVelocity() < 0) { - if (forwardFeederServo.getPower() >= 0) { - throw new RuntimeException("When running launch motors backwards, forward feeder servo must too."); - } - if (backwardFeederServo.getPower() >= 0) { - throw new RuntimeException("When running launch motors backwards, backward feeder servo must too."); - } // If the slot is empty, fill it up with a ball! if ((transferSlot != -1) && (slotBalls.get(transferSlot) == null)) { slotBalls.set(transferSlot, new Ball(BallColor.PURPLE, 0, 0)); @@ -479,12 +460,6 @@ void simulate(Pose2d pose, double deltaT) { if (lowerLaunchMotor.getVelocity() <= 0) { throw new RuntimeException("A transfer is requested when lower launcher motor isn't running forward. That won't work!"); } - if (forwardFeederServo.getPower() < FEEDER_POWER) { - throw new RuntimeException("A transfer is requested when forward feeder servo isn't running. That won't work!"); - } - if (Math.abs(backwardFeederServo.getPower()) < FEEDER_POWER) { - throw new RuntimeException("A transfer is requested when backward feeder servo isn't running. That won't work!"); - } if (slotBalls.get(transferSlot) == null) { if (!hasIntaken && !hasLaunched) { // A transfer has been requested there was no intake done. Assume that this From 78b06357755838baf53f0d98bfdb36063658b5d6 Mon Sep 17 00:00:00 2001 From: anya-codes Date: Tue, 10 Feb 2026 20:06:56 -0800 Subject: [PATCH 195/198] Fixed the problem with the drum not rotating to intake slots on intake, and the index out of bounds error caused by the first fix --- .../ftc/team417/ComplexMechGlob.java | 17 ++++++++++------- 1 file changed, 10 insertions(+), 7 deletions(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java b/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java index 10b388aa2d03..06f55526354e 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java @@ -95,7 +95,7 @@ public class ComplexMechGlob extends MechGlob { //a class encompassing all code public static double TRANSFER_TIME_UP = 0.6; public static double TRANSFER_TIME_DOWN = 0.25; //TODO will need to tune the time for paddle transfer - public static double PADDLE_TRANSFER_TIME_UP = 0.25; // How long we wait before bringing the paddles down (we don't need time for down because they don't interfere with drum) + public static double PADDLE_TRANSFER_TIME_UP = 3; // How long we wait before bringing the paddles down (we don't need time for down because they don't interfere with drum) // how long we wait before continuing after the color detector // detects. this is 0 because it will likely become obsolete @@ -115,7 +115,6 @@ public class ComplexMechGlob extends MechGlob { //a class encompassing all code public static double INTAKE_BACK_TIME = 0.25; public static double NEAR_AUTO_VELOCTIY = 835; public static double FAR_AUTO_VELOCITY = 1040; - //Todo: Update these constants with the values Caden has public static double LEFT_PADDLE_INACTIVE_POSITION = -1; public static double RIGHT_PADDLE_INACTIVE_POSITION = -1; public static double LEFT_PADDLE_ACTIVE_POSITION = 0.6; @@ -481,9 +480,13 @@ void update () { if (waitState == WaitState.IDLE) { if (userIntakeSpeed > 0) { int minSlot = findNearestSlot(INTAKE_POSITIONS, RequestedColor.PURPLE); + if (minSlot == -1) { + minSlot = findNearestSlot(INTAKE_POSITIONS, RequestedColor.NONE); + } + int a = findSlotFromPosition(hwDrumPosition, INTAKE_POSITIONS); if (a == -1) { - addToDrumQueue(minSlot, WaitState.INTAKE); + addToDrumQueue(INTAKE_POSITIONS[minSlot], WaitState.INTAKE); } // if (minSlot != -1) { @@ -548,10 +551,7 @@ void update () { if (transferTimer == null) { transferTimer = new ElapsedTime(); } - if (transferTimer.seconds() <= TRANSFER_TIME_UP) { - transferPosition = TRANSFER_ACTIVE_POSITION; - } - if (transferTimer.seconds() >= TRANSFER_TIME_UP + TRANSFER_TIME_DOWN) { + if (transferTimer.seconds() >= TRANSFER_TIME_UP) { if (paddleTransferTimer == null) { paddleTransferTimer = new ElapsedTime(); } @@ -560,6 +560,9 @@ void update () { rightPaddlePosition = RIGHT_PADDLE_ACTIVE_POSITION; } + transferPosition = TRANSFER_ACTIVE_POSITION; + } + if (transferTimer.seconds() >= TRANSFER_TIME_UP + TRANSFER_TIME_DOWN) { waitState = WaitState.IDLE; transferTimer = null; paddleTransferTimer = null; From c1b3941968609be15ac2ae0bf6be7fa4fa3dd1ee Mon Sep 17 00:00:00 2001 From: anya-codes Date: Thu, 12 Feb 2026 20:58:00 -0800 Subject: [PATCH 196/198] Tuned time constants to speed up transfer. Added code for paddle delay. --- .../ftc/team417/ComplexMechGlob.java | 85 ++++++++++++------- 1 file changed, 53 insertions(+), 32 deletions(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java b/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java index 06f55526354e..6af14f04d8a8 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java @@ -92,10 +92,10 @@ void stopLaunch() {} @Config public class ComplexMechGlob extends MechGlob { //a class encompassing all code that IS for slowbot public static double FEEDER_POWER = 1; - public static double TRANSFER_TIME_UP = 0.6; - public static double TRANSFER_TIME_DOWN = 0.25; - //TODO will need to tune the time for paddle transfer - public static double PADDLE_TRANSFER_TIME_UP = 3; // How long we wait before bringing the paddles down (we don't need time for down because they don't interfere with drum) + public static double TRANSFER_TIME_UP = 0.3; //was 0.6 + public static double TRANSFER_TIME_DOWN = 0.7; //was 0.25 + public static double PADDLE_TRANSFER_DELAY = 0.2; + public static double PADDLE_TRANSFER_TIME_UP = 0.5; // How long we wait before bringing the paddles down (we don't need time for down because they don't interfere with drum) // how long we wait before continuing after the color detector // detects. this is 0 because it will likely become obsolete @@ -110,15 +110,15 @@ public class ComplexMechGlob extends MechGlob { //a class encompassing all code public static double FLYWHEEL_VELOCITY_TOLERANCE = 22; //this is an epsiiiiiiiiilon was 10 public static double VOLTAGE_TOLERANCE = 0.04; //THIS IS AN EPSILON AS WELLLLLL public static double DRUM_GATE_OPEN_POSITION = 1; - public static double DRUM_GATE_CLOSED_POSITION = 0.7; + public static double DRUM_GATE_CLOSED_POSITION = 0.5; public static double MOTOR_D_VALUE = 1; public static double INTAKE_BACK_TIME = 0.25; public static double NEAR_AUTO_VELOCTIY = 835; public static double FAR_AUTO_VELOCITY = 1040; - public static double LEFT_PADDLE_INACTIVE_POSITION = -1; - public static double RIGHT_PADDLE_INACTIVE_POSITION = -1; - public static double LEFT_PADDLE_ACTIVE_POSITION = 0.6; - public static double RIGHT_PADDLE_ACTIVE_POSITION = 0.6; + public static double LEFT_PADDLE_INACTIVE_POSITION = 0.2; + public static double RIGHT_PADDLE_INACTIVE_POSITION = 0.2; + public static double LEFT_PADDLE_ACTIVE_POSITION = 0.8; + public static double RIGHT_PADDLE_ACTIVE_POSITION = 0.8; ElapsedTime transferTimer; ElapsedTime transferDownTimer; @@ -144,9 +144,9 @@ enum WaitState { } WaitState waitState = WaitState.IDLE; // arrays with placeholder values for servo positions and voltages relative to intake and launch - double [] INTAKE_POSITIONS = {0.067, 0.44, 0.803}; + double [] INTAKE_POSITIONS = {0.078, 0.451, 0.814}; double [] INTAKE_VOLTS = {2.94, 1.83, 0.74}; - double [] LAUNCH_POSITIONS = {0.627, 1, 0.258}; + double [] LAUNCH_POSITIONS = {0.638, 1.11, 0.269}; double [] LAUNCH_VOLTS = {1.27, 0.155, 2.37}; double lastQueuedPosition; //where the servo was *queued* to go last. NOT THE SAME AS hwDrumPosition! double hwDrumPosition; //where the drum was *told* to go last. NOT THE SAME AS lastQueuedPosition! @@ -216,8 +216,6 @@ public DrumRequest(double newPosition, double oldPosition, WaitState nextState, servoLeftPaddle = hardwareMap.get(Servo.class, "servoLeftPaddle"); coolColorDetector = new CoolColorDetector(hardwareMap, telemetry); slotOccupiedBy = new ArrayList<>(Arrays.asList(preloads)); - servoRightPaddle.setPosition(RIGHT_PADDLE_INACTIVE_POSITION); - servoLeftPaddle.setPosition(LEFT_PADDLE_INACTIVE_POSITION); /* @@ -242,6 +240,10 @@ public DrumRequest(double newPosition, double oldPosition, WaitState nextState, motLLauncher.setDirection(DcMotorSimple.Direction.REVERSE); servoRightPaddle.setDirection(Servo.Direction.REVERSE); + servoRightPaddle.setPosition(RIGHT_PADDLE_INACTIVE_POSITION); + servoLeftPaddle.setPosition(LEFT_PADDLE_INACTIVE_POSITION); + + @@ -479,15 +481,16 @@ void update () { if (waitState == WaitState.IDLE) { if (userIntakeSpeed > 0) { - int minSlot = findNearestSlot(INTAKE_POSITIONS, RequestedColor.PURPLE); - if (minSlot == -1) { - minSlot = findNearestSlot(INTAKE_POSITIONS, RequestedColor.NONE); - } - - int a = findSlotFromPosition(hwDrumPosition, INTAKE_POSITIONS); - if (a == -1) { - addToDrumQueue(INTAKE_POSITIONS[minSlot], WaitState.INTAKE); - } + addToDrumQueue(INTAKE_POSITIONS[0], WaitState.INTAKE); +// int minSlot = findNearestSlot(INTAKE_POSITIONS, RequestedColor.PURPLE); +// if (minSlot == -1) { +// minSlot = findNearestSlot(INTAKE_POSITIONS, RequestedColor.NONE); +// } +// +// int a = findSlotFromPosition(hwDrumPosition, INTAKE_POSITIONS); +// if (a == -1) { +// addToDrumQueue(INTAKE_POSITIONS[minSlot], WaitState.INTAKE); +// } // if (minSlot != -1) { // addToDrumQueue(INTAKE_POSITIONS[minSlot], WaitState.INTAKE); @@ -551,22 +554,40 @@ void update () { if (transferTimer == null) { transferTimer = new ElapsedTime(); } - if (transferTimer.seconds() >= TRANSFER_TIME_UP) { - if (paddleTransferTimer == null) { - paddleTransferTimer = new ElapsedTime(); - } - if (paddleTransferTimer.seconds() < PADDLE_TRANSFER_TIME_UP) { - leftPaddlePosition = LEFT_PADDLE_ACTIVE_POSITION; - rightPaddlePosition = RIGHT_PADDLE_ACTIVE_POSITION; - } - + if ((transferTimer.seconds() <= TRANSFER_TIME_UP)) { transferPosition = TRANSFER_ACTIVE_POSITION; } + if ((transferTimer.seconds() >= PADDLE_TRANSFER_DELAY) && (transferTimer.seconds() <= PADDLE_TRANSFER_DELAY + PADDLE_TRANSFER_TIME_UP)) { + leftPaddlePosition = LEFT_PADDLE_ACTIVE_POSITION; + rightPaddlePosition = RIGHT_PADDLE_ACTIVE_POSITION; + } if (transferTimer.seconds() >= TRANSFER_TIME_UP + TRANSFER_TIME_DOWN) { waitState = WaitState.IDLE; transferTimer = null; - paddleTransferTimer = null; } + +// if (transferTimer == null) { +// transferTimer = new ElapsedTime(); +// } +// if (transferTimer.seconds() <= TRANSFER_TIME_UP) { +// transferPosition = TRANSFER_ACTIVE_POSITION; +// } +// // only bring the paddles up when the transfer is fully up +// if (transferTimer.seconds() >= TRANSFER_TIME_UP+PADDLE_TRANSFER_DELAY) { +// transferPosition = TRANSFER_ACTIVE_POSITION; +// if (paddleTransferTimer == null) { +// paddleTransferTimer = new ElapsedTime(); +// } +// if (paddleTransferTimer.seconds() < PADDLE_TRANSFER_TIME_UP) { +// leftPaddlePosition = LEFT_PADDLE_ACTIVE_POSITION; +// rightPaddlePosition = RIGHT_PADDLE_ACTIVE_POSITION; +// } +// } +// if (transferTimer.seconds() >= TRANSFER_TIME_UP + TRANSFER_TIME_DOWN) { +// waitState = WaitState.IDLE; +// transferTimer = null; +// paddleTransferTimer = null; +// } } if (waitState == WaitState.INTAKE) { From 464db563bd93ca1132a10a6d1fdc6dc765e8767e Mon Sep 17 00:00:00 2001 From: anya-codes Date: Tue, 17 Feb 2026 21:46:35 -0800 Subject: [PATCH 197/198] Removed feeder code, added comments in update(), simplified some of the logic for transfer and scan in update() --- .../ftc/team417/ComplexMechGlob.java | 118 +++++++----------- .../firstinspires/ftc/team417/Preload.java | 4 - 2 files changed, 43 insertions(+), 79 deletions(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java b/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java index 6af14f04d8a8..d0a294f0c4e6 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java @@ -124,8 +124,6 @@ public class ComplexMechGlob extends MechGlob { //a class encompassing all code ElapsedTime transferDownTimer; ElapsedTime intakeTimer; ElapsedTime intakeBackTimer; - ElapsedTime paddleTransferTimer; - ElapsedTime scanTimer; //wait this long after drum rotates to slot before scanning double userIntakeSpeed; ArrayList drumQueue = new ArrayList<> (); @@ -167,8 +165,6 @@ enum WaitState { DcMotorEx motLLauncher; DcMotorEx motULauncher; DcMotorEx motIntake; - CRServo servoBLaunchFeeder; - CRServo servoFLaunchFeeder; Servo servoDrumGate; NormalizedColorSensor sensorColor1; NormalizedColorSensor sensorColor2; @@ -209,8 +205,6 @@ public DrumRequest(double newPosition, double oldPosition, WaitState nextState, motLLauncher = hardwareMap.get(DcMotorEx.class, "motLLauncher"); motULauncher = hardwareMap.get(DcMotorEx.class, "motULauncher"); motIntake = hardwareMap.get(DcMotorEx.class, "motIntake"); -// servoBLaunchFeeder = hardwareMap.get(CRServo.class, "servoBLaunchFeeder"); -// servoFLaunchFeeder = hardwareMap.get(CRServo.class, "servoFLaunchFeeder"); servoDrumGate = hardwareMap.get(Servo.class, "servoDrumGate"); servoRightPaddle = hardwareMap.get(Servo.class, "servoRightPaddle"); servoLeftPaddle = hardwareMap.get(Servo.class, "servoLeftPaddle"); @@ -379,8 +373,6 @@ void calculateLaunchVelocity () { else { upperLaunchVelocity = 0; lowerLaunchVelocity = 0; -// servoBLaunchFeeder.setPower(0); -// servoFLaunchFeeder.setPower(0); feederPower = 0; } } @@ -449,16 +441,16 @@ void update () { // allow the intake to run if the driver wants it to intakePower = REVERSE_INTAKE_SPEED; } else if (userIntakeSpeed > 0) { - intakePower= INTAKE_SPEED; + intakePower = INTAKE_SPEED; + } // if we are in the intake waitState, allow the intake to run // if (waitState == WaitState.INTAKE) { // intakePower = INTAKE_SPEED; // } else if (!drumQueue.isEmpty() && drumQueue.get(0).nextState == WaitState.INTAKE) { // intakePower = INTAKE_SPEED; // } - } // whenever we see a ball in the drum then we move the intake back so the ball immediately - // doesn't get stuck in the drum + // so it doesn't get stuck in the drum if (intakeBackTimer != null) { if(intakeBackTimer.seconds() < INTAKE_BACK_TIME) { intakePower = -1; @@ -468,10 +460,10 @@ void update () { } if (intakePower > 0 && waitState != WaitState.DRUM_MOVE) { - gatePosition = DRUM_GATE_OPEN_POSITION; - scanRequired = true; + gatePosition = DRUM_GATE_OPEN_POSITION; // Open gate to allow ball in + scanRequired = true; // New ball means we will need to detect color if driver presses color launch } else { - gatePosition = DRUM_GATE_CLOSED_POSITION; + gatePosition = DRUM_GATE_CLOSED_POSITION; // By default close gate to prevent balls from falling out } if (engageOhCrap) { gatePosition = DRUM_GATE_OPEN_POSITION; @@ -479,51 +471,50 @@ void update () { telemetry.addLine(String.format("intake: %.1f, waitState: %s, gatePosition: %.1f", intakePower, waitState, gatePosition)); + // TODO might be able to remove this state - and stay in current state (for intake/scan) instead of going here if (waitState == WaitState.IDLE) { + // When driver starts the intake, need to rotate to an intake slot if (userIntakeSpeed > 0) { - addToDrumQueue(INTAKE_POSITIONS[0], WaitState.INTAKE); -// int minSlot = findNearestSlot(INTAKE_POSITIONS, RequestedColor.PURPLE); -// if (minSlot == -1) { -// minSlot = findNearestSlot(INTAKE_POSITIONS, RequestedColor.NONE); -// } -// -// int a = findSlotFromPosition(hwDrumPosition, INTAKE_POSITIONS); -// if (a == -1) { -// addToDrumQueue(INTAKE_POSITIONS[minSlot], WaitState.INTAKE); -// } - -// if (minSlot != -1) { -// addToDrumQueue(INTAKE_POSITIONS[minSlot], WaitState.INTAKE); - waitState = WaitState.INTAKE; -// } + // At the beginning or after launching, all slots should be NONE. Find the nearest one. + int minSlot = findNearestSlot(INTAKE_POSITIONS, RequestedColor.NONE); + + // TODO might not need this. Test without it and delete if not needed. + if (minSlot == -1) { + minSlot = findNearestSlot(INTAKE_POSITIONS, RequestedColor.PURPLE); + } + + // Check if our current slot is an intake slot. In that case, we don't need to do anything + int currSlot = findSlotFromPosition(hwDrumPosition, INTAKE_POSITIONS); + + // If it isn't, go to the intake slot we found earlier + if (currSlot == -1 && minSlot != -1) { + addToDrumQueue(INTAKE_POSITIONS[minSlot], WaitState.INTAKE); + waitState = WaitState.INTAKE; + } } + // this makes it so that after we are done launching the drum goes to intake position // if (drumQueue.isEmpty() && slotOccupiedBy.stream().allMatch(e -> e == PixelColor.NONE)) { // addToDrumQueue(INTAKE_POSITIONS[0], WaitState.INTAKE); // } } if (waitState == WaitState.SCAN) { - if (drumAtPosition()) { - if (scanTimer == null) { - scanTimer = new ElapsedTime(); - } - if (scanTimer.seconds() >= 1) { - PixelColor slotCo = coolColorDetector.detectArtifactColor(); - telemetry.addData("slotco", slotCo); - scan(); - scanTimer = null; - waitState = WaitState.IDLE; - } - } + // Rotate to all 3 slots and then detect and store colors + scan(); + + // Then go to IDLE to wait for launch request + waitState = WaitState.IDLE; } // let a firing request interrupt an intake if (waitState == WaitState.IDLE || waitState == WaitState.INTAKE) { + // If there is a request in drumQueue find the requested position and go to DRUM_MOVE to move there if (!drumQueue.isEmpty()) { hwDrumPosition = drumQueue.get(0).newPosition; waitState = WaitState.DRUM_MOVE; } } + if (waitState == WaitState.DRUM_MOVE) { if (drumAtPosition()) { DrumRequest request = drumQueue.get(0); @@ -554,63 +545,42 @@ void update () { if (transferTimer == null) { transferTimer = new ElapsedTime(); } + // Move the 4bar transfer up if ((transferTimer.seconds() <= TRANSFER_TIME_UP)) { transferPosition = TRANSFER_ACTIVE_POSITION; } + // Wait to ensure the 4bar is fully up, then bring the paddles up if ((transferTimer.seconds() >= PADDLE_TRANSFER_DELAY) && (transferTimer.seconds() <= PADDLE_TRANSFER_DELAY + PADDLE_TRANSFER_TIME_UP)) { leftPaddlePosition = LEFT_PADDLE_ACTIVE_POSITION; rightPaddlePosition = RIGHT_PADDLE_ACTIVE_POSITION; } + // Everything goes down and we go to IDLE if (transferTimer.seconds() >= TRANSFER_TIME_UP + TRANSFER_TIME_DOWN) { waitState = WaitState.IDLE; transferTimer = null; } - -// if (transferTimer == null) { -// transferTimer = new ElapsedTime(); -// } -// if (transferTimer.seconds() <= TRANSFER_TIME_UP) { -// transferPosition = TRANSFER_ACTIVE_POSITION; -// } -// // only bring the paddles up when the transfer is fully up -// if (transferTimer.seconds() >= TRANSFER_TIME_UP+PADDLE_TRANSFER_DELAY) { -// transferPosition = TRANSFER_ACTIVE_POSITION; -// if (paddleTransferTimer == null) { -// paddleTransferTimer = new ElapsedTime(); -// } -// if (paddleTransferTimer.seconds() < PADDLE_TRANSFER_TIME_UP) { -// leftPaddlePosition = LEFT_PADDLE_ACTIVE_POSITION; -// rightPaddlePosition = RIGHT_PADDLE_ACTIVE_POSITION; -// } -// } -// if (transferTimer.seconds() >= TRANSFER_TIME_UP + TRANSFER_TIME_DOWN) { -// waitState = WaitState.IDLE; -// transferTimer = null; -// paddleTransferTimer = null; -// } } if (waitState == WaitState.INTAKE) { launchDistance = LaunchDistance.OFF; - // Because slot colors don't matter right now - // so we assume all 3 slots are purple + // By default we assume all 3 slots are purple + // We will check the color later if the driver needs to launch a color slotOccupiedBy.set(0, PixelColor.PURPLE); slotOccupiedBy.set(1, PixelColor.PURPLE); slotOccupiedBy.set(2, PixelColor.PURPLE); - waitState = WaitState.IDLE; } + + // Manually overrides an ongoing launch if (waitState == WaitState.TRANSFER_DOWN) { if (transferDownTimer == null) { transferDownTimer = new ElapsedTime(); - } - while (transferDownTimer.seconds() <= TRANSFER_TIME_DOWN) { + + // Mark slot as empty because we stopped the transfer int currSlot = findSlotFromPosition(hwDrumPosition, LAUNCH_POSITIONS); slotOccupiedBy.set(currSlot, PixelColor.NONE); - transferPosition = TRANSFER_INACTIVE_POSITION; - leftPaddlePosition = LEFT_PADDLE_INACTIVE_POSITION; - rightPaddlePosition = RIGHT_PADDLE_INACTIVE_POSITION; } - if (transferTimer.seconds() > TRANSFER_TIME_DOWN) { + // If transfer is fully down, go to IDLE to wait for next action + if (transferDownTimer.seconds() > TRANSFER_TIME_DOWN) { waitState = WaitState.IDLE; transferTimer = null; transferDownTimer = null; @@ -632,8 +602,6 @@ void update () { motLLauncher.setVelocity(lowerLaunchVelocity); motULauncher.setVelocity(upperLaunchVelocity); motIntake.setPower(intakePower); -// servoBLaunchFeeder.setPower(feederPower); -// servoFLaunchFeeder.setPower(feederPower); telemetry.addData("hwDrumPos", hwDrumPosition); telemetry.addData("currVoltage ", "%.2f", analogDrum.getVoltage()); } diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/Preload.java b/team417/src/main/java/org/firstinspires/ftc/team417/Preload.java index abc25f5f3dd0..074f7dc149b0 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/Preload.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/Preload.java @@ -65,10 +65,8 @@ public void runOpMode() throws InterruptedException { // Only allow intaking/out-taking when not moving the drum: if (gamepad1.a) { // Intake a ball: - feederPower = -ComplexMechGlob.FEEDER_POWER; } else if (gamepad1.b) { // Eject a ball (without the flywheel!): - feederPower = ComplexMechGlob.FEEDER_POWER; transferPosition = ComplexMechGlob.TRANSFER_ACTIVE_POSITION; timeSinceTransfer = new ElapsedTime(); // Start the transfer timer } @@ -78,8 +76,6 @@ public void runOpMode() throws InterruptedException { loopTime = new ElapsedTime(); // Feed the servos: - mechGlob.servoBLaunchFeeder.setPower(feederPower); - mechGlob.servoFLaunchFeeder.setPower(feederPower); mechGlob.servoTransfer.setPosition(transferPosition); mechGlob.servoDrum.setPosition(currentDrumPosition); From b92dc4c6bcd56ca68ecdb934af4f77a9218020b9 Mon Sep 17 00:00:00 2001 From: anya-codes Date: Thu, 5 Mar 2026 21:48:48 -0800 Subject: [PATCH 198/198] Added a wait between the drum movement to launch position and the transfer going up. Tuned the drum gate positions. Fixed an issue with the intake default to purple. Changed the hue values for purple and green to more accurately detect colors --- .../ftc/team417/ComplexMechGlob.java | 17 +++++++++++------ .../ftc/team417/CoolColorDetector.java | 4 ++-- 2 files changed, 13 insertions(+), 8 deletions(-) diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java b/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java index d0a294f0c4e6..a3b61afd6d4a 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/ComplexMechGlob.java @@ -97,6 +97,8 @@ public class ComplexMechGlob extends MechGlob { //a class encompassing all code public static double PADDLE_TRANSFER_DELAY = 0.2; public static double PADDLE_TRANSFER_TIME_UP = 0.5; // How long we wait before bringing the paddles down (we don't need time for down because they don't interfere with drum) + //todo can decrease this + public static double DRUM_TRANSFER_WAIT_TIME = 0.5; // how long we wait before continuing after the color detector // detects. this is 0 because it will likely become obsolete public static double INTAKE_TIMER = 0; @@ -109,8 +111,8 @@ public class ComplexMechGlob extends MechGlob { //a class encompassing all code public static double INTAKE_SPEED = 1; public static double FLYWHEEL_VELOCITY_TOLERANCE = 22; //this is an epsiiiiiiiiilon was 10 public static double VOLTAGE_TOLERANCE = 0.04; //THIS IS AN EPSILON AS WELLLLLL - public static double DRUM_GATE_OPEN_POSITION = 1; - public static double DRUM_GATE_CLOSED_POSITION = 0.5; + public static double DRUM_GATE_OPEN_POSITION = 0.5; + public static double DRUM_GATE_CLOSED_POSITION = 0.75; public static double MOTOR_D_VALUE = 1; public static double INTAKE_BACK_TIME = 0.25; public static double NEAR_AUTO_VELOCTIY = 835; @@ -489,8 +491,9 @@ void update () { // If it isn't, go to the intake slot we found earlier if (currSlot == -1 && minSlot != -1) { addToDrumQueue(INTAKE_POSITIONS[minSlot], WaitState.INTAKE); - waitState = WaitState.INTAKE; } + + waitState = WaitState.INTAKE; } // this makes it so that after we are done launching the drum goes to intake position @@ -545,17 +548,18 @@ void update () { if (transferTimer == null) { transferTimer = new ElapsedTime(); } + // Move the 4bar transfer up - if ((transferTimer.seconds() <= TRANSFER_TIME_UP)) { + if ((transferTimer.seconds() <= TRANSFER_TIME_UP+DRUM_TRANSFER_WAIT_TIME) && (transferTimer.seconds() >= DRUM_TRANSFER_WAIT_TIME)) { transferPosition = TRANSFER_ACTIVE_POSITION; } // Wait to ensure the 4bar is fully up, then bring the paddles up - if ((transferTimer.seconds() >= PADDLE_TRANSFER_DELAY) && (transferTimer.seconds() <= PADDLE_TRANSFER_DELAY + PADDLE_TRANSFER_TIME_UP)) { + if ((transferTimer.seconds() >= PADDLE_TRANSFER_DELAY+DRUM_TRANSFER_WAIT_TIME) && (transferTimer.seconds() <= PADDLE_TRANSFER_DELAY + PADDLE_TRANSFER_TIME_UP + DRUM_TRANSFER_WAIT_TIME)) { leftPaddlePosition = LEFT_PADDLE_ACTIVE_POSITION; rightPaddlePosition = RIGHT_PADDLE_ACTIVE_POSITION; } // Everything goes down and we go to IDLE - if (transferTimer.seconds() >= TRANSFER_TIME_UP + TRANSFER_TIME_DOWN) { + if (transferTimer.seconds() >= TRANSFER_TIME_UP + TRANSFER_TIME_DOWN+DRUM_TRANSFER_WAIT_TIME) { waitState = WaitState.IDLE; transferTimer = null; } @@ -603,6 +607,7 @@ void update () { motULauncher.setVelocity(upperLaunchVelocity); motIntake.setPower(intakePower); telemetry.addData("hwDrumPos", hwDrumPosition); + telemetry.addData("transferpos", transferPosition); telemetry.addData("currVoltage ", "%.2f", analogDrum.getVoltage()); } } diff --git a/team417/src/main/java/org/firstinspires/ftc/team417/CoolColorDetector.java b/team417/src/main/java/org/firstinspires/ftc/team417/CoolColorDetector.java index 0ba114cc6a39..43e04a603e2b 100644 --- a/team417/src/main/java/org/firstinspires/ftc/team417/CoolColorDetector.java +++ b/team417/src/main/java/org/firstinspires/ftc/team417/CoolColorDetector.java @@ -18,10 +18,10 @@ public class CoolColorDetector { public static double MAXIMUM_DISTANCE = 30; public static double MINIMUM_DISTANCE = 21; - public static double PURPLE_MIN_HUE = 200; + public static double PURPLE_MIN_HUE = 190; public static double PURPLE_MAX_HUE = 320; //increased from 235 to loosen requirement public static double GREEN_MIN_HUE = 100; //decreased from 150 to loosen requirement - public static double GREEN_MAX_HUE = 180; + public static double GREEN_MAX_HUE = 170; public static double MIN_VALUE = 0.25; public static float GAIN = 85f; // adjust for brightness