From c0b06f91fc44ed88156bdf8041e801d23483e4e6 Mon Sep 17 00:00:00 2001 From: anya-codes Date: Sun, 21 Sep 2025 14:48:28 -0700 Subject: [PATCH 001/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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/191] 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); }