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()
+
+ /**
+ * Set the active camera according to input from the gamepad.
+ */
+ private void doCameraSwitching() {
+ if (visionPortal.getCameraState() == CameraState.STREAMING) {
+ // If the left bumper is pressed, use Webcam 1.
+ // If the right bumper is pressed, use Webcam 2.
+ boolean newLeftBumper = gamepad1.left_bumper;
+ boolean newRightBumper = gamepad1.right_bumper;
+ if (newLeftBumper && !oldLeftBumper) {
+ visionPortal.setActiveCamera(webcam1);
+ } else if (newRightBumper && !oldRightBumper) {
+ visionPortal.setActiveCamera(webcam2);
+ }
+ oldLeftBumper = newLeftBumper;
+ oldRightBumper = newRightBumper;
+ }
+
+ } // end method doCameraSwitching()
+
+} // end class
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptBlackboard.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptBlackboard.java
new file mode 100644
index 0000000..2c93abb
--- /dev/null
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptBlackboard.java
@@ -0,0 +1,87 @@
+/* Copyright (c) 2025 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.robotcontroller.external.samples;
+
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+/*
+ * Demonstrates how to use an OpMode to store data in the blackboard and retrieve it.
+ * This is useful for storing information in Auto and then retrieving it in your TeleOp.
+ *
+ * Be aware that this is NOT saved across power reboots or downloads or robot resets.
+ *
+ * You also need to make sure that both the storing and retrieving are done using the same
+ * type. For example, storing a Double in the blackboard and retrieving it as an Integer
+ * will fail when you typecast it.
+ *
+ * The blackboard is a standard hashmap so you can use methods like:
+ * put, get, containsKey, remove, etc.
+ */
+@TeleOp(name = "Concept: Blackboard", group = "Concept")
+@Disabled
+public class ConceptBlackboard extends OpMode {
+ // We use constants here so we won't have the problem of typos in different places when we
+ // meant to refer to the same thing.
+ public static final String TIMES_STARTED_KEY = "Times started";
+ public static final String ALLIANCE_KEY = "Alliance";
+
+ /**
+ * This method will be called once, when the INIT button is pressed.
+ */
+ @Override
+ public void init() {
+ // This gets us what is in the blackboard or the default if it isn't in there.
+ Object timesStarted = blackboard.getOrDefault(TIMES_STARTED_KEY, 0);
+ blackboard.put(TIMES_STARTED_KEY, (int) timesStarted + 1);
+
+ telemetry.addData("OpMode started times", blackboard.get(TIMES_STARTED_KEY));
+ }
+
+ /**
+ * This method will be called repeatedly during the period between when
+ * the START button is pressed and when the OpMode is stopped.
+ *
+ * If the left bumper is pressed it will store the value "RED" in the blackboard for alliance.
+ * If the right bumper is pressed it will store the value "BLUE" in the blackboard for alliance.
+ *
+ * You'll notice that if you quit the OpMode and come back in, the value will persist.
+ */
+ @Override
+ public void loop() {
+ if (gamepad1.left_bumper) {
+ blackboard.put(ALLIANCE_KEY, "RED");
+ } else if (gamepad1.right_bumper) {
+ blackboard.put(ALLIANCE_KEY, "BLUE");
+ }
+ telemetry.addData("Alliance", blackboard.get(ALLIANCE_KEY));
+ }
+}
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptExploringIMUOrientation.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptExploringIMUOrientation.java
new file mode 100644
index 0000000..751d54f
--- /dev/null
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptExploringIMUOrientation.java
@@ -0,0 +1,184 @@
+/*
+Copyright (c) 2022 REV Robotics, 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 REV Robotics 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.robotcontroller.external.samples;
+
+import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
+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.IMU;
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+import org.firstinspires.ftc.robotcore.external.navigation.AngularVelocity;
+import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles;
+
+/*
+ * This OpMode demonstrates the impact of setting the IMU orientation correctly or incorrectly. This
+ * code assumes there is an IMU configured with the name "imu".
+ *
+ * Note: This OpMode is more of a tool than a code sample. The User Interface portion of this code
+ * goes beyond simply showing how to interface to the IMU.
+ * For a minimal example of interfacing to an IMU, please see the SensorIMUOrthogonal or SensorIMUNonOrthogonal sample OpModes.
+ *
+ * This OpMode enables you to re-specify the Hub Mounting orientation dynamically by using gamepad controls.
+ * While doing so, the sample will display how Pitch, Roll and Yaw angles change as the hub is moved.
+ *
+ * The gamepad controls let you change the two parameters that specify how the Control/Expansion Hub is mounted.
+ * The first parameter specifies which direction the printed logo on the Hub is pointing.
+ * The second parameter specifies which direction the USB connector on the Hub is pointing.
+ * All directions are relative to the robot, and left/right is as viewed from behind the robot.
+ *
+ * How will you know if you have chosen the correct Orientation? With the correct orientation
+ * parameters selected, pitch/roll/yaw should act as follows:
+ *
+ * Pitch value should INCREASE as the robot is tipped UP at the front. (Rotation about X)
+ * Roll value should INCREASE as the robot is tipped UP at the left side. (Rotation about Y)
+ * Yaw value should INCREASE as the robot is rotated Counter Clockwise. (Rotation about Z)
+ *
+ * The Yaw can be reset (to zero) by pressing the Y button on the gamepad (Triangle on a PS4 controller)
+ *
+ * The rotational velocities should follow the change in corresponding axes.
+ */
+
+@TeleOp(name="Concept: IMU Orientation", group="Concept")
+@Disabled
+public class ConceptExploringIMUOrientation extends LinearOpMode {
+ static RevHubOrientationOnRobot.LogoFacingDirection[] logoFacingDirections
+ = RevHubOrientationOnRobot.LogoFacingDirection.values();
+ static RevHubOrientationOnRobot.UsbFacingDirection[] usbFacingDirections
+ = RevHubOrientationOnRobot.UsbFacingDirection.values();
+ static int LAST_DIRECTION = logoFacingDirections.length - 1;
+ static float TRIGGER_THRESHOLD = 0.2f;
+
+ IMU imu;
+ int logoFacingDirectionPosition;
+ int usbFacingDirectionPosition;
+ boolean orientationIsValid = true;
+
+ @Override public void runOpMode() throws InterruptedException {
+ imu = hardwareMap.get(IMU.class, "imu");
+ logoFacingDirectionPosition = 0; // Up
+ usbFacingDirectionPosition = 2; // Forward
+
+ updateOrientation();
+
+ boolean justChangedLogoDirection = false;
+ boolean justChangedUsbDirection = false;
+
+ // Loop until stop requested
+ while (!isStopRequested()) {
+
+ // Check to see if Yaw reset is requested (Y button)
+ if (gamepad1.y) {
+ telemetry.addData("Yaw", "Resetting\n");
+ imu.resetYaw();
+ } else {
+ telemetry.addData("Yaw", "Press Y (triangle) on Gamepad to reset.\n");
+ }
+
+ // Check to see if new Logo Direction is requested
+ if (gamepad1.left_bumper || gamepad1.right_bumper) {
+ if (!justChangedLogoDirection) {
+ justChangedLogoDirection = true;
+ if (gamepad1.left_bumper) {
+ logoFacingDirectionPosition--;
+ if (logoFacingDirectionPosition < 0) {
+ logoFacingDirectionPosition = LAST_DIRECTION;
+ }
+ } else {
+ logoFacingDirectionPosition++;
+ if (logoFacingDirectionPosition > LAST_DIRECTION) {
+ logoFacingDirectionPosition = 0;
+ }
+ }
+ updateOrientation();
+ }
+ } else {
+ justChangedLogoDirection = false;
+ }
+
+ // Check to see if new USB Direction is requested
+ if (gamepad1.left_trigger > TRIGGER_THRESHOLD || gamepad1.right_trigger > TRIGGER_THRESHOLD) {
+ if (!justChangedUsbDirection) {
+ justChangedUsbDirection = true;
+ if (gamepad1.left_trigger > TRIGGER_THRESHOLD) {
+ usbFacingDirectionPosition--;
+ if (usbFacingDirectionPosition < 0) {
+ usbFacingDirectionPosition = LAST_DIRECTION;
+ }
+ } else {
+ usbFacingDirectionPosition++;
+ if (usbFacingDirectionPosition > LAST_DIRECTION) {
+ usbFacingDirectionPosition = 0;
+ }
+ }
+ updateOrientation();
+ }
+ } else {
+ justChangedUsbDirection = false;
+ }
+
+ // Display User instructions and IMU data
+ telemetry.addData("logo Direction (set with bumpers)", logoFacingDirections[logoFacingDirectionPosition]);
+ telemetry.addData("usb Direction (set with triggers)", usbFacingDirections[usbFacingDirectionPosition] + "\n");
+
+ if (orientationIsValid) {
+ YawPitchRollAngles orientation = imu.getRobotYawPitchRollAngles();
+ AngularVelocity angularVelocity = imu.getRobotAngularVelocity(AngleUnit.DEGREES);
+
+ telemetry.addData("Yaw (Z)", "%.2f Deg. (Heading)", orientation.getYaw(AngleUnit.DEGREES));
+ telemetry.addData("Pitch (X)", "%.2f Deg.", orientation.getPitch(AngleUnit.DEGREES));
+ telemetry.addData("Roll (Y)", "%.2f Deg.\n", orientation.getRoll(AngleUnit.DEGREES));
+ telemetry.addData("Yaw (Z) velocity", "%.2f Deg/Sec", angularVelocity.zRotationRate);
+ telemetry.addData("Pitch (X) velocity", "%.2f Deg/Sec", angularVelocity.xRotationRate);
+ telemetry.addData("Roll (Y) velocity", "%.2f Deg/Sec", angularVelocity.yRotationRate);
+ } else {
+ telemetry.addData("Error", "Selected orientation on robot is invalid");
+ }
+
+ telemetry.update();
+ }
+ }
+
+ // apply any requested orientation changes.
+ void updateOrientation() {
+ RevHubOrientationOnRobot.LogoFacingDirection logo = logoFacingDirections[logoFacingDirectionPosition];
+ RevHubOrientationOnRobot.UsbFacingDirection usb = usbFacingDirections[usbFacingDirectionPosition];
+ try {
+ RevHubOrientationOnRobot orientationOnRobot = new RevHubOrientationOnRobot(logo, usb);
+ imu.initialize(new IMU.Parameters(orientationOnRobot));
+ orientationIsValid = true;
+ } catch (IllegalArgumentException e) {
+ orientationIsValid = false;
+ }
+ }
+}
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptGamepadEdgeDetection.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptGamepadEdgeDetection.java
new file mode 100644
index 0000000..90243ac
--- /dev/null
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptGamepadEdgeDetection.java
@@ -0,0 +1,92 @@
+/*
+Copyright (c) 2024 Miriam Sinton-Remes
+
+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 FITNESSFOR 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.robotcontroller.external.samples;
+
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+/*
+ * This OpMode illustrates using edge detection on a gamepad.
+ *
+ * Simply checking the state of a gamepad button each time could result in triggering an effect
+ * multiple times. Edge detection ensures that you only detect one button press, regardless of how
+ * long the button is held.
+ *
+ * There are two main types of edge detection. Rising edge detection will trigger when a button is
+ * first pressed. Falling edge detection will trigger when the button is released.
+ *
+ * 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: Gamepad Edge Detection", group ="Concept")
+public class ConceptGamepadEdgeDetection extends LinearOpMode {
+
+ @Override
+ public void runOpMode() {
+ // Wait for the DS start button to be pressed
+ waitForStart();
+
+ while (opModeIsActive()) {
+ // Update the telemetry
+ telemetryButtonData();
+
+ // Wait 2 seconds before doing another check
+ sleep(2000);
+ }
+ }
+
+ public void telemetryButtonData() {
+ // Add the status of the Gamepad 1 Left Bumper
+ telemetry.addData("Gamepad 1 Left Bumper Pressed", gamepad1.leftBumperWasPressed());
+ telemetry.addData("Gamepad 1 Left Bumper Released", gamepad1.leftBumperWasReleased());
+ telemetry.addData("Gamepad 1 Left Bumper Status", gamepad1.left_bumper);
+
+ // Add an empty line to separate the buttons in telemetry
+ telemetry.addLine();
+
+ // Add the status of the Gamepad 1 Right Bumper
+ telemetry.addData("Gamepad 1 Right Bumper Pressed", gamepad1.rightBumperWasPressed());
+ telemetry.addData("Gamepad 1 Right Bumper Released", gamepad1.rightBumperWasReleased());
+ telemetry.addData("Gamepad 1 Right Bumper Status", gamepad1.right_bumper);
+
+ // Add a note that the telemetry is only updated every 2 seconds
+ telemetry.addLine("\nTelemetry is updated every 2 seconds.");
+
+ // Update the telemetry on the DS screen
+ telemetry.update();
+ }
+}
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptGamepadRumble.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptGamepadRumble.java
new file mode 100644
index 0000000..cf846e1
--- /dev/null
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptGamepadRumble.java
@@ -0,0 +1,200 @@
+package org.firstinspires.ftc.robotcontroller.external.samples;
+
+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.Gamepad;
+import com.qualcomm.robotcore.util.ElapsedTime;
+
+/*
+ * This OpMode illustrates using the rumble feature of many gamepads.
+ *
+ * Note: Some gamepads "rumble" better than others.
+ * The Xbox & PS4 have a left (rumble1) and right (rumble2) rumble motor.
+ * These two gamepads have two distinct rumble modes: Large on the left, and small on the right
+ * The Etpark gamepad may only respond to rumble1, and may only run at full power.
+ * The Logitech F310 gamepad does not have *any* rumble ability.
+ *
+ * Moral: You should use this sample to experiment with your specific gamepads to explore their rumble features.
+ *
+ * The rumble motors are accessed through the standard gamepad1 and gamepad2 objects.
+ * Several new methods were added to the Gamepad class in FTC SDK Rev 7
+ * The key methods are as follows:
+ *
+ * .rumble(double rumble1, double rumble2, int durationMs)
+ * This method sets the rumble power of both motors for a specific time duration.
+ * Both rumble arguments are motor-power levels in the 0.0 to 1.0 range.
+ * durationMs is the desired length of the rumble action in milliseconds.
+ * This method returns immediately.
+ * Note:
+ * Use a durationMs of Gamepad.RUMBLE_DURATION_CONTINUOUS to provide a continuous rumble
+ * Use a power of 0, or duration of 0 to stop a rumble.
+ *
+ * .rumbleBlips(int count) allows an easy way to signal the driver with a series of rumble blips.
+ * Just specify how many blips you want.
+ * This method returns immediately.
+ *
+ * .runRumbleEffect(customRumbleEffect) allows you to run a custom rumble sequence that you have
+ * built using the Gamepad.RumbleEffect.Builder()
+ * A "custom effect" is a sequence of steps, where each step can rumble any of the
+ * rumble motors for a specific period at a specific power level.
+ * The Custom Effect will play as the (un-blocked) OpMode continues to run
+ *
+ * .isRumbling() returns true if there is a rumble effect in progress.
+ * Use this call to prevent stepping on a current rumble.
+ *
+ * .stopRumble() Stop any ongoing rumble or custom rumble effect.
+ *
+ * .rumble(int durationMs) Full power rumble for fixed duration.
+ *
+ * Note: Whenever a new Rumble command is issued, any currently executing rumble action will
+ * be truncated, and the new action started immediately. Take these precautions:
+ * 1) Do Not SPAM the rumble motors by issuing rapid fire commands
+ * 2) Multiple sources for rumble commands must coordinate to avoid tromping on each other.
+ *
+ * This can be achieved several possible ways:
+ * 1) Only having one source for rumble actions
+ * 2) Issuing rumble commands on transitions, rather than states.
+ * e.g. The moment a touch sensor is pressed, rather than the entire time it is being pressed.
+ * 3) Scheduling rumble commands based on timed events. e.g. 10 seconds prior to endgame
+ * 4) Rumble on non-overlapping mechanical actions. e.g. arm fully-extended or fully-retracted.
+ * 5) Use isRumbling() to hold off on a new rumble if one is already in progress.
+ *
+ * The examples shown here are representstive of how to invoke a gamepad rumble.
+ * It is assumed that these will be modified to suit the specific robot and team strategy needs.
+ *
+ * ######## Read the telemetry display on the Driver Station Screen for instructions. ######
+ *
+ * Ex 1) This example shows a) how to create a custom rumble effect, and then b) how to trigger it based
+ * on game time. One use for this might be to alert the driver that half-time or End-game is approaching.
+ *
+ * Ex 2) This example shows tying the rumble power to a changing sensor value.
+ * In this case it is the Gamepad trigger, but it could be any sensor output scaled to the 0-1 range.
+ * Since it takes over the rumble motors, it is only performed when the Left Bumper is pressed.
+ * Note that this approach MUST include a way to turn OFF the rumble when the button is released.
+ *
+ * Ex 3) This example shows a simple way to trigger a 3-blip sequence. In this case it is
+ * triggered by the gamepad A (Cross) button, but it could be any sensor, like a touch or light sensor.
+ * Note that this code ensures that it only rumbles once when the input goes true.
+ *
+ * Ex 4) This example shows how to trigger a single rumble when an input value gets over a certain value.
+ * In this case it is reading the Right Trigger, but it could be any variable sensor, like a
+ * range sensor, or position sensor. The code needs to ensure that it is only triggered once, so
+ * it waits till the sensor drops back below the threshold before it can trigger again.
+ *
+ * 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: Gamepad Rumble", group ="Concept")
+public class ConceptGamepadRumble extends LinearOpMode
+{
+ boolean lastA = false; // Use to track the prior button state.
+ boolean lastLB = false; // Use to track the prior button state.
+ boolean highLevel = false; // used to prevent multiple level-based rumbles.
+ boolean secondHalf = false; // Use to prevent multiple half-time warning rumbles.
+
+ Gamepad.RumbleEffect customRumbleEffect; // Use to build a custom rumble sequence.
+ ElapsedTime runtime = new ElapsedTime(); // Use to determine when end game is starting.
+
+ final double HALF_TIME = 60.0; // Wait this many seconds before rumble-alert for half-time.
+ final double TRIGGER_THRESHOLD = 0.75; // Squeeze more than 3/4 to get rumble.
+
+ @Override
+ public void runOpMode()
+ {
+ // Example 1. a) start by creating a three-pulse rumble sequence: right, LEFT, LEFT
+ customRumbleEffect = new Gamepad.RumbleEffect.Builder()
+ .addStep(0.0, 1.0, 500) // Rumble right motor 100% for 500 mSec
+ .addStep(0.0, 0.0, 300) // Pause for 300 mSec
+ .addStep(1.0, 0.0, 250) // Rumble left motor 100% for 250 mSec
+ .addStep(0.0, 0.0, 250) // Pause for 250 mSec
+ .addStep(1.0, 0.0, 250) // Rumble left motor 100% for 250 mSec
+ .build();
+
+ telemetry.addData(">", "Press Start");
+ telemetry.update();
+
+ waitForStart();
+ runtime.reset(); // Start game timer.
+
+ // Loop while monitoring buttons for rumble triggers
+ while (opModeIsActive())
+ {
+ // Read and save the current gamepad button states.
+ boolean currentA = gamepad1.a ;
+ boolean currentLB = gamepad1.left_bumper ;
+
+ // Display the current Rumble status. Just for interest.
+ telemetry.addData(">", "Are we RUMBLING? %s\n", gamepad1.isRumbling() ? "YES" : "no" );
+
+ // ----------------------------------------------------------------------------------------
+ // Example 1. b) Watch the runtime timer, and run the custom rumble when we hit half-time.
+ // Make sure we only signal once by setting "secondHalf" flag to prevent further rumbles.
+ // ----------------------------------------------------------------------------------------
+ if ((runtime.seconds() > HALF_TIME) && !secondHalf) {
+ gamepad1.runRumbleEffect(customRumbleEffect);
+ secondHalf =true;
+ }
+
+ // Display the time remaining while we are still counting down.
+ if (!secondHalf) {
+ telemetry.addData(">", "Halftime Alert Countdown: %3.0f Sec \n", (HALF_TIME - runtime.seconds()) );
+ }
+
+
+ // ----------------------------------------------------------------------------------------
+ // Example 2. If Left Bumper is being pressed, power the rumble motors based on the two trigger depressions.
+ // This is useful to see how the rumble feels at various power levels.
+ // ----------------------------------------------------------------------------------------
+ if (currentLB) {
+ // Left Bumper is being pressed, so send left and right "trigger" values to left and right rumble motors.
+ gamepad1.rumble(gamepad1.left_trigger, gamepad1.right_trigger, Gamepad.RUMBLE_DURATION_CONTINUOUS);
+
+ // Show what is being sent to rumbles
+ telemetry.addData(">", "Squeeze triggers to control rumbles");
+ telemetry.addData("> : Rumble", "Left: %.0f%% Right: %.0f%%", gamepad1.left_trigger * 100, gamepad1.right_trigger * 100);
+ } else {
+ // Make sure rumble is turned off when Left Bumper is released (only one time each press)
+ if (lastLB) {
+ gamepad1.stopRumble();
+ }
+
+ // Prompt for manual rumble action
+ telemetry.addData(">", "Hold Left-Bumper to test Manual Rumble");
+ telemetry.addData(">", "Press A (Cross) for three blips");
+ telemetry.addData(">", "Squeeze right trigger slowly for 1 blip");
+ }
+ lastLB = currentLB; // remember the current button state for next time around the loop
+
+
+ // ----------------------------------------------------------------------------------------
+ // Example 3. Blip 3 times at the moment that A (Cross) is pressed. (look for pressed transition)
+ // BUT !!! Skip it altogether if the Gamepad is already rumbling.
+ // ----------------------------------------------------------------------------------------
+ if (currentA && !lastA) {
+ if (!gamepad1.isRumbling()) // Check for possible overlap of rumbles.
+ gamepad1.rumbleBlips(3);
+ }
+ lastA = currentA; // remember the current button state for next time around the loop
+
+
+ // ----------------------------------------------------------------------------------------
+ // Example 4. Rumble once when gamepad right trigger goes above the THRESHOLD.
+ // ----------------------------------------------------------------------------------------
+ if (gamepad1.right_trigger > TRIGGER_THRESHOLD) {
+ if (!highLevel) {
+ gamepad1.rumble(0.9, 0, 200); // 200 mSec burst on left motor.
+ highLevel = true; // Hold off any more triggers
+ }
+ } else {
+ highLevel = false; // We can trigger again now.
+ }
+
+ // Send the telemetry data to the Driver Station, and then pause to pace the program.
+ telemetry.update();
+ sleep(10);
+ }
+ }
+}
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptGamepadTouchpad.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptGamepadTouchpad.java
new file mode 100644
index 0000000..84d8cec
--- /dev/null
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptGamepadTouchpad.java
@@ -0,0 +1,77 @@
+package org.firstinspires.ftc.robotcontroller.external.samples;
+
+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.Telemetry;
+
+/*
+ * This OpMode illustrates using the touchpad feature found on some gamepads.
+ *
+ * The Sony PS4 gamepad can detect two distinct touches on the central touchpad.
+ * Other gamepads with different touchpads may provide mixed results.
+ *
+ * The touchpads are accessed through the standard gamepad1 and gamepad2 objects.
+ * Several new members were added to the Gamepad class in FTC SDK Rev 7
+ *
+ * .touchpad_finger_1 returns true if at least one finger is detected.
+ * .touchpad_finger_1_x finger 1 X coordinate. Valid if touchpad_finger_1 is true
+ * .touchpad_finger_1_y finger 1 Y coordinate. Valid if touchpad_finger_1 is true
+ *
+ * .touchpad_finger_2 returns true if a second finger is detected
+ * .touchpad_finger_2_x finger 2 X coordinate. Valid if touchpad_finger_2 is true
+ * .touchpad_finger_2_y finger 2 Y coordinate. Valid if touchpad_finger_2 is true
+ *
+ * Finger touches are reported with an X and Y coordinate in following coordinate system.
+ *
+ * 1) X is the Horizontal axis, and Y is the vertical axis
+ * 2) The 0,0 origin is at the center of the touchpad.
+ * 3) 1.0, 1.0 is at the top right corner of the touchpad.
+ * 4) -1.0,-1.0 is at the bottom left corner of the touchpad.
+ *
+ * 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: Gamepad Touchpad", group ="Concept")
+public class ConceptGamepadTouchpad extends LinearOpMode
+{
+ @Override
+ public void runOpMode()
+ {
+ telemetry.setDisplayFormat(Telemetry.DisplayFormat.MONOSPACE);
+
+ telemetry.addData(">", "Press Start");
+ telemetry.update();
+
+ waitForStart();
+
+ while (opModeIsActive())
+ {
+ boolean finger = false;
+
+ // Display finger 1 x & y position if finger detected
+ if(gamepad1.touchpad_finger_1)
+ {
+ finger = true;
+ telemetry.addLine(String.format("Finger 1: x=%5.2f y=%5.2f\n", gamepad1.touchpad_finger_1_x, gamepad1.touchpad_finger_1_y));
+ }
+
+ // Display finger 2 x & y position if finger detected
+ if(gamepad1.touchpad_finger_2)
+ {
+ finger = true;
+ telemetry.addLine(String.format("Finger 2: x=%5.2f y=%5.2f\n", gamepad1.touchpad_finger_2_x, gamepad1.touchpad_finger_2_y));
+ }
+
+ if(!finger)
+ {
+ telemetry.addLine("No fingers");
+ }
+
+ telemetry.update();
+ sleep(10);
+ }
+ }
+}
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptLEDStick.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptLEDStick.java
new file mode 100644
index 0000000..01729bb
--- /dev/null
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptLEDStick.java
@@ -0,0 +1,123 @@
+package org.firstinspires.ftc.robotcontroller.external.samples;
+/*
+ Copyright (c) 2021-24 Alan Smith
+
+ 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 Alan Smith 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 FITNESSFOR 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 android.graphics.Color;
+
+import com.qualcomm.hardware.sparkfun.SparkFunLEDStick;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.util.Range;
+
+/*
+ * This OpMode illustrates how to use the SparkFun QWIIC LED Strip
+ *
+ * This is a simple way to add a strip of 10 LEDs to your robot where you can set the color of each
+ * LED or the whole strip. This allows for driver feedback or even just fun ways to show your team
+ * colors.
+ *
+ * Why?
+ * Because more LEDs == more fun!!
+ *
+ * This OpMode assumes that the QWIIC LED Stick is attached to an I2C interface named "back_leds" in the robot configuration.
+ *
+ * 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
+ *
+ * You can buy this product here: https://www.sparkfun.com/products/18354
+ * Don't forget to also buy this to make it easy to connect to your Control or Expansion Hub:
+ * https://www.sparkfun.com/products/25596
+ */
+@TeleOp(name = "Concept: LED Stick", group = "Concept")
+@Disabled
+public class ConceptLEDStick extends OpMode {
+ private boolean wasUp;
+ private boolean wasDown;
+ private int brightness = 5; // this needs to be between 0 and 31
+ private final static double END_GAME_TIME = 120 - 30;
+
+ private SparkFunLEDStick ledStick;
+
+ @Override
+ public void init() {
+ ledStick = hardwareMap.get(SparkFunLEDStick.class, "back_leds");
+ ledStick.setBrightness(brightness);
+ ledStick.setColor(Color.GREEN);
+ }
+
+ @Override
+ public void start() {
+ resetRuntime();
+ }
+
+ @Override
+ public void loop() {
+ telemetry.addLine("Hold the A button to turn blue");
+ telemetry.addLine("Hold the B button to turn red");
+ telemetry.addLine("Hold the left bumper to turn off");
+ telemetry.addLine("Use DPAD Up/Down to change brightness");
+
+ if (getRuntime() > END_GAME_TIME) {
+ int[] ledColors = {Color.RED, Color.YELLOW, Color.RED, Color.YELLOW, Color.RED,
+ Color.YELLOW, Color.RED, Color.YELLOW, Color.RED, Color.YELLOW};
+ ledStick.setColors(ledColors);
+ } else if (gamepad1.a) {
+ ledStick.setColor(Color.BLUE);
+ } else if (gamepad1.b) {
+ ledStick.setColor(Color.RED);
+ } else if (gamepad1.left_bumper) {
+ ledStick.turnAllOff();
+ } else {
+ ledStick.setColor(Color.GREEN);
+ }
+
+ /*
+ * Use DPAD up and down to change brightness
+ */
+ int newBrightness = brightness;
+ if (gamepad1.dpad_up && !wasUp) {
+ newBrightness = brightness + 1;
+ } else if (gamepad1.dpad_down && !wasDown) {
+ newBrightness = brightness - 1;
+ }
+ if (newBrightness != brightness) {
+ brightness = Range.clip(newBrightness, 0, 31);
+ ledStick.setBrightness(brightness);
+ }
+ telemetry.addData("Brightness", brightness);
+
+ wasDown = gamepad1.dpad_down;
+ wasUp = gamepad1.dpad_up;
+ }
+}
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptMotorBulkRead.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptMotorBulkRead.java
new file mode 100644
index 0000000..5439f78
--- /dev/null
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptMotorBulkRead.java
@@ -0,0 +1,227 @@
+/* Copyright (c) 2019 Phil Malone. 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.robotcontroller.external.samples;
+
+import com.qualcomm.hardware.lynx.LynxModule;
+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.DcMotorEx;
+import com.qualcomm.robotcore.util.ElapsedTime;
+
+import java.util.List;
+
+/*
+ * This OpMode illustrates how to use the Expansion Hub's Bulk-Read feature to speed up control cycle times.
+ * In this example there are 4 motors that need their encoder positions, and velocities read.
+ * The sample is written to work with one or two expansion hubs, with no assumption as to where the motors are located.
+ *
+ * Three scenarios are tested:
+ * Cache Mode = OFF This is the normal default, where no cache is used, and every read produces a discrete transaction with
+ * an expansion hub, which is the slowest approach, but guarentees that the value is as fresh (recent) as possible..
+ *
+ * Cache Mode = AUTO This mode will attempt to minimize the number of discrete read commands, by performing bulk-reads
+ * and then returning values that have been cached. The cache is updated automatically whenever any specific encoder is re-read.
+ * This mode will always return new data, but it may perform more bulk-reads than absolutely required.
+ * Extra reads will be performed if multiple encoder/velocity reads are performed on the same encoder in one control cycle.
+ * This mode is a good compromise between the OFF and MANUAL modes.
+ * Note: If there are significant user-program delays between encoder reads, the cached value may not be fresh (recent).
+ * You can issue a clearBulkCache() call at any time force a fresh bulk-read on the next encoder read.
+ *
+ * Cache Mode = MANUAL This mode requires the user's code to determine the best time to clear the cached bulk-read data.
+ * Well organized code will reset the cache once at the beginning of the control cycle, and then immediately read and store all the encoder values.
+ * This approach will produce the shortest cycle times, but it does require the user to manually clear the cache.
+ * Since NO automatic Bulk-Reads are performed, neglecting to clear the bulk cache will result in the same values being returned
+ * each time an encoder read is performed.
+ *
+ * -------------------------------------
+ *
+ * General tip to speed up your control cycles:
+ *
+ * No matter what method you use to read encoders and other inputs, you should try to
+ * avoid reading the same encoder input multiple times around a control loop.
+ * Under normal conditions, this will slow down the control loop.
+ * The preferred method is to read all the required inputs ONCE at the beginning of the loop,
+ * and save the values in variable that can be used by other parts of the control code.
+ *
+ * eg: if you are sending encoder positions to your telemetry display, putting a getCurrentPosition()
+ * call in the telemetry statement will force the code to go and get another copy which will take time.
+ * It's much better read the position into a variable once, and use that variable for control AND display.
+ * Reading saved variables takes no time at all.
+ *
+ * Once you put all your sensor reads at the beginning of the control cycle, it's very easy to use
+ * the bulk-read AUTO mode to streamline your cycle timing.
+ */
+@TeleOp (name = "Motor Bulk Reads", group = "Tests")
+@Disabled
+public class ConceptMotorBulkRead extends LinearOpMode {
+
+ final int TEST_CYCLES = 500; // Number of control cycles to run to determine cycle times.
+
+ private DcMotorEx m1, m2, m3, m4; // Motor Objects
+ private long e1, e2, e3, e4; // Encoder Values
+ private double v1, v2, v3, v4; // Velocities
+
+ // Cycle Times
+ double t1 = 0;
+ double t2 = 0;
+ double t3 = 0;
+
+ @Override
+ public void runOpMode() {
+
+ int cycles;
+
+ // Important Step 1: Make sure you use DcMotorEx when you instantiate your motors.
+ m1 = hardwareMap.get(DcMotorEx.class, "m1"); // Configure the robot to use these 4 motor names,
+ m2 = hardwareMap.get(DcMotorEx.class, "m2"); // or change these strings to match your existing Robot Configuration.
+ m3 = hardwareMap.get(DcMotorEx.class, "m3");
+ m4 = hardwareMap.get(DcMotorEx.class, "m4");
+
+ // Important Step 2: Get access to a list of Expansion Hub Modules to enable changing caching methods.
+ List allHubs = hardwareMap.getAll(LynxModule.class);
+
+ ElapsedTime timer = new ElapsedTime();
+
+ telemetry.addData(">", "Press START to start tests");
+ telemetry.addData(">", "Test results will update for each access method.");
+ telemetry.update();
+ waitForStart();
+
+ // --------------------------------------------------------------------------------------
+ // Run control loop using legacy encoder reads
+ // In this mode, a single read is done for each encoder position, and a bulk read is done for each velocity read.
+ // This is the worst case scenario.
+ // This is the same as using LynxModule.BulkCachingMode.OFF
+ // --------------------------------------------------------------------------------------
+
+ displayCycleTimes("Test 1 of 3 (Wait for completion)");
+
+ timer.reset();
+ cycles = 0;
+ while (opModeIsActive() && (cycles++ < TEST_CYCLES)) {
+ e1 = m1.getCurrentPosition();
+ e2 = m2.getCurrentPosition();
+ e3 = m3.getCurrentPosition();
+ e4 = m4.getCurrentPosition();
+
+ v1 = m1.getVelocity();
+ v2 = m2.getVelocity();
+ v3 = m3.getVelocity();
+ v4 = m4.getVelocity();
+
+ // Put Control loop action code here.
+
+ }
+ // calculate the average cycle time.
+ t1 = timer.milliseconds() / cycles;
+ displayCycleTimes("Test 2 of 3 (Wait for completion)");
+
+ // --------------------------------------------------------------------------------------
+ // Run test cycles using AUTO cache mode
+ // In this mode, only one bulk read is done per cycle, UNLESS you read a specific encoder/velocity item AGAIN in that cycle.
+ // --------------------------------------------------------------------------------------
+
+ // Important Step 3: Option A. Set all Expansion hubs to use the AUTO Bulk Caching mode
+ for (LynxModule module : allHubs) {
+ module.setBulkCachingMode(LynxModule.BulkCachingMode.AUTO);
+ }
+
+ timer.reset();
+ cycles = 0;
+ while (opModeIsActive() && (cycles++ < TEST_CYCLES)) {
+ e1 = m1.getCurrentPosition(); // Uses 1 bulk-read for all 4 encoder/velocity reads,
+ e2 = m2.getCurrentPosition(); // but don't do any `get` operations more than once per cycle.
+ e3 = m3.getCurrentPosition();
+ e4 = m4.getCurrentPosition();
+
+ v1 = m1.getVelocity();
+ v2 = m2.getVelocity();
+ v3 = m3.getVelocity();
+ v4 = m4.getVelocity();
+
+ // Put Control loop action code here.
+
+ }
+ // calculate the average cycle time.
+ t2 = timer.milliseconds() / cycles;
+ displayCycleTimes("Test 3 of 3 (Wait for completion)");
+
+ // --------------------------------------------------------------------------------------
+ // Run test cycles using MANUAL cache mode
+ // In this mode, only one block read is done each control cycle.
+ // This is the MOST efficient method, but it does require that the cache is cleared manually each control cycle.
+ // --------------------------------------------------------------------------------------
+
+ // Important Step 3: Option B. Set all Expansion hubs to use the MANUAL Bulk Caching mode
+ for (LynxModule module : allHubs) {
+ module.setBulkCachingMode(LynxModule.BulkCachingMode.MANUAL);
+ }
+
+ timer.reset();
+ cycles = 0;
+ while (opModeIsActive() && (cycles++ < TEST_CYCLES)) {
+
+ // Important Step 4: If you are using MANUAL mode, you must clear the BulkCache once per control cycle
+ for (LynxModule module : allHubs) {
+ module.clearBulkCache();
+ }
+
+ e1 = m1.getCurrentPosition(); // Uses 1 bulk-read to obtain ALL the motor data
+ e2 = m2.getCurrentPosition(); // There is no penalty for doing more `get` operations in this cycle,
+ e3 = m3.getCurrentPosition(); // but they will return the same data.
+ e4 = m4.getCurrentPosition();
+
+ v1 = m1.getVelocity();
+ v2 = m2.getVelocity();
+ v3 = m3.getVelocity();
+ v4 = m4.getVelocity();
+
+ // Put Control loop action code here.
+
+ }
+ // calculate the average cycle time.
+ t3 = timer.milliseconds() / cycles;
+ displayCycleTimes("Complete");
+
+ // wait until op-mode is stopped by user, before clearing display.
+ while (opModeIsActive()) ;
+ }
+
+ // Display three comparison times.
+ void displayCycleTimes(String status) {
+ telemetry.addData("Testing", status);
+ telemetry.addData("Cache = OFF", "%5.1f mS/cycle", t1);
+ telemetry.addData("Cache = AUTO", "%5.1f mS/cycle", t2);
+ telemetry.addData("Cache = MANUAL", "%5.1f mS/cycle", t3);
+ telemetry.update();
+ }
+}
+
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptNullOp.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptNullOp.java
new file mode 100644
index 0000000..4a887bd
--- /dev/null
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptNullOp.java
@@ -0,0 +1,89 @@
+/* Copyright (c) 2017 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.robotcontroller.external.samples;
+
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.util.ElapsedTime;
+
+/*
+ * Demonstrates an empty iterative OpMode
+ */
+@TeleOp(name = "Concept: NullOp", group = "Concept")
+@Disabled
+public class ConceptNullOp extends OpMode {
+
+ private ElapsedTime runtime = new ElapsedTime();
+
+ /**
+ * This method will be called once, when the INIT button is pressed.
+ */
+ @Override
+ public void init() {
+ telemetry.addData("Status", "Initialized");
+ }
+
+ /**
+ * This method will be called repeatedly during the period between when
+ * the INIT button is pressed and when the START button is pressed (or the
+ * OpMode is stopped).
+ */
+ @Override
+ public void init_loop() {
+ }
+
+ /**
+ * This method will be called once, when the START button is pressed.
+ */
+ @Override
+ public void start() {
+ runtime.reset();
+ }
+
+ /**
+ * This method will be called repeatedly during the period between when
+ * the START button is pressed and when the OpMode is stopped.
+ */
+ @Override
+ public void loop() {
+ telemetry.addData("Status", "Run Time: " + runtime.toString());
+ }
+
+ /**
+ * This method will be called once, when this OpMode is stopped.
+ *
+ * Your ability to control hardware from this method will be limited.
+ */
+ @Override
+ public void stop() {
+
+ }
+}
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptRampMotorSpeed.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptRampMotorSpeed.java
new file mode 100644
index 0000000..6e0be37
--- /dev/null
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptRampMotorSpeed.java
@@ -0,0 +1,114 @@
+/* Copyright (c) 2017 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.robotcontroller.external.samples;
+
+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.DcMotor;
+
+/*
+ * This OpMode ramps a single motor speed up and down repeatedly until Stop is pressed.
+ * The code is structured as a LinearOpMode
+ *
+ * This code assumes a DC motor configured with the name "left_drive" as is found on a Robot.
+ *
+ * INCREMENT sets how much to increase/decrease the power each cycle
+ * CYCLE_MS sets the update period.
+ *
+ * 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: Ramp Motor Speed", group = "Concept")
+@Disabled
+public class ConceptRampMotorSpeed extends LinearOpMode {
+
+ static final double INCREMENT = 0.01; // amount to ramp motor each CYCLE_MS cycle
+ static final int CYCLE_MS = 50; // period of each cycle
+ static final double MAX_FWD = 1.0; // Maximum FWD power applied to motor
+ static final double MAX_REV = -1.0; // Maximum REV power applied to motor
+
+ // Define class members
+ DcMotor motor;
+ double power = 0;
+ boolean rampUp = true;
+
+
+ @Override
+ public void runOpMode() {
+
+ // Connect to motor (Assume standard left wheel)
+ // Change the text in quotes to match any motor name on your robot.
+ motor = hardwareMap.get(DcMotor.class, "left_drive");
+
+ // Wait for the start button
+ telemetry.addData(">", "Press Start to run Motors." );
+ telemetry.update();
+ waitForStart();
+
+ // Ramp motor speeds till stop pressed.
+ while(opModeIsActive()) {
+
+ // Ramp the motors, according to the rampUp variable.
+ if (rampUp) {
+ // Keep stepping up until we hit the max value.
+ power += INCREMENT ;
+ if (power >= MAX_FWD ) {
+ power = MAX_FWD;
+ rampUp = !rampUp; // Switch ramp direction
+ }
+ }
+ else {
+ // Keep stepping down until we hit the min value.
+ power -= INCREMENT ;
+ if (power <= MAX_REV ) {
+ power = MAX_REV;
+ rampUp = !rampUp; // Switch ramp direction
+ }
+ }
+
+ // Display the current value
+ telemetry.addData("Motor Power", "%5.2f", power);
+ telemetry.addData(">", "Press Stop to end test." );
+ telemetry.update();
+
+ // Set the motor to the new power and pause;
+ motor.setPower(power);
+ sleep(CYCLE_MS);
+ idle();
+ }
+
+ // Turn off motor and signal done;
+ motor.setPower(0);
+ telemetry.addData(">", "Done");
+ telemetry.update();
+
+ }
+}
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptRevLED.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptRevLED.java
new file mode 100644
index 0000000..9c168d5
--- /dev/null
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptRevLED.java
@@ -0,0 +1,78 @@
+/*
+ Copyright (c) 2024 Alan Smith
+ 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 Alan Smith 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 FITNESSFOR 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.robotcontroller.external.samples;
+
+/*
+ * This OpMode illustrates how to use the REV Digital Indicator
+ *
+ * This is a simple way to add the REV Digital Indicator which has a red and green LED.
+ * (and as you may remember, red + green = yellow so when they are both on you can get yellow)
+ *
+ * Why?
+ * You can use this to show information to the driver. For example, green might be that you can
+ * pick up more game elements and red would be that you already have the possession limit.
+ *
+ * This OpMode assumes that the REV Digital Indicator is setup as 2 Digital Channels named
+ * front_led_green and front_led_red. (the green should be the lower of the 2 channels it is plugged
+ * into and the red should be the higher)
+ *
+ * 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
+ *
+ * You can buy this product here: https://www.revrobotics.com/rev-31-2010/
+ */
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.hardware.LED;
+
+@TeleOp(name = "Concept: RevLED", group = "Concept")
+@Disabled
+public class ConceptRevLED extends OpMode {
+ LED frontLED_red;
+ LED frontLED_green;
+
+ @Override
+ public void init() {
+ frontLED_green = hardwareMap.get(LED.class, "front_led_green");
+ frontLED_red = hardwareMap.get(LED.class, "front_led_red");
+ }
+
+ @Override
+ public void loop() {
+ if (gamepad1.a) {
+ frontLED_red.on();
+ } else {
+ frontLED_red.off();
+ }
+ if (gamepad1.b) {
+ frontLED_green.on();
+ } else {
+ frontLED_green.off();
+ }
+ }
+}
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptRevSPARKMini.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptRevSPARKMini.java
new file mode 100644
index 0000000..798d689
--- /dev/null
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptRevSPARKMini.java
@@ -0,0 +1,111 @@
+/* Copyright (c) 2018 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.robotcontroller.external.samples;
+
+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.DcMotorSimple;
+import com.qualcomm.robotcore.util.ElapsedTime;
+import com.qualcomm.robotcore.util.Range;
+
+
+/*
+ * This OpMode demonstrates a POV Drive system, with commented-out code for a Tank Drive system,
+ * for a two wheeled robot using two REV SPARKminis.
+ * To use this example, connect two REV SPARKminis into servo ports on the Control Hub. On the
+ * robot configuration, use the drop down list under 'Servos' to select 'REV SPARKmini Controller'
+ * and name them 'left_drive' and 'right_drive'.
+ *
+ * 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="REV SPARKmini Simple Drive Example", group="Concept")
+@Disabled
+public class ConceptRevSPARKMini extends LinearOpMode {
+
+ // Declare OpMode members.
+ private ElapsedTime runtime = new ElapsedTime();
+ private DcMotorSimple leftDrive = null;
+ private DcMotorSimple rightDrive = null;
+
+ @Override
+ public void runOpMode() {
+ telemetry.addData("Status", "Initialized");
+ telemetry.update();
+
+ // Initialize the hardware variables. Note that the strings used here as parameters
+ // to 'get' must correspond to the names assigned during robot configuration.
+ leftDrive = hardwareMap.get(DcMotorSimple.class, "left_drive");
+ rightDrive = hardwareMap.get(DcMotorSimple.class, "right_drive");
+
+ // Most robots need the motor on one side to be reversed to drive forward
+ // Reverse the motor that runs backward when connected directly to the battery
+ leftDrive.setDirection(DcMotorSimple.Direction.FORWARD);
+ rightDrive.setDirection(DcMotorSimple.Direction.REVERSE);
+
+ // Wait for the game to start (driver presses START)
+ waitForStart();
+ runtime.reset();
+
+ // run until the end of the match (driver presses STOP)
+ while (opModeIsActive()) {
+
+ // Setup a variable for each drive wheel to save power level for telemetry
+ double leftPower;
+ double rightPower;
+
+ // Choose to drive using either Tank Mode, or POV Mode
+ // Comment out the method that's not used. The default below is POV.
+
+ // POV Mode uses left stick to go forward, and right stick to turn.
+ // - This uses basic math to combine motions and is easier to drive straight.
+ double drive = -gamepad1.left_stick_y;
+ double turn = gamepad1.right_stick_x;
+ leftPower = Range.clip(drive + turn, -1.0, 1.0) ;
+ rightPower = Range.clip(drive - turn, -1.0, 1.0) ;
+
+ // Tank Mode uses one stick to control each wheel.
+ // - This requires no math, but it is hard to drive forward slowly and keep straight.
+ // leftPower = -gamepad1.left_stick_y ;
+ // rightPower = -gamepad1.right_stick_y ;
+
+ // Send calculated power to wheels
+ leftDrive.setPower(leftPower);
+ rightDrive.setPower(rightPower);
+
+ // Show the elapsed game time and wheel power.
+ telemetry.addData("Status", "Run Time: " + runtime.toString());
+ telemetry.addData("Motors", "left (%.2f), right (%.2f)", leftPower, rightPower);
+ telemetry.update();
+ }
+ }
+}
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptScanServo.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptScanServo.java
new file mode 100644
index 0000000..2b8ad33
--- /dev/null
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptScanServo.java
@@ -0,0 +1,115 @@
+/* Copyright (c) 2017 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.robotcontroller.external.samples;
+
+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.Servo;
+
+/*
+ * This OpMode scans a single servo back and forward until Stop is pressed.
+ * The code is structured as a LinearOpMode
+ * INCREMENT sets how much to increase/decrease the servo position each cycle
+ * CYCLE_MS sets the update period.
+ *
+ * This code assumes a Servo configured with the name "left_hand" as is found on a Robot.
+ *
+ * NOTE: When any servo position is set, ALL attached servos are activated, so ensure that any other
+ * connected servos are able to move freely before running this test.
+ *
+ * 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: Scan Servo", group = "Concept")
+@Disabled
+public class ConceptScanServo extends LinearOpMode {
+
+ static final double INCREMENT = 0.01; // amount to slew servo each CYCLE_MS cycle
+ static final int CYCLE_MS = 50; // period of each cycle
+ static final double MAX_POS = 1.0; // Maximum rotational position
+ static final double MIN_POS = 0.0; // Minimum rotational position
+
+ // Define class members
+ Servo servo;
+ double position = (MAX_POS - MIN_POS) / 2; // Start at halfway position
+ boolean rampUp = true;
+
+
+ @Override
+ public void runOpMode() {
+
+ // Connect to servo (Assume Robot Left Hand)
+ // Change the text in quotes to match any servo name on your robot.
+ servo = hardwareMap.get(Servo.class, "left_hand");
+
+ // Wait for the start button
+ telemetry.addData(">", "Press Start to scan Servo." );
+ telemetry.update();
+ waitForStart();
+
+
+ // Scan servo till stop pressed.
+ while(opModeIsActive()){
+
+ // slew the servo, according to the rampUp (direction) variable.
+ if (rampUp) {
+ // Keep stepping up until we hit the max value.
+ position += INCREMENT ;
+ if (position >= MAX_POS ) {
+ position = MAX_POS;
+ rampUp = !rampUp; // Switch ramp direction
+ }
+ }
+ else {
+ // Keep stepping down until we hit the min value.
+ position -= INCREMENT ;
+ if (position <= MIN_POS ) {
+ position = MIN_POS;
+ rampUp = !rampUp; // Switch ramp direction
+ }
+ }
+
+ // Display the current value
+ telemetry.addData("Servo Position", "%5.2f", position);
+ telemetry.addData(">", "Press Stop to end test." );
+ telemetry.update();
+
+ // Set the servo to the new position and pause;
+ servo.setPosition(position);
+ sleep(CYCLE_MS);
+ idle();
+ }
+
+ // Signal done;
+ telemetry.addData(">", "Done");
+ telemetry.update();
+ }
+}
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptSoundsASJava.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptSoundsASJava.java
new file mode 100644
index 0000000..1ceaa17
--- /dev/null
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptSoundsASJava.java
@@ -0,0 +1,133 @@
+/* Copyright (c) 2018 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.robotcontroller.external.samples;
+
+import com.qualcomm.ftccommon.SoundPlayer;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+/*
+ * This OpMode demonstrates how to play simple sounds on both the RC and DS phones.
+ * It illustrates how to build sounds into your application as a resource.
+ * This technique is best suited for use with Android Studio since it assumes you will be creating a new application
+ *
+ * If you are using OnBotJava, please see the ConceptSoundsOnBotJava sample
+ *
+ * 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
+ *
+ * Operation:
+ *
+ * Gamepad X & B buttons are used to trigger sounds in this example, but any event can be used.
+ * Note: Time should be allowed for sounds to complete before playing other sounds.
+ *
+ * For sound files to be used as a compiled-in resource, they need to be located in a folder called "raw" under your "res" (resources) folder.
+ * You can create your own "raw" folder from scratch, or you can copy the one from the FtcRobotController module.
+ *
+ * Android Studio coders will ultimately need a folder in your path as follows:
+ * /TeamCode/src/main/res/raw
+ *
+ * Copy any .wav files you want to play into this folder.
+ * Make sure that your files ONLY use lower-case characters, and have no spaces or special characters other than underscore.
+ *
+ * The name you give your .wav files will become the resource ID for these sounds.
+ * eg: gold.wav becomes R.raw.gold
+ *
+ * If you wish to use the sounds provided for this sample, they are located in:
+ * /FtcRobotController/src/main/res/raw
+ * You can copy and paste the entire 'raw' folder using Android Studio.
+ *
+ */
+
+@TeleOp(name="Concept: Sound Resources", group="Concept")
+@Disabled
+public class ConceptSoundsASJava extends LinearOpMode {
+
+ // Declare OpMode members.
+ private boolean goldFound; // Sound file present flags
+ private boolean silverFound;
+
+ private boolean isX = false; // Gamepad button state variables
+ private boolean isB = false;
+
+ private boolean wasX = false; // Gamepad button history variables
+ private boolean WasB = false;
+
+ @Override
+ public void runOpMode() {
+
+ // Determine Resource IDs for sounds built into the RC application.
+ int silverSoundID = hardwareMap.appContext.getResources().getIdentifier("silver", "raw", hardwareMap.appContext.getPackageName());
+ int goldSoundID = hardwareMap.appContext.getResources().getIdentifier("gold", "raw", hardwareMap.appContext.getPackageName());
+
+ // Determine if sound resources are found.
+ // Note: Preloading is NOT required, but it's a good way to verify all your sounds are available before you run.
+ if (goldSoundID != 0)
+ goldFound = SoundPlayer.getInstance().preload(hardwareMap.appContext, goldSoundID);
+
+ if (silverSoundID != 0)
+ silverFound = SoundPlayer.getInstance().preload(hardwareMap.appContext, silverSoundID);
+
+ // Display sound status
+ telemetry.addData("gold resource", goldFound ? "Found" : "NOT found\n Add gold.wav to /src/main/res/raw" );
+ telemetry.addData("silver resource", silverFound ? "Found" : "Not found\n Add silver.wav to /src/main/res/raw" );
+
+ // Wait for the game to start (driver presses START)
+ telemetry.addData(">", "Press Start to continue");
+ telemetry.update();
+ waitForStart();
+
+ telemetry.addData(">", "Press X, B to play sounds.");
+ telemetry.update();
+
+ // run until the end of the match (driver presses STOP)
+ while (opModeIsActive()) {
+
+ // say Silver each time gamepad X is pressed (This sound is a resource)
+ if (silverFound && (isX = gamepad1.x) && !wasX) {
+ SoundPlayer.getInstance().startPlaying(hardwareMap.appContext, silverSoundID);
+ telemetry.addData("Playing", "Resource Silver");
+ telemetry.update();
+ }
+
+ // say Gold each time gamepad B is pressed (This sound is a resource)
+ if (goldFound && (isB = gamepad1.b) && !WasB) {
+ SoundPlayer.getInstance().startPlaying(hardwareMap.appContext, goldSoundID);
+ telemetry.addData("Playing", "Resource Gold");
+ telemetry.update();
+ }
+
+ // Save last button states
+ wasX = isX;
+ WasB = isB;
+ }
+ }
+}
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptSoundsOnBotJava.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptSoundsOnBotJava.java
new file mode 100644
index 0000000..fbb7416
--- /dev/null
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptSoundsOnBotJava.java
@@ -0,0 +1,120 @@
+/* Copyright (c) 2018 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.robotcontroller.external.samples;
+
+import com.qualcomm.ftccommon.SoundPlayer;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import java.io.File;
+
+/*
+ * This OpMode demonstrates how to play simple sounds on both the RC and DS phones.
+ * It illustrates how to play sound files that have been copied to the RC Phone
+ * This technique is best suited for use with OnBotJava since it does not require the app to be modified.
+ *
+ * Operation:
+ *
+ * Gamepad X & B buttons are used to trigger sounds in this example, but any event can be used.
+ * Note: Time should be allowed for sounds to complete before playing other sounds.
+ *
+ * To play a new sound, you will need to copy the .wav files to the phone, and then provide the full path to them as part of your OpMode.
+ * This is done in this sample for the two sound files. silver.wav and gold.wav
+ *
+ * You can put the files in a variety of soundPaths, but we recommend you put them in the /FIRST/blocks/sounds folder.
+ * Your OpModes will have guaranteed access to this folder, and you can transfer files into this folder using the BLOCKS web page.
+ * -- There is a link called "sounds" on the right hand side of the color bar on the BLOCKS page that can be used to send sound files to this folder by default.
+ * Or you can use Windows File Manager, or ADB to transfer the sound files
+ *
+ * To get full use of THIS sample, you will need to copy two sound file called silver.wav and gold.wav to /FIRST/blocks/sounds on the RC phone.
+ * They can be located here:
+ * https://github.com/ftctechnh/ftc_app/tree/master/FtcRobotController/src/main/res/raw/gold.wav
+ * https://github.com/ftctechnh/ftc_app/tree/master/FtcRobotController/src/main/res/raw/silver.wav
+ */
+
+@TeleOp(name="Concept: Sound Files", group="Concept")
+@Disabled
+public class ConceptSoundsOnBotJava extends LinearOpMode {
+
+ // Point to sound files on the phone's drive
+ private String soundPath = "/FIRST/blocks/sounds";
+ private File goldFile = new File("/sdcard" + soundPath + "/gold.wav");
+ private File silverFile = new File("/sdcard" + soundPath + "/silver.wav");
+
+ // Declare OpMode members.
+ private boolean isX = false; // Gamepad button state variables
+ private boolean isB = false;
+
+ private boolean wasX = false; // Gamepad button history variables
+ private boolean WasB = false;
+
+ @Override
+ public void runOpMode() {
+
+ // Make sure that the sound files exist on the phone
+ boolean goldFound = goldFile.exists();
+ boolean silverFound = silverFile.exists();
+
+ // Display sound status
+ telemetry.addData("gold sound", goldFound ? "Found" : "NOT Found \nCopy gold.wav to " + soundPath );
+ telemetry.addData("silver sound", silverFound ? "Found" : "NOT Found \nCopy silver.wav to " + soundPath );
+
+ // Wait for the game to start (driver presses PLAY)
+ telemetry.addData(">", "Press Start to continue");
+ telemetry.update();
+ waitForStart();
+
+ telemetry.addData(">", "Press X or B to play sounds.");
+ telemetry.update();
+
+ // run until the end of the match (driver presses STOP)
+ while (opModeIsActive()) {
+
+ // say Silver each time gamepad X is pressed (This sound is a resource)
+ if (silverFound && (isX = gamepad1.x) && !wasX) {
+ SoundPlayer.getInstance().startPlaying(hardwareMap.appContext, silverFile);
+ telemetry.addData("Playing", "Silver File");
+ telemetry.update();
+ }
+
+ // say Gold each time gamepad B is pressed (This sound is a resource)
+ if (goldFound && (isB = gamepad1.b) && !WasB) {
+ SoundPlayer.getInstance().startPlaying(hardwareMap.appContext, goldFile);
+ telemetry.addData("Playing", "Gold File");
+ telemetry.update();
+ }
+
+ // Save last button states
+ wasX = isX;
+ WasB = isB;
+ }
+ }
+}
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptSoundsSKYSTONE.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptSoundsSKYSTONE.java
new file mode 100644
index 0000000..983e434
--- /dev/null
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptSoundsSKYSTONE.java
@@ -0,0 +1,122 @@
+/* Copyright (c) 2018 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.robotcontroller.external.samples;
+
+import android.content.Context;
+import com.qualcomm.ftccommon.SoundPlayer;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+/*
+ * This OpMode demonstrates how to play one of the several SKYSTONE/Star Wars sounds loaded into the SDK.
+ * It does this by creating a simple "chooser" controlled by the gamepad Up Down buttons.
+ * This code also prevents sounds from stacking up by setting a "playing" flag, which is cleared when the sound finishes playing.
+ *
+ * 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
+ *
+ * Operation:
+ * Use the DPAD to change the selected sound, and the Right Bumper to play it.
+ */
+
+@TeleOp(name="SKYSTONE Sounds", group="Concept")
+@Disabled
+public class ConceptSoundsSKYSTONE extends LinearOpMode {
+
+ // List of available sound resources
+ String sounds[] = {"ss_alarm", "ss_bb8_down", "ss_bb8_up", "ss_darth_vader", "ss_fly_by",
+ "ss_mf_fail", "ss_laser", "ss_laser_burst", "ss_light_saber", "ss_light_saber_long", "ss_light_saber_short",
+ "ss_light_speed", "ss_mine", "ss_power_up", "ss_r2d2_up", "ss_roger_roger", "ss_siren", "ss_wookie" };
+ boolean soundPlaying = false;
+
+ @Override
+ public void runOpMode() {
+
+ // Variables for choosing from the available sounds
+ int soundIndex = 0;
+ int soundID = -1;
+ boolean was_dpad_up = false;
+ boolean was_dpad_down = false;
+
+ Context myApp = hardwareMap.appContext;
+
+ // create a sound parameter that holds the desired player parameters.
+ SoundPlayer.PlaySoundParams params = new SoundPlayer.PlaySoundParams();
+ params.loopControl = 0;
+ params.waitForNonLoopingSoundsToFinish = true;
+
+ // In this sample, we will skip waiting for the user to press play, and start displaying sound choices right away
+ while (!isStopRequested()) {
+
+ // Look for DPAD presses to change the selection
+ if (gamepad1.dpad_down && !was_dpad_down) {
+ // Go to next sound (with list wrap) and display it
+ soundIndex = (soundIndex + 1) % sounds.length;
+ }
+
+ if (gamepad1.dpad_up && !was_dpad_up) {
+ // Go to previous sound (with list wrap) and display it
+ soundIndex = (soundIndex + sounds.length - 1) % sounds.length;
+ }
+
+ // Look for trigger to see if we should play sound
+ // Only start a new sound if we are currently not playing one.
+ if (gamepad1.right_bumper && !soundPlaying) {
+
+ // Determine Resource IDs for the sounds you want to play, and make sure it's valid.
+ if ((soundID = myApp.getResources().getIdentifier(sounds[soundIndex], "raw", myApp.getPackageName())) != 0){
+
+ // Signal that the sound is now playing.
+ soundPlaying = true;
+
+ // Start playing, and also Create a callback that will clear the playing flag when the sound is complete.
+ SoundPlayer.getInstance().startPlaying(myApp, soundID, params, null,
+ new Runnable() {
+ public void run() {
+ soundPlaying = false;
+ }} );
+ }
+ }
+
+ // Remember the last state of the dpad to detect changes.
+ was_dpad_up = gamepad1.dpad_up;
+ was_dpad_down = gamepad1.dpad_down;
+
+ // Display the current sound choice, and the playing status.
+ telemetry.addData("", "Use DPAD up/down to choose sound.");
+ telemetry.addData("", "Press Right Bumper to play sound.");
+ telemetry.addData("", "");
+ telemetry.addData("Sound >", sounds[soundIndex]);
+ telemetry.addData("Status >", soundPlaying ? "Playing" : "Stopped");
+ telemetry.update();
+ }
+ }
+}
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptTelemetry.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptTelemetry.java
new file mode 100644
index 0000000..f2c6097
--- /dev/null
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptTelemetry.java
@@ -0,0 +1,177 @@
+/* Copyright (c) 2017 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.robotcontroller.external.samples;
+
+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.VoltageSensor;
+import com.qualcomm.robotcore.util.ElapsedTime;
+import org.firstinspires.ftc.robotcore.external.Func;
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+
+/*
+ * This OpMode illustrates various ways in which telemetry can be
+ * transmitted from the robot controller to the driver station. The sample illustrates
+ * numeric and text data, formatted output, and optimized evaluation of expensive-to-acquire
+ * information. The telemetry log is illustrated by scrolling a poem
+ * to the driver station.
+ *
+ * Also see the Telemetry javadocs.
+ */
+@TeleOp(name = "Concept: Telemetry", group = "Concept")
+@Disabled
+public class ConceptTelemetry extends LinearOpMode {
+ /** Keeps track of the line of the poem which is to be emitted next */
+ int poemLine = 0;
+
+ /** Keeps track of how long it's been since we last emitted a line of poetry */
+ ElapsedTime poemElapsed = new ElapsedTime();
+
+ static final String[] poem = new String[] {
+
+ "Mary had a little lamb,",
+ "His fleece was white as snow,",
+ "And everywhere that Mary went,",
+ "The lamb was sure to go.",
+ "",
+ "He followed her to school one day,",
+ "Which was against the rule,",
+ "It made the children laugh and play",
+ "To see a lamb at school.",
+ "",
+ "And so the teacher turned it out,",
+ "But still it lingered near,",
+ "And waited patiently about,",
+ "Till Mary did appear.",
+ "",
+ "\"Why does the lamb love Mary so?\"",
+ "The eager children cry.",
+ "\"Why, Mary loves the lamb, you know,\"",
+ "The teacher did reply.",
+ "",
+ ""
+ };
+
+ @Override public void runOpMode() {
+
+ /* we keep track of how long it's been since the OpMode was started, just
+ * to have some interesting data to show */
+ ElapsedTime opmodeRunTime = new ElapsedTime();
+
+ // We show the log in oldest-to-newest order, as that's better for poetry
+ telemetry.log().setDisplayOrder(Telemetry.Log.DisplayOrder.OLDEST_FIRST);
+ // We can control the number of lines shown in the log
+ telemetry.log().setCapacity(6);
+ // The interval between lines of poetry, in seconds
+ double sPoemInterval = 0.6;
+
+ /*
+ * Wait until we've been given the ok to go. For something to do, we emit the
+ * elapsed time as we sit here and wait. If we didn't want to do anything while
+ * we waited, we would just call waitForStart().
+ */
+ while (!isStarted()) {
+ telemetry.addData("time", "%.1f seconds", opmodeRunTime.seconds());
+ telemetry.update();
+ idle();
+ }
+
+ // Ok, we've been given the ok to go
+
+ /*
+ * As an illustration, the first line on our telemetry display will display the battery voltage.
+ * The idea here is that it's expensive to compute the voltage (at least for purposes of illustration)
+ * so you don't want to do it unless the data is _actually_ going to make it to the
+ * driver station (recall that telemetry transmission is throttled to reduce bandwidth use.
+ * Note that getBatteryVoltage() below returns 'Infinity' if there's no voltage sensor attached.
+ *
+ * @see Telemetry#getMsTransmissionInterval()
+ */
+ telemetry.addData("voltage", "%.1f volts", new Func() {
+ @Override public Double value() {
+ return getBatteryVoltage();
+ }
+ });
+
+ // Reset to keep some timing stats for the post-'start' part of the OpMode
+ opmodeRunTime.reset();
+ int loopCount = 1;
+
+ // Go go gadget robot!
+ while (opModeIsActive()) {
+
+ // Emit poetry if it's been a while
+ if (poemElapsed.seconds() > sPoemInterval) {
+ emitPoemLine();
+ }
+
+ // As an illustration, show some loop timing information
+ telemetry.addData("loop count", loopCount);
+ telemetry.addData("ms/loop", "%.3f ms", opmodeRunTime.milliseconds() / loopCount);
+
+ // Show joystick information as some other illustrative data
+ telemetry.addLine("left joystick | ")
+ .addData("x", gamepad1.left_stick_x)
+ .addData("y", gamepad1.left_stick_y);
+ telemetry.addLine("right joystick | ")
+ .addData("x", gamepad1.right_stick_x)
+ .addData("y", gamepad1.right_stick_y);
+
+ /*
+ * Transmit the telemetry to the driver station, subject to throttling.
+ * See the documentation for Telemetry.getMsTransmissionInterval() for more information.
+ */
+ telemetry.update();
+
+ // Update loop info
+ loopCount++;
+ }
+ }
+
+ // emits a line of poetry to the telemetry log
+ void emitPoemLine() {
+ telemetry.log().add(poem[poemLine]);
+ poemLine = (poemLine+1) % poem.length;
+ poemElapsed.reset();
+ }
+
+ // Computes the current battery voltage
+ double getBatteryVoltage() {
+ double result = Double.POSITIVE_INFINITY;
+ for (VoltageSensor sensor : hardwareMap.voltageSensor) {
+ double voltage = sensor.getVoltage();
+ if (voltage > 0) {
+ result = Math.min(result, voltage);
+ }
+ }
+ return result;
+ }
+}
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVisionColorLocator_Circle.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVisionColorLocator_Circle.java
new file mode 100644
index 0000000..4ec17aa
--- /dev/null
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVisionColorLocator_Circle.java
@@ -0,0 +1,245 @@
+/*
+ * Copyright (c) 2024 Phil Malone
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+package org.firstinspires.ftc.robotcontroller.external.samples;
+
+import android.graphics.Color;
+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;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
+import org.firstinspires.ftc.vision.VisionPortal;
+import org.firstinspires.ftc.vision.opencv.Circle;
+import org.firstinspires.ftc.vision.opencv.ColorBlobLocatorProcessor;
+import org.firstinspires.ftc.vision.opencv.ColorRange;
+import org.firstinspires.ftc.vision.opencv.ImageRegion;
+
+import java.util.List;
+
+/*
+ * This OpMode illustrates how to use a video source (camera) to locate specifically colored regions.
+ * This sample is targeted towards circular blobs. To see rectangles, look at ConceptVisionColorLocator_Rectangle
+ *
+ * Unlike a "color sensor" which determines the color of nearby object, this "color locator"
+ * will search the Region Of Interest (ROI) in a camera image, and find any "blobs" of color that
+ * match the requested color range. These blobs can be further filtered and sorted to find the one
+ * most likely to be the item the user is looking for.
+ *
+ * To perform this function, a VisionPortal runs a ColorBlobLocatorProcessor process.
+ * The ColorBlobLocatorProcessor (CBLP) process is created first, and then the VisionPortal is built.
+ * The (CBLP) analyses the ROI and locates pixels that match the ColorRange to form a "mask".
+ * The matching pixels are then collected into contiguous "blobs" of pixels.
+ * The outer boundaries of these blobs are called its "contour". For each blob, the process then
+ * creates the smallest possible circle that will fully encase the contour. The user can then call
+ * getBlobs() to retrieve the list of Blobs, where each contains the contour and the circle.
+ * Note: The default sort order for Blobs is ContourArea, in descending order, so the biggest
+ * contours are listed first.
+ *
+ * A colored enclosing circle is drawn on the camera preview to show the location of each Blob
+ * The original Blob contour can also be added to the preview.
+ * This is helpful when configuring the ColorBlobLocatorProcessor parameters.
+ *
+ * Tip: Connect an HDMI monitor to the Control Hub to view the Color Location process in real-time.
+ * Or use a screen copy utility like ScrCpy.exe to view the video remotely.
+ *
+ * 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: Vision Color-Locator (Circle)", group = "Concept")
+public class ConceptVisionColorLocator_Circle extends LinearOpMode {
+ @Override
+ public void runOpMode() {
+ /* Build a "Color Locator" vision processor based on the ColorBlobLocatorProcessor class.
+ * - Specify the color range you are looking for. Use a predefined color, or create your own
+ *
+ * .setTargetColorRange(ColorRange.BLUE) // use a predefined color match
+ * Available colors are: RED, BLUE, YELLOW, GREEN, ARTIFACT_GREEN, ARTIFACT_PURPLE
+ * .setTargetColorRange(new ColorRange(ColorSpace.YCrCb, // or define your own color match
+ * new Scalar( 32, 176, 0),
+ * new Scalar(255, 255, 132)))
+ *
+ * - Focus the color locator by defining a RegionOfInterest (ROI) which you want to search.
+ * This can be the entire frame, or a sub-region defined using:
+ * 1) standard image coordinates or 2) a normalized +/- 1.0 coordinate system.
+ * Use one form of the ImageRegion class to define the ROI.
+ * ImageRegion.entireFrame()
+ * ImageRegion.asImageCoordinates(50, 50, 150, 150) 100x100 pixels at upper left corner
+ * ImageRegion.asUnityCenterCoordinates(-0.5, 0.5, 0.5, -0.5) 50% width/height in center
+ *
+ * - Define which contours are included.
+ * You can get ALL the contours, ignore contours that are completely inside another contour.
+ * .setContourMode(ColorBlobLocatorProcessor.ContourMode.ALL_FLATTENED_HIERARCHY)
+ * .setContourMode(ColorBlobLocatorProcessor.ContourMode.EXTERNAL_ONLY)
+ * EXTERNAL_ONLY helps to avoid bright reflection spots from breaking up solid colors.
+ *
+ * - Turn the displays of contours ON or OFF.
+ * Turning these on helps debugging but takes up valuable CPU time.
+ * .setDrawContours(true) Draws an outline of each contour.
+ * .setEnclosingCircleColor(int color) Draws a circle around each contour. 0 to disable.
+ * .setBoxFitColor(int color) Draws a rectangle around each contour. 0 to disable. ON by default.
+ *
+ *
+ * - include any pre-processing of the image or mask before looking for Blobs.
+ * There are some extra processing you can include to improve the formation of blobs.
+ * Using these features requires an understanding of how they may effect the final
+ * blobs. The "pixels" argument sets the NxN kernel size.
+ * .setBlurSize(int pixels)
+ * Blurring an image helps to provide a smooth color transition between objects,
+ * and smoother contours. The higher the number, the more blurred the image becomes.
+ * Note: Even "pixels" values will be incremented to satisfy the "odd number" requirement.
+ * Blurring too much may hide smaller features. A size of 5 is good for a 320x240 image.
+ *
+ * .setErodeSize(int pixels)
+ * Erosion removes floating pixels and thin lines so that only substantive objects remain.
+ * Erosion can grow holes inside regions, and also shrink objects.
+ * "pixels" in the range of 2-4 are suitable for low res images.
+ *
+ * .setDilateSize(int pixels)
+ * Dilation makes objects and lines more visible by filling in small holes, and making
+ * filled shapes appear larger. Dilation is useful for joining broken parts of an
+ * object, such as when removing noise from an image.
+ * "pixels" in the range of 2-4 are suitable for low res images.
+ *
+ * .setMorphOperationType(MorphOperationType morphOperationType)
+ * This defines the order in which the Erode/Dilate actions are performed.
+ * OPENING: Will Erode and then Dilate which will make small noise blobs go away
+ * CLOSING: Will Dilate and then Erode which will tend to fill in any small holes in blob edges.
+ */
+ ColorBlobLocatorProcessor colorLocator = new ColorBlobLocatorProcessor.Builder()
+ .setTargetColorRange(ColorRange.ARTIFACT_PURPLE) // Use a predefined color match
+ .setContourMode(ColorBlobLocatorProcessor.ContourMode.EXTERNAL_ONLY)
+ .setRoi(ImageRegion.asUnityCenterCoordinates(-0.75, 0.75, 0.75, -0.75))
+ .setDrawContours(true) // Show contours on the Stream Preview
+ .setBoxFitColor(0) // Disable the drawing of rectangles
+ .setCircleFitColor(Color.rgb(255, 255, 0)) // Draw a circle
+ .setBlurSize(5) // Smooth the transitions between different colors in image
+
+ // the following options have been added to fill in perimeter holes.
+ .setDilateSize(15) // Expand blobs to fill any divots on the edges
+ .setErodeSize(15) // Shrink blobs back to original size
+ .setMorphOperationType(ColorBlobLocatorProcessor.MorphOperationType.CLOSING)
+
+ .build();
+ /*
+ * Build a vision portal to run the Color Locator process.
+ *
+ * - Add the colorLocator process created above.
+ * - Set the desired video resolution.
+ * Since a high resolution will not improve this process, choose a lower resolution
+ * that is supported by your camera. This will improve overall performance and reduce
+ * latency.
+ * - Choose your video source. This may be
+ * .setCamera(hardwareMap.get(WebcamName.class, "Webcam 1")) ..... for a webcam
+ * or
+ * .setCamera(BuiltinCameraDirection.BACK) ... for a Phone Camera
+ */
+ VisionPortal portal = new VisionPortal.Builder()
+ .addProcessor(colorLocator)
+ .setCameraResolution(new Size(320, 240))
+ .setCamera(hardwareMap.get(WebcamName.class, "Webcam 1"))
+ .build();
+
+ telemetry.setMsTransmissionInterval(100); // Speed up telemetry updates for debugging.
+ telemetry.setDisplayFormat(Telemetry.DisplayFormat.MONOSPACE);
+
+ // WARNING: To view the stream preview on the Driver Station, this code runs in INIT mode.
+ while (opModeIsActive() || opModeInInit()) {
+ telemetry.addData("preview on/off", "... Camera Stream\n");
+
+ // Read the current list
+ List blobs = colorLocator.getBlobs();
+
+ /*
+ * The list of Blobs can be filtered to remove unwanted Blobs.
+ * Note: All contours will be still displayed on the Stream Preview, but only those
+ * that satisfy the filter conditions will remain in the current list of
+ * "blobs". Multiple filters may be used.
+ *
+ * To perform a filter
+ * ColorBlobLocatorProcessor.Util.filterByCriteria(criteria, minValue, maxValue, blobs);
+ *
+ * The following criteria are currently supported.
+ *
+ * ColorBlobLocatorProcessor.BlobCriteria.BY_CONTOUR_AREA
+ * A Blob's area is the number of pixels contained within the Contour. Filter out any
+ * that are too big or small. Start with a large range and then refine the range based
+ * on the likely size of the desired object in the viewfinder.
+ *
+ * ColorBlobLocatorProcessor.BlobCriteria.BY_DENSITY
+ * A blob's density is an indication of how "full" the contour is.
+ * If you put a rubber band around the contour you would get the "Convex Hull" of the
+ * contour. The density is the ratio of Contour-area to Convex Hull-area.
+ *
+ * ColorBlobLocatorProcessor.BlobCriteria.BY_ASPECT_RATIO
+ * A blob's Aspect ratio is the ratio of boxFit long side to short side.
+ * A perfect Square has an aspect ratio of 1. All others are > 1
+ *
+ * ColorBlobLocatorProcessor.BlobCriteria.BY_ARC_LENGTH
+ * A blob's arc length is the perimeter of the blob.
+ * This can be used in conjunction with an area filter to detect oddly shaped blobs.
+ *
+ * ColorBlobLocatorProcessor.BlobCriteria.BY_CIRCULARITY
+ * A blob's circularity is how circular it is based on the known area and arc length.
+ * A perfect circle has a circularity of 1. All others are < 1
+ */
+ ColorBlobLocatorProcessor.Util.filterByCriteria(
+ ColorBlobLocatorProcessor.BlobCriteria.BY_CONTOUR_AREA,
+ 50, 20000, blobs); // filter out very small blobs.
+
+ ColorBlobLocatorProcessor.Util.filterByCriteria(
+ ColorBlobLocatorProcessor.BlobCriteria.BY_CIRCULARITY,
+ 0.6, 1, blobs); /* filter out non-circular blobs.
+ * NOTE: You may want to adjust the minimum value depending on your use case.
+ * Circularity values will be affected by shadows, and will therefore vary based
+ * on the location of the camera on your robot and venue lighting. It is strongly
+ * encouraged to test your vision on the competition field if your event allows
+ * sensor calibration time.
+ */
+
+ /*
+ * The list of Blobs can be sorted using the same Blob attributes as listed above.
+ * No more than one sort call should be made. Sorting can use ascending or descending order.
+ * Here is an example.:
+ * ColorBlobLocatorProcessor.Util.sortByCriteria(
+ * ColorBlobLocatorProcessor.BlobCriteria.BY_CONTOUR_AREA, SortOrder.DESCENDING, blobs);
+ */
+
+ telemetry.addLine("Circularity Radius Center");
+
+ // Display the Blob's circularity, and the size (radius) and center location of its circleFit.
+ for (ColorBlobLocatorProcessor.Blob b : blobs) {
+
+ Circle circleFit = b.getCircle();
+ telemetry.addLine(String.format("%5.3f %3d (%3d,%3d)",
+ b.getCircularity(), (int) circleFit.getRadius(), (int) circleFit.getX(), (int) circleFit.getY()));
+ }
+
+ telemetry.update();
+ sleep(100); // Match the telemetry update interval.
+ }
+ }
+}
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVisionColorLocator_Rectangle.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVisionColorLocator_Rectangle.java
new file mode 100644
index 0000000..5681f71
--- /dev/null
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVisionColorLocator_Rectangle.java
@@ -0,0 +1,218 @@
+/*
+ * Copyright (c) 2024 Phil Malone
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+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;
+import com.qualcomm.robotcore.util.SortOrder;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
+import org.firstinspires.ftc.vision.VisionPortal;
+import org.firstinspires.ftc.vision.opencv.ColorBlobLocatorProcessor;
+import org.firstinspires.ftc.vision.opencv.ColorRange;
+import org.firstinspires.ftc.vision.opencv.ImageRegion;
+import org.opencv.core.RotatedRect;
+
+import java.util.List;
+
+/*
+ * This OpMode illustrates how to use a video source (camera) to locate specifically colored regions
+ *
+ * Unlike a "color sensor" which determines the color of nearby object, this "color locator"
+ * will search the Region Of Interest (ROI) in a camera image, and find any "blobs" of color that
+ * match the requested color range. These blobs can be further filtered and sorted to find the one
+ * most likely to be the item the user is looking for.
+ *
+ * To perform this function, a VisionPortal runs a ColorBlobLocatorProcessor process.
+ * The ColorBlobLocatorProcessor (CBLP) process is created first, and then the VisionPortal is built.
+ * The (CBLP) analyses the ROI and locates pixels that match the ColorRange to form a "mask".
+ * The matching pixels are then collected into contiguous "blobs" of pixels.
+ * The outer boundaries of these blobs are called its "contour". For each blob, the process then
+ * creates the smallest possible rectangle "boxFit" that will fully encase the contour. The user can
+ * then call getBlobs() to retrieve the list of Blobs, where each contains the contour and the boxFit.
+ * Note: The default sort order for Blobs is ContourArea, in descending order, so the biggest
+ * contours are listed first.
+ *
+ * A colored boxFit rectangle is drawn on the camera preview to show the location of each Blob
+ * The original Blob contour can also be added to the preview.
+ * This is helpful when configuring the ColorBlobLocatorProcessor parameters.
+ *
+ * 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: Color-Locator (Rectangle)", group = "Concept")
+public class ConceptVisionColorLocator_Rectangle extends LinearOpMode
+{
+ @Override
+ public void runOpMode()
+ {
+ /* Build a "Color Locator" vision processor based on the ColorBlobLocatorProcessor class.
+ * - Specify the color range you are looking for. Use a predefined color, or create your own
+ *
+ * .setTargetColorRange(ColorRange.BLUE) // use a predefined color match
+ * Available colors are: RED, BLUE YELLOW GREEN ARTIFACT_GREEN and ARTIFACT_PURPLE
+ * .setTargetColorRange(new ColorRange(ColorSpace.YCrCb, // or define your own color match
+ * new Scalar( 32, 176, 0),
+ * new Scalar(255, 255, 132)))
+ *
+ * - Focus the color locator by defining a RegionOfInterest (ROI) which you want to search.
+ * This can be the entire frame, or a sub-region defined using:
+ * 1) standard image coordinates or 2) a normalized +/- 1.0 coordinate system.
+ * Use one form of the ImageRegion class to define the ROI.
+ * ImageRegion.entireFrame()
+ * ImageRegion.asImageCoordinates(50, 50, 150, 150) 100x100 pixels at upper left corner
+ * ImageRegion.asUnityCenterCoordinates(-0.5, 0.5, 0.5, -0.5) 50% width/height in center
+ *
+ * - Define which contours are included.
+ * You can get ALL the contours, ignore contours that are completely inside another contour.
+ * .setContourMode(ColorBlobLocatorProcessor.ContourMode.ALL_FLATTENED_HIERARCHY)
+ * .setContourMode(ColorBlobLocatorProcessor.ContourMode.EXTERNAL_ONLY)
+ * EXTERNAL_ONLY helps to avoid bright reflection spots from breaking up solid colors.
+ *
+ * - turn the display of contours ON or OFF. Helps debugging but takes up valuable CPU time.
+ * .setDrawContours(true)
+ *
+ * - include any pre-processing of the image or mask before looking for Blobs.
+ * There are some extra processing you can include to improve the formation of blobs.
+ * Using these features requires an understanding of how they may effect the final blobs.
+ * The "pixels" argument sets the NxN kernel size.
+ * .setBlurSize(int pixels)
+ * Blurring an image helps to provide a smooth color transition between objects,
+ * and smoother contours. The higher the number, the more blurred the image becomes.
+ * Note: Even "pixels" values will be incremented to satisfy the "odd number" requirement.
+ * Blurring too much may hide smaller features. A size of 5 is good for a 320x240 image.
+ *
+ * .setErodeSize(int pixels)
+ * Erosion removes floating pixels and thin lines so that only substantive objects remain.
+ * Erosion can grow holes inside regions, and also shrink objects.
+ * "pixels" in the range of 2-4 are suitable for low res images.
+ *
+ * .setDilateSize(int pixels)
+ * Dilation makes objects and lines more visible by filling in small holes, and making
+ * filled shapes appear larger. Dilation is useful for joining broken parts of an
+ * object, such as when removing noise from an image.
+ * "pixels" in the range of 2-4 are suitable for low res images.
+ */
+ ColorBlobLocatorProcessor colorLocator = new ColorBlobLocatorProcessor.Builder()
+ .setTargetColorRange(ColorRange.ARTIFACT_PURPLE) // use a predefined color match
+ .setContourMode(ColorBlobLocatorProcessor.ContourMode.EXTERNAL_ONLY)
+ .setRoi(ImageRegion.asUnityCenterCoordinates(-0.75, 0.75, 0.75, -0.75))
+ .setDrawContours(true) // Show contours on the Stream Preview
+ .setBlurSize(5) // Smooth the transitions between different colors in image
+ .build();
+
+ /*
+ * Build a vision portal to run the Color Locator process.
+ *
+ * - Add the colorLocator process created above.
+ * - Set the desired video resolution.
+ * A high resolution will not improve this process, so choose a lower resolution that is
+ * supported by your camera. This will improve overall performance and reduce latency.
+ * - Choose your video source. This may be
+ * .setCamera(hardwareMap.get(WebcamName.class, "Webcam 1")) ..... for a webcam
+ * or
+ * .setCamera(BuiltinCameraDirection.BACK) ... for a Phone Camera
+ */
+ VisionPortal portal = new VisionPortal.Builder()
+ .addProcessor(colorLocator)
+ .setCameraResolution(new Size(320, 240))
+ .setCamera(hardwareMap.get(WebcamName.class, "Webcam 1"))
+ .build();
+
+ telemetry.setMsTransmissionInterval(100); // Speed up telemetry updates for debugging.
+ telemetry.setDisplayFormat(Telemetry.DisplayFormat.MONOSPACE);
+
+ // WARNING: To view the stream preview on the Driver Station, this code runs in INIT mode.
+ while (opModeIsActive() || opModeInInit())
+ {
+ telemetry.addData("preview on/off", "... Camera Stream\n");
+
+ // Read the current list
+ List blobs = colorLocator.getBlobs();
+
+ /*
+ * The list of Blobs can be filtered to remove unwanted Blobs.
+ * Note: All contours will be still displayed on the Stream Preview, but only those
+ * that satisfy the filter conditions will remain in the current list of "blobs".
+ * Multiple filters may be used.
+ *
+ * To perform a filter
+ * ColorBlobLocatorProcessor.Util.filterByCriteria(criteria, minValue, maxValue, blobs);
+ *
+ * The following criteria are currently supported.
+ *
+ * ColorBlobLocatorProcessor.BlobCriteria.BY_CONTOUR_AREA
+ * A Blob's area is the number of pixels contained within the Contour. Filter out any
+ * that are too big or small. Start with a large range and then refine the range
+ * based on the likely size of the desired object in the viewfinder.
+ *
+ * ColorBlobLocatorProcessor.BlobCriteria.BY_DENSITY
+ * A blob's density is an indication of how "full" the contour is.
+ * If you put a rubber band around the contour you get the "Convex Hull" of the contour.
+ * The density is the ratio of Contour-area to Convex Hull-area.
+ *
+ * ColorBlobLocatorProcessor.BlobCriteria.BY_ASPECT_RATIO
+ * A blob's Aspect ratio is the ratio of boxFit long side to short side.
+ * A perfect Square has an aspect ratio of 1. All others are > 1
+ *
+ * ColorBlobLocatorProcessor.BlobCriteria.BY_ARC_LENGTH
+ * A blob's arc length is the perimeter of the blob.
+ * This can be used in conjunction with an area filter to detect oddly shaped blobs.
+ *
+ * ColorBlobLocatorProcessor.BlobCriteria.BY_CIRCULARITY
+ * A blob's circularity is how circular it is based on the known area and arc length.
+ * A perfect circle has a circularity of 1. All others are < 1
+ */
+ ColorBlobLocatorProcessor.Util.filterByCriteria(
+ ColorBlobLocatorProcessor.BlobCriteria.BY_CONTOUR_AREA,
+ 50, 20000, blobs); // filter out very small blobs.
+
+ /*
+ * The list of Blobs can be sorted using the same Blob attributes as listed above.
+ * No more than one sort call should be made. Sorting can be ASCENDING or DESCENDING.
+ * Here is an example.:
+ * ColorBlobLocatorProcessor.Util.sortByCriteria(
+ * ColorBlobLocatorProcessor.BlobCriteria.BY_CONTOUR_AREA, SortOrder.DESCENDING, blobs);
+ */
+
+ telemetry.addLine("Ctr:(X,Y) Area Dens Aspect Arc Circ");
+
+ // Display the size (area) and center location for each Blob.
+ for(ColorBlobLocatorProcessor.Blob b : blobs)
+ {
+ RotatedRect boxFit = b.getBoxFit();
+ telemetry.addLine(String.format("(%3d,%3d) %5d %4.2f %5.2f %3d %5.3f ",
+ (int) boxFit.center.x, (int) boxFit.center.y, b.getContourArea(), b.getDensity(),
+ b.getAspectRatio(), (int) b.getArcLength(), b.getCircularity()));
+ }
+
+ telemetry.update();
+ sleep(100); // Match the telemetry update interval.
+ }
+ }
+}
\ No newline at end of file
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVisionColorSensor.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVisionColorSensor.java
new file mode 100644
index 0000000..5cce468
--- /dev/null
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVisionColorSensor.java
@@ -0,0 +1,158 @@
+/*
+ * Copyright (c) 2024 Phil Malone
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+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;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
+import org.firstinspires.ftc.vision.VisionPortal;
+import org.firstinspires.ftc.vision.opencv.ImageRegion;
+import org.firstinspires.ftc.vision.opencv.PredominantColorProcessor;
+
+/*
+ * This OpMode illustrates how to use a video source (camera) as a color sensor
+ *
+ * A "color sensor" will typically determine the color of the object that it is pointed at.
+ *
+ * This sample performs the same function, except it uses a video camera to inspect an object or scene.
+ * The user may choose to inspect all, or just a Region of Interest (ROI), of the active camera view.
+ * The user must also provide a list of "acceptable colors" (Swatches) from which the closest matching
+ * color will be selected.
+ *
+ * To perform this function, a VisionPortal runs a PredominantColorProcessor process.
+ * The PredominantColorProcessor (PCP) process is created first, and then the VisionPortal is built.
+ * The (PCP) analyses the ROI and splits the colored pixels into several color-clusters.
+ * The largest of these clusters is then considered to be the "Predominant Color"
+ * The process then matches the Predominant Color with the closest Swatch and returns that match.
+ *
+ * To aid the user, a colored rectangle is drawn on the camera preview to show the RegionOfInterest,
+ * The Predominant Color is used to paint the rectangle border, so the user can visualize the color.
+ *
+ * Tip: Connect an HDMI monitor to the Control Hub to view the Color Sensor process in real-time.
+ * Or use a screen copy utility like ScrCpy.exe to view the video remotely.
+ *
+ * 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: Vision Color-Sensor", group = "Concept")
+public class ConceptVisionColorSensor extends LinearOpMode
+{
+ @Override
+ public void runOpMode()
+ {
+ /* Build a "Color Sensor" vision processor based on the PredominantColorProcessor class.
+ *
+ * - Focus the color sensor by defining a RegionOfInterest (ROI) which you want to inspect.
+ * This can be the entire frame, or a sub-region defined using:
+ * 1) standard image coordinates or 2) a normalized +/- 1.0 coordinate system.
+ * Use one form of the ImageRegion class to define the ROI.
+ * ImageRegion.entireFrame()
+ * ImageRegion.asImageCoordinates(50, 50, 150, 150) 100x100 square at the top left corner
+ * ImageRegion.asUnityCenterCoordinates(-0.1, 0.1, 0.1, -0.1) 10% W * H centered square
+ *
+ * - Set the list of "acceptable" color swatches (matches).
+ * Only colors that you assign here will be returned.
+ * If you know the sensor will be pointing to one of a few specific colors, enter them here.
+ * Or, if the sensor may be pointed randomly, provide some additional colors that may match.
+ * Possible choices are:
+ * RED, ORANGE, YELLOW, GREEN, CYAN, BLUE, PURPLE, MAGENTA, BLACK, WHITE
+ * Note: For the 2026 season ARTIFACT_PURPLE and ARTIFACT_GREEN have been added.
+ *
+ * Note that in the example shown below, only some of the available colors are included.
+ * This will force any other colored region into one of these colors.
+ * eg: Green may be reported as YELLOW, as this may be the "closest" match.
+ */
+ PredominantColorProcessor colorSensor = new PredominantColorProcessor.Builder()
+ .setRoi(ImageRegion.asUnityCenterCoordinates(-0.1, 0.1, 0.1, -0.1))
+ .setSwatches(
+ PredominantColorProcessor.Swatch.ARTIFACT_GREEN,
+ PredominantColorProcessor.Swatch.ARTIFACT_PURPLE,
+ PredominantColorProcessor.Swatch.RED,
+ PredominantColorProcessor.Swatch.BLUE,
+ PredominantColorProcessor.Swatch.YELLOW,
+ PredominantColorProcessor.Swatch.BLACK,
+ PredominantColorProcessor.Swatch.WHITE)
+ .build();
+
+ /*
+ * Build a vision portal to run the Color Sensor process.
+ *
+ * - Add the colorSensor process created above.
+ * - Set the desired video resolution.
+ * Since a high resolution will not improve this process, choose a lower resolution
+ * supported by your camera. This will improve overall performance and reduce latency.
+ * - Choose your video source. This may be
+ * .setCamera(hardwareMap.get(WebcamName.class, "Webcam 1")) ..... for a webcam
+ * or
+ * .setCamera(BuiltinCameraDirection.BACK) ... for a Phone Camera
+ */
+ VisionPortal portal = new VisionPortal.Builder()
+ .addProcessor(colorSensor)
+ .setCameraResolution(new Size(320, 240))
+ .setCamera(hardwareMap.get(WebcamName.class, "Webcam 1"))
+ .build();
+
+ telemetry.setMsTransmissionInterval(100); // Speed up telemetry updates, for debugging.
+ telemetry.setDisplayFormat(Telemetry.DisplayFormat.MONOSPACE);
+
+ // WARNING: To view the stream preview on the Driver Station, this code runs in INIT mode.
+ while (opModeIsActive() || opModeInInit())
+ {
+ telemetry.addLine("Preview on/off: 3 dots, Camera Stream\n");
+
+ // Request the most recent color analysis. This will return the closest matching
+ // colorSwatch and the predominant color in the RGB, HSV and YCrCb color spaces.
+ // The color space values are returned as three-element int[] arrays as follows:
+ // RGB Red 0-255, Green 0-255, Blue 0-255
+ // HSV Hue 0-180, Saturation 0-255, Value 0-255
+ // YCrCb Luminance(Y) 0-255, Cr 0-255 (center 128), Cb 0-255 (center 128)
+ //
+ // Note: to take actions based on the detected color, simply use the colorSwatch or
+ // color space value in a comparison or switch. eg:
+
+ // if (result.closestSwatch == PredominantColorProcessor.Swatch.RED) {.. some code ..}
+ // or:
+ // if (result.RGB[0] > 128) {... some code ...}
+
+ PredominantColorProcessor.Result result = colorSensor.getAnalysis();
+
+ // Display the Color Sensor result.
+ telemetry.addData("Best Match", result.closestSwatch);
+ telemetry.addLine(String.format("RGB (%3d, %3d, %3d)",
+ result.RGB[0], result.RGB[1], result.RGB[2]));
+ telemetry.addLine(String.format("HSV (%3d, %3d, %3d)",
+ result.HSV[0], result.HSV[1], result.HSV[2]));
+ telemetry.addLine(String.format("YCrCb (%3d, %3d, %3d)",
+ result.YCrCb[0], result.YCrCb[1], result.YCrCb[2]));
+ telemetry.update();
+
+ sleep(20);
+ }
+ }
+}
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveByEncoder_Linear.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveByEncoder_Linear.java
new file mode 100644
index 0000000..63293d0
--- /dev/null
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveByEncoder_Linear.java
@@ -0,0 +1,187 @@
+/* Copyright (c) 2017 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.robotcontroller.external.samples;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.util.ElapsedTime;
+
+/*
+ * This OpMode illustrates the concept of driving a path based on encoder counts.
+ * The code is structured as a LinearOpMode
+ *
+ * The code REQUIRES that you DO have encoders on the wheels,
+ * otherwise you would use: RobotAutoDriveByTime;
+ *
+ * This code ALSO requires that the drive Motors have been configured such that a positive
+ * power command moves them forward, and causes the encoders to count UP.
+ *
+ * The desired path in this example is:
+ * - Drive forward for 48 inches
+ * - Spin right for 12 Inches
+ * - Drive Backward for 24 inches
+ * - Stop and close the claw.
+ *
+ * The code is written using a method called: encoderDrive(speed, leftInches, rightInches, timeoutS)
+ * that performs the actual movement.
+ * This method assumes that each movement is relative to the last stopping place.
+ * There are other ways to perform encoder based moves, but this method is probably the simplest.
+ * This code uses the RUN_TO_POSITION mode to enable the Motor controllers to generate the run profile
+ *
+ * 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
+ */
+
+@Autonomous(name="Robot: Auto Drive By Encoder", group="Robot")
+@Disabled
+public class RobotAutoDriveByEncoder_Linear extends LinearOpMode {
+
+ /* Declare OpMode members. */
+ private DcMotor leftDrive = null;
+ private DcMotor rightDrive = null;
+
+ private ElapsedTime runtime = new ElapsedTime();
+
+ // Calculate the COUNTS_PER_INCH for your specific drive train.
+ // Go to your motor vendor website to determine your motor's COUNTS_PER_MOTOR_REV
+ // For external drive gearing, set DRIVE_GEAR_REDUCTION as needed.
+ // For example, use a value of 2.0 for a 12-tooth spur gear driving a 24-tooth spur gear.
+ // This is gearing DOWN for less speed and more torque.
+ // For gearing UP, use a gear ratio less than 1.0. Note this will affect the direction of wheel rotation.
+ static final double COUNTS_PER_MOTOR_REV = 1440 ; // eg: TETRIX Motor Encoder
+ static final double DRIVE_GEAR_REDUCTION = 1.0 ; // No External Gearing.
+ static final double WHEEL_DIAMETER_INCHES = 4.0 ; // For figuring circumference
+ static final double COUNTS_PER_INCH = (COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION) /
+ (WHEEL_DIAMETER_INCHES * 3.1415);
+ static final double DRIVE_SPEED = 0.6;
+ static final double TURN_SPEED = 0.5;
+
+ @Override
+ public void runOpMode() {
+
+ // Initialize the drive system variables.
+ leftDrive = hardwareMap.get(DcMotor.class, "left_drive");
+ rightDrive = hardwareMap.get(DcMotor.class, "right_drive");
+
+ // To drive forward, most robots need the motor on one side to be reversed, because the axles point in opposite directions.
+ // When run, this OpMode should start both motors driving 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);
+
+ leftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+ rightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+
+ leftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+ rightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+
+ // Send telemetry message to indicate successful Encoder reset
+ telemetry.addData("Starting at", "%7d :%7d",
+ leftDrive.getCurrentPosition(),
+ rightDrive.getCurrentPosition());
+ telemetry.update();
+
+ // Wait for the game to start (driver presses START)
+ waitForStart();
+
+ // Step through each leg of the path,
+ // Note: Reverse movement is obtained by setting a negative distance (not speed)
+ encoderDrive(DRIVE_SPEED, 48, 48, 5.0); // S1: Forward 47 Inches with 5 Sec timeout
+ encoderDrive(TURN_SPEED, 12, -12, 4.0); // S2: Turn Right 12 Inches with 4 Sec timeout
+ encoderDrive(DRIVE_SPEED, -24, -24, 4.0); // S3: Reverse 24 Inches with 4 Sec timeout
+
+ telemetry.addData("Path", "Complete");
+ telemetry.update();
+ sleep(1000); // pause to display final telemetry message.
+ }
+
+ /*
+ * Method to perform a relative move, based on encoder counts.
+ * Encoders are not reset as the move is based on the current position.
+ * Move will stop if any of three conditions occur:
+ * 1) Move gets to the desired position
+ * 2) Move runs out of time
+ * 3) Driver stops the OpMode running.
+ */
+ public void encoderDrive(double speed,
+ double leftInches, double rightInches,
+ double timeoutS) {
+ int newLeftTarget;
+ int newRightTarget;
+
+ // Ensure that the OpMode is still active
+ if (opModeIsActive()) {
+
+ // Determine new target position, and pass to motor controller
+ newLeftTarget = leftDrive.getCurrentPosition() + (int)(leftInches * COUNTS_PER_INCH);
+ newRightTarget = rightDrive.getCurrentPosition() + (int)(rightInches * COUNTS_PER_INCH);
+ leftDrive.setTargetPosition(newLeftTarget);
+ rightDrive.setTargetPosition(newRightTarget);
+
+ // Turn On RUN_TO_POSITION
+ leftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+ rightDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+
+ // reset the timeout time and start motion.
+ runtime.reset();
+ leftDrive.setPower(Math.abs(speed));
+ rightDrive.setPower(Math.abs(speed));
+
+ // keep looping while we are still active, and there is time left, and both motors are running.
+ // Note: We use (isBusy() && isBusy()) in the loop test, which means that when EITHER motor hits
+ // its target position, the motion will stop. This is "safer" in the event that the robot will
+ // always end the motion as soon as possible.
+ // However, if you require that BOTH motors have finished their moves before the robot continues
+ // onto the next step, use (isBusy() || isBusy()) in the loop test.
+ while (opModeIsActive() &&
+ (runtime.seconds() < timeoutS) &&
+ (leftDrive.isBusy() && rightDrive.isBusy())) {
+
+ // Display it for the driver.
+ telemetry.addData("Running to", " %7d :%7d", newLeftTarget, newRightTarget);
+ telemetry.addData("Currently at", " at %7d :%7d",
+ leftDrive.getCurrentPosition(), rightDrive.getCurrentPosition());
+ telemetry.update();
+ }
+
+ // Stop all motion;
+ leftDrive.setPower(0);
+ rightDrive.setPower(0);
+
+ // Turn off RUN_TO_POSITION
+ leftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+ rightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+
+ sleep(250); // optional pause after each move.
+ }
+ }
+}
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveByGyro_Linear.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveByGyro_Linear.java
new file mode 100644
index 0000000..ab70934
--- /dev/null
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveByGyro_Linear.java
@@ -0,0 +1,429 @@
+/* Copyright (c) 2022 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.robotcontroller.external.samples;
+
+import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.IMU;
+import com.qualcomm.robotcore.util.ElapsedTime;
+import com.qualcomm.robotcore.util.Range;
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles;
+
+/*
+ * This OpMode illustrates the concept of driving an autonomous path based on Gyro (IMU) heading and encoder counts.
+ * The code is structured as a LinearOpMode
+ *
+ * The path to be followed by the robot is built from a series of drive, turn or pause steps.
+ * Each step on the path is defined by a single function call, and these can be strung together in any order.
+ *
+ * The code REQUIRES that you have encoders on the drive motors, otherwise you should use: RobotAutoDriveByTime;
+ *
+ * This code uses the Universal IMU interface so it will work with either the BNO055, or BHI260 IMU.
+ * To run as written, the Control/Expansion hub should be mounted horizontally on a flat part of the robot chassis.
+ * The REV Logo should be facing UP, and the USB port should be facing forward.
+ * If this is not the configuration of your REV Control Hub, then the code should be modified to reflect the correct orientation.
+ *
+ * This sample requires that the drive Motors have been configured with names : left_drive and right_drive.
+ * It also requires that a positive power command moves both motors forward, and causes the encoders to count UP.
+ * So please verify that both of your motors move the robot forward on the first move. If not, make the required correction.
+ * See the beginning of runOpMode() to set the FORWARD/REVERSE option for each motor.
+ *
+ * This code uses RUN_TO_POSITION mode for driving straight, and RUN_USING_ENCODER mode for turning and holding.
+ * Note: This code implements the requirement of calling setTargetPosition() at least once before switching to RUN_TO_POSITION mode.
+ *
+ * Notes:
+ *
+ * All angles are referenced to the coordinate-frame that is set whenever resetHeading() is called.
+ * In this sample, the heading is reset when the Start button is touched on the Driver Station.
+ * Note: It would be possible to reset the heading after each move, but this would accumulate steering errors.
+ *
+ * The angle of movement/rotation is assumed to be a standardized rotation around the robot Z axis,
+ * which means that a Positive rotation is Counter Clockwise, looking down on the field.
+ * This is consistent with the FTC field coordinate conventions set out in the document:
+ * https://ftc-docs.firstinspires.org/field-coordinate-system
+ *
+ * Control Approach.
+ *
+ * To reach, or maintain a required heading, this code implements a basic Proportional Controller where:
+ *
+ * Steering power = Heading Error * Proportional Gain.
+ *
+ * "Heading Error" is calculated by taking the difference between the desired heading and the actual heading,
+ * and then "normalizing" it by converting it to a value in the +/- 180 degree range.
+ *
+ * "Proportional Gain" is a constant that YOU choose to set the "strength" of the steering response.
+ *
+ * Use Android Studio to Copy this Class, and Paste it into your "TeamCode" folder with a new name.
+ * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list
+ */
+
+@Autonomous(name="Robot: Auto Drive By Gyro", group="Robot")
+@Disabled
+public class RobotAutoDriveByGyro_Linear extends LinearOpMode {
+
+ /* Declare OpMode members. */
+ private DcMotor leftDrive = null;
+ private DcMotor rightDrive = null;
+ private IMU imu = null; // Control/Expansion Hub IMU
+
+ private double headingError = 0;
+
+ // These variable are declared here (as class members) so they can be updated in various methods,
+ // but still be displayed by sendTelemetry()
+ private double targetHeading = 0;
+ private double driveSpeed = 0;
+ private double turnSpeed = 0;
+ private double leftSpeed = 0;
+ private double rightSpeed = 0;
+ private int leftTarget = 0;
+ private int rightTarget = 0;
+
+ // Calculate the COUNTS_PER_INCH for your specific drive train.
+ // Go to your motor vendor website to determine your motor's COUNTS_PER_MOTOR_REV
+ // For external drive gearing, set DRIVE_GEAR_REDUCTION as needed.
+ // For example, use a value of 2.0 for a 12-tooth spur gear driving a 24-tooth spur gear.
+ // This is gearing DOWN for less speed and more torque.
+ // For gearing UP, use a gear ratio less than 1.0. Note this will affect the direction of wheel rotation.
+ static final double COUNTS_PER_MOTOR_REV = 537.7 ; // eg: GoBILDA 312 RPM Yellow Jacket
+ static final double DRIVE_GEAR_REDUCTION = 1.0 ; // No External Gearing.
+ static final double WHEEL_DIAMETER_INCHES = 4.0 ; // For figuring circumference
+ static final double COUNTS_PER_INCH = (COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION) /
+ (WHEEL_DIAMETER_INCHES * 3.1415);
+
+ // These constants define the desired driving/control characteristics
+ // They can/should be tweaked to suit the specific robot drive train.
+ static final double DRIVE_SPEED = 0.4; // Max driving speed for better distance accuracy.
+ static final double TURN_SPEED = 0.2; // Max turn speed to limit turn rate.
+ static final double HEADING_THRESHOLD = 1.0 ; // How close must the heading get to the target before moving to next step.
+ // Requiring more accuracy (a smaller number) will often make the turn take longer to get into the final position.
+ // Define the Proportional control coefficient (or GAIN) for "heading control".
+ // We define one value when Turning (larger errors), and the other is used when Driving straight (smaller errors).
+ // Increase these numbers if the heading does not correct strongly enough (eg: a heavy robot or using tracks)
+ // Decrease these numbers if the heading does not settle on the correct value (eg: very agile robot with omni wheels)
+ static final double P_TURN_GAIN = 0.02; // Larger is more responsive, but also less stable.
+ static final double P_DRIVE_GAIN = 0.03; // Larger is more responsive, but also less stable.
+
+
+ @Override
+ public void runOpMode() {
+
+ // Initialize the drive system variables.
+ leftDrive = hardwareMap.get(DcMotor.class, "left_drive");
+ rightDrive = hardwareMap.get(DcMotor.class, "right_drive");
+
+ // To drive forward, most robots need the motor on one side to be reversed, because the axles point in opposite directions.
+ // When run, this OpMode should start both motors driving 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);
+
+ /* The next two lines define Hub orientation.
+ * The Default Orientation (shown) is when a hub is mounted horizontally with the printed logo pointing UP and the USB port pointing FORWARD.
+ *
+ * To Do: EDIT these two lines to match YOUR mounting configuration.
+ */
+ RevHubOrientationOnRobot.LogoFacingDirection logoDirection = RevHubOrientationOnRobot.LogoFacingDirection.UP;
+ RevHubOrientationOnRobot.UsbFacingDirection usbDirection = RevHubOrientationOnRobot.UsbFacingDirection.FORWARD;
+ RevHubOrientationOnRobot orientationOnRobot = new RevHubOrientationOnRobot(logoDirection, usbDirection);
+
+ // Now initialize the IMU with this mounting orientation
+ // This sample expects the IMU to be in a REV Hub and named "imu".
+ imu = hardwareMap.get(IMU.class, "imu");
+ imu.initialize(new IMU.Parameters(orientationOnRobot));
+
+ // Ensure the robot is stationary. Reset the encoders and set the motors to BRAKE mode
+ leftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+ rightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+ leftDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+ rightDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+
+ // Wait for the game to start (Display Gyro value while waiting)
+ while (opModeInInit()) {
+ telemetry.addData(">", "Robot Heading = %4.0f", getHeading());
+ telemetry.update();
+ }
+
+ // Set the encoders for closed loop speed control, and reset the heading.
+ leftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+ rightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+ imu.resetYaw();
+
+ // Step through each leg of the path,
+ // Notes: Reverse movement is obtained by setting a negative distance (not speed)
+ // holdHeading() is used after turns to let the heading stabilize
+ // Add a sleep(2000) after any step to keep the telemetry data visible for review
+
+ driveStraight(DRIVE_SPEED, 24.0, 0.0); // Drive Forward 24"
+ turnToHeading( TURN_SPEED, -45.0); // Turn CW to -45 Degrees
+ holdHeading( TURN_SPEED, -45.0, 0.5); // Hold -45 Deg heading for a 1/2 second
+
+ driveStraight(DRIVE_SPEED, 17.0, -45.0); // Drive Forward 17" at -45 degrees (12"x and 12"y)
+ turnToHeading( TURN_SPEED, 45.0); // Turn CCW to 45 Degrees
+ holdHeading( TURN_SPEED, 45.0, 0.5); // Hold 45 Deg heading for a 1/2 second
+
+ driveStraight(DRIVE_SPEED, 17.0, 45.0); // Drive Forward 17" at 45 degrees (-12"x and 12"y)
+ turnToHeading( TURN_SPEED, 0.0); // Turn CW to 0 Degrees
+ holdHeading( TURN_SPEED, 0.0, 1.0); // Hold 0 Deg heading for 1 second
+
+ driveStraight(DRIVE_SPEED,-48.0, 0.0); // Drive in Reverse 48" (should return to approx. staring position)
+
+ telemetry.addData("Path", "Complete");
+ telemetry.update();
+ sleep(1000); // Pause to display last telemetry message.
+ }
+
+ /*
+ * ====================================================================================================
+ * Driving "Helper" functions are below this line.
+ * These provide the high and low level methods that handle driving straight and turning.
+ * ====================================================================================================
+ */
+
+ // ********** HIGH Level driving functions. ********************
+
+ /**
+ * Drive in a straight line, on a fixed compass heading (angle), based on encoder counts.
+ * Move will stop if either of these conditions occur:
+ * 1) Move gets to the desired position
+ * 2) Driver stops the OpMode running.
+ *
+ * @param maxDriveSpeed MAX Speed for forward/rev motion (range 0 to +1.0) .
+ * @param distance Distance (in inches) to move from current position. Negative distance means move backward.
+ * @param heading Absolute Heading Angle (in Degrees) relative to last gyro reset.
+ * 0 = fwd. +ve is CCW from fwd. -ve is CW from forward.
+ * If a relative angle is required, add/subtract from the current robotHeading.
+ */
+ public void driveStraight(double maxDriveSpeed,
+ double distance,
+ double heading) {
+
+ // Ensure that the OpMode is still active
+ if (opModeIsActive()) {
+
+ // Determine new target position, and pass to motor controller
+ int moveCounts = (int)(distance * COUNTS_PER_INCH);
+ leftTarget = leftDrive.getCurrentPosition() + moveCounts;
+ rightTarget = rightDrive.getCurrentPosition() + moveCounts;
+
+ // Set Target FIRST, then turn on RUN_TO_POSITION
+ leftDrive.setTargetPosition(leftTarget);
+ rightDrive.setTargetPosition(rightTarget);
+
+ leftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+ rightDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+
+ // Set the required driving speed (must be positive for RUN_TO_POSITION)
+ // Start driving straight, and then enter the control loop
+ maxDriveSpeed = Math.abs(maxDriveSpeed);
+ moveRobot(maxDriveSpeed, 0);
+
+ // keep looping while we are still active, and BOTH motors are running.
+ while (opModeIsActive() &&
+ (leftDrive.isBusy() && rightDrive.isBusy())) {
+
+ // Determine required steering to keep on heading
+ turnSpeed = getSteeringCorrection(heading, P_DRIVE_GAIN);
+
+ // if driving in reverse, the motor correction also needs to be reversed
+ if (distance < 0)
+ turnSpeed *= -1.0;
+
+ // Apply the turning correction to the current driving speed.
+ moveRobot(driveSpeed, turnSpeed);
+
+ // Display drive status for the driver.
+ sendTelemetry(true);
+ }
+
+ // Stop all motion & Turn off RUN_TO_POSITION
+ moveRobot(0, 0);
+ leftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+ rightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+ }
+ }
+
+ /**
+ * Spin on the central axis to point in a new direction.
+ *
+ * Move will stop if either of these conditions occur:
+ *
+ * 1) Move gets to the heading (angle)
+ *
+ * 2) Driver stops the OpMode running.
+ *
+ * @param maxTurnSpeed Desired MAX speed of turn. (range 0 to +1.0)
+ * @param heading Absolute Heading Angle (in Degrees) relative to last gyro reset.
+ * 0 = fwd. +ve is CCW from fwd. -ve is CW from forward.
+ * If a relative angle is required, add/subtract from current heading.
+ */
+ public void turnToHeading(double maxTurnSpeed, double heading) {
+
+ // Run getSteeringCorrection() once to pre-calculate the current error
+ getSteeringCorrection(heading, P_DRIVE_GAIN);
+
+ // keep looping while we are still active, and not on heading.
+ while (opModeIsActive() && (Math.abs(headingError) > HEADING_THRESHOLD)) {
+
+ // Determine required steering to keep on heading
+ turnSpeed = getSteeringCorrection(heading, P_TURN_GAIN);
+
+ // Clip the speed to the maximum permitted value.
+ turnSpeed = Range.clip(turnSpeed, -maxTurnSpeed, maxTurnSpeed);
+
+ // Pivot in place by applying the turning correction
+ moveRobot(0, turnSpeed);
+
+ // Display drive status for the driver.
+ sendTelemetry(false);
+ }
+
+ // Stop all motion;
+ moveRobot(0, 0);
+ }
+
+ /**
+ * Obtain & hold a heading for a finite amount of time
+ *
+ * Move will stop once the requested time has elapsed
+ *
+ * This function is useful for giving the robot a moment to stabilize its heading between movements.
+ *
+ * @param maxTurnSpeed Maximum differential turn speed (range 0 to +1.0)
+ * @param heading Absolute Heading Angle (in Degrees) relative to last gyro reset.
+ * 0 = fwd. +ve is CCW from fwd. -ve is CW from forward.
+ * If a relative angle is required, add/subtract from current heading.
+ * @param holdTime Length of time (in seconds) to hold the specified heading.
+ */
+ public void holdHeading(double maxTurnSpeed, double heading, double holdTime) {
+
+ ElapsedTime holdTimer = new ElapsedTime();
+ holdTimer.reset();
+
+ // keep looping while we have time remaining.
+ while (opModeIsActive() && (holdTimer.time() < holdTime)) {
+ // Determine required steering to keep on heading
+ turnSpeed = getSteeringCorrection(heading, P_TURN_GAIN);
+
+ // Clip the speed to the maximum permitted value.
+ turnSpeed = Range.clip(turnSpeed, -maxTurnSpeed, maxTurnSpeed);
+
+ // Pivot in place by applying the turning correction
+ moveRobot(0, turnSpeed);
+
+ // Display drive status for the driver.
+ sendTelemetry(false);
+ }
+
+ // Stop all motion;
+ moveRobot(0, 0);
+ }
+
+ // ********** LOW Level driving functions. ********************
+
+ /**
+ * Use a Proportional Controller to determine how much steering correction is required.
+ *
+ * @param desiredHeading The desired absolute heading (relative to last heading reset)
+ * @param proportionalGain Gain factor applied to heading error to obtain turning power.
+ * @return Turning power needed to get to required heading.
+ */
+ public double getSteeringCorrection(double desiredHeading, double proportionalGain) {
+ targetHeading = desiredHeading; // Save for telemetry
+
+ // Determine the heading current error
+ headingError = targetHeading - getHeading();
+
+ // Normalize the error to be within +/- 180 degrees
+ while (headingError > 180) headingError -= 360;
+ while (headingError <= -180) headingError += 360;
+
+ // Multiply the error by the gain to determine the required steering correction/ Limit the result to +/- 1.0
+ return Range.clip(headingError * proportionalGain, -1, 1);
+ }
+
+ /**
+ * Take separate drive (fwd/rev) and turn (right/left) requests,
+ * combines them, and applies the appropriate speed commands to the left and right wheel motors.
+ * @param drive forward motor speed
+ * @param turn clockwise turning motor speed.
+ */
+ public void moveRobot(double drive, double turn) {
+ driveSpeed = drive; // save this value as a class member so it can be used by telemetry.
+ turnSpeed = turn; // save this value as a class member so it can be used by telemetry.
+
+ leftSpeed = drive - turn;
+ rightSpeed = drive + turn;
+
+ // Scale speeds down if either one exceeds +/- 1.0;
+ double max = Math.max(Math.abs(leftSpeed), Math.abs(rightSpeed));
+ if (max > 1.0)
+ {
+ leftSpeed /= max;
+ rightSpeed /= max;
+ }
+
+ leftDrive.setPower(leftSpeed);
+ rightDrive.setPower(rightSpeed);
+ }
+
+ /**
+ * Display the various control parameters while driving
+ *
+ * @param straight Set to true if we are driving straight, and the encoder positions should be included in the telemetry.
+ */
+ private void sendTelemetry(boolean straight) {
+
+ if (straight) {
+ telemetry.addData("Motion", "Drive Straight");
+ telemetry.addData("Target Pos L:R", "%7d:%7d", leftTarget, rightTarget);
+ telemetry.addData("Actual Pos L:R", "%7d:%7d", leftDrive.getCurrentPosition(),
+ rightDrive.getCurrentPosition());
+ } else {
+ telemetry.addData("Motion", "Turning");
+ }
+
+ telemetry.addData("Heading- Target : Current", "%5.2f : %5.0f", targetHeading, getHeading());
+ telemetry.addData("Error : Steer Pwr", "%5.1f : %5.1f", headingError, turnSpeed);
+ telemetry.addData("Wheel Speeds L : R", "%5.2f : %5.2f", leftSpeed, rightSpeed);
+ telemetry.update();
+ }
+
+ /**
+ * read the Robot heading directly from the IMU (in degrees)
+ */
+ public double getHeading() {
+ YawPitchRollAngles orientation = imu.getRobotYawPitchRollAngles();
+ return orientation.getYaw(AngleUnit.DEGREES);
+ }
+}
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveByTime_Linear.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveByTime_Linear.java
new file mode 100644
index 0000000..a714748
--- /dev/null
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveByTime_Linear.java
@@ -0,0 +1,128 @@
+/* Copyright (c) 2017 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.robotcontroller.external.samples;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.util.ElapsedTime;
+
+/*
+ * This OpMode illustrates the concept of driving a path based on time.
+ * The code is structured as a LinearOpMode
+ *
+ * The code assumes that you do NOT have encoders on the wheels,
+ * otherwise you would use: RobotAutoDriveByEncoder;
+ *
+ * The desired path in this example is:
+ * - Drive forward for 3 seconds
+ * - Spin right for 1.3 seconds
+ * - Drive Backward for 1 Second
+ *
+ * The code is written in a simple form with no optimizations.
+ * However, there are several ways that this type of sequence could be streamlined,
+ *
+ * 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
+ */
+
+@Autonomous(name="Robot: Auto Drive By Time", group="Robot")
+@Disabled
+public class RobotAutoDriveByTime_Linear extends LinearOpMode {
+
+ /* Declare OpMode members. */
+ private DcMotor leftDrive = null;
+ private DcMotor rightDrive = null;
+
+ private ElapsedTime runtime = new ElapsedTime();
+
+
+ static final double FORWARD_SPEED = 0.6;
+ static final double TURN_SPEED = 0.5;
+
+ @Override
+ public void runOpMode() {
+
+ // Initialize the drive system variables.
+ leftDrive = hardwareMap.get(DcMotor.class, "left_drive");
+ rightDrive = hardwareMap.get(DcMotor.class, "right_drive");
+
+ // To drive forward, most robots need the motor on one side to be reversed, because the axles point in opposite directions.
+ // When run, this OpMode should start both motors driving 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);
+
+ // Send telemetry message to signify robot waiting;
+ telemetry.addData("Status", "Ready to run"); //
+ telemetry.update();
+
+ // Wait for the game to start (driver presses START)
+ waitForStart();
+
+ // Step through each leg of the path, ensuring that the OpMode has not been stopped along the way.
+
+ // Step 1: Drive forward for 3 seconds
+ leftDrive.setPower(FORWARD_SPEED);
+ rightDrive.setPower(FORWARD_SPEED);
+ runtime.reset();
+ while (opModeIsActive() && (runtime.seconds() < 3.0)) {
+ telemetry.addData("Path", "Leg 1: %4.1f S Elapsed", runtime.seconds());
+ telemetry.update();
+ }
+
+ // Step 2: Spin right for 1.3 seconds
+ leftDrive.setPower(TURN_SPEED);
+ rightDrive.setPower(-TURN_SPEED);
+ runtime.reset();
+ while (opModeIsActive() && (runtime.seconds() < 1.3)) {
+ telemetry.addData("Path", "Leg 2: %4.1f S Elapsed", runtime.seconds());
+ telemetry.update();
+ }
+
+ // Step 3: Drive Backward for 1 Second
+ leftDrive.setPower(-FORWARD_SPEED);
+ rightDrive.setPower(-FORWARD_SPEED);
+ runtime.reset();
+ while (opModeIsActive() && (runtime.seconds() < 1.0)) {
+ telemetry.addData("Path", "Leg 3: %4.1f S Elapsed", runtime.seconds());
+ telemetry.update();
+ }
+
+ // Step 4: Stop
+ leftDrive.setPower(0);
+ rightDrive.setPower(0);
+
+ telemetry.addData("Path", "Complete");
+ telemetry.update();
+ sleep(1000);
+ }
+}
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveToAprilTagOmni.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveToAprilTagOmni.java
new file mode 100644
index 0000000..4b777e2
--- /dev/null
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveToAprilTagOmni.java
@@ -0,0 +1,321 @@
+/* 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.
+ */
+
+package org.firstinspires.ftc.robotcontroller.external.samples;
+
+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.DcMotor;
+import com.qualcomm.robotcore.util.Range;
+import org.firstinspires.ftc.robotcore.external.hardware.camera.BuiltinCameraDirection;
+import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
+import org.firstinspires.ftc.robotcore.external.hardware.camera.controls.ExposureControl;
+import org.firstinspires.ftc.robotcore.external.hardware.camera.controls.GainControl;
+import org.firstinspires.ftc.vision.VisionPortal;
+import org.firstinspires.ftc.vision.apriltag.AprilTagDetection;
+import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor;
+
+import java.util.List;
+import java.util.concurrent.TimeUnit;
+
+/*
+ * This OpMode illustrates using a camera to locate and drive towards a specific AprilTag.
+ * The code assumes a Holonomic (Mecanum or X Drive) Robot.
+ *
+ * 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
+ *
+ * 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
+ *
+ * The drive goal is to rotate to keep the Tag centered in the camera, while strafing to be directly in front of the tag, and
+ * driving towards the tag to achieve the desired distance.
+ * To reduce any motion blur (which will interrupt the detection process) the Camera exposure is reduced to a very low value (5mS)
+ * You can determine the best Exposure and Gain values by using the ConceptAprilTagOptimizeExposure OpMode in this Samples folder.
+ *
+ * The code assumes a Robot Configuration with motors named: front_left_drive and front_right_drive, back_left_drive and back_right_drive.
+ * The motor directions must be set so a positive power goes forward on all wheels.
+ * This sample assumes that the current game AprilTag Library (usually for the current season) is being loaded by default,
+ * so you should choose to approach a valid tag ID.
+ *
+ * Under manual control, the left stick will move forward/back & left/right. The right stick will rotate the robot.
+ * Manually drive the robot until it displays Target data on the Driver Station.
+ *
+ * Press and hold the *Left Bumper* to enable the automatic "Drive to target" mode.
+ * Release the Left Bumper to return to manual driving mode.
+ *
+ * Under "Drive To Target" mode, the robot has three goals:
+ * 1) Turn the robot to always keep the Tag centered on the camera frame. (Use the Target Bearing to turn the robot.)
+ * 2) Strafe the robot towards the centerline of the Tag, so it approaches directly in front of the tag. (Use the Target Yaw to strafe the robot)
+ * 3) Drive towards the Tag to get to the desired distance. (Use Tag Range to drive the robot forward/backward)
+ *
+ * Use DESIRED_DISTANCE to set how close you want the robot to get to the target.
+ * Speed and Turn sensitivity can be adjusted using the SPEED_GAIN, STRAFE_GAIN and TURN_GAIN constants.
+ *
+ * Use Android Studio to Copy this Class, and Paste it into the TeamCode/src/main/java/org/firstinspires/ftc/teamcode folder.
+ * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list.
+ *
+ */
+
+@TeleOp(name="Omni Drive To AprilTag", group = "Concept")
+@Disabled
+public class RobotAutoDriveToAprilTagOmni extends LinearOpMode
+{
+ // Adjust these numbers to suit your robot.
+ final double DESIRED_DISTANCE = 12.0; // this is how close the camera should get to the target (inches)
+
+ // Set the GAIN constants to control the relationship between the measured position error, and how much power is
+ // applied to the drive motors to correct the error.
+ // Drive = Error * Gain Make these values smaller for smoother control, or larger for a more aggressive response.
+ final double SPEED_GAIN = 0.02 ; // Forward Speed Control "Gain". e.g. Ramp up to 50% power at a 25 inch error. (0.50 / 25.0)
+ final double STRAFE_GAIN = 0.015 ; // Strafe Speed Control "Gain". e.g. Ramp up to 37% power at a 25 degree Yaw error. (0.375 / 25.0)
+ final double TURN_GAIN = 0.01 ; // Turn Control "Gain". e.g. Ramp up to 25% power at a 25 degree error. (0.25 / 25.0)
+
+ final double MAX_AUTO_SPEED = 0.5; // Clip the approach speed to this max value (adjust for your robot)
+ final double MAX_AUTO_STRAFE= 0.5; // Clip the strafing speed to this max value (adjust for your robot)
+ final double MAX_AUTO_TURN = 0.3; // Clip the turn speed to this max value (adjust for your robot)
+
+ private DcMotor frontLeftDrive = null; // Used to control the left front drive wheel
+ private DcMotor frontRightDrive = null; // Used to control the right front drive wheel
+ private DcMotor backLeftDrive = null; // Used to control the left back drive wheel
+ private DcMotor backRightDrive = null; // Used to control the right back drive wheel
+
+ private static final boolean USE_WEBCAM = true; // Set true to use a webcam, or false for a phone camera
+ private static final int DESIRED_TAG_ID = -1; // Choose the tag you want to approach or set to -1 for ANY tag.
+ private VisionPortal visionPortal; // Used to manage the video source.
+ private AprilTagProcessor aprilTag; // Used for managing the AprilTag detection process.
+ private AprilTagDetection desiredTag = null; // Used to hold the data for a detected AprilTag
+
+ @Override public void runOpMode()
+ {
+ boolean targetFound = false; // Set to true when an AprilTag target is detected
+ double drive = 0; // Desired forward power/speed (-1 to +1)
+ double strafe = 0; // Desired strafe power/speed (-1 to +1)
+ double turn = 0; // Desired turning power/speed (-1 to +1)
+
+ // Initialize the Apriltag Detection process
+ initAprilTag();
+
+ // Initialize the hardware variables. Note that the strings used here as parameters
+ // to 'get' must match the names assigned during the robot configuration.
+ // step (using the FTC Robot Controller app on the phone).
+ frontLeftDrive = hardwareMap.get(DcMotor.class, "front_left_drive");
+ frontRightDrive = hardwareMap.get(DcMotor.class, "front_right_drive");
+ backLeftDrive = hardwareMap.get(DcMotor.class, "back_left_drive");
+ backRightDrive = hardwareMap.get(DcMotor.class, "back_right_drive");
+
+ // To drive forward, most robots need the motor on one side to be reversed, because the axles point in opposite directions.
+ // When run, this OpMode should start both motors driving 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
+ frontLeftDrive.setDirection(DcMotor.Direction.REVERSE);
+ backLeftDrive.setDirection(DcMotor.Direction.REVERSE);
+ frontRightDrive.setDirection(DcMotor.Direction.FORWARD);
+ backRightDrive.setDirection(DcMotor.Direction.FORWARD);
+
+ if (USE_WEBCAM)
+ setManualExposure(6, 250); // Use low exposure time to reduce motion blur
+
+ // Wait for driver to press start
+ telemetry.addData("Camera preview on/off", "3 dots, Camera Stream");
+ telemetry.addData(">", "Touch START to start OpMode");
+ telemetry.update();
+ waitForStart();
+
+ while (opModeIsActive())
+ {
+ targetFound = false;
+ desiredTag = null;
+
+ // Step through the list of detected tags and look for a matching tag
+ List currentDetections = aprilTag.getDetections();
+ for (AprilTagDetection detection : currentDetections) {
+ // Look to see if we have size info on this tag.
+ if (detection.metadata != null) {
+ // Check to see if we want to track towards this tag.
+ if ((DESIRED_TAG_ID < 0) || (detection.id == DESIRED_TAG_ID)) {
+ // Yes, we want to use this tag.
+ targetFound = true;
+ desiredTag = detection;
+ break; // don't look any further.
+ } else {
+ // This tag is in the library, but we do not want to track it right now.
+ telemetry.addData("Skipping", "Tag ID %d is not desired", detection.id);
+ }
+ } else {
+ // This tag is NOT in the library, so we don't have enough information to track to it.
+ telemetry.addData("Unknown", "Tag ID %d is not in TagLibrary", detection.id);
+ }
+ }
+
+ // Tell the driver what we see, and what to do.
+ if (targetFound) {
+ telemetry.addData("\n>","HOLD Left-Bumper to Drive to Target\n");
+ telemetry.addData("Found", "ID %d (%s)", desiredTag.id, desiredTag.metadata.name);
+ telemetry.addData("Range", "%5.1f inches", desiredTag.ftcPose.range);
+ telemetry.addData("Bearing","%3.0f degrees", desiredTag.ftcPose.bearing);
+ telemetry.addData("Yaw","%3.0f degrees", desiredTag.ftcPose.yaw);
+ } else {
+ telemetry.addData("\n>","Drive using joysticks to find valid target\n");
+ }
+
+ // If Left Bumper is being pressed, AND we have found the desired target, Drive to target Automatically .
+ if (gamepad1.left_bumper && targetFound) {
+
+ // Determine heading, range and Yaw (tag image rotation) error so we can use them to control the robot automatically.
+ double rangeError = (desiredTag.ftcPose.range - DESIRED_DISTANCE);
+ double headingError = desiredTag.ftcPose.bearing;
+ double yawError = desiredTag.ftcPose.yaw;
+
+ // Use the speed and turn "gains" to calculate how we want the robot to move.
+ drive = Range.clip(rangeError * SPEED_GAIN, -MAX_AUTO_SPEED, MAX_AUTO_SPEED);
+ turn = Range.clip(headingError * TURN_GAIN, -MAX_AUTO_TURN, MAX_AUTO_TURN) ;
+ strafe = Range.clip(-yawError * STRAFE_GAIN, -MAX_AUTO_STRAFE, MAX_AUTO_STRAFE);
+
+ telemetry.addData("Auto","Drive %5.2f, Strafe %5.2f, Turn %5.2f ", drive, strafe, turn);
+ } else {
+
+ // drive using manual POV Joystick mode. Slow things down to make the robot more controlable.
+ drive = -gamepad1.left_stick_y / 2.0; // Reduce drive rate to 50%.
+ strafe = -gamepad1.left_stick_x / 2.0; // Reduce strafe rate to 50%.
+ turn = -gamepad1.right_stick_x / 3.0; // Reduce turn rate to 33%.
+ telemetry.addData("Manual","Drive %5.2f, Strafe %5.2f, Turn %5.2f ", drive, strafe, turn);
+ }
+ telemetry.update();
+
+ // Apply desired axes motions to the drivetrain.
+ moveRobot(drive, strafe, turn);
+ sleep(10);
+ }
+ }
+
+ /**
+ * Move robot according to desired axes motions
+ *
+ * Positive X is forward
+ *
+ * Positive Y is strafe left
+ *
+ * Positive Yaw is counter-clockwise
+ */
+ public void moveRobot(double x, double y, double yaw) {
+ // Calculate wheel powers.
+ double frontLeftPower = x - y - yaw;
+ double frontRightPower = x + y + yaw;
+ double backLeftPower = x + y - yaw;
+ double backRightPower = x - y + yaw;
+
+ // Normalize wheel powers to be less than 1.0
+ double max = Math.max(Math.abs(frontLeftPower), Math.abs(frontRightPower));
+ max = Math.max(max, Math.abs(backLeftPower));
+ max = Math.max(max, Math.abs(backRightPower));
+
+ if (max > 1.0) {
+ frontLeftPower /= max;
+ frontRightPower /= max;
+ backLeftPower /= max;
+ backRightPower /= max;
+ }
+
+ // Send powers to the wheels.
+ frontLeftDrive.setPower(frontLeftPower);
+ frontRightDrive.setPower(frontRightPower);
+ backLeftDrive.setPower(backLeftPower);
+ backRightDrive.setPower(backRightPower);
+ }
+
+ /**
+ * Initialize the AprilTag processor.
+ */
+ private void initAprilTag() {
+ // Create the AprilTag processor by using a builder.
+ aprilTag = new AprilTagProcessor.Builder().build();
+
+ // Adjust Image Decimation to trade-off detection-range for detection-rate.
+ // e.g. Some typical detection data using a Logitech C920 WebCam
+ // Decimation = 1 .. Detect 2" Tag from 10 feet away at 10 Frames per second
+ // Decimation = 2 .. Detect 2" Tag from 6 feet away at 22 Frames per second
+ // Decimation = 3 .. Detect 2" Tag from 4 feet away at 30 Frames Per Second
+ // Decimation = 3 .. Detect 5" Tag from 10 feet away at 30 Frames Per Second
+ // Note: Decimation can be changed on-the-fly to adapt during a match.
+ aprilTag.setDecimation(2);
+
+ // Create the vision portal by using a builder.
+ if (USE_WEBCAM) {
+ visionPortal = new VisionPortal.Builder()
+ .setCamera(hardwareMap.get(WebcamName.class, "Webcam 1"))
+ .addProcessor(aprilTag)
+ .build();
+ } else {
+ visionPortal = new VisionPortal.Builder()
+ .setCamera(BuiltinCameraDirection.BACK)
+ .addProcessor(aprilTag)
+ .build();
+ }
+ }
+
+ /*
+ Manually set the camera gain and exposure.
+ This can only be called AFTER calling initAprilTag(), and only works for Webcams;
+ */
+ private void setManualExposure(int exposureMS, int gain) {
+ // Wait for the camera to be open, then use the controls
+
+ if (visionPortal == null) {
+ return;
+ }
+
+ // Make sure camera is streaming before we try to set the exposure controls
+ if (visionPortal.getCameraState() != VisionPortal.CameraState.STREAMING) {
+ telemetry.addData("Camera", "Waiting");
+ telemetry.update();
+ while (!isStopRequested() && (visionPortal.getCameraState() != VisionPortal.CameraState.STREAMING)) {
+ sleep(20);
+ }
+ telemetry.addData("Camera", "Ready");
+ telemetry.update();
+ }
+
+ // Set camera controls unless we are stopping.
+ if (!isStopRequested())
+ {
+ ExposureControl exposureControl = visionPortal.getCameraControl(ExposureControl.class);
+ if (exposureControl.getMode() != ExposureControl.Mode.Manual) {
+ exposureControl.setMode(ExposureControl.Mode.Manual);
+ sleep(50);
+ }
+ exposureControl.setExposure((long)exposureMS, TimeUnit.MILLISECONDS);
+ sleep(20);
+ GainControl gainControl = visionPortal.getCameraControl(GainControl.class);
+ gainControl.setGain(gain);
+ sleep(20);
+ }
+ }
+}
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveToAprilTagTank.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveToAprilTagTank.java
new file mode 100644
index 0000000..ba3eb4f
--- /dev/null
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveToAprilTagTank.java
@@ -0,0 +1,298 @@
+/* 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.
+ */
+
+package org.firstinspires.ftc.robotcontroller.external.samples;
+
+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.DcMotor;
+import com.qualcomm.robotcore.util.Range;
+import org.firstinspires.ftc.robotcore.external.hardware.camera.BuiltinCameraDirection;
+import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
+import org.firstinspires.ftc.robotcore.external.hardware.camera.controls.ExposureControl;
+import org.firstinspires.ftc.robotcore.external.hardware.camera.controls.GainControl;
+import org.firstinspires.ftc.vision.VisionPortal;
+import org.firstinspires.ftc.vision.apriltag.AprilTagDetection;
+import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor;
+
+import java.util.List;
+import java.util.concurrent.TimeUnit;
+
+/*
+ * This OpMode illustrates using a camera to locate and drive towards a specific AprilTag.
+ * The code assumes a basic two-wheel (Tank) Robot Drivetrain
+ *
+ * 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
+ *
+ * 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
+ *
+ * The driving goal is to rotate to keep the tag centered in the camera, while driving towards the tag to achieve the desired distance.
+ * To reduce any motion blur (which will interrupt the detection process) the Camera exposure is reduced to a very low value (5mS)
+ * You can determine the best exposure and gain values by using the ConceptAprilTagOptimizeExposure OpMode in this Samples folder.
+ *
+ * The code assumes a Robot Configuration with motors named left_drive and right_drive.
+ * The motor directions must be set so a positive power goes forward on both wheels;
+ * This sample assumes that the default AprilTag Library (usually for the current season) is being loaded by default
+ * so you should choose to approach a valid tag ID.
+ *
+ * Under manual control, the left stick will move forward/back, and the right stick will rotate the robot.
+ * This is called POV Joystick mode, different than Tank Drive (where each joystick controls a wheel).
+ *
+ * Manually drive the robot until it displays Target data on the Driver Station.
+ * Press and hold the *Left Bumper* to enable the automatic "Drive to target" mode.
+ * Release the Left Bumper to return to manual driving mode.
+ *
+ * Under "Drive To Target" mode, the robot has two goals:
+ * 1) Turn the robot to always keep the Tag centered on the camera frame. (Use the Target Bearing to turn the robot.)
+ * 2) Drive towards the Tag to get to the desired distance. (Use Tag Range to drive the robot forward/backward)
+ *
+ * Use DESIRED_DISTANCE to set how close you want the robot to get to the target.
+ * Speed and Turn sensitivity can be adjusted using the SPEED_GAIN and TURN_GAIN constants.
+ *
+ * Use Android Studio to Copy this Class, and Paste it into the TeamCode/src/main/java/org/firstinspires/ftc/teamcode folder.
+ * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list.
+ *
+ */
+
+@TeleOp(name="Tank Drive To AprilTag", group = "Concept")
+@Disabled
+public class RobotAutoDriveToAprilTagTank extends LinearOpMode
+{
+ // Adjust these numbers to suit your robot.
+ final double DESIRED_DISTANCE = 12.0; // this is how close the camera should get to the target (inches)
+
+ // Set the GAIN constants to control the relationship between the measured position error, and how much power is
+ // applied to the drive motors to correct the error.
+ // Drive = Error * Gain Make these values smaller for smoother control, or larger for a more aggressive response.
+ final double SPEED_GAIN = 0.02 ; // Speed Control "Gain". e.g. Ramp up to 50% power at a 25 inch error. (0.50 / 25.0)
+ final double TURN_GAIN = 0.01 ; // Turn Control "Gain". e.g. Ramp up to 25% power at a 25 degree error. (0.25 / 25.0)
+
+ final double MAX_AUTO_SPEED = 0.5; // Clip the approach speed to this max value (adjust for your robot)
+ final double MAX_AUTO_TURN = 0.25; // Clip the turn speed to this max value (adjust for your robot)
+
+ private DcMotor leftDrive = null; // Used to control the left drive wheel
+ private DcMotor rightDrive = null; // Used to control the right drive wheel
+
+ private static final boolean USE_WEBCAM = true; // Set true to use a webcam, or false for a phone camera
+ private static final int DESIRED_TAG_ID = -1; // Choose the tag you want to approach or set to -1 for ANY tag.
+ private VisionPortal visionPortal; // Used to manage the video source.
+ private AprilTagProcessor aprilTag; // Used for managing the AprilTag detection process.
+ private AprilTagDetection desiredTag = null; // Used to hold the data for a detected AprilTag
+
+ @Override public void runOpMode()
+ {
+ boolean targetFound = false; // Set to true when an AprilTag target is detected
+ double drive = 0; // Desired forward power/speed (-1 to +1) +ve is forward
+ double turn = 0; // Desired turning power/speed (-1 to +1) +ve is CounterClockwise
+
+ // Initialize the Apriltag Detection process
+ initAprilTag();
+
+ // Initialize the hardware variables. Note that the strings used here as parameters
+ // to 'get' must match the names assigned during the robot configuration.
+ // step (using the FTC Robot Controller app on the phone).
+ leftDrive = hardwareMap.get(DcMotor.class, "left_drive");
+ rightDrive = hardwareMap.get(DcMotor.class, "right_drive");
+
+ // To drive forward, most robots need the motor on one side to be reversed because the axles point in opposite directions.
+ // When run, this OpMode should start both motors driving forward. So adjust these two lines based on your first test drive.
+ // Note: The settings here assume direct drive on left and right wheels. Single Gear Reduction or 90 Deg drives may require direction flips
+ leftDrive.setDirection(DcMotor.Direction.REVERSE);
+ rightDrive.setDirection(DcMotor.Direction.FORWARD);
+
+ if (USE_WEBCAM)
+ setManualExposure(6, 250); // Use low exposure time to reduce motion blur
+
+ // Wait for the driver to press Start
+ telemetry.addData("Camera preview on/off", "3 dots, Camera Stream");
+ telemetry.addData(">", "Touch START to start OpMode");
+ telemetry.update();
+ waitForStart();
+
+ while (opModeIsActive())
+ {
+ targetFound = false;
+ desiredTag = null;
+
+ // Step through the list of detected tags and look for a matching tag
+ List currentDetections = aprilTag.getDetections();
+ for (AprilTagDetection detection : currentDetections) {
+ // Look to see if we have size info on this tag.
+ if (detection.metadata != null) {
+ // Check to see if we want to track towards this tag.
+ if ((DESIRED_TAG_ID < 0) || (detection.id == DESIRED_TAG_ID)) {
+ // Yes, we want to use this tag.
+ targetFound = true;
+ desiredTag = detection;
+ break; // don't look any further.
+ } else {
+ // This tag is in the library, but we do not want to track it right now.
+ telemetry.addData("Skipping", "Tag ID %d is not desired", detection.id);
+ }
+ } else {
+ // This tag is NOT in the library, so we don't have enough information to track to it.
+ telemetry.addData("Unknown", "Tag ID %d is not in TagLibrary", detection.id);
+ }
+ }
+
+ // Tell the driver what we see, and what to do.
+ if (targetFound) {
+ telemetry.addData("\n>","HOLD Left-Bumper to Drive to Target\n");
+ telemetry.addData("Found", "ID %d (%s)", desiredTag.id, desiredTag.metadata.name);
+ telemetry.addData("Range", "%5.1f inches", desiredTag.ftcPose.range);
+ telemetry.addData("Bearing","%3.0f degrees", desiredTag.ftcPose.bearing);
+ } else {
+ telemetry.addData("\n>","Drive using joysticks to find valid target\n");
+ }
+
+ // If Left Bumper is being pressed, AND we have found the desired target, Drive to target Automatically .
+ if (gamepad1.left_bumper && targetFound) {
+
+ // Determine heading and range error so we can use them to control the robot automatically.
+ double rangeError = (desiredTag.ftcPose.range - DESIRED_DISTANCE);
+ double headingError = desiredTag.ftcPose.bearing;
+
+ // Use the speed and turn "gains" to calculate how we want the robot to move. Clip it to the maximum
+ drive = Range.clip(rangeError * SPEED_GAIN, -MAX_AUTO_SPEED, MAX_AUTO_SPEED);
+ turn = Range.clip(headingError * TURN_GAIN, -MAX_AUTO_TURN, MAX_AUTO_TURN) ;
+
+ telemetry.addData("Auto","Drive %5.2f, Turn %5.2f", drive, turn);
+ } else {
+
+ // drive using manual POV Joystick mode.
+ drive = -gamepad1.left_stick_y / 2.0; // Reduce drive rate to 50%.
+ turn = -gamepad1.right_stick_x / 4.0; // Reduce turn rate to 25%.
+ telemetry.addData("Manual","Drive %5.2f, Turn %5.2f", drive, turn);
+ }
+ telemetry.update();
+
+ // Apply desired axes motions to the drivetrain.
+ moveRobot(drive, turn);
+ sleep(10);
+ }
+ }
+
+ /**
+ * Move robot according to desired axes motions
+ *
+ * Positive X is forward
+ *
+ * Positive Yaw is counter-clockwise
+ */
+ public void moveRobot(double x, double yaw) {
+ // Calculate left and right wheel powers.
+ double leftPower = x - yaw;
+ double rightPower = x + yaw;
+
+ // Normalize wheel powers to be less than 1.0
+ double max = Math.max(Math.abs(leftPower), Math.abs(rightPower));
+ if (max >1.0) {
+ leftPower /= max;
+ rightPower /= max;
+ }
+
+ // Send powers to the wheels.
+ leftDrive.setPower(leftPower);
+ rightDrive.setPower(rightPower);
+ }
+
+ /**
+ * Initialize the AprilTag processor.
+ */
+ private void initAprilTag() {
+ // Create the AprilTag processor by using a builder.
+ aprilTag = new AprilTagProcessor.Builder().build();
+
+ // Adjust Image Decimation to trade-off detection-range for detection-rate.
+ // e.g. Some typical detection data using a Logitech C920 WebCam
+ // Decimation = 1 .. Detect 2" Tag from 10 feet away at 10 Frames per second
+ // Decimation = 2 .. Detect 2" Tag from 6 feet away at 22 Frames per second
+ // Decimation = 3 .. Detect 2" Tag from 4 feet away at 30 Frames Per Second
+ // Decimation = 3 .. Detect 5" Tag from 10 feet away at 30 Frames Per Second
+ // Note: Decimation can be changed on-the-fly to adapt during a match.
+ aprilTag.setDecimation(2);
+
+ // Create the vision portal by using a builder.
+ if (USE_WEBCAM) {
+ visionPortal = new VisionPortal.Builder()
+ .setCamera(hardwareMap.get(WebcamName.class, "Webcam 1"))
+ .addProcessor(aprilTag)
+ .build();
+ } else {
+ visionPortal = new VisionPortal.Builder()
+ .setCamera(BuiltinCameraDirection.BACK)
+ .addProcessor(aprilTag)
+ .build();
+ }
+ }
+
+ /*
+ Manually set the camera gain and exposure.
+ This can only be called AFTER calling initAprilTag(), and only works for Webcams;
+ */
+ private void setManualExposure(int exposureMS, int gain) {
+ // Wait for the camera to be open, then use the controls
+
+ if (visionPortal == null) {
+ return;
+ }
+
+ // Make sure camera is streaming before we try to set the exposure controls
+ if (visionPortal.getCameraState() != VisionPortal.CameraState.STREAMING) {
+ telemetry.addData("Camera", "Waiting");
+ telemetry.update();
+ while (!isStopRequested() && (visionPortal.getCameraState() != VisionPortal.CameraState.STREAMING)) {
+ sleep(20);
+ }
+ telemetry.addData("Camera", "Ready");
+ telemetry.update();
+ }
+
+ // Set camera controls unless we are stopping.
+ if (!isStopRequested())
+ {
+ ExposureControl exposureControl = visionPortal.getCameraControl(ExposureControl.class);
+ if (exposureControl.getMode() != ExposureControl.Mode.Manual) {
+ exposureControl.setMode(ExposureControl.Mode.Manual);
+ sleep(50);
+ }
+ exposureControl.setExposure((long)exposureMS, TimeUnit.MILLISECONDS);
+ sleep(20);
+ GainControl gainControl = visionPortal.getCameraControl(GainControl.class);
+ gainControl.setGain(gain);
+ sleep(20);
+ telemetry.addData("Camera", "Ready");
+ telemetry.update();
+ }
+ }
+}
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveToLine_Linear.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveToLine_Linear.java
new file mode 100644
index 0000000..780f260
--- /dev/null
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveToLine_Linear.java
@@ -0,0 +1,142 @@
+/* Copyright (c) 2017 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.robotcontroller.external.samples;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.NormalizedColorSensor;
+import com.qualcomm.robotcore.hardware.NormalizedRGBA;
+import com.qualcomm.robotcore.hardware.SwitchableLight;
+
+/*
+ * This OpMode illustrates the concept of driving up to a line and then stopping.
+ * The code is structured as a LinearOpMode
+ *
+ * The Sensor used here can be a REV Color Sensor V2 or V3. Make sure the white LED is turned on.
+ * The sensor can be plugged into any I2C port, and must be named "sensor_color" in the active configuration.
+ *
+ * Depending on the height of your color sensor, you may want to set the sensor "gain".
+ * The higher the gain, the greater the reflected light reading will be.
+ * Use the SensorColor sample in this folder to determine the minimum gain value that provides an
+ * "Alpha" reading of 1.0 when you are on top of the white line. In this sample, we use a gain of 15
+ * which works well with a Rev V2 color sensor
+ *
+ * Setting the correct WHITE_THRESHOLD value is key to stopping correctly.
+ * This should be set halfway between the bare-tile, and white-line "Alpha" values.
+ * The reflected light value can be read on the screen once the OpMode has been INIT, but before it is STARTED.
+ * Move the sensor on and off the white line and note the min and max readings.
+ * Edit this code to make WHITE_THRESHOLD halfway between the min and max.
+ *
+ * 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
+ */
+
+@Autonomous(name="Robot: Auto Drive To Line", group="Robot")
+@Disabled
+public class RobotAutoDriveToLine_Linear extends LinearOpMode {
+
+ /* Declare OpMode members. */
+ private DcMotor leftDrive = null;
+ private DcMotor rightDrive = null;
+
+ /** The variable to store a reference to our color sensor hardware object */
+ NormalizedColorSensor colorSensor;
+
+ static final double WHITE_THRESHOLD = 0.5; // spans between 0.0 - 1.0 from dark to light
+ static final double APPROACH_SPEED = 0.25;
+
+ @Override
+ public void runOpMode() {
+
+ // Initialize the drive system variables.
+ leftDrive = hardwareMap.get(DcMotor.class, "left_drive");
+ rightDrive = hardwareMap.get(DcMotor.class, "right_drive");
+
+ // To drive forward, most robots need the motor on one side to be reversed, because the axles point in opposite directions.
+ // When run, this OpMode should start both motors driving 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);
+
+ // If there are encoders connected, switch to RUN_USING_ENCODER mode for greater accuracy
+ // leftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+ // rightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+
+ // 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 necessary, turn ON the white LED (if there is no LED switch on the sensor)
+ if (colorSensor instanceof SwitchableLight) {
+ ((SwitchableLight)colorSensor).enableLight(true);
+ }
+
+ // Some sensors allow you to set your light sensor gain for optimal sensitivity...
+ // See the SensorColor sample in this folder for how to determine the optimal gain.
+ // A gain of 15 causes a Rev Color Sensor V2 to produce an Alpha value of 1.0 at about 1.5" above the floor.
+ colorSensor.setGain(15);
+
+ // Wait for driver to press START)
+ // Abort this loop is started or stopped.
+ while (opModeInInit()) {
+
+ // Send telemetry message to signify robot waiting;
+ telemetry.addData("Status", "Ready to drive to white line."); //
+
+ // Display the light level while we are waiting to start
+ getBrightness();
+ }
+
+ // Start the robot moving forward, and then begin looking for a white line.
+ leftDrive.setPower(APPROACH_SPEED);
+ rightDrive.setPower(APPROACH_SPEED);
+
+ // run until the white line is seen OR the driver presses STOP;
+ while (opModeIsActive() && (getBrightness() < WHITE_THRESHOLD)) {
+ sleep(5);
+ }
+
+ // Stop all motors
+ leftDrive.setPower(0);
+ rightDrive.setPower(0);
+ }
+
+ // to obtain reflected light, read the normalized values from the color sensor. Return the Alpha channel.
+ double getBrightness() {
+ NormalizedRGBA colors = colorSensor.getNormalizedColors();
+ telemetry.addData("Light Level (0 to 1)", "%4.2f", colors.alpha);
+ telemetry.update();
+
+ return colors.alpha;
+ }
+}
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotTeleopMecanumFieldRelativeDrive.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotTeleopMecanumFieldRelativeDrive.java
new file mode 100644
index 0000000..c5c64d0
--- /dev/null
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotTeleopMecanumFieldRelativeDrive.java
@@ -0,0 +1,163 @@
+/* Copyright (c) 2025 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.robotcontroller.external.samples;
+
+import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.IMU;
+
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+/*
+ * This OpMode illustrates how to program your robot to drive field relative. This means
+ * that the robot drives the direction you push the joystick regardless of the current orientation
+ * of the robot.
+ *
+ * This OpMode assumes that you have four mecanum wheels each on its own motor named:
+ * front_left_motor, front_right_motor, back_left_motor, back_right_motor
+ *
+ * and that the left motors are flipped such that when they turn clockwise the wheel moves backwards
+ *
+ * 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 = "Robot: Field Relative Mecanum Drive", group = "Robot")
+@Disabled
+public class RobotTeleopMecanumFieldRelativeDrive extends OpMode {
+ // This declares the four motors needed
+ DcMotor frontLeftDrive;
+ DcMotor frontRightDrive;
+ DcMotor backLeftDrive;
+ DcMotor backRightDrive;
+
+ // This declares the IMU needed to get the current direction the robot is facing
+ IMU imu;
+
+ @Override
+ public void init() {
+ frontLeftDrive = hardwareMap.get(DcMotor.class, "front_left_drive");
+ frontRightDrive = hardwareMap.get(DcMotor.class, "front_right_drive");
+ backLeftDrive = hardwareMap.get(DcMotor.class, "back_left_drive");
+ backRightDrive = hardwareMap.get(DcMotor.class, "back_right_drive");
+
+ // We set the left motors in reverse which is needed for drive trains where the left
+ // motors are opposite to the right ones.
+ backLeftDrive.setDirection(DcMotor.Direction.REVERSE);
+ frontLeftDrive.setDirection(DcMotor.Direction.REVERSE);
+
+ // This uses RUN_USING_ENCODER to be more accurate. If you don't have the encoder
+ // wires, you should remove these
+ frontLeftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+ frontRightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+ backLeftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+ backRightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+
+ imu = hardwareMap.get(IMU.class, "imu");
+ // This needs to be changed to match the orientation on your robot
+ RevHubOrientationOnRobot.LogoFacingDirection logoDirection =
+ RevHubOrientationOnRobot.LogoFacingDirection.UP;
+ RevHubOrientationOnRobot.UsbFacingDirection usbDirection =
+ RevHubOrientationOnRobot.UsbFacingDirection.FORWARD;
+
+ RevHubOrientationOnRobot orientationOnRobot = new
+ RevHubOrientationOnRobot(logoDirection, usbDirection);
+ imu.initialize(new IMU.Parameters(orientationOnRobot));
+ }
+
+ @Override
+ public void loop() {
+ telemetry.addLine("Press A to reset Yaw");
+ telemetry.addLine("Hold left bumper to drive in robot relative");
+ telemetry.addLine("The left joystick sets the robot direction");
+ telemetry.addLine("Moving the right joystick left and right turns the robot");
+
+ // If you press the A button, then you reset the Yaw to be zero from the way
+ // the robot is currently pointing
+ if (gamepad1.a) {
+ imu.resetYaw();
+ }
+ // If you press the left bumper, you get a drive from the point of view of the robot
+ // (much like driving an RC vehicle)
+ if (gamepad1.left_bumper) {
+ drive(-gamepad1.left_stick_y, gamepad1.left_stick_x, gamepad1.right_stick_x);
+ } else {
+ driveFieldRelative(-gamepad1.left_stick_y, gamepad1.left_stick_x, gamepad1.right_stick_x);
+ }
+ }
+
+ // This routine drives the robot field relative
+ private void driveFieldRelative(double forward, double right, double rotate) {
+ // First, convert direction being asked to drive to polar coordinates
+ double theta = Math.atan2(forward, right);
+ double r = Math.hypot(right, forward);
+
+ // Second, rotate angle by the angle the robot is pointing
+ theta = AngleUnit.normalizeRadians(theta -
+ imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS));
+
+ // Third, convert back to cartesian
+ double newForward = r * Math.sin(theta);
+ double newRight = r * Math.cos(theta);
+
+ // Finally, call the drive method with robot relative forward and right amounts
+ drive(newForward, newRight, rotate);
+ }
+
+ // Thanks to FTC16072 for sharing this code!!
+ public void drive(double forward, double right, double rotate) {
+ // This calculates the power needed for each wheel based on the amount of forward,
+ // strafe right, and rotate
+ double frontLeftPower = forward + right + rotate;
+ double frontRightPower = forward - right - rotate;
+ double backRightPower = forward + right - rotate;
+ double backLeftPower = forward - right + rotate;
+
+ double maxPower = 1.0;
+ double maxSpeed = 1.0; // make this slower for outreaches
+
+ // This is needed to make sure we don't pass > 1.0 to any wheel
+ // It allows us to keep all of the motors in proportion to what they should
+ // be and not get clipped
+ maxPower = Math.max(maxPower, Math.abs(frontLeftPower));
+ maxPower = Math.max(maxPower, Math.abs(frontRightPower));
+ maxPower = Math.max(maxPower, Math.abs(backRightPower));
+ maxPower = Math.max(maxPower, Math.abs(backLeftPower));
+
+ // We multiply by maxSpeed so that it can be set lower for outreaches
+ // When a young child is driving the robot, we may not want to allow full
+ // speed.
+ frontLeftDrive.setPower(maxSpeed * (frontLeftPower / maxPower));
+ frontRightDrive.setPower(maxSpeed * (frontRightPower / maxPower));
+ backLeftDrive.setPower(maxSpeed * (backLeftPower / maxPower));
+ backRightDrive.setPower(maxSpeed * (backRightPower / maxPower));
+ }
+}
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotTeleopPOV_Linear.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotTeleopPOV_Linear.java
new file mode 100644
index 0000000..af3840d
--- /dev/null
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotTeleopPOV_Linear.java
@@ -0,0 +1,159 @@
+/* Copyright (c) 2017 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.robotcontroller.external.samples;
+
+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.DcMotor;
+import com.qualcomm.robotcore.hardware.Servo;
+import com.qualcomm.robotcore.util.Range;
+
+/*
+ * This OpMode executes a POV Game style Teleop for a direct drive robot
+ * The code is structured as a LinearOpMode
+ *
+ * In this mode the left stick moves the robot FWD and back, the Right stick turns left and right.
+ * It raises and lowers the arm using the Gamepad Y and A buttons respectively.
+ * It also opens and closes the claws slowly using the left and right Bumper buttons.
+ *
+ * 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="Robot: Teleop POV", group="Robot")
+@Disabled
+public class RobotTeleopPOV_Linear extends LinearOpMode {
+
+ /* Declare OpMode members. */
+ public DcMotor leftDrive = null;
+ public DcMotor rightDrive = null;
+ public DcMotor leftArm = null;
+ public Servo leftClaw = null;
+ public Servo rightClaw = null;
+
+ double clawOffset = 0;
+
+ public static final double MID_SERVO = 0.5 ;
+ public static final double CLAW_SPEED = 0.02 ; // sets rate to move servo
+ public static final double ARM_UP_POWER = 0.45 ;
+ public static final double ARM_DOWN_POWER = -0.45 ;
+
+ @Override
+ public void runOpMode() {
+ double left;
+ double right;
+ double drive;
+ double turn;
+ double max;
+
+ // Define and Initialize Motors
+ leftDrive = hardwareMap.get(DcMotor.class, "left_drive");
+ rightDrive = hardwareMap.get(DcMotor.class, "right_drive");
+ leftArm = hardwareMap.get(DcMotor.class, "left_arm");
+
+ // 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);
+
+ // If there are encoders connected, switch to RUN_USING_ENCODER mode for greater accuracy
+ // leftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+ // rightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+
+ // Define and initialize ALL installed servos.
+ leftClaw = hardwareMap.get(Servo.class, "left_hand");
+ rightClaw = hardwareMap.get(Servo.class, "right_hand");
+ leftClaw.setPosition(MID_SERVO);
+ rightClaw.setPosition(MID_SERVO);
+
+ // Send telemetry message to signify robot waiting;
+ telemetry.addData(">", "Robot Ready. Press START."); //
+ telemetry.update();
+
+ // Wait for the game to start (driver presses START)
+ waitForStart();
+
+ // run until the end of the match (driver presses STOP)
+ while (opModeIsActive()) {
+
+ // Run wheels in POV mode (note: The joystick goes negative when pushed forward, so negate it)
+ // In this mode the Left stick moves the robot fwd and back, the Right stick turns left and right.
+ // This way it's also easy to just drive straight, or just turn.
+ drive = -gamepad1.left_stick_y;
+ turn = gamepad1.right_stick_x;
+
+ // Combine drive and turn for blended motion.
+ left = drive + turn;
+ right = drive - turn;
+
+ // Normalize the values so neither exceed +/- 1.0
+ max = Math.max(Math.abs(left), Math.abs(right));
+ if (max > 1.0)
+ {
+ left /= max;
+ right /= max;
+ }
+
+ // Output the safe vales to the motor drives.
+ leftDrive.setPower(left);
+ rightDrive.setPower(right);
+
+ // Use gamepad left & right Bumpers to open and close the claw
+ if (gamepad1.right_bumper)
+ clawOffset += CLAW_SPEED;
+ else if (gamepad1.left_bumper)
+ clawOffset -= CLAW_SPEED;
+
+ // Move both servos to new position. Assume servos are mirror image of each other.
+ clawOffset = Range.clip(clawOffset, -0.5, 0.5);
+ leftClaw.setPosition(MID_SERVO + clawOffset);
+ rightClaw.setPosition(MID_SERVO - clawOffset);
+
+ // Use gamepad buttons to move arm up (Y) and down (A)
+ if (gamepad1.y)
+ leftArm.setPower(ARM_UP_POWER);
+ else if (gamepad1.a)
+ leftArm.setPower(ARM_DOWN_POWER);
+ else
+ leftArm.setPower(0.0);
+
+ // Send telemetry message to signify robot running;
+ telemetry.addData("claw", "Offset = %.2f", clawOffset);
+ telemetry.addData("left", "%.2f", left);
+ telemetry.addData("right", "%.2f", right);
+ telemetry.update();
+
+ // Pace this loop so jaw action is reasonable speed.
+ sleep(50);
+ }
+ }
+}
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotTeleopTank_Iterative.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotTeleopTank_Iterative.java
new file mode 100644
index 0000000..a622f27
--- /dev/null
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotTeleopTank_Iterative.java
@@ -0,0 +1,160 @@
+/* Copyright (c) 2017 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.robotcontroller.external.samples;
+
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.Servo;
+import com.qualcomm.robotcore.util.Range;
+
+/*
+ * This OpMode executes a Tank Drive control TeleOp a direct drive robot
+ * The code is structured as an Iterative OpMode
+ *
+ * In this mode, the left and right joysticks control the left and right motors respectively.
+ * Pushing a joystick forward will make the attached motor drive forward.
+ * It raises and lowers the claw using the Gamepad Y and A buttons respectively.
+ * It also opens and closes the claws slowly using the left and right Bumper buttons.
+ *
+ * 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="Robot: Teleop Tank", group="Robot")
+@Disabled
+public class RobotTeleopTank_Iterative extends OpMode{
+
+ /* Declare OpMode members. */
+ public DcMotor leftDrive = null;
+ public DcMotor rightDrive = null;
+ public DcMotor leftArm = null;
+ public Servo leftClaw = null;
+ public Servo rightClaw = null;
+
+ double clawOffset = 0;
+
+ public static final double MID_SERVO = 0.5 ;
+ public static final double CLAW_SPEED = 0.02 ; // sets rate to move servo
+ public static final double ARM_UP_POWER = 0.50 ; // Run arm motor up at 50% power
+ public static final double ARM_DOWN_POWER = -0.25 ; // Run arm motor down at -25% power
+
+ /*
+ * Code to run ONCE when the driver hits INIT
+ */
+ @Override
+ public void init() {
+ // Define and Initialize Motors
+ leftDrive = hardwareMap.get(DcMotor.class, "left_drive");
+ rightDrive = hardwareMap.get(DcMotor.class, "right_drive");
+ leftArm = hardwareMap.get(DcMotor.class, "left_arm");
+
+ // To drive forward, most robots need the motor on one side to be reversed, because the axles point in opposite directions.
+ // Pushing the left and right sticks 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);
+
+ // If there are encoders connected, switch to RUN_USING_ENCODER mode for greater accuracy
+ // leftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+ // rightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+
+ // Define and initialize ALL installed servos.
+ leftClaw = hardwareMap.get(Servo.class, "left_hand");
+ rightClaw = hardwareMap.get(Servo.class, "right_hand");
+ leftClaw.setPosition(MID_SERVO);
+ rightClaw.setPosition(MID_SERVO);
+
+ // Send telemetry message to signify robot waiting;
+ telemetry.addData(">", "Robot Ready. Press START."); //
+ }
+
+ /*
+ * Code to run REPEATEDLY after the driver hits INIT, but before they hit START
+ */
+ @Override
+ public void init_loop() {
+ }
+
+ /*
+ * Code to run ONCE when the driver hits START
+ */
+ @Override
+ public void start() {
+ }
+
+ /*
+ * Code to run REPEATEDLY after the driver hits START but before they hit STOP
+ */
+ @Override
+ public void loop() {
+ double left;
+ double right;
+
+ // Run wheels in tank mode (note: The joystick goes negative when pushed forward, so negate it)
+ left = -gamepad1.left_stick_y;
+ right = -gamepad1.right_stick_y;
+
+ leftDrive.setPower(left);
+ rightDrive.setPower(right);
+
+ // Use gamepad left & right Bumpers to open and close the claw
+ if (gamepad1.right_bumper)
+ clawOffset += CLAW_SPEED;
+ else if (gamepad1.left_bumper)
+ clawOffset -= CLAW_SPEED;
+
+ // Move both servos to new position. Assume servos are mirror image of each other.
+ clawOffset = Range.clip(clawOffset, -0.5, 0.5);
+ leftClaw.setPosition(MID_SERVO + clawOffset);
+ rightClaw.setPosition(MID_SERVO - clawOffset);
+
+ // Use gamepad buttons to move the arm up (Y) and down (A)
+ if (gamepad1.y)
+ leftArm.setPower(ARM_UP_POWER);
+ else if (gamepad1.a)
+ leftArm.setPower(ARM_DOWN_POWER);
+ else
+ leftArm.setPower(0.0);
+
+ // Send telemetry message to signify robot running;
+ telemetry.addData("claw", "Offset = %.2f", clawOffset);
+ telemetry.addData("left", "%.2f", left);
+ telemetry.addData("right", "%.2f", right);
+ }
+
+ /*
+ * Code to run ONCE after the driver hits STOP
+ */
+ @Override
+ public void stop() {
+ }
+}
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SampleRevBlinkinLedDriver.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SampleRevBlinkinLedDriver.java
new file mode 100644
index 0000000..bcf5b80
--- /dev/null
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SampleRevBlinkinLedDriver.java
@@ -0,0 +1,163 @@
+/*
+ * Copyright (c) 2018 Craig MacFarlane
+ *
+ * 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 Craig MacFarlane 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.robotcontroller.external.samples;
+
+import com.qualcomm.hardware.rev.RevBlinkinLedDriver;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.robotcore.internal.system.Deadline;
+
+import java.util.concurrent.TimeUnit;
+
+/*
+ * This OpMode demonstrates use of the REV Robotics Blinkin LED Driver.
+ * AUTO mode cycles through all of the patterns.
+ * MANUAL mode allows the user to manually change patterns using the
+ * left and right bumpers of a gamepad.
+ *
+ * Configure the driver on a servo port, and name it "blinkin".
+ *
+ * Displays the first pattern upon init.
+ */
+@TeleOp(name="BlinkinExample")
+@Disabled
+public class SampleRevBlinkinLedDriver extends OpMode {
+
+ /*
+ * Change the pattern every 10 seconds in AUTO mode.
+ */
+ private final static int LED_PERIOD = 10;
+
+ /*
+ * Rate limit gamepad button presses to every 500ms.
+ */
+ private final static int GAMEPAD_LOCKOUT = 500;
+
+ RevBlinkinLedDriver blinkinLedDriver;
+ RevBlinkinLedDriver.BlinkinPattern pattern;
+
+ Telemetry.Item patternName;
+ Telemetry.Item display;
+ DisplayKind displayKind;
+ Deadline ledCycleDeadline;
+ Deadline gamepadRateLimit;
+
+ protected enum DisplayKind {
+ MANUAL,
+ AUTO
+ }
+
+ @Override
+ public void init()
+ {
+ displayKind = DisplayKind.AUTO;
+
+ blinkinLedDriver = hardwareMap.get(RevBlinkinLedDriver.class, "blinkin");
+ pattern = RevBlinkinLedDriver.BlinkinPattern.RAINBOW_RAINBOW_PALETTE;
+ blinkinLedDriver.setPattern(pattern);
+
+ display = telemetry.addData("Display Kind: ", displayKind.toString());
+ patternName = telemetry.addData("Pattern: ", pattern.toString());
+
+ ledCycleDeadline = new Deadline(LED_PERIOD, TimeUnit.SECONDS);
+ gamepadRateLimit = new Deadline(GAMEPAD_LOCKOUT, TimeUnit.MILLISECONDS);
+ }
+
+ @Override
+ public void loop()
+ {
+ handleGamepad();
+
+ if (displayKind == DisplayKind.AUTO) {
+ doAutoDisplay();
+ } else {
+ /*
+ * MANUAL mode: Nothing to do, setting the pattern as a result of a gamepad event.
+ */
+ }
+ }
+
+ /*
+ * handleGamepad
+ *
+ * Responds to a gamepad button press. Demonstrates rate limiting for
+ * button presses. If loop() is called every 10ms and and you don't rate
+ * limit, then any given button press may register as multiple button presses,
+ * which in this application is problematic.
+ *
+ * A: Manual mode, Right bumper displays the next pattern, left bumper displays the previous pattern.
+ * B: Auto mode, pattern cycles, changing every LED_PERIOD seconds.
+ */
+ protected void handleGamepad()
+ {
+ if (!gamepadRateLimit.hasExpired()) {
+ return;
+ }
+
+ if (gamepad1.a) {
+ setDisplayKind(DisplayKind.MANUAL);
+ gamepadRateLimit.reset();
+ } else if (gamepad1.b) {
+ setDisplayKind(DisplayKind.AUTO);
+ gamepadRateLimit.reset();
+ } else if ((displayKind == DisplayKind.MANUAL) && (gamepad1.left_bumper)) {
+ pattern = pattern.previous();
+ displayPattern();
+ gamepadRateLimit.reset();
+ } else if ((displayKind == DisplayKind.MANUAL) && (gamepad1.right_bumper)) {
+ pattern = pattern.next();
+ displayPattern();
+ gamepadRateLimit.reset();
+ }
+ }
+
+ protected void setDisplayKind(DisplayKind displayKind)
+ {
+ this.displayKind = displayKind;
+ display.setValue(displayKind.toString());
+ }
+
+ protected void doAutoDisplay()
+ {
+ if (ledCycleDeadline.hasExpired()) {
+ pattern = pattern.next();
+ displayPattern();
+ ledCycleDeadline.reset();
+ }
+ }
+
+ protected void displayPattern()
+ {
+ blinkinLedDriver.setPattern(pattern);
+ patternName.setValue(pattern.toString());
+ }
+}
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorAndyMarkIMUNonOrthogonal.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorAndyMarkIMUNonOrthogonal.java
new file mode 100644
index 0000000..cd678ef
--- /dev/null
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorAndyMarkIMUNonOrthogonal.java
@@ -0,0 +1,193 @@
+/* Copyright (c) 2025 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.robotcontroller.external.samples;
+
+import com.qualcomm.hardware.andymark.AndyMarkIMUOrientationOnRobot;
+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.IMU;
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+import org.firstinspires.ftc.robotcore.external.navigation.AngularVelocity;
+import org.firstinspires.ftc.robotcore.external.navigation.Orientation;
+import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles;
+
+import static com.qualcomm.hardware.andymark.AndyMarkIMUOrientationOnRobot.xyzOrientation;
+
+/*
+ * This OpMode shows how to use the AndyMark IMU sensor. It assumes that the AndyMark IMU is
+ * configured with the name "imu".
+ *
+ * The sample will display the current Yaw, Pitch and Roll of the robot.
+ *
+ * With the correct orientation parameters selected, pitch/roll/yaw should act as follows:
+ * Pitch value should INCREASE as the robot is tipped UP at the front. (Rotation about X)
+ * Roll value should INCREASE as the robot is tipped UP at the left side. (Rotation about Y)
+ * Yaw value should INCREASE as the robot is rotated Counter Clockwise. (Rotation about Z)
+ *
+ * The yaw can be reset (to zero) by pressing the Y button on the gamepad (Triangle on a PS4 controller).
+ *
+ * This specific sample DOES NOT assume that the AndyMark IMU is mounted on one of the three
+ * orthogonal planes (X/Y, X/Z or Y/Z) OR that the AndyMark IMU has only been rotated in a range of
+ * 90 degree increments.
+ *
+ * Note: if your AndyMark IMU is mounted Orthogonally (on a orthogonal surface, angled at some
+ * multiple of 90 Degrees), then you should use the simpler SensorAndyMarkIMUOrthogonal sample in
+ * this folder.
+ *
+ * But... If your AndyMark IMU is mounted Non-Orthogonally, you must specify one or more rotational
+ * angles that transform a "Default" IMU orientation into your desired orientation. That is what is
+ * illustrated here.
+ *
+ * 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.
+ *
+ * Finally, edit this OpMode to use at least one angle around an axis to orient your AndyMark IMU.
+ */
+@TeleOp(name = "Sensor: AndyMark IMU Non-Orthogonal", group = "Sensor")
+@Disabled // Comment this out to add to the OpMode list
+public class SensorAndyMarkIMUNonOrthogonal extends LinearOpMode
+{
+ // The AndyMark IMU sensor object
+ private IMU imu;
+
+ //----------------------------------------------------------------------------------------------
+ // Main logic
+ //----------------------------------------------------------------------------------------------
+
+ @Override public void runOpMode() throws InterruptedException {
+
+ // Retrieve and initialize the AndyMark IMU.
+ // This sample expects the AndyMark IMU to be named "imu".
+ imu = hardwareMap.get(IMU.class, "imu");
+
+ /* Define how the AndyMark IMU is mounted to the robot to get the correct Yaw, Pitch, and
+ * Roll values.
+ *
+ * You can apply up to three axis rotations to orient your AndyMark IMU according to how
+ * it's mounted on the robot.
+ *
+ * The starting point for these rotations is the "Default" IMU orientation, which is:
+ * 1) IMU laying flat on a horizontal surface, with the AndyMark logo facing up.
+ * 2) Rotated such that the I2C port is facing forward on the robot.
+ *
+ * The order that the rotations are performed matters, so this sample shows doing them in
+ * the order X, Y, then Z.
+ * For specifying non-orthogonal IMU mounting orientations, we must temporarily use axes
+ * defined relative to the AndyMark IMU itself, instead of the usual Robot Coordinate System
+ * axes used for the results the AndyMark IMU gives us. In the starting orientation, the
+ * AndyMark IMU axes are aligned with the Robot Coordinate System:
+ *
+ * X Axis: Starting at center of IMU, pointing out towards the side.
+ * Y Axis: Starting at center of IMU, pointing out towards I2C port.
+ * Z Axis: Starting at center of IMU, pointing up through the AndyMark logo.
+ *
+ * Positive rotation is defined by right-hand rule with thumb pointing in +ve direction on
+ * axis.
+ *
+ * Some examples.
+ *
+ * ----------------------------------------------------------------------------------------------------------------------------------
+ * Example A) Assume that the AndyMark IMU is mounted on a sloped plate at the back of the
+ * robot, with the I2C port coming out the top of the AndyMark IMU. The plate is tilted UP
+ * 60 degrees from horizontal.
+ *
+ * To get the "Default" IMU into this configuration you would just need a single rotation.
+ * 1) Rotate the AndyMark IMU +60 degrees around the X axis to tilt up the front edge.
+ * 2) No rotation around the Y or Z axes.
+ *
+ * So the X,Y,Z rotations would be 60,0,0
+ *
+ * ----------------------------------------------------------------------------------------------------------------------------------
+ * Example B) Assume that the AndyMark IMU is laying flat on the chassis, but it has been
+ * twisted 30 degrees towards the right front wheel.
+ *
+ * To get the "Default" IMU into this configuration you would just need a single rotation,
+ * but around a different axis.
+ * 1) No rotation around the X or Y axes.
+ * 2) Rotate the AndyMark IMU -30 degrees (Clockwise) around the Z axis, since a positive
+ * angle would be Counter Clockwise.
+ *
+ * So the X,Y,Z rotations would be 0,0,-30
+ *
+ * ----------------------------------------------------------------------------------------------------------------------------------
+ * Example C) Assume that the AndyMark IMU is mounted on a vertical plate on the right side
+ * of the robot, with the AndyMark logo facing out, and the AndyMark IMU rotated so that the
+ * I2C port is facing down 30 degrees towards the back wheels of the robot.
+ *
+ * To get the "Default" IMU into this configuration will require several rotations.
+ * 1) Rotate the AndyMark IMU +90 degrees around the X axis to get it standing upright with
+ * the logo pointing backwards on the robot
+ * 2) Next, rotate the AndyMark IMU +90 around the Y axis to get the logo facing to the
+ * right.
+ * 3) Finally rotate the AndyMark IMU +120 degrees around the Z axis to take the I2C port
+ * from vertical to sloping down 30 degrees and facing towards the back of the robot.
+ *
+ * So the X,Y,Z rotations would be 90,90,120
+ */
+
+ // The next three lines define the desired axis rotations.
+ // To Do: EDIT these values to match YOUR mounting configuration.
+ double xRotation = 0; // enter the desired X rotation angle here.
+ double yRotation = 0; // enter the desired Y rotation angle here.
+ double zRotation = 0; // enter the desired Z rotation angle here.
+
+ Orientation imuRotation = xyzOrientation(xRotation, yRotation, zRotation);
+
+ // Now initialize the AndyMark IMU with this mounting orientation.
+ AndyMarkIMUOrientationOnRobot orientationOnRobot = new AndyMarkIMUOrientationOnRobot(imuRotation);
+ imu.initialize(new IMU.Parameters(orientationOnRobot));
+
+ // Loop and update the dashboard.
+ while (!isStopRequested()) {
+ telemetry.addData("IMU orientation", "X=%.1f, Y=%.1f, Z=%.1f \n", xRotation, yRotation, zRotation);
+
+ // Check to see if heading reset is requested.
+ if (gamepad1.y) {
+ telemetry.addData("Yaw", "Resetting\n");
+ imu.resetYaw();
+ } else {
+ telemetry.addData("Yaw", "Press Y (triangle) on Gamepad to reset\n");
+ }
+
+ // Retrieve rotational angles and velocities.
+ YawPitchRollAngles orientation = imu.getRobotYawPitchRollAngles();
+ AngularVelocity angularVelocity = imu.getRobotAngularVelocity(AngleUnit.DEGREES);
+
+ telemetry.addData("Yaw (Z)", "%.2f Deg. (Heading)", orientation.getYaw(AngleUnit.DEGREES));
+ telemetry.addData("Pitch (X)", "%.2f Deg.", orientation.getPitch(AngleUnit.DEGREES));
+ telemetry.addData("Roll (Y)", "%.2f Deg.\n", orientation.getRoll(AngleUnit.DEGREES));
+ telemetry.addData("Yaw (Z) velocity", "%.2f Deg/Sec", angularVelocity.zRotationRate);
+ telemetry.addData("Pitch (X) velocity", "%.2f Deg/Sec", angularVelocity.xRotationRate);
+ telemetry.addData("Roll (Y) velocity", "%.2f Deg/Sec", angularVelocity.yRotationRate);
+ telemetry.update();
+ }
+ }
+}
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorAndyMarkIMUOrthogonal.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorAndyMarkIMUOrthogonal.java
new file mode 100644
index 0000000..58cee6d
--- /dev/null
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorAndyMarkIMUOrthogonal.java
@@ -0,0 +1,144 @@
+/* Copyright (c) 2025 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.robotcontroller.external.samples;
+
+import com.qualcomm.hardware.andymark.AndyMarkIMUOrientationOnRobot;
+import com.qualcomm.hardware.andymark.AndyMarkIMUOrientationOnRobot.I2cPortFacingDirection;
+import com.qualcomm.hardware.andymark.AndyMarkIMUOrientationOnRobot.LogoFacingDirection;
+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.IMU;
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+import org.firstinspires.ftc.robotcore.external.navigation.AngularVelocity;
+import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles;
+
+/*
+ * This OpMode shows how to use the AndyMark IMU sensor. It assumes that the AndyMark IMU is
+ * configured with the name "imu".
+ *
+ * The sample will display the current Yaw, Pitch and Roll of the robot.
+ *
+ * With the correct orientation parameters selected, pitch/roll/yaw should act as follows:
+ * Pitch value should INCREASE as the robot is tipped UP at the front. (Rotation about X)
+ * Roll value should INCREASE as the robot is tipped UP at the left side. (Rotation about Y)
+ * Yaw value should INCREASE as the robot is rotated Counter Clockwise. (Rotation about Z)
+ *
+ * The yaw can be reset (to zero) by pressing the Y button on the gamepad (Triangle on a PS4 controller).
+ *
+ * This specific sample assumes that the AndyMark IMU is mounted on one of the three orthogonal
+ * planes (X/Y, X/Z or Y/Z) and that the AndyMark IMU has only been rotated in a range of 90 degree
+ * increments.
+ *
+ * Note: if your AndyMark IMU is mounted on a surface angled at some non-90 Degree multiple (like
+ * 30), then you should use the SensorAndyMarkIMUNonOrthogonal sample in this folder.
+ *
+ * This "Orthogonal" requirement means that:
+ *
+ * 1) The AndyMark logo printed on the top of the AndyMark IMU can ONLY be pointing in one of six directions:
+ * FORWARD, BACKWARD, UP, DOWN, LEFT and RIGHT.
+ *
+ * 2) The I2C port can only be pointing in one of the same six directions:
+ * FORWARD, BACKWARD, UP, DOWN, LEFT and RIGHT.
+ *
+ * So, to fully define how your AndyMark IMU is mounted to the robot, you must simply specify:
+ * LogoFacingDirection
+ * I2cPortFacingDirection
+ *
+ * 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.
+ *
+ * Finally, choose the two correct parameters to define how your AndyMark IMU is mounted and edit
+ * this OpMode to use those parameters.
+ */
+@TeleOp(name = "Sensor: AndyMark IMU Orthogonal", group = "Sensor")
+@Disabled // Comment this out to add to the OpMode list
+public class SensorAndyMarkIMUOrthogonal extends LinearOpMode
+{
+ // The AndyMark IMU sensor object
+ private IMU imu;
+
+ //----------------------------------------------------------------------------------------------
+ // Main logic
+ //----------------------------------------------------------------------------------------------
+
+ @Override public void runOpMode() throws InterruptedException {
+
+ // Retrieve and initialize the AndyMark IMU.
+ // This sample expects the AndyMark IMU to be named "imu".
+ imu = hardwareMap.get(IMU.class, "imu");
+
+ /* Define how the AndyMark IMU is mounted to the robot to get the correct Yaw, Pitch, and
+ * Roll values.
+ *
+ * Two input parameters are required to fully specify the Orientation.
+ * The first parameter specifies the direction the AndyMark logo on the IMU is pointing.
+ * The second parameter specifies the direction the I2C port on the IMU is pointing.
+ * All directions are relative to the robot, and left/right is as-viewed from behind the robot.
+ */
+
+ /* The next two lines define the IMU orientation.
+ * To Do: EDIT these two lines to match YOUR mounting configuration.
+ */
+ LogoFacingDirection logoDirection = LogoFacingDirection.UP;
+ I2cPortFacingDirection i2cDirection = I2cPortFacingDirection.FORWARD;
+
+ AndyMarkIMUOrientationOnRobot orientationOnRobot = new AndyMarkIMUOrientationOnRobot(logoDirection, i2cDirection);
+
+ // Now initialize the AndyMark IMU with this mounting orientation.
+ // Note: if you choose two conflicting directions, this initialization will cause a code exception.
+ imu.initialize(new IMU.Parameters(orientationOnRobot));
+
+ // Loop and update the dashboard.
+ while (!isStopRequested()) {
+ telemetry.addData("IMU orientation", "Logo=%s I2C=%s\n ", logoDirection, i2cDirection);
+
+ // Check to see if heading reset is requested.
+ if (gamepad1.y) {
+ telemetry.addData("Yaw", "Resetting\n");
+ imu.resetYaw();
+ } else {
+ telemetry.addData("Yaw", "Press Y (triangle) on Gamepad to reset\n");
+ }
+
+ // Retrieve rotational angles and velocities.
+ YawPitchRollAngles orientation = imu.getRobotYawPitchRollAngles();
+ AngularVelocity angularVelocity = imu.getRobotAngularVelocity(AngleUnit.DEGREES);
+
+ telemetry.addData("Yaw (Z)", "%.2f Deg. (Heading)", orientation.getYaw(AngleUnit.DEGREES));
+ telemetry.addData("Pitch (X)", "%.2f Deg.", orientation.getPitch(AngleUnit.DEGREES));
+ telemetry.addData("Roll (Y)", "%.2f Deg.\n", orientation.getRoll(AngleUnit.DEGREES));
+ telemetry.addData("Yaw (Z) velocity", "%.2f Deg/Sec", angularVelocity.zRotationRate);
+ telemetry.addData("Pitch (X) velocity", "%.2f Deg/Sec", angularVelocity.xRotationRate);
+ telemetry.addData("Roll (Y) velocity", "%.2f Deg/Sec", angularVelocity.yRotationRate);
+ telemetry.update();
+ }
+ }
+}
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorAndyMarkTOF.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorAndyMarkTOF.java
new file mode 100644
index 0000000..ce6e943
--- /dev/null
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorAndyMarkTOF.java
@@ -0,0 +1,85 @@
+/*
+Copyright (c) 2025 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.robotcontroller.external.samples;
+
+import com.qualcomm.hardware.andymark.AndyMarkTOF;
+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 org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
+
+/*
+ * This OpMode illustrates how to use the AndyMark 2m TOF Lidar Sensor.
+ *
+ * The OpMode assumes that the sensor is configured with a name of "sensor_distance".
+ *
+ * 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: AndyMarkTOF", group = "Sensor")
+@Disabled
+public class SensorAndyMarkTOF extends LinearOpMode {
+
+ private DistanceSensor sensorDistance;
+
+ @Override
+ public void runOpMode() {
+ // you can use this as a regular DistanceSensor.
+ sensorDistance = hardwareMap.get(DistanceSensor.class, "sensor_distance");
+
+ // you can also cast this to a AndyMarkTOF if you want to use added
+ // methods associated with the AndyMarkTOF class.
+ AndyMarkTOF sensorTimeOfFlight = (AndyMarkTOF) sensorDistance;
+
+ telemetry.addData(">>", "Press start to continue");
+ telemetry.update();
+
+ waitForStart();
+ while(opModeIsActive()) {
+ // generic DistanceSensor methods.
+ telemetry.addData("deviceName", sensorDistance.getDeviceName() );
+ telemetry.addData("range", String.format("%.01f mm", sensorDistance.getDistance(DistanceUnit.MM)));
+ telemetry.addData("range", String.format("%.01f cm", sensorDistance.getDistance(DistanceUnit.CM)));
+ telemetry.addData("range", String.format("%.01f m", sensorDistance.getDistance(DistanceUnit.METER)));
+ telemetry.addData("range", String.format("%.01f in", sensorDistance.getDistance(DistanceUnit.INCH)));
+
+ // AndyMarkTOF specific methods.
+ telemetry.addData("ID", String.format("%x", sensorTimeOfFlight.getModelID()));
+ telemetry.addData("did time out", Boolean.toString(sensorTimeOfFlight.didTimeoutOccur()));
+
+ telemetry.update();
+ }
+ }
+
+}
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorBNO055IMU.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorBNO055IMU.java
new file mode 100644
index 0000000..405da1e
--- /dev/null
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorBNO055IMU.java
@@ -0,0 +1,186 @@
+/* Copyright (c) 2017 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.robotcontroller.external.samples;
+
+import com.qualcomm.hardware.bosch.BNO055IMU;
+import com.qualcomm.hardware.bosch.JustLoggingAccelerationIntegrator;
+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.Func;
+import org.firstinspires.ftc.robotcore.external.navigation.Acceleration;
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+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.Position;
+import org.firstinspires.ftc.robotcore.external.navigation.Velocity;
+
+import java.util.Locale;
+
+/*
+ * This OpMode gives a short demo on how to use the BNO055 Inertial Motion Unit (IMU) from AdaFruit.
+ *
+ * Note: this is a Legacy example that will not work with newer Control/Expansion Hubs that use a different IMU
+ * Please use the new SensorIMUOrthogonal or SensorIMUNonOrthogonal samples for a more universal IMU interface.
+ *
+ * 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
+ *
+ * @see Adafruit IMU
+ */
+@TeleOp(name = "Sensor: BNO055 IMU", group = "Sensor")
+@Disabled // Comment this out to add to the OpMode list
+public class SensorBNO055IMU extends LinearOpMode
+ {
+ //----------------------------------------------------------------------------------------------
+ // State
+ //----------------------------------------------------------------------------------------------
+
+ // The IMU sensor object
+ BNO055IMU imu;
+
+ // State used for updating telemetry
+ Orientation angles;
+ Acceleration gravity;
+
+ //----------------------------------------------------------------------------------------------
+ // Main logic
+ //----------------------------------------------------------------------------------------------
+
+ @Override public void runOpMode() {
+
+ // Set up the parameters with which we will use our IMU. Note that integration
+ // algorithm here just reports accelerations to the logcat log; it doesn't actually
+ // provide positional information.
+ BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
+ parameters.angleUnit = BNO055IMU.AngleUnit.DEGREES;
+ parameters.accelUnit = BNO055IMU.AccelUnit.METERS_PERSEC_PERSEC;
+ parameters.calibrationDataFile = "BNO055IMUCalibration.json"; // see the calibration sample OpMode
+ parameters.loggingEnabled = true;
+ parameters.loggingTag = "IMU";
+ parameters.accelerationIntegrationAlgorithm = new JustLoggingAccelerationIntegrator();
+
+ // Retrieve and initialize the IMU. We expect the IMU to be attached to an I2C port
+ // on a Core Device Interface Module, configured to be a sensor of type "AdaFruit IMU",
+ // and named "imu".
+ imu = hardwareMap.get(BNO055IMU.class, "imu");
+ imu.initialize(parameters);
+
+ // Set up our telemetry dashboard
+ composeTelemetry();
+
+ // Wait until we're told to go
+ waitForStart();
+
+ // Start the logging of measured acceleration
+ imu.startAccelerationIntegration(new Position(), new Velocity(), 1000);
+
+ // Loop and update the dashboard
+ while (opModeIsActive()) {
+ telemetry.update();
+ }
+ }
+
+ //----------------------------------------------------------------------------------------------
+ // Telemetry Configuration
+ //----------------------------------------------------------------------------------------------
+
+ void composeTelemetry() {
+
+ // At the beginning of each telemetry update, grab a bunch of data
+ // from the IMU that we will then display in separate lines.
+ telemetry.addAction(new Runnable() { @Override public void run()
+ {
+ // Acquiring the angles is relatively expensive; we don't want
+ // to do that in each of the three items that need that info, as that's
+ // three times the necessary expense.
+ angles = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES);
+ gravity = imu.getGravity();
+ }
+ });
+
+ telemetry.addLine()
+ .addData("status", new Func() {
+ @Override public String value() {
+ return imu.getSystemStatus().toShortString();
+ }
+ })
+ .addData("calib", new Func() {
+ @Override public String value() {
+ return imu.getCalibrationStatus().toString();
+ }
+ });
+
+ telemetry.addLine()
+ .addData("heading", new Func() {
+ @Override public String value() {
+ return formatAngle(angles.angleUnit, angles.firstAngle);
+ }
+ })
+ .addData("roll", new Func() {
+ @Override public String value() {
+ return formatAngle(angles.angleUnit, angles.secondAngle);
+ }
+ })
+ .addData("pitch", new Func() {
+ @Override public String value() {
+ return formatAngle(angles.angleUnit, angles.thirdAngle);
+ }
+ });
+
+ telemetry.addLine()
+ .addData("grvty", new Func() {
+ @Override public String value() {
+ return gravity.toString();
+ }
+ })
+ .addData("mag", new Func() {
+ @Override public String value() {
+ return String.format(Locale.getDefault(), "%.3f",
+ Math.sqrt(gravity.xAccel*gravity.xAccel
+ + gravity.yAccel*gravity.yAccel
+ + gravity.zAccel*gravity.zAccel));
+ }
+ });
+ }
+
+ //----------------------------------------------------------------------------------------------
+ // Formatting
+ //----------------------------------------------------------------------------------------------
+
+ String formatAngle(AngleUnit angleUnit, double angle) {
+ return formatDegrees(AngleUnit.DEGREES.fromUnit(angleUnit, angle));
+ }
+
+ String formatDegrees(double degrees){
+ return String.format(Locale.getDefault(), "%.1f", AngleUnit.DEGREES.normalize(degrees));
+ }
+}
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorBNO055IMUCalibration.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorBNO055IMUCalibration.java
new file mode 100644
index 0000000..93f1789
--- /dev/null
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorBNO055IMUCalibration.java
@@ -0,0 +1,230 @@
+/* Copyright (c) 2017 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.robotcontroller.external.samples;
+
+import com.qualcomm.hardware.bosch.BNO055IMU;
+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.util.ReadWriteFile;
+import org.firstinspires.ftc.robotcore.external.Func;
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+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.internal.system.AppUtil;
+
+import java.io.File;
+import java.util.Locale;
+
+/*
+ * This OpMode calibrates a BNO055 IMU per
+ * "Section 3.11 Calibration" of the BNO055 specification.
+ *
+ * Note: this is a Legacy example that will not work with newer Control/Expansion Hubs that use a different IMU
+ * Please use the new SensorIMUOrthogonal or SensorIMUNonOrthogonal samples for a more universal IMU interface.
+ *
+ * Manual calibration of the IMU is definitely NOT necessary: except for the magnetometer (which is not used by the
+ * default "IMU" SensorMode), the BNO055 is internally self-calibrating and thus can be very successfully used without
+ * manual intervention. That said, performing a one-time calibration, saving the results persistently, then loading them
+ * again at each run can help reduce the time that automatic calibration requires.
+ *
+ * This summary of the calibration process from Intel is informative:
+ * http://iotdk.intel.com/docs/master/upm/classupm_1_1_b_n_o055.html
+ *
+ * "This device requires calibration in order to operate accurately. [...] Calibration data is
+ * lost on a power cycle. See one of the examples for a description of how to calibrate the device,
+ * but in essence:
+ *
+ * There is a calibration status register available [...] that returns the calibration status
+ * of the accelerometer (ACC), magnetometer (MAG), gyroscope (GYR), and overall system (SYS).
+ * Each of these values range from 0 (uncalibrated) to 3 (fully calibrated). Calibration [ideally]
+ * involves certain motions to get all 4 values at 3. The motions are as follows (though see the
+ * datasheet for more information):
+ *
+ * 1. GYR: Simply let the sensor sit flat for a few seconds.
+ * 2. ACC: Move the sensor in various positions. Start flat, then rotate slowly by 45
+ * degrees, hold for a few seconds, then continue rotating another 45 degrees and
+ * hold, etc. 6 or more movements of this type may be required. You can move through
+ * any axis you desire, but make sure that the device is lying at least once
+ * perpendicular to the x, y, and z axis.
+ * 3. MAG: Move slowly in a figure 8 pattern in the air, until the calibration values reaches 3.
+ * 4. SYS: This will usually reach 3 when the other items have also reached 3. If not, continue
+ * slowly moving the device though various axes until it does."
+ *
+ * To calibrate the IMU, run this sample OpMode with a gamepad attached to the driver station.
+ * Once the IMU has reached sufficient calibration as reported on telemetry, press the 'A'
+ * button on the gamepad to write the calibration to a file. That file can then be indicated
+ * later when running an OpMode which uses the IMU.
+ *
+ * Note: if your intended uses of the IMU do not include use of all its sensors (for example,
+ * you might not use the magnetometer), then it makes little sense for you to wait for full
+ * calibration of the sensors you are not using before saving the calibration data. Indeed,
+ * it appears that in a SensorMode that doesn't use the magnetometer (for example), the
+ * magnetometer cannot actually be calibrated.
+ *
+ * References:
+ * The AdafruitBNO055IMU Javadoc
+ * The BNO055IMU.Parameters.calibrationDataFile Javadoc
+ * The BNO055 product page: https://www.bosch-sensortec.com/bst/products/all_products/bno055
+ * The BNO055 datasheet: https://www.bosch-sensortec.com/media/boschsensortec/downloads/datasheets/bst-bno055-ds000.pdf
+ */
+@TeleOp(name = "Sensor: BNO055 IMU Calibration", group = "Sensor")
+@Disabled // Uncomment this to add to the OpMode list
+public class SensorBNO055IMUCalibration extends LinearOpMode
+ {
+ //----------------------------------------------------------------------------------------------
+ // State
+ //----------------------------------------------------------------------------------------------
+
+ // Our sensors, motors, and other devices go here, along with other long term state
+ BNO055IMU imu;
+
+ // State used for updating telemetry
+ Orientation angles;
+
+ //----------------------------------------------------------------------------------------------
+ // Main logic
+ //----------------------------------------------------------------------------------------------
+
+ @Override public void runOpMode() {
+
+ telemetry.log().setCapacity(12);
+ telemetry.log().add("");
+ telemetry.log().add("Please refer to the calibration instructions");
+ telemetry.log().add("contained in the Adafruit IMU calibration");
+ telemetry.log().add("sample OpMode.");
+ telemetry.log().add("");
+ telemetry.log().add("When sufficient calibration has been reached,");
+ telemetry.log().add("press the 'A' button to write the current");
+ telemetry.log().add("calibration data to a file.");
+ telemetry.log().add("");
+
+ // We are expecting the IMU to be attached to an I2C port on a Core Device Interface Module and named "imu".
+ BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
+ parameters.loggingEnabled = true;
+ parameters.loggingTag = "IMU";
+ imu = hardwareMap.get(BNO055IMU.class, "imu");
+ imu.initialize(parameters);
+
+ composeTelemetry();
+ telemetry.log().add("Waiting for start...");
+
+ // Wait until we're told to go
+ while (!isStarted()) {
+ telemetry.update();
+ idle();
+ }
+
+ telemetry.log().add("...started...");
+
+ while (opModeIsActive()) {
+
+ if (gamepad1.a) {
+
+ // Get the calibration data
+ BNO055IMU.CalibrationData calibrationData = imu.readCalibrationData();
+
+ // Save the calibration data to a file. You can choose whatever file
+ // name you wish here, but you'll want to indicate the same file name
+ // when you initialize the IMU in an OpMode in which it is used. If you
+ // have more than one IMU on your robot, you'll of course want to use
+ // different configuration file names for each.
+ String filename = "AdafruitIMUCalibration.json";
+ File file = AppUtil.getInstance().getSettingsFile(filename);
+ ReadWriteFile.writeFile(file, calibrationData.serialize());
+ telemetry.log().add("saved to '%s'", filename);
+
+ // Wait for the button to be released
+ while (gamepad1.a) {
+ telemetry.update();
+ idle();
+ }
+ }
+
+ telemetry.update();
+ }
+ }
+
+ void composeTelemetry() {
+
+ // At the beginning of each telemetry update, grab a bunch of data
+ // from the IMU that we will then display in separate lines.
+ telemetry.addAction(new Runnable() { @Override public void run()
+ {
+ // Acquiring the angles is relatively expensive; we don't want
+ // to do that in each of the three items that need that info, as that's
+ // three times the necessary expense.
+ angles = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES);
+ }
+ });
+
+ telemetry.addLine()
+ .addData("status", new Func() {
+ @Override public String value() {
+ return imu.getSystemStatus().toShortString();
+ }
+ })
+ .addData("calib", new Func() {
+ @Override public String value() {
+ return imu.getCalibrationStatus().toString();
+ }
+ });
+
+ telemetry.addLine()
+ .addData("heading", new Func() {
+ @Override public String value() {
+ return formatAngle(angles.angleUnit, angles.firstAngle);
+ }
+ })
+ .addData("roll", new Func() {
+ @Override public String value() {
+ return formatAngle(angles.angleUnit, angles.secondAngle);
+ }
+ })
+ .addData("pitch", new Func() {
+ @Override public String value() {
+ return formatAngle(angles.angleUnit, angles.thirdAngle);
+ }
+ });
+ }
+
+ //----------------------------------------------------------------------------------------------
+ // Formatting
+ //----------------------------------------------------------------------------------------------
+
+ String formatAngle(AngleUnit angleUnit, double angle) {
+ return formatDegrees(AngleUnit.DEGREES.fromUnit(angleUnit, angle));
+ }
+
+ String formatDegrees(double degrees){
+ return String.format(Locale.getDefault(), "%.1f", AngleUnit.DEGREES.normalize(degrees));
+ }
+}
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorColor.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorColor.java
new file mode 100644
index 0000000..d0411af
--- /dev/null
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorColor.java
@@ -0,0 +1,225 @@
+/* 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.robotcontroller.external.samples;
+
+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));
+ }
+ });
+ }
+ }
+}
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorDigitalTouch.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorDigitalTouch.java
new file mode 100644
index 0000000..44c3ca9
--- /dev/null
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorDigitalTouch.java
@@ -0,0 +1,78 @@
+/* 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 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.robotcontroller.external.samples;
+
+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.DigitalChannel;
+
+/*
+ * This OpMode demonstrates how to use a digital channel.
+ *
+ * The OpMode assumes that the digital channel is configured with a name of "digitalTouch".
+ *
+ * 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: digital channel", group = "Sensor")
+@Disabled
+public class SensorDigitalTouch extends LinearOpMode {
+ DigitalChannel digitalTouch; // Digital channel Object
+
+ @Override
+ public void runOpMode() {
+
+ // get a reference to our touchSensor object.
+ digitalTouch = hardwareMap.get(DigitalChannel.class, "digitalTouch");
+
+ digitalTouch.setMode(DigitalChannel.Mode.INPUT);
+ telemetry.addData("DigitalTouchSensorExample", "Press start to continue...");
+ telemetry.update();
+
+ // wait for the start button to be pressed.
+ waitForStart();
+
+ // while the OpMode is active, loop and read the digital channel.
+ // Note we use opModeIsActive() as our loop condition because it is an interruptible method.
+ while (opModeIsActive()) {
+
+ // button is pressed if value returned is LOW or false.
+ // send the info back to driver station using telemetry function.
+ if (digitalTouch.getState() == false) {
+ telemetry.addData("Button", "PRESSED");
+ } else {
+ telemetry.addData("Button", "NOT PRESSED");
+ }
+
+ telemetry.update();
+ }
+ }
+}
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorGoBildaPinpoint.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorGoBildaPinpoint.java
new file mode 100644
index 0000000..5b0f6e9
--- /dev/null
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorGoBildaPinpoint.java
@@ -0,0 +1,116 @@
+/*
+* Copyright (c) 2025 Alan Smith
+*
+* Permission is hereby granted, free of charge, to any person obtaining a copy
+* of this software and associated documentation files (the "Software"), to deal
+* in the Software without restriction, including without limitation the rights
+* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+* copies of the Software, and to permit persons to whom the Software is
+* furnished to do so, subject to the following conditions:
+
+* The above copyright notice and this permission notice shall be included in all
+* copies or substantial portions of the Software.
+
+* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+* SOFTWARE.
+*/
+package org.firstinspires.ftc.robotcontroller.external.samples;
+
+import com.qualcomm.hardware.gobilda.GoBildaPinpointDriver;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
+import org.firstinspires.ftc.robotcore.external.navigation.Pose2D;
+
+/*
+ * This OpMode illustrates how to use the GoBildaPinpoint
+ *
+ * The OpMode assumes that the sensor is configured with a name of "pinpoint".
+ *
+ * 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
+ *
+ * See the sensor's product page: https://www.gobilda.com/pinpoint-odometry-computer-imu-sensor-fusion-for-2-wheel-odometry/
+ */
+@TeleOp(name = "Sensor: GoBilda Pinpoint", group = "Sensor")
+@Disabled
+public class SensorGoBildaPinpoint extends OpMode {
+ // Create an instance of the sensor
+ GoBildaPinpointDriver pinpoint;
+
+ @Override
+ public void init() {
+ // Get a reference to the sensor
+ pinpoint = hardwareMap.get(GoBildaPinpointDriver.class, "pinpoint");
+
+ // Configure the sensor
+ configurePinpoint();
+
+ // Set the location of the robot - this should be the place you are starting the robot from
+ pinpoint.setPosition(new Pose2D(DistanceUnit.INCH, 0, 0, AngleUnit.DEGREES, 0));
+ }
+
+ @Override
+ public void loop() {
+ telemetry.addLine("Push your robot around to see it track");
+ telemetry.addLine("Press A to reset the position");
+ if(gamepad1.a){
+ // You could use readings from April Tags here to give a new known position to the pinpoint
+ pinpoint.setPosition(new Pose2D(DistanceUnit.INCH, 0, 0, AngleUnit.DEGREES, 0));
+ }
+ pinpoint.update();
+ Pose2D pose2D = pinpoint.getPosition();
+
+ telemetry.addData("X coordinate (IN)", pose2D.getX(DistanceUnit.INCH));
+ telemetry.addData("Y coordinate (IN)", pose2D.getY(DistanceUnit.INCH));
+ telemetry.addData("Heading angle (DEGREES)", pose2D.getHeading(AngleUnit.DEGREES));
+ }
+
+ public void configurePinpoint(){
+ /*
+ * Set the odometry pod positions relative to the point that you want the position to be measured from.
+ *
+ * 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.
+ */
+ pinpoint.setOffsets(-84.0, -168.0, DistanceUnit.MM); //these are tuned for 3110-0002-0001 Product Insight #1
+
+ /*
+ * Set the kind of pods used by your robot. If you're using goBILDA odometry pods, select either
+ * the goBILDA_SWINGARM_POD, or the goBILDA_4_BAR_POD.
+ * If you're using another kind of odometry pod, uncomment setEncoderResolution and input the
+ * number of ticks per unit of your odometry pod. For example:
+ * pinpoint.setEncoderResolution(13.26291192, DistanceUnit.MM);
+ */
+ pinpoint.setEncoderResolution(GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_4_BAR_POD);
+
+ /*
+ * Set the direction that each of the two odometry pods count. The X (forward) pod should
+ * increase when you move the robot forward. And the Y (strafe) pod should increase when
+ * you move the robot to the left.
+ */
+ pinpoint.setEncoderDirections(GoBildaPinpointDriver.EncoderDirection.FORWARD,
+ GoBildaPinpointDriver.EncoderDirection.FORWARD);
+
+ /*
+ * Before running the robot, recalibrate the IMU. This needs to happen when the robot is stationary
+ * The IMU will automatically calibrate when first powered on, but recalibrating before running
+ * the robot is a good idea to ensure that the calibration is "good".
+ * resetPosAndIMU will reset the position to 0,0,0 and also recalibrate the IMU.
+ * This is recommended before you run your autonomous, as a bad initial calibration can cause
+ * an incorrect starting value for x, y, and heading.
+ */
+ pinpoint.resetPosAndIMU();
+ }
+}
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorHuskyLens.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorHuskyLens.java
new file mode 100644
index 0000000..af7ca55
--- /dev/null
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorHuskyLens.java
@@ -0,0 +1,160 @@
+/*
+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.
+*/
+package org.firstinspires.ftc.robotcontroller.external.samples;
+
+import com.qualcomm.hardware.dfrobot.HuskyLens;
+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.internal.system.Deadline;
+
+import java.util.concurrent.TimeUnit;
+
+/*
+ * This OpMode illustrates how to use the DFRobot HuskyLens.
+ *
+ * The HuskyLens is a Vision Sensor with a built-in object detection model. It can
+ * detect a number of predefined objects and AprilTags in the 36h11 family, can
+ * recognize colors, and can be trained to detect custom objects. See this website for
+ * documentation: https://wiki.dfrobot.com/HUSKYLENS_V1.0_SKU_SEN0305_SEN0336
+ *
+ * For detailed instructions on how a HuskyLens is used in FTC, please see this tutorial:
+ * https://ftc-docs.firstinspires.org/en/latest/devices/huskylens/huskylens.html
+ *
+ * This sample illustrates how to detect AprilTags, but can be used to detect other types
+ * of objects by changing the algorithm. It assumes that the HuskyLens is configured with
+ * a name of "huskylens".
+ *
+ * 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: HuskyLens", group = "Sensor")
+@Disabled
+public class SensorHuskyLens extends LinearOpMode {
+
+ private final int READ_PERIOD = 1;
+
+ private HuskyLens huskyLens;
+
+ @Override
+ public void runOpMode()
+ {
+ huskyLens = hardwareMap.get(HuskyLens.class, "huskylens");
+
+ /*
+ * This sample rate limits the reads solely to allow a user time to observe
+ * what is happening on the Driver Station telemetry. Typical applications
+ * would not likely rate limit.
+ */
+ Deadline rateLimit = new Deadline(READ_PERIOD, TimeUnit.SECONDS);
+
+ /*
+ * Immediately expire so that the first time through we'll do the read.
+ */
+ rateLimit.expire();
+
+ /*
+ * Basic check to see if the device is alive and communicating. This is not
+ * technically necessary here as the HuskyLens class does this in its
+ * doInitialization() method which is called when the device is pulled out of
+ * the hardware map. However, sometimes it's unclear why a device reports as
+ * failing on initialization. In the case of this device, it's because the
+ * call to knock() failed.
+ */
+ if (!huskyLens.knock()) {
+ telemetry.addData(">>", "Problem communicating with " + huskyLens.getDeviceName());
+ } else {
+ telemetry.addData(">>", "Press start to continue");
+ }
+
+ /*
+ * The device uses the concept of an algorithm to determine what types of
+ * objects it will look for and/or what mode it is in. The algorithm may be
+ * selected using the scroll wheel on the device, or via software as shown in
+ * the call to selectAlgorithm().
+ *
+ * The SDK itself does not assume that the user wants a particular algorithm on
+ * startup, and hence does not set an algorithm.
+ *
+ * Users, should, in general, explicitly choose the algorithm they want to use
+ * within the OpMode by calling selectAlgorithm() and passing it one of the values
+ * found in the enumeration HuskyLens.Algorithm.
+ *
+ * Other algorithm choices for FTC might be: OBJECT_RECOGNITION, COLOR_RECOGNITION or OBJECT_CLASSIFICATION.
+ */
+ huskyLens.selectAlgorithm(HuskyLens.Algorithm.TAG_RECOGNITION);
+
+ telemetry.update();
+ waitForStart();
+
+ /*
+ * Looking for AprilTags per the call to selectAlgorithm() above. A handy grid
+ * for testing may be found at https://wiki.dfrobot.com/HUSKYLENS_V1.0_SKU_SEN0305_SEN0336#target_20.
+ *
+ * Note again that the device only recognizes the 36h11 family of tags out of the box.
+ */
+ while(opModeIsActive()) {
+ if (!rateLimit.hasExpired()) {
+ continue;
+ }
+ rateLimit.reset();
+
+ /*
+ * All algorithms, except for LINE_TRACKING, return a list of Blocks where a
+ * Block represents the outline of a recognized object along with its ID number.
+ * ID numbers allow you to identify what the device saw. See the HuskyLens documentation
+ * referenced in the header comment above for more information on IDs and how to
+ * assign them to objects.
+ *
+ * Returns an empty array if no objects are seen.
+ */
+ HuskyLens.Block[] blocks = huskyLens.blocks();
+ telemetry.addData("Block count", blocks.length);
+ for (int i = 0; i < blocks.length; i++) {
+ telemetry.addData("Block", blocks[i].toString());
+ /*
+ * Here inside the FOR loop, you could save or evaluate specific info for the currently recognized Bounding Box:
+ * - blocks[i].width and blocks[i].height (size of box, in pixels)
+ * - blocks[i].left and blocks[i].top (edges of box)
+ * - blocks[i].x and blocks[i].y (center location)
+ * - blocks[i].id (Color ID)
+ *
+ * These values have Java type int (integer).
+ */
+ }
+
+ telemetry.update();
+ }
+ }
+}
\ No newline at end of file
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorIMUNonOrthogonal.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorIMUNonOrthogonal.java
new file mode 100644
index 0000000..70bc8d4
--- /dev/null
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorIMUNonOrthogonal.java
@@ -0,0 +1,184 @@
+/* Copyright (c) 2022 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.robotcontroller.external.samples;
+
+import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
+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.IMU;
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+import org.firstinspires.ftc.robotcore.external.navigation.AngularVelocity;
+import org.firstinspires.ftc.robotcore.external.navigation.Orientation;
+import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles;
+
+import static com.qualcomm.hardware.rev.RevHubOrientationOnRobot.xyzOrientation;
+
+/*
+ * This OpMode shows how to use the new universal IMU interface. This
+ * interface may be used with the BNO055 IMU or the BHI260 IMU. It assumes that an IMU is configured
+ * on the robot with the name "imu".
+ *
+ * The sample will display the current Yaw, Pitch and Roll of the robot.
+ * With the correct orientation parameters selected, pitch/roll/yaw should act as follows:
+ * Pitch value should INCREASE as the robot is tipped UP at the front. (Rotation about X)
+ * Roll value should INCREASE as the robot is tipped UP at the left side. (Rotation about Y)
+ * Yaw value should INCREASE as the robot is rotated Counter Clockwise. (Rotation about Z)
+ *
+ * The yaw can be reset (to zero) by pressing the Y button on the gamepad (Triangle on a PS4 controller)
+ *
+ * This specific sample DOES NOT assume that the Hub is mounted on one of the three orthogonal
+ * planes (X/Y, X/Z or Y/Z) OR that the Hub has only been rotated in a range of 90 degree increments.
+ *
+ * Note: if your Hub is mounted Orthogonally (on a orthogonal surface, angled at some multiple of
+ * 90 Degrees) then you should use the simpler SensorIMUOrthogonal sample in this folder.
+ *
+ * But... If your Hub is mounted Non-Orthogonally, you must specify one or more rotational angles
+ * that transform a "Default" Hub orientation into your desired orientation. That is what is
+ * illustrated here.
+ *
+ * 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.
+ *
+ * Finally, edit this OpMode to use at least one angle around an axis to orient your Hub.
+ */
+@TeleOp(name = "Sensor: IMU Non-Orthogonal", group = "Sensor")
+@Disabled // Comment this out to add to the OpMode list
+public class SensorIMUNonOrthogonal extends LinearOpMode
+{
+ // The IMU sensor object
+ IMU imu;
+
+ //----------------------------------------------------------------------------------------------
+ // Main logic
+ //----------------------------------------------------------------------------------------------
+
+ @Override public void runOpMode() throws InterruptedException {
+
+ // Retrieve and initialize the IMU.
+ // This sample expects the IMU to be in a REV Hub and named "imu".
+ imu = hardwareMap.get(IMU.class, "imu");
+
+ /* Define how the hub is mounted to the robot to get the correct Yaw, Pitch and Roll values.
+ *
+ * You can apply up to three axis rotations to orient your Hub according to how it's mounted on the robot.
+ *
+ * The starting point for these rotations is the "Default" Hub orientation, which is:
+ * 1) Hub laying flat on a horizontal surface, with the Printed Logo facing UP
+ * 2) Rotated such that the USB ports are facing forward on the robot.
+ *
+ * If you are using a REV External IMU, the "Default" orientation is the same as for a REV Hub, but instead of
+ * the USB ports facing forward, the I2C port faces forward.
+ *
+ * The order that the rotations are performed matters, so this sample shows doing them in the order X, Y, then Z.
+ * For specifying non-orthogonal hub mounting orientations, we must temporarily use axes
+ * defined relative to the Hub itself, instead of the usual Robot Coordinate System axes
+ * used for the results the IMU gives us. In the starting orientation, the Hub axes are
+ * aligned with the Robot Coordinate System:
+ *
+ * X Axis: Starting at Center of Hub, pointing out towards I2C connectors
+ * Y Axis: Starting at Center of Hub, pointing out towards USB connectors
+ * Z Axis: Starting at Center of Hub, pointing Up through LOGO
+ *
+ * Positive rotation is defined by right-hand rule with thumb pointing in +ve direction on axis.
+ *
+ * Some examples.
+ *
+ * ----------------------------------------------------------------------------------------------------------------------------------
+ * Example A) Assume that the hub is mounted on a sloped plate at the back of the robot, with the USB ports coming out the top of the hub.
+ * The plate is tilted UP 60 degrees from horizontal.
+ *
+ * To get the "Default" hub into this configuration you would just need a single rotation.
+ * 1) Rotate the Hub +60 degrees around the X axis to tilt up the front edge.
+ * 2) No rotation around the Y or Z axes.
+ *
+ * So the X,Y,Z rotations would be 60,0,0
+ *
+ * ----------------------------------------------------------------------------------------------------------------------------------
+ * Example B) Assume that the hub is laying flat on the chassis, but it has been twisted 30 degrees towards the right front wheel to make
+ * the USB cable accessible.
+ *
+ * To get the "Default" hub into this configuration you would just need a single rotation, but around a different axis.
+ * 1) No rotation around the X or Y axes.
+ * 2) Rotate the Hub -30 degrees (Clockwise) around the Z axis, since a positive angle would be Counter Clockwise.
+ *
+ * So the X,Y,Z rotations would be 0,0,-30
+ *
+ * ----------------------------------------------------------------------------------------------------------------------------------
+ * Example C) Assume that the hub is mounted on a vertical plate on the right side of the robot, with the Logo facing out, and the
+ * Hub rotated so that the USB ports are facing down 30 degrees towards the back wheels of the robot.
+ *
+ * To get the "Default" hub into this configuration will require several rotations.
+ * 1) Rotate the hub +90 degrees around the X axis to get it standing upright with the logo pointing backwards on the robot
+ * 2) Next, rotate the hub +90 around the Y axis to get it facing to the right.
+ * 3) Finally rotate the hub +120 degrees around the Z axis to take the USB ports from vertical to sloping down 30 degrees and
+ * facing towards the back of the robot.
+ *
+ * So the X,Y,Z rotations would be 90,90,120
+ */
+
+ // The next three lines define the desired axis rotations.
+ // To Do: EDIT these values to match YOUR mounting configuration.
+ double xRotation = 0; // enter the desired X rotation angle here.
+ double yRotation = 0; // enter the desired Y rotation angle here.
+ double zRotation = 0; // enter the desired Z rotation angle here.
+
+ Orientation hubRotation = xyzOrientation(xRotation, yRotation, zRotation);
+
+ // Now initialize the IMU with this mounting orientation
+ RevHubOrientationOnRobot orientationOnRobot = new RevHubOrientationOnRobot(hubRotation);
+ imu.initialize(new IMU.Parameters(orientationOnRobot));
+
+ // Loop and update the dashboard
+ while (!isStopRequested()) {
+ telemetry.addData("Hub orientation", "X=%.1f, Y=%.1f, Z=%.1f \n", xRotation, yRotation, zRotation);
+
+ // Check to see if heading reset is requested
+ if (gamepad1.y) {
+ telemetry.addData("Yaw", "Resetting\n");
+ imu.resetYaw();
+ } else {
+ telemetry.addData("Yaw", "Press Y (triangle) on Gamepad to reset\n");
+ }
+
+ // Retrieve Rotational Angles and Velocities
+ YawPitchRollAngles orientation = imu.getRobotYawPitchRollAngles();
+ AngularVelocity angularVelocity = imu.getRobotAngularVelocity(AngleUnit.DEGREES);
+
+ telemetry.addData("Yaw (Z)", "%.2f Deg. (Heading)", orientation.getYaw(AngleUnit.DEGREES));
+ telemetry.addData("Pitch (X)", "%.2f Deg.", orientation.getPitch(AngleUnit.DEGREES));
+ telemetry.addData("Roll (Y)", "%.2f Deg.\n", orientation.getRoll(AngleUnit.DEGREES));
+ telemetry.addData("Yaw (Z) velocity", "%.2f Deg/Sec", angularVelocity.zRotationRate);
+ telemetry.addData("Pitch (X) velocity", "%.2f Deg/Sec", angularVelocity.xRotationRate);
+ telemetry.addData("Roll (Y) velocity", "%.2f Deg/Sec", angularVelocity.yRotationRate);
+ telemetry.update();
+ }
+ }
+}
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorIMUOrthogonal.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorIMUOrthogonal.java
new file mode 100644
index 0000000..af4c202
--- /dev/null
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorIMUOrthogonal.java
@@ -0,0 +1,146 @@
+/* Copyright (c) 2022 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.robotcontroller.external.samples;
+
+import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
+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.IMU;
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+import org.firstinspires.ftc.robotcore.external.navigation.AngularVelocity;
+import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles;
+
+/*
+ * This OpMode shows how to use the new universal IMU interface. This
+ * interface may be used with the BNO055 IMU or the BHI260 IMU. It assumes that an IMU is configured
+ * on the robot with the name "imu".
+ *
+ * The sample will display the current Yaw, Pitch and Roll of the robot.
+ * With the correct orientation parameters selected, pitch/roll/yaw should act as follows:
+ * Pitch value should INCREASE as the robot is tipped UP at the front. (Rotation about X)
+ * Roll value should INCREASE as the robot is tipped UP at the left side. (Rotation about Y)
+ * Yaw value should INCREASE as the robot is rotated Counter Clockwise. (Rotation about Z)
+ *
+ * The yaw can be reset (to zero) by pressing the Y button on the gamepad (Triangle on a PS4 controller)
+ *
+ * This specific sample assumes that the Hub is mounted on one of the three orthogonal planes
+ * (X/Y, X/Z or Y/Z) and that the Hub has only been rotated in a range of 90 degree increments.
+ *
+ * Note: if your Hub is mounted on a surface angled at some non-90 Degree multiple (like 30) look at
+ * the alternative SensorIMUNonOrthogonal sample in this folder.
+ *
+ * This "Orthogonal" requirement means that:
+ *
+ * 1) The Logo printed on the top of the Hub can ONLY be pointing in one of six directions:
+ * FORWARD, BACKWARD, UP, DOWN, LEFT and RIGHT.
+ *
+ * 2) The USB ports can only be pointing in one of the same six directions:
+ * FORWARD, BACKWARD, UP, DOWN, LEFT and RIGHT.
+ *
+ * So, To fully define how your Hub is mounted to the robot, you must simply specify:
+ * logoFacingDirection
+ * usbFacingDirection
+ *
+ * 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.
+ *
+ * Finally, choose the two correct parameters to define how your Hub is mounted and edit this OpMode
+ * to use those parameters.
+ */
+@TeleOp(name = "Sensor: IMU Orthogonal", group = "Sensor")
+@Disabled // Comment this out to add to the OpMode list
+public class SensorIMUOrthogonal extends LinearOpMode
+{
+ // The IMU sensor object
+ IMU imu;
+
+ //----------------------------------------------------------------------------------------------
+ // Main logic
+ //----------------------------------------------------------------------------------------------
+
+ @Override public void runOpMode() throws InterruptedException {
+
+ // Retrieve and initialize the IMU.
+ // This sample expects the IMU to be in a REV Hub and named "imu".
+ imu = hardwareMap.get(IMU.class, "imu");
+
+ /* Define how the hub is mounted on the robot to get the correct Yaw, Pitch and Roll values.
+ *
+ * Two input parameters are required to fully specify the Orientation.
+ * The first parameter specifies the direction the printed logo on the Hub is pointing.
+ * The second parameter specifies the direction the USB connector on the Hub is pointing.
+ * All directions are relative to the robot, and left/right is as-viewed from behind the robot.
+ *
+ * If you are using a REV 9-Axis IMU, you can use the Rev9AxisImuOrientationOnRobot class instead of the
+ * RevHubOrientationOnRobot class, which has an I2cPortFacingDirection instead of a UsbFacingDirection.
+ */
+
+ /* The next two lines define Hub orientation.
+ * The Default Orientation (shown) is when a hub is mounted horizontally with the printed logo pointing UP and the USB port pointing FORWARD.
+ *
+ * To Do: EDIT these two lines to match YOUR mounting configuration.
+ */
+ RevHubOrientationOnRobot.LogoFacingDirection logoDirection = RevHubOrientationOnRobot.LogoFacingDirection.UP;
+ RevHubOrientationOnRobot.UsbFacingDirection usbDirection = RevHubOrientationOnRobot.UsbFacingDirection.FORWARD;
+
+ RevHubOrientationOnRobot orientationOnRobot = new RevHubOrientationOnRobot(logoDirection, usbDirection);
+
+ // Now initialize the IMU with this mounting orientation
+ // Note: if you choose two conflicting directions, this initialization will cause a code exception.
+ imu.initialize(new IMU.Parameters(orientationOnRobot));
+
+ // Loop and update the dashboard
+ while (!isStopRequested()) {
+
+ telemetry.addData("Hub orientation", "Logo=%s USB=%s\n ", logoDirection, usbDirection);
+
+ // Check to see if heading reset is requested
+ if (gamepad1.y) {
+ telemetry.addData("Yaw", "Resetting\n");
+ imu.resetYaw();
+ } else {
+ telemetry.addData("Yaw", "Press Y (triangle) on Gamepad to reset\n");
+ }
+
+ // Retrieve Rotational Angles and Velocities
+ YawPitchRollAngles orientation = imu.getRobotYawPitchRollAngles();
+ AngularVelocity angularVelocity = imu.getRobotAngularVelocity(AngleUnit.DEGREES);
+
+ telemetry.addData("Yaw (Z)", "%.2f Deg. (Heading)", orientation.getYaw(AngleUnit.DEGREES));
+ telemetry.addData("Pitch (X)", "%.2f Deg.", orientation.getPitch(AngleUnit.DEGREES));
+ telemetry.addData("Roll (Y)", "%.2f Deg.\n", orientation.getRoll(AngleUnit.DEGREES));
+ telemetry.addData("Yaw (Z) velocity", "%.2f Deg/Sec", angularVelocity.zRotationRate);
+ telemetry.addData("Pitch (X) velocity", "%.2f Deg/Sec", angularVelocity.xRotationRate);
+ telemetry.addData("Roll (Y) velocity", "%.2f Deg/Sec", angularVelocity.yRotationRate);
+ telemetry.update();
+ }
+ }
+}
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorKLNavxMicro.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorKLNavxMicro.java
new file mode 100644
index 0000000..ccc945f
--- /dev/null
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorKLNavxMicro.java
@@ -0,0 +1,128 @@
+/* Copyright (c) 2017 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.robotcontroller.external.samples;
+
+import com.qualcomm.hardware.kauailabs.NavxMicroNavigationSensor;
+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.Gyroscope;
+import com.qualcomm.robotcore.hardware.IntegratingGyroscope;
+import com.qualcomm.robotcore.util.ElapsedTime;
+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;
+
+/*
+ * This OpMode shows how to use Kauai Labs navX Micro Robotics Navigation
+ * Sensor. It assumes that the sensor is configured with a name of "navx".
+ *
+ * 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: KL navX Micro", group = "Sensor")
+@Disabled
+public class SensorKLNavxMicro extends LinearOpMode {
+
+ /** In this sample, for illustration purposes we use two interfaces on the one gyro object.
+ * That's likely atypical: you'll probably use one or the other in any given situation,
+ * depending on what you're trying to do. {@link IntegratingGyroscope} (and it's base interface,
+ * {@link Gyroscope}) are common interfaces supported by possibly several different gyro
+ * implementations. {@link NavxMicroNavigationSensor}, by contrast, provides functionality that
+ * is unique to the navX Micro sensor.
+ */
+ IntegratingGyroscope gyro;
+ NavxMicroNavigationSensor navxMicro;
+
+ // A timer helps provide feedback while calibration is taking place
+ ElapsedTime timer = new ElapsedTime();
+
+ @Override public void runOpMode() throws InterruptedException {
+ // Get a reference to a Modern Robotics GyroSensor object. We use several interfaces
+ // on this object to illustrate which interfaces support which functionality.
+ navxMicro = hardwareMap.get(NavxMicroNavigationSensor.class, "navx");
+ gyro = (IntegratingGyroscope)navxMicro;
+ // If you're only interested int the IntegratingGyroscope interface, the following will suffice.
+ // gyro = hardwareMap.get(IntegratingGyroscope.class, "navx");
+
+ // The gyro automatically starts calibrating. This takes a few seconds.
+ telemetry.log().add("Gyro Calibrating. Do Not Move!");
+
+ // Wait until the gyro calibration is complete
+ timer.reset();
+ while (navxMicro.isCalibrating()) {
+ telemetry.addData("calibrating", "%s", Math.round(timer.seconds())%2==0 ? "|.." : "..|");
+ telemetry.update();
+ Thread.sleep(50);
+ }
+ telemetry.log().clear(); telemetry.log().add("Gyro Calibrated. Press Start.");
+ telemetry.clear(); telemetry.update();
+
+ // Wait for the start button to be pressed
+ waitForStart();
+ telemetry.log().clear();
+
+ while (opModeIsActive()) {
+
+ // Read dimensionalized data from the gyro. This gyro can report angular velocities
+ // about all three axes. Additionally, it internally integrates the Z axis to
+ // be able to report an absolute angular Z orientation.
+ AngularVelocity rates = gyro.getAngularVelocity(AngleUnit.DEGREES);
+ Orientation angles = gyro.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES);
+
+ telemetry.addLine()
+ .addData("dx", formatRate(rates.xRotationRate))
+ .addData("dy", formatRate(rates.yRotationRate))
+ .addData("dz", "%s deg/s", formatRate(rates.zRotationRate));
+
+ telemetry.addLine()
+ .addData("heading", formatAngle(angles.angleUnit, angles.firstAngle))
+ .addData("roll", formatAngle(angles.angleUnit, angles.secondAngle))
+ .addData("pitch", "%s deg", formatAngle(angles.angleUnit, angles.thirdAngle));
+ telemetry.update();
+
+ idle(); // Always call idle() at the bottom of your while(opModeIsActive()) loop
+ }
+ }
+
+ String formatRate(float rate) {
+ return String.format("%.3f", rate);
+ }
+
+ String formatAngle(AngleUnit angleUnit, double angle) {
+ return formatDegrees(AngleUnit.DEGREES.fromUnit(angleUnit, angle));
+ }
+
+ String formatDegrees(double degrees){
+ return String.format("%.1f", AngleUnit.DEGREES.normalize(degrees));
+ }
+}
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorLimelight3A.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorLimelight3A.java
new file mode 100644
index 0000000..2579cd0
--- /dev/null
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorLimelight3A.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.robotcontroller.external.samples;
+
+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 SensorLimelight3A 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();
+ }
+}
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorMRColor.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorMRColor.java
new file mode 100644
index 0000000..32b37e0
--- /dev/null
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorMRColor.java
@@ -0,0 +1,138 @@
+/* Copyright (c) 2017 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.robotcontroller.external.samples;
+
+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.ColorSensor;
+
+/*
+ *
+ * This OpMode that shows how to use
+ * a Modern Robotics Color Sensor.
+ *
+ * The OpMode assumes that the color sensor
+ * is configured with a name of "sensor_color".
+ *
+ * You can use the X button on gamepad1 to toggle the LED on and off.
+ *
+ * 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: MR Color", group = "Sensor")
+@Disabled
+public class SensorMRColor extends LinearOpMode {
+
+ ColorSensor colorSensor; // Hardware Device Object
+
+
+ @Override
+ public void runOpMode() {
+
+ // hsvValues is an array that will hold the hue, saturation, and value information.
+ float hsvValues[] = {0F,0F,0F};
+
+ // values is a reference to the hsvValues array.
+ final float values[] = hsvValues;
+
+ // get a reference to the RelativeLayout so we can 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());
+ final View relativeLayout = ((Activity) hardwareMap.appContext).findViewById(relativeLayoutId);
+
+ // bPrevState and bCurrState represent the previous and current state of the button.
+ boolean bPrevState = false;
+ boolean bCurrState = false;
+
+ // bLedOn represents the state of the LED.
+ boolean bLedOn = true;
+
+ // get a reference to our ColorSensor object.
+ colorSensor = hardwareMap.get(ColorSensor.class, "sensor_color");
+
+ // Set the LED in the beginning
+ colorSensor.enableLed(bLedOn);
+
+ // wait for the start button to be pressed.
+ waitForStart();
+
+ // while the OpMode is active, loop and read the RGB data.
+ // Note we use opModeIsActive() as our loop condition because it is an interruptible method.
+ while (opModeIsActive()) {
+
+ // check the status of the x button on either gamepad.
+ bCurrState = gamepad1.x;
+
+ // check for button state transitions.
+ if (bCurrState && (bCurrState != bPrevState)) {
+
+ // button is transitioning to a pressed state. So Toggle LED
+ bLedOn = !bLedOn;
+ colorSensor.enableLed(bLedOn);
+ }
+
+ // update previous state variable.
+ bPrevState = bCurrState;
+
+ // convert the RGB values to HSV values.
+ Color.RGBToHSV(colorSensor.red() * 8, colorSensor.green() * 8, colorSensor.blue() * 8, hsvValues);
+
+ // send the info back to driver station using telemetry function.
+ telemetry.addData("LED", bLedOn ? "On" : "Off");
+ telemetry.addData("Clear", colorSensor.alpha());
+ telemetry.addData("Red ", colorSensor.red());
+ telemetry.addData("Green", colorSensor.green());
+ telemetry.addData("Blue ", colorSensor.blue());
+ telemetry.addData("Hue", hsvValues[0]);
+
+ // change the background color to match the color detected by the RGB sensor.
+ // pass a reference to the hue, saturation, and value array as an argument
+ // to the HSVToColor method.
+ relativeLayout.post(new Runnable() {
+ public void run() {
+ relativeLayout.setBackgroundColor(Color.HSVToColor(0xff, values));
+ }
+ });
+
+ telemetry.update();
+ }
+
+ // Set the panel back to the default color
+ relativeLayout.post(new Runnable() {
+ public void run() {
+ relativeLayout.setBackgroundColor(Color.WHITE);
+ }
+ });
+ }
+}
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorMRGyro.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorMRGyro.java
new file mode 100644
index 0000000..91c0062
--- /dev/null
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorMRGyro.java
@@ -0,0 +1,160 @@
+/* Copyright (c) 2017 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.robotcontroller.external.samples;
+
+import com.qualcomm.hardware.modernrobotics.ModernRoboticsI2cGyro;
+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.IntegratingGyroscope;
+import com.qualcomm.robotcore.util.ElapsedTime;
+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;
+
+/*
+ * This OpMode shows how to use the Modern Robotics Gyro.
+ *
+ * The OpMode assumes that the gyro sensor is attached to a Device Interface Module
+ * I2C channel and is configured with a name of "gyro".
+ *
+ * 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: MR Gyro", group = "Sensor")
+@Disabled
+public class SensorMRGyro extends LinearOpMode {
+
+ /* In this sample, for illustration purposes we use two interfaces on the one gyro object.
+ * That's likely atypical: you'll probably use one or the other in any given situation,
+ * depending on what you're trying to do. {@link IntegratingGyroscope} (and it's base interface,
+ * {@link Gyroscope}) are common interfaces supported by possibly several different gyro
+ * implementations. {@link ModernRoboticsI2cGyro}, by contrast, provides functionality that
+ * is unique to the Modern Robotics gyro sensor.
+ */
+ IntegratingGyroscope gyro;
+ ModernRoboticsI2cGyro modernRoboticsI2cGyro;
+
+ // A timer helps provide feedback while calibration is taking place
+ ElapsedTime timer = new ElapsedTime();
+
+ @Override
+ public void runOpMode() {
+
+ boolean lastResetState = false;
+ boolean curResetState = false;
+
+ // Get a reference to a Modern Robotics gyro object. We use several interfaces
+ // on this object to illustrate which interfaces support which functionality.
+ modernRoboticsI2cGyro = hardwareMap.get(ModernRoboticsI2cGyro.class, "gyro");
+ gyro = (IntegratingGyroscope)modernRoboticsI2cGyro;
+ // If you're only interested int the IntegratingGyroscope interface, the following will suffice.
+ // gyro = hardwareMap.get(IntegratingGyroscope.class, "gyro");
+ // A similar approach will work for the Gyroscope interface, if that's all you need.
+
+ // Start calibrating the gyro. This takes a few seconds and is worth performing
+ // during the initialization phase at the start of each OpMode.
+ telemetry.log().add("Gyro Calibrating. Do Not Move!");
+ modernRoboticsI2cGyro.calibrate();
+
+ // Wait until the gyro calibration is complete
+ timer.reset();
+ while (!isStopRequested() && modernRoboticsI2cGyro.isCalibrating()) {
+ telemetry.addData("calibrating", "%s", Math.round(timer.seconds())%2==0 ? "|.." : "..|");
+ telemetry.update();
+ sleep(50);
+ }
+
+ telemetry.log().clear(); telemetry.log().add("Gyro Calibrated. Press Start.");
+ telemetry.clear(); telemetry.update();
+
+ // Wait for the start button to be pressed
+ waitForStart();
+ telemetry.log().clear();
+ telemetry.log().add("Press A & B to reset heading");
+
+ // Loop until we're asked to stop
+ while (opModeIsActive()) {
+
+ // If the A and B buttons are pressed just now, reset Z heading.
+ curResetState = (gamepad1.a && gamepad1.b);
+ if (curResetState && !lastResetState) {
+ modernRoboticsI2cGyro.resetZAxisIntegrator();
+ }
+ lastResetState = curResetState;
+
+ // The raw() methods report the angular rate of change about each of the
+ // three axes directly as reported by the underlying sensor IC.
+ int rawX = modernRoboticsI2cGyro.rawX();
+ int rawY = modernRoboticsI2cGyro.rawY();
+ int rawZ = modernRoboticsI2cGyro.rawZ();
+ int heading = modernRoboticsI2cGyro.getHeading();
+ int integratedZ = modernRoboticsI2cGyro.getIntegratedZValue();
+
+ // Read dimensionalized data from the gyro. This gyro can report angular velocities
+ // about all three axes. Additionally, it internally integrates the Z axis to
+ // be able to report an absolute angular Z orientation.
+ AngularVelocity rates = gyro.getAngularVelocity(AngleUnit.DEGREES);
+ float zAngle = gyro.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES).firstAngle;
+
+ // Read administrative information from the gyro
+ int zAxisOffset = modernRoboticsI2cGyro.getZAxisOffset();
+ int zAxisScalingCoefficient = modernRoboticsI2cGyro.getZAxisScalingCoefficient();
+
+ telemetry.addLine()
+ .addData("dx", formatRate(rates.xRotationRate))
+ .addData("dy", formatRate(rates.yRotationRate))
+ .addData("dz", "%s deg/s", formatRate(rates.zRotationRate));
+ telemetry.addData("angle", "%s deg", formatFloat(zAngle));
+ telemetry.addData("heading", "%3d deg", heading);
+ telemetry.addData("integrated Z", "%3d", integratedZ);
+ telemetry.addLine()
+ .addData("rawX", formatRaw(rawX))
+ .addData("rawY", formatRaw(rawY))
+ .addData("rawZ", formatRaw(rawZ));
+ telemetry.addLine().addData("z offset", zAxisOffset).addData("z coeff", zAxisScalingCoefficient);
+ telemetry.update();
+ }
+ }
+
+ String formatRaw(int rawValue) {
+ return String.format("%d", rawValue);
+ }
+
+ String formatRate(float rate) {
+ return String.format("%.3f", rate);
+ }
+
+ String formatFloat(float rate) {
+ return String.format("%.3f", rate);
+ }
+
+}
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorMROpticalDistance.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorMROpticalDistance.java
new file mode 100644
index 0000000..6d2dfa3
--- /dev/null
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorMROpticalDistance.java
@@ -0,0 +1,70 @@
+/* Copyright (c) 2017 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.robotcontroller.external.samples;
+
+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.OpticalDistanceSensor;
+
+/*
+ * This OpMode shows how to use a Modern Robotics Optical Distance Sensor
+ * It assumes that the ODS sensor is configured with a name of "sensor_ods".
+ *
+ * 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: MR ODS", group = "Sensor")
+@Disabled
+public class SensorMROpticalDistance extends LinearOpMode {
+
+ OpticalDistanceSensor odsSensor; // Hardware Device Object
+
+ @Override
+ public void runOpMode() {
+
+ // get a reference to our Light Sensor object.
+ odsSensor = hardwareMap.get(OpticalDistanceSensor.class, "sensor_ods");
+
+ // wait for the start button to be pressed.
+ waitForStart();
+
+ // while the OpMode is active, loop and read the light levels.
+ // Note we use opModeIsActive() as our loop condition because it is an interruptible method.
+ while (opModeIsActive()) {
+
+ // send the info back to driver station using telemetry function.
+ telemetry.addData("Raw", odsSensor.getRawLightDetected());
+ telemetry.addData("Normal", odsSensor.getLightDetected());
+
+ telemetry.update();
+ }
+ }
+}
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorMRRangeSensor.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorMRRangeSensor.java
new file mode 100644
index 0000000..2039ef9
--- /dev/null
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorMRRangeSensor.java
@@ -0,0 +1,70 @@
+/* Copyright (c) 2017 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.robotcontroller.external.samples;
+
+import com.qualcomm.hardware.modernrobotics.ModernRoboticsI2cRangeSensor;
+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.DistanceUnit;
+
+/*
+ * This OpMode illustrates how to use the Modern Robotics Range Sensor.
+ *
+ * The OpMode assumes that the range sensor is configured with a name of "sensor_range".
+ *
+ * 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
+ *
+ * @see MR Range Sensor
+ */
+@TeleOp(name = "Sensor: MR range sensor", group = "Sensor")
+@Disabled // comment out or remove this line to enable this OpMode
+public class SensorMRRangeSensor extends LinearOpMode {
+
+ ModernRoboticsI2cRangeSensor rangeSensor;
+
+ @Override public void runOpMode() {
+
+ // get a reference to our compass
+ rangeSensor = hardwareMap.get(ModernRoboticsI2cRangeSensor.class, "sensor_range");
+
+ // wait for the start button to be pressed
+ waitForStart();
+
+ while (opModeIsActive()) {
+ telemetry.addData("raw ultrasonic", rangeSensor.rawUltrasonic());
+ telemetry.addData("raw optical", rangeSensor.rawOptical());
+ telemetry.addData("cm optical", "%.2f cm", rangeSensor.cmOptical());
+ telemetry.addData("cm", "%.2f cm", rangeSensor.getDistance(DistanceUnit.CM));
+ telemetry.update();
+ }
+ }
+}
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorOctoQuad.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorOctoQuad.java
new file mode 100644
index 0000000..312d7f5
--- /dev/null
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorOctoQuad.java
@@ -0,0 +1,156 @@
+/*
+ * Copyright (c) 2025 DigitalChickenLabs
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+package org.firstinspires.ftc.robotcontroller.external.samples;
+
+import com.qualcomm.hardware.digitalchickenlabs.OctoQuad;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.util.ElapsedTime;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+
+/*
+ * This OpMode illustrates how to use DigitalChickenLabs OctoQuad Quad Encoder & Pulse Width I/F Module
+ *
+ * The OctoQuad has 8 input channels that can used to read either Relative Quadrature Encoders or
+ * Pulse-Width Absolute Encoder inputs. Relative Quadrature encoders are found on most FTC motors,
+ * and some stand-alone position sensors like the REV Thru-Bore encoder. Pulse-Width encoders are
+ * less common. The REV Thru-Bore encoder can provide its absolute position via a variable pulse width,
+ * as can several sonar rangefinders such as the MaxBotix MB1000 series.
+ *
+ * Note: SDK 11.0+ requires that the OctoQuad is running firmware V3.0 or greater.
+ * Visit https://github.com/DigitalChickenLabs/OctoQuad/tree/master/firmware for instruction
+ * on how to upgrade your OctoQuad's firmware.
+ *
+ * This basic sample shows how an OctoQuad can be used to read the position of three Odometry pods
+ * fitted with REV Thru-Bore encoders. For a more advanced example with additional OctoQuad
+ * capabilities, see the SensorOctoQuadAdv sample.
+ *
+ * This OpMode assumes the OctoQuad is attached to an I2C interface named "octoquad" in the robot config.
+ *
+ * The code assumes the first three OctoQuad inputs are connected as follows
+ * - Chan 0: for measuring forward motion on the left side of the robot.
+ * - Chan 1: for measuring forward motion on the right side of the robot.
+ * - Chan 2: for measuring Lateral (strafing) motion.
+ *
+ * The encoder values may be reset to zero by pressing the X (left most) button on Gamepad 1.
+ *
+ * This sample does not show how to interpret these readings, just how to obtain and display them.
+ *
+ * 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
+ *
+ * See the sensor's product page: https://www.tindie.com/products/35114/
+ */
+@Disabled
+@TeleOp(name = "OctoQuad Basic", group="OctoQuad")
+public class SensorOctoQuad extends LinearOpMode {
+
+ // Identify which encoder OctoQuad inputs are connected to each odometry pod.
+ private final int ODO_LEFT = 0; // Facing forward direction on left side of robot
+ private final int ODO_RIGHT = 1; // Facing forward direction on right side or robot
+ private final int ODO_PERP = 2; // Facing perpendicular direction at the center of the robot
+
+ // Declare the OctoQuad object;
+ private OctoQuad octoquad;
+
+ private int posLeft;
+ private int posRight;
+ private int posPerp;
+ private int velLeft;
+ private int velRight;
+ private int velPerp;
+
+ /**
+ * This function is executed when this OpMode is selected from the Driver Station.
+ */
+ @Override
+ public void runOpMode() {
+
+ // Connect to OctoQuad by referring to its name in the Robot Configuration.
+ octoquad = hardwareMap.get(OctoQuad.class, "octoquad");
+
+ // Read the Firmware Revision number from the OctoQuad and display it as telemetry.
+ telemetry.addData("OctoQuad Firmware Version ", octoquad.getFirmwareVersion());
+
+ // Reverse the count-direction of any encoder that is not what you require.
+ // e.g. if you push the robot forward and the left encoder counts down, then reverse it.
+ octoquad.setSingleEncoderDirection(ODO_LEFT, OctoQuad.EncoderDirection.REVERSE);
+ octoquad.setSingleEncoderDirection(ODO_RIGHT, OctoQuad.EncoderDirection.FORWARD);
+ octoquad.setSingleEncoderDirection(ODO_PERP, OctoQuad.EncoderDirection.FORWARD);
+
+ // set the interval over which pulses are counted to determine velocity.
+ octoquad.setAllVelocitySampleIntervals(50); // 50 mSec means 20 velocity updates per second.
+
+ // Save any changes that are made, just in case there is a sensor power glitch.
+ octoquad.saveParametersToFlash();
+
+ telemetry.addLine("\nPress START to read encoder values");
+ telemetry.update();
+
+ waitForStart();
+
+ // Configure the telemetry for optimal display of data.
+ telemetry.setDisplayFormat(Telemetry.DisplayFormat.MONOSPACE);
+ telemetry.setMsTransmissionInterval(100);
+
+ // Set all the encoder inputs to zero.
+ octoquad.resetAllPositions();
+
+ // Loop while displaying the odometry pod positions.
+ while (opModeIsActive()) {
+ telemetry.addData(">", "Press X to Reset Encoders\n");
+
+ // Check for X button to reset encoders.
+ if (gamepad1.x) {
+ // Reset the position of all encoders to zero.
+ octoquad.resetAllPositions();
+ }
+
+ // Read all the encoder data. Load into local members.
+ readOdometryPods();
+
+ // Display the values.
+ telemetry.addData("Left P", "%7d V :%6d CPS ", posLeft, velLeft);
+ telemetry.addData("Right P", "%7d V :%6d CPS ", posRight, velRight);
+ telemetry.addData("Perp P", "%7d V :%6d CPS ", posPerp, velPerp);
+ telemetry.update();
+ }
+ }
+
+ private void readOdometryPods() {
+ // For best performance, we should only perform ONE transaction with the OctoQuad each cycle.
+ // This can be achieved in one of two ways:
+ // 1) by doing a block data read once per control cycle
+ // or
+ // 2) by doing individual caching reads, but only reading each encoder value ONCE per cycle.
+ //
+ // Since method 2 is simplest, we will use it here.
+ posLeft = octoquad.readSinglePosition_Caching(ODO_LEFT);
+ posRight = octoquad.readSinglePosition_Caching(ODO_RIGHT);
+ posPerp = octoquad.readSinglePosition_Caching(ODO_PERP);
+ velLeft = octoquad.readSingleVelocity_Caching(ODO_LEFT) * 20; // scale up to cps
+ velRight = octoquad.readSingleVelocity_Caching(ODO_RIGHT) * 20; // scale up to cps
+ velPerp = octoquad.readSingleVelocity_Caching(ODO_PERP) * 20; // scale up to cps
+ }
+}
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorOctoQuadAdv.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorOctoQuadAdv.java
new file mode 100644
index 0000000..0e412ef
--- /dev/null
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorOctoQuadAdv.java
@@ -0,0 +1,288 @@
+/*
+ * Copyright (c) 2024 DigitalChickenLabs
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+package org.firstinspires.ftc.robotcontroller.external.samples;
+
+import com.qualcomm.hardware.digitalchickenlabs.OctoQuad;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.util.ElapsedTime;
+import com.qualcomm.robotcore.util.MovingStatistics;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+
+import java.util.ArrayList;
+import java.util.List;
+
+/*
+ * This OpMode illustrates how to use advanced features of the DigitalChickenLabs OctoQuad Quadrature
+ * Encoder & Pulse Width Interface Module. The OctoQuad has 8 input channels that can used to read
+ * either Quadrature Encoder signals (like with most FTC motors) or Pulse-Width style Absolute Encoder
+ * inputs (eg: REV Through Bore encoder pulse width output).
+ *
+ * This OpMode illustrates several of the more advanced features of an OctoQuad, including Pulse Width
+ * measurement and velocity measurement. For a more basic OpMode just showing how to read encoder
+ * inputs, see the SensorOctoQuad sample.
+ *
+ * This OpMode assumes the OctoQuad is attached to an I2C interface named "octoquad" in the robot config.
+ *
+ * One system that requires a lot of encoder inputs is a Swerve Drive system.
+ * Such a drive requires two encoders per drive module:
+ * One encoder for the Drive motor/wheel position.velocity, and one for Steer motor angle.
+ * The Drive motor requires a quadrature encoder, and the Steering motor requires an Absolute encoder.
+ *
+ * This sample will configure an OctoQuad for a swerve drive, as follows
+ * - Configure 4 Relative Quadrature Encoder inputs and 4 Absolute Pulse-Width Encoder inputs
+ * - Configure a velocity sample interval of 25 mSec
+ * - Configure a pulse width input range of 1-1024 uSec for a REV Encoder's Absolute Pulse output.
+ *
+ * An OctoSwerveDrive class will be created to initialize the OctoQuad, and manage 4 swerve modules.
+ * An OctoSwerveModule class will be created to configure and read a single swerve module.
+ *
+ * Wiring:
+ * The OctoQuad will be configured to accept Quadrature encoders on the first four channels and
+ * Absolute (pulse width) encoders on the last four channels.
+ *
+ * The standard encoder cable will connect each Driver Motor encoder to the OctoQuad. (channels 0-3)
+ *
+ * A modified version of the REV 6-4 pin cable (shipped with the encoder) connects the steering encoder
+ * to the OctoQuad. (channels 4-7)
+ *
+ * To connect the Absolute position signal from a REV Thru-Bore encoder to the OctoQuad, the
+ * Provided 6-pin to 4-pin cable will need to be modified.
+ * At the 6-pin connector end, move the yellow wire from its initial pin-3 position to the
+ * ABS pin-5 position. This can be done easily by using a small flathead screwdriver to lift the
+ * small white tab holding the metal wire crimp in place and gently pulling the wire out.
+ * See the OctoSwerveDrive() constructor below for the correct wheel/channel assignment.
+ *
+ * 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
+ *
+ * See the sensor's product page: https://www.tindie.com/products/35114/
+ */
+@Disabled
+@TeleOp(name="OctoQuad Advanced", group="OctoQuad")
+public class SensorOctoQuadAdv extends LinearOpMode {
+
+ @Override
+ public void runOpMode() throws InterruptedException {
+ // Connect to the OctoQuad by looking up its name in the hardwareMap.
+ OctoQuad octoquad = hardwareMap.get(OctoQuad.class, "octoquad");
+
+ // Create the interface for the Swerve Drive Encoders
+ OctoSwerveDrive octoSwerveDrive = new OctoSwerveDrive(octoquad);
+
+ // Display the OctoQuad firmware revision
+ telemetry.addLine("OctoQuad Firmware v" + octoquad.getFirmwareVersion());
+ telemetry.addLine("\nPress START to read encoder values");
+ telemetry.update();
+
+ waitForStart();
+
+ // Configure the telemetry for optimal display of data.
+ telemetry.setDisplayFormat(Telemetry.DisplayFormat.MONOSPACE);
+ telemetry.setMsTransmissionInterval(50);
+
+ // Run stats to determine cycle times.
+ MovingStatistics avgTime = new MovingStatistics(100);
+ ElapsedTime elapsedTime = new ElapsedTime();
+
+ while (opModeIsActive()) {
+ telemetry.addData(">", "Press X to Reset Encoders\n");
+
+ if(gamepad1.x) {
+ octoquad.resetAllPositions();
+ }
+
+ // read all the swerve drive encoders and update positions and velocities.
+ octoSwerveDrive.updateModules();
+ octoSwerveDrive.show(telemetry);
+
+ // Update cycle time stats
+ avgTime.add(elapsedTime.nanoseconds());
+ elapsedTime.reset();
+
+ telemetry.addData("Loop time", "%.1f mS", avgTime.getMean()/1000000);
+ telemetry.update();
+ }
+ }
+}
+
+// ============================ Internal (Inner) Classes =============================
+
+/***
+ * OctoSwerveDrive class manages 4 Swerve Modules
+ * - Performs general OctoQuad initialization
+ * - Creates 4 module classes, one for each swerve module
+ * - Updates swerve drive status by reading all data from OctoQuad and Updating each module
+ * - Displays all swerve drive data as telemetry
+ */
+class OctoSwerveDrive {
+
+ private final OctoQuad octoquad;
+ private final List allModules = new ArrayList<>();
+
+ // members to hold encoder data for each module.
+ public final OctoSwerveModule LeftFront;
+ public final OctoSwerveModule RightFront;
+ public final OctoSwerveModule LeftBack;
+ public final OctoSwerveModule RightBack;
+
+ // Prepare an object to hold an entire OctoQuad encoder readable register bank (pos and vel)
+ private final OctoQuad.EncoderDataBlock encoderDataBlock = new OctoQuad.EncoderDataBlock();
+
+ public OctoSwerveDrive(OctoQuad octoquad) {
+ this.octoquad = octoquad;
+
+ // Clear out all prior settings and encoder data before setting up desired configuration
+ octoquad.resetEverything();
+
+ // Assume first 4 channels are relative encoders and the next 4 are absolute encoders
+ octoquad.setChannelBankConfig(OctoQuad.ChannelBankConfig.BANK1_QUADRATURE_BANK2_PULSE_WIDTH);
+
+ // Create the four OctoSwerveModules, and add them to a 'list' for easier reference.
+
+ // Notes:
+ // The wheel/channel order is set here. Ensure your encoder cables match.
+ //
+ // The angleOffset must be set for each module so a forward facing wheel shows a steer
+ // angle of 0 degrees. The process for determining the correct angleOffsets is as follows:
+ // 1) Set all the angleValues (below) to zero then build and deploy the code.
+ // 2) Rotate all the swerve drives so the wheels are forward in the desired 0 degree position
+ // 3) Run the OpMode, view the telemetry and record the "Degrees" value for each module.
+ // 4) Update the code by entering the recorded Degrees value for each module as the
+ // angleOffset (last) parameter in the lines below.
+ //
+ // Rebuild and deploy the new code. Verify that the telemetry now indicates 0 degrees when
+ // the wheels are facing forward. Also verify that the correct module values change
+ // appropriately when you manually spin (drive) and rotate (steer) a wheel.
+
+ allModules.add(LeftFront = new OctoSwerveModule(octoquad, "LF ",0,0));// Drive=0, Steer=4
+ allModules.add(RightFront = new OctoSwerveModule(octoquad, "RF ",1,0));// Drive=1, Steer=5
+ allModules.add(LeftBack = new OctoSwerveModule(octoquad, "LB ",2,0));// Drive=2, Steer=6
+ allModules.add(RightBack = new OctoSwerveModule(octoquad, "RB ",3,0));// Drive=3, Steer=7
+
+ // now make sure the settings persist through any power glitches.
+ octoquad.saveParametersToFlash();
+ }
+
+ /**
+ * Updates all 4 swerve modules
+ */
+ public void updateModules() {
+ // Read full OctoQuad data block and then pass it to each swerve module to update themselves.
+ octoquad.readAllEncoderData(encoderDataBlock);
+ for (OctoSwerveModule module : allModules) {
+ module.updateModule(encoderDataBlock);
+ }
+ }
+
+ /**
+ * Generate telemetry data for all modules.
+ * @param telemetry OpMode Telemetry object
+ */
+ public void show(Telemetry telemetry) {
+ // create general header block and then have each module add its own telemetry
+ telemetry.addData("pos", " Count CPS Degree DPS");
+ for (OctoSwerveModule module : allModules) {
+ module.show(telemetry);
+ }
+ }
+}
+
+/***
+ * The OctoSwerveModule class manages a single swerve module
+ */
+class OctoSwerveModule {
+
+ public double driveCounts;
+ public double driveCountsPerSec;
+ public double steerDegrees;
+ public double steerDegreesPerSec;
+
+ private final String name;
+ private final int channel;
+ private final double angleOffset;
+
+ private static final int VELOCITY_SAMPLE_INTERVAL_MS = 25; // To provide 40 updates/Sec.
+ private static final double DEGREES_PER_US = (360.0 / 1024.0); // REV Through Bore Encoder
+ private static final double VELOCITY_SAMPLES_PER_S = (1000.0 / VELOCITY_SAMPLE_INTERVAL_MS);
+
+ // The correct drive and turn directions must be set for the Swerve Module.
+ // Forward motion must generate an increasing drive count.
+ // Counter Clockwise steer rotation must generate an increasing Steer Angle (degrees)
+ private static final boolean INVERT_DRIVE_ENCODER = false;
+ private static final boolean INVERT_STEER_ENCODER = false;
+
+ /***
+ * @param octoquad provide access to configure OctoQuad
+ * @param name name used for telemetry display
+ * @param quadChannel Quadrature encoder channel. Pulse Width channel is this + 4
+ * @param angleOffset Angle to subtract from absolute encoder to calibrate zero position.
+ */
+ public OctoSwerveModule (OctoQuad octoquad, String name, int quadChannel, double angleOffset) {
+ this.name = name;
+ this.channel = quadChannel;
+ this.angleOffset = angleOffset;
+
+ // Set both encoder directions.
+ octoquad.setSingleEncoderDirection(channel,
+ INVERT_DRIVE_ENCODER ? OctoQuad.EncoderDirection.REVERSE : OctoQuad.EncoderDirection.FORWARD);
+ octoquad.setSingleEncoderDirection(channel + 4,
+ INVERT_STEER_ENCODER ? OctoQuad.EncoderDirection.REVERSE : OctoQuad.EncoderDirection.FORWARD);
+
+ // Set the velocity sample interval on both encoders
+ octoquad.setSingleVelocitySampleInterval(channel, VELOCITY_SAMPLE_INTERVAL_MS);
+ octoquad.setSingleVelocitySampleInterval(channel + 4, VELOCITY_SAMPLE_INTERVAL_MS);
+
+ // Setup Absolute encoder pulse range to match REV Through Bore encoder.
+ octoquad.setSingleChannelPulseWidthParams (channel + 4,
+ new OctoQuad.ChannelPulseWidthParams(1,1024));
+ }
+
+ /***
+ * Calculate the Swerve module's position and velocity values
+ * @param encoderDataBlock most recent full data block read from OctoQuad.
+ */
+ public void updateModule(OctoQuad.EncoderDataBlock encoderDataBlock) {
+ driveCounts = encoderDataBlock.positions[channel];
+ driveCountsPerSec = encoderDataBlock.velocities[channel] * VELOCITY_SAMPLES_PER_S;
+
+ // convert uS to degrees. Add in any possible direction flip.
+ steerDegrees = AngleUnit.normalizeDegrees(
+ (encoderDataBlock.positions[channel+ 4] * DEGREES_PER_US) - angleOffset);
+ // convert uS/interval to deg per sec. Add in any possible direction flip.
+ steerDegreesPerSec = encoderDataBlock.velocities[channel + 4] *
+ DEGREES_PER_US * VELOCITY_SAMPLES_PER_S;
+ }
+
+ /**
+ * Display the Swerve module's state as telemetry
+ * @param telemetry OpMode Telemetry object
+ */
+ public void show(Telemetry telemetry) {
+ telemetry.addData(name, "%8.0f %7.0f %7.0f %6.0f",
+ driveCounts, driveCountsPerSec, steerDegrees, steerDegreesPerSec);
+ }
+}
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorOctoQuadLocalization.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorOctoQuadLocalization.java
new file mode 100644
index 0000000..486c468
--- /dev/null
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorOctoQuadLocalization.java
@@ -0,0 +1,255 @@
+/*
+ * Copyright (c) 2025 DigitalChickenLabs
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+package org.firstinspires.ftc.robotcontroller.external.samples;
+
+import com.qualcomm.hardware.digitalchickenlabs.OctoQuad;
+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.Telemetry;
+
+/*
+ * This OpMode demonstrates how to use the "absolute localizer" feature of the
+ * Digital Chicken OctoQuad FTC Edition MK2.
+ *
+ * Note: The MK2 OctoQuad has an Inertial Measurement Unit (IMU) that the MK1 does not, so this
+ * code will ONLY work with the MK2 version.
+ *
+ * It is STRONGLY recommended to read the Quick Start guide for the localizer feature, located here:
+ * https://github.com/DigitalChickenLabs/OctoQuad/blob/master/documentation/
+ *
+ * This OpMode assumes that the OctoQuad is attached to an I2C interface named "octoquad" in the
+ * robot configuration.
+ *
+ * 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
+ *
+ * See the sensor's product page: https://www.tindie.com/products/37799/
+ */
+
+@Disabled
+@TeleOp(name="OctoQuad Localizer", group="OctoQuad")
+public class SensorOctoQuadLocalization extends LinearOpMode
+{
+ // #####################################################################################
+ //
+ // YOU MUST ADJUST THESE CONSTANTS FOR YOUR ROBOT!
+ // SEE THE QUICKSTART GUIDE:
+ // https://github.com/DigitalChickenLabs/OctoQuad/blob/master/documentation/
+ //
+ // AND THE TUNING OPMODES:
+ // https://github.com/DigitalChickenLabs/OctoQuad/blob/master/code_examples/FTC
+ //
+ // #####################################################################################
+ static final int DEADWHEEL_PORT_X = 0; // encoder port on OctoQuad
+ static final int DEADWHEEL_PORT_Y = 1; // encoder port on OctoQuad
+ static final OctoQuad.EncoderDirection DEADWHEEL_X_DIR = OctoQuad.EncoderDirection.FORWARD;
+ static final OctoQuad.EncoderDirection DEADWHEEL_Y_DIR = OctoQuad.EncoderDirection.REVERSE;
+ static final float X_TICKS_PER_MM = 12.34f; // eg. 19.89f for a goBILDA 4-Bar Odometry Pod
+ static final float Y_TICKS_PER_MM = 12.34f; // eg. 19.89f for a goBILDA 4-Bar Odometry Pod
+ static final float TCP_OFFSET_X_MM = 123.4f; // eg. 147.0f from QuickStart Guide diagram
+ static final float TCP_OFFSET_Y_MM = 123.4f; // eg.-158.0f from QuickStart Guide diagram
+ static final float IMU_SCALAR = 1.0f; // Rotational scale factor.
+ // #####################################################################################
+
+ // Conversion factor for radians --> degrees
+ static final double RAD2DEG = 180/Math.PI;
+
+ // For tracking the number of CRC mismatches
+ int badPackets = 0;
+ int totalPackets = 0;
+ boolean warn = false;
+
+ // Data structure which will store the localizer data read from the OctoQuad
+ OctoQuad.LocalizerDataBlock localizer = new OctoQuad.LocalizerDataBlock();
+
+ /*
+ * Main OpMode function
+ */
+ public void runOpMode()
+ {
+ // Begin by retrieving a handle to the device from the hardware map.
+ OctoQuad oq = hardwareMap.get(OctoQuad.class, "octoquad");
+
+ // Increase the telemetry update frequency to make the data display a bit less laggy
+ telemetry.setMsTransmissionInterval(50);
+ telemetry.setDisplayFormat(Telemetry.DisplayFormat.HTML);
+
+ // Configure a range of parameters for the absolute localizer
+ // --> Read the quick start guide for an explanation of these!!
+ // IMPORTANT: these parameter changes will not take effect until the localizer is reset!
+ oq.setSingleEncoderDirection(DEADWHEEL_PORT_X, DEADWHEEL_X_DIR);
+ oq.setSingleEncoderDirection(DEADWHEEL_PORT_Y, DEADWHEEL_Y_DIR);
+ oq.setLocalizerPortX(DEADWHEEL_PORT_X);
+ oq.setLocalizerPortY(DEADWHEEL_PORT_Y);
+ oq.setLocalizerCountsPerMM_X(X_TICKS_PER_MM);
+ oq.setLocalizerCountsPerMM_Y(Y_TICKS_PER_MM);
+ oq.setLocalizerTcpOffsetMM_X(TCP_OFFSET_X_MM);
+ oq.setLocalizerTcpOffsetMM_Y(TCP_OFFSET_Y_MM);
+ oq.setLocalizerImuHeadingScalar(IMU_SCALAR);
+ oq.setLocalizerVelocityIntervalMS(25);
+ oq.setI2cRecoveryMode(OctoQuad.I2cRecoveryMode.MODE_1_PERIPH_RST_ON_FRAME_ERR);
+
+ // Resetting the localizer will apply the parameters configured above.
+ // This function will NOT block until calibration of the IMU is complete -
+ // for that you need to look at the status returned by getLocalizerStatus()
+ oq.resetLocalizerAndCalibrateIMU();
+
+ /*
+ * Init Loop
+ */
+ while (opModeInInit())
+ {
+ telemetry.addData("OQ Firmware Version", oq.getFirmwareVersionString());
+ telemetry.addData("Localizer Status", oq.getLocalizerStatus());
+ telemetry.addData("Heading Axis Detection", oq.getLocalizerHeadingAxisChoice());
+ telemetry.addLine(" ");
+
+ warnIfNotTuned();
+
+ telemetry.addLine("Press Play to start navigating");
+ telemetry.update();
+
+ sleep(100);
+ }
+
+ /*
+ * Don't proceed to the main loop until calibration is complete
+ */
+ while (!isStopRequested() && (oq.getLocalizerStatus() != OctoQuad.LocalizerStatus.RUNNING))
+ {
+ telemetry.addLine("Waiting for IMU Calibration to complete\n");
+ telemetry.addData("Localizer Status", oq.getLocalizerStatus());
+ telemetry.update();
+ sleep(100);
+ }
+
+ // Establish a starting position for the robot. This will be 0,0,0 by default so this line
+ // is rather redundant, but you could change it to be anything you want
+ oq.setLocalizerPose(0, 0, 0);
+
+ // Not required for localizer, but left in here since raw counts are displayed
+ // on telemetry for debugging
+ oq.resetAllPositions();
+
+ /*
+ * MAIN LOOP
+ */
+ while (opModeIsActive())
+ {
+ // Use the Gamepad A/Cross button to teleport to a new location and heading
+ if (gamepad1.crossWasPressed())
+ {
+ oq.setLocalizerPose(200, 200, (float)(Math.PI/2.0f));
+ }
+
+ // Read updated data from the OctoQuad into the 'localizer' data structure
+ oq.readLocalizerData(localizer);
+
+ // #################################################################################
+ // IMPORTANT! Check whether the received CRC for the data is correct. An incorrect
+ // CRC indicates that the data was corrupted in transit and cannot be
+ // trusted. This can be caused by internal or external ESD.
+ //
+ // If the CRC is NOT reported as OK, you should throw away the data and
+ // try to read again.
+ //
+ // NOTE: Raw odometry pod counts are displayed to verify correct direction of rotation
+ // When the robot is pushed FWD, the X pod counts must increase in value
+ // When the robot is pushed LEFT, the Y pod counts must increase in value
+ // Use the setSingleEncoderDirection() method to make any reversals.
+ // #################################################################################
+ if (localizer.crcOk)
+ {
+ warnIfNotTuned();
+
+ // Display Robot position data
+ telemetry.addData("Localizer Status", localizer.localizerStatus);
+ telemetry.addData("Heading deg", "%.2f", localizer.heading_rad * RAD2DEG);
+ telemetry.addData("Rotation dps", "%.2f", localizer.velHeading_radS * RAD2DEG);
+ telemetry.addData("X Pos mm", localizer.posX_mm);
+ telemetry.addData("Y Pos mm", localizer.posY_mm);
+ telemetry.addData("X Vel mm/s", localizer.velX_mmS);
+ telemetry.addData("Y Vel mm/s", localizer.velY_mmS);
+ telemetry.addLine("\nPress Gamepad A (Cross) to teleport");
+
+ // #############################################################################
+ // IMPORTANT!!
+ //
+ // These two encoder status lines will slow the loop down,
+ // so they can be removed once the encoder direction has been verified.
+ // #############################################################################
+ telemetry.addData("\nRaw X Pod Counts", oq.readSinglePosition_Caching(DEADWHEEL_PORT_X));
+ telemetry.addData("Raw Y Pod Counts", oq.readSinglePosition_Caching(DEADWHEEL_PORT_Y));
+ }
+ else
+ {
+ badPackets++;
+ telemetry.addLine("Data CRC not valid");
+ }
+ totalPackets++;
+
+ // Print some statistics about CRC validation
+ telemetry.addLine(String.format("%d CRC error(s) in %d packets", badPackets, totalPackets));
+
+ // Send updated telemetry to the Driver Station
+ telemetry.update();
+ }
+ }
+
+ long lastWarnFlashTimeMs = 0;
+ boolean warnFlash = false;
+
+ void warnIfNotTuned()
+ {
+ String warnString = "";
+ if (IMU_SCALAR == 1.0f)
+ {
+ warnString += "WARNING: IMU_SCALAR not tuned.
";
+ warn = true;
+ }
+ if (X_TICKS_PER_MM == 12.34f || Y_TICKS_PER_MM == 12.34f)
+ {
+ warnString += "WARNING: TICKS_PER_MM not tuned.
";
+ warn = true;
+ }
+ if (TCP_OFFSET_X_MM == 123.4f || TCP_OFFSET_Y_MM == 123.4f)
+ {
+ warnString += "WARNING: TCP_OFFSET not tuned.
";
+ warn = true;
+ }
+ if (warn)
+ {
+ warnString +="
- Read the code COMMENTS for tuning help.
";
+
+ if (System.currentTimeMillis() - lastWarnFlashTimeMs > 1000)
+ {
+ lastWarnFlashTimeMs = System.currentTimeMillis();
+ warnFlash = !warnFlash;
+ }
+
+ telemetry.addLine(String.format("%s",
+ warnFlash ? "red" : "white", warnString));
+ }
+ }
+}
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorREV2mDistance.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorREV2mDistance.java
new file mode 100644
index 0000000..13883c3
--- /dev/null
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorREV2mDistance.java
@@ -0,0 +1,87 @@
+/*
+Copyright (c) 2018 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.robotcontroller.external.samples;
+
+import com.qualcomm.hardware.rev.Rev2mDistanceSensor;
+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 org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
+
+/*
+ * This OpMode illustrates how to use the REV Robotics 2M Distance Sensor.
+ *
+ * The OpMode assumes that the sensor is configured with a name of "sensor_distance".
+ *
+ * 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
+ *
+ * See the sensor's product page: https://www.revrobotics.com/rev-31-1505/
+ */
+@TeleOp(name = "Sensor: REV2mDistance", group = "Sensor")
+@Disabled
+public class SensorREV2mDistance extends LinearOpMode {
+
+ private DistanceSensor sensorDistance;
+
+ @Override
+ public void runOpMode() {
+ // you can use this as a regular DistanceSensor.
+ sensorDistance = hardwareMap.get(DistanceSensor.class, "sensor_distance");
+
+ // you can also cast this to a Rev2mDistanceSensor if you want to use added
+ // methods associated with the Rev2mDistanceSensor class.
+ Rev2mDistanceSensor sensorTimeOfFlight = (Rev2mDistanceSensor) sensorDistance;
+
+ telemetry.addData(">>", "Press start to continue");
+ telemetry.update();
+
+ waitForStart();
+ while(opModeIsActive()) {
+ // generic DistanceSensor methods.
+ telemetry.addData("deviceName", sensorDistance.getDeviceName() );
+ telemetry.addData("range", String.format("%.01f mm", sensorDistance.getDistance(DistanceUnit.MM)));
+ telemetry.addData("range", String.format("%.01f cm", sensorDistance.getDistance(DistanceUnit.CM)));
+ telemetry.addData("range", String.format("%.01f m", sensorDistance.getDistance(DistanceUnit.METER)));
+ telemetry.addData("range", String.format("%.01f in", sensorDistance.getDistance(DistanceUnit.INCH)));
+
+ // Rev2mDistanceSensor specific methods.
+ telemetry.addData("ID", String.format("%x", sensorTimeOfFlight.getModelID()));
+ telemetry.addData("did time out", Boolean.toString(sensorTimeOfFlight.didTimeoutOccur()));
+
+ telemetry.update();
+ }
+ }
+
+}
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorSparkFunOTOS.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorSparkFunOTOS.java
new file mode 100644
index 0000000..3a25230
--- /dev/null
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorSparkFunOTOS.java
@@ -0,0 +1,156 @@
+/*
+ SPDX-License-Identifier: MIT
+
+ Copyright (c) 2024 SparkFun Electronics
+*/
+package org.firstinspires.ftc.robotcontroller.external.samples;
+
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import com.qualcomm.hardware.sparkfun.SparkFunOTOS;
+
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
+
+/*
+ * This OpMode illustrates how to use the SparkFun Qwiic Optical Tracking Odometry Sensor (OTOS)
+ *
+ * The OpMode assumes that the sensor is configured with a name of "sensor_otos".
+ *
+ * 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
+ *
+ * See the sensor's product page: https://www.sparkfun.com/products/24904
+ */
+@TeleOp(name = "Sensor: SparkFun OTOS", group = "Sensor")
+@Disabled
+public class SensorSparkFunOTOS extends LinearOpMode {
+ // Create an instance of the sensor
+ SparkFunOTOS myOtos;
+
+ @Override
+ public void runOpMode() throws InterruptedException {
+ // Get a reference to the sensor
+ myOtos = hardwareMap.get(SparkFunOTOS.class, "sensor_otos");
+
+ // All the configuration for the OTOS is done in this helper method, check it out!
+ configureOtos();
+
+ // Wait for the start button to be pressed
+ waitForStart();
+
+ // Loop until the OpMode ends
+ while (opModeIsActive()) {
+ // Get the latest position, which includes the x and y coordinates, plus the
+ // heading angle
+ SparkFunOTOS.Pose2D pos = myOtos.getPosition();
+
+ // Reset the tracking if the user requests it
+ if (gamepad1.y) {
+ myOtos.resetTracking();
+ }
+
+ // Re-calibrate the IMU if the user requests it
+ if (gamepad1.x) {
+ myOtos.calibrateImu();
+ }
+
+ // Inform user of available controls
+ telemetry.addLine("Press Y (triangle) on Gamepad to reset tracking");
+ telemetry.addLine("Press X (square) on Gamepad to calibrate the IMU");
+ telemetry.addLine();
+
+ // Log the position to the telemetry
+ telemetry.addData("X coordinate", pos.x);
+ telemetry.addData("Y coordinate", pos.y);
+ telemetry.addData("Heading angle", pos.h);
+
+ // Update the telemetry on the driver station
+ telemetry.update();
+ }
+ }
+
+ private void configureOtos() {
+ telemetry.addLine("Configuring OTOS...");
+ telemetry.update();
+
+ // Set the desired units for linear and angular measurements. Can be either
+ // meters or inches for linear, and radians or degrees for angular. If not
+ // set, the default is inches and degrees. Note that this setting is not
+ // persisted in the sensor, so you need to set at the start of all your
+ // OpModes if using the non-default value.
+ // myOtos.setLinearUnit(DistanceUnit.METER);
+ myOtos.setLinearUnit(DistanceUnit.INCH);
+ // myOtos.setAngularUnit(AnguleUnit.RADIANS);
+ myOtos.setAngularUnit(AngleUnit.DEGREES);
+
+ // Assuming you've mounted your sensor to a robot and it's not centered,
+ // you can specify the offset for the sensor relative to the center of the
+ // robot. The units default to inches and degrees, but if you want to use
+ // different units, specify them before setting the offset! Note that as of
+ // firmware version 1.0, these values will be lost after a power cycle, so
+ // you will need to set them each time you power up the sensor. For example, if
+ // the sensor is mounted 5 inches to the left (negative X) and 10 inches
+ // forward (positive Y) of the center of the robot, and mounted 90 degrees
+ // clockwise (negative rotation) from the robot's orientation, the offset
+ // would be {-5, 10, -90}. These can be any value, even the angle can be
+ // tweaked slightly to compensate for imperfect mounting (eg. 1.3 degrees).
+ SparkFunOTOS.Pose2D offset = new SparkFunOTOS.Pose2D(0, 0, 0);
+ myOtos.setOffset(offset);
+
+ // Here we can set the linear and angular scalars, which can compensate for
+ // scaling issues with the sensor measurements. Note that as of firmware
+ // version 1.0, these values will be lost after a power cycle, so you will
+ // need to set them each time you power up the sensor. They can be any value
+ // from 0.872 to 1.127 in increments of 0.001 (0.1%). It is recommended to
+ // first set both scalars to 1.0, then calibrate the angular scalar, then
+ // the linear scalar. To calibrate the angular scalar, spin the robot by
+ // multiple rotations (eg. 10) to get a precise error, then set the scalar
+ // to the inverse of the error. Remember that the angle wraps from -180 to
+ // 180 degrees, so for example, if after 10 rotations counterclockwise
+ // (positive rotation), the sensor reports -15 degrees, the required scalar
+ // would be 3600/3585 = 1.004. To calibrate the linear scalar, move the
+ // robot a known distance and measure the error; do this multiple times at
+ // multiple speeds to get an average, then set the linear scalar to the
+ // inverse of the error. For example, if you move the robot 100 inches and
+ // the sensor reports 103 inches, set the linear scalar to 100/103 = 0.971
+ myOtos.setLinearScalar(1.0);
+ myOtos.setAngularScalar(1.0);
+
+ // The IMU on the OTOS includes a gyroscope and accelerometer, which could
+ // have an offset. Note that as of firmware version 1.0, the calibration
+ // will be lost after a power cycle; the OTOS performs a quick calibration
+ // when it powers up, but it is recommended to perform a more thorough
+ // calibration at the start of all your OpModes. Note that the sensor must
+ // be completely stationary and flat during calibration! When calling
+ // calibrateImu(), you can specify the number of samples to take and whether
+ // to wait until the calibration is complete. If no parameters are provided,
+ // it will take 255 samples and wait until done; each sample takes about
+ // 2.4ms, so about 612ms total
+ myOtos.calibrateImu();
+
+ // Reset the tracking algorithm - this resets the position to the origin,
+ // but can also be used to recover from some rare tracking errors
+ myOtos.resetTracking();
+
+ // After resetting the tracking, the OTOS will report that the robot is at
+ // the origin. If your robot does not start at the origin, or you have
+ // another source of location information (eg. vision odometry), you can set
+ // the OTOS location to match and it will continue to track from there.
+ SparkFunOTOS.Pose2D currentPosition = new SparkFunOTOS.Pose2D(0, 0, 0);
+ myOtos.setPosition(currentPosition);
+
+ // Get the hardware and firmware version
+ SparkFunOTOS.Version hwVersion = new SparkFunOTOS.Version();
+ SparkFunOTOS.Version fwVersion = new SparkFunOTOS.Version();
+ myOtos.getVersionInfo(hwVersion, fwVersion);
+
+ telemetry.addLine("OTOS configured! Press start to get position data!");
+ telemetry.addLine();
+ telemetry.addLine(String.format("OTOS Hardware Version: v%d.%d", hwVersion.major, hwVersion.minor));
+ telemetry.addLine(String.format("OTOS Firmware Version: v%d.%d", fwVersion.major, fwVersion.minor));
+ telemetry.update();
+ }
+}
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorTouch.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorTouch.java
new file mode 100644
index 0000000..3d79447
--- /dev/null
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorTouch.java
@@ -0,0 +1,78 @@
+/* Copyright (c) 2017 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.robotcontroller.external.samples;
+
+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.TouchSensor;
+
+/*
+ * This OpMode demonstrates how to use a REV Robotics Touch Sensor, REV Robotics Magnetic Limit Switch, or other device
+ * that implements the TouchSensor interface. Any touch sensor that connects its output to ground when pressed
+ * (known as "active low") can be configured as a "REV Touch Sensor". This includes REV's Magnetic Limit Switch.
+ *
+ * The OpMode assumes that the touch sensor is configured with a name of "sensor_touch".
+ *
+ * A REV Robotics Touch Sensor must be configured on digital port number 1, 3, 5, or 7.
+ * A Magnetic Limit Switch can be configured on any digital port.
+ *
+ * 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: REV touch sensor", group = "Sensor")
+@Disabled
+public class SensorTouch extends LinearOpMode {
+ TouchSensor touchSensor; // Touch sensor Object
+
+ @Override
+ public void runOpMode() {
+
+ // get a reference to our touchSensor object.
+ touchSensor = hardwareMap.get(TouchSensor.class, "sensor_touch");
+
+ // wait for the start button to be pressed.
+ waitForStart();
+
+ // while the OpMode is active, loop and read whether the sensor is being pressed.
+ // Note we use opModeIsActive() as our loop condition because it is an interruptible method.
+ while (opModeIsActive()) {
+
+ // send the info back to driver station using telemetry function.
+ if (touchSensor.isPressed()) {
+ telemetry.addData("Touch Sensor", "Is Pressed");
+ } else {
+ telemetry.addData("Touch Sensor", "Is Not Pressed");
+ }
+
+ telemetry.update();
+ }
+ }
+}
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/UtilityCameraFrameCapture.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/UtilityCameraFrameCapture.java
new file mode 100644
index 0000000..69420cc
--- /dev/null
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/UtilityCameraFrameCapture.java
@@ -0,0 +1,127 @@
+/*
+ * 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.
+ */
+
+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;
+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 java.util.Locale;
+
+/*
+ * This OpMode helps calibrate a webcam or RC phone camera, useful for AprilTag pose estimation
+ * with the FTC VisionPortal. It captures a camera frame (image) and stores it on the Robot Controller
+ * (Control Hub or RC phone), with each press of the gamepad button X (or Square).
+ * Full calibration instructions are here:
+ *
+ * https://ftc-docs.firstinspires.org/camera-calibration
+ *
+ * In Android Studio, copy this class into your "teamcode" folder with a new name.
+ * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list.
+ *
+ * In OnBot Java, use "Add File" to add this OpMode from the list of Samples.
+ */
+
+@TeleOp(name = "Utility: Camera Frame Capture", group = "Utility")
+@Disabled
+public class UtilityCameraFrameCapture extends LinearOpMode
+{
+ /*
+ * EDIT THESE PARAMETERS AS NEEDED
+ */
+ final boolean USING_WEBCAM = false;
+ final BuiltinCameraDirection INTERNAL_CAM_DIR = BuiltinCameraDirection.BACK;
+ final int RESOLUTION_WIDTH = 640;
+ final int RESOLUTION_HEIGHT = 480;
+
+ // Internal state
+ boolean lastX;
+ int frameCount;
+ long capReqTime;
+
+ @Override
+ public void runOpMode()
+ {
+ VisionPortal portal;
+
+ if (USING_WEBCAM)
+ {
+ portal = new VisionPortal.Builder()
+ .setCamera(hardwareMap.get(WebcamName.class, "Webcam 1"))
+ .setCameraResolution(new Size(RESOLUTION_WIDTH, RESOLUTION_HEIGHT))
+ .build();
+ }
+ else
+ {
+ portal = new VisionPortal.Builder()
+ .setCamera(INTERNAL_CAM_DIR)
+ .setCameraResolution(new Size(RESOLUTION_WIDTH, RESOLUTION_HEIGHT))
+ .build();
+ }
+
+ while (!isStopRequested())
+ {
+ boolean x = gamepad1.x;
+
+ if (x && !lastX)
+ {
+ portal.saveNextFrameRaw(String.format(Locale.US, "CameraFrameCapture-%06d", frameCount++));
+ capReqTime = System.currentTimeMillis();
+ }
+
+ lastX = x;
+
+ telemetry.addLine("######## Camera Capture Utility ########");
+ telemetry.addLine(String.format(Locale.US, " > Resolution: %dx%d", RESOLUTION_WIDTH, RESOLUTION_HEIGHT));
+ telemetry.addLine(" > Press X (or Square) to capture a frame");
+ telemetry.addData(" > Camera Status", portal.getCameraState());
+
+ if (capReqTime != 0)
+ {
+ telemetry.addLine("\nCaptured Frame!");
+ }
+
+ if (capReqTime != 0 && System.currentTimeMillis() - capReqTime > 1000)
+ {
+ capReqTime = 0;
+ }
+
+ telemetry.update();
+ }
+ }
+}
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/UtilityOctoQuadConfigMenu.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/UtilityOctoQuadConfigMenu.java
new file mode 100644
index 0000000..60be6c9
--- /dev/null
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/UtilityOctoQuadConfigMenu.java
@@ -0,0 +1,821 @@
+/*
+ * Copyright (c) 2024 DigitalChickenLabs
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+package org.firstinspires.ftc.robotcontroller.external.samples;
+
+import com.qualcomm.hardware.digitalchickenlabs.OctoQuad;
+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.Gamepad;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+
+import java.util.ArrayList;
+import java.util.Stack;
+
+/*
+ * This OpMode illustrates how to use the DigitalChickenLabs OctoQuad Quadrature Encoder & Pulse Width Interface Module.
+ *
+ * The OctoQuad has 8 input channels that can used to read either Relative Quadrature Encoders or Pulse-Width Absolute Encoder inputs.
+ * Relative Quadrature encoders are found on most FTC motors, and some stand-alone position sensors like the REV Thru-Bore encoder.
+ * Pulse-Width encoders are less common. The REV Thru-Bore encoder can provide its absolute position via a variable pulse width,
+ * as can several sonar rangefinders such as the MaxBotix MB1000 series.
+ *
+ * This OpMode assumes that the OctoQuad is attached to an I2C interface named "octoquad" in the robot configuration.
+ *
+ * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list
+ *
+ * Select, Init and run the "OctoQuad Configuration Tool" OpMode
+ * Read the blue User-Interface tips at the top of the telemetry screen.
+ * Use the UI buttons to navigate the menu and make any desired changes to the OctoQuad configuration.
+ * Use the Program Settings To FLASH option to make any changes permanent.
+ *
+ * See the sensor's product page: https://www.tindie.com/products/digitalchickenlabs/octoquad-8ch-quadrature-pulse-width-decoder/
+ */
+@TeleOp(name = "OctoQuad Configuration Tool", group="OctoQuad")
+@Disabled
+public class UtilityOctoQuadConfigMenu extends LinearOpMode
+{
+ TelemetryMenu.MenuElement rootMenu = new TelemetryMenu.MenuElement("OctoQuad Config Menu", true);
+ TelemetryMenu.MenuElement menuHwInfo = new TelemetryMenu.MenuElement("Hardware Information", false);
+ TelemetryMenu.EnumOption optionI2cResetMode;
+ TelemetryMenu.EnumOption optionChannelBankConfig;
+
+ TelemetryMenu.MenuElement menuEncoderDirections = new TelemetryMenu.MenuElement("Set Encoder Directions", false);
+ TelemetryMenu.BooleanOption[] optionsEncoderDirections = new TelemetryMenu.BooleanOption[OctoQuad.NUM_ENCODERS];
+
+ TelemetryMenu.MenuElement menuVelocityIntervals = new TelemetryMenu.MenuElement("Velocity Measurement Intervals", false);
+ TelemetryMenu.IntegerOption[] optionsVelocityIntervals = new TelemetryMenu.IntegerOption[OctoQuad.NUM_ENCODERS];
+
+ TelemetryMenu.MenuElement menuAbsParams = new TelemetryMenu.MenuElement("Abs. Encoder Pulse Width Params", false);
+ TelemetryMenu.IntegerOption[] optionsAbsParamsMax = new TelemetryMenu.IntegerOption[OctoQuad.NUM_ENCODERS];
+ TelemetryMenu.IntegerOption[] optionsAbsParamsMin = new TelemetryMenu.IntegerOption[OctoQuad.NUM_ENCODERS];
+ TelemetryMenu.BooleanOption[] optionsAbsParamsWrapTracking = new TelemetryMenu.BooleanOption[OctoQuad.NUM_ENCODERS];
+
+ TelemetryMenu.OptionElement optionProgramToFlash;
+ TelemetryMenu.OptionElement optionSendToRAM;
+
+ TelemetryMenu.StaticClickableOption optionExit;
+
+ OctoQuad octoquad;
+
+ boolean error = false;
+
+ @Override
+ public void runOpMode()
+ {
+ octoquad = hardwareMap.getAll(OctoQuad.class).get(0);
+
+ if(octoquad.getChipId() != OctoQuad.OCTOQUAD_CHIP_ID)
+ {
+ telemetry.addLine("Error: cannot communicate with OctoQuad. Check your wiring and configuration and try again");
+ telemetry.update();
+
+ error = true;
+ }
+ else
+ {
+ if(octoquad.getFirmwareVersion().maj != OctoQuad.SUPPORTED_FW_VERSION_MAJ)
+ {
+ telemetry.addLine("Error: The OctoQuad is running a different major firmware version than this driver was built for. Cannot run configuration tool");
+ telemetry.update();
+
+ error = true;
+ }
+ }
+
+ if(error)
+ {
+ waitForStart();
+ return;
+ }
+
+ telemetry.addLine("Retrieving current configuration from OctoQuad");
+ telemetry.update();
+
+ optionExit = new TelemetryMenu.StaticClickableOption("Exit configuration menu")
+ {
+ @Override
+ void onClick() // called on OpMode thread
+ {
+ requestOpModeStop();
+ }
+ };
+
+ optionI2cResetMode = new TelemetryMenu.EnumOption("I2C Reset Mode", OctoQuad.I2cRecoveryMode.values(), octoquad.getI2cRecoveryMode());
+ optionChannelBankConfig = new TelemetryMenu.EnumOption("Channel Bank Modes", OctoQuad.ChannelBankConfig.values(), octoquad.getChannelBankConfig());
+
+ menuHwInfo.addChild(new TelemetryMenu.StaticItem("Board Firmware: v" + octoquad.getFirmwareVersion()));
+ //menuHwInfo.addChild(new TelemetryMenu.StaticItem("Board unique ID: FIXME"));
+
+ for(int i = 0; i < OctoQuad.NUM_ENCODERS; i++)
+ {
+ optionsEncoderDirections[i] = new TelemetryMenu.BooleanOption(
+ String.format("Encoder %d direction", i),
+ octoquad.getSingleEncoderDirection(i) == OctoQuad.EncoderDirection.REVERSE,
+ "-",
+ "+");
+ }
+ menuEncoderDirections.addChildren(optionsEncoderDirections);
+
+ for(int i = 0; i < OctoQuad.NUM_ENCODERS; i++)
+ {
+ optionsVelocityIntervals[i] = new TelemetryMenu.IntegerOption(
+ String.format("Chan %d velocity intvl", i),
+ OctoQuad.MIN_VELOCITY_MEASUREMENT_INTERVAL_MS,
+ OctoQuad.MAX_VELOCITY_MEASUREMENT_INTERVAL_MS,
+ octoquad.getSingleVelocitySampleInterval(i));
+ }
+ menuVelocityIntervals.addChildren(optionsVelocityIntervals);
+
+ for(int i = 0; i < OctoQuad.NUM_ENCODERS; i++)
+ {
+ OctoQuad.ChannelPulseWidthParams params = octoquad.getSingleChannelPulseWidthParams(i);
+
+ optionsAbsParamsMax[i] = new TelemetryMenu.IntegerOption(
+ String.format("Chan %d max pulse length", i),
+ OctoQuad.MIN_PULSE_WIDTH_US,
+ OctoQuad.MAX_PULSE_WIDTH_US,
+ params.max_length_us);
+
+ optionsAbsParamsMin[i] = new TelemetryMenu.IntegerOption(
+ String.format("Chan %d min pulse length", i),
+ OctoQuad.MIN_PULSE_WIDTH_US,
+ OctoQuad.MAX_PULSE_WIDTH_US,
+ params.min_length_us);
+
+ optionsAbsParamsWrapTracking[i] = new TelemetryMenu.BooleanOption(
+ String.format("Chan %d wrap tracking enabled", i),
+ octoquad.getSingleChannelPulseWidthTracksWrap(i),
+ "yes",
+ "no");
+ }
+ menuAbsParams.addChildren(optionsAbsParamsMin);
+ menuAbsParams.addChildren(optionsAbsParamsMax);
+ menuAbsParams.addChildren(optionsAbsParamsWrapTracking);
+
+ optionProgramToFlash = new TelemetryMenu.OptionElement()
+ {
+ String name = "Program Settings to FLASH";
+ long lastClickTime = 0;
+
+ @Override
+ protected String getDisplayText()
+ {
+ if(lastClickTime == 0)
+ {
+ return name;
+ }
+ else
+ {
+ if(System.currentTimeMillis() - lastClickTime < 1000)
+ {
+ return name + " **OK**";
+ }
+ else
+ {
+ lastClickTime = 0;
+ return name;
+ }
+ }
+ }
+
+ @Override
+ void onClick()
+ {
+ sendSettingsToRam();
+ octoquad.saveParametersToFlash();
+ lastClickTime = System.currentTimeMillis();
+ }
+ };
+
+ optionSendToRAM = new TelemetryMenu.OptionElement()
+ {
+ String name = "Send Settings to RAM";
+ long lastClickTime = 0;
+
+ @Override
+ protected String getDisplayText()
+ {
+ if(lastClickTime == 0)
+ {
+ return name;
+ }
+ else
+ {
+ if(System.currentTimeMillis() - lastClickTime < 1000)
+ {
+ return name + " **OK**";
+ }
+ else
+ {
+ lastClickTime = 0;
+ return name;
+ }
+ }
+ }
+
+ @Override
+ void onClick()
+ {
+ sendSettingsToRam();
+ lastClickTime = System.currentTimeMillis();
+ }
+ };
+
+ rootMenu.addChild(menuHwInfo);
+ rootMenu.addChild(optionI2cResetMode);
+ rootMenu.addChild(optionChannelBankConfig);
+ rootMenu.addChild(menuEncoderDirections);
+ rootMenu.addChild(menuVelocityIntervals);
+ rootMenu.addChild(menuAbsParams);
+ rootMenu.addChild(optionProgramToFlash);
+ rootMenu.addChild(optionSendToRAM);
+ rootMenu.addChild(optionExit);
+
+ TelemetryMenu menu = new TelemetryMenu(telemetry, rootMenu);
+
+ while (!isStopRequested())
+ {
+ menu.loop(gamepad1);
+ telemetry.update();
+ sleep(20);
+ }
+ }
+
+ void sendSettingsToRam()
+ {
+ for(int i = 0; i < OctoQuad.NUM_ENCODERS; i++)
+ {
+ octoquad.setSingleEncoderDirection(i, optionsEncoderDirections[i].getValue() ? OctoQuad.EncoderDirection.REVERSE : OctoQuad.EncoderDirection.FORWARD);
+ octoquad.setSingleVelocitySampleInterval(i, optionsVelocityIntervals[i].getValue());
+
+ OctoQuad.ChannelPulseWidthParams params = new OctoQuad.ChannelPulseWidthParams();
+ params.max_length_us = optionsAbsParamsMax[i].getValue();
+ params.min_length_us = optionsAbsParamsMin[i].getValue();
+
+ octoquad.setSingleChannelPulseWidthParams(i, params);
+ octoquad.setSingleChannelPulseWidthTracksWrap(i, optionsAbsParamsWrapTracking[i].val);
+ }
+
+ octoquad.setI2cRecoveryMode((OctoQuad.I2cRecoveryMode) optionI2cResetMode.getValue());
+ octoquad.setChannelBankConfig((OctoQuad.ChannelBankConfig) optionChannelBankConfig.getValue());
+ }
+
+ /*
+ * Copyright (c) 2023 OpenFTC Team
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+ public static class TelemetryMenu
+ {
+ private final MenuElement root;
+ private MenuElement currentLevel;
+
+ private boolean dpadUpPrev;
+ private boolean dpadDnPrev;
+ private boolean dpadRightPrev;
+ private boolean dpadLeftPrev;
+ private boolean xPrev;
+ private boolean lbPrev;
+
+ private int selectedIdx = 0;
+ private Stack selectedIdxStack = new Stack<>();
+
+ private final Telemetry telemetry;
+
+ /**
+ * TelemetryMenu constructor
+ * @param telemetry pass in 'telemetry' from your OpMode
+ * @param root the root menu element
+ */
+ public TelemetryMenu(Telemetry telemetry, MenuElement root)
+ {
+ this.root = root;
+ this.currentLevel = root;
+ this.telemetry = telemetry;
+
+ telemetry.setDisplayFormat(Telemetry.DisplayFormat.HTML);
+ telemetry.setMsTransmissionInterval(50);
+ }
+
+ /**
+ * Call this from inside your loop to put the current menu state into
+ * telemetry, and process gamepad inputs for navigating the menu
+ * @param gamepad the gamepad you want to use to navigate the menu
+ */
+ public void loop(Gamepad gamepad)
+ {
+ // Capture current state of the gamepad buttons we care about;
+ // We can only look once or we risk a race condition
+ boolean dpadUp = gamepad.dpad_up;
+ boolean dpadDn = gamepad.dpad_down;
+ boolean dpadRight = gamepad.dpad_right;
+ boolean dpadLeft = gamepad.dpad_left;
+ boolean x = gamepad.x;
+ boolean lb = gamepad.left_bumper;
+
+ // Figure out who our children our at this level
+ // and figure out which item is currently highlighted
+ // with the selection pointer
+ ArrayList children = currentLevel.children();
+ Element currentSelection = children.get(selectedIdx);
+
+ // Left and right are inputs to the selected item (if it's an Option)
+ if (currentSelection instanceof OptionElement)
+ {
+ if (dpadRight && !dpadRightPrev) // rising edge
+ {
+ ((OptionElement) currentSelection).onRightInput();
+ }
+ else if (dpadLeft && !dpadLeftPrev) // rising edge
+ {
+ ((OptionElement) currentSelection).onLeftInput();
+ }
+ }
+
+ // Up and down navigate the current selection pointer
+ if (dpadUp && !dpadUpPrev) // rising edge
+ {
+ selectedIdx--; // Move selection pointer up
+ }
+ else if (dpadDn && !dpadDnPrev) // rising edge
+ {
+ selectedIdx++; // Move selection pointer down
+ }
+
+ // Make selected index sane (don't let it go out of bounds) :eyes:
+ if (selectedIdx >= children.size())
+ {
+ selectedIdx = children.size()-1;
+ }
+ else if (selectedIdx < 0)
+ {
+ selectedIdx = 0;
+ }
+
+ // Select: either enter submenu or input to option
+ else if (x && !xPrev) // rising edge
+ {
+ // Select up element
+ if (currentSelection instanceof SpecialUpElement)
+ {
+ // We can only go up if we're not at the root level
+ if (currentLevel != root)
+ {
+ // Restore selection pointer to where it was before
+ selectedIdx = selectedIdxStack.pop();
+
+ // Change to the parent level
+ currentLevel = currentLevel.parent();
+ }
+ }
+ // Input to option
+ else if (currentSelection instanceof OptionElement)
+ {
+ ((OptionElement) currentSelection).onClick();
+ }
+ // Enter submenu
+ else if (currentSelection instanceof MenuElement)
+ {
+ // Save our current selection pointer so we can restore it
+ // later if the user navigates back up a level
+ selectedIdxStack.push(selectedIdx);
+
+ // We have no idea what's in the submenu :monkey: so best to
+ // just set the selection pointer to the first element
+ selectedIdx = 0;
+
+ // Now the current level becomes the submenu that the selection
+ // pointer was on
+ currentLevel = (MenuElement) currentSelection;
+ }
+ }
+
+ // Go up a level
+ else if (lb && !lbPrev)
+ {
+ // We can only go up if we're not at the root level
+ if (currentLevel != root)
+ {
+ // Restore selection pointer to where it was before
+ selectedIdx = selectedIdxStack.pop();
+
+ // Change to the parent level
+ currentLevel = currentLevel.parent();
+ }
+ }
+
+ // Save the current button states so that we can look for
+ // the rising edge the next time around the loop :)
+ dpadUpPrev = dpadUp;
+ dpadDnPrev = dpadDn;
+ dpadRightPrev = dpadRight;
+ dpadLeftPrev = dpadLeft;
+ xPrev = x;
+ lbPrev = lb;
+
+ // Start building the text display.
+ // First, we add the static directions for gamepad operation
+ StringBuilder builder = new StringBuilder();
+ builder.append("");
+ builder.append("Navigate items.....dpad up/down\n")
+ .append("Select.............X or Square\n")
+ .append("Edit option........dpad left/right\n")
+ .append("Up one level.......left bumper\n");
+ builder.append("");
+ builder.append("\n");
+
+ // Now actually add the menu options. We start by adding the name of the current menu level.
+ builder.append("");
+ builder.append("Current Menu: ").append(currentLevel.name).append("\n");
+
+ // Now we loop through all the child elements of this level and add them
+ for (int i = 0; i < children.size(); i++)
+ {
+ // If the selection pointer is at this index, put a green dot in the box :)
+ if (selectedIdx == i)
+ {
+ builder.append("[•] ");
+ }
+ // Otherwise, just put an empty box
+ else
+ {
+ builder.append("[ ] ");
+ }
+
+ // Figure out who the selection pointer is pointing at :eyes:
+ Element e = children.get(i);
+
+ // If it's pointing at a submenu, indicate that it's a submenu to the user
+ // by prefixing "> " to the name.
+ if (e instanceof MenuElement)
+ {
+ builder.append("> ");
+ }
+
+ // Finally, add the element's name
+ builder.append(e.getDisplayText());
+
+ // We musn't forget the newline
+ builder.append("\n");
+ }
+
+ // Don't forget to close the font tag either
+ builder.append("");
+
+ // Build the string!!!! :nerd:
+ String menu = builder.toString();
+
+ // Add it to telemetry
+ telemetry.addLine(menu);
+ }
+
+ public static class MenuElement extends Element
+ {
+ private String name;
+ private ArrayList children = new ArrayList<>();
+
+ /**
+ * Create a new MenuElement; may either be the root menu, or a submenu (set isRoot accordingly)
+ * @param name the name for this menu
+ * @param isRoot whether this is a root menu, or a submenu
+ */
+ public MenuElement(String name, boolean isRoot)
+ {
+ this.name = name;
+
+ // If it's not the root menu, we add the up one level option as the first element
+ if (!isRoot)
+ {
+ children.add(new SpecialUpElement());
+ }
+ }
+
+ /**
+ * Add a child element to this menu (may either be an Option or another menu)
+ * @param child the child element to add
+ */
+ public void addChild(Element child)
+ {
+ child.setParent(this);
+ children.add(child);
+ }
+
+ /**
+ * Add multiple child elements to this menu (may either be option, or another menu)
+ * @param children the children to add
+ */
+ public void addChildren(Element[] children)
+ {
+ for (Element e : children)
+ {
+ e.setParent(this);
+ this.children.add(e);
+ }
+ }
+
+ @Override
+ protected String getDisplayText()
+ {
+ return name;
+ }
+
+ private ArrayList children()
+ {
+ return children;
+ }
+ }
+
+ public static abstract class OptionElement extends Element
+ {
+ /**
+ * Override this to get notified when the element is clicked
+ */
+ void onClick() {}
+
+ /**
+ * Override this to get notified when the element gets a "left edit" input
+ */
+ protected void onLeftInput() {}
+
+ /**
+ * Override this to get notified when the element gets a "right edit" input
+ */
+ protected void onRightInput() {}
+ }
+
+ public static class EnumOption extends OptionElement
+ {
+ protected int idx = 0;
+ protected Enum[] e;
+ protected String name;
+
+ public EnumOption(String name, Enum[] e)
+ {
+ this.e = e;
+ this.name = name;
+ }
+
+ public EnumOption(String name, Enum[] e, Enum def)
+ {
+ this(name, e);
+ idx = def.ordinal();
+ }
+
+ @Override
+ public void onLeftInput()
+ {
+ idx++;
+
+ if(idx > e.length-1)
+ {
+ idx = 0;
+ }
+ }
+
+ @Override
+ public void onRightInput()
+ {
+ idx--;
+
+ if(idx < 0)
+ {
+ idx = e.length-1;
+ }
+ }
+
+ @Override
+ public void onClick()
+ {
+ //onRightInput();
+ }
+
+ @Override
+ protected String getDisplayText()
+ {
+ return String.format("%s: %s", name, e[idx].name());
+ }
+
+ public Enum getValue()
+ {
+ return e[idx];
+ }
+ }
+
+ public static class IntegerOption extends OptionElement
+ {
+ protected int i;
+ protected int min;
+ protected int max;
+ protected String name;
+
+ public IntegerOption(String name, int min, int max, int def)
+ {
+ this.name = name;
+ this.min = min;
+ this.max = max;
+ this.i = def;
+ }
+
+ @Override
+ public void onLeftInput()
+ {
+ i--;
+
+ if(i < min)
+ {
+ i = max;
+ }
+ }
+
+ @Override
+ public void onRightInput()
+ {
+ i++;
+
+ if(i > max)
+ {
+ i = min;
+ }
+ }
+
+ @Override
+ public void onClick()
+ {
+ //onRightInput();
+ }
+
+ @Override
+ protected String getDisplayText()
+ {
+ return String.format("%s: %d", name, i);
+ }
+
+ public int getValue()
+ {
+ return i;
+ }
+ }
+
+ static class BooleanOption extends OptionElement
+ {
+ private String name;
+ private boolean val = true;
+
+ private String customTrue;
+ private String customFalse;
+
+ BooleanOption(String name, boolean def)
+ {
+ this.name = name;
+ this.val = def;
+ }
+
+ BooleanOption(String name, boolean def, String customTrue, String customFalse)
+ {
+ this(name, def);
+ this.customTrue = customTrue;
+ this.customFalse = customFalse;
+ }
+
+ @Override
+ public void onLeftInput()
+ {
+ val = !val;
+ }
+
+ @Override
+ public void onRightInput()
+ {
+ val = !val;
+ }
+
+ @Override
+ public void onClick()
+ {
+ val = !val;
+ }
+
+ @Override
+ protected String getDisplayText()
+ {
+ String valStr;
+
+ if(customTrue != null && customFalse != null)
+ {
+ valStr = val ? customTrue : customFalse;
+ }
+ else
+ {
+ valStr = val ? "true" : "false";
+ }
+
+ return String.format("%s: %s", name, valStr);
+ }
+
+ public boolean getValue()
+ {
+ return val;
+ }
+ }
+
+ /**
+ *
+ */
+ public static class StaticItem extends OptionElement
+ {
+ private String name;
+
+ public StaticItem(String name)
+ {
+ this.name = name;
+ }
+
+ @Override
+ protected String getDisplayText()
+ {
+ return name;
+ }
+ }
+
+ public static abstract class StaticClickableOption extends OptionElement
+ {
+ private String name;
+
+ public StaticClickableOption(String name)
+ {
+ this.name = name;
+ }
+
+ abstract void onClick();
+
+ @Override
+ protected String getDisplayText()
+ {
+ return name;
+ }
+ }
+
+ private static abstract class Element
+ {
+ private MenuElement parent;
+
+ protected void setParent(MenuElement parent)
+ {
+ this.parent = parent;
+ }
+
+ protected MenuElement parent()
+ {
+ return parent;
+ }
+
+ protected abstract String getDisplayText();
+ }
+
+ private static class SpecialUpElement extends Element
+ {
+ @Override
+ protected String getDisplayText()
+ {
+ return ".. ↰ Up One Level";
+ }
+ }
+ }
+}
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/externalhardware/ConceptExternalHardwareClass.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/externalhardware/ConceptExternalHardwareClass.java
new file mode 100644
index 0000000..7a721fe
--- /dev/null
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/externalhardware/ConceptExternalHardwareClass.java
@@ -0,0 +1,142 @@
+/* Copyright (c) 2022 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.robotcontroller.external.samples;
+
+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.util.Range;
+
+/*
+ * This OpMode illustrates how to use an external "hardware" class to modularize all the robot's sensors and actuators.
+ * This approach is very efficient because the same hardware class can be used by all of your teleop and autonomous OpModes
+ * without requiring many copy & paste operations. Once you have defined and tested the hardware class with one OpMode,
+ * it is instantly available to other OpModes.
+ *
+ * The real benefit of this approach is that as you tweak your robot hardware, you only need to make changes in ONE place (the Hardware Class).
+ * So, to be effective you should put as much or your hardware setup and access code as possible in the hardware class.
+ * Essentially anything you do with hardware in BOTH Teleop and Auto should likely go in the hardware class.
+ *
+ * The Hardware Class is created in a separate file, and then an "instance" of this class is created in each OpMode.
+ * In order for the class to do typical OpMode things (like send telemetry data) it must be passed a reference to the
+ * OpMode object when it's created, so it can access all core OpMode functions. This is illustrated below.
+ *
+ * In this concept sample, the hardware class file is called RobotHardware.java and it must accompany this sample OpMode.
+ * So, if you copy ConceptExternalHardwareClass.java into TeamCode (using Android Studio or OnBotJava) then RobotHardware.java
+ * must also be copied to the same location (maintaining its name).
+ *
+ * For comparison purposes, this sample and its accompanying hardware class duplicates the functionality of the
+ * RobotTelopPOV_Linear OpMode. It assumes three motors (left_drive, right_drive and arm) and two servos (left_hand and right_hand)
+ *
+ * View the RobotHardware.java class file for more details
+ *
+ * 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
+ *
+ * In OnBot Java, add a new OpMode, select this sample, and select TeleOp.
+ * Also add another new file named RobotHardware.java, select the sample with that name, and select Not an OpMode.
+ */
+
+@TeleOp(name="Concept: Robot Hardware Class", group="Robot")
+@Disabled
+public class ConceptExternalHardwareClass extends LinearOpMode {
+
+ // Create a RobotHardware object to be used to access robot hardware.
+ // Prefix any hardware functions with "robot." to access this class.
+ RobotHardware robot = new RobotHardware(this);
+
+ @Override
+ public void runOpMode() {
+ double drive = 0;
+ double turn = 0;
+ double arm = 0;
+ double handOffset = 0;
+
+ // initialize all the hardware, using the hardware class. See how clean and simple this is?
+ robot.init();
+
+ // Send telemetry message to signify robot waiting;
+ // Wait for the game to start (driver presses START)
+ waitForStart();
+
+ // run until the end of the match (driver presses STOP)
+ while (opModeIsActive()) {
+
+ // Run wheels in POV mode (note: The joystick goes negative when pushed forward, so negate it)
+ // In this mode the Left stick moves the robot fwd and back, the Right stick turns left and right.
+ // This way it's also easy to just drive straight, or just turn.
+ drive = -gamepad1.left_stick_y;
+ turn = gamepad1.right_stick_x;
+
+ // Combine drive and turn for blended motion. Use RobotHardware class
+ robot.driveRobot(drive, turn);
+
+ // Use gamepad left & right Bumpers to open and close the claw
+ // Use the SERVO constants defined in RobotHardware class.
+ // Each time around the loop, the servos will move by a small amount.
+ // Limit the total offset to half of the full travel range
+ if (gamepad1.right_bumper)
+ handOffset += robot.HAND_SPEED;
+ else if (gamepad1.left_bumper)
+ handOffset -= robot.HAND_SPEED;
+ handOffset = Range.clip(handOffset, -0.5, 0.5);
+
+ // Move both servos to new position. Use RobotHardware class
+ robot.setHandPositions(handOffset);
+
+ // Use gamepad buttons to move arm up (Y) and down (A)
+ // Use the MOTOR constants defined in RobotHardware class.
+ if (gamepad1.y)
+ arm = robot.ARM_UP_POWER;
+ else if (gamepad1.a)
+ arm = robot.ARM_DOWN_POWER;
+ else
+ arm = 0;
+
+ robot.setArmPower(arm);
+
+ // Send telemetry messages to explain controls and show robot status
+ telemetry.addData("Drive", "Left Stick");
+ telemetry.addData("Turn", "Right Stick");
+ telemetry.addData("Arm Up/Down", "Y & A Buttons");
+ telemetry.addData("Hand Open/Closed", "Left and Right Bumpers");
+ telemetry.addData("-", "-------");
+
+ telemetry.addData("Drive Power", "%.2f", drive);
+ telemetry.addData("Turn Power", "%.2f", turn);
+ telemetry.addData("Arm Power", "%.2f", arm);
+ telemetry.addData("Hand Position", "Offset = %.2f", handOffset);
+ telemetry.update();
+
+ // Pace this loop so hands move at a reasonable speed.
+ sleep(50);
+ }
+ }
+}
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/externalhardware/RobotHardware.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/externalhardware/RobotHardware.java
new file mode 100644
index 0000000..b1c8de6
--- /dev/null
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/externalhardware/RobotHardware.java
@@ -0,0 +1,167 @@
+/* Copyright (c) 2022 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.robotcontroller.external.samples;
+
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.Servo;
+import com.qualcomm.robotcore.util.Range;
+
+/*
+ * This file works in conjunction with the External Hardware Class sample called: ConceptExternalHardwareClass.java
+ * Please read the explanations in that Sample about how to use this class definition.
+ *
+ * This file defines a Java Class that performs all the setup and configuration for a sample robot's hardware (motors and sensors).
+ * It assumes three motors (left_drive, right_drive and arm) and two servos (left_hand and right_hand)
+ *
+ * This one file/class can be used by ALL of your OpModes without having to cut & paste the code each time.
+ *
+ * Where possible, the actual hardware objects are "abstracted" (or hidden) so the OpMode code just makes calls into the class,
+ * rather than accessing the internal hardware directly. This is why the objects are declared "private".
+ *
+ * Use Android Studio to Copy this Class, and Paste it into your team's code folder with *exactly the same name*.
+ *
+ * Or... In OnBot Java, add a new file named RobotHardware.java, select this sample, and select Not an OpMode.
+ * Also add a new OpMode, select the sample ConceptExternalHardwareClass.java, and select TeleOp.
+ *
+ */
+
+public class RobotHardware {
+
+ /* Declare OpMode members. */
+ private LinearOpMode myOpMode = null; // gain access to methods in the calling OpMode.
+
+ // Define Motor and Servo objects (Make them private so they can't be accessed externally)
+ private DcMotor leftDrive = null;
+ private DcMotor rightDrive = null;
+ private DcMotor armMotor = null;
+ private Servo leftHand = null;
+ private Servo rightHand = null;
+
+ // Define Drive constants. Make them public so they CAN be used by the calling OpMode
+ public static final double MID_SERVO = 0.5 ;
+ public static final double HAND_SPEED = 0.02 ; // sets rate to move servo
+ public static final double ARM_UP_POWER = 0.45 ;
+ public static final double ARM_DOWN_POWER = -0.45 ;
+
+ // Define a constructor that allows the OpMode to pass a reference to itself.
+ public RobotHardware (LinearOpMode opmode) {
+ myOpMode = opmode;
+ }
+
+ /**
+ * Initialize all the robot's hardware.
+ * This method must be called ONCE when the OpMode is initialized.
+ *
+ * All of the hardware devices are accessed via the hardware map, and initialized.
+ */
+ public void init() {
+ // Define and Initialize Motors (note: need to use reference to actual OpMode).
+ leftDrive = myOpMode.hardwareMap.get(DcMotor.class, "left_drive");
+ rightDrive = myOpMode.hardwareMap.get(DcMotor.class, "right_drive");
+ armMotor = myOpMode.hardwareMap.get(DcMotor.class, "arm");
+
+ // 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);
+
+ // If there are encoders connected, switch to RUN_USING_ENCODER mode for greater accuracy
+ // leftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+ // rightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+
+ // Define and initialize ALL installed servos.
+ leftHand = myOpMode.hardwareMap.get(Servo.class, "left_hand");
+ rightHand = myOpMode.hardwareMap.get(Servo.class, "right_hand");
+ leftHand.setPosition(MID_SERVO);
+ rightHand.setPosition(MID_SERVO);
+
+ myOpMode.telemetry.addData(">", "Hardware Initialized");
+ myOpMode.telemetry.update();
+ }
+
+ /**
+ * Calculates the left/right motor powers required to achieve the requested
+ * robot motions: Drive (Axial motion) and Turn (Yaw motion).
+ * Then sends these power levels to the motors.
+ *
+ * @param Drive Fwd/Rev driving power (-1.0 to 1.0) +ve is forward
+ * @param Turn Right/Left turning power (-1.0 to 1.0) +ve is CW
+ */
+ public void driveRobot(double Drive, double Turn) {
+ // Combine drive and turn for blended motion.
+ double left = Drive + Turn;
+ double right = Drive - Turn;
+
+ // Scale the values so neither exceed +/- 1.0
+ double max = Math.max(Math.abs(left), Math.abs(right));
+ if (max > 1.0)
+ {
+ left /= max;
+ right /= max;
+ }
+
+ // Use existing function to drive both wheels.
+ setDrivePower(left, right);
+ }
+
+ /**
+ * Pass the requested wheel motor powers to the appropriate hardware drive motors.
+ *
+ * @param leftWheel Fwd/Rev driving power (-1.0 to 1.0) +ve is forward
+ * @param rightWheel Fwd/Rev driving power (-1.0 to 1.0) +ve is forward
+ */
+ public void setDrivePower(double leftWheel, double rightWheel) {
+ // Output the values to the motor drives.
+ leftDrive.setPower(leftWheel);
+ rightDrive.setPower(rightWheel);
+ }
+
+ /**
+ * Pass the requested arm power to the appropriate hardware drive motor
+ *
+ * @param power driving power (-1.0 to 1.0)
+ */
+ public void setArmPower(double power) {
+ armMotor.setPower(power);
+ }
+
+ /**
+ * Send the two hand-servos to opposing (mirrored) positions, based on the passed offset.
+ *
+ * @param offset
+ */
+ public void setHandPositions(double offset) {
+ offset = Range.clip(offset, -0.5, 0.5);
+ leftHand.setPosition(MID_SERVO + offset);
+ rightHand.setPosition(MID_SERVO - offset);
+ }
+}
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/readme.md b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/readme.md
new file mode 100644
index 0000000..326978d
--- /dev/null
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/readme.md
@@ -0,0 +1,45 @@
+
+## Caution
+No Team-specific code should be placed or modified in this ``.../samples`` folder.
+
+Samples should be Copied from here, and then Pasted into the team's
+[/TeamCode/src/main/java/org/firstinspires/ftc/teamcode](../../../../../../../../../../TeamCode/src/main/java/org/firstinspires/ftc/teamcode)
+ folder, using the Android Studio cut and paste commands. This automatically changes all file and
+class names to be consistent. From there, the sample can be modified to suit the team's needs.
+
+For more detailed instructions see the /teamcode readme.
+
+### Naming of Samples
+
+To gain a better understanding of how the samples are organized, and how to interpret the
+naming system, it will help to understand the conventions that were used during their creation.
+
+These conventions are described (in detail) in the sample_conventions.md file in this folder.
+
+To summarize: A range of different samples classes will reside in the java/external/samples.
+The class names will follow a naming convention which indicates the purpose of each class.
+The prefix of the name will be one of the following:
+
+Basic: This is a minimally functional OpMode used to illustrate the skeleton/structure
+ of a particular style of OpMode. These are bare bones examples.
+
+Sensor: This is a Sample OpMode that shows how to use a specific sensor.
+ It is not intended to drive a functioning robot, it is simply showing the minimal code
+ required to read and display the sensor values.
+
+Robot: This is a Sample OpMode that assumes a simple two-motor (differential) drive base.
+ It may be used to provide a common baseline driving OpMode, or
+ to demonstrate how a particular sensor or concept can be used to navigate.
+
+Concept: This is a sample OpMode that illustrates performing a specific function or concept.
+ These may be complex, but their operation should be explained clearly in the comments,
+ or the comments should reference an external doc, guide or tutorial.
+ Each OpMode should try to only demonstrate a single concept so they are easy to
+ locate based on their name. These OpModes may not produce a drivable robot.
+
+After the prefix, other conventions will apply:
+
+* Sensor class names are constructed as: Sensor - Company - Type
+* Robot class names are constructed as: Robot - Mode - Action - OpModetype
+* Concept class names are constructed as: Concept - Topic - OpModetype
+
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/sample_conventions.md b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/sample_conventions.md
new file mode 100644
index 0000000..e85e625
--- /dev/null
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/sample_conventions.md
@@ -0,0 +1,113 @@
+## Sample Class/Opmode conventions
+#### V 1.1.0 8/9/2017
+
+This document defines the FTC Sample OpMode and Class conventions.
+
+### OpMode Name
+
+To gain a better understanding of how the samples are organized, and how to interpret the
+naming system, it will help to understand the conventions that were used during their creation.
+
+To summarize: A range of different samples classes will reside in the java/external/samples.
+The class names will follow a naming convention which indicates the purpose of each class.
+The prefix of the name will be one of the following:
+
+Basic: This is a minimally functional OpMode used to illustrate the skeleton/structure
+ of a particular style of OpMode. These are bare bones examples.
+
+Sensor: This is a Sample OpMode that shows how to use a specific sensor.
+ It is not intended to drive a functioning robot, it is simply showing the minimal code
+ required to read and display the sensor values.
+
+Robot: This is a Sample OpMode that assumes a simple two-motor (differential) drive base.
+ It may be used to provide a common baseline driving OpMode, or
+ to demonstrate how a particular sensor or concept can be used to navigate.
+
+Concept: This is a sample OpMode that illustrates performing a specific function or concept.
+ These may be complex, but their operation should be explained clearly in the comments,
+ or the comments should reference an external doc, guide or tutorial.
+ Each OpMode should try to only demonstrate a single concept so they are easy to
+ locate based on their name. These OpModes may not produce a drivable robot.
+
+Utility: This sample type is provided as a useful tool, or aide, to perform some specific development task.
+ It is not expected to be something you would include in your own robot code.
+ To use the tool, comment out the @Disabled annotation and build the App.
+ Read the comments found in the sample for an explanation of its intended use.
+
+After the prefix, other conventions will apply:
+
+* Sensor class names should constructed as: Sensor - Company - Type
+* Robot class names should be constructed as: Robot - Mode - Action - OpModetype
+* Concept class names should be constructed as: Concept - Topic - OpModetype
+
+### Sample OpMode Content/Style
+
+Code is formatted as per the Google Style Guide:
+
+https://google.github.io/styleguide/javaguide.html
+
+With “Sensor” and “Hardware” samples, the code should demonstrate the essential function,
+and not be embellished with too much additional “clever” code. If a sensor has special
+addressing needs, or has a variety of modes or outputs, these should be demonstrated as
+simply as possible.
+
+Special programming methods, or robot control techniques should be reserved for “Concept” Samples,
+and where possible, Samples should strive to only demonstrate a single concept,
+eg: State machine coding, or a User Menu system, and not combine them into a single “all inclusive”
+sample. This will prevent an “all inclusive” Sample being deleted just because one part of it
+becomes obsolete.
+
+### Device Configuration Names
+
+The following device names are used in the external samples
+
+** Motors:
+left_drive
+right_drive
+left_arm
+
+** Servos:
+left_hand
+right_hand
+arm
+claw
+
+** Sensors:
+sensor_color
+sensor_ir
+sensor_light
+sensor_ods
+sensor_range
+sensor_touch
+sensor_color_distance
+sensor_digital
+digin
+digout
+
+** Localization:
+compass
+gyro
+imu
+navx
+
+### Device Object Names
+
+Device Object names should use the same words as the device’s configuration name, but they
+should be re-structured to be a suitable Java variable name. This should keep the same word order,
+but adopt the style of beginning with a lower case letter, and then each subsequent word
+starting with an upper case letter.
+
+Eg: from the examples above: tool, leftMotor, rightClawServo, rearLightSensor.
+
+Note: Sometimes it’s helpful to put the device type first, followed by the variant.
+eg: motorLeft and motorRight, but this should only be done if the same word order
+is used on the device configuration name.
+
+### OpMode code Comments
+
+Sample comments should read like normal code comments, that is, as an explanation of what the
+sample code is doing. They should NOT be directives to the user,
+like: “insert your joystick code here” as these comments typically aren’t
+detailed enough to be useful. They also often get left in the code and become garbage.
+
+Instead, an example of the joystick code should be shown with a comment describing what it is doing.
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/internal/FtcOpModeRegister.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/internal/FtcOpModeRegister.java
new file mode 100644
index 0000000..ceab67d
--- /dev/null
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/internal/FtcOpModeRegister.java
@@ -0,0 +1,68 @@
+/* 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. */
+
+package org.firstinspires.ftc.robotcontroller.internal;
+
+import com.qualcomm.robotcore.eventloop.opmode.OpModeManager;
+import com.qualcomm.robotcore.eventloop.opmode.OpModeRegister;
+
+/**
+ * {@link FtcOpModeRegister} is responsible for registering OpModes for use in an FTC game.
+ * @see #register(OpModeManager)
+ */
+public class FtcOpModeRegister implements OpModeRegister {
+
+ /**
+ * {@link #register(OpModeManager)} is called by the SDK game in order to register
+ * OpMode classes or instances that will participate in an FTC game.
+ *
+ * There are two mechanisms by which an OpMode may be registered.
+ *
+ * 1) The preferred method is by means of class annotations in the OpMode itself.
+ * See, for example the class annotations in {@link org.firstinspires.ftc.robotcontroller.external.samples.ConceptNullOp}.
+ *
+ * 2) The other, retired, method is to modify this {@link #register(OpModeManager)}
+ * method to include explicit calls to OpModeManager.register().
+ * This method of modifying this file directly is discouraged, as it
+ * makes updates to the SDK harder to integrate into your code.
+ *
+ * @param manager the object which contains methods for carrying out OpMode registrations
+ *
+ * @see com.qualcomm.robotcore.eventloop.opmode.TeleOp
+ * @see com.qualcomm.robotcore.eventloop.opmode.Autonomous
+ */
+ public void register(OpModeManager manager) {
+
+ /**
+ * Any manual OpMode class registrations should go here.
+ */
+ }
+}
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/internal/FtcRobotControllerActivity.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/internal/FtcRobotControllerActivity.java
new file mode 100644
index 0000000..3f1f77c
--- /dev/null
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/internal/FtcRobotControllerActivity.java
@@ -0,0 +1,845 @@
+/* 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. */
+
+package org.firstinspires.ftc.robotcontroller.internal;
+
+import android.app.ActionBar;
+import android.app.Activity;
+import android.app.ActivityManager;
+import android.content.ComponentName;
+import android.content.Context;
+import android.content.Intent;
+import android.content.ServiceConnection;
+import android.content.SharedPreferences;
+import android.content.res.Configuration;
+import android.hardware.usb.UsbDevice;
+import android.hardware.usb.UsbManager;
+import android.net.wifi.WifiManager;
+import android.os.Bundle;
+import android.os.IBinder;
+import android.preference.PreferenceManager;
+import androidx.annotation.NonNull;
+import androidx.annotation.Nullable;
+import androidx.annotation.StringRes;
+import android.view.Menu;
+import android.view.MenuItem;
+import android.view.MotionEvent;
+import android.view.View;
+import android.view.WindowManager;
+import android.webkit.WebView;
+import android.widget.ImageButton;
+import android.widget.LinearLayout;
+import android.widget.LinearLayout.LayoutParams;
+import android.widget.PopupMenu;
+import android.widget.TextView;
+
+import com.google.blocks.ftcrobotcontroller.ProgrammingWebHandlers;
+import com.google.blocks.ftcrobotcontroller.runtime.BlocksOpMode;
+import com.qualcomm.ftccommon.ClassManagerFactory;
+import com.qualcomm.ftccommon.FtcAboutActivity;
+import com.qualcomm.ftccommon.FtcEventLoop;
+import com.qualcomm.ftccommon.FtcEventLoopIdle;
+import com.qualcomm.ftccommon.FtcRobotControllerService;
+import com.qualcomm.ftccommon.FtcRobotControllerService.FtcRobotControllerBinder;
+import com.qualcomm.ftccommon.FtcRobotControllerSettingsActivity;
+import com.qualcomm.ftccommon.LaunchActivityConstantsList;
+import com.qualcomm.ftccommon.LaunchActivityConstantsList.RequestCode;
+import com.qualcomm.ftccommon.Restarter;
+import com.qualcomm.ftccommon.UpdateUI;
+import com.qualcomm.ftccommon.configuration.EditParameters;
+import com.qualcomm.ftccommon.configuration.FtcLoadFileActivity;
+import com.qualcomm.ftccommon.configuration.RobotConfigFile;
+import com.qualcomm.ftccommon.configuration.RobotConfigFileManager;
+import com.qualcomm.ftcrobotcontroller.BuildConfig;
+import com.qualcomm.ftcrobotcontroller.R;
+import com.qualcomm.hardware.HardwareFactory;
+import com.qualcomm.robotcore.eventloop.EventLoopManager;
+import com.qualcomm.robotcore.eventloop.opmode.FtcRobotControllerServiceState;
+import com.qualcomm.robotcore.eventloop.opmode.OpModeRegister;
+import com.qualcomm.robotcore.hardware.configuration.LynxConstants;
+import com.qualcomm.robotcore.hardware.configuration.Utility;
+import com.qualcomm.robotcore.robot.Robot;
+import com.qualcomm.robotcore.robot.RobotState;
+import com.qualcomm.robotcore.util.ClockWarningSource;
+import com.qualcomm.robotcore.util.Device;
+import com.qualcomm.robotcore.util.Dimmer;
+import com.qualcomm.robotcore.util.ImmersiveMode;
+import com.qualcomm.robotcore.util.RobotLog;
+import com.qualcomm.robotcore.util.WebServer;
+import com.qualcomm.robotcore.wifi.NetworkConnection;
+import com.qualcomm.robotcore.wifi.NetworkConnectionFactory;
+import com.qualcomm.robotcore.wifi.NetworkType;
+
+import org.firstinspires.ftc.ftccommon.external.SoundPlayingRobotMonitor;
+import org.firstinspires.ftc.ftccommon.internal.AnnotatedHooksClassFilter;
+import org.firstinspires.ftc.ftccommon.internal.FtcRobotControllerWatchdogService;
+import org.firstinspires.ftc.ftccommon.internal.ProgramAndManageActivity;
+import org.firstinspires.ftc.onbotjava.ExternalLibraries;
+import org.firstinspires.ftc.onbotjava.OnBotJavaHelperImpl;
+import org.firstinspires.ftc.onbotjava.OnBotJavaProgrammingMode;
+import org.firstinspires.ftc.robotcore.external.navigation.MotionDetection;
+import org.firstinspires.ftc.robotcore.internal.hardware.android.AndroidBoard;
+import org.firstinspires.ftc.robotcore.internal.network.DeviceNameManagerFactory;
+import org.firstinspires.ftc.robotcore.internal.network.PreferenceRemoterRC;
+import org.firstinspires.ftc.robotcore.internal.network.StartResult;
+import org.firstinspires.ftc.robotcore.internal.network.WifiDirectChannelChanger;
+import org.firstinspires.ftc.robotcore.internal.network.WifiMuteEvent;
+import org.firstinspires.ftc.robotcore.internal.network.WifiMuteStateMachine;
+import org.firstinspires.ftc.robotcore.internal.opmode.ClassManager;
+import org.firstinspires.ftc.robotcore.internal.opmode.OnBotJavaHelper;
+import org.firstinspires.ftc.robotcore.internal.system.AppAliveNotifier;
+import org.firstinspires.ftc.robotcore.internal.system.AppUtil;
+import org.firstinspires.ftc.robotcore.internal.system.Assert;
+import org.firstinspires.ftc.robotcore.internal.system.PreferencesHelper;
+import org.firstinspires.ftc.robotcore.internal.system.ServiceController;
+import org.firstinspires.ftc.robotcore.internal.ui.ThemedActivity;
+import org.firstinspires.ftc.robotcore.internal.ui.UILocation;
+import org.firstinspires.ftc.robotcore.internal.webserver.RobotControllerWebInfo;
+import org.firstinspires.ftc.robotserver.internal.programmingmode.ProgrammingModeManager;
+import org.firstinspires.inspection.RcInspectionActivity;
+import org.threeten.bp.YearMonth;
+import org.xmlpull.v1.XmlPullParserException;
+
+import java.io.FileNotFoundException;
+import java.util.List;
+import java.util.Queue;
+import java.util.concurrent.ConcurrentLinkedQueue;
+
+@SuppressWarnings("WeakerAccess")
+public class FtcRobotControllerActivity extends Activity
+ {
+ public static final String TAG = "RCActivity";
+ public String getTag() { return TAG; }
+
+ private static final int REQUEST_CONFIG_WIFI_CHANNEL = 1;
+ private static final int NUM_GAMEPADS = 2;
+
+ protected WifiManager.WifiLock wifiLock;
+ protected RobotConfigFileManager cfgFileMgr;
+
+ private OnBotJavaHelper onBotJavaHelper;
+
+ protected ProgrammingModeManager programmingModeManager;
+
+ protected UpdateUI.Callback callback;
+ protected Context context;
+ protected Utility utility;
+ protected StartResult prefRemoterStartResult = new StartResult();
+ protected StartResult deviceNameStartResult = new StartResult();
+ protected PreferencesHelper preferencesHelper;
+ protected final SharedPreferencesListener sharedPreferencesListener = new SharedPreferencesListener();
+
+ protected ImageButton buttonMenu;
+ protected TextView textDeviceName;
+ protected TextView textNetworkConnectionStatus;
+ protected TextView textRobotStatus;
+ protected TextView[] textGamepad = new TextView[NUM_GAMEPADS];
+ protected TextView textOpMode;
+ protected TextView textErrorMessage;
+ protected ImmersiveMode immersion;
+
+ protected UpdateUI updateUI;
+ protected Dimmer dimmer;
+ protected LinearLayout entireScreenLayout;
+
+ protected FtcRobotControllerService controllerService;
+ protected NetworkType networkType;
+
+ protected FtcEventLoop eventLoop;
+ protected Queue receivedUsbAttachmentNotifications;
+
+ protected WifiMuteStateMachine wifiMuteStateMachine;
+ protected MotionDetection motionDetection;
+
+ private static boolean permissionsValidated = false;
+
+ private WifiDirectChannelChanger wifiDirectChannelChanger;
+
+ protected class RobotRestarter implements Restarter {
+
+ public void requestRestart() {
+ requestRobotRestart();
+ }
+
+ }
+
+ protected boolean serviceShouldUnbind = false;
+ protected ServiceConnection connection = new ServiceConnection() {
+ @Override
+ public void onServiceConnected(ComponentName name, IBinder service) {
+ FtcRobotControllerBinder binder = (FtcRobotControllerBinder) service;
+ onServiceBind(binder.getService());
+ }
+
+ @Override
+ public void onServiceDisconnected(ComponentName name) {
+ RobotLog.vv(FtcRobotControllerService.TAG, "%s.controllerService=null", TAG);
+ controllerService = null;
+ }
+ };
+
+ @Override
+ protected void onNewIntent(Intent intent) {
+ super.onNewIntent(intent);
+
+ if (UsbManager.ACTION_USB_DEVICE_ATTACHED.equals(intent.getAction())) {
+ UsbDevice usbDevice = intent.getParcelableExtra(UsbManager.EXTRA_DEVICE);
+ RobotLog.vv(TAG, "ACTION_USB_DEVICE_ATTACHED: %s", usbDevice.getDeviceName());
+
+ if (usbDevice != null) { // paranoia
+ // We might get attachment notifications before the event loop is set up, so
+ // we hold on to them and pass them along only when we're good and ready.
+ if (receivedUsbAttachmentNotifications != null) { // *total* paranoia
+ receivedUsbAttachmentNotifications.add(usbDevice);
+ passReceivedUsbAttachmentsToEventLoop();
+ }
+ }
+ }
+ }
+
+ protected void passReceivedUsbAttachmentsToEventLoop() {
+ if (this.eventLoop != null) {
+ for (;;) {
+ UsbDevice usbDevice = receivedUsbAttachmentNotifications.poll();
+ if (usbDevice == null)
+ break;
+ this.eventLoop.onUsbDeviceAttached(usbDevice);
+ }
+ }
+ else {
+ // Paranoia: we don't want the pending list to grow without bound when we don't
+ // (yet) have an event loop
+ while (receivedUsbAttachmentNotifications.size() > 100) {
+ receivedUsbAttachmentNotifications.poll();
+ }
+ }
+ }
+
+ /**
+ * There are cases where a permission may be revoked and the system restart will restart the
+ * FtcRobotControllerActivity, instead of the launch activity. Detect when that happens, and throw
+ * the device back to the permission validator activity.
+ */
+ protected boolean enforcePermissionValidator() {
+ if (!permissionsValidated) {
+ RobotLog.vv(TAG, "Redirecting to permission validator");
+ Intent permissionValidatorIntent = new Intent(AppUtil.getDefContext(), PermissionValidatorWrapper.class);
+ startActivity(permissionValidatorIntent);
+ finish();
+ return true;
+ } else {
+ RobotLog.vv(TAG, "Permissions validated already");
+ return false;
+ }
+ }
+
+ public static void setPermissionsValidated() {
+ permissionsValidated = true;
+ }
+
+ @Override
+ protected void onCreate(Bundle savedInstanceState) {
+ super.onCreate(savedInstanceState);
+
+ if (enforcePermissionValidator()) {
+ return;
+ }
+
+ RobotLog.onApplicationStart(); // robustify against onCreate() following onDestroy() but using the same app instance, which apparently does happen
+ RobotLog.vv(TAG, "onCreate()");
+ ThemedActivity.appAppThemeToActivity(getTag(), this); // do this way instead of inherit to help AppInventor
+
+ // Oddly, sometimes after a crash & restart the root activity will be something unexpected, like from the before crash? We don't yet understand
+ RobotLog.vv(TAG, "rootActivity is of class %s", AppUtil.getInstance().getRootActivity().getClass().getSimpleName());
+ RobotLog.vv(TAG, "launchActivity is of class %s", FtcRobotControllerWatchdogService.launchActivity());
+ Assert.assertTrue(FtcRobotControllerWatchdogService.isLaunchActivity(AppUtil.getInstance().getRootActivity()));
+ Assert.assertTrue(AppUtil.getInstance().isRobotController());
+
+ // Quick check: should we pretend we're not here, and so allow the Lynx to operate as
+ // a stand-alone USB-connected module?
+ if (LynxConstants.isRevControlHub()) {
+ // Double-sure check that we can talk to the DB over the serial TTY
+ AndroidBoard.getInstance().getAndroidBoardIsPresentPin().setState(true);
+ }
+
+ context = this;
+ utility = new Utility(this);
+
+ DeviceNameManagerFactory.getInstance().start(deviceNameStartResult);
+
+ PreferenceRemoterRC.getInstance().start(prefRemoterStartResult);
+
+ receivedUsbAttachmentNotifications = new ConcurrentLinkedQueue();
+ eventLoop = null;
+
+ setContentView(R.layout.activity_ftc_controller);
+
+ preferencesHelper = new PreferencesHelper(TAG, context);
+ preferencesHelper.writeBooleanPrefIfDifferent(context.getString(R.string.pref_rc_connected), true);
+ preferencesHelper.getSharedPreferences().registerOnSharedPreferenceChangeListener(sharedPreferencesListener);
+
+ // Check if this RC app is from a later FTC season than what was installed previously
+ int ftcSeasonYearOfPreviouslyInstalledRc = preferencesHelper.readInt(getString(R.string.pref_ftc_season_year_of_current_rc), 0);
+ int ftcSeasonYearOfCurrentlyInstalledRc = AppUtil.getInstance().getFtcSeasonYear(AppUtil.getInstance().getLocalSdkBuildMonth()).getValue();
+ if (ftcSeasonYearOfCurrentlyInstalledRc > ftcSeasonYearOfPreviouslyInstalledRc) {
+ preferencesHelper.writeIntPrefIfDifferent(getString(R.string.pref_ftc_season_year_of_current_rc), ftcSeasonYearOfCurrentlyInstalledRc);
+ // Since it's a new FTC season, we should reset certain settings back to their default values.
+ preferencesHelper.writeBooleanPrefIfDifferent(getString(R.string.pref_warn_about_2_4_ghz_band), true);
+ preferencesHelper.writeBooleanPrefIfDifferent(getString(R.string.pref_warn_about_obsolete_software), true);
+ preferencesHelper.writeBooleanPrefIfDifferent(getString(R.string.pref_warn_about_mismatched_app_versions), true);
+ preferencesHelper.writeBooleanPrefIfDifferent(getString(R.string.pref_warn_about_incorrect_clocks), true);
+ }
+
+ entireScreenLayout = (LinearLayout) findViewById(R.id.entire_screen);
+ buttonMenu = (ImageButton) findViewById(R.id.menu_buttons);
+ buttonMenu.setOnClickListener(new View.OnClickListener() {
+ @Override
+ public void onClick(View v) {
+ PopupMenu popupMenu = new PopupMenu(FtcRobotControllerActivity.this, v);
+ popupMenu.setOnMenuItemClickListener(new PopupMenu.OnMenuItemClickListener() {
+ @Override
+ public boolean onMenuItemClick(MenuItem item) {
+ return onOptionsItemSelected(item); // Delegate to the handler for the hardware menu button
+ }
+ });
+ popupMenu.inflate(R.menu.ftc_robot_controller);
+ AnnotatedHooksClassFilter.getInstance().callOnCreateMenuMethods(
+ FtcRobotControllerActivity.this, popupMenu.getMenu());
+ popupMenu.show();
+ }
+ });
+
+ updateMonitorLayout(getResources().getConfiguration());
+
+ BlocksOpMode.setActivityAndWebView(this, (WebView) findViewById(R.id.webViewBlocksRuntime));
+
+ ExternalLibraries.getInstance().onCreate();
+ onBotJavaHelper = new OnBotJavaHelperImpl();
+
+ /*
+ * Paranoia as the ClassManagerFactory requires EXTERNAL_STORAGE permissions
+ * and we've seen on the DS where the finish() call above does not short-circuit
+ * the onCreate() call for the activity and then we crash here because we don't
+ * have permissions. So...
+ */
+ if (permissionsValidated) {
+ ClassManager.getInstance().setOnBotJavaClassHelper(onBotJavaHelper);
+ ClassManagerFactory.registerFilters();
+ ClassManagerFactory.processAllClasses();
+ }
+
+ cfgFileMgr = new RobotConfigFileManager(this);
+
+ // Clean up 'dirty' status after a possible crash
+ RobotConfigFile configFile = cfgFileMgr.getActiveConfig();
+ if (configFile.isDirty()) {
+ configFile.markClean();
+ cfgFileMgr.setActiveConfig(false, configFile);
+ }
+
+ textDeviceName = (TextView) findViewById(R.id.textDeviceName);
+ textNetworkConnectionStatus = (TextView) findViewById(R.id.textNetworkConnectionStatus);
+ textRobotStatus = (TextView) findViewById(R.id.textRobotStatus);
+ textOpMode = (TextView) findViewById(R.id.textOpMode);
+ textErrorMessage = (TextView) findViewById(R.id.textErrorMessage);
+ textGamepad[0] = (TextView) findViewById(R.id.textGamepad1);
+ textGamepad[1] = (TextView) findViewById(R.id.textGamepad2);
+ immersion = new ImmersiveMode(getWindow().getDecorView());
+ dimmer = new Dimmer(this);
+ dimmer.longBright();
+
+ programmingModeManager = new ProgrammingModeManager();
+ programmingModeManager.register(new ProgrammingWebHandlers());
+ programmingModeManager.register(new OnBotJavaProgrammingMode());
+
+ updateUI = createUpdateUI();
+ callback = createUICallback(updateUI);
+
+ PreferenceManager.setDefaultValues(this, R.xml.app_settings, false);
+
+ WifiManager wifiManager = (WifiManager) getApplicationContext().getSystemService(Context.WIFI_SERVICE);
+ wifiLock = wifiManager.createWifiLock(WifiManager.WIFI_MODE_FULL_HIGH_PERF, "");
+
+ hittingMenuButtonBrightensScreen();
+
+ wifiLock.acquire();
+ callback.networkConnectionUpdate(NetworkConnection.NetworkEvent.DISCONNECTED);
+ readNetworkType();
+ ServiceController.startService(FtcRobotControllerWatchdogService.class);
+ bindToService();
+ RobotLog.logAppInfo();
+ RobotLog.logDeviceInfo();
+ AndroidBoard.getInstance().logAndroidBoardInfo();
+
+ if (preferencesHelper.readBoolean(getString(R.string.pref_wifi_automute), false)) {
+ initWifiMute(true);
+ }
+
+ FtcAboutActivity.setBuildTimeFromBuildConfig(BuildConfig.APP_BUILD_TIME);
+
+ // check to see if there is a preferred Wi-Fi to use.
+ checkPreferredChannel();
+
+ AnnotatedHooksClassFilter.getInstance().callOnCreateMethods(this);
+ }
+
+ protected UpdateUI createUpdateUI() {
+ Restarter restarter = new RobotRestarter();
+ UpdateUI result = new UpdateUI(this, dimmer);
+ result.setRestarter(restarter);
+ result.setTextViews(textNetworkConnectionStatus, textRobotStatus, textGamepad, textOpMode, textErrorMessage, textDeviceName);
+ return result;
+ }
+
+ protected UpdateUI.Callback createUICallback(UpdateUI updateUI) {
+ UpdateUI.Callback result = updateUI.new Callback();
+ result.setStateMonitor(new SoundPlayingRobotMonitor());
+ return result;
+ }
+
+ @Override
+ protected void onStart() {
+ super.onStart();
+ RobotLog.vv(TAG, "onStart()");
+
+ entireScreenLayout.setOnTouchListener(new View.OnTouchListener() {
+ @Override
+ public boolean onTouch(View v, MotionEvent event) {
+ dimmer.handleDimTimer();
+ return false;
+ }
+ });
+ }
+
+ @Override
+ protected void onResume() {
+ super.onResume();
+ RobotLog.vv(TAG, "onResume()");
+
+ // In case the user just got back from fixing their clock, refresh ClockWarningSource
+ ClockWarningSource.getInstance().onPossibleRcClockUpdate();
+ }
+
+ @Override
+ protected void onPause() {
+ super.onPause();
+ RobotLog.vv(TAG, "onPause()");
+ }
+
+ @Override
+ protected void onStop() {
+ // Note: this gets called even when the configuration editor is launched. That is, it gets
+ // called surprisingly often. So, we don't actually do much here.
+ super.onStop();
+ RobotLog.vv(TAG, "onStop()");
+ }
+
+ @Override
+ protected void onDestroy() {
+ super.onDestroy();
+ RobotLog.vv(TAG, "onDestroy()");
+
+ shutdownRobot(); // Ensure the robot is put away to bed
+ if (callback != null) callback.close();
+
+ PreferenceRemoterRC.getInstance().stop(prefRemoterStartResult);
+ DeviceNameManagerFactory.getInstance().stop(deviceNameStartResult);
+
+ unbindFromService();
+ // If the app manually (?) is stopped, then we don't need the auto-starting function (?)
+ ServiceController.stopService(FtcRobotControllerWatchdogService.class);
+ if (wifiLock != null) wifiLock.release();
+ if (preferencesHelper != null) preferencesHelper.getSharedPreferences().unregisterOnSharedPreferenceChangeListener(sharedPreferencesListener);
+
+ RobotLog.cancelWriteLogcatToDisk();
+
+ AnnotatedHooksClassFilter.getInstance().callOnDestroyMethods(this);
+ }
+
+ protected void bindToService() {
+ readNetworkType();
+ Intent intent = new Intent(this, FtcRobotControllerService.class);
+ intent.putExtra(NetworkConnectionFactory.NETWORK_CONNECTION_TYPE, networkType);
+ serviceShouldUnbind = bindService(intent, connection, Context.BIND_AUTO_CREATE);
+ }
+
+ protected void unbindFromService() {
+ if (serviceShouldUnbind) {
+ unbindService(connection);
+ serviceShouldUnbind = false;
+ }
+ }
+
+ protected void readNetworkType() {
+ // Control hubs are always running the access point model. Everything else, for the time
+ // being always runs the Wi-Fi Direct model.
+ if (Device.isRevControlHub() == true) {
+ networkType = NetworkType.RCWIRELESSAP;
+ } else {
+ networkType = NetworkType.fromString(preferencesHelper.readString(context.getString(R.string.pref_pairing_kind), NetworkType.globalDefaultAsString()));
+ }
+
+ // update the app_settings
+ preferencesHelper.writeStringPrefIfDifferent(context.getString(R.string.pref_pairing_kind), networkType.toString());
+ }
+
+ @Override
+ public void onWindowFocusChanged(boolean hasFocus) {
+ super.onWindowFocusChanged(hasFocus);
+
+ if (hasFocus) {
+ immersion.hideSystemUI();
+ getWindow().setFlags(WindowManager.LayoutParams.FLAG_TRANSLUCENT_NAVIGATION, WindowManager.LayoutParams.FLAG_TRANSLUCENT_NAVIGATION);
+ }
+ }
+
+ @Override
+ public boolean onCreateOptionsMenu(Menu menu) {
+ getMenuInflater().inflate(R.menu.ftc_robot_controller, menu);
+ AnnotatedHooksClassFilter.getInstance().callOnCreateMenuMethods(this, menu);
+ return true;
+ }
+
+ private boolean isRobotRunning() {
+ if (controllerService == null) {
+ return false;
+ }
+
+ Robot robot = controllerService.getRobot();
+
+ if ((robot == null) || (robot.eventLoopManager == null)) {
+ return false;
+ }
+
+ RobotState robotState = robot.eventLoopManager.state;
+
+ if (robotState != RobotState.RUNNING) {
+ return false;
+ } else {
+ return true;
+ }
+ }
+
+ @Override
+ public boolean onOptionsItemSelected(MenuItem item) {
+ int id = item.getItemId();
+
+ if (id == R.id.action_program_and_manage) {
+ if (isRobotRunning()) {
+ Intent programmingModeIntent = new Intent(AppUtil.getDefContext(), ProgramAndManageActivity.class);
+ RobotControllerWebInfo webInfo = programmingModeManager.getWebServer().getConnectionInformation();
+ programmingModeIntent.putExtra(LaunchActivityConstantsList.RC_WEB_INFO, webInfo.toJson());
+ startActivity(programmingModeIntent);
+ } else {
+ AppUtil.getInstance().showToast(UILocation.ONLY_LOCAL, context.getString(R.string.toastWifiUpBeforeProgrammingMode));
+ }
+ } else if (id == R.id.action_inspection_mode) {
+ Intent inspectionModeIntent = new Intent(AppUtil.getDefContext(), RcInspectionActivity.class);
+ startActivity(inspectionModeIntent);
+ return true;
+ } else if (id == R.id.action_restart_robot) {
+ dimmer.handleDimTimer();
+ AppUtil.getInstance().showToast(UILocation.BOTH, context.getString(R.string.toastRestartingRobot));
+ requestRobotRestart();
+ return true;
+ }
+ else if (id == R.id.action_configure_robot) {
+ EditParameters parameters = new EditParameters();
+ Intent intentConfigure = new Intent(AppUtil.getDefContext(), FtcLoadFileActivity.class);
+ parameters.putIntent(intentConfigure);
+ startActivityForResult(intentConfigure, RequestCode.CONFIGURE_ROBOT_CONTROLLER.ordinal());
+ }
+ else if (id == R.id.action_settings) {
+ // historical: this once erroneously used FTC_CONFIGURE_REQUEST_CODE_ROBOT_CONTROLLER
+ Intent settingsIntent = new Intent(AppUtil.getDefContext(), FtcRobotControllerSettingsActivity.class);
+ startActivityForResult(settingsIntent, RequestCode.SETTINGS_ROBOT_CONTROLLER.ordinal());
+ return true;
+ }
+ else if (id == R.id.action_about) {
+ Intent intent = new Intent(AppUtil.getDefContext(), FtcAboutActivity.class);
+ startActivity(intent);
+ return true;
+ }
+ else if (id == R.id.action_exit_app) {
+
+ //Clear backstack and everything to prevent edge case where VM might be
+ //restarted (after it was exited) if more than one activity was on the
+ //backstack for some reason.
+ finishAffinity();
+
+ //For lollipop and up, we can clear ourselves from the recents list too
+ if (android.os.Build.VERSION.SDK_INT >= android.os.Build.VERSION_CODES.LOLLIPOP) {
+ ActivityManager manager = (ActivityManager) getSystemService(ACTIVITY_SERVICE);
+ List tasks = manager.getAppTasks();
+
+ for (ActivityManager.AppTask task : tasks) {
+ task.finishAndRemoveTask();
+ }
+ }
+
+ // Allow the user to use the Control Hub operating system's UI, instead of relaunching the app
+ AppAliveNotifier.getInstance().disableAppWatchdogUntilNextAppStart();
+
+ //Finally, nuke the VM from orbit
+ AppUtil.getInstance().exitApplication();
+
+ return true;
+ }
+
+ return super.onOptionsItemSelected(item);
+ }
+
+ @Override
+ public void onConfigurationChanged(Configuration newConfig) {
+ super.onConfigurationChanged(newConfig);
+ // don't destroy assets on screen rotation
+ updateMonitorLayout(newConfig);
+ }
+
+ /**
+ * Updates the orientation of monitorContainer (which contains cameraMonitorView)
+ * based on the given configuration. Makes the children split the space.
+ */
+ private void updateMonitorLayout(Configuration configuration) {
+ LinearLayout monitorContainer = (LinearLayout) findViewById(R.id.monitorContainer);
+ if (configuration.orientation == Configuration.ORIENTATION_LANDSCAPE) {
+ // When the phone is landscape, lay out the monitor views horizontally.
+ monitorContainer.setOrientation(LinearLayout.HORIZONTAL);
+ for (int i = 0; i < monitorContainer.getChildCount(); i++) {
+ View view = monitorContainer.getChildAt(i);
+ view.setLayoutParams(new LayoutParams(0, LayoutParams.MATCH_PARENT, 1 /* weight */));
+ }
+ } else {
+ // When the phone is portrait, lay out the monitor views vertically.
+ monitorContainer.setOrientation(LinearLayout.VERTICAL);
+ for (int i = 0; i < monitorContainer.getChildCount(); i++) {
+ View view = monitorContainer.getChildAt(i);
+ view.setLayoutParams(new LayoutParams(LayoutParams.MATCH_PARENT, 0, 1 /* weight */));
+ }
+ }
+ monitorContainer.requestLayout();
+ }
+
+ @Override
+ protected void onActivityResult(int request, int result, Intent intent) {
+ if (request == REQUEST_CONFIG_WIFI_CHANNEL) {
+ if (result == RESULT_OK) {
+ AppUtil.getInstance().showToast(UILocation.BOTH, context.getString(R.string.toastWifiConfigurationComplete));
+ }
+ }
+ // was some historical confusion about launch codes here, so we err safely
+ if (request == RequestCode.CONFIGURE_ROBOT_CONTROLLER.ordinal() || request == RequestCode.SETTINGS_ROBOT_CONTROLLER.ordinal()) {
+ // We always do a refresh, whether it was a cancel or an OK, for robustness
+ shutdownRobot();
+ cfgFileMgr.getActiveConfigAndUpdateUI();
+ updateUIAndRequestRobotSetup();
+ }
+ }
+
+ public void onServiceBind(final FtcRobotControllerService service) {
+ RobotLog.vv(FtcRobotControllerService.TAG, "%s.controllerService=bound", TAG);
+ controllerService = service;
+ updateUI.setControllerService(controllerService);
+
+ controllerService.setOnBotJavaHelper(onBotJavaHelper);
+
+ updateUIAndRequestRobotSetup();
+ programmingModeManager.setState(new FtcRobotControllerServiceState() {
+ @NonNull
+ @Override
+ public WebServer getWebServer() {
+ return service.getWebServer();
+ }
+
+ @Nullable
+ @Override
+ public OnBotJavaHelper getOnBotJavaHelper() {
+ return service.getOnBotJavaHelper();
+ }
+
+ @Override
+ public EventLoopManager getEventLoopManager() {
+ return service.getRobot().eventLoopManager;
+ }
+ });
+
+ AnnotatedHooksClassFilter.getInstance().callWebHandlerRegistrarMethods(this,
+ service.getWebServer().getWebHandlerManager());
+ }
+
+ private void updateUIAndRequestRobotSetup() {
+ if (controllerService != null) {
+ callback.networkConnectionUpdate(controllerService.getNetworkConnectionStatus());
+ callback.updateRobotStatus(controllerService.getRobotStatus());
+ // Only show this first-time toast on headless systems: what we have now on non-headless suffices
+ requestRobotSetup(LynxConstants.isRevControlHub()
+ ? new Runnable() {
+ @Override public void run() {
+ showRestartRobotCompleteToast(R.string.toastRobotSetupComplete);
+ }
+ }
+ : null);
+ }
+ }
+
+ private void requestRobotSetup(@Nullable Runnable runOnComplete) {
+ if (controllerService == null) return;
+
+ RobotConfigFile file = cfgFileMgr.getActiveConfigAndUpdateUI();
+ HardwareFactory hardwareFactory = new HardwareFactory(context);
+ try {
+ hardwareFactory.setXmlPullParser(file.getXml());
+ } catch (FileNotFoundException | XmlPullParserException e) {
+ RobotLog.ww(TAG, e, "Unable to set configuration file %s. Falling back on noConfig.", file.getName());
+ file = RobotConfigFile.noConfig(cfgFileMgr);
+ try {
+ hardwareFactory.setXmlPullParser(file.getXml());
+ cfgFileMgr.setActiveConfigAndUpdateUI(false, file);
+ } catch (FileNotFoundException | XmlPullParserException e1) {
+ RobotLog.ee(TAG, e1, "Failed to fall back on noConfig");
+ }
+ }
+
+ OpModeRegister userOpModeRegister = createOpModeRegister();
+ eventLoop = new FtcEventLoop(hardwareFactory, userOpModeRegister, callback, this);
+ FtcEventLoopIdle idleLoop = new FtcEventLoopIdle(hardwareFactory, userOpModeRegister, callback, this);
+
+ controllerService.setCallback(callback);
+ controllerService.setupRobot(eventLoop, idleLoop, runOnComplete);
+
+ passReceivedUsbAttachmentsToEventLoop();
+ AndroidBoard.showErrorIfUnknownControlHub();
+
+ AnnotatedHooksClassFilter.getInstance().callOnCreateEventLoopMethods(this, eventLoop);
+ }
+
+ protected OpModeRegister createOpModeRegister() {
+ return new FtcOpModeRegister();
+ }
+
+ private void shutdownRobot() {
+ if (controllerService != null) controllerService.shutdownRobot();
+ }
+
+ private void requestRobotRestart() {
+ AppUtil.getInstance().showToast(UILocation.BOTH, AppUtil.getDefContext().getString(R.string.toastRestartingRobot));
+ //
+ RobotLog.clearGlobalErrorMsg();
+ RobotLog.clearGlobalWarningMsg();
+ shutdownRobot();
+ requestRobotSetup(new Runnable() {
+ @Override public void run() {
+ showRestartRobotCompleteToast(R.string.toastRestartRobotComplete);
+ }
+ });
+ }
+
+ private void showRestartRobotCompleteToast(@StringRes int resid) {
+ AppUtil.getInstance().showToast(UILocation.BOTH, AppUtil.getDefContext().getString(resid));
+ }
+
+ private void checkPreferredChannel() {
+ // For P2P network, check to see what preferred channel is.
+ if (networkType == NetworkType.WIFIDIRECT) {
+ int prefChannel = preferencesHelper.readInt(getString(com.qualcomm.ftccommon.R.string.pref_wifip2p_channel), -1);
+ if (prefChannel == -1) {
+ prefChannel = 0;
+ RobotLog.vv(TAG, "pref_wifip2p_channel: No preferred channel defined. Will use a default value of %d", prefChannel);
+ } else {
+ RobotLog.vv(TAG, "pref_wifip2p_channel: Found existing preferred channel (%d).", prefChannel);
+ }
+
+ // attempt to set the preferred channel.
+ RobotLog.vv(TAG, "pref_wifip2p_channel: attempting to set preferred channel...");
+ wifiDirectChannelChanger = new WifiDirectChannelChanger();
+ wifiDirectChannelChanger.changeToChannel(prefChannel);
+ }
+ }
+
+ protected void hittingMenuButtonBrightensScreen() {
+ ActionBar actionBar = getActionBar();
+ if (actionBar != null) {
+ actionBar.addOnMenuVisibilityListener(new ActionBar.OnMenuVisibilityListener() {
+ @Override
+ public void onMenuVisibilityChanged(boolean isVisible) {
+ if (isVisible) {
+ dimmer.handleDimTimer();
+ }
+ }
+ });
+ }
+ }
+
+ protected class SharedPreferencesListener implements SharedPreferences.OnSharedPreferenceChangeListener {
+ @Override public void onSharedPreferenceChanged(SharedPreferences sharedPreferences, String key) {
+ if (key.equals(context.getString(R.string.pref_app_theme))) {
+ ThemedActivity.restartForAppThemeChange(getTag(), getString(R.string.appThemeChangeRestartNotifyRC));
+ } else if (key.equals(context.getString(R.string.pref_wifi_automute))) {
+ if (preferencesHelper.readBoolean(context.getString(R.string.pref_wifi_automute), false)) {
+ initWifiMute(true);
+ } else {
+ initWifiMute(false);
+ }
+ }
+ }
+ }
+
+ protected void initWifiMute(boolean enable) {
+ if (enable) {
+ wifiMuteStateMachine = new WifiMuteStateMachine();
+ wifiMuteStateMachine.initialize();
+ wifiMuteStateMachine.start();
+
+ motionDetection = new MotionDetection(2.0, 10);
+ motionDetection.startListening();
+ motionDetection.registerListener(new MotionDetection.MotionDetectionListener() {
+ @Override
+ public void onMotionDetected(double vector)
+ {
+ wifiMuteStateMachine.consumeEvent(WifiMuteEvent.USER_ACTIVITY);
+ }
+ });
+ } else {
+ wifiMuteStateMachine.stop();
+ wifiMuteStateMachine = null;
+ motionDetection.stopListening();
+ motionDetection.purgeListeners();
+ motionDetection = null;
+ }
+ }
+
+ @Override
+ public void onUserInteraction() {
+ if (wifiMuteStateMachine != null) {
+ wifiMuteStateMachine.consumeEvent(WifiMuteEvent.USER_ACTIVITY);
+ }
+ }
+}
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/internal/PermissionValidatorWrapper.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/internal/PermissionValidatorWrapper.java
new file mode 100644
index 0000000..a0094bc
--- /dev/null
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/internal/PermissionValidatorWrapper.java
@@ -0,0 +1,91 @@
+/*
+ * Copyright (c) 2018 Craig MacFarlane
+ *
+ * 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 Craig MacFarlane 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.robotcontroller.internal;
+
+import android.Manifest;
+import android.os.Bundle;
+
+import com.qualcomm.ftcrobotcontroller.R;
+
+import org.firstinspires.ftc.robotcore.internal.system.Misc;
+import org.firstinspires.ftc.robotcore.internal.system.PermissionValidatorActivity;
+
+import java.util.ArrayList;
+import java.util.List;
+
+public class PermissionValidatorWrapper extends PermissionValidatorActivity {
+
+ private final String TAG = "PermissionValidatorWrapper";
+
+ /*
+ * The list of dangerous permissions the robot controller needs.
+ */
+ protected List robotControllerPermissions = new ArrayList() {{
+ add(Manifest.permission.WRITE_EXTERNAL_STORAGE);
+ add(Manifest.permission.READ_EXTERNAL_STORAGE);
+ add(Manifest.permission.CAMERA);
+ add(Manifest.permission.ACCESS_COARSE_LOCATION);
+ add(Manifest.permission.ACCESS_FINE_LOCATION);
+ add(Manifest.permission.READ_PHONE_STATE);
+ }};
+
+ private final static Class startApplication = FtcRobotControllerActivity.class;
+
+ public String mapPermissionToExplanation(final String permission) {
+ if (permission.equals(Manifest.permission.WRITE_EXTERNAL_STORAGE)) {
+ return Misc.formatForUser(R.string.permRcWriteExternalStorageExplain);
+ } else if (permission.equals(Manifest.permission.READ_EXTERNAL_STORAGE)) {
+ return Misc.formatForUser(R.string.permRcReadExternalStorageExplain);
+ } else if (permission.equals(Manifest.permission.CAMERA)) {
+ return Misc.formatForUser(R.string.permRcCameraExplain);
+ } else if (permission.equals(Manifest.permission.ACCESS_COARSE_LOCATION)) {
+ return Misc.formatForUser(R.string.permAccessLocationExplain);
+ } else if (permission.equals(Manifest.permission.ACCESS_FINE_LOCATION)) {
+ return Misc.formatForUser(R.string.permAccessLocationExplain);
+ } else if (permission.equals(Manifest.permission.READ_PHONE_STATE)) {
+ return Misc.formatForUser(R.string.permReadPhoneState);
+ }
+ return Misc.formatForUser(R.string.permGenericExplain);
+ }
+
+ @Override
+ protected void onCreate(Bundle savedInstanceState)
+ {
+ super.onCreate(savedInstanceState);
+
+ permissions = robotControllerPermissions;
+ }
+
+ protected Class onStartApplication()
+ {
+ FtcRobotControllerActivity.setPermissionsValidated();
+ return startApplication;
+ }
+}
diff --git a/FtcRobotController/src/main/res/drawable-xhdpi/icon_menu.png b/FtcRobotController/src/main/res/drawable-xhdpi/icon_menu.png
new file mode 100644
index 0000000..6b9e997
Binary files /dev/null and b/FtcRobotController/src/main/res/drawable-xhdpi/icon_menu.png differ
diff --git a/FtcRobotController/src/main/res/drawable-xhdpi/icon_robotcontroller.png b/FtcRobotController/src/main/res/drawable-xhdpi/icon_robotcontroller.png
new file mode 100644
index 0000000..022552f
Binary files /dev/null and b/FtcRobotController/src/main/res/drawable-xhdpi/icon_robotcontroller.png differ
diff --git a/FtcRobotController/src/main/res/layout/activity_ftc_controller.xml b/FtcRobotController/src/main/res/layout/activity_ftc_controller.xml
new file mode 100644
index 0000000..6524f94
--- /dev/null
+++ b/FtcRobotController/src/main/res/layout/activity_ftc_controller.xml
@@ -0,0 +1,184 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/FtcRobotController/src/main/res/menu/ftc_robot_controller.xml b/FtcRobotController/src/main/res/menu/ftc_robot_controller.xml
new file mode 100644
index 0000000..657c1aa
--- /dev/null
+++ b/FtcRobotController/src/main/res/menu/ftc_robot_controller.xml
@@ -0,0 +1,78 @@
+
+
+
diff --git a/FtcRobotController/src/main/res/raw/gold.wav b/FtcRobotController/src/main/res/raw/gold.wav
new file mode 100644
index 0000000..3a7baf8
Binary files /dev/null and b/FtcRobotController/src/main/res/raw/gold.wav differ
diff --git a/FtcRobotController/src/main/res/raw/silver.wav b/FtcRobotController/src/main/res/raw/silver.wav
new file mode 100644
index 0000000..25918a7
Binary files /dev/null and b/FtcRobotController/src/main/res/raw/silver.wav differ
diff --git a/FtcRobotController/src/main/res/values/dimens.xml b/FtcRobotController/src/main/res/values/dimens.xml
new file mode 100644
index 0000000..63f1bab
--- /dev/null
+++ b/FtcRobotController/src/main/res/values/dimens.xml
@@ -0,0 +1,40 @@
+
+
+
+
+
+ 16dp
+ 5dp
+
+
\ No newline at end of file
diff --git a/FtcRobotController/src/main/res/values/strings.xml b/FtcRobotController/src/main/res/values/strings.xml
new file mode 100644
index 0000000..6ea191e
--- /dev/null
+++ b/FtcRobotController/src/main/res/values/strings.xml
@@ -0,0 +1,72 @@
+
+
+
+
+
+
+ FTC Robot Controller
+
+
+ Self Inspect
+ Program & Manage
+ Blocks
+ Settings
+ Restart Robot
+ Configure Robot
+ About
+ Exit
+
+
+ Configuration Complete
+ Restarting Robot
+ The Robot Controller must be fully up and running before entering Program and Manage Mode.
+
+
+
+ - @style/AppThemeRedRC
+ - @style/AppThemeGreenRC
+ - @style/AppThemeBlueRC
+ - @style/AppThemePurpleRC
+ - @style/AppThemeOrangeRC
+ - @style/AppThemeTealRC
+
+
+ pref_ftc_season_year_of_current_rc_new
+
+ @string/packageNameRobotController
+
+
diff --git a/FtcRobotController/src/main/res/values/styles.xml b/FtcRobotController/src/main/res/values/styles.xml
new file mode 100644
index 0000000..07689c0
--- /dev/null
+++ b/FtcRobotController/src/main/res/values/styles.xml
@@ -0,0 +1,23 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/FtcRobotController/src/main/res/xml/app_settings.xml b/FtcRobotController/src/main/res/xml/app_settings.xml
new file mode 100644
index 0000000..58d3aa9
--- /dev/null
+++ b/FtcRobotController/src/main/res/xml/app_settings.xml
@@ -0,0 +1,93 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/FtcRobotController/src/main/res/xml/device_filter.xml b/FtcRobotController/src/main/res/xml/device_filter.xml
new file mode 100644
index 0000000..7b75350
--- /dev/null
+++ b/FtcRobotController/src/main/res/xml/device_filter.xml
@@ -0,0 +1,49 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/LICENSE b/LICENSE
new file mode 100644
index 0000000..88b776b
--- /dev/null
+++ b/LICENSE
@@ -0,0 +1,29 @@
+Copyright (c) 2014-2022 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.
+
diff --git a/TeamCode/build.gradle b/TeamCode/build.gradle
new file mode 100644
index 0000000..93b4474
--- /dev/null
+++ b/TeamCode/build.gradle
@@ -0,0 +1,54 @@
+//
+// build.gradle in TeamCode
+//
+// Most of the definitions for building your module reside in a common, shared
+// file 'build.common.gradle'. Being factored in this way makes it easier to
+// integrate updates to the FTC into your code. If you really need to customize
+// the build definitions, you can place those customizations in this file, but
+// please think carefully as to whether such customizations are really necessary
+// before doing so.
+
+
+// Custom definitions may go here
+repositories {
+ // Dairy releases repository
+ maven {
+ url = "https://repo.dairy.foundation/releases"
+ }
+ // Dairy snapshots repository
+ maven {
+ url = "https://repo.dairy.foundation/snapshots"
+ }
+}
+
+buildscript {
+ repositories {
+ mavenCentral()
+ maven {
+ url "https://repo.dairy.foundation/releases"
+ }
+ }
+ dependencies {
+ classpath "dev.frozenmilk:Load:0.2.4"
+ }
+}
+
+
+// Include common definitions from above.
+apply from: '../build.common.gradle'
+apply from: '../build.dependencies.gradle'
+apply plugin: 'dev.frozenmilk.sinister.sloth.load'
+
+android {
+ namespace = 'org.firstinspires.ftc.teamcode'
+
+ packagingOptions {
+ jniLibs.useLegacyPackaging true
+ }
+}
+
+dependencies {
+ implementation project(':FtcRobotController')
+ implementation("dev.frozenmilk.sinister:Sloth:0.2.4")
+ implementation("com.bylazar.sloth:fullpanels:0.2.4+1.0.12")
+}
diff --git a/TeamCode/build.gradle.old b/TeamCode/build.gradle.old
new file mode 100644
index 0000000..878287a
--- /dev/null
+++ b/TeamCode/build.gradle.old
@@ -0,0 +1,28 @@
+//
+// build.gradle in TeamCode
+//
+// Most of the definitions for building your module reside in a common, shared
+// file 'build.common.gradle'. Being factored in this way makes it easier to
+// integrate updates to the FTC into your code. If you really need to customize
+// the build definitions, you can place those customizations in this file, but
+// please think carefully as to whether such customizations are really necessary
+// before doing so.
+
+
+// Custom definitions may go here
+
+// Include common definitions from above.
+apply from: '../build.common.gradle'
+apply from: '../build.dependencies.gradle'
+
+android {
+ namespace = 'org.firstinspires.ftc.teamcode'
+
+ packagingOptions {
+ jniLibs.useLegacyPackaging true
+ }
+}
+
+dependencies {
+ implementation project(':FtcRobotController')
+}
diff --git a/TeamCode/lib/OpModeAnnotationProcessor.jar b/TeamCode/lib/OpModeAnnotationProcessor.jar
new file mode 100644
index 0000000..4825cc3
Binary files /dev/null and b/TeamCode/lib/OpModeAnnotationProcessor.jar differ
diff --git a/TeamCode/src/main/AndroidManifest.xml b/TeamCode/src/main/AndroidManifest.xml
new file mode 100644
index 0000000..3705b31
--- /dev/null
+++ b/TeamCode/src/main/AndroidManifest.xml
@@ -0,0 +1,11 @@
+
+
+
+
+
+
+
+
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Hware/hwMap.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Hware/hwMap.java
new file mode 100755
index 0000000..228227d
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Hware/hwMap.java
@@ -0,0 +1,403 @@
+package org.firstinspires.ftc.teamcode.Hware;
+
+import android.graphics.Color;
+
+import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+import com.qualcomm.robotcore.hardware.IMU; // Added
+import com.qualcomm.robotcore.hardware.NormalizedColorSensor;
+import com.qualcomm.robotcore.hardware.NormalizedRGBA;
+import com.qualcomm.robotcore.hardware.PIDFCoefficients;
+import com.qualcomm.robotcore.hardware.Servo;
+import com.qualcomm.robotcore.hardware.CRServo;
+import com.qualcomm.robotcore.hardware.SwitchableLight;
+
+import org.firstinspires.ftc.teamcode.teleOp.Constants;
+import org.firstinspires.ftc.vision.apriltag.AprilTagDetection;
+
+import com.qualcomm.hardware.limelightvision.LLResult;
+import com.qualcomm.hardware.limelightvision.Limelight3A;
+import com.qualcomm.robotcore.util.ElapsedTime;
+
+public class hwMap {
+ public static class IntakeHwMap {
+ public DcMotor frontIntakeMotor;
+
+ public IntakeHwMap(HardwareMap hardwareMap) {
+ frontIntakeMotor = hardwareMap.dcMotor.get(Constants.IntakeConstants.FRONT_INTAKE_MOTOR);
+ frontIntakeMotor.setDirection(Constants.IntakeConstants.INTAKE_DIRECTION);
+ frontIntakeMotor.setZeroPowerBehavior(Constants.IntakeConstants.INTAKE_ZERO_POWER_BEHAVIOR);
+ frontIntakeMotor.setMode(Constants.IntakeConstants.INTAKE_RUNMODE);
+ }
+ }
+
+ public static class LedHwMap {
+ public org.firstinspires.ftc.teamcode.Prism.GoBildaPrismDriver led1;
+ public org.firstinspires.ftc.teamcode.Prism.GoBildaPrismDriver led2;
+
+ public LedHwMap(HardwareMap hardwareMap) {
+ /*led1 = hardwareMap.get(org.firstinspires.ftc.teamcode.Prism.GoBildaPrismDriver.class, "led1");
+ led2 = hardwareMap.get(org.firstinspires.ftc.teamcode.Prism.GoBildaPrismDriver.class, "led2");*/
+ }
+ }
+
+ public static class LiftHwMap {
+ public Servo ptoLeft;
+ public Servo ptoRight;
+ public DcMotorEx leftFront, leftRear, rightFront, rightRear;
+
+ public LiftHwMap(HardwareMap hardwareMap) {/*
+ ptoLeft = hardwareMap.servo.get(Constants.LiftConstants.PTO_LEFT);
+ ptoRight = hardwareMap.servo.get(Constants.LiftConstants.PTO_RIGHT);
+
+ leftFront = hardwareMap.get(DcMotorEx.class, "fl");
+ leftRear = hardwareMap.get(DcMotorEx.class, "bl");
+ rightFront = hardwareMap.get(DcMotorEx.class, "fr");
+ rightRear = hardwareMap.get(DcMotorEx.class, "br");
+
+ ptoRight.setDirection(Constants.LiftConstants.PTO_RIGHT_DIR);
+ ptoLeft.setDirection(Constants.LiftConstants.PTO_LEFT_DIR);
+
+ leftFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+ leftRear.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+ rightFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+ rightRear.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);*/
+ }
+ }
+
+ public static class DriveHwMap {
+ public DcMotorEx frontLeftMotor;
+ public DcMotorEx backLeftMotor;
+ public DcMotorEx frontRightMotor;
+ public DcMotorEx backRightMotor;
+ public IMU imu;
+
+ public DriveHwMap(HardwareMap hardwareMap) {
+ frontLeftMotor = hardwareMap.get(DcMotorEx.class, Constants.DriveConstants.FRONT_LEFT_MOTOR);
+ backLeftMotor = hardwareMap.get(DcMotorEx.class, Constants.DriveConstants.BACK_LEFT_MOTOR);
+ frontRightMotor = hardwareMap.get(DcMotorEx.class, Constants.DriveConstants.FRONT_RIGHT_MOTOR);
+ backRightMotor = hardwareMap.get(DcMotorEx.class, Constants.DriveConstants.BACK_RIGHT_MOTOR);
+
+ frontLeftMotor.setDirection(DcMotorSimple.Direction.REVERSE);
+ backLeftMotor.setDirection(DcMotorSimple.Direction.REVERSE);
+ frontRightMotor.setDirection(DcMotorSimple.Direction.FORWARD);
+ backRightMotor.setDirection(DcMotorSimple.Direction.FORWARD);
+
+ imu = hardwareMap.get(IMU.class, "imu");
+ IMU.Parameters parameters = new IMU.Parameters(
+ new RevHubOrientationOnRobot(
+ RevHubOrientationOnRobot.LogoFacingDirection.RIGHT,
+ RevHubOrientationOnRobot.UsbFacingDirection.UP
+ )
+ );
+ imu.initialize(parameters);
+ imu.resetYaw();
+
+
+ resetEncoders();
+ setDriveMotorModes(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+
+ setDriveMotorZero(DcMotor.ZeroPowerBehavior.BRAKE);
+ }
+
+ public void setDriveMotorZero(DcMotor.ZeroPowerBehavior zero) {
+ frontLeftMotor.setZeroPowerBehavior(zero);
+ backLeftMotor.setZeroPowerBehavior(zero);
+ frontRightMotor.setZeroPowerBehavior(zero);
+ backRightMotor.setZeroPowerBehavior(zero);
+ }
+ public void setMotorTargetPositions(double inches){
+ setDriveMotorModes(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+ setDriveMotorModes(DcMotor.RunMode.RUN_USING_ENCODER);
+ frontLeftMotor.setTargetPosition(-inchesToTicks(inches));
+ frontRightMotor.setTargetPosition(inchesToTicks(inches));
+ backLeftMotor.setTargetPosition(-inchesToTicks(inches));
+ backRightMotor.setTargetPosition(inchesToTicks(inches));
+
+ }
+ public void setDriveMotorModes(DcMotor.RunMode mode) {
+ frontLeftMotor.setMode(mode);
+ backLeftMotor.setMode(mode);
+ frontRightMotor.setMode(mode);
+ backRightMotor.setMode(mode);
+ }
+
+ public void setMotorPowers(double frontLeftPower, double frontRightPower,
+ double backLeftPower, double backRightPower) {
+ frontLeftMotor.setPower(frontLeftPower);
+ frontRightMotor.setPower(frontRightPower);
+ backLeftMotor.setPower(backLeftPower);
+ backRightMotor.setPower(backRightPower);
+ }
+
+
+ public void stopMotors() {
+ setMotorPowers(0, 0, 0, 0);
+ }
+
+ public void resetEncoders() {
+ setDriveMotorModes(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+ setDriveMotorModes(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+ }
+
+ public int[] getEncoderPositions() {
+ return new int[] {
+ frontLeftMotor.getCurrentPosition(),
+ frontRightMotor.getCurrentPosition(),
+ backLeftMotor.getCurrentPosition(),
+ backRightMotor.getCurrentPosition()
+ };
+ }
+
+ private int inchesToTicks(double inches) {
+ double wheelDiameter = Constants.DriveConstants.WHEEL_DIAMETER; // in inches
+ double ticksPerRev = Constants.DriveConstants.TICKS_PER_REVOLUTION;
+ double gearRatio = 2.0;
+ double circumference = Math.PI * wheelDiameter;
+
+ return (int) (inches * (ticksPerRev * gearRatio) / circumference);
+ }
+ }
+
+ public static class TransferHwMap {
+ public Servo Limiter;
+
+ public NormalizedColorSensor indexA;
+ public NormalizedColorSensor indexB;
+ public NormalizedColorSensor indexC;
+
+ public NormalizedColorSensor[] sensors;
+
+ public TransferHwMap(HardwareMap hardwareMap) {
+ Limiter = hardwareMap.servo.get("limiter");
+
+
+ indexA = hardwareMap.get(NormalizedColorSensor.class, Constants.TransferConstants.INDEX_SENSOR_A);
+ indexB = hardwareMap.get(NormalizedColorSensor.class, Constants.TransferConstants.INDEX_SENSOR_B);
+ indexC = hardwareMap.get(NormalizedColorSensor.class, Constants.TransferConstants.INDEX_SENSOR_C);
+
+ Limiter.setDirection(Constants.TransferConstants.LIMITER_SERVO_DIR);
+
+ this.sensors = new NormalizedColorSensor[]{ indexA, indexB, indexC };
+
+
+ float gainFactor = 30.0f;
+
+ for (NormalizedColorSensor sensor : sensors) {
+ if (sensor instanceof SwitchableLight) ((SwitchableLight) sensor).enableLight(true);
+ sensor.setGain(gainFactor);
+ }
+ }
+
+ public void resetLimiter() {
+ setLimiter(false);
+ }
+
+ public double getServoPos(int input) {
+ return Limiter.getPosition();
+ }
+
+ public void setLimiter(boolean limit) {
+ Limiter.setPosition(limit ? Constants.TransferConstants.LIMIT_POS : Constants.TransferConstants.UNLOCK_POS);
+ }
+
+ public int detectArtifactColor(int index) {
+ if(index < 1 || index > 3) return 0;
+ int i = index - 1;
+
+ NormalizedRGBA colors2 = sensors[i].getNormalizedColors();
+
+ float[] hsv = new float[3];
+ Color.colorToHSV(colors2.toColor(), hsv);
+
+ int color = detectColor(hsv[0], hsv[1], hsv[2]);
+
+ double confidence = hsv[1] * hsv[2];
+
+ // Decision Matrix
+ if (color == 0 && confidence == 0) return 0;
+ else {
+ return color;
+ }
+
+ }
+
+ private int detectColor(float hue, float sat, float val) {
+ if (sat < 0.15 || val < 0.1) return 0;
+
+ if (hue > 170 && hue < 310) return 1;
+ if (hue > 80 && hue < 170) return 2;
+
+ return 0;
+ }
+ }
+
+ public static class TurretHwMap {
+
+ public Limelight3A limelight;
+
+ public DcMotorEx turretLeftMotor;
+ public DcMotorEx turretRightMotor;
+
+ public DcMotorEx turretEncoder; // External encoder for flywheels if needed, or specific port
+
+ public DcMotor turretrotation;
+ public Servo hoodservo;
+
+ private double lastKnownDistance = 0.0;
+ private ElapsedTime noTargetTimer = new ElapsedTime();
+
+ PIDFCoefficients pidfCoefficients = new PIDFCoefficients(22.5, 0, 0.001, 15.6);
+
+ public TurretHwMap(HardwareMap hardwareMap) {
+ noTargetTimer.reset();
+
+ // --- Flywheel Motors ---
+ turretLeftMotor = hardwareMap.get(DcMotorEx.class, Constants.TurretConstants.TURRET_LEFT_MOTOR);
+ turretRightMotor = hardwareMap.get(DcMotorEx.class, Constants.TurretConstants.TURRET_RIGHT_MOTOR);
+
+ turretLeftMotor.setDirection(Constants.TurretConstants.TURRET_MOTOR_L_DIRECTION);
+ turretRightMotor.setDirection(Constants.TurretConstants.TURRET_MOTOR_R_DIRECTION);
+
+ turretRightMotor.setMode(DcMotorEx.RunMode.RUN_USING_ENCODER);
+ turretLeftMotor.setMode(DcMotorEx.RunMode.RUN_USING_ENCODER);
+
+ turretRightMotor.setPIDFCoefficients(DcMotorEx.RunMode.RUN_USING_ENCODER, pidfCoefficients);
+ turretLeftMotor.setPIDFCoefficients(DcMotorEx.RunMode.RUN_USING_ENCODER, pidfCoefficients);
+
+ // --- Turret Rotation Motor ---
+ turretrotation = hardwareMap.dcMotor.get(Constants.TurretConstants.TURRET_ROTATION_MOTOR);
+ turretrotation.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+ turretrotation.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+
+ // --- Hood Servo ---
+ hoodservo = hardwareMap.servo.get(Constants.TurretConstants.HOOD_TURRET_SERVO);
+ hoodservo.setDirection(Servo.Direction.FORWARD);
+
+ // --- External Encoder (if used separately) ---
+
+ initLimelight(hardwareMap);
+ }
+
+ public void setPIDF(double p, double i, double d, double f) {
+ PIDFCoefficients pidf = new PIDFCoefficients(p, i, d, f);
+ turretLeftMotor.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, pidf);
+ turretRightMotor.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, pidf);
+ }
+
+
+ public double getTurretRotationPosition() {
+ return turretrotation.getCurrentPosition();
+ }
+
+ public void setTurretRotationPower(double power) {
+ turretrotation.setPower(power);
+ }
+
+ public void stopTurretRotation() {
+ turretrotation.setPower(0);
+ }
+
+ public void setTurretVelocity(double velocity) {
+ turretLeftMotor.setVelocity(-velocity);
+ turretRightMotor.setVelocity(velocity);
+ }
+
+ public double[] getFlywheelVelocities() {
+ double[] velArray = {turretLeftMotor.getVelocity(), turretRightMotor.getVelocity()};
+ return velArray;
+ }
+
+ public double getTurretVelocity(){
+ return turretLeftMotor.getVelocity();
+ }
+
+ public void setTurretPower(double power) {
+ turretLeftMotor.setPower(power);
+ turretRightMotor.setPower(-power);
+ }
+
+ public void turretOff() {
+ turretLeftMotor.setPower(0);
+ turretRightMotor.setPower(0);
+ }
+
+ public void setHoodPos(double pos) {
+ hoodservo.setPosition(pos);
+ }
+
+ public LLResult getLimelightResult() {
+ return limelight.getLatestResult();
+ }
+
+ public boolean hasTarget() {
+ LLResult result = limelight.getLatestResult();
+ return result != null && result.isValid();
+ }
+
+ public double getTargetTX() {
+ LLResult result = limelight.getLatestResult();
+ if (result != null && result.isValid()) {
+ return result.getTx();
+ }
+ return 0;
+ }
+
+ public double getTargetTY() {
+ LLResult result = limelight.getLatestResult();
+ if (result != null && result.isValid()) {
+ return result.getTy();
+ }
+ return 0;
+ }
+
+ public double getTargetTA() { // Area
+ LLResult result = limelight.getLatestResult();
+ if (result != null && result.isValid()) {
+ return result.getTa();
+ }
+ return 0;
+ }
+
+ public double getDistanceFromAprilTag() {
+ if (hasTarget()) {
+ double x = getTargetTA();
+ if (x > 0) {
+ double currentDistance = (74.34095 * Math.pow(x, -0.518935)) + 4;
+ lastKnownDistance = currentDistance;
+ noTargetTimer.reset();
+ return currentDistance;
+ }
+ }
+ if (noTargetTimer.seconds() < 5.0) {
+ return lastKnownDistance;
+ }
+ return 0.0;
+ }
+
+ public void setPipeline(int pipelineIndex) {
+ if (pipelineIndex >= 0 && pipelineIndex <= 2) {
+ limelight.pipelineSwitch(pipelineIndex);
+ }
+ }
+
+ private void initLimelight(HardwareMap hardwareMap) {
+ limelight = hardwareMap.get(Limelight3A.class, "limelight");
+ limelight.pipelineSwitch(0);
+ limelight.start();
+ }
+
+ public void shutdown() {
+ if (limelight != null) {
+ limelight.stop();
+ }
+ }
+
+ public boolean isTurretReady(double velocity) {
+ return velocity >= getTurretVelocity() * 0.9;
+ }
+ }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Prism/Color.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Prism/Color.java
new file mode 100755
index 0000000..afcc91b
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Prism/Color.java
@@ -0,0 +1,56 @@
+/* MIT License
+ * Copyright (c) [2025] [Base 10 Assets, LLC]
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+package org.firstinspires.ftc.teamcode.Prism;
+
+public class Color {
+ public int red;
+ public int green;
+ public int blue;
+
+ public Color(int red, int green, int blue)
+ {
+ this.red = Math.min(red, 255);
+ this.green = Math.min(green, 255);
+ this.blue = Math.min(blue, 255);
+ }
+
+ @Override
+ public String toString()
+ {
+ return String.format("%d, %d, %d", red, green, blue);
+ }
+
+ public static final Color RED = new Color(255, 0, 0);
+ public static final Color ORANGE = new Color(255, 165, 0);
+ public static final Color YELLOW = new Color(255, 255, 0);
+ public static final Color OLIVE = new Color(128, 128, 0);
+ public static final Color GREEN = new Color(0, 255, 0);
+ public static final Color CYAN = new Color(0, 255, 255);
+ public static final Color BLUE = new Color(0, 0, 255);
+ public static final Color TEAL = new Color(0, 128, 128);
+ public static final Color MAGENTA = new Color(255, 0, 255);
+ public static final Color PURPLE = new Color(128, 0, 128);
+ public static final Color PINK = new Color(255, 20, 128);
+ public static final Color WHITE = new Color(255, 255, 255);
+ public static final Color TRANSPARENT = new Color(0, 0, 0);
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Prism/Direction.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Prism/Direction.java
new file mode 100755
index 0000000..44205f3
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Prism/Direction.java
@@ -0,0 +1,28 @@
+/* MIT License
+ * Copyright (c) [2025] [Base 10 Assets, LLC]
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+package org.firstinspires.ftc.teamcode.Prism;
+
+public enum Direction {
+ Forward,
+ Backward
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Prism/GoBildaPrismDriver.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Prism/GoBildaPrismDriver.java
new file mode 100755
index 0000000..c6c481e
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Prism/GoBildaPrismDriver.java
@@ -0,0 +1,441 @@
+/* MIT License
+ * Copyright (c) [2025] [Base 10 Assets, LLC]
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+package org.firstinspires.ftc.teamcode.Prism;
+
+import static com.qualcomm.robotcore.util.TypeConversion.byteArrayToInt;
+import static com.qualcomm.robotcore.util.TypeConversion.unsignedByteToInt;
+
+import com.qualcomm.hardware.lynx.LynxI2cDeviceSynch;
+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 java.nio.ByteOrder;
+import java.util.Arrays;
+import java.util.concurrent.TimeUnit;
+
+@I2cDeviceType
+@DeviceProperties(
+ name = "goBILDA® Prism RGB LED Driver",
+ xmlTag = "goBILDAPrism",
+ description ="Prism RGB LED Driver (6-30V Input, I²C / PWM Control)"
+)
+public class GoBildaPrismDriver extends I2cDeviceSynchDevice {
+ private static final byte DEFAULT_ADDRESS = 0x38;
+ private final int MAXIMUM_NUMBER_OF_ANIMATIONS = 10;
+ private final int MAXIMUM_NUMBER_OF_ANIMATION_GROUPS = 8;
+ private PrismAnimations.AnimationBase[] animations = new PrismAnimations.AnimationBase[MAXIMUM_NUMBER_OF_ANIMATIONS];
+
+ //#region Public Types
+ public enum LayerHeight
+ {
+ LAYER_0 (Register.ANIMATION_SLOT_0),
+ LAYER_1 (Register.ANIMATION_SLOT_1),
+ LAYER_2 (Register.ANIMATION_SLOT_2),
+ LAYER_3 (Register.ANIMATION_SLOT_3),
+ LAYER_4 (Register.ANIMATION_SLOT_4),
+ LAYER_5 (Register.ANIMATION_SLOT_5),
+ LAYER_6 (Register.ANIMATION_SLOT_6),
+ LAYER_7 (Register.ANIMATION_SLOT_7),
+ LAYER_8 (Register.ANIMATION_SLOT_8),
+ LAYER_9 (Register.ANIMATION_SLOT_9),
+ DISABLED(Register.NULL);
+
+ /* Package Private */ final Register register;
+ /* Package Private */ final int index;
+
+ LayerHeight(Register register){
+ this.register = register;
+ if(register == Register.NULL)
+ this.index = -1;
+ else
+ this.index = register.address - Register.ANIMATION_SLOT_0.address;
+ }
+ }
+
+ public enum Artboard
+ {
+ ARTBOARD_0 (0,0),
+ ARTBOARD_1 (1,1),
+ ARTBOARD_2 (2,2),
+ ARTBOARD_3 (3,3),
+ ARTBOARD_4 (4,4),
+ ARTBOARD_5 (5,5),
+ ARTBOARD_6 (6,6),
+ ARTBOARD_7 (7,7);
+
+ /* Package Private */ final byte bitmask;
+ public final int index;
+
+ Artboard(int val,int index){
+ this.bitmask = (byte)(1 << val);
+ this.index = index;
+ }
+ }
+ //#endregion
+
+ //#region Package-Private types
+ /**
+ * Captures the length of each type of register used on the device.
+ */
+ /* Package Private */ enum RegisterType
+ {
+ INT8(1,255),
+ INT16(2, 65535),
+ INT24(3, 16777215),
+ INT32(4, 2147483647);
+
+ /* Package Private */ final int lengthBytes;
+ /* Package Private */ final int maxValue;
+
+ RegisterType(int lengthBytes, int maxValue){
+ this.lengthBytes = lengthBytes;
+ this.maxValue = maxValue;
+ }
+ }
+
+ /* Package Private */ enum RegisterAccess
+ {
+ READ_ONLY,
+ WRITE_ONLY,
+ READ_AND_WRITE;
+ }
+
+ /**
+ * Register map, including register address and register type
+ */
+ /* Package Private */ enum Register
+ {
+ DEVICE_ID (0 , RegisterType.INT8 , RegisterAccess.READ_ONLY),
+ FIRMWARE_VERSION (1 , RegisterType.INT24, RegisterAccess.READ_ONLY),
+ HARDWARE_VERSION (2 , RegisterType.INT16, RegisterAccess.READ_ONLY),
+ POWER_CYCLE_COUNT (3 , RegisterType.INT32, RegisterAccess.READ_ONLY),
+ RUNTIME_IN_MINUTES(4 , RegisterType.INT32, RegisterAccess.READ_ONLY),
+ STATUS (5 , RegisterType.INT32, RegisterAccess.READ_ONLY),
+ CONTROL (6 , RegisterType.INT32, RegisterAccess.WRITE_ONLY),
+ ARTBOARD_CONTROL (7 , RegisterType.INT32, RegisterAccess.WRITE_ONLY),
+ ANIMATION_SLOT_0 (8 , RegisterType.INT32, RegisterAccess.READ_AND_WRITE),
+ ANIMATION_SLOT_1 (9 , RegisterType.INT32, RegisterAccess.READ_AND_WRITE),
+ ANIMATION_SLOT_2 (10, RegisterType.INT32, RegisterAccess.READ_AND_WRITE),
+ ANIMATION_SLOT_3 (11, RegisterType.INT32, RegisterAccess.READ_AND_WRITE),
+ ANIMATION_SLOT_4 (12, RegisterType.INT32, RegisterAccess.READ_AND_WRITE),
+ ANIMATION_SLOT_5 (13, RegisterType.INT32, RegisterAccess.READ_AND_WRITE),
+ ANIMATION_SLOT_6 (14, RegisterType.INT32, RegisterAccess.READ_AND_WRITE),
+ ANIMATION_SLOT_7 (15, RegisterType.INT32, RegisterAccess.READ_AND_WRITE),
+ ANIMATION_SLOT_8 (16, RegisterType.INT32, RegisterAccess.READ_AND_WRITE),
+ ANIMATION_SLOT_9 (17, RegisterType.INT32, RegisterAccess.READ_AND_WRITE),
+ NULL (18, RegisterType.INT8 , RegisterAccess.READ_ONLY);
+
+ /* Package Private */ final int address;
+ /* Package Private */ final RegisterType registerType;
+ /* Package Private */ final RegisterAccess registerAccess;
+
+ Register(int address, RegisterType registerType, RegisterAccess registerAccess){
+ this.address = address;
+ this.registerType = registerType;
+ this.registerAccess = registerAccess;
+ }
+ }
+
+ private int readInt(Register register){
+ return byteArrayToInt(deviceClient.read(register.address,register.registerType.lengthBytes),ByteOrder.LITTLE_ENDIAN);
+ }
+
+ //#endregion
+
+ public GoBildaPrismDriver(I2cDeviceSynchSimple deviceClient, boolean deviceClientIsOwned)
+ {
+ super(deviceClient, deviceClientIsOwned);
+
+ this.deviceClient.setI2cAddress(I2cAddr.create7bit(DEFAULT_ADDRESS));
+ super.registerArmingStateCallback(false);
+ }
+
+ @Override
+ public Manufacturer getManufacturer()
+ {
+ return Manufacturer.GoBilda;
+ }
+
+ @Override
+ protected synchronized boolean doInitialize()
+ {
+ ((LynxI2cDeviceSynch)(deviceClient)).setBusSpeed(LynxI2cDeviceSynch.BusSpeed.FAST_400K);
+ return true;
+ }
+
+ @Override
+ public String getDeviceName()
+ {
+ return "goBILDA® Prism RGB LED Driver";
+ }
+
+ /**
+ * @return 3 if device is functional.
+ */
+ public int getDeviceID(){
+ //return readInt(Register.DEVICE_ID);
+ byte[] packet = deviceClient.read(Register.DEVICE_ID.address, Register.DEVICE_ID.registerType.lengthBytes);
+ return packet[0];
+ }
+
+ /**
+ * @return Firmware Version of device. Array[0] is the major version, Array[1] is the minor version
+ * Array[2] is the patch version.
+ */
+ public int[] getFirmwareVersion(){
+ byte[] packet = deviceClient.read(Register.FIRMWARE_VERSION.address,Register.FIRMWARE_VERSION.registerType.lengthBytes);
+ int[] output = new int[3];
+ output[0] = packet[2];
+ output[1] = packet[1];
+ output[2] = packet[0];
+ return output;
+ }
+
+ /**
+ * @return Hardware version of the device as a string.
+ */
+ public String getFirmwareVersionString(){
+ byte[] packet = deviceClient.read(Register.FIRMWARE_VERSION.address,Register.FIRMWARE_VERSION.registerType.lengthBytes);
+ int[] output = new int[3];
+ output[0] = packet[2];
+ output[1] = packet[1];
+ output[2] = packet[0];
+ return String.format("%d.%d.%d", output[0], output[1], output[2]);
+ }
+
+ /**
+ * @return Hardware version of device. Array[0] is the major version, Array[1] is the minor version.
+ */
+ public int[] getHardwareVersion(){
+ byte[] packet = deviceClient.read(Register.HARDWARE_VERSION.address,Register.HARDWARE_VERSION.registerType.lengthBytes);
+ int[] output = new int[2];
+ output[0] = packet[1];
+ output[1] = packet[0];
+ return output;
+ }
+ public String getHardwareVersionString(){
+ byte[] packet = deviceClient.read(Register.HARDWARE_VERSION.address,Register.HARDWARE_VERSION.registerType.lengthBytes);
+ int[] output = new int[2];
+ output[0] = packet[1];
+ output[1] = packet[0];
+ return String.format("%d.%d", output[0], output[1]);
+ }
+
+ /**
+ * @return The number of times the device has power cycled in its lifetime.
+ */
+ public int getPowerCycleCount(){
+ return readInt(Register.POWER_CYCLE_COUNT);
+ }
+
+ /**
+ * @return Total device runtime in minutes
+ */
+ public long getRunTime(TimeUnit timeUnit){
+ return timeUnit.convert(readInt(Register.RUNTIME_IN_MINUTES),TimeUnit.MINUTES);
+ }
+
+ /**
+ * @return total LEDs in strip
+ */
+ public int getNumberOfLEDs(){
+ byte[] packet = deviceClient.read(Register.STATUS.address,Register.STATUS.registerType.lengthBytes);
+ return unsignedByteToInt(packet[0]);
+ }
+
+ /**
+ * @return Current Animation frames per second
+ */
+ public int getCurrentFPS(){
+ byte[] inputPacket = deviceClient.read(Register.STATUS.address,Register.STATUS.registerType.lengthBytes);
+ byte[] outputPacket = new byte[4];
+ outputPacket[0] = inputPacket[1];
+ outputPacket[1] = inputPacket[2];
+
+ return byteArrayToInt(outputPacket,ByteOrder.LITTLE_ENDIAN);
+ }
+
+ /**
+ * @return
+ */
+ public boolean fpsLimited(){
+ byte[] packet = deviceClient.read(Register.STATUS.address, Register.STATUS.registerType.lengthBytes);
+ //return packet[3] >> 7;
+ return false;
+ }
+
+ /**
+ * return What artboard is set as default boot animation?
+ */
+ public int getBootAnimationArtboard(){
+ byte[] packet = deviceClient.read(Register.STATUS.address, Register.STATUS.registerType.lengthBytes);
+ return 0;
+ }
+
+
+ /**
+ * Inserts an animation into the specified slot in the animations array.
+ *
+ * @param height the height where the animation should be inserted (0-9)
+ * @param animation the AnimationBase object to insert into the array
+ * @return true if the animation was successfully inserted, false if the index
+ * is out of bounds or the animation is null
+ */
+ public boolean insertAnimation(LayerHeight height, PrismAnimations.AnimationBase animation)
+ {
+ if(height == LayerHeight.DISABLED || animation == null)
+ return false;
+
+ animations[height.index] = animation;
+ animations[height.index].layerHeight = height;
+ return true;
+ }
+
+ /**
+ * Inserts an animation at the specified index and immediately updates it over I2C.
+ *
+ * @param height the height where the animation should be inserted and updated (0-9)
+ * @param animation the AnimationBase object to insert and update
+ * @return true if both insertion and update operations were successful,
+ * false if either operation failed
+ */
+ public boolean insertAndUpdateAnimation(LayerHeight height, PrismAnimations.AnimationBase animation)
+ {
+ if(insertAnimation(height, animation))
+ return updateAnimationFromIndex(height, true);
+ return false;
+ }
+
+ /**
+ * Updates all animations in the manager by sending their data over I2C.
+ * Iterates through all animation slots and updates any non-null animations.
+ *
+ * @return true if the update process completed (currently always returns true)
+ */
+ public boolean updateAllAnimations()
+ {
+ for(int i = 0; i < MAXIMUM_NUMBER_OF_ANIMATIONS; i++){
+ if(animations[i] != null && animations[i].layerHeight != LayerHeight.DISABLED)
+ animations[i].updateAnimationOverI2C(this.deviceClient, false);
+ }
+ return true;
+ }
+
+ /**
+ * Updates a specific animation at the given Layer Height by sending its data over I2C.
+ *
+ * @param height the height of the animation to update (0-9)
+ * @return true if the animation was successfully updated, false if the index
+ * is out of bounds or if no animation exists at the specified index
+ */
+ public boolean updateAnimationFromIndex(LayerHeight height)
+ {
+ return updateAnimationFromIndex(height, false);
+ }
+
+ public void clearAllAnimations()
+ {
+ byte[] packet = TypeConversion.intToByteArray(1 << 25, ByteOrder.LITTLE_ENDIAN);
+
+ deviceClient.write(Register.CONTROL.address, packet);
+ Arrays.fill(animations, null);
+ }
+
+ public void setTargetFPS(int targetFPS)
+ {
+ final int boundedTargetFps = Math.max(0, Math.min(targetFPS, 0x7FFF));
+ final int ChangeTargetFpsBit = 1 << 15;
+ final int ChangeTargetFpsCommand = ChangeTargetFpsBit | boundedTargetFps;
+ byte[] packet = TypeConversion.intToByteArray(ChangeTargetFpsCommand, ByteOrder.LITTLE_ENDIAN);
+ deviceClient.write(Register.CONTROL.address, packet);
+ }
+
+ public void setStripLength(int stripLength)
+ {
+ final int boundedStripLength = Math.max(0, Math.min(stripLength, 0xFF));
+ final int ChangeStripLengthBit = 1 << 24;
+ final int ChangeStripLengthCommand = ChangeStripLengthBit | (boundedStripLength << 16);
+ byte[] packet = TypeConversion.intToByteArray(ChangeStripLengthCommand, ByteOrder.LITTLE_ENDIAN);
+ deviceClient.write(Register.CONTROL.address, packet);
+ }
+
+ public void saveCurrentAnimationsToArtboard(Artboard artboard)
+ {
+ byte[] data = {
+ artboard.bitmask,
+ 0,
+ 0,
+ 0
+ };
+ deviceClient.write(Register.ARTBOARD_CONTROL.address, data);
+ }
+
+ public void loadAnimationsFromArtboard(Artboard artboard)
+ {
+ byte[] data = {
+ 0,
+ artboard.bitmask,
+ 0,
+ 0
+ };
+ deviceClient.write(Register.ARTBOARD_CONTROL.address, data);
+ }
+
+ public void setDefaultBootArtboard(Artboard artboard)
+ {
+ byte[] data = {
+ 0,
+ 0,
+ artboard.bitmask,
+ 0
+ };
+ deviceClient.write(Register.ARTBOARD_CONTROL.address, data);
+ }
+
+ public void enableDefaultBootArtboard(boolean enable)
+ {
+ byte[] data = {
+ 0,
+ 0,
+ 0,
+ enable ? (byte)0b00000001 : (byte)0b00000010
+ };
+ deviceClient.write(Register.ARTBOARD_CONTROL.address, data);
+ }
+
+ private boolean updateAnimationFromIndex(LayerHeight height, boolean isBeingInserted)
+ {
+ if(height == LayerHeight.DISABLED || animations[height.index] == null)
+ return false;
+
+ boolean animationEnabled = animations[height.index].layerHeight != LayerHeight.DISABLED;
+ if(animationEnabled)
+ animations[height.index].updateAnimationOverI2C(this.deviceClient, isBeingInserted);
+ return animationEnabled;
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Prism/PrismAnimations.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Prism/PrismAnimations.java
new file mode 100755
index 0000000..9f2fcc3
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Prism/PrismAnimations.java
@@ -0,0 +1,1068 @@
+/* MIT License
+ * Copyright (c) [2025] [Base 10 Assets, LLC]
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+package org.firstinspires.ftc.teamcode.Prism;
+
+import java.nio.ByteBuffer;
+import java.nio.ByteOrder;
+import java.util.concurrent.TimeUnit;
+
+import com.qualcomm.robotcore.hardware.I2cDeviceSynchSimple;
+import com.qualcomm.robotcore.util.TypeConversion;
+import static org.firstinspires.ftc.teamcode.Prism.GoBildaPrismDriver.LayerHeight;
+
+public class PrismAnimations {
+ public enum AnimationType{
+ NONE(0),
+ SOLID(1),
+ BLINK(2),
+ PULSE(3),
+ SINE_WAVE(4),
+ DROID_SCAN(5),
+ RAINBOW(6),
+ SNAKES(7),
+ RANDOM(8),
+ SPARKLE(9),
+ SINGLE_FILL(10),
+ RAINBOW_SNAKES(11),
+ POLICE_LIGHTS(12);
+
+ public final int AnimationTypeIndex;
+ AnimationType(int animationType){
+ this.AnimationTypeIndex = animationType;
+ }
+ }
+
+ public static abstract class AnimationBase {
+ protected final AnimationType animationType;
+ protected int brightness = 100;
+ protected int startIndex = 0;
+ protected int stopIndex = 255;
+ protected LayerHeight layerHeight;
+
+ //region Constructors
+ protected AnimationBase(AnimationType type){
+ this.animationType = type;
+ }
+
+ /**
+ * @param brightness from 0 to 100.
+ */
+ protected AnimationBase(AnimationType type, int brightness) {
+ this(type);
+ this.brightness = Math.min(brightness, 100);
+ }
+
+ /**
+ * Set both the first, and last LED in a string that you'd like this animation to show on.
+ * @param startIndex from 0 to 255.
+ * @param stopIndex from 0 to 255.
+ */
+ protected AnimationBase(AnimationType type, int startIndex, int stopIndex) {
+ this(type);
+ this.startIndex = Math.min(startIndex, 255);
+ this.stopIndex = Math.min(stopIndex, 255);
+ }
+
+ /**
+ * @param brightness from 0 to 100.
+ * @param startIndex from 0 to 255.
+ * @param stopIndex from 0 to 255.
+ */
+ protected AnimationBase(AnimationType type, int brightness, int startIndex, int stopIndex){
+ this(type, brightness);
+ this.startIndex = Math.min(startIndex, 255);
+ this.stopIndex = Math.min(stopIndex, 255);
+ }
+
+ /**
+ * @param brightness from 0 to 100.
+ * @param startIndex from 0 to 255.
+ * @param stopIndex from 0 to 255.
+ * @param layerHeight the height that this animation should sit at from 0 to 9.
+ */
+ protected AnimationBase(AnimationType type, int brightness, int startIndex, int stopIndex, LayerHeight layerHeight){
+ this(type, brightness, startIndex, stopIndex);
+ this.layerHeight = layerHeight;
+ }
+ //endregion
+
+ //region Setters / Getters
+ public int getBrightness() { return brightness; }
+ public int getStartIndex() { return startIndex; }
+ public int getStopIndex() { return stopIndex; }
+
+ /**
+ * Brightness of selected animation.
+ * @param brightness From 0 to 100.
+ */
+ public void setBrightness(int brightness) { this.brightness = Math.min(brightness, 100); }
+
+ /**
+ * The first LED in the string that you'd like this animation to display on.
+ * @param startIndex from 0 to 255.
+ */
+ public void setStartIndex(int startIndex) { this.startIndex = Math.min(startIndex, 255); }
+
+ /**
+ * The last LED in the string that you'd like this animation to display on.
+ * @param stopIndex from 0 to 255.
+ */
+ public void setStopIndex(int stopIndex) { this.stopIndex = Math.min(stopIndex, 255); }
+
+ /**
+ * Set both the first, and last LED in a string that you'd like this animation to show on.
+ * @param startIndex from 0 to 255.
+ * @param stopIndex from 0 to 255.
+ */
+ public void setIndexes(int startIndex, int stopIndex){
+ this.startIndex = (byte)startIndex;
+ this.stopIndex = (byte)stopIndex;
+ }
+ //endregion
+
+ protected abstract void updateAnimationSpecificValuesOverI2C(I2cDeviceSynchSimple deviceClient);
+
+ protected void updateAnimationOverI2C(I2cDeviceSynchSimple deviceClient, boolean isInsertingAnimation)
+ {
+ if(isInsertingAnimation)
+ deviceClient.write(layerHeight.register.address, GetByteArray(0, animationType.AnimationTypeIndex));
+ deviceClient.write(layerHeight.register.address, GetByteArray(1, brightness));
+ deviceClient.write(layerHeight.register.address, GetByteArray(2, startIndex));
+ deviceClient.write(layerHeight.register.address, GetByteArray(3, stopIndex));
+ updateAnimationSpecificValuesOverI2C(deviceClient);
+ }
+
+ protected byte[] floatToByteArray (float value, ByteOrder byteOrder)
+ {
+ return ByteBuffer.allocate(4).order(byteOrder).putFloat(value).array();
+ }
+
+ private byte unsignedIntToByte(int data)
+ {
+ int boundedData = Math.min(Math.max(data, 0), 255);
+ ByteBuffer buffer = ByteBuffer.allocate(4).putInt(boundedData);
+
+ return buffer.array()[3];
+ }
+
+ protected byte[] GetByteArray(int subRegister, Color color)
+ {
+ return new byte[] {
+ (byte)Math.max(0, Math.min(subRegister, 12)),
+ (byte)color.red,
+ (byte)color.green,
+ (byte)color.blue
+ };
+ }
+
+ protected byte[] GetByteArray(int subRegister, int data)
+ {
+ byte[] packet = TypeConversion.intToByteArray(data, ByteOrder.LITTLE_ENDIAN);
+ return new byte[] {
+ (byte)Math.max(0, Math.min(subRegister, 12)),
+ packet[0],
+ packet[1],
+ packet[2],
+ packet[3]
+ };
+ }
+
+ protected byte[] GetByteArray(int subRegister, float data)
+ {
+ byte[] packet = floatToByteArray(data, ByteOrder.LITTLE_ENDIAN);
+ return new byte[] {
+ (byte)Math.max(0, Math.min(subRegister, 12)),
+ packet[0],
+ packet[1],
+ packet[2],
+ packet[3]
+ };
+ }
+
+ protected byte[] GetByteArray(int subRegister, byte data)
+ {
+ return new byte[]{
+ (byte)subRegister,
+ data
+ };
+ }
+
+ protected byte[] GetByteArray(int subRegister, Direction direction)
+ {
+ boolean bool = direction == Direction.Forward;
+ return new byte[]{
+ (byte)Math.max(0, Math.min(subRegister, 12)),
+ (byte) (bool ? 1 : 0)
+ };
+ }
+
+ protected byte[] GetByteArray(int subRegister, Color... colors)
+ {
+ byte[] array = new byte[1+(colors.length*3)];
+
+ array[0] = (byte)Math.max(0, Math.min(subRegister, 12));
+
+ for(int i = 0; i < colors.length; i++){
+ array[1+(i*3)] = unsignedIntToByte(colors[i].red);
+ array[1+(i*3+1)] = unsignedIntToByte(colors[i].green);
+ array[1+(i*3+2)] = unsignedIntToByte(colors[i].blue);
+ }
+
+ return array;
+ }
+ }
+
+ public static class Blink extends AnimationBase{
+ private Color primaryColor = Color.BLUE;
+ private Color secondaryColor = Color.RED;
+ private int period = 2000;
+ private int primaryColorPeriod = 1000;
+
+ public Blink(){ super(AnimationType.BLINK); }
+ public Blink(Color primaryColor) {
+ this();
+ this.primaryColor = primaryColor;
+ }
+ public Blink(Color primaryColor, Color secondaryColor) {
+ this(primaryColor);
+ this.secondaryColor = secondaryColor;
+ }
+ public Blink(Color primaryColor, Color secondaryColor, int period) {
+ this(primaryColor, secondaryColor);
+ this.period = period;
+ }
+ public Blink(Color primaryColor, Color secondaryColor, int period, int primaryColorPeriod) {
+ this(primaryColor, secondaryColor, period);
+ this.primaryColorPeriod = primaryColorPeriod;
+ }
+
+ /**
+ * set the length of one loop of an animation in milliseconds.
+ * @param period from 0 - 4,294,697,295. Larger is longer.
+ */
+ public void setPeriod(int period) { this.period = period; }
+
+ /**
+ * set the length of one loop of an animation in specified unit.
+ * @param duration duration of loop.
+ * @param timeUnit unit to use.
+ */
+ public void setPeriod(int duration, TimeUnit timeUnit){
+ this.period = Math.toIntExact(timeUnit.toMillis(duration));
+ }
+ public void setPrimaryColor(Color color) { primaryColor = color; }
+ public void setSecondaryColor(Color color) { secondaryColor = color; }
+ public void setPrimaryColor(int red, int green, int blue) { primaryColor = new Color(red, green, blue); }
+ public void setSecondaryColor(int red, int green, int blue) { secondaryColor = new Color(red, green, blue); }
+ /**
+ * set the duration of the primary color in a blink.
+ * @param primaryColorPeriod from 0 - 4,294,697,295. Larger is longer.
+ */
+ public void setPrimaryColorPeriod(int primaryColorPeriod) { this.primaryColorPeriod = primaryColorPeriod; }
+
+ /**
+ * set the duration of the primary color in a blink.
+ * @param duration duration of loop.
+ * @param timeUnit unit to use.
+ */
+ public void setPrimaryColorPeriod(int duration, TimeUnit timeUnit){
+ this.primaryColorPeriod = Math.toIntExact(timeUnit.toMillis(duration));
+ }
+
+ public int getPeriod() { return period; }
+ public Color getPrimaryColor() { return primaryColor; }
+ public Color getSecondaryColor() { return secondaryColor; }
+ public int getPrimaryColorPeriod() { return primaryColorPeriod; }
+
+ @Override
+ protected void updateAnimationSpecificValuesOverI2C(I2cDeviceSynchSimple deviceClient)
+ {
+ deviceClient.write(layerHeight.register.address, GetByteArray(4, primaryColor));
+ deviceClient.write(layerHeight.register.address, GetByteArray(5, secondaryColor));
+ deviceClient.write(layerHeight.register.address, GetByteArray(6, period));
+ deviceClient.write(layerHeight.register.address, GetByteArray(7, primaryColorPeriod));
+ }
+ }
+
+ public static class DroidScan extends AnimationBase{
+ private Color primaryColor = Color.RED;
+ private Color secondaryColor = Color.TRANSPARENT;
+ private DroidScanStyle style = DroidScanStyle.BOTH_TAIL;
+ private float speed = .4f;
+ private int eyeWidth = 3;
+ private int trailWidth = 3;
+
+ public enum DroidScanStyle{
+ NO_TAIL(0),
+ FRONT_TAIL(1),
+ BACK_TAIL(2),
+ BOTH_TAIL(3);
+
+ /* Package Private */ final byte styleValue;
+ DroidScanStyle(int value){
+ styleValue = (byte)value;
+ }
+ }
+
+ public DroidScan(){ super(AnimationType.DROID_SCAN); }
+ public DroidScan(Color primaryColor) {
+ this();
+ this.primaryColor = primaryColor;
+ }
+ public DroidScan(Color primaryColor, Color secondaryColor) {
+ this(primaryColor);
+ this.secondaryColor = secondaryColor;
+ }
+
+ public void setSecondaryColor(Color color) { this.secondaryColor = color; }
+ public void setPrimaryColor(Color color) { this.primaryColor = color; }
+
+ /**
+ * Sets the speed of the animation
+ * @param speed from 0 to 1.
+ */
+ public void setSpeed(float speed) { this.speed = speed; }
+
+ /**
+ * Sets the width of the "eye" in the animation in pixels
+ * @param eyeWidth from 0 to 255.
+ */
+ public void setEyeWidth(int eyeWidth) { this.eyeWidth = eyeWidth; }
+
+ /**
+ * Sets the width of the trail of the eye in the animation in pixels
+ * @param trailWidth from 0 to 255.
+ */
+ public void setTrailWidth(int trailWidth) { this.trailWidth = trailWidth; }
+ public void setDroidScanStyle(DroidScanStyle style) { this.style = style; }
+
+ public Color getSecondaryColor() { return secondaryColor; }
+ public Color getPrimaryColor() { return primaryColor; }
+ public float getSpeed() { return speed; }
+ public int getEyeWidth() { return eyeWidth; }
+ public int getTrailWidth() { return trailWidth; }
+ public DroidScanStyle getDroidScanStyle() { return style; }
+
+ @Override
+ protected void updateAnimationSpecificValuesOverI2C(I2cDeviceSynchSimple deviceClient)
+ {
+ deviceClient.write(layerHeight.register.address, GetByteArray(4 , primaryColor));
+ deviceClient.write(layerHeight.register.address, GetByteArray(5 , secondaryColor));
+ deviceClient.write(layerHeight.register.address, GetByteArray(6 , speed));
+ deviceClient.write(layerHeight.register.address, GetByteArray(7 , (byte)eyeWidth));
+ deviceClient.write(layerHeight.register.address, GetByteArray(8 , (byte)trailWidth));
+ deviceClient.write(layerHeight.register.address, GetByteArray(11, style.styleValue));
+ }
+ }
+
+ public static class PoliceLights extends AnimationBase {
+ private Color red = Color.RED;
+ private Color white = Color.WHITE;
+ private Color blue = Color.BLUE;
+ private int period = 1000;
+ private PoliceLightsStyle style = PoliceLightsStyle.Style1;
+
+ public enum PoliceLightsStyle{
+ Style1(0),
+ Style2(1),
+ Style3(2);
+
+ public byte styleValue;
+ PoliceLightsStyle(int value){
+ styleValue = (byte)value;
+ }
+ }
+
+ public PoliceLights(){ super(AnimationType.POLICE_LIGHTS); }
+
+ /**
+ * set the length of one loop of an animation in milliseconds.
+ * @param period from 0 - 4,294,697,295. Larger is longer.
+ */
+ public void setPeriod(int period) { this.period = period; }
+
+ /**
+ * set the length of one loop of an animation in specified unit.
+ * @param duration duration of loop.
+ * @param timeUnit unit to use.
+ */
+ public void setPeriod(int duration, TimeUnit timeUnit){
+ this.period = Math.toIntExact(timeUnit.toMillis(duration));
+ }
+ public void setPoliceLightsStyle(PoliceLightsStyle style){this.style = style;}
+ public void setPrimaryColor(Color color){this.red = color;}
+ public void setSecondaryColor(Color color){this.white = color;}
+ public void setTertiaryColor(Color color){this.blue = color;}
+
+ public PoliceLightsStyle getPoliceLightsStyle(){return style;}
+ public Color getPrimaryColor(){return this.red;}
+ public Color getSecondaryColor(){return this.white;}
+ public Color getTertiaryColor(){return this.blue;}
+
+ @Override
+ protected void updateAnimationSpecificValuesOverI2C(I2cDeviceSynchSimple deviceClient)
+ {
+ deviceClient.write(layerHeight.register.address, GetByteArray(6 , period));
+ deviceClient.write(layerHeight.register.address, GetByteArray(7 , red));
+ deviceClient.write(layerHeight.register.address, GetByteArray(8 , white));
+ deviceClient.write(layerHeight.register.address, GetByteArray(9 , blue));
+ deviceClient.write(layerHeight.register.address, GetByteArray(11, style.styleValue));
+ }
+ }
+
+ public static class Pulse extends AnimationBase {
+ private Color primaryColor = Color.GREEN;
+ private Color secondaryColor = Color.RED;
+ private int period = 1000;
+
+ public Pulse(){ super(AnimationType.PULSE); }
+ public Pulse(Color primaryColor) {
+ this();
+ this.primaryColor = primaryColor;
+ }
+ public Pulse(Color primaryColor, Color secondaryColor) {
+ this(primaryColor);
+ this.secondaryColor = secondaryColor;
+ }
+ public Pulse(Color primaryColor, Color secondaryColor, int period) {
+ this(primaryColor, secondaryColor);
+ this.period = period;
+ }
+
+ /**
+ * set the period of the pulse in milliseconds.
+ * @param period from 0 - 4,294,697,295. Larger is longer.
+ */
+ public void setPeriod(int period) { this.period = period; }
+
+ /**
+ * set the period of the pulse in specified unit.
+ * @param duration duration of loop.
+ * @param timeUnit unit to use.
+ */
+ public void setPeriod(int duration, TimeUnit timeUnit){
+ this.period = Math.toIntExact(timeUnit.toMillis(duration));
+ }
+ public void setPrimaryColor(Color color) { primaryColor = color; }
+ public void setSecondaryColor(Color color) { secondaryColor = color; }
+ public void setPrimaryColor(int red, int green, int blue) { primaryColor = new Color((byte)red, (byte)green, (byte)blue); }
+ public void setSecondaryColor(int red, int green, int blue) { secondaryColor = new Color((byte)red, (byte)green, (byte)blue); }
+
+ public int getPeriod() { return period; }
+ public Color getPrimaryColor() { return primaryColor; }
+ public Color getSecondaryColor() { return secondaryColor; }
+
+ @Override
+ protected void updateAnimationSpecificValuesOverI2C(I2cDeviceSynchSimple deviceClient)
+ {
+ deviceClient.write(layerHeight.register.address, GetByteArray(4, primaryColor));
+ deviceClient.write(layerHeight.register.address, GetByteArray(5, secondaryColor));
+ deviceClient.write(layerHeight.register.address, GetByteArray(6, period));
+ }
+ }
+
+ public static class Rainbow extends AnimationBase {
+ private float startHue = 0.0f;
+ private float stopHue = 360.0f;
+ private float speed = 0.50f; // 0.00 - 1.00
+ private int repeatAfter = 25;
+ private Direction direction = Direction.Forward;
+
+ public Rainbow(){ super(AnimationType.RAINBOW); }
+ public Rainbow(float startHue) {
+ this();
+ this.startHue = startHue;
+ }
+ public Rainbow(float startHue, float stopHue){
+ this(startHue);
+ this.stopHue = stopHue;
+ }
+ public Rainbow(float startHue, float stopHue, float speed){
+ this(startHue, stopHue);
+ this.speed = speed;
+ }
+ public Rainbow(float startHue, float stopHue, float speed, Direction direction){
+ this(startHue, stopHue, speed);
+ this.direction = direction;
+ }
+ public Rainbow(float startHue, float stopHue, float speed, Direction direction, int repeatAfter){
+ this(startHue, stopHue, speed, direction);
+ this.repeatAfter = repeatAfter;
+ }
+
+ //region Getters/Setters
+ public float getSpeed() { return speed;}
+ public float getStopHue() { return stopHue; }
+ public float getStartHue() { return startHue; }
+ public Direction getDirection() { return direction; }
+ public int getRepeatAfter() { return repeatAfter; }
+
+ /**
+ * Sets the speed of the animation
+ * @param speed from 0 to 1.
+ */
+ public void setSpeed(float speed) { this.speed = speed; }
+
+ /**
+ * Can be used to limit the colors shown in the rainbow. This is a hue so it reflects
+ * the position as an angle on a color wheel. Red is 0, Green is 120, Blue is 240, etc.
+ * @param startHue from 0 to 360.
+ */
+ public void setStartHue(float startHue){ this.startHue = startHue; }
+
+ /**
+ * Can be used to limit the colors shown in the rainbow. This is a hue so it reflects
+ * the position as an angle on a color wheel. Red is 0, Green is 120, Blue is 240, etc.
+ * @param stopHue from 0 to 360.
+ */
+ public void setStopHue(float stopHue) { this.stopHue = stopHue; }
+ public void setDirection(Direction direction) { this.direction = direction; }
+
+ /**
+ * The number of pixels before the rainbow repeats. Default is 20.
+ * @param repeatAfter From 0 to 255.
+ */
+ public void setRepeatAfter(int repeatAfter) { this.repeatAfter = repeatAfter; }
+ public void setHues(float startHue, float stopHue) {
+ this.startHue = startHue;
+ this.stopHue = stopHue;
+ }
+ //endregion
+ @Override
+ protected void updateAnimationSpecificValuesOverI2C(I2cDeviceSynchSimple deviceClient)
+ {
+ deviceClient.write(layerHeight.register.address, GetByteArray(4, startHue));
+ deviceClient.write(layerHeight.register.address, GetByteArray(5, stopHue));
+ deviceClient.write(layerHeight.register.address, GetByteArray(6, speed));
+ deviceClient.write(layerHeight.register.address, GetByteArray(7, direction));
+ deviceClient.write(layerHeight.register.address, GetByteArray(9, (byte)repeatAfter));
+ }
+ }
+
+ public static class RainbowSnakes extends AnimationBase {
+ private int numberOfSnakes = 3;
+ private int snakeLength = 5;
+ private int spacingBetween = 2;
+ private int repeatAfter = 15;
+ private float startHue = 0.0f;
+ private float stopHue = 360.0f;
+ private Color backgroundColor = Color.TRANSPARENT;
+ private float speed = 0.50f;
+ private Direction direction = Direction.Backward;
+
+ //region Constructors
+ public RainbowSnakes(){ super(AnimationType.RAINBOW_SNAKES); }
+ public RainbowSnakes(float startHue, float stopHue) {
+ this();
+ this.startHue = startHue;
+ this.stopHue = stopHue;
+ }
+ public RainbowSnakes(float startHue, float stopHue, int numberOfSnakes){
+ this(startHue, stopHue);
+ this.numberOfSnakes = (byte)numberOfSnakes;
+ }
+ public RainbowSnakes(float startHue, float stopHue, int numberOfSnakes, int snakeLength){
+ this(startHue, stopHue, numberOfSnakes);
+ this.snakeLength = (byte)snakeLength;
+ }
+ public RainbowSnakes(float startHue, float stopHue, int numberOfSnakes, int snakeLength, int spacingBetween){
+ this(startHue, stopHue, numberOfSnakes, snakeLength);
+ this.spacingBetween = (byte)spacingBetween;
+ }
+ public RainbowSnakes(float startHue, float stopHue, int numberOfSnakes, int snakeLength, int spacingBetween, int repeatAfter){
+ this(startHue, stopHue, numberOfSnakes, snakeLength, spacingBetween);
+ this.repeatAfter = (byte)repeatAfter;
+ }
+ public RainbowSnakes(float startHue, float stopHue, int numberOfSnakes, int snakeLength, int spacingBetween, int repeatAfter, Color backgroundColor){
+ this(startHue, stopHue, numberOfSnakes, snakeLength, spacingBetween, repeatAfter);
+ this.backgroundColor = backgroundColor;
+ }
+ public RainbowSnakes(float startHue, float stopHue, int numberOfSnakes, int snakeLength, int spacingBetween, int repeatAfter, Color backgroundColor, float speed){
+ this(startHue, stopHue, numberOfSnakes, snakeLength, spacingBetween, repeatAfter, backgroundColor);
+ this.speed = speed;
+ }
+ public RainbowSnakes(float startHue, float stopHue, int numberOfSnakes, int snakeLength, int spacingBetween, int repeatAfter, Color backgroundColor, float speed, Direction direction){
+ this(startHue, stopHue, numberOfSnakes, snakeLength, spacingBetween, repeatAfter, backgroundColor, speed);
+ this.direction = direction;
+ }
+ //endregion
+
+ //region Getters/Setters
+ public float getSpeed() { return speed; }
+ public float getStartHue() { return startHue;}
+ public float getStopHue() { return stopHue; }
+ public int getSnakeLength() { return snakeLength; }
+ public int getRepeatAfter() { return repeatAfter; }
+ public Direction getDirection() { return direction; }
+ public int getSpacingBetween() { return spacingBetween; }
+ public Color getBackgroundColor() { return backgroundColor; }
+ public int getNumberOfSnakes() { return numberOfSnakes; }
+
+ /**
+ * Sets the speed of the animation
+ * @param speed from 0 to 1.
+ */
+ public void setSpeed(float speed) { this.speed = speed; }
+
+ /**
+ * Can be used to limit the colors shown in the rainbow. This is a hue so it reflects
+ * the position as an angle on a color wheel. Red is 0, Green is 120, Blue is 240, etc.
+ * @param startHue from 0 to 360.
+ */
+ public void setStartHue(float startHue) { this.startHue = startHue; }
+
+ /**
+ * Can be used to limit the colors shown in the rainbow. This is a hue so it reflects
+ * the position as an angle on a color wheel. Red is 0, Green is 120, Blue is 240, etc.
+ * @param stopHue from 0 to 360.
+ */
+ public void setStopHue(float stopHue) { this.stopHue = stopHue; }
+ public void setHues(float startHue, float stopHue) {
+ this.startHue = startHue;
+ this.stopHue = stopHue;
+ }
+ public void setDirection(Direction direction) { this.direction = direction; }
+
+ /**
+ * Length in pixels of each snake.
+ * @param snakeLength from 0 to 255.
+ */
+ public void setSnakeLength(int snakeLength) { this.snakeLength = snakeLength; }
+
+ /**
+ * The number of pixels between the last snake and the first snake of a repeating animation.
+ * @param repeatAfter from 0 to 255.
+ */
+ public void setRepeatAfter(int repeatAfter) { this.repeatAfter = repeatAfter; }
+
+ /**
+ * The number of pixels between consecutive snakes.
+ * @param spacingBetween from 0 to 255.
+ */
+ public void setSpacingBetween(int spacingBetween) { this.spacingBetween = spacingBetween; }
+ public void setBackgroundColor(Color backgroundColor) { this.backgroundColor = backgroundColor; }
+ public void setNumberOfSnakes(int numberOfSnakes) { this.numberOfSnakes = numberOfSnakes; }
+ //endregion
+
+ @Override
+ protected void updateAnimationSpecificValuesOverI2C(I2cDeviceSynchSimple deviceClient)
+ {
+ deviceClient.write(layerHeight.register.address, GetByteArray(4 , startHue));
+ deviceClient.write(layerHeight.register.address, GetByteArray(5 , stopHue));
+ deviceClient.write(layerHeight.register.address, GetByteArray(6 , speed));
+ deviceClient.write(layerHeight.register.address, GetByteArray(7 , direction));
+ deviceClient.write(layerHeight.register.address, GetByteArray(8 , (byte)spacingBetween));
+ deviceClient.write(layerHeight.register.address, GetByteArray(9 , (byte)repeatAfter));
+ deviceClient.write(layerHeight.register.address, GetByteArray(10, backgroundColor));
+ deviceClient.write(layerHeight.register.address, GetByteArray(11, (byte)snakeLength));
+ deviceClient.write(layerHeight.register.address, GetByteArray(12, (byte)numberOfSnakes));
+ }
+ }
+
+ public static class Random extends AnimationBase {
+ private float startHue = 0f;
+ private float stopHue = 360f;
+ private float speed = 0.1f;
+
+ //region Constructors
+ public Random(){ super(AnimationType.RANDOM); }
+ public Random(float startHue, float stopHue){
+ this();
+ setHues(startHue, stopHue);
+ }
+ public Random(float startHue, float stopHue, float speed){
+ this(startHue, stopHue);
+ this.speed = speed;
+ }
+ //endregion
+
+ //region Getters/Setters
+ public float getSpeed() { return speed; }
+ public float getStopHue() { return stopHue; }
+ public float getStartHue() { return startHue; }
+
+ /**
+ * Sets the speed of the animation
+ * @param speed from 0 to 1.
+ */
+ public void setSpeed(float speed) { this.speed = speed; }
+
+ /**
+ * Can be used to limit the colors shown in the rainbow. This is a hue so it reflects
+ * the position as an angle on a color wheel. Red is 0, Green is 120, Blue is 240, etc.
+ * @param startHue from 0 to 360.
+ */
+ public void setStartHue(float startHue) { this.startHue = startHue; }
+
+ /**
+ * Can be used to limit the colors shown in the rainbow. This is a hue so it reflects
+ * the position as an angle on a color wheel. Red is 0, Green is 120, Blue is 240, etc.
+ * @param stopHue from 0 to 360.
+ */
+ public void setStopHue(float stopHue) { this.stopHue = stopHue; }
+ public void setHues(float startHue, float stopHue) {
+ this.startHue = startHue;
+ this.stopHue = stopHue;
+ }
+ //endregion
+ @Override
+ protected void updateAnimationSpecificValuesOverI2C(I2cDeviceSynchSimple deviceClient)
+ {
+ deviceClient.write(layerHeight.register.address, GetByteArray(4, startHue));
+ deviceClient.write(layerHeight.register.address, GetByteArray(5, stopHue));
+ deviceClient.write(layerHeight.register.address, GetByteArray(6, speed));
+ }
+ }
+
+ public static class SineWave extends AnimationBase {
+ private Color secondaryColor = Color.BLUE;
+ private Color primaryColor = Color.RED;
+ private Direction direction = Direction.Forward;
+ private float offset = 0.5f;
+ private float speed = 0.5f;
+ private int period = 1000;
+
+ public SineWave(){ super(AnimationType.SINE_WAVE); }
+ public SineWave(Color primaryColor) {
+ this();
+ this.primaryColor = primaryColor;
+ }
+ public SineWave(Color primaryColor, Color secondaryColor) {
+ this(primaryColor);
+ this.secondaryColor = secondaryColor;
+ }
+ public SineWave(Color primaryColor, Color secondaryColor, int period) {
+ this(primaryColor, secondaryColor);
+ this.period = period;
+ }
+ public SineWave(Color primaryColor, Color secondaryColor, int period, float speed) {
+ this(primaryColor, secondaryColor, period);
+ this.speed = speed;
+ }
+ public SineWave(Color primaryColor, Color secondaryColor, int period, float speed, float offset) {
+ this(primaryColor, secondaryColor, period, speed);
+ this.offset = offset;
+ }
+ public SineWave(Color primaryColor, Color secondaryColor, int period, float speed, float offset, Direction direction) {
+ this(primaryColor, secondaryColor, period, speed, offset);
+ this.direction = direction;
+ }
+
+ /**
+ * set the period of the sine wave in milliseconds.
+ * @param period from 0 - 4,294,697,295. Larger is longer.
+ */
+ public void setPeriod(int period) { this.period = period; }
+
+ /**
+ * set the period of the sine wave in specified unit.
+ * @param duration duration of loop.
+ * @param timeUnit unit to use.
+ */
+ public void setPeriod(int duration, TimeUnit timeUnit){
+ this.period = Math.toIntExact(timeUnit.toMillis(duration));
+ }
+
+ /**
+ * Sets the speed of the animation
+ * @param speed from 0 to 1.
+ */
+ public void setSpeed(float speed) { this.speed = speed; }
+
+ /**
+ * Sets the vertical height of the sine wave, this will increase the contrast between the
+ * two shown colors.
+ * @param offset from 0 to 1.
+ */
+ public void setOffset(float offset) { this.offset = offset; }
+ public void setPrimaryColor(Color color) { primaryColor = color; }
+ public void setSecondaryColor(Color color) { secondaryColor = color; }
+ public void setDirection(Direction direction) { this.direction = direction; }
+ public void setPrimaryColor(int red, int green, int blue) { primaryColor = new Color((byte)red, (byte)green, (byte)blue); }
+ public void setSecondaryColor(int red, int green, int blue) { secondaryColor = new Color((byte)red, (byte)green, (byte)blue); }
+
+ public int getPeriod() { return period; }
+ public float getSpeed() { return speed; }
+ public float getOffset(){ return offset; }
+ public Direction getDirection() { return direction; }
+ public Color getPrimaryColor() { return primaryColor; }
+ public Color getSecondaryColor() { return secondaryColor; }
+
+ @Override
+ protected void updateAnimationSpecificValuesOverI2C(I2cDeviceSynchSimple deviceClient)
+ {
+ deviceClient.write(layerHeight.register.address, GetByteArray(4, primaryColor));
+ deviceClient.write(layerHeight.register.address, GetByteArray(5, secondaryColor));
+ deviceClient.write(layerHeight.register.address, GetByteArray(6, period));
+ deviceClient.write(layerHeight.register.address, GetByteArray(7, direction));
+ deviceClient.write(layerHeight.register.address, GetByteArray(8, offset));
+ deviceClient.write(layerHeight.register.address, GetByteArray(9, speed));
+ }
+ }
+
+ public static class SingleFill extends AnimationBase {
+ private Color[] colors = {Color.WHITE, Color.GREEN, Color.BLUE};
+ private int period = 750;
+ private float speed = 0.5f;
+ private int pixelLength = 3;
+ private Direction direction = Direction.Backward;
+ private SingleFillStyle style = SingleFillStyle.FillIn;
+
+ public enum SingleFillStyle{
+ FillIn(0),
+ FillOut(1);
+
+ final byte styleValue;
+ SingleFillStyle(int styleValue){ this.styleValue = (byte)styleValue; }
+ }
+
+ public SingleFill(){ super(AnimationType.SINGLE_FILL); }
+ public SingleFill(Color... colors) {
+ this();
+ this.colors = colors;
+ }
+
+ public int getPeriod() { return period; }
+ public float getSpeed() { return speed; }
+ public Color[] getColors() { return colors; }
+ public int getPixelLength() { return pixelLength; }
+ public Direction getDirection() { return direction; }
+ public SingleFillStyle getStyle() { return style; }
+
+ /**
+ * set the period of the single fill in milliseconds.
+ * @param period from 0 - 4,294,697,295. Larger is longer.
+ */
+ public void setPeriod(int period) { this.period = period; }
+
+ /**
+ * set the period of the single fill in specified unit.
+ * @param duration duration of loop.
+ * @param timeUnit unit to use.
+ */
+ public void setPeriod(int duration, TimeUnit timeUnit){
+ this.period = Math.toIntExact(timeUnit.toMillis(duration));
+ }
+
+ /**
+ * Sets the speed of the animation
+ * @param speed from 0 to 1.
+ */
+ public void setSpeed(float speed) { this.speed = speed; }
+ public void setColors(Color... colors) {
+ for(int i = 0; i < Math.min(colors.length,10); i++){
+ this.colors[i] = colors[i];
+ }
+ }
+ public void setPixelLength(int pixelLength) { this.pixelLength = pixelLength; }
+ public void setStyle(SingleFillStyle style) { this.style = style; }
+ public void setDirection(Direction direction) { this.direction = direction; }
+
+ @Override
+ protected void updateAnimationSpecificValuesOverI2C(I2cDeviceSynchSimple deviceClient)
+ {
+ deviceClient.write(layerHeight.register.address, GetByteArray(4, (byte)colors.length));
+ deviceClient.write(layerHeight.register.address, GetByteArray(5, colors));
+ deviceClient.write(layerHeight.register.address, GetByteArray(6, period));
+ deviceClient.write(layerHeight.register.address, GetByteArray(7, direction));
+ deviceClient.write(layerHeight.register.address, GetByteArray(8, (byte)pixelLength));
+ deviceClient.write(layerHeight.register.address, GetByteArray(9, speed));
+ deviceClient.write(layerHeight.register.address, GetByteArray(11, style.styleValue));
+ }
+ }
+
+ public static class Snakes extends AnimationBase {
+ private Color[] colors = {Color.RED, Color.WHITE, Color.BLUE};
+ private int snakeLength = 5;
+ private int spacingBetween = 2;
+ private int repeatAfter = 15;
+ private Color backgroundColor = Color.TRANSPARENT;
+ private float speed = 0.50f;
+ private Direction direction = Direction.Backward;
+
+ //region Constructors
+ public Snakes(){ super(AnimationType.SNAKES); }
+ public Snakes(Color... colors) {
+ this();
+ this.colors = colors;
+ }
+ public Snakes(int snakeLength, Color... colors){
+ this(colors);
+ this.snakeLength = (byte)snakeLength;
+ }
+ public Snakes(int snakeLength, int spacingBetween, Color... colors){
+ this(snakeLength, colors);
+ this.spacingBetween = (byte)spacingBetween;
+ }
+ public Snakes(int snakeLength, int spacingBetween, int repeatAfter, Color... colors){
+ this(snakeLength, spacingBetween, colors);
+ this.repeatAfter = (byte)repeatAfter;
+ }
+ public Snakes(int snakeLength, int spacingBetween, int repeatAfter, Color backgroundColor, Color... colors){
+ this(snakeLength, spacingBetween, repeatAfter, colors);
+ this.backgroundColor = backgroundColor;
+ }
+ public Snakes(int snakeLength, int spacingBetween, int repeatAfter, Color backgroundColor, float speed, Color... colors){
+ this(snakeLength, spacingBetween, repeatAfter, backgroundColor, colors);
+ this.speed = speed;
+ }
+ public Snakes(int snakeLength, int spacingBetween, int repeatAfter, Color backgroundColor, float speed, Direction direction, Color... colors){
+ this(snakeLength, spacingBetween, repeatAfter, backgroundColor, speed, colors);
+ this.direction = direction;
+ }
+ //endregion
+
+ //region Getters/Setters
+ public float getSpeed() { return speed; }
+ public Color[] getColors() { return colors; }
+ public int getSnakeLength() { return snakeLength; }
+ public int getRepeatAfter() { return repeatAfter; }
+ public Direction getDirection() { return direction; }
+ public int getSpacingBetween() { return spacingBetween; }
+ public Color getBackgroundColor() { return backgroundColor; }
+
+ /**
+ * Sets the speed of the animation.
+ * @param speed from 0 to 1.
+ */
+ public void setSpeed(float speed) { this.speed = speed; }
+ public void setColors(Color... colors) { this.colors = colors;}
+ public void setDirection(Direction direction) { this.direction = direction; }
+
+ /**
+ * Length in pixels of each snake.
+ * @param snakeLength from 0 to 255.
+ */
+ public void setSnakeLength(int snakeLength) { this.snakeLength = snakeLength; }
+
+ /**
+ * The number of pixels between the last snake and the first snake of a repeating animation.
+ * @param repeatAfter from 0 to 255.
+ */
+ public void setRepeatAfter(int repeatAfter) { this.repeatAfter = repeatAfter; }
+
+ /**
+ * The number of pixels between consecutive snakes.
+ * @param spacingBetween from 0 to 255.
+ */
+ public void setSpacingBetween(int spacingBetween) { this.spacingBetween = spacingBetween; }
+ public void setBackgroundColor(Color backgroundColor) { this.backgroundColor = backgroundColor; }
+ //endregion
+ @Override
+ protected void updateAnimationSpecificValuesOverI2C(I2cDeviceSynchSimple deviceClient)
+ {
+ deviceClient.write(layerHeight.register.address, GetByteArray(4, (byte)colors.length));
+ deviceClient.write(layerHeight.register.address, GetByteArray(5, colors));
+ deviceClient.write(layerHeight.register.address, GetByteArray(6, speed));
+ deviceClient.write(layerHeight.register.address, GetByteArray(7, direction));
+ deviceClient.write(layerHeight.register.address, GetByteArray(8, (byte)spacingBetween));
+ deviceClient.write(layerHeight.register.address, GetByteArray(9, (byte)repeatAfter));
+ deviceClient.write(layerHeight.register.address, GetByteArray(10, backgroundColor));
+ deviceClient.write(layerHeight.register.address, GetByteArray(11, (byte)snakeLength));
+ }
+ }
+
+ public static class Solid extends AnimationBase {
+ private Color primaryColor = Color.RED;
+
+ public Solid(){super(AnimationType.SOLID);}
+ public Solid(Color primaryColor) {
+ super(AnimationType.SOLID);
+ this.primaryColor = primaryColor;
+ }
+ public Solid(Color primaryColor, int brightness) {
+ super(AnimationType.SOLID, brightness);
+ this.primaryColor = primaryColor;
+ }
+ public Solid(Color primaryColor, int startIndex, int stopIndex){
+ super(AnimationType.SOLID, startIndex, stopIndex);
+ this.primaryColor = primaryColor;
+ }
+ public Solid(Color primaryColor, int brightness, int startIndex, int stopIndex){
+ super(AnimationType.SOLID, brightness, startIndex, stopIndex);
+ this.primaryColor = primaryColor;
+ }
+
+ public void setPrimaryColor(int red, int green, int blue) { primaryColor = new Color((byte)red, (byte)green, (byte)blue); }
+ public void setPrimaryColor(Color color) { primaryColor = color; }
+ public Color getPrimaryColor() { return primaryColor; }
+
+ @Override
+ protected void updateAnimationSpecificValuesOverI2C(I2cDeviceSynchSimple deviceClient)
+ {
+ deviceClient.write(layerHeight.register.address, GetByteArray(4, primaryColor));
+ }
+ }
+
+ public static class Sparkle extends AnimationBase {
+ private Color primaryColor = Color.WHITE;
+ private Color secondaryColor = Color.TRANSPARENT;
+ private int sparkleProbability = 16;
+ private int period = 100;
+
+ //region Constructors
+ public Sparkle() { super(AnimationType.SPARKLE); }
+ public Sparkle(Color primaryColor){
+ this();
+ this.primaryColor = primaryColor;
+ }
+ public Sparkle(Color primaryColor, Color secondaryColor){
+ this(primaryColor);
+ this.secondaryColor = secondaryColor;
+ }
+ public Sparkle(Color primaryColor, Color secondaryColor, int period){
+ this(primaryColor, secondaryColor);
+ this.period = period;
+ }
+ //endregion
+
+ //region Getters/Setters
+ public int getPeriod() { return period; }
+ public Color getPrimaryColor() { return primaryColor; }
+ public Color getSecondaryColor() { return secondaryColor; }
+ public int getSparkleProbability() { return sparkleProbability; }
+
+ /**
+ * sets the time between updating the sparkles.
+ * @param period from 0 - 4,294,697,295. Larger is longer.
+ */
+ public void setPeriod(int period) { this.period = period; }
+ public void setPrimaryColor(Color primaryColor) { this.primaryColor = primaryColor; }
+ public void setSecondaryColor(Color secondaryColor) { this.secondaryColor = secondaryColor; }
+
+ /**
+ * Sets the probability/density of sparkles. Lower is denser. Default is 16.
+ * @param sparkleProbability from 0 to 255.
+ */
+ public void setSparkleProbability(int sparkleProbability) { this.sparkleProbability = sparkleProbability; };
+ //endregion
+ @Override
+ protected void updateAnimationSpecificValuesOverI2C(I2cDeviceSynchSimple deviceClient)
+ {
+ deviceClient.write(layerHeight.register.address, GetByteArray(4, primaryColor));
+ deviceClient.write(layerHeight.register.address, GetByteArray(5, secondaryColor));
+ deviceClient.write(layerHeight.register.address, GetByteArray(6, period));
+ deviceClient.write(layerHeight.register.address, GetByteArray(7, (byte)sparkleProbability));
+ }
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/Auton.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/Auton.java
new file mode 100755
index 0000000..535128a
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/Auton.java
@@ -0,0 +1,860 @@
+package org.firstinspires.ftc.teamcode.auton;
+
+import com.bylazar.configurables.annotations.Configurable;
+import com.pedropathing.follower.Follower;
+import com.pedropathing.geometry.BezierCurve;
+import com.pedropathing.geometry.BezierLine;
+import com.pedropathing.geometry.Pose;
+import com.pedropathing.paths.PathChain;
+import com.pedropathing.util.Timer;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+
+import org.firstinspires.ftc.teamcode.pedroPathing.Constants;
+import org.firstinspires.ftc.teamcode.Hware.hwMap;
+
+import com.qualcomm.hardware.lynx.LynxModule;
+import java.util.List;
+
+public class Auton {
+
+ // ==================================================================================
+ // BLUE ALLIANCE
+ // ==================================================================================
+
+ @Autonomous(name = "Pedro Blue Far", group = "Pedro", preselectTeleOp = "Blue Final")
+ public static class BlueFar extends BaseAuto {
+ public BlueFar() {
+ super(1, false, true); // Alliance 1 (Blue), isNet=false, useTimer=true
+ }
+
+ @Override
+ public void buildPaths() {
+ // Start Pose
+ startPose = new Pose(59, 9, Math.toRadians(90));
+ follower.setStartingPose(startPose);
+
+ // Poses
+ Pose scorePosePreload = new Pose(59, 15, Math.toRadians(118));
+ Pose scorePoseGeneric = new Pose(59, 15, Math.toRadians(118));
+ Pose set1Inter = new Pose(44, 36, Math.toRadians(180));
+ Pose set1Pick = new Pose(17, 36, Math.toRadians(180));
+ Pose set2Inter = new Pose(41, 60, Math.toRadians(180));
+ Pose set2Pick = new Pose(16, 63, Math.toRadians(180));
+ Pose set2Control1 = new Pose(17.711, 56.837);
+ Pose set2Control2 = new Pose(25.306, 62.677);
+ Pose scoreSet1Control = new Pose(36.621, 19.900);
+
+ // Corner Specific Poses
+ Pose cornerInter = new Pose(9.444, 24.561, Math.toRadians(-90));
+ Pose cornerInterControl = new Pose(37.157, 26.830);
+ Pose cornerArtifact1 = new Pose(12.598, 18.008, Math.toRadians(-135));
+ Pose cornerArtifact1Control = new Pose(11.997, 24.364);
+ Pose cornerArtifact2 = new Pose(10.551, 13.380, Math.toRadians(-160));
+ Pose cornerArtifact3 = new Pose(9.712, 8.993, Math.toRadians(-160));
+ Pose parkPose = new Pose(54.614, 19.545, Math.toRadians(135));
+
+ // --- PATH DEFINITIONS ---
+ scorePreloadPath = follower.pathBuilder()
+ .addPath(new BezierLine(startPose, scorePosePreload))
+ .setLinearHeadingInterpolation(startPose.getHeading(), scorePosePreload.getHeading())
+ .build();
+
+ grabSet1Path = follower.pathBuilder()
+ .addPath(new BezierLine(scorePosePreload, set1Inter))
+ .setLinearHeadingInterpolation(scorePosePreload.getHeading(), Math.toRadians(180))
+ .addPath(new BezierLine(set1Inter, set1Pick))
+ .setConstantHeadingInterpolation(Math.toRadians(180))
+ .build();
+
+ scoreSet1Path = follower.pathBuilder()
+ .addPath(new BezierCurve(set1Pick, scoreSet1Control, scorePoseGeneric))
+ .setLinearHeadingInterpolation(Math.toRadians(180), scorePoseGeneric.getHeading())
+ .build();
+
+ grabSet2Path = follower.pathBuilder()
+ .addPath(new BezierLine(scorePoseGeneric, set2Inter))
+ .setLinearHeadingInterpolation(scorePoseGeneric.getHeading(), Math.toRadians(180))
+ .addPath(new BezierCurve(set2Inter, set2Control1, set2Control2, set2Pick))
+ .setConstantHeadingInterpolation(Math.toRadians(180))
+ .build();
+
+ scoreSet2Path = follower.pathBuilder()
+ .addPath(new BezierLine(set2Pick, scorePoseGeneric))
+ .setLinearHeadingInterpolation(Math.toRadians(180), scorePoseGeneric.getHeading())
+ .build();
+
+ // Corner Sequence
+ grabEndGamePath = follower.pathBuilder()
+ .addPath(new BezierCurve(scorePoseGeneric, cornerInterControl, cornerInter))
+ .setLinearHeadingInterpolation(scorePoseGeneric.getHeading(), Math.toRadians(-90))
+ .addPath(new BezierCurve(cornerInter, cornerArtifact1Control, cornerArtifact1))
+ .setLinearHeadingInterpolation(Math.toRadians(-90), Math.toRadians(-135))
+ .addPath(new BezierLine(cornerArtifact1, cornerArtifact2))
+ .setLinearHeadingInterpolation(Math.toRadians(-135), Math.toRadians(-160))
+ .addPath(new BezierLine(cornerArtifact2, cornerArtifact3))
+ .setConstantHeadingInterpolation(Math.toRadians(-160))
+ .build();
+
+ scoreEndGamePath = follower.pathBuilder()
+ .addPath(new BezierLine(cornerArtifact3, scorePoseGeneric))
+ .setLinearHeadingInterpolation(Math.toRadians(-160), scorePoseGeneric.getHeading())
+ .build();
+
+ parkPath = follower.pathBuilder()
+ .addPath(new BezierLine(scorePoseGeneric, parkPose))
+ .setLinearHeadingInterpolation(scorePoseGeneric.getHeading(), parkPose.getHeading())
+ .build();
+ }
+ }
+
+ @Autonomous(name = "Pedro Blue Net", group = "Pedro", preselectTeleOp = "Blue Final")
+ public static class BlueNet extends BaseAuto {
+ public BlueNet() {
+ super(1, true, true); // Alliance 1 (Blue), isNet=true, useTimer=true
+ }
+
+ @Override
+ public void buildPaths() {
+ startPose = new Pose(31.5, 137, Math.toRadians(180));
+ follower.setStartingPose(startPose);
+
+ // Waypoints
+ Pose preloadScorePose = new Pose(54, 90, Math.toRadians(144));
+ Pose set2DrivePose = new Pose(39, 60, Math.toRadians(180));
+ Pose set2PickPose = new Pose(14, 60, Math.toRadians(180)); // tangential
+ Pose set2ScorePose = new Pose(54, 90, Math.toRadians(144));
+ Pose gate1PickPose = new Pose(10.652, 61.079, Math.toRadians(140));
+ Pose gate1ScorePose = new Pose(54, 90, Math.toRadians(144));
+ Pose gate2PickPose = new Pose(10.652, 61.079, Math.toRadians(140));
+ Pose gate2ScorePose = new Pose(54, 90, Math.toRadians(144));
+ Pose set3DrivePose = new Pose(38.515, 47.156, Math.toRadians(220));
+ Pose set3PickPose = new Pose(16.419, 35.945, Math.toRadians(185));
+ Pose set3ScorePose = new Pose(54, 90, Math.toRadians(144));
+ Pose set1DrivePose = new Pose(39, 84, Math.toRadians(180));
+ Pose set1PickPose = new Pose(29.689, 106.279, Math.toRadians(144));
+ Pose set1ScorePose = new Pose(44, 100, Math.toRadians(148));
+ Pose parkTargetPose = new Pose(36.219, 93.170, Math.toRadians(148));
+
+ // Controls
+ Pose preloadControl = new Pose(27.400, 115.541);
+ Pose set2ScoreControl = new Pose(50.386, 78.923);
+ Pose gate1PickControl = new Pose(40.119, 55.595);
+ Pose gate1ScoreControl = new Pose(41.918, 62.187);
+ Pose gate2PickControl = new Pose(42.081, 61.914);
+ Pose gate2ScoreControl = new Pose(42.318, 61.514);
+ Pose set3PickControl = new Pose(29.622, 37.842);
+ Pose set3ScoreControl = new Pose(9.594, 52.541);
+ Pose set1PickControl = new Pose(14.863, 77.692);
+
+ scorePreloadPath = follower.pathBuilder()
+ .addPath(new BezierCurve(startPose, preloadControl, preloadScorePose))
+ .setLinearHeadingInterpolation(startPose.getHeading(), preloadScorePose.getHeading())
+ .build();
+
+ driveToSet2Path = follower.pathBuilder()
+ .addPath(new BezierLine(preloadScorePose, set2DrivePose))
+ .setLinearHeadingInterpolation(preloadScorePose.getHeading(), set2DrivePose.getHeading())
+ .build();
+
+ grabSet2Path = follower.pathBuilder()
+ .addPath(new BezierLine(set2DrivePose, set2PickPose))
+ .setTangentHeadingInterpolation()
+ .build();
+
+ scoreSet2Path = follower.pathBuilder()
+ .addPath(new BezierCurve(set2PickPose, set2ScoreControl, set2ScorePose))
+ .setLinearHeadingInterpolation(set2PickPose.getHeading(), set2ScorePose.getHeading())
+ .build();
+
+ grabGate1Path = follower.pathBuilder()
+ .addPath(new BezierCurve(set2ScorePose, gate1PickControl, gate1PickPose))
+ .setLinearHeadingInterpolation(set2ScorePose.getHeading(), gate1PickPose.getHeading())
+ .build();
+
+ scoreGate1Path = follower.pathBuilder()
+ .addPath(new BezierCurve(gate1PickPose, gate1ScoreControl, gate1ScorePose))
+ .setLinearHeadingInterpolation(gate1PickPose.getHeading(), gate1ScorePose.getHeading())
+ .build();
+
+ grabGate2Path = follower.pathBuilder()
+ .addPath(new BezierCurve(gate1ScorePose, gate2PickControl, gate2PickPose))
+ .setLinearHeadingInterpolation(gate1ScorePose.getHeading(), gate2PickPose.getHeading())
+ .build();
+
+ scoreGate2Path = follower.pathBuilder()
+ .addPath(new BezierCurve(gate2PickPose, gate2ScoreControl, gate2ScorePose))
+ .setLinearHeadingInterpolation(gate2PickPose.getHeading(), gate2ScorePose.getHeading())
+ .build();
+
+ driveToSet3Path = follower.pathBuilder()
+ .addPath(new BezierLine(gate2ScorePose, set3DrivePose))
+ .setLinearHeadingInterpolation(gate2ScorePose.getHeading(), set3DrivePose.getHeading())
+ .build();
+
+ grabSet3Path = follower.pathBuilder()
+ .addPath(new BezierCurve(set3DrivePose, set3PickControl, set3PickPose))
+ .setLinearHeadingInterpolation(set3DrivePose.getHeading(), set3PickPose.getHeading())
+ .build();
+
+ scoreSet3Path = follower.pathBuilder()
+ .addPath(new BezierCurve(set3PickPose, set3ScoreControl, set3ScorePose))
+ .setLinearHeadingInterpolation(set3PickPose.getHeading(), set3ScorePose.getHeading())
+ .build();
+
+ driveToSet1Path = follower.pathBuilder()
+ .addPath(new BezierLine(set3ScorePose, set1DrivePose))
+ .setLinearHeadingInterpolation(set3ScorePose.getHeading(), set1DrivePose.getHeading())
+ .build();
+
+ grabSet1Path = follower.pathBuilder()
+ .addPath(new BezierCurve(set1DrivePose, set1PickControl, set1PickPose))
+ .setLinearHeadingInterpolation(set1DrivePose.getHeading(), set1PickPose.getHeading())
+ .build();
+
+ scoreSet1Path = follower.pathBuilder()
+ .addPath(new BezierLine(set1PickPose, set1ScorePose))
+ .setLinearHeadingInterpolation(set1PickPose.getHeading(), set1ScorePose.getHeading())
+ .build();
+
+ parkPath = follower.pathBuilder()
+ .addPath(new BezierLine(set1ScorePose, parkTargetPose))
+ .setConstantHeadingInterpolation(set1ScorePose.getHeading())
+ .build();
+ }
+ }
+
+ // ==================================================================================
+ // RED ALLIANCE
+ // ==================================================================================
+
+ @Autonomous(name = "Pedro Red Far", group = "Pedro", preselectTeleOp = "Red Final")
+ public static class RedFar extends BaseAuto {
+ public RedFar() {
+ super(2, false, true); // Alliance 2 (Red), isNet=false, useTimer=true
+ }
+
+ @Override
+ public void buildPaths() {
+ startPose = new Pose(85, 9, Math.toRadians(90));
+ follower.setStartingPose(startPose);
+
+ Pose scorePosePreload = new Pose(85, 15, Math.toRadians(62));
+ Pose scorePoseGeneric = new Pose(85, 15, Math.toRadians(62));
+ Pose set1Inter = new Pose(100, 36, Math.toRadians(0));
+ Pose set1Pick = new Pose(127, 36, Math.toRadians(0));
+ Pose set2Inter = new Pose(103, 60, Math.toRadians(0));
+ Pose set2Pick = new Pose(128, 63, Math.toRadians(0));
+ Pose set2Control1 = new Pose(126.289, 56.837);
+ Pose set2Control2 = new Pose(118.694, 62.677);
+ Pose scoreSet1Control = new Pose(107.379, 19.900);
+
+ // Corner Poses
+ Pose cornerInter = new Pose(134.556, 24.561, Math.toRadians(270));
+ Pose cornerInterControl = new Pose(106.843, 26.830);
+ Pose cornerArtifact1 = new Pose(131.402, 18.008, Math.toRadians(315));
+ Pose cornerArtifact1Control = new Pose(132.003, 24.364);
+ Pose cornerArtifact2 = new Pose(133.449, 13.380, Math.toRadians(340));
+ Pose cornerArtifact3 = new Pose(134.288, 8.993, Math.toRadians(340));
+ Pose parkPose = new Pose(89.386, 19.545, Math.toRadians(45));
+
+ scorePreloadPath = follower.pathBuilder()
+ .addPath(new BezierLine(startPose, scorePosePreload))
+ .setLinearHeadingInterpolation(startPose.getHeading(), Math.toRadians(62))
+ .build();
+
+ grabSet1Path = follower.pathBuilder()
+ .addPath(new BezierLine(scorePosePreload, set1Inter))
+ .setLinearHeadingInterpolation(Math.toRadians(62), Math.toRadians(0))
+ .addPath(new BezierLine(set1Inter, set1Pick))
+ .setConstantHeadingInterpolation(Math.toRadians(0))
+ .build();
+
+ scoreSet1Path = follower.pathBuilder()
+ .addPath(new BezierCurve(set1Pick, scoreSet1Control, scorePoseGeneric))
+ .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(62))
+ .build();
+
+ grabSet2Path = follower.pathBuilder()
+ .addPath(new BezierLine(scorePoseGeneric, set2Inter))
+ .setLinearHeadingInterpolation(Math.toRadians(62), Math.toRadians(0))
+ .addPath(new BezierCurve(set2Inter, set2Control1, set2Control2, set2Pick))
+ .setConstantHeadingInterpolation(Math.toRadians(0))
+ .build();
+
+ scoreSet2Path = follower.pathBuilder()
+ .addPath(new BezierLine(set2Pick, scorePoseGeneric))
+ .setLinearHeadingInterpolation(Math.toRadians(0), Math.toRadians(62))
+ .build();
+
+ // Corner Sequence
+ grabEndGamePath = follower.pathBuilder()
+ .addPath(new BezierCurve(scorePoseGeneric, cornerInterControl, cornerInter))
+ .setLinearHeadingInterpolation(Math.toRadians(62), Math.toRadians(270))
+ .addPath(new BezierCurve(cornerInter, cornerArtifact1Control, cornerArtifact1))
+ .setLinearHeadingInterpolation(Math.toRadians(270), Math.toRadians(315))
+ .addPath(new BezierLine(cornerArtifact1, cornerArtifact2))
+ .setLinearHeadingInterpolation(Math.toRadians(315), Math.toRadians(340))
+ .addPath(new BezierLine(cornerArtifact2, cornerArtifact3))
+ .setConstantHeadingInterpolation(Math.toRadians(340))
+ .build();
+
+ scoreEndGamePath = follower.pathBuilder()
+ .addPath(new BezierLine(cornerArtifact3, scorePoseGeneric))
+ .setLinearHeadingInterpolation(Math.toRadians(340), Math.toRadians(62))
+ .build();
+
+ parkPath = follower.pathBuilder()
+ .addPath(new BezierLine(scorePoseGeneric, parkPose))
+ .setLinearHeadingInterpolation(Math.toRadians(62), Math.toRadians(45))
+ .build();
+ }
+ }
+
+ @Autonomous(name = "Pedro Red Net", group = "Pedro", preselectTeleOp = "Red Final")
+ public static class RedNet extends BaseAuto {
+ public RedNet() {
+ super(2, true, true); // Alliance 2 (Red), isNet=true, useTimer=true
+ }
+
+ @Override
+ public void buildPaths() {
+ startPose = new Pose(112.5, 137, Math.toRadians(0));
+ follower.setStartingPose(startPose);
+
+ // Waypoints
+ Pose preloadScorePose = new Pose(90, 90, Math.toRadians(36));
+ Pose set2DrivePose = new Pose(105, 60, Math.toRadians(0));
+ Pose set2PickPose = new Pose(130, 60, Math.toRadians(0)); // tangential
+ Pose set2ScorePose = new Pose(90, 90, Math.toRadians(36));
+ Pose gate1PickPose = new Pose(133.348, 61.079, Math.toRadians(40));
+ Pose gate1ScorePose = new Pose(90, 90, Math.toRadians(36));
+ Pose gate2PickPose = new Pose(133.348, 61.079, Math.toRadians(40));
+ Pose gate2ScorePose = new Pose(90, 90, Math.toRadians(36));
+ Pose set3DrivePose = new Pose(105.485, 47.156, Math.toRadians(-40));
+ Pose set3PickPose = new Pose(127.581, 35.945, Math.toRadians(-5));
+ Pose set3ScorePose = new Pose(90, 90, Math.toRadians(36));
+ Pose set1DrivePose = new Pose(105, 84, Math.toRadians(0));
+ Pose set1PickPose = new Pose(114.311, 106.279, Math.toRadians(36));
+ Pose set1ScorePose = new Pose(100, 100, Math.toRadians(32));
+ Pose parkTargetPose = new Pose(107.781, 93.170, Math.toRadians(32));
+
+ // Controls
+ Pose preloadControl = new Pose(116.600, 115.541);
+ Pose set2ScoreControl = new Pose(93.614, 78.923);
+ Pose gate1PickControl = new Pose(103.881, 55.595);
+ Pose gate1ScoreControl = new Pose(102.082, 62.187);
+ Pose gate2PickControl = new Pose(101.919, 61.914);
+ Pose gate2ScoreControl = new Pose(101.682, 61.514);
+ Pose set3PickControl = new Pose(114.378, 37.842);
+ Pose set3ScoreControl = new Pose(134.406, 52.541);
+ Pose set1PickControl = new Pose(129.137, 77.692);
+
+ scorePreloadPath = follower.pathBuilder()
+ .addPath(new BezierCurve(startPose, preloadControl, preloadScorePose))
+ .setLinearHeadingInterpolation(startPose.getHeading(), preloadScorePose.getHeading())
+ .build();
+
+ driveToSet2Path = follower.pathBuilder()
+ .addPath(new BezierLine(preloadScorePose, set2DrivePose))
+ .setLinearHeadingInterpolation(preloadScorePose.getHeading(), set2DrivePose.getHeading())
+ .build();
+
+ grabSet2Path = follower.pathBuilder()
+ .addPath(new BezierLine(set2DrivePose, set2PickPose))
+ .setTangentHeadingInterpolation()
+ .build();
+
+ scoreSet2Path = follower.pathBuilder()
+ .addPath(new BezierCurve(set2PickPose, set2ScoreControl, set2ScorePose))
+ .setLinearHeadingInterpolation(set2PickPose.getHeading(), set2ScorePose.getHeading())
+ .build();
+
+ grabGate1Path = follower.pathBuilder()
+ .addPath(new BezierCurve(set2ScorePose, gate1PickControl, gate1PickPose))
+ .setLinearHeadingInterpolation(set2ScorePose.getHeading(), gate1PickPose.getHeading())
+ .build();
+
+ scoreGate1Path = follower.pathBuilder()
+ .addPath(new BezierCurve(gate1PickPose, gate1ScoreControl, gate1ScorePose))
+ .setLinearHeadingInterpolation(gate1PickPose.getHeading(), gate1ScorePose.getHeading())
+ .build();
+
+ grabGate2Path = follower.pathBuilder()
+ .addPath(new BezierCurve(gate1ScorePose, gate2PickControl, gate2PickPose))
+ .setLinearHeadingInterpolation(gate1ScorePose.getHeading(), gate2PickPose.getHeading())
+ .build();
+
+ scoreGate2Path = follower.pathBuilder()
+ .addPath(new BezierCurve(gate2PickPose, gate2ScoreControl, gate2ScorePose))
+ .setLinearHeadingInterpolation(gate2PickPose.getHeading(), gate2ScorePose.getHeading())
+ .build();
+
+ driveToSet3Path = follower.pathBuilder()
+ .addPath(new BezierLine(gate2ScorePose, set3DrivePose))
+ .setLinearHeadingInterpolation(gate2ScorePose.getHeading(), set3DrivePose.getHeading())
+ .build();
+
+ grabSet3Path = follower.pathBuilder()
+ .addPath(new BezierCurve(set3DrivePose, set3PickControl, set3PickPose))
+ .setLinearHeadingInterpolation(set3DrivePose.getHeading(), set3PickPose.getHeading())
+ .build();
+
+ scoreSet3Path = follower.pathBuilder()
+ .addPath(new BezierCurve(set3PickPose, set3ScoreControl, set3ScorePose))
+ .setLinearHeadingInterpolation(set3PickPose.getHeading(), set3ScorePose.getHeading())
+ .build();
+
+ driveToSet1Path = follower.pathBuilder()
+ .addPath(new BezierLine(set3ScorePose, set1DrivePose))
+ .setLinearHeadingInterpolation(set3ScorePose.getHeading(), set1DrivePose.getHeading())
+ .build();
+
+ grabSet1Path = follower.pathBuilder()
+ .addPath(new BezierCurve(set1DrivePose, set1PickControl, set1PickPose))
+ .setLinearHeadingInterpolation(set1DrivePose.getHeading(), set1PickPose.getHeading())
+ .build();
+
+ scoreSet1Path = follower.pathBuilder()
+ .addPath(new BezierLine(set1PickPose, set1ScorePose))
+ .setLinearHeadingInterpolation(set1PickPose.getHeading(), set1ScorePose.getHeading())
+ .build();
+
+ parkPath = follower.pathBuilder()
+ .addPath(new BezierLine(set1ScorePose, parkTargetPose))
+ .setConstantHeadingInterpolation(set1ScorePose.getHeading())
+ .build();
+ }
+ }
+
+ // ==================================================================================
+ // BASE CLASS
+ // ==================================================================================
+
+ @Configurable
+ public static abstract class BaseAuto extends LinearOpMode {
+ protected Follower follower;
+ protected List allHubs;
+ protected Timer pathTimer, actionTimer, opmodeTimer;
+ protected pedroStateMachine systems;
+
+ protected final int allianceTarget;
+ protected final boolean isNet;
+ protected final boolean useTimer;
+
+ public static double timerTimeout = 4;
+ public static double gateWaitTime = 3.0;
+
+ protected Pose startPose;
+
+ // Shared/Common Path Chains
+ protected PathChain scorePreloadPath;
+ protected PathChain grabSet1Path, scoreSet1Path;
+ protected PathChain grabSet2Path, scoreSet2Path;
+ protected PathChain parkPath;
+
+ // Far Specific Path Chains
+ protected PathChain grabEndGamePath, scoreEndGamePath;
+
+ // Net Specific Path Chains
+ protected PathChain driveToSet2Path;
+ protected PathChain grabGate1Path, scoreGate1Path;
+ protected PathChain grabGate2Path, scoreGate2Path;
+ protected PathChain driveToSet3Path, grabSet3Path, scoreSet3Path;
+ protected PathChain driveToSet1Path;
+
+ public enum PathState {
+ DRIVE_TO_SCORE_PRELOAD, SCORE_PRELOAD,
+ DRIVE_TO_GRAB_SET_1, GRAB_SET_1,
+ DRIVE_TO_SCORE_SET_1, SCORE_SET_1,
+ DRIVE_TO_GRAB_SET_2, GRAB_SET_2,
+ DRIVE_TO_SCORE_SET_2, SCORE_SET_2,
+ DRIVE_TO_ENDGAME_COLLECTION, // Corner sequence
+ DO_ENDGAME_COLLECTION,
+ DRIVE_TO_SCORE_ENDGAME,
+ SCORE_ENDGAME,
+ DRIVE_TO_PARK, DONE,
+
+ // Net specific
+ GRAB_GATE_1, WAIT_GATE_1, SCORE_GATE_1,
+ GRAB_GATE_2, WAIT_GATE_2, SCORE_GATE_2,
+ DRIVE_TO_GRAB_SET_3, GRAB_SET_3,
+ DRIVE_TO_SCORE_SET_3, SCORE_SET_3
+ }
+
+ protected PathState pathState;
+
+ public BaseAuto(int allianceTarget, boolean isNet, boolean useTimer) {
+ this.allianceTarget = allianceTarget;
+ this.isNet = isNet;
+ this.useTimer = useTimer;
+ }
+
+ public abstract void buildPaths();
+
+ public void setPathState(PathState newPathState) {
+ pathState = newPathState;
+ pathTimer.resetTimer();
+ actionTimer.resetTimer();
+ }
+
+ @Override
+ public void runOpMode() throws InterruptedException {
+ pathTimer = new Timer();
+ actionTimer = new Timer();
+ opmodeTimer = new Timer();
+ follower = Constants.createFollower(hardwareMap);
+
+ allHubs = hardwareMap.getAll(LynxModule.class);
+ for (LynxModule hub : allHubs) {
+ hub.setBulkCachingMode(LynxModule.BulkCachingMode.MANUAL);
+ }
+
+ // Subclass sets startPose and paths
+ buildPaths();
+
+ hwMap.IntakeHwMap intakeHw = new hwMap.IntakeHwMap(hardwareMap);
+ hwMap.TransferHwMap transferHw = new hwMap.TransferHwMap(hardwareMap);
+ hwMap.TurretHwMap turretHw = new hwMap.TurretHwMap(hardwareMap);
+
+ systems = new pedroStateMachine(intakeHw, transferHw, turretHw);
+ systems.setAlliance(allianceTarget);
+
+ pathState = PathState.DRIVE_TO_SCORE_PRELOAD;
+
+ while (!isStarted() && !isStopRequested()) {
+
+ telemetry.addLine("=== Unified Auton Ready ===");
+ telemetry.addData("Alliance", allianceTarget == 1 ? "Blue" : "Red");
+ telemetry.addData("Type", isNet ? "NET (Gate)" : "FAR (Corner)");
+ telemetry.addData("Safety Timer", useTimer ? "ON (1.5s)" : "OFF");
+ telemetry.update();
+ }
+
+ waitForStart();
+ systems.update(allianceTarget, follower.getPose());
+ opmodeTimer.resetTimer();
+ setPathState(PathState.DRIVE_TO_SCORE_PRELOAD);
+
+ while (opModeIsActive()) {
+
+ for (LynxModule hub : allHubs) {
+ hub.clearBulkCache();
+ }
+ follower.update();
+ statePathUpdate();
+ systems.update(allianceTarget, follower.getPose());
+
+ telemetry.addData("Path State", pathState);
+ telemetry.addData("System State", systems.getCurrentState());
+ telemetry.update();
+ }
+ }
+
+ public void statePathUpdate() {
+ if (isNet) {
+ statePathUpdateNet();
+ } else {
+ statePathUpdateFar();
+ }
+ }
+
+ public void statePathUpdateFar() {
+ switch(pathState) {
+ // --- PRELOAD ---
+ case DRIVE_TO_SCORE_PRELOAD:
+ follower.followPath(scorePreloadPath, true);
+ systems.initiateTurretSpinUp();
+ setPathState(PathState.SCORE_PRELOAD);
+ break;
+
+ case SCORE_PRELOAD:
+ if (!follower.isBusy()) {
+ systems.setAutonState(pedroStateMachine.AutonState.SCORING);
+ if (checkScoringCondition()) {
+ setPathState(PathState.DRIVE_TO_GRAB_SET_1);
+ }
+ }
+ break;
+
+ // --- SET 1 ---
+ case DRIVE_TO_GRAB_SET_1:
+ follower.followPath(grabSet1Path, true);
+ systems.setAutonState(pedroStateMachine.AutonState.INTAKING);
+ setPathState(PathState.GRAB_SET_1);
+ break;
+
+ case GRAB_SET_1:
+ if (!follower.isBusy()) {
+ systems.setAutonState(pedroStateMachine.AutonState.IDLE);
+ setPathState(PathState.DRIVE_TO_SCORE_SET_1);
+ }
+ break;
+
+ case DRIVE_TO_SCORE_SET_1:
+ follower.followPath(scoreSet1Path, true);
+ systems.initiateTurretSpinUp();
+ setPathState(PathState.SCORE_SET_1);
+ break;
+
+ case SCORE_SET_1:
+ if (!follower.isBusy()) {
+ systems.setAutonState(pedroStateMachine.AutonState.SCORING);
+ if (checkScoringCondition()) {
+ setPathState(PathState.DRIVE_TO_GRAB_SET_2);
+ }
+ }
+ break;
+
+ // --- SET 2 ---
+ case DRIVE_TO_GRAB_SET_2:
+ follower.followPath(grabSet2Path, true);
+ systems.setAutonState(pedroStateMachine.AutonState.INTAKING);
+ setPathState(PathState.GRAB_SET_2);
+ break;
+
+ case GRAB_SET_2:
+ if (!follower.isBusy()) {
+ systems.setAutonState(pedroStateMachine.AutonState.IDLE);
+ setPathState(PathState.DRIVE_TO_SCORE_SET_2);
+ }
+ break;
+
+ case DRIVE_TO_SCORE_SET_2:
+ follower.followPath(scoreSet2Path, true);
+ systems.initiateTurretSpinUp();
+ setPathState(PathState.SCORE_SET_2);
+ break;
+
+ case SCORE_SET_2:
+ if (!follower.isBusy()) {
+ systems.setAutonState(pedroStateMachine.AutonState.SCORING);
+ if (checkScoringCondition()) {
+ setPathState(PathState.DRIVE_TO_PARK);
+ }
+ }
+ break;
+
+ // --- END GAME (Corner) ---
+ case DRIVE_TO_ENDGAME_COLLECTION:
+ follower.followPath(grabEndGamePath, true);
+ systems.setAutonState(pedroStateMachine.AutonState.INTAKING);
+ setPathState(PathState.DO_ENDGAME_COLLECTION);
+ break;
+
+ case DO_ENDGAME_COLLECTION:
+ if (!follower.isBusy()) {
+ systems.setAutonState(pedroStateMachine.AutonState.IDLE);
+ setPathState(PathState.DRIVE_TO_SCORE_ENDGAME);
+ }
+ break;
+
+ case DRIVE_TO_SCORE_ENDGAME:
+ follower.followPath(scoreEndGamePath, true);
+ systems.initiateTurretSpinUp();
+ setPathState(PathState.SCORE_ENDGAME);
+ break;
+
+ case SCORE_ENDGAME:
+ if (!follower.isBusy()) {
+ systems.setAutonState(pedroStateMachine.AutonState.SCORING);
+ if (checkScoringCondition()) {
+ setPathState(PathState.DRIVE_TO_PARK);
+ }
+ }
+ break;
+
+ // --- PARK ---
+ case DRIVE_TO_PARK:
+ follower.followPath(parkPath, true);
+ setPathState(PathState.DONE);
+ break;
+
+ case DONE:
+ telemetry.addLine("Completed Far Auton");
+ systems.setAutonState(pedroStateMachine.AutonState.IDLE);
+ break;
+ }
+ }
+
+ public void statePathUpdateNet() {
+ switch(pathState) {
+ // --- PRELOAD ---
+ case DRIVE_TO_SCORE_PRELOAD:
+ follower.followPath(scorePreloadPath, true);
+ systems.initiateTurretSpinUp();
+ systems.setAutonState(pedroStateMachine.AutonState.SCORING); // Shoot while driving
+ setPathState(PathState.SCORE_PRELOAD);
+ break;
+
+ case SCORE_PRELOAD:
+ if (!follower.isBusy() && checkScoringCondition()) {
+ follower.followPath(driveToSet2Path, true);
+ setPathState(PathState.DRIVE_TO_GRAB_SET_2);
+ }
+ break;
+
+ // --- SET 2 ---
+ case DRIVE_TO_GRAB_SET_2:
+ if (!follower.isBusy()) {
+ follower.followPath(grabSet2Path, true);
+ systems.setAutonState(pedroStateMachine.AutonState.INTAKING);
+ setPathState(PathState.GRAB_SET_2);
+ }
+ break;
+
+ case GRAB_SET_2:
+ if (!follower.isBusy()) {
+ systems.setAutonState(pedroStateMachine.AutonState.IDLE);
+ follower.followPath(scoreSet2Path, true);
+ systems.initiateTurretSpinUp();
+ setPathState(PathState.SCORE_SET_2);
+ }
+ break;
+
+ case SCORE_SET_2:
+ if (!follower.isBusy()) {
+ systems.setAutonState(pedroStateMachine.AutonState.SCORING);
+ if (checkScoringCondition()) {
+ follower.followPath(grabGate1Path, true);
+ systems.setAutonState(pedroStateMachine.AutonState.INTAKING);
+ setPathState(PathState.GRAB_GATE_1);
+ }
+ }
+ break;
+
+ // --- GATE 1 ---
+ case GRAB_GATE_1:
+ if (!follower.isBusy()) {
+ setPathState(PathState.WAIT_GATE_1);
+ }
+ break;
+
+ case WAIT_GATE_1:
+ // Stop at the gate while intaking to pull in game elements
+ if (actionTimer.getElapsedTimeSeconds() > gateWaitTime) {
+ systems.setAutonState(pedroStateMachine.AutonState.IDLE);
+ follower.followPath(scoreGate1Path, true);
+ systems.initiateTurretSpinUp();
+ setPathState(PathState.SCORE_GATE_1);
+ }
+ break;
+
+ case SCORE_GATE_1:
+ if (!follower.isBusy()) {
+ systems.setAutonState(pedroStateMachine.AutonState.SCORING);
+ if (checkScoringCondition()) {
+ follower.followPath(grabGate2Path, true);
+ systems.setAutonState(pedroStateMachine.AutonState.INTAKING);
+ setPathState(PathState.GRAB_GATE_2);
+ }
+ }
+ break;
+
+ // --- GATE 2 ---
+ case GRAB_GATE_2:
+ if (!follower.isBusy()) {
+ setPathState(PathState.WAIT_GATE_2);
+ }
+ break;
+
+ case WAIT_GATE_2:
+ if (actionTimer.getElapsedTimeSeconds() > gateWaitTime) {
+ systems.setAutonState(pedroStateMachine.AutonState.IDLE);
+ follower.followPath(scoreGate2Path, true);
+ systems.initiateTurretSpinUp();
+ setPathState(PathState.SCORE_GATE_2);
+ }
+ break;
+
+ case SCORE_GATE_2:
+ if (!follower.isBusy()) {
+ systems.setAutonState(pedroStateMachine.AutonState.SCORING);
+ if (checkScoringCondition()) {
+ follower.followPath(driveToSet3Path, true);
+ setPathState(PathState.DRIVE_TO_GRAB_SET_3);
+ }
+ }
+ break;
+
+ // --- SET 3 ---
+ case DRIVE_TO_GRAB_SET_3:
+ if (!follower.isBusy()) {
+ follower.followPath(grabSet3Path, true);
+ systems.setAutonState(pedroStateMachine.AutonState.INTAKING);
+ setPathState(PathState.GRAB_SET_3);
+ }
+ break;
+
+ case GRAB_SET_3:
+ if (!follower.isBusy()) {
+ systems.setAutonState(pedroStateMachine.AutonState.IDLE);
+ follower.followPath(scoreSet3Path, true);
+ systems.initiateTurretSpinUp();
+ setPathState(PathState.SCORE_SET_3);
+ }
+ break;
+
+ case SCORE_SET_3:
+ if (!follower.isBusy()) {
+ systems.setAutonState(pedroStateMachine.AutonState.SCORING);
+ if (checkScoringCondition()) {
+ follower.followPath(driveToSet1Path, true);
+ setPathState(PathState.DRIVE_TO_GRAB_SET_1);
+ }
+ }
+ break;
+
+ // --- SET 1 ---
+ case DRIVE_TO_GRAB_SET_1:
+ if (!follower.isBusy()) {
+ follower.followPath(grabSet1Path, true);
+ systems.setAutonState(pedroStateMachine.AutonState.INTAKING);
+ setPathState(PathState.GRAB_SET_1);
+ }
+ break;
+
+ case GRAB_SET_1:
+ if (!follower.isBusy()) {
+ systems.setAutonState(pedroStateMachine.AutonState.IDLE);
+ follower.followPath(scoreSet1Path, true);
+ systems.initiateTurretSpinUp();
+ systems.setAutonState(pedroStateMachine.AutonState.SCORING); // Shoot while driving
+ setPathState(PathState.SCORE_SET_1);
+ }
+ break;
+
+ case SCORE_SET_1:
+ if (!follower.isBusy() && checkScoringCondition()) {
+ follower.followPath(parkPath, true);
+ setPathState(PathState.DRIVE_TO_PARK);
+ }
+ break;
+
+ // --- PARK ---
+ case DRIVE_TO_PARK:
+ if (!follower.isBusy()) {
+ setPathState(PathState.DONE);
+ }
+ break;
+
+ case DONE:
+ telemetry.addLine("Completed Net Auton");
+ systems.setAutonState(pedroStateMachine.AutonState.IDLE);
+ break;
+ }
+ }
+
+ protected boolean checkScoringCondition() {
+ if (systems.isScoringComplete()) {
+ return true;
+ }
+ if (useTimer && pathTimer.getElapsedTimeSeconds() > timerTimeout) {
+ // Failsafe triggered
+ telemetry.addLine("WARNING: Scoring Timeout Triggered!");
+ return true;
+ }
+ return false;
+ }
+ }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/TimeAuton.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/TimeAuton.java
new file mode 100755
index 0000000..d5fdcd3
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/TimeAuton.java
@@ -0,0 +1,274 @@
+package org.firstinspires.ftc.teamcode.auton;
+
+import com.pedropathing.follower.Follower;
+import com.pedropathing.geometry.Pose;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.util.ElapsedTime;
+
+import org.firstinspires.ftc.teamcode.Hware.hwMap;
+import org.firstinspires.ftc.teamcode.pedroPathing.Constants;
+import org.firstinspires.ftc.teamcode.subsystems.Intake;
+import org.firstinspires.ftc.teamcode.subsystems.TransferSys;
+import org.firstinspires.ftc.teamcode.subsystems.Turret;
+import org.firstinspires.ftc.teamcode.util.AutoTransfer;
+/**
+ * Unified Autonomous for Net and Far sides (Blue and Red).
+ * Uses PedroPathing for Pose estimation only.
+ * Uses Time/Power for movement.
+ */
+public abstract class TimeAuton extends LinearOpMode {
+
+ // Subsystems
+ protected Intake intake;
+ protected TransferSys transfer;
+ protected Turret turret;
+ protected Follower follower;
+
+ // Drive Motors (Direct access for time-based drive)
+ protected DcMotor frontLeft, frontRight, backLeft, backRight;
+
+ // Constants
+ protected static final double DRIVE_POWER = 0.8;
+ protected static final double STRAFE_POWER = 0.8;
+
+ // State
+ protected int alliance = 1; // 1 = Blue, 2 = Red
+ protected boolean isNetSide = false;
+ protected final ElapsedTime timer = new ElapsedTime();
+
+ @Override
+ public void runOpMode() throws InterruptedException {
+ // 1. Initialize Hardware
+ hwMap.IntakeHwMap intakeHw = new hwMap.IntakeHwMap(hardwareMap);
+ hwMap.TransferHwMap transferHw = new hwMap.TransferHwMap(hardwareMap);
+ hwMap.TurretHwMap turretHw = new hwMap.TurretHwMap(hardwareMap);
+
+ intake = new Intake(intakeHw);
+ transfer = new TransferSys(transferHw);
+ turret = new Turret(turretHw);
+
+ transfer.closeLimiter();
+
+
+ // Initialize Drive Motors
+ frontLeft = hardwareMap.get(DcMotor.class, org.firstinspires.ftc.teamcode.teleOp.Constants.DriveConstants.FRONT_LEFT_MOTOR);
+ frontRight = hardwareMap.get(DcMotor.class, org.firstinspires.ftc.teamcode.teleOp.Constants.DriveConstants.FRONT_RIGHT_MOTOR);
+ backLeft = hardwareMap.get(DcMotor.class, org.firstinspires.ftc.teamcode.teleOp.Constants.DriveConstants.BACK_LEFT_MOTOR);
+ backRight = hardwareMap.get(DcMotor.class, org.firstinspires.ftc.teamcode.teleOp.Constants.DriveConstants.BACK_RIGHT_MOTOR);
+
+ configureDriveMotors();
+
+ // Initialize Pedro Follower (For Pose detection only)
+ follower = Constants.createFollower(hardwareMap);
+
+
+ // 2. Setup Specific Auton Configuration
+ configureAuton();
+
+ // Set Start Pose based on Alliance/Side
+ setStartPose();
+
+ AutoTransfer.reset();
+
+
+ turret.setAlliance(alliance);
+ AutoTransfer.initAuton(alliance);
+
+ telemetry.addLine("Initialized Unified Auton");
+ telemetry.addData("Alliance", alliance == 1 ? "BLUE" : "RED");
+ telemetry.addData("Side", isNetSide ? "NET" : "FAR");
+ telemetry.update();
+
+ waitForStart();
+
+ if(opModeIsActive()) {
+ follower.startTeleopDrive(); // Keeps pose updating without following a path
+
+ // 3. Run the Sequence
+ if (!isNetSide) {
+ runFarSequence();
+ } else {
+ runNetSequence();
+ }
+ }
+ }
+
+ // --- Abstract Methods to be defined by subclasses ---
+ protected abstract void configureAuton();
+
+ // --- Sequences ---
+
+ private void runFarSequence() {
+ // 1. Drive Front for 0.6 seconds
+ drive(DRIVE_POWER, 0, 0, 600);
+
+ // 2. Shoot all artifacts
+ shootAllArtifacts();
+
+ // 3. Drive Front for 1.5 seconds
+ drive(DRIVE_POWER, 0, 0, 1500);
+ }
+
+ private void runNetSequence() {
+ // 1. Drive Backward for 1.5 seconds
+ drive(-DRIVE_POWER, 0, 0, 1500);
+
+ // 2. Stop (Handled automatically by drive finish) & Shoot
+ shootAllArtifacts();
+
+ // 3. Strafe based on alliance for 0.7 seconds
+ if (alliance == 1) { // Blue -> Strafe Left
+ drive(0, -STRAFE_POWER, 0, 1600);
+ } else { // Red -> Strafe Right
+ drive(0, STRAFE_POWER, 0, 1600);
+ }
+ }
+
+ // --- Core Logic Methods ---
+
+ protected void shootAllArtifacts() {
+ // 1. Spin up Turret
+ turret.setTurretState(Turret.TurretState.LAUNCH);
+
+ // 2. Wait for Velocity
+ timer.reset();
+ while(opModeIsActive() && timer.seconds() < 2.0) {
+ updateSystems();
+ if(turret.isTurretReady()) break;
+
+ }
+
+ telemetry.addLine("LAUNCHING");
+ telemetry.update();
+
+ // 3. START FEEDING: Open Gate AND Run Intake
+ transfer.openLimiter();
+ transfer.setTransferState(TransferSys.TransferState.LAUNCHING);
+ intake.setIntakeState(Intake.IntakeState.LAUNCH); // <--- ADD THIS LINE
+
+ // 4. Wait for shot to finish
+ while(opModeIsActive() && transfer.getTransferState() == TransferSys.TransferState.LAUNCHING) {
+ updateSystems();
+ }
+
+ // 5. Stop everything
+ turret.setTurretState(Turret.TurretState.IDLE);
+ transfer.setTransferState(TransferSys.TransferState.IDLE);
+ intake.setIntakeState(Intake.IntakeState.IDLE); // <--- ADD THIS LINE
+ }
+
+ protected void updateSystems() {
+ follower.update(); // Update Odometry Pose
+ Pose currentPose = follower.getPose();
+
+ AutoTransfer.updatePose(currentPose);
+
+ // Update Turret with current Pose
+ turret.updateAuton(alliance, currentPose, 0.2);
+
+ transfer.updateTurretReady(turret.isTurretReady());
+ transfer.update();
+
+ // Telemetry for debugging
+ telemetry.addData("Turret Ready", turret.isTurretReady());
+ telemetry.addData("Transfer State", transfer.getTransferState());
+ telemetry.update();
+ }
+
+ // --- Drive Helper Methods ---
+
+ protected void drive(double x, double y, double rot, long durationMs) {
+ timer.reset();
+ while (opModeIsActive() && timer.milliseconds() < durationMs) {
+ double denominator = Math.max(Math.abs(x) + Math.abs(y) + Math.abs(rot), 1);
+
+ // Standard Mecanum formulas
+ double fl = (x + y + rot) / denominator;
+ double bl = (x - y + rot) / denominator;
+ double fr = (x - y - rot) / denominator;
+ double br = (x + y - rot) / denominator;
+
+ frontLeft.setPower(fl);
+ backLeft.setPower(bl);
+ frontRight.setPower(fr);
+ backRight.setPower(br);
+
+ updateSystems();
+ }
+ stopDrive();
+ }
+
+ protected void stopDrive() {
+ frontLeft.setPower(0);
+ backLeft.setPower(0);
+ frontRight.setPower(0);
+ backRight.setPower(0);
+ }
+
+ private void configureDriveMotors() {
+ frontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+ frontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+ backLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+ backRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+
+ frontLeft.setDirection(DcMotor.Direction.REVERSE);
+ backLeft.setDirection(DcMotor.Direction.REVERSE);
+ }
+
+ private void setStartPose() {
+ if (alliance == 1) { // Blue
+ if (isNetSide) follower.setStartingPose(new Pose(21, 124.4, Math.toRadians(-36)));
+ else follower.setStartingPose(new Pose(54, 8.65, Math.toRadians(90)));
+ } else { // Red
+ if (isNetSide) follower.setStartingPose(new Pose(123, 124.4, Math.toRadians(90)));
+ else follower.setStartingPose(new Pose(88, 8.65, Math.toRadians(90)));
+ }
+ }
+
+ // =================================================================
+ // ACTUAL AUTONOMOUS OPMODES
+ // =================================================================
+
+ @Disabled
+ @Autonomous(name = "Blue Far (Auto)", group = "Unified", preselectTeleOp = "Red Final")
+ public static class BlueFar extends TimeAuton {
+ @Override
+ protected void configureAuton() {
+ alliance = 1;
+ isNetSide = false;
+ }
+ }
+
+ @Disabled
+ @Autonomous(name = "Red Far (Auto)", group = "Unified", preselectTeleOp = "Red Final")
+ public static class RedFar extends TimeAuton {
+ @Override
+ protected void configureAuton() {
+ alliance = 2;
+ isNetSide = false;
+ }
+ }
+
+ @Disabled
+ @Autonomous(name = "Blue Net (Auto)", group = "Unified", preselectTeleOp = "Blue Final")
+ public static class BlueNet extends TimeAuton {
+ @Override
+ protected void configureAuton() {
+ alliance = 1;
+ isNetSide = true;
+ }
+ }
+
+ @Disabled
+ @Autonomous(name = "Red Net (Auto)", group = "Unified", preselectTeleOp = "Blue Final")
+ public static class RedNet extends TimeAuton {
+ @Override
+ protected void configureAuton() {
+ alliance = 2;
+ isNetSide = true;
+ }
+ }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/pedroStateMachine.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/pedroStateMachine.java
new file mode 100755
index 0000000..b03732a
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/pedroStateMachine.java
@@ -0,0 +1,134 @@
+package org.firstinspires.ftc.teamcode.auton;
+
+import com.pedropathing.geometry.Pose;
+
+import org.firstinspires.ftc.teamcode.Hware.hwMap;
+import org.firstinspires.ftc.teamcode.subsystems.DriveTrain; // Unused but kept for reference
+import org.firstinspires.ftc.teamcode.subsystems.Intake;
+import org.firstinspires.ftc.teamcode.subsystems.TransferSys;
+import org.firstinspires.ftc.teamcode.subsystems.Turret;
+import org.firstinspires.ftc.teamcode.subsystems.Turret.TurretState;
+import org.firstinspires.ftc.teamcode.subsystems.TransferSys.TransferState;
+import org.firstinspires.ftc.teamcode.subsystems.Intake.IntakeState;
+
+import org.firstinspires.ftc.teamcode.util.AutoTransfer;
+
+public class pedroStateMachine {
+
+ public enum AutonState {
+ IDLE,
+ INTAKING,
+ SCORING
+ }
+
+ private AutonState currentState = AutonState.IDLE;
+ private final Intake m_intake;
+ private final TransferSys m_transfer;
+ private final Turret m_turret;
+
+
+
+ private int alliance = 0;
+
+ public pedroStateMachine(hwMap.IntakeHwMap h_intake, hwMap.TransferHwMap h_transfer, hwMap.TurretHwMap h_turret) {
+ this.m_intake = new Intake(h_intake);
+ this.m_transfer = new TransferSys(h_transfer);
+ this.m_turret = new Turret(h_turret);
+
+ setAutonState(AutonState.IDLE);
+ AutoTransfer.reset();
+ }
+
+ public void setAlliance(int alliance) {
+ this.alliance = alliance;
+ m_turret.setAlliance(alliance);
+ AutoTransfer.initAuton(alliance);
+ }
+
+ public void setAutonState(AutonState newState) {
+ if (this.currentState == newState) return;
+
+ handleStateExit(this.currentState);
+ this.currentState = newState;
+ handleStateEntry(newState);
+ }
+
+ private void handleStateExit(AutonState oldState) {
+ switch (oldState) {
+ case INTAKING:
+ m_intake.setIntakeState(IntakeState.IDLE);
+ // Ensure gate is closed when we stop intaking to hold rings
+ m_transfer.setTransferState(TransferState.IDLE);
+ break;
+ case SCORING:
+ m_transfer.setTransferState(TransferState.IDLE);
+ m_turret.setTurretState(Turret.TurretState.IDLE);
+ m_intake.setIntakeState(Intake.IntakeState.IDLE);
+ break;
+ }
+ }
+
+ private void handleStateEntry(AutonState newState) {
+ switch (newState) {
+ case INTAKING:
+ m_intake.setIntakeState(IntakeState.INTAKE);
+ m_turret.setTurretState(TurretState.INTAKING); // Optional: position turret for intake stability
+ m_transfer.closeLimiter(); // Ensure rings don't fly out back
+ break;
+ case SCORING:
+ m_intake.setIntakeState(IntakeState.LAUNCH); // Feed rings
+ m_turret.setTurretState(TurretState.LAUNCH);
+ m_transfer.setTransferState(TransferState.LAUNCHING);
+ break;
+ case IDLE:
+ m_intake.setIntakeState(IntakeState.IDLE);
+ m_turret.setTurretState(TurretState.IDLE);
+ m_transfer.setTransferState(TransferState.IDLE);
+ break;
+ }
+ }
+
+ public void update(int allianceTarget, Pose currentPose) {
+ m_turret.updateAuton(allianceTarget, currentPose, 0.2);
+
+ if (currentState == AutonState.SCORING) {
+ m_transfer.updateTurretReady(m_turret.hasReachedVelocity());
+ }
+
+ m_transfer.update();
+
+
+ if (currentState == AutonState.SCORING) {
+ if (m_transfer.getTransferState() != TransferSys.TransferState.LAUNCHING) {
+ setAutonState(AutonState.IDLE);
+ }
+ }
+
+ AutoTransfer.updatePose(currentPose);
+ }
+
+ public void initiateTurretSpinUp() {
+ m_turret.setTurretState(TurretState.LAUNCH);
+ }
+
+ public boolean isTurretTracking() {
+ return m_turret.hasTarget();
+ }
+
+ public boolean isScoringComplete() {
+ if (currentState == AutonState.IDLE && m_transfer.getTransferState() == TransferState.IDLE) {
+ return true;
+ }
+ return false;
+ }
+
+ public double getTurretDistance() {
+ return m_turret.getDistance();
+ }
+
+ // Getters
+ public Intake getIntake() { return m_intake; }
+ public Turret getTurret() { return m_turret; }
+ public TransferSys getTransfer() { return m_transfer; }
+ public AutonState getCurrentState() { return currentState; }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/todo.txt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/todo.txt
new file mode 100755
index 0000000..e36f90c
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/auton/todo.txt
@@ -0,0 +1,29 @@
+Issues:
+ Therefore, the revised list of remaining handoff and connecting logic issues is:
+
+ 1. `hwMap.java ` - LED/Flicker Servo Port Conflict: LEDA, LEDB, and LEDC are mapped to the same hardware servo names as flickA, flickB, and flickC. This creates an alias, meaning controlling one set of servos will inadvertently control the other, leading to unintended behavior and potential conflicts.
+ DONE (diff thing but i did what was needed) - 2. `hwMap.java` - Incomplete `getAprilTagPose()` Implementation: The getAprilTagPose() method returns an empty double[0], rendering getDistanceFromAprilTag() (which relies on getAprilTagPose()) ineffective, always returning -1. This impacts the accuracy of turret aiming calculations.
+ DONE - 3. `hwMap.java` - `isTurretReady()` Placeholder: The isTurretReady(double turretPower) method always returns true. This is a placeholder and does not reflect actual turret readiness, potentially causing finalTeleOp to proceed with scoring prematurely.
+ DONE - i just removed the if alliance = 0 4. `subsystems/Turret.java` - `motif` Update Restriction: The motif (AprilTag pattern) in Turret.java is only updated via hardware.getMotif() when alliance == 0 within the update() method. If alliance is set to a non-zero value (e.g., 1 or 2 by finalTeleOp), the motif will not be updated from new Limelight detections in
+ teleop, potentially using an outdated pattern for TransferSys.
+ 5. `subsystems/Turret.java` - `updateAuton()` Limelight Pipeline Management: The updateAuton method (used in autonomous) calls hardware.setPipeline(allianceTarget). If allianceTarget (which is 1 from pedroRedNet.java) does not correspond to the correct Limelight pipeline index for AprilTags, the Limelight may switch to
+ an incorrect pipeline, hindering accurate AprilTag tracking.
+ 6. `subsystems/Turret.java` - Unused `apriltags` array: The apriltags array is declared in Turret.java but is never used, indicating potentially vestigial or incomplete logic.
+ 7. `subsystems/TransferSys.java` - Incorrect `flickTimer` Logic in `processFlickSequence()`: The flickTimer is reset when liftCurrentItem() is called, and then FLICK_RESET_MS is measured from that same flickTimer to determine when to dropCurrentItem(). This means the FLICK_RESET_MS duration is not correctly measured from
+ the moment the item is flicked up, but from the moment it started moving up, leading to potentially incorrect timing for the drop sequence.
+ 8. `subsystems/TransferSys.java` - Redundant `indexAllArtifacts()` Calls: indexAllArtifacts() is called when setTransferState(TransferState.INDEXING) and at the beginning of startFlickSequence(). While not a critical error, it could be redundant if the artifacts have already been indexed.
+ 9. `subsystems/TransferSys.java` - Simplistic `updateMotif` Logic: The updateMotif method creates a motif where only one slot can be color 2 (Green), and the rest are 1 (Purple), based on a single pattern input. This might be too simplistic or inflexible if more complex motif patterns are needed.
+ 10. `teleOp/StateMachine.java` - Direct `TurretState.LAUNCH` in `handleGameStateEntry(GameState.SCORING)`: The handleGameStateEntry for SCORING directly sets m_turret.setTurretState(Turret.TurretState.LAUNCH). This immediately commands the turret to launch without checking if it has acquired a target or is otherwise
+ ready, potentially leading to inaccurate shots.
+ 11. `teleOp/StateMachine.java` - `isMotifEdited` Flag Never Resets: The isMotifEdited flag in StateMachine.java (which mirrors the one in TransferSys.java) is set to true when m_transfer.isMotifEdited() is true but is never reset. This means that once a motif is detected and updated once, isMotifEdited() will always
+ return true, making the alliance detection logic in finalTeleOp.java trigger only once.
+ 12. `teleOp/StateMachine.java` - Unused `handleStatePeriodic()`: The handleStatePeriodic() method is defined but never called, indicating incomplete or unused functionality.
+ 13. `teleOp/finalTeleOp.java` - `alliance` and `isMotifEdited()` Fragility: The initial detection and setting of alliance = 1 based on stateMachine.isMotifEdited() is fragile due to the isMotifEdited flag never being reset. This can lead to alliance being stuck at 0 or 1 incorrectly if motif detection doesn't happen as
+ expected or if the flag is not reset.
+ 14. `teleOp/finalTeleOp.java` - Conflicting `hoodServo` Control: finalTeleOp.java directly initializes hoodServo and controls its position via hoodPosition. However, the Turret subsystem also has a hoodservo and a setHoodPos() method. This creates a potential conflict where finalTeleOp might be directly controlling the
+ servo while the Turret subsystem also tries to manage it, or the Turret's setHoodPos is being bypassed for manual control. Control should be consolidated through the Turret subsystem.
+ 15. `teleOp/finalTeleOp.java` - Redundant `stateMachine.getTransfer().indexAllArtifacts()`: indexAllArtifacts() is called at the very end of the while (opModeIsActive()) loop. This re-indexes artifacts on every loop iteration, which might be inefficient if indexing is a resource-intensive operation and not always needed.
+ 16. `teleOp/finalTeleOp.java` - Coupled `manualSAM` and `manualFlicker`: manualSAM = manualFlicker; directly couples manual flicker control with manual Shot Adjustment Module control. This might not always be the desired behavior and could limit flexibility.
+ 17. `teleOp/finalTeleOp.java` - Blocking `sleep()` Calls in `manualFlicker`: The use of sleep() calls within the manualFlicker block in the main while (opModeIsActive()) loop will block the entire robot's operation, preventing other subsystems from updating or responding to input for the duration of the sleep. Servo
+ operations should ideally be managed with non-blocking timers within a state machine.
+
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java
new file mode 100755
index 0000000..87e6271
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java
@@ -0,0 +1,59 @@
+package org.firstinspires.ftc.teamcode.pedroPathing;
+
+import com.pedropathing.control.FilteredPIDFCoefficients;
+import com.pedropathing.control.PIDFCoefficients;
+import com.pedropathing.follower.Follower;
+import com.pedropathing.follower.FollowerConstants;
+import com.pedropathing.ftc.drivetrains.MecanumConstants;
+import com.pedropathing.ftc.FollowerBuilder;
+import com.pedropathing.ftc.localization.Encoder;
+import com.pedropathing.ftc.localization.constants.ThreeWheelConstants;
+import com.pedropathing.ftc.localization.constants.ThreeWheelIMUConstants;
+import com.pedropathing.ftc.localization.constants.TwoWheelConstants;
+import com.pedropathing.paths.PathConstraints;
+import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+
+import org.firstinspires.ftc.teamcode.teleOp.Constants.DriveConstants;
+
+public class Constants {
+ public static FollowerConstants followerConstants = new FollowerConstants()
+ .mass(12);
+
+ public static MecanumConstants driveConstants = new MecanumConstants()
+ .maxPower(0.8)
+ .rightFrontMotorName(DriveConstants.FRONT_RIGHT_MOTOR)
+ .rightRearMotorName(DriveConstants.BACK_RIGHT_MOTOR)
+ .leftRearMotorName(DriveConstants.BACK_LEFT_MOTOR)
+ .leftFrontMotorName(DriveConstants.FRONT_LEFT_MOTOR)
+ .leftFrontMotorDirection(DriveConstants.FRONT_LEFT_DIRECTION)
+ .leftRearMotorDirection(DriveConstants.BACK_LEFT_DIRECTION)
+ .rightFrontMotorDirection(DriveConstants.FRONT_RIGHT_DIRECTION)
+ .rightRearMotorDirection(DriveConstants.BACK_RIGHT_DIRECTION);
+
+ public static PathConstraints pathConstraints = new PathConstraints(0.99, 100, 1, 1);
+
+
+ public static ThreeWheelConstants localizerConstants = new ThreeWheelConstants()
+ .forwardTicksToInches(0.001978956)
+ .strafeTicksToInches(0.001978956)
+ .turnTicksToInches(0.001978956)
+ .leftPodY(3.75)
+ .rightPodY(-3.25)
+ .strafePodX(-6.25)
+ .leftEncoder_HardwareMapName("intake")
+ .rightEncoder_HardwareMapName("fl")
+ .strafeEncoder_HardwareMapName("fr")
+ .leftEncoderDirection(Encoder.FORWARD)
+ .rightEncoderDirection(Encoder.FORWARD)
+ .strafeEncoderDirection(Encoder.FORWARD);
+
+
+ public static Follower createFollower(HardwareMap hardwareMap) {
+ return new FollowerBuilder(followerConstants, hardwareMap)
+ .pathConstraints(pathConstraints)
+ .mecanumDrivetrain(driveConstants)
+ .threeWheelLocalizer(localizerConstants)
+ .build();
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java.old b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java.old
new file mode 100755
index 0000000..158c37d
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java.old
@@ -0,0 +1,69 @@
+package org.firstinspires.ftc.teamcode.pedroPathing;
+
+import com.pedropathing.control.FilteredPIDFCoefficients;
+import com.pedropathing.control.PIDFCoefficients;
+import com.pedropathing.follower.Follower;
+import com.pedropathing.follower.FollowerConstants;
+import com.pedropathing.ftc.drivetrains.MecanumConstants;
+import com.pedropathing.ftc.FollowerBuilder;
+import com.pedropathing.ftc.localization.Encoder;
+import com.pedropathing.ftc.localization.constants.TwoWheelConstants; // Changed import
+import com.pedropathing.paths.PathConstraints;
+import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+
+import org.firstinspires.ftc.teamcode.teleOp.Constants.DriveConstants;
+
+public class Constants {
+
+ public static FollowerConstants followerConstants = new FollowerConstants()
+ .mass(12.27)
+ .forwardZeroPowerAcceleration(-36.474)
+ .lateralZeroPowerAcceleration(-75.984)
+ .translationalPIDFCoefficients(new PIDFCoefficients(0.08, 0, 0, 0.019))
+ .headingPIDFCoefficients(new PIDFCoefficients(0.932, 0, 0, 0.025))
+ .drivePIDFCoefficients(new FilteredPIDFCoefficients(.025,0,0.00001, 0.8, .01));
+
+ public static MecanumConstants driveConstants = new MecanumConstants()
+ .maxPower(0.8)
+ .rightFrontMotorName(DriveConstants.FRONT_RIGHT_MOTOR)
+ .rightRearMotorName(DriveConstants.BACK_RIGHT_MOTOR)
+ .leftRearMotorName(DriveConstants.BACK_LEFT_MOTOR)
+ .leftFrontMotorName(DriveConstants.FRONT_LEFT_MOTOR)
+ .leftFrontMotorDirection(DriveConstants.FRONT_LEFT_DIRECTION)
+ .leftRearMotorDirection(DriveConstants.BACK_LEFT_DIRECTION)
+ .rightFrontMotorDirection(DriveConstants.FRONT_RIGHT_DIRECTION)
+ .rightRearMotorDirection(DriveConstants.BACK_RIGHT_DIRECTION)
+ .xVelocity(80.106)
+ .yVelocity(64.8032);
+
+ public static PathConstraints pathConstraints = new PathConstraints(0.99, 100, 1.1, 1);
+
+ public static TwoWheelConstants localizerConstants = new TwoWheelConstants()
+ .forwardTicksToInches(0.0016334)
+ .strafeTicksToInches(0.00200734)
+ //.forwardPodY(5.2)
+ //.forwardEncoder_HardwareMapName("fl")
+ .forwardPodY(5.51)
+ .forwardEncoder_HardwareMapName("br")
+
+ .strafePodX(6.43)
+ .strafeEncoder_HardwareMapName("fl")
+
+ .forwardEncoderDirection(Encoder.REVERSE)
+ .strafeEncoderDirection(Encoder.REVERSE)
+
+ .IMU_HardwareMapName("imu")
+ .IMU_Orientation(new RevHubOrientationOnRobot(
+ RevHubOrientationOnRobot.LogoFacingDirection.RIGHT,
+ RevHubOrientationOnRobot.UsbFacingDirection.UP
+ ));
+
+ public static Follower createFollower(HardwareMap hardwareMap) {
+ return new FollowerBuilder(followerConstants, hardwareMap)
+ .pathConstraints(pathConstraints)
+ .mecanumDrivetrain(driveConstants)
+ .twoWheelLocalizer(localizerConstants)
+ .build();
+ }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java
new file mode 100755
index 0000000..5fb6b32
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java
@@ -0,0 +1,1359 @@
+package org.firstinspires.ftc.teamcode.pedroPathing;
+
+import static org.firstinspires.ftc.teamcode.pedroPathing.Tuning.changes;
+import static org.firstinspires.ftc.teamcode.pedroPathing.Tuning.drawOnlyCurrent;
+import static org.firstinspires.ftc.teamcode.pedroPathing.Tuning.draw;
+import static org.firstinspires.ftc.teamcode.pedroPathing.Tuning.follower;
+import static org.firstinspires.ftc.teamcode.pedroPathing.Tuning.stopRobot;
+import static org.firstinspires.ftc.teamcode.pedroPathing.Tuning.telemetryM;
+
+import com.bylazar.configurables.PanelsConfigurables;
+import com.bylazar.configurables.annotations.Configurable;
+import com.bylazar.configurables.annotations.IgnoreConfigurable;
+import com.bylazar.field.FieldManager;
+import com.bylazar.field.PanelsField;
+import com.bylazar.field.Style;
+import com.bylazar.telemetry.PanelsTelemetry;
+import com.bylazar.telemetry.TelemetryManager;
+import com.pedropathing.ErrorCalculator;
+import com.pedropathing.follower.Follower;
+import com.pedropathing.geometry.*;
+import com.pedropathing.math.*;
+import com.pedropathing.paths.*;
+import com.pedropathing.telemetry.SelectableOpMode;
+import com.pedropathing.util.*;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import java.util.ArrayList;
+import java.util.List;
+
+/**
+ * This is the Tuning class. It contains a selection menu for various tuning OpModes.
+ *
+ * @author Baron Henderson - 20077 The Indubitables
+ * @version 1.0, 6/26/2025
+ */
+@Configurable
+@TeleOp(name = "Tuning", group = "Pedro Pathing")
+public class Tuning extends SelectableOpMode {
+ public static Follower follower;
+
+ @IgnoreConfigurable
+ static PoseHistory poseHistory;
+
+ @IgnoreConfigurable
+ static TelemetryManager telemetryM;
+
+ @IgnoreConfigurable
+ static ArrayList changes = new ArrayList<>();
+
+ public Tuning() {
+ super("Select a Tuning OpMode", s -> {
+ s.folder("Localization", l -> {
+ l.add("Localization Test", LocalizationTest::new);
+ l.add("Forward Tuner", ForwardTuner::new);
+ l.add("Lateral Tuner", LateralTuner::new);
+ l.add("Turn Tuner", TurnTuner::new);
+ });
+ s.folder("Automatic", a -> {
+ a.add("Forward Velocity Tuner", ForwardVelocityTuner::new);
+ a.add("Lateral Velocity Tuner", LateralVelocityTuner::new);
+ a.add("Forward Zero Power Acceleration Tuner", ForwardZeroPowerAccelerationTuner::new);
+ a.add("Lateral Zero Power Acceleration Tuner", LateralZeroPowerAccelerationTuner::new);
+ });
+ s.folder("Manual", p -> {
+ p.add("Translational Tuner", TranslationalTuner::new);
+ p.add("Heading Tuner", HeadingTuner::new);
+ p.add("Drive Tuner", DriveTuner::new);
+ p.add("Line Tuner", Line::new);
+ p.add("Centripetal Tuner", CentripetalTuner::new);
+ });
+ s.folder("Tests", p -> {
+ p.add("Line", Line::new);
+ p.add("Triangle", Triangle::new);
+ p.add("Circle", Circle::new);
+ });
+ });
+ }
+
+ @Override
+ public void onSelect() {
+ if (follower == null) {
+ follower = Constants.createFollower(hardwareMap);
+ PanelsConfigurables.INSTANCE.refreshClass(this);
+ } else {
+ follower = Constants.createFollower(hardwareMap);
+ }
+
+ follower.setStartingPose(new Pose());
+
+ poseHistory = follower.getPoseHistory();
+
+ telemetryM = PanelsTelemetry.INSTANCE.getTelemetry();
+
+ Drawing.init();
+ }
+
+ @Override
+ public void onLog(List lines) {}
+
+ public static void drawOnlyCurrent() {
+ try {
+ Drawing.drawRobot(follower.getPose());
+ Drawing.sendPacket();
+ } catch (Exception e) {
+ throw new RuntimeException("Drawing failed " + e);
+ }
+ }
+
+ public static void draw() {
+ Drawing.drawDebug(follower);
+ }
+
+ /** This creates a full stop of the robot by setting the drive motors to run at 0 power. */
+ public static void stopRobot() {
+ follower.startTeleopDrive(true);
+ follower.setTeleOpDrive(0,0,0,true);
+ }
+}
+
+/**
+ * This is the LocalizationTest OpMode. This is basically just a simple mecanum drive attached to a
+ * PoseUpdater. The OpMode will print out the robot's pose to telemetry as well as draw the robot.
+ * You should use this to check the robot's localization.
+ *
+ * @author Anyi Lin - 10158 Scott's Bots
+ * @author Baron Henderson - 20077 The Indubitables
+ * @version 1.0, 5/6/2024
+ */
+class LocalizationTest extends OpMode {
+ @Override
+ public void init() {
+ follower.setStartingPose(new Pose(72,72));
+ }
+
+ /** This initializes the PoseUpdater, the mecanum drive motors, and the Panels telemetry. */
+ @Override
+ public void init_loop() {
+ telemetryM.debug("This will print your robot's position to telemetry while "
+ + "allowing robot control through a basic mecanum drive on gamepad 1.");
+ telemetryM.update(telemetry);
+ follower.update();
+ drawOnlyCurrent();
+ }
+
+ @Override
+ public void start() {
+ follower.startTeleopDrive();
+ follower.update();
+ }
+
+ /**
+ * This updates the robot's pose estimate, the simple mecanum drive, and updates the
+ * Panels telemetry with the robot's position as well as draws the robot's position.
+ */
+ @Override
+ public void loop() {
+ follower.setTeleOpDrive(-gamepad1.left_stick_y, -gamepad1.left_stick_x, -gamepad1.right_stick_x, true);
+ follower.update();
+
+ telemetryM.debug("x:" + follower.getPose().getX());
+ telemetryM.debug("y:" + follower.getPose().getY());
+ telemetryM.debug("heading:" + follower.getPose().getHeading());
+ telemetryM.debug("total heading:" + follower.getTotalHeading());
+ telemetryM.update(telemetry);
+
+ draw();
+ }
+}
+
+/**
+ * This is the ForwardTuner OpMode. This tracks the forward movement of the robot and displays the
+ * necessary ticks to inches multiplier. This displayed multiplier is what's necessary to scale the
+ * robot's current distance in ticks to the specified distance in inches. So, to use this, run the
+ * tuner, then pull/push the robot to the specified distance using a ruler on the ground. When you're
+ * at the end of the distance, record the ticks to inches multiplier. Feel free to run multiple trials
+ * and average the results. Then, input the multiplier into the forward ticks to inches in your
+ * localizer of choice.
+ *
+ * @author Anyi Lin - 10158 Scott's Bots
+ * @author Baron Henderson - 20077 The Indubitables
+ * @version 1.0, 5/6/2024
+ */
+class ForwardTuner extends OpMode {
+ public static double DISTANCE = 48;
+
+ @Override
+ public void init() {
+ follower.setStartingPose(new Pose(72,72));
+ follower.update();
+ drawOnlyCurrent();
+ }
+
+ /** This initializes the PoseUpdater as well as the Panels telemetry. */
+ @Override
+ public void init_loop() {
+ telemetryM.debug("Pull your robot forward " + DISTANCE + " inches. Your forward ticks to inches will be shown on the telemetry.");
+ telemetryM.update(telemetry);
+ drawOnlyCurrent();
+ }
+
+ /**
+ * This updates the robot's pose estimate, and updates the Panels telemetry with the
+ * calculated multiplier and draws the robot.
+ */
+ @Override
+ public void loop() {
+ follower.update();
+
+ telemetryM.debug("Distance Moved: " + (follower.getPose().getX() - 72));
+ telemetryM.debug("The multiplier will display what your forward ticks to inches should be to scale your current distance to " + DISTANCE + " inches.");
+ telemetryM.debug("Multiplier: " + (DISTANCE / ((follower.getPose().getX() - 72) / follower.getPoseTracker().getLocalizer().getForwardMultiplier())));
+ telemetryM.update(telemetry);
+
+ draw();
+ }
+}
+
+/**
+ * This is the LateralTuner OpMode. This tracks the strafe movement of the robot and displays the
+ * necessary ticks to inches multiplier. This displayed multiplier is what's necessary to scale the
+ * robot's current distance in ticks to the specified distance in inches. So, to use this, run the
+ * tuner, then pull/push the robot to the specified distance using a ruler on the ground. When you're
+ * at the end of the distance, record the ticks to inches multiplier. Feel free to run multiple trials
+ * and average the results. Then, input the multiplier into the strafe ticks to inches in your
+ * localizer of choice.
+ *
+ * @author Anyi Lin - 10158 Scott's Bots
+ * @author Baron Henderson - 20077 The Indubitables
+ * @version 2.0, 6/26/2025
+ */
+class LateralTuner extends OpMode {
+ public static double DISTANCE = 48;
+
+ @Override
+ public void init() {
+ follower.setStartingPose(new Pose(72,72));
+ follower.update();
+ drawOnlyCurrent();
+ }
+
+ /** This initializes the PoseUpdater as well as the Panels telemetry. */
+ @Override
+ public void init_loop() {
+ telemetryM.debug("Pull your robot to the right " + DISTANCE + " inches. Your strafe ticks to inches will be shown on the telemetry.");
+ telemetryM.update(telemetry);
+ drawOnlyCurrent();
+ }
+
+ /**
+ * This updates the robot's pose estimate, and updates the Panels telemetry with the
+ * calculated multiplier and draws the robot.
+ */
+ @Override
+ public void loop() {
+ follower.update();
+
+ telemetryM.debug("Distance Moved: " + (follower.getPose().getY() - 72));
+ telemetryM.debug("The multiplier will display what your strafe ticks to inches should be to scale your current distance to " + DISTANCE + " inches.");
+ telemetryM.debug("Multiplier: " + (DISTANCE / ((follower.getPose().getY() - 72) / follower.getPoseTracker().getLocalizer().getLateralMultiplier())));
+ telemetryM.update(telemetry);
+
+ draw();
+ }
+}
+
+/**
+ * This is the TurnTuner OpMode. This tracks the turning movement of the robot and displays the
+ * necessary ticks to inches multiplier. This displayed multiplier is what's necessary to scale the
+ * robot's current angle in ticks to the specified angle in radians. So, to use this, run the
+ * tuner, then pull/push the robot to the specified angle using a protractor or lines on the ground.
+ * When you're at the end of the angle, record the ticks to inches multiplier. Feel free to run
+ * multiple trials and average the results. Then, input the multiplier into the turning ticks to
+ * radians in your localizer of choice.
+ *
+ * @author Anyi Lin - 10158 Scott's Bots
+ * @author Baron Henderson - 20077 The Indubitables
+ * @version 1.0, 5/6/2024
+ */
+class TurnTuner extends OpMode {
+ public static double ANGLE = 2 * Math.PI;
+
+ @Override
+ public void init() {
+ follower.setStartingPose(new Pose(72,72));
+ follower.update();
+ drawOnlyCurrent();
+ }
+
+ /** This initializes the PoseUpdater as well as the Panels telemetry. */
+ @Override
+ public void init_loop() {
+ telemetryM.debug("Turn your robot " + ANGLE + " radians. Your turn ticks to inches will be shown on the telemetry.");
+ telemetryM.update(telemetry);
+
+ drawOnlyCurrent();
+ }
+
+ /**
+ * This updates the robot's pose estimate, and updates the Panels telemetry with the
+ * calculated multiplier and draws the robot.
+ */
+ @Override
+ public void loop() {
+ follower.update();
+
+ telemetryM.debug("Total Angle: " + follower.getTotalHeading());
+ telemetryM.debug("The multiplier will display what your turn ticks to inches should be to scale your current angle to " + ANGLE + " radians.");
+ telemetryM.debug("Multiplier: " + (ANGLE / (follower.getTotalHeading() / follower.getPoseTracker().getLocalizer().getTurningMultiplier())));
+ telemetryM.update(telemetry);
+
+ draw();
+ }
+}
+
+/**
+ * This is the ForwardVelocityTuner autonomous follower OpMode. This runs the robot forwards at max
+ * power until it reaches some specified distance. It records the most recent velocities, and on
+ * reaching the end of the distance, it averages them and prints out the velocity obtained. It is
+ * recommended to run this multiple times on a full battery to get the best results. What this does
+ * is, when paired with StrafeVelocityTuner, allows FollowerConstants to create a Vector that
+ * empirically represents the direction your mecanum wheels actually prefer to go in, allowing for
+ * more accurate following.
+ *
+ * @author Anyi Lin - 10158 Scott's Bots
+ * @author Aaron Yang - 10158 Scott's Bots
+ * @author Harrison Womack - 10158 Scott's Bots
+ * @author Baron Henderson - 20077 The Indubitables
+ * @version 1.0, 3/13/2024
+ */
+class ForwardVelocityTuner extends OpMode {
+ private final ArrayList velocities = new ArrayList<>();
+ public static double DISTANCE = 48;
+ public static double RECORD_NUMBER = 10;
+
+ private boolean end;
+
+ @Override
+ public void init() {
+ follower.setStartingPose(new Pose(72, 72));
+ }
+
+ /** This initializes the drive motors as well as the cache of velocities and the Panels telemetry. */
+ @Override
+ public void init_loop() {
+ telemetryM.debug("The robot will run at 1 power until it reaches " + DISTANCE + " inches forward.");
+ telemetryM.debug("Make sure you have enough room, since the robot has inertia after cutting power.");
+ telemetryM.debug("After running the distance, the robot will cut power from the drivetrain and display the forward velocity.");
+ telemetryM.debug("Press B on game pad 1 to stop.");
+ telemetryM.debug("pose", follower.getPose());
+ telemetryM.update(telemetry);
+ follower.update();
+ drawOnlyCurrent();
+ }
+
+ /** This starts the OpMode by setting the drive motors to run forward at full power. */
+ @Override
+ public void start() {
+ for (int i = 0; i < RECORD_NUMBER; i++) {
+ velocities.add(0.0);
+ }
+ follower.startTeleopDrive(true);
+ follower.update();
+ end = false;
+ }
+
+ /**
+ * This runs the OpMode. At any point during the running of the OpMode, pressing B on
+ * game pad 1 will stop the OpMode. This continuously records the RECORD_NUMBER most recent
+ * velocities, and when the robot has run forward enough, these last velocities recorded are
+ * averaged and printed.
+ */
+ @Override
+ public void loop() {
+ if (gamepad1.bWasPressed()) {
+ stopRobot();
+ requestOpModeStop();
+ }
+
+ follower.update();
+ draw();
+
+
+ if (!end) {
+ if (Math.abs(follower.getPose().getX()) > (DISTANCE + 72)) {
+ end = true;
+ stopRobot();
+ } else {
+ follower.setTeleOpDrive(1,0,0,true);
+ //double currentVelocity = Math.abs(follower.getVelocity().getXComponent());
+ double currentVelocity = Math.abs(follower.poseTracker.getLocalizer().getVelocity().getX());
+ velocities.add(currentVelocity);
+ velocities.remove(0);
+ }
+ } else {
+ stopRobot();
+ double average = 0;
+ for (double velocity : velocities) {
+ average += velocity;
+ }
+ average /= velocities.size();
+ telemetryM.debug("Forward Velocity: " + average);
+ telemetryM.debug("\n");
+ telemetryM.debug("Press A to set the Forward Velocity temporarily (while robot remains on).");
+
+ for (int i = 0; i < velocities.size(); i++) {
+ telemetry.addData(String.valueOf(i), velocities.get(i));
+ }
+
+ telemetryM.update(telemetry);
+ telemetry.update();
+
+ if (gamepad1.aWasPressed()) {
+ follower.setXVelocity(average);
+ String message = "XMovement: " + average;
+ changes.add(message);
+ }
+ }
+ }
+}
+
+/**
+ * This is the StrafeVelocityTuner autonomous follower OpMode. This runs the robot left at max
+ * power until it reaches some specified distance. It records the most recent velocities, and on
+ * reaching the end of the distance, it averages them and prints out the velocity obtained. It is
+ * recommended to run this multiple times on a full battery to get the best results. What this does
+ * is, when paired with ForwardVelocityTuner, allows FollowerConstants to create a Vector that
+ * empirically represents the direction your mecanum wheels actually prefer to go in, allowing for
+ * more accurate following.
+ *
+ * @author Anyi Lin - 10158 Scott's Bots
+ * @author Aaron Yang - 10158 Scott's Bots
+ * @author Harrison Womack - 10158 Scott's Bots
+ * @author Baron Henderson - 20077 The Indubitables
+ * @version 1.0, 3/13/2024
+ */
+class LateralVelocityTuner extends OpMode {
+ private final ArrayList velocities = new ArrayList<>();
+
+ public static double DISTANCE = 48;
+ public static double RECORD_NUMBER = 10;
+
+ private boolean end;
+
+ @Override
+ public void init() {
+ follower.setStartingPose(new Pose(72, 72));
+ }
+
+ /**
+ * This initializes the drive motors as well as the cache of velocities and the Panels
+ * telemetryM.
+ */
+ @Override
+ public void init_loop() {
+ telemetryM.debug("The robot will run at 1 power until it reaches " + DISTANCE + " inches to the left.");
+ telemetryM.debug("Make sure you have enough room, since the robot has inertia after cutting power.");
+ telemetryM.debug("After running the distance, the robot will cut power from the drivetrain and display the strafe velocity.");
+ telemetryM.debug("Press B on Gamepad 1 to stop.");
+ telemetryM.update(telemetry);
+ follower.update();
+ drawOnlyCurrent();
+ }
+
+ /** This starts the OpMode by setting the drive motors to run left at full power. */
+ @Override
+ public void start() {
+ for (int i = 0; i < RECORD_NUMBER; i++) {
+ velocities.add(0.0);
+ }
+ follower.startTeleopDrive(true);
+ follower.update();
+ }
+
+ /**
+ * This runs the OpMode. At any point during the running of the OpMode, pressing B on
+ * game pad1 will stop the OpMode. This continuously records the RECORD_NUMBER most recent
+ * velocities, and when the robot has run sideways enough, these last velocities recorded are
+ * averaged and printed.
+ */
+ @Override
+ public void loop() {
+ if (gamepad1.bWasPressed()) {
+ stopRobot();
+ requestOpModeStop();
+ }
+
+ follower.update();
+ draw();
+
+ if (!end) {
+ if (Math.abs(follower.getPose().getY()) > (DISTANCE + 72)) {
+ end = true;
+ stopRobot();
+ } else {
+ follower.setTeleOpDrive(0,1,0,true);
+ double currentVelocity = Math.abs(follower.getVelocity().dot(new Vector(1, Math.PI / 2)));
+ velocities.add(currentVelocity);
+ velocities.remove(0);
+ }
+ } else {
+ stopRobot();
+ double average = 0;
+ for (double velocity : velocities) {
+ average += velocity;
+ }
+ average /= velocities.size();
+
+ telemetryM.debug("Strafe Velocity: " + average);
+ telemetryM.debug("\n");
+ telemetryM.debug("Press A to set the Lateral Velocity temporarily (while robot remains on).");
+ telemetryM.update(telemetry);
+
+ if (gamepad1.aWasPressed()) {
+ follower.setYVelocity(average);
+ String message = "YMovement: " + average;
+ changes.add(message);
+ }
+ }
+ }
+}
+
+/**
+ * This is the ForwardZeroPowerAccelerationTuner autonomous follower OpMode. This runs the robot
+ * forward until a specified velocity is achieved. Then, the robot cuts power to the motors, setting
+ * them to zero power. The deceleration, or negative acceleration, is then measured until the robot
+ * stops. The accelerations across the entire time the robot is slowing down is then averaged and
+ * that number is then printed. This is used to determine how the robot will decelerate in the
+ * forward direction when power is cut, making the estimations used in the calculations for the
+ * drive Vector more accurate and giving better braking at the end of Paths.
+ *
+ * @author Anyi Lin - 10158 Scott's Bots
+ * @author Baron Henderson - 20077 The Indubitables
+ * @author Aaron Yang - 10158 Scott's Bots
+ * @author Harrison Womack - 10158 Scott's Bots
+ * @version 1.0, 3/13/2024
+ */
+class ForwardZeroPowerAccelerationTuner extends OpMode {
+ private final ArrayList accelerations = new ArrayList<>();
+ public static double VELOCITY = 30;
+
+ private double previousVelocity;
+ private long previousTimeNano;
+
+ private boolean stopping;
+ private boolean end;
+
+ @Override
+ public void init() {
+ follower.setStartingPose(new Pose(72, 72));
+ }
+
+ /** This initializes the drive motors as well as the Panels telemetryM. */
+ @Override
+ public void init_loop() {
+ telemetryM.debug("The robot will run forward until it reaches " + VELOCITY + " inches per second.");
+ telemetryM.debug("Then, it will cut power from the drivetrain and roll to a stop.");
+ telemetryM.debug("Make sure you have enough room.");
+ telemetryM.debug("After stopping, the forward zero power acceleration (natural deceleration) will be displayed.");
+ telemetryM.debug("Press B on Gamepad 1 to stop.");
+ telemetryM.update(telemetry);
+ follower.update();
+ drawOnlyCurrent();
+ }
+
+ /** This starts the OpMode by setting the drive motors to run forward at full power. */
+ @Override
+ public void start() {
+ follower.startTeleopDrive(false);
+ follower.update();
+ follower.setTeleOpDrive(1,0,0,true);
+ }
+
+ /**
+ * This runs the OpMode. At any point during the running of the OpMode, pressing B on
+ * game pad 1 will stop the OpMode. When the robot hits the specified velocity, the robot will
+ * record its deceleration / negative acceleration until it stops. Then, it will average all the
+ * recorded deceleration / negative acceleration and print that value.
+ */
+ @Override
+ public void loop() {
+ if (gamepad1.bWasPressed()) {
+ stopRobot();
+ requestOpModeStop();
+ }
+
+ follower.update();
+ draw();
+
+ Vector heading = new Vector(1.0, follower.getPose().getHeading());
+ if (!end) {
+ if (!stopping) {
+ if (follower.getVelocity().dot(heading) > VELOCITY) {
+ previousVelocity = follower.getVelocity().dot(heading);
+ previousTimeNano = System.nanoTime();
+ stopping = true;
+ follower.setTeleOpDrive(0,0,0,true);
+ }
+ } else {
+ double currentVelocity = follower.getVelocity().dot(heading);
+ accelerations.add((currentVelocity - previousVelocity) / ((System.nanoTime() - previousTimeNano) / Math.pow(10.0, 9)));
+ previousVelocity = currentVelocity;
+ previousTimeNano = System.nanoTime();
+ if (currentVelocity < follower.getConstraints().getVelocityConstraint()) {
+ end = true;
+ }
+ }
+ } else {
+ double average = 0;
+ for (double acceleration : accelerations) {
+ average += acceleration;
+ }
+ average /= accelerations.size();
+
+ telemetryM.debug("Forward Zero Power Acceleration (Deceleration): " + average);
+ telemetryM.debug("\n");
+ telemetryM.debug("Press A to set the Forward Zero Power Acceleration temporarily (while robot remains on).");
+ telemetryM.update(telemetry);
+
+ if (gamepad1.aWasPressed()) {
+ follower.getConstants().setForwardZeroPowerAcceleration(average);
+ String message = "Forward Zero Power Acceleration: " + average;
+ changes.add(message);
+ }
+ }
+ }
+}
+
+/**
+ * This is the LateralZeroPowerAccelerationTuner autonomous follower OpMode. This runs the robot
+ * to the left until a specified velocity is achieved. Then, the robot cuts power to the motors, setting
+ * them to zero power. The deceleration, or negative acceleration, is then measured until the robot
+ * stops. The accelerations across the entire time the robot is slowing down is then averaged and
+ * that number is then printed. This is used to determine how the robot will decelerate in the
+ * forward direction when power is cut, making the estimations used in the calculations for the
+ * drive Vector more accurate and giving better braking at the end of Paths.
+ *
+ * @author Anyi Lin - 10158 Scott's Bots
+ * @author Aaron Yang - 10158 Scott's Bots
+ * @author Harrison Womack - 10158 Scott's Bots
+ * @author Baron Henderson - 20077 The Indubitables
+ * @version 1.0, 3/13/2024
+ */
+class LateralZeroPowerAccelerationTuner extends OpMode {
+ private final ArrayList accelerations = new ArrayList<>();
+ public static double VELOCITY = 30;
+ private double previousVelocity;
+ private long previousTimeNano;
+ private boolean stopping;
+ private boolean end;
+
+ @Override
+ public void init() {
+ follower.setStartingPose(new Pose(72, 72));
+ }
+
+ /** This initializes the drive motors as well as the Panels telemetry. */
+ @Override
+ public void init_loop() {
+ telemetryM.debug("The robot will run to the left until it reaches " + VELOCITY + " inches per second.");
+ telemetryM.debug("Then, it will cut power from the drivetrain and roll to a stop.");
+ telemetryM.debug("Make sure you have enough room.");
+ telemetryM.debug("After stopping, the lateral zero power acceleration (natural deceleration) will be displayed.");
+ telemetryM.debug("Press B on game pad 1 to stop.");
+ telemetryM.update(telemetry);
+ follower.update();
+ drawOnlyCurrent();
+ }
+
+ /** This starts the OpMode by setting the drive motors to run forward at full power. */
+ @Override
+ public void start() {
+ follower.startTeleopDrive(false);
+ follower.update();
+ follower.setTeleOpDrive(0,1,0,true);
+ }
+
+ /**
+ * This runs the OpMode. At any point during the running of the OpMode, pressing B on
+ * game pad 1 will stop the OpMode. When the robot hits the specified velocity, the robot will
+ * record its deceleration / negative acceleration until it stops. Then, it will average all the
+ * recorded deceleration / negative acceleration and print that value.
+ */
+ @Override
+ public void loop() {
+ if (gamepad1.bWasPressed()) {
+ stopRobot();
+ requestOpModeStop();
+ }
+
+ follower.update();
+ draw();
+
+ Vector heading = new Vector(1.0, follower.getPose().getHeading() - Math.PI / 2);
+ if (!end) {
+ if (!stopping) {
+ if (Math.abs(follower.getVelocity().dot(heading)) > VELOCITY) {
+ previousVelocity = Math.abs(follower.getVelocity().dot(heading));
+ previousTimeNano = System.nanoTime();
+ stopping = true;
+ follower.setTeleOpDrive(0,0,0,true);
+ }
+ } else {
+ double currentVelocity = Math.abs(follower.getVelocity().dot(heading));
+ accelerations.add((currentVelocity - previousVelocity) / ((System.nanoTime() - previousTimeNano) / Math.pow(10.0, 9)));
+ previousVelocity = currentVelocity;
+ previousTimeNano = System.nanoTime();
+ if (currentVelocity < follower.getConstraints().getVelocityConstraint()) {
+ end = true;
+ }
+ }
+ } else {
+ double average = 0;
+ for (double acceleration : accelerations) {
+ average += acceleration;
+ }
+ average /= accelerations.size();
+
+ telemetryM.debug("Lateral Zero Power Acceleration (Deceleration): " + average);
+ telemetryM.debug("\n");
+ telemetryM.debug("Press A to set the Lateral Zero Power Acceleration temporarily (while robot remains on).");
+ telemetryM.update(telemetry);
+
+ if (gamepad1.aWasPressed()) {
+ follower.getConstants().setLateralZeroPowerAcceleration(average);
+ String message = "Lateral Zero Power Acceleration: " + average;
+ changes.add(message);
+ }
+ }
+ }
+}
+
+/**
+ * This is the Translational PIDF Tuner OpMode. It will keep the robot in place.
+ * The user should push the robot laterally to test the PIDF and adjust the PIDF values accordingly.
+ *
+ * @author Baron Henderson - 20077 The Indubitables
+ * @author Anyi Lin - 10158 Scott's Bots
+ * @author Aaron Yang - 10158 Scott's Bots
+ * @author Harrison Womack - 10158 Scott's Bots
+ * @version 1.0, 3/12/2024
+ */
+class TranslationalTuner extends OpMode {
+ public static double DISTANCE = 40;
+ private boolean forward = true;
+
+ private Path forwards;
+ private Path backwards;
+
+ @Override
+ public void init() {
+ follower.setStartingPose(new Pose(72, 72));
+ }
+
+ /** This initializes the Follower and creates the forward and backward Paths. */
+ @Override
+ public void init_loop() {
+ telemetryM.debug("This will activate the translational PIDF(s)");
+ telemetryM.debug("The robot will try to stay in place while you push it laterally.");
+ telemetryM.debug("You can adjust the PIDF values to tune the robot's translational PIDF(s).");
+ telemetryM.update(telemetry);
+ follower.update();
+ drawOnlyCurrent();
+ }
+
+ @Override
+ public void start() {
+ follower.deactivateAllPIDFs();
+ follower.activateTranslational();
+ forwards = new Path(new BezierLine(new Pose(72,72), new Pose(DISTANCE + 72,72)));
+ forwards.setConstantHeadingInterpolation(0);
+ backwards = new Path(new BezierLine(new Pose(DISTANCE + 72,72), new Pose(72,72)));
+ backwards.setConstantHeadingInterpolation(0);
+ follower.followPath(forwards);
+ }
+
+ /** This runs the OpMode, updating the Follower as well as printing out the debug statements to the Telemetry */
+ @Override
+ public void loop() {
+ follower.update();
+ draw();
+
+ if (!follower.isBusy()) {
+ if (forward) {
+ forward = false;
+ follower.followPath(backwards);
+ } else {
+ forward = true;
+ follower.followPath(forwards);
+ }
+ }
+
+ telemetryM.debug("Push the robot laterally to test the Translational PIDF(s).");
+ telemetryM.addData("Zero Line", 0);
+ telemetryM.addData("Error X", follower.errorCalculator.getTranslationalError().getXComponent());
+ telemetryM.addData("Error Y", follower.errorCalculator.getTranslationalError().getYComponent());
+ telemetryM.update(telemetry);
+ }
+}
+
+/**
+ * This is the Heading PIDF Tuner OpMode. It will keep the robot in place.
+ * The user should try to turn the robot to test the PIDF and adjust the PIDF values accordingly.
+ * It will try to keep the robot at a constant heading while the user tries to turn it.
+ *
+ * @author Baron Henderson - 20077 The Indubitables
+ * @author Anyi Lin - 10158 Scott's Bots
+ * @author Aaron Yang - 10158 Scott's Bots
+ * @author Harrison Womack - 10158 Scott's Bots
+ * @version 1.0, 3/12/2024
+ */
+class HeadingTuner extends OpMode {
+ public static double DISTANCE = 40;
+ private boolean forward = true;
+
+ private Path forwards;
+ private Path backwards;
+
+ @Override
+ public void init() {
+ follower.setStartingPose(new Pose(72, 72));
+ }
+
+ /**
+ * This initializes the Follower and creates the forward and backward Paths. Additionally, this
+ * initializes the Panels telemetry.
+ */
+ @Override
+ public void init_loop() {
+ telemetryM.debug("This will activate the heading PIDF(s).");
+ telemetryM.debug("The robot will try to stay at a constant heading while you try to turn it.");
+ telemetryM.debug("You can adjust the PIDF values to tune the robot's heading PIDF(s).");
+ telemetryM.update(telemetry);
+ follower.update();
+ drawOnlyCurrent();
+ }
+
+ @Override
+ public void start() {
+ follower.deactivateAllPIDFs();
+ follower.activateHeading();
+ forwards = new Path(new BezierLine(new Pose(72,72), new Pose(DISTANCE + 72,72)));
+ forwards.setConstantHeadingInterpolation(0);
+ backwards = new Path(new BezierLine(new Pose(DISTANCE + 72,72), new Pose(72,72)));
+ backwards.setConstantHeadingInterpolation(0);
+ follower.followPath(forwards);
+ }
+
+ /**
+ * This runs the OpMode, updating the Follower as well as printing out the debug statements to
+ * the Telemetry, as well as the Panels.
+ */
+ @Override
+ public void loop() {
+ follower.update();
+ draw();
+
+ if (!follower.isBusy()) {
+ if (forward) {
+ forward = false;
+ follower.followPath(backwards);
+ } else {
+ forward = true;
+ follower.followPath(forwards);
+ }
+ }
+
+ telemetryM.debug("Turn the robot manually to test the Heading PIDF(s).");
+ telemetryM.addData("Zero Line", 0);
+ telemetryM.addData("Error", follower.errorCalculator.getHeadingError());
+ telemetryM.update(telemetry);
+ }
+}
+
+/**
+ * This is the Drive PIDF Tuner OpMode. It will run the robot in a straight line going forward and back.
+ *
+ * @author Baron Henderson - 20077 The Indubitables
+ * @author Anyi Lin - 10158 Scott's Bots
+ * @author Aaron Yang - 10158 Scott's Bots
+ * @author Harrison Womack - 10158 Scott's Bots
+ * @version 1.0, 3/12/2024
+ */
+class DriveTuner extends OpMode {
+ public static double DISTANCE = 40;
+ private boolean forward = true;
+
+ private PathChain forwards;
+ private PathChain backwards;
+
+ @Override
+ public void init() {
+ follower.setStartingPose(new Pose(72, 72));
+ }
+
+ /**
+ * This initializes the Follower and creates the forward and backward Paths. Additionally, this
+ * initializes the Panels telemetry.
+ */
+ @Override
+ public void init_loop() {
+ telemetryM.debug("This will run the robot in a straight line going " + DISTANCE + "inches forward.");
+ telemetryM.debug("The robot will go forward and backward continuously along the path.");
+ telemetryM.debug("Make sure you have enough room.");
+ telemetryM.update(telemetry);
+ follower.update();
+ drawOnlyCurrent();
+ }
+
+ @Override
+ public void start() {
+ follower.deactivateAllPIDFs();
+ follower.activateDrive();
+
+ forwards = follower.pathBuilder()
+ .setGlobalDeceleration()
+ .addPath(new BezierLine(new Pose(72,72), new Pose(DISTANCE + 72,72)))
+ .setConstantHeadingInterpolation(0)
+ .build();
+
+ backwards = follower.pathBuilder()
+ .setGlobalDeceleration()
+ .addPath(new BezierLine(new Pose(DISTANCE + 72,72), new Pose(72,72)))
+ .setConstantHeadingInterpolation(0)
+ .build();
+
+ follower.followPath(forwards);
+ }
+
+ /**
+ * This runs the OpMode, updating the Follower as well as printing out the debug statements to
+ * the Telemetry, as well as the Panels.
+ */
+ @Override
+ public void loop() {
+ follower.update();
+ draw();
+
+ if (!follower.isBusy()) {
+ if (forward) {
+ forward = false;
+ follower.followPath(backwards);
+ } else {
+ forward = true;
+ follower.followPath(forwards);
+ }
+ }
+
+ telemetryM.debug("Driving forward?: " + forward);
+ telemetryM.addData("Zero Line", 0);
+ telemetryM.addData("Error", follower.errorCalculator.getDriveErrors()[1]);
+ telemetryM.update(telemetry);
+ }
+}
+
+/**
+ * This is the Line Test Tuner OpMode. It will drive the robot forward and back
+ * The user should push the robot laterally and angular to test out the drive, heading, and translational PIDFs.
+ *
+ * @author Baron Henderson - 20077 The Indubitables
+ * @author Anyi Lin - 10158 Scott's Bots
+ * @author Aaron Yang - 10158 Scott's Bots
+ * @author Harrison Womack - 10158 Scott's Bots
+ * @version 1.0, 3/12/2024
+ */
+class Line extends OpMode {
+ public static double DISTANCE = 40;
+ private boolean forward = true;
+
+ private Path forwards;
+ private Path backwards;
+
+ @Override
+ public void init() {
+ follower.setStartingPose(new Pose(72, 72));
+ }
+
+ /** This initializes the Follower and creates the forward and backward Paths. */
+ @Override
+ public void init_loop() {
+ telemetryM.debug("This will activate all the PIDF(s)");
+ telemetryM.debug("The robot will go forward and backward continuously along the path while correcting.");
+ telemetryM.debug("You can adjust the PIDF values to tune the robot's drive PIDF(s).");
+ telemetryM.update(telemetry);
+ follower.update();
+ drawOnlyCurrent();
+ }
+
+ @Override
+ public void start() {
+ follower.activateAllPIDFs();
+ forwards = new Path(new BezierLine(new Pose(72,72), new Pose(DISTANCE + 72,72)));
+ forwards.setConstantHeadingInterpolation(0);
+ backwards = new Path(new BezierLine(new Pose(DISTANCE + 72,72), new Pose(72,72)));
+ backwards.setConstantHeadingInterpolation(0);
+ follower.followPath(forwards);
+ }
+
+ /** This runs the OpMode, updating the Follower as well as printing out the debug statements to the Telemetry */
+ @Override
+ public void loop() {
+ follower.update();
+ draw();
+
+ if (!follower.isBusy()) {
+ if (forward) {
+ forward = false;
+ follower.followPath(backwards);
+ } else {
+ forward = true;
+ follower.followPath(forwards);
+ }
+ }
+
+ telemetryM.debug("Driving Forward?: " + forward);
+ telemetryM.update(telemetry);
+ }
+}
+
+/**
+ * This is the Centripetal Tuner OpMode. It runs the robot in a specified distance
+ * forward and to the left. On reaching the end of the forward Path, the robot runs the backward
+ * Path the same distance back to the start. Rinse and repeat! This is good for testing a variety
+ * of Vectors, like the drive Vector, the translational Vector, the heading Vector, and the
+ * centripetal Vector.
+ *
+ * @author Baron Henderson - 20077 The Indubitables
+ * @author Anyi Lin - 10158 Scott's Bots
+ * @author Aaron Yang - 10158 Scott's Bots
+ * @author Harrison Womack - 10158 Scott's Bots
+ * @version 1.0, 3/13/2024
+ */
+class CentripetalTuner extends OpMode {
+ public static double DISTANCE = 20;
+ private boolean forward = true;
+
+ private Path forwards;
+ private Path backwards;
+
+ @Override
+ public void init() {
+ follower.setStartingPose(new Pose(72, 72));
+ }
+
+ /**
+ * This initializes the Follower and creates the forward and backward Paths.
+ * Additionally, this initializes the Panels telemetry.
+ */
+ @Override
+ public void init_loop() {
+ telemetryM.debug("This will run the robot in a curve going " + DISTANCE + " inches to the left and the same number of inches forward.");
+ telemetryM.debug("The robot will go continuously along the path.");
+ telemetryM.debug("Make sure you have enough room.");
+ telemetryM.update(telemetry);
+ follower.update();
+ drawOnlyCurrent();
+ }
+
+ @Override
+ public void start() {
+ follower.activateAllPIDFs();
+ forwards = new Path(new BezierCurve(new Pose(72,72), new Pose(Math.abs(DISTANCE) + 72,72), new Pose(Math.abs(DISTANCE) + 72,DISTANCE + 72)));
+ backwards = new Path(new BezierCurve(new Pose(Math.abs(DISTANCE) + 72,DISTANCE + 72), new Pose(Math.abs(DISTANCE) + 72,72), new Pose(72,72)));
+
+ backwards.setTangentHeadingInterpolation();
+ backwards.reverseHeadingInterpolation();
+
+ follower.followPath(forwards);
+ }
+
+ /**
+ * This runs the OpMode, updating the Follower as well as printing out the debug statements to
+ * the Telemetry, as well as the Panels.
+ */
+ @Override
+ public void loop() {
+ follower.update();
+ draw();
+ if (!follower.isBusy()) {
+ if (forward) {
+ forward = false;
+ follower.followPath(backwards);
+ } else {
+ forward = true;
+ follower.followPath(forwards);
+ }
+ }
+
+ telemetryM.debug("Driving away from the origin along the curve?: " + forward);
+ telemetryM.update(telemetry);
+ }
+}
+
+/**
+ * This is the Triangle autonomous OpMode.
+ * It runs the robot in a triangle, with the starting point being the bottom-middle point.
+ *
+ * @author Baron Henderson - 20077 The Indubitables
+ * @author Samarth Mahapatra - 1002 CircuitRunners Robotics Surge
+ * @version 1.0, 12/30/2024
+ */
+class Triangle extends OpMode {
+
+ private final Pose startPose = new Pose(72, 72, Math.toRadians(0));
+ private final Pose interPose = new Pose(24 + 72, -24 + 72, Math.toRadians(90));
+ private final Pose endPose = new Pose(24 + 72, 24 + 72, Math.toRadians(45));
+
+ private PathChain triangle;
+
+ /**
+ * This runs the OpMode, updating the Follower as well as printing out the debug statements to
+ * the Telemetry, as well as the Panels.
+ */
+ @Override
+ public void loop() {
+ follower.update();
+ draw();
+
+ if (follower.atParametricEnd()) {
+ follower.followPath(triangle, true);
+ }
+ }
+
+ @Override
+ public void init() {
+ follower.setStartingPose(new Pose(72, 72));
+ }
+
+ @Override
+ public void init_loop() {
+ telemetryM.debug("This will run in a roughly triangular shape, starting on the bottom-middle point.");
+ telemetryM.debug("So, make sure you have enough space to the left, front, and right to run the OpMode.");
+ telemetryM.update(telemetry);
+ follower.update();
+ drawOnlyCurrent();
+ }
+
+ /** Creates the PathChain for the "triangle".*/
+ @Override
+ public void start() {
+ follower.setStartingPose(startPose);
+
+ triangle = follower.pathBuilder()
+ .addPath(new BezierLine(startPose, interPose))
+ .setLinearHeadingInterpolation(startPose.getHeading(), interPose.getHeading())
+ .addPath(new BezierLine(interPose, endPose))
+ .setLinearHeadingInterpolation(interPose.getHeading(), endPose.getHeading())
+ .addPath(new BezierLine(endPose, startPose))
+ .setLinearHeadingInterpolation(endPose.getHeading(), startPose.getHeading())
+ .build();
+
+ follower.followPath(triangle);
+ }
+}
+
+/**
+ * This is the Circle autonomous OpMode. It runs the robot in a PathChain that's actually not quite
+ * a circle, but some Bezier curves that have control points set essentially in a square. However,
+ * it turns enough to tune your centripetal force correction and some of your heading. Some lag in
+ * heading is to be expected.
+ *
+ * @author Anyi Lin - 10158 Scott's Bots
+ * @author Aaron Yang - 10158 Scott's Bots
+ * @author Harrison Womack - 10158 Scott's Bots
+ * @version 1.0, 3/12/2024
+ */
+class Circle extends OpMode {
+ public static double RADIUS = 10;
+ private PathChain circle;
+
+ public void start() {
+ circle = follower.pathBuilder()
+ .addPath(new BezierCurve(new Pose(72, 72), new Pose(RADIUS + 72, 72), new Pose(RADIUS + 72, RADIUS + 72)))
+ .setHeadingInterpolation(HeadingInterpolator.facingPoint(72, RADIUS + 72))
+ .addPath(new BezierCurve(new Pose(RADIUS + 72, RADIUS + 72), new Pose(RADIUS + 72, (2 * RADIUS) + 72), new Pose(72, (2 * RADIUS) + 72)))
+ .setHeadingInterpolation(HeadingInterpolator.facingPoint(72, RADIUS + 72))
+ .addPath(new BezierCurve(new Pose(72, (2 * RADIUS) + 72), new Pose(-RADIUS + 72, (2 * RADIUS) + 72), new Pose(-RADIUS + 72, RADIUS + 72)))
+ .setHeadingInterpolation(HeadingInterpolator.facingPoint(72, RADIUS + 72))
+ .addPath(new BezierCurve(new Pose(-RADIUS + 72, RADIUS + 72), new Pose(-RADIUS + 72, 72), new Pose(72, 72)))
+ .setHeadingInterpolation(HeadingInterpolator.facingPoint(72, RADIUS + 72))
+ .build();
+ follower.followPath(circle);
+ }
+
+ @Override
+ public void init_loop() {
+ telemetryM.debug("This will run in a roughly circular shape of radius " + RADIUS + ", starting on the right-most edge. ");
+ telemetryM.debug("So, make sure you have enough space to the left, front, and back to run the OpMode.");
+ telemetryM.debug("It will also continuously face the center of the circle to test your heading and centripetal correction.");
+ telemetryM.update(telemetry);
+ follower.update();
+ drawOnlyCurrent();
+ }
+
+ @Override
+ public void init() {
+ follower.setStartingPose(new Pose(72, 72));
+ }
+
+ /**
+ * This runs the OpMode, updating the Follower as well as printing out the debug statements to
+ * the Telemetry, as well as the FTC Dashboard.
+ */
+ @Override
+ public void loop() {
+ follower.update();
+ draw();
+
+ if (follower.atParametricEnd()) {
+ follower.followPath(circle);
+ }
+ }
+}
+
+/**
+ * This is the Drawing class. It handles the drawing of stuff on Panels Dashboard, like the robot.
+ *
+ * @author Lazar - 19234
+ * @version 1.1, 5/19/2025
+ */
+class Drawing {
+ public static final double ROBOT_RADIUS = 9; // woah
+ private static final FieldManager panelsField = PanelsField.INSTANCE.getField();
+
+ private static final Style robotLook = new Style(
+ "", "#3F51B5", 0.75
+ );
+ private static final Style historyLook = new Style(
+ "", "#4CAF50", 0.75
+ );
+
+ /**
+ * This prepares Panels Field for using Pedro Offsets
+ */
+ public static void init() {
+ panelsField.setOffsets(PanelsField.INSTANCE.getPresets().getPEDRO_PATHING());
+ }
+
+ /**
+ * This draws everything that will be used in the Follower's telemetryDebug() method. This takes
+ * a Follower as an input, so an instance of the DashbaordDrawingHandler class is not needed.
+ *
+ * @param follower Pedro Follower instance.
+ */
+ public static void drawDebug(Follower follower) {
+ if (follower.getCurrentPath() != null) {
+ drawPath(follower.getCurrentPath(), robotLook);
+ Pose closestPoint = follower.getPointFromPath(follower.getCurrentPath().getClosestPointTValue());
+ drawRobot(new Pose(closestPoint.getX(), closestPoint.getY(), follower.getCurrentPath().getHeadingGoal(follower.getCurrentPath().getClosestPointTValue())), robotLook);
+ }
+ drawPoseHistory(follower.getPoseHistory(), historyLook);
+ drawRobot(follower.getPose(), historyLook);
+
+ sendPacket();
+ }
+
+ /**
+ * This draws a robot at a specified Pose with a specified
+ * look. The heading is represented as a line.
+ *
+ * @param pose the Pose to draw the robot at
+ * @param style the parameters used to draw the robot with
+ */
+ public static void drawRobot(Pose pose, Style style) {
+ if (pose == null || Double.isNaN(pose.getX()) || Double.isNaN(pose.getY()) || Double.isNaN(pose.getHeading())) {
+ return;
+ }
+
+ panelsField.setStyle(style);
+ panelsField.moveCursor(pose.getX(), pose.getY());
+ panelsField.circle(ROBOT_RADIUS);
+
+ Vector v = pose.getHeadingAsUnitVector();
+ v.setMagnitude(v.getMagnitude() * ROBOT_RADIUS);
+ double x1 = pose.getX() + v.getXComponent() / 2, y1 = pose.getY() + v.getYComponent() / 2;
+ double x2 = pose.getX() + v.getXComponent(), y2 = pose.getY() + v.getYComponent();
+
+ panelsField.setStyle(style);
+ panelsField.moveCursor(x1, y1);
+ panelsField.line(x2, y2);
+ }
+
+ /**
+ * This draws a robot at a specified Pose. The heading is represented as a line.
+ *
+ * @param pose the Pose to draw the robot at
+ */
+ public static void drawRobot(Pose pose) {
+ drawRobot(pose, robotLook);
+ }
+
+ /**
+ * This draws a Path with a specified look.
+ *
+ * @param path the Path to draw
+ * @param style the parameters used to draw the Path with
+ */
+ public static void drawPath(Path path, Style style) {
+ double[][] points = path.getPanelsDrawingPoints();
+
+ for (int i = 0; i < points[0].length; i++) {
+ for (int j = 0; j < points.length; j++) {
+ if (Double.isNaN(points[j][i])) {
+ points[j][i] = 0;
+ }
+ }
+ }
+
+ panelsField.setStyle(style);
+ panelsField.moveCursor(points[0][0], points[0][1]);
+ panelsField.line(points[1][0], points[1][1]);
+ }
+
+ /**
+ * This draws all the Paths in a PathChain with a
+ * specified look.
+ *
+ * @param pathChain the PathChain to draw
+ * @param style the parameters used to draw the PathChain with
+ */
+ public static void drawPath(PathChain pathChain, Style style) {
+ for (int i = 0; i < pathChain.size(); i++) {
+ drawPath(pathChain.getPath(i), style);
+ }
+ }
+
+ /**
+ * This draws the pose history of the robot.
+ *
+ * @param poseTracker the PoseHistory to get the pose history from
+ * @param style the parameters used to draw the pose history with
+ */
+ public static void drawPoseHistory(PoseHistory poseTracker, Style style) {
+ panelsField.setStyle(style);
+
+ int size = poseTracker.getXPositionsArray().length;
+ for (int i = 0; i < size - 1; i++) {
+
+ panelsField.moveCursor(poseTracker.getXPositionsArray()[i], poseTracker.getYPositionsArray()[i]);
+ panelsField.line(poseTracker.getXPositionsArray()[i + 1], poseTracker.getYPositionsArray()[i + 1]);
+ }
+ }
+
+ /**
+ * This draws the pose history of the robot.
+ *
+ * @param poseTracker the PoseHistory to get the pose history from
+ */
+ public static void drawPoseHistory(PoseHistory poseTracker) {
+ drawPoseHistory(poseTracker, historyLook);
+ }
+
+ /**
+ * This tries to send the current packet to FTControl Panels.
+ */
+ public static void sendPacket() {
+ panelsField.update();
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/readme.md b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/readme.md
new file mode 100755
index 0000000..4d1da42
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/readme.md
@@ -0,0 +1,131 @@
+## TeamCode Module
+
+Welcome!
+
+This module, TeamCode, is the place where you will write/paste the code for your team's
+robot controller App. This module is currently empty (a clean slate) but the
+process for adding OpModes is straightforward.
+
+## Creating your own OpModes
+
+The easiest way to create your own OpMode is to copy a Sample OpMode and make it your own.
+
+Sample opmodes exist in the FtcRobotController module.
+To locate these samples, find the FtcRobotController module in the "Project/Android" tab.
+
+Expand the following tree elements:
+ FtcRobotController/java/org.firstinspires.ftc.robotcontroller/external/samples
+
+### Naming of Samples
+
+To gain a better understanding of how the samples are organized, and how to interpret the
+naming system, it will help to understand the conventions that were used during their creation.
+
+These conventions are described (in detail) in the sample_conventions.md file in this folder.
+
+To summarize: A range of different samples classes will reside in the java/external/samples.
+The class names will follow a naming convention which indicates the purpose of each class.
+The prefix of the name will be one of the following:
+
+Basic: This is a minimally functional OpMode used to illustrate the skeleton/structure
+ of a particular style of OpMode. These are bare bones examples.
+
+Sensor: This is a Sample OpMode that shows how to use a specific sensor.
+ It is not intended to drive a functioning robot, it is simply showing the minimal code
+ required to read and display the sensor values.
+
+Robot: This is a Sample OpMode that assumes a simple two-motor (differential) drive base.
+ It may be used to provide a common baseline driving OpMode, or
+ to demonstrate how a particular sensor or concept can be used to navigate.
+
+Concept: This is a sample OpMode that illustrates performing a specific function or concept.
+ These may be complex, but their operation should be explained clearly in the comments,
+ or the comments should reference an external doc, guide or tutorial.
+ Each OpMode should try to only demonstrate a single concept so they are easy to
+ locate based on their name. These OpModes may not produce a drivable robot.
+
+After the prefix, other conventions will apply:
+
+* Sensor class names are constructed as: Sensor - Company - Type
+* Robot class names are constructed as: Robot - Mode - Action - OpModetype
+* Concept class names are constructed as: Concept - Topic - OpModetype
+
+Once you are familiar with the range of samples available, you can choose one to be the
+basis for your own robot. In all cases, the desired sample(s) needs to be copied into
+your TeamCode module to be used.
+
+This is done inside Android Studio directly, using the following steps:
+
+ 1) Locate the desired sample class in the Project/Android tree.
+
+ 2) Right click on the sample class and select "Copy"
+
+ 3) Expand the TeamCode/java folder
+
+ 4) Right click on the org.firstinspires.ftc.teamcode folder and select "Paste"
+
+ 5) You will be prompted for a class name for the copy.
+ Choose something meaningful based on the purpose of this class.
+ Start with a capital letter, and remember that there may be more similar classes later.
+
+Once your copy has been created, you should prepare it for use on your robot.
+This is done by adjusting the OpMode's name, and enabling it to be displayed on the
+Driver Station's OpMode list.
+
+Each OpMode sample class begins with several lines of code like the ones shown below:
+
+```
+ @TeleOp(name="Template: Linear OpMode", group="Linear Opmode")
+ @Disabled
+```
+
+The name that will appear on the driver station's "opmode list" is defined by the code:
+ ``name="Template: Linear OpMode"``
+You can change what appears between the quotes to better describe your opmode.
+The "group=" portion of the code can be used to help organize your list of OpModes.
+
+As shown, the current OpMode will NOT appear on the driver station's OpMode list because of the
+ ``@Disabled`` annotation which has been included.
+This line can simply be deleted , or commented out, to make the OpMode visible.
+
+
+
+## ADVANCED Multi-Team App management: Cloning the TeamCode Module
+
+In some situations, you have multiple teams in your club and you want them to all share
+a common code organization, with each being able to *see* the others code but each having
+their own team module with their own code that they maintain themselves.
+
+In this situation, you might wish to clone the TeamCode module, once for each of these teams.
+Each of the clones would then appear along side each other in the Android Studio module list,
+together with the FtcRobotController module (and the original TeamCode module).
+
+Selective Team phones can then be programmed by selecting the desired Module from the pulldown list
+prior to clicking to the green Run arrow.
+
+Warning: This is not for the inexperienced Software developer.
+You will need to be comfortable with File manipulations and managing Android Studio Modules.
+These changes are performed OUTSIDE of Android Studios, so close Android Studios before you do this.
+
+Also.. Make a full project backup before you start this :)
+
+To clone TeamCode, do the following:
+
+Note: Some names start with "Team" and others start with "team". This is intentional.
+
+1) Using your operating system file management tools, copy the whole "TeamCode"
+ folder to a sibling folder with a corresponding new name, eg: "Team0417".
+
+2) In the new Team0417 folder, delete the TeamCode.iml file.
+
+3) the new Team0417 folder, rename the "src/main/java/org/firstinspires/ftc/teamcode" folder
+ to a matching name with a lowercase 'team' eg: "team0417".
+
+4) In the new Team0417/src/main folder, edit the "AndroidManifest.xml" file, change the line that contains
+ package="org.firstinspires.ftc.teamcode"
+ to be
+ package="org.firstinspires.ftc.team0417"
+
+5) Add: include ':Team0417' to the "/settings.gradle" file.
+
+6) Open up Android Studios and clean out any old files by using the menu to "Build/Clean Project""
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/DriveTrain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/DriveTrain.java
new file mode 100755
index 0000000..fe5f557
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/DriveTrain.java
@@ -0,0 +1,192 @@
+package org.firstinspires.ftc.teamcode.subsystems;
+
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.util.Range;
+import org.firstinspires.ftc.teamcode.Hware.hwMap;
+import org.firstinspires.ftc.teamcode.teleOp.Constants;
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles;
+
+public class DriveTrain {
+ private final hwMap.DriveHwMap h_driveTrain;
+
+ // --- Anchor Mode State ---
+ private boolean isAnchorActive = false;
+
+ private double targetFwdPod = 0; // Uses frontLeft encoder
+ private double targetStrafePod = 0; // Uses backLeft encoder
+ private double targetHeading = 0; // Uses IMU (Degrees)
+
+ // PID Storage
+
+ private double driveP = -0.001;
+ private double driveI = 0.0;
+ private double driveD = -0.00015;
+
+ private double strafeP = -0.002;
+ private double strafeI = 0.0;
+ private double strafeD = -0.00008;
+
+ private double turnP = -0.08;
+ private double turnI = 0.0;
+ private double turnD = -0.0001; // Tuned for Degrees
+
+ private double anchorMaxPower = 0.7;
+
+ private double lastFwdErr = 0, lastStrErr = 0, lastTurnErr = 0;
+ private double sumFwd = 0, sumStr = 0, sumTurn = 0;
+ private long lastLoopTime = System.currentTimeMillis();
+
+ public enum DriveState {
+ NORMAL, TURBO, PRECISION, STOP
+ }
+ private DriveState currentState = DriveState.NORMAL;
+ private double speedMultiplier = 1.0;
+
+ public DriveTrain(hwMap.DriveHwMap hardware) {
+ this.h_driveTrain = hardware;
+ }
+
+ public void update() {
+ if (isAnchorActive) {
+ runAnchorControlLoop();
+ }
+ }
+
+ private void runAnchorControlLoop() {
+ long currentTime = System.currentTimeMillis();
+ double dt = (currentTime - lastLoopTime) / 1000.0;
+ if(dt == 0) dt = 0.001;
+ lastLoopTime = currentTime;
+
+ double currentFwd = h_driveTrain.backRightMotor.getCurrentPosition();
+ double currentStrafe = h_driveTrain.frontLeftMotor.getCurrentPosition();
+ double currentHeading = getHeadingDegrees();
+
+ // 2. Calculate Errors
+ double errorForward = targetFwdPod - currentFwd;
+ double errorStrafe = targetStrafePod - currentStrafe;
+
+ // Heading Error (Wrapped -180 to 180)
+ double errorTurn = targetHeading - currentHeading;
+ while (errorTurn > 180) errorTurn -= 360;
+ while (errorTurn < -180) errorTurn += 360;
+
+ // 3. PID Calculations
+
+ // Forward
+ sumFwd += errorForward * dt;
+ double dFwd = (errorForward - lastFwdErr) / dt;
+ double powerFwd = (errorForward * driveP) + (sumFwd * driveI) + (dFwd * driveD);
+
+ // Strafe
+ sumStr += errorStrafe * dt;
+ double dStr = (errorStrafe - lastStrErr) / dt;
+ double powerStr = (errorStrafe * strafeP) + (sumStr * strafeI) + (dStr * strafeD);
+
+ // Turn
+ sumTurn += errorTurn * dt;
+ double dTurn = (errorTurn - lastTurnErr) / dt;
+ double powerTurn = (errorTurn * turnP) + (sumTurn * turnI) + (dTurn * turnD);
+
+ lastFwdErr = errorForward;
+ lastStrErr = errorStrafe;
+ lastTurnErr = errorTurn;
+
+ powerFwd = Range.clip(powerFwd, -anchorMaxPower, anchorMaxPower);
+ powerStr = Range.clip(powerStr, -anchorMaxPower, anchorMaxPower);
+ powerTurn = Range.clip(powerTurn, -anchorMaxPower, anchorMaxPower);
+
+ double fl = powerFwd + powerStr + powerTurn;
+ double bl = powerFwd - powerStr + powerTurn;
+ double fr = powerFwd - powerStr - powerTurn;
+ double br = powerFwd + powerStr - powerTurn;
+
+ double max = Math.max(Math.abs(fl), Math.max(Math.abs(bl), Math.max(Math.abs(fr), Math.abs(br))));
+ if (max > 1.0) {
+ fl /= max; bl /= max; fr /= max; br /= max;
+ }
+
+ h_driveTrain.setMotorPowers(fl, fr, bl, br);
+ }
+
+ public void startAnchor() {
+ if (!isAnchorActive) {
+ h_driveTrain.resetEncoders();
+ targetFwdPod = h_driveTrain.frontLeftMotor.getCurrentPosition();
+ targetStrafePod = h_driveTrain.backLeftMotor.getCurrentPosition();
+ targetHeading = getHeadingDegrees();
+
+ sumFwd = 0; sumStr = 0; sumTurn = 0;
+ lastFwdErr = 0; lastStrErr = 0; lastTurnErr = 0;
+ lastLoopTime = System.currentTimeMillis();
+
+ isAnchorActive = true;
+ this.currentState = DriveState.STOP;
+ }
+ }
+
+ public void stopAnchor() {
+ if (isAnchorActive) {
+ isAnchorActive = false;
+ stop();
+ this.currentState = DriveState.NORMAL;
+ }
+ }
+
+ private double getHeadingDegrees() {
+ YawPitchRollAngles orientation = h_driveTrain.imu.getRobotYawPitchRollAngles();
+ return orientation.getYaw(AngleUnit.DEGREES);
+ }
+
+ public void updateAnchorPID(double dP, double dI, double dD,
+ double sP, double sI, double sD,
+ double tP, double tI, double tD,
+ double maxP) {
+ this.driveP = dP; this.driveI = dI; this.driveD = dD;
+ this.strafeP = sP; this.strafeI = sI; this.strafeD = sD;
+ this.turnP = tP; this.turnI = tI; this.turnD = tD;
+ this.anchorMaxPower = maxP;
+ }
+
+ public void teleopDrive(double x, double y, double rx) {
+ if (isAnchorActive) {
+ if (Math.abs(x) > 0.1 || Math.abs(y) > 0.1 || Math.abs(rx) > 0.1) {
+ stopAnchor();
+ } else {
+ return;
+ }
+ }
+
+ double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1);
+ double frontLeftPower = (((y + x + rx) / denominator) * speedMultiplier);
+ double backLeftPower = (((y - x + rx) / denominator) * speedMultiplier);
+ double frontRightPower = (((y - x - rx) / denominator) * speedMultiplier);
+ double backRightPower = (((y + x - rx) / denominator) * speedMultiplier);
+
+
+ h_driveTrain.setMotorPowers(frontLeftPower, frontRightPower, backLeftPower, backRightPower);
+ }
+
+ // Getters for Tuning Telemetry
+ public boolean isAnchorActive() { return isAnchorActive; }
+ public double getLastForwardError() { return lastFwdErr; }
+ public double getLastStrafeError() { return lastStrErr; }
+ public double getLastTurnError() { return lastTurnErr; }
+
+ public void stop() {
+ if (currentState != DriveState.STOP) setDriveState(DriveState.STOP);
+ h_driveTrain.stopMotors();
+ }
+
+ public void setDriveState(DriveState state) {
+ this.currentState = state;
+ switch (state) {
+ case NORMAL: speedMultiplier = Constants.DriveConstants.NORMAL_SPEED_MULTIPLIER; break;
+ case PRECISION: speedMultiplier = Constants.DriveConstants.PRECISION_SPEED_MULTIPLIER; break;
+ case TURBO: speedMultiplier = Constants.DriveConstants.TURBO_SPEED_MULTIPLIER; break;
+ case STOP: speedMultiplier = Constants.DriveConstants.STOP_SPEED_MULTIPLIER; stop(); break;
+ }
+ }
+ public DriveState getDriveState() { return currentState; }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Intake.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Intake.java
new file mode 100755
index 0000000..ffb3620
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Intake.java
@@ -0,0 +1,49 @@
+package org.firstinspires.ftc.teamcode.subsystems;
+
+import org.firstinspires.ftc.teamcode.Hware.hwMap;
+import org.firstinspires.ftc.teamcode.teleOp.Constants;
+
+public class Intake {
+ private double runPower = 1;
+ private final hwMap.IntakeHwMap h_intake;
+ public enum IntakeState {
+ INTAKE,
+ EXTAKE,
+ IDLE,
+ LAUNCH,
+ STOP
+ }
+
+ private IntakeState currentState = IntakeState.IDLE;
+ public Intake(hwMap.IntakeHwMap hardware) {
+ this.h_intake = hardware;
+ }
+ public void setIntakeState(IntakeState state) {
+ this.currentState = state;
+
+ switch (state) {
+ case INTAKE:
+ h_intake.frontIntakeMotor.setPower(Constants.IntakeConstants.INTAKE_POWER);
+ //h_intake.backIntakeMotor.setPower(Constants.IntakeConstants.INTAKE_POWER);
+ break;
+ case EXTAKE:
+ h_intake.frontIntakeMotor.setPower(Constants.IntakeConstants.EXTAKE_POWER);
+ //h_intake.backIntakeMotor.setPower(Constants.IntakeConstants.EXTAKE_POWER);
+ break;
+ case LAUNCH:
+ h_intake.frontIntakeMotor.setPower(Constants.IntakeConstants.LAUNCH_POWER);
+ //h_intake.backIntakeMotor.setPower(Constants.IntakeConstants.LAUNCH_POWER);
+ break;
+ case IDLE:
+ h_intake.frontIntakeMotor.setPower(Constants.IntakeConstants.IDLE_POWER);
+ //h_intake.backIntakeMotor.setPower(Constants.IntakeConstants.IDLE_POWER);
+ break;
+ case STOP:
+ break;
+ }
+ }
+
+ public IntakeState getCurrentState() {
+ return this.currentState;
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/LED.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/LED.java
new file mode 100755
index 0000000..59e6a46
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/LED.java
@@ -0,0 +1,76 @@
+package org.firstinspires.ftc.teamcode.subsystems;
+
+import org.firstinspires.ftc.teamcode.Hware.hwMap;
+import org.firstinspires.ftc.teamcode.Prism.Color;
+import org.firstinspires.ftc.teamcode.Prism.GoBildaPrismDriver.LayerHeight;
+import org.firstinspires.ftc.teamcode.Prism.PrismAnimations;
+
+public class LED {
+ private final hwMap.LedHwMap hardware;
+
+ // Track states to prevent spamming the I2C bus
+ private int currentLed1State = -1;
+ private int currentLed2State = -1;
+
+ // Reusable animations for efficiency
+ private final PrismAnimations.Solid solidRed = new PrismAnimations.Solid(Color.RED);
+ private final PrismAnimations.Solid solidOrange = new PrismAnimations.Solid(Color.ORANGE);
+ private final PrismAnimations.Solid solidGreen = new PrismAnimations.Solid(Color.GREEN);
+ private final PrismAnimations.Solid solidBlue = new PrismAnimations.Solid(Color.BLUE);
+ private final PrismAnimations.Solid solidPurple = new PrismAnimations.Solid(Color.PURPLE); // or MAGENTA if PURPLE is missing
+
+ public LED(hwMap.LedHwMap hardware) {
+ this.hardware = hardware;
+
+ solidRed.setBrightness(50);
+ solidOrange.setBrightness(50);
+ solidGreen.setBrightness(50);
+ solidBlue.setBrightness(50);
+ solidPurple.setBrightness(50);
+ }
+
+ public void update(boolean isScoring, boolean isTurretReady, boolean hasTarget, boolean isAnchorActive) {
+ /*
+ int newLed1State = 0;
+ if (isScoring) {
+ if (isTurretReady) {
+ newLed1State = 2;
+ } else {
+ newLed1State = 1;
+ }
+ }
+
+ if (newLed1State != currentLed1State) {
+ if (newLed1State == 0) {
+ hardware.led1.insertAndUpdateAnimation(LayerHeight.LAYER_0, solidRed);
+ } else if (newLed1State == 1) {
+ hardware.led1.insertAndUpdateAnimation(LayerHeight.LAYER_0, solidOrange);
+ } else if (newLed1State == 2) {
+ hardware.led1.insertAndUpdateAnimation(LayerHeight.LAYER_0, solidGreen);
+ }
+ currentLed1State = newLed1State;
+ }
+
+ int newLed2State = 0;
+ if (hasTarget && isAnchorActive) {
+ newLed2State = 3;
+ } else if (hasTarget) {
+ newLed2State = 1;
+ } else if (isAnchorActive) {
+ newLed2State = 2;
+ }
+
+ if (newLed2State != currentLed2State) {
+ if (newLed2State == 0) {
+ hardware.led2.insertAndUpdateAnimation(LayerHeight.LAYER_0, solidRed);
+ } else if (newLed2State == 1) {
+ hardware.led2.insertAndUpdateAnimation(LayerHeight.LAYER_0, solidGreen);
+ } else if (newLed2State == 2) {
+ hardware.led2.insertAndUpdateAnimation(LayerHeight.LAYER_0, solidBlue);
+ } else if (newLed2State == 3) {
+ hardware.led2.insertAndUpdateAnimation(LayerHeight.LAYER_0, solidPurple);
+ }
+ currentLed2State = newLed2State;
+ }*/
+ }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Lift.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Lift.java
new file mode 100755
index 0000000..dba0302
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Lift.java
@@ -0,0 +1,77 @@
+package org.firstinspires.ftc.teamcode.subsystems;
+
+import org.firstinspires.ftc.teamcode.Hware.hwMap;
+import org.firstinspires.ftc.teamcode.teleOp.Constants;
+
+public class Lift {
+
+ private final hwMap.LiftHwMap h_lift;
+
+ public enum LiftState {
+ BEGIN_LIFT, // Engages PTO and pulls up
+ HOLD_LIFT, // Keeps PTO engaged but reduces power to hold position
+ END_LIFT, // Stops motors and Disengages PTO
+ MANUAL_DOWN // Optional: In case you need to reverse/lower
+ }
+
+ private LiftState currentState = LiftState.END_LIFT;
+
+ public Lift(hwMap.LiftHwMap hardware) {
+ this.h_lift = hardware;
+ }
+
+ public void setLiftState(LiftState state) {
+ this.currentState = state;
+
+ switch (state) {
+ case BEGIN_LIFT:
+ // Engage servos, run motors at lift power
+ setPtoState(
+ Constants.LiftConstants.POS_ENGAGED,
+ Constants.LiftConstants.POWER_LIFTING
+ );
+ break;
+
+ case HOLD_LIFT:
+ // Keep servos engaged, run motors at holding power (gravity compensation)
+ setPtoState(
+ Constants.LiftConstants.POS_ENGAGED,
+ Constants.LiftConstants.POWER_HOLDING
+ );
+ break;
+
+ case MANUAL_DOWN:
+ // Engage servos, reverse motors
+ setPtoState(
+ Constants.LiftConstants.POS_ENGAGED,
+ -Constants.LiftConstants.POWER_LIFTING
+ );
+ break;
+
+ case END_LIFT:
+ default:
+ // Disengage servos, cut power
+ setPtoState(
+ Constants.LiftConstants.POS_DISENGAGED,
+ 0
+ );
+ break;
+ }
+ }
+ private void setPtoState(double servoPos, double motorPower) {
+ // Set Servo Positions
+ /*h_lift.ptoLeft.setPosition(servoPos);
+ h_lift.ptoRight.setPosition(servoPos);
+
+ // Set Drive Motor Powers
+ // Note: You might need to invert the right side depending on your drive train config
+ h_lift.leftFront.setPower(motorPower);
+ h_lift.leftRear.setPower(motorPower);
+ h_lift.rightFront.setPower(motorPower);
+ h_lift.rightRear.setPower(motorPower);*/
+ }
+
+ public LiftState getState() {
+ return currentState;
+ }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/TransferSys.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/TransferSys.java
new file mode 100755
index 0000000..b84b382
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/TransferSys.java
@@ -0,0 +1,106 @@
+package org.firstinspires.ftc.teamcode.subsystems;
+
+import com.qualcomm.robotcore.util.ElapsedTime;
+import org.firstinspires.ftc.teamcode.Hware.hwMap;
+
+public class TransferSys {
+ private final hwMap.TransferHwMap hardware;
+
+ private boolean isTurretReady = false;
+ private boolean limiterClosed = true;
+
+ public double launchDuration = 2; // CHANGEABLE TODO
+
+ public enum TransferState {
+ LAUNCHING, // Gate OPEN
+ IDLE, // Gate CLOSED
+ STOP // Reset
+ }
+
+ private TransferState currentState = TransferState.IDLE;
+ private final ElapsedTime timer = new ElapsedTime();
+ private boolean isLaunchSequenceActive = false;
+
+ public TransferSys(hwMap.TransferHwMap hardware) {
+ this.hardware = hardware;
+ timer.reset();
+ }
+
+ public void setTransferState(TransferState state) {
+ if (this.currentState == state) return;
+ this.currentState = state;
+
+ switch (state) {
+ case LAUNCHING:
+ isLaunchSequenceActive = false;
+ break;
+ case IDLE:
+ closeLimiter();
+ isLaunchSequenceActive = false;
+ break;
+ case STOP:
+ hardware.resetLimiter();
+ limiterClosed = false;
+ break;
+ }
+ }
+
+
+ public void update() {
+ switch (currentState) {
+ case LAUNCHING:
+ if (!isLaunchSequenceActive) {
+ if (isTurretReady) {
+ openLimiter();
+ timer.reset();
+ isLaunchSequenceActive = true;
+ } else {
+ closeLimiter();
+ }
+ }
+ else {
+ openLimiter();
+
+ if (timer.seconds() >= launchDuration) {
+ setTransferState(TransferState.IDLE);
+ }
+ }
+ break;
+
+ case IDLE:
+ case STOP:
+ if (currentState == TransferState.IDLE && !limiterClosed) {
+ closeLimiter();
+ }
+ break;
+ }
+ }
+
+ public void updateTurretReady(boolean turretReady) {
+ this.isTurretReady = turretReady;
+ }
+
+ public void closeLimiter() {
+ hardware.setLimiter(true);
+ limiterClosed = true;
+ }
+
+ public void openLimiter() {
+ hardware.setLimiter(false);
+ limiterClosed = false;
+ }
+
+ public boolean getLimiterState() {
+ return limiterClosed;
+ }
+
+ public TransferState getTransferState() {
+ return currentState;
+ }
+
+ public boolean hasArtifacts() {
+ return hardware.detectArtifactColor(1) != 0 ||
+ hardware.detectArtifactColor(2) != 0 ||
+ hardware.detectArtifactColor(3) != 0;
+ }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Turret.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Turret.java
new file mode 100755
index 0000000..d154c1a
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsystems/Turret.java
@@ -0,0 +1,317 @@
+package org.firstinspires.ftc.teamcode.subsystems;
+
+import static org.firstinspires.ftc.teamcode.tuning.allInOne.OFFSET_GAIN;
+import static org.firstinspires.ftc.teamcode.tuning.allInOne.SERVO_MIN;
+
+import com.pedropathing.geometry.Pose;
+import com.qualcomm.hardware.limelightvision.LLResult;
+import com.qualcomm.robotcore.util.ElapsedTime;
+import com.qualcomm.robotcore.util.Range;
+import org.firstinspires.ftc.teamcode.Hware.hwMap;
+import org.firstinspires.ftc.teamcode.teleOp.Constants;
+
+public class Turret {
+
+ private final hwMap.TurretHwMap hardware;
+ private double turretPower = Constants.TurretConstants.TURRET_POWER_MID;
+ private int alliance = 0;
+ private double distance = 0;
+
+ private double targetVelocity = 0;
+
+ // PID Variables
+ private double integralSum = 0;
+ private double lastError = 0;
+
+ // Targeting Variables
+ private double targetCorrectionOffsetTicks = 0;
+ private double fusedTargetTicks = 0;
+ public double turretPowerRotation;
+ public static double offsetTicks = 0.0;
+
+ public double offsetHood = 0;
+
+ private Pose robotPose;
+
+ private final ElapsedTime loopTimer = new ElapsedTime();
+
+ public enum TurretState {
+ LAUNCH,
+ IDLE,
+ EXTAKE,
+ INTAKING
+ }
+ private TurretState currentState = TurretState.IDLE;
+
+ public Turret(hwMap.TurretHwMap hardware) {
+ this.hardware = hardware;
+ loopTimer.reset();
+ }
+
+ public void setAlliance(int alliance) {
+ this.alliance = alliance;
+ if (alliance == 2) {
+ hardware.setPipeline(Constants.TurretConstants.LIMELIGHT_PIPELINE_RED_TARGET);
+ } else if(alliance == 1) {
+ hardware.setPipeline(Constants.TurretConstants.LIMELIGHT_PIPELINE_BLUE_TARGET);
+ } else {
+ hardware.setPipeline(0);
+ }
+ }
+
+ public void stop() {
+ if (currentState != TurretState.IDLE) {
+ setTurretState(TurretState.IDLE);
+ }
+ hardware.turretOff();
+ hardware.stopTurretRotation();
+ }
+
+ public void setTurretState(TurretState state) {
+ this.currentState = state;
+
+ switch (state) {
+ case IDLE:
+ hardware.turretOff();
+ hardware.stopTurretRotation();
+ integralSum = 0;
+ lastError = 0;
+ break;
+ case EXTAKE:
+ hardware.setTurretPower(Constants.TurretConstants.EXTAKE_POWER);
+ break;
+ case INTAKING:
+ hardware.setTurretPower(Constants.TurretConstants.INTAKE_POWER);
+ break;
+ }
+ }
+
+
+ public TurretState getTurretState() {
+ return currentState;
+ }
+
+ public boolean hasTarget() {
+ return hardware.hasTarget();
+ }
+
+ public void update(boolean manualTracking, boolean manualSAM, Pose currentPose) {
+ double dt = loopTimer.seconds();
+ loopTimer.reset();
+ if (dt < 0.001) dt = 0.001;
+
+ updatePose(currentPose);
+
+ double rawTicks = hardware.getTurretRotationPosition();
+ double currentDegrees = rawTicks / Constants.TurretConstants.TICKS_PER_DEGREE;
+
+ distance = getDistance();
+
+ if (!manualTracking) {
+ calculateHybridTarget(currentPose, currentDegrees, rawTicks);
+ runPIDControl(rawTicks, dt);
+ } else {
+ hardware.stopTurretRotation();
+ }
+
+ if (currentState == TurretState.LAUNCH) {
+ handleLaunchLogic();
+ }
+ }
+
+ public void updateAuton(int alliance, Pose currentPose, double offset) {
+ updatePose(currentPose);
+ double dt = loopTimer.seconds();
+ loopTimer.reset();
+ if (dt < 0.001) dt = 0.001;
+
+ setAlliance(alliance);
+
+ double rawTicks = hardware.getTurretRotationPosition();
+ double currentDegrees = rawTicks / Constants.TurretConstants.TICKS_PER_DEGREE;
+
+ distance = getDistance();
+
+ calculateHybridTarget(currentPose, currentDegrees, rawTicks);
+ runPIDControl(rawTicks, dt);
+ handleLaunchLogic(-offset);
+ }
+
+ private void handleLaunchLogic() {
+ if (distance > 0) {
+ targetVelocity = getFlywheelVelocity(distance);
+ hardware.setTurretVelocity(targetVelocity);
+
+ double calculatedHoodPos = getHoodPOS(distance);
+ double currentVel = hardware.getFlywheelVelocities()[0];
+ double dropThreshold = targetVelocity * 0.95;
+ double recoilOffset = 0.0;
+
+ recoilOffset = (currentVel - targetVelocity) * 0.000004;
+
+ setHoodPos(calculatedHoodPos + recoilOffset);
+ } else {
+ hardware.setTurretVelocity(0);
+ }
+ }
+
+ private void handleLaunchLogic(double offset) {
+ if (distance > 0) {
+ targetVelocity = getFlywheelVelocity(distance);
+ hardware.setTurretVelocity(targetVelocity);
+
+ double calculatedHoodPos = getHoodPOS(distance);
+ double currentVel = hardware.getFlywheelVelocities()[0];
+ double dropThreshold = targetVelocity * 0.95;
+ double recoilOffset = 0.0;
+
+ recoilOffset = (currentVel - targetVelocity) * 0.000004;
+
+ setHoodPos(calculatedHoodPos + recoilOffset);
+ } else {
+ hardware.setTurretVelocity(0);
+ }
+ }
+
+ private void calculateHybridTarget(Pose robotPose, double currentDegrees, double currentTicks) {
+ double odoTargetTicks = calculateOdometryTargetTicks(robotPose);
+ LLResult result = hardware.getLimelightResult();
+
+ if (result != null && result.isValid()) {
+ double rawErrorTicks = getErrorTicks(currentDegrees, result, odoTargetTicks);
+ targetCorrectionOffsetTicks = (targetCorrectionOffsetTicks * (1.0 - Constants.TurretConstants.OFFSET_SMOOTHING))
+ + (rawErrorTicks * Constants.TurretConstants.OFFSET_SMOOTHING);
+ }
+
+ double rawTarget = odoTargetTicks + targetCorrectionOffsetTicks;
+ fusedTargetTicks = Range.clip(rawTarget, Constants.TurretConstants.SOFT_LIMIT_NEG, Constants.TurretConstants.SOFT_LIMIT_POS);
+ }
+
+ private double calculateOdometryTargetTicks(Pose robotPose) {
+ double targetX = (alliance == 2) ? Constants.TurretConstants.GOAL_RED_X : Constants.TurretConstants.GOAL_BLUE_X;
+ double targetY = (alliance == 2) ? Constants.TurretConstants.GOAL_RED_Y : Constants.TurretConstants.GOAL_BLUE_Y;
+
+ double dx = targetX - robotPose.getX();
+ double dy = targetY - robotPose.getY();
+
+ double targetFieldHeading = Math.atan2(dy, dx);
+ double robotHeading = robotPose.getHeading();
+
+ double relativeRad = targetFieldHeading - robotHeading;
+
+ while (relativeRad > Math.PI) relativeRad -= (2 * Math.PI);
+ while (relativeRad < -Math.PI) relativeRad += (2 * Math.PI);
+
+ double relativeDegrees = Math.toDegrees(relativeRad);
+ double staticOffset = (alliance == 2) ? Constants.TurretConstants.RED_TARGET_OFFSET_DEGREES : Constants.TurretConstants.BLUE_TARGET_OFFSET_DEGREES;
+
+ return (relativeDegrees + staticOffset) * Constants.TurretConstants.TICKS_PER_DEGREE;
+ }
+
+ private double getErrorTicks(double currentDegrees, LLResult result, double odoTargetTicks) {
+ double tx = result.getTx();
+ double visionTargetDegrees = currentDegrees + (tx * Constants.TurretConstants.LL_LOGIC_MULTIPLIER);
+ double finalVisionDegrees = visionTargetDegrees + Constants.TurretConstants.LL_TARGET_OFFSET_DEGREES;
+ double visionTargetTicks = finalVisionDegrees * Constants.TurretConstants.TICKS_PER_DEGREE;
+
+ double rawErrorTicks = visionTargetTicks - odoTargetTicks;
+ double ticksPerRev = 360.0 * Constants.TurretConstants.TICKS_PER_DEGREE;
+
+ while (rawErrorTicks > (ticksPerRev / 2)) rawErrorTicks -= ticksPerRev;
+ while (rawErrorTicks < -(ticksPerRev / 2)) rawErrorTicks += ticksPerRev;
+ return rawErrorTicks;
+ }
+
+ private void runPIDControl(double currentTicks, double dt) {
+ fusedTargetTicks = fusedTargetTicks + offsetTicks;
+ double error = fusedTargetTicks - currentTicks;
+ double derivative = (error - lastError) / dt;
+
+ if (Math.abs(error) < (15 * Constants.TurretConstants.TICKS_PER_DEGREE)) {
+ integralSum += (error * dt);
+ } else {
+ integralSum = 0;
+ }
+
+ double integralTerm = Range.clip(Constants.TurretConstants.KI * integralSum, -Constants.TurretConstants.MAX_INTEGRAL, Constants.TurretConstants.MAX_INTEGRAL);
+ double output = (Constants.TurretConstants.KP * error) + integralTerm + (Constants.TurretConstants.KD * derivative);
+ output = Range.clip(output, -Constants.TurretConstants.MAX_POWER, Constants.TurretConstants.MAX_POWER);
+
+ hardware.setTurretRotationPower(output);
+ turretPowerRotation = output;
+ lastError = error;
+ }
+
+ public boolean hasReachedVelocity() {
+ double currentVel = hardware.getFlywheelVelocities()[0];
+
+ if (Math.abs(targetVelocity) > 1000) {
+ double absCurrent = Math.abs(currentVel);
+ double absTarget = Math.abs(targetVelocity);
+
+ double error = Math.abs(absCurrent - absTarget);
+ double tolerance = absTarget * 0.03;
+
+ return error <= tolerance;
+ }
+ return false;
+ }
+
+ public boolean isTurretReady() {
+ return hasReachedVelocity();
+ }
+
+
+ public void updatePose(Pose robotPose) {
+ this.robotPose = robotPose;
+ }
+
+ public double getDistance() {
+ if (hardware.hasTarget()) {
+ double ty = hardware.getTargetTY();
+ double angleToGoalDegrees = Constants.TurretConstants.CAMERA_MOUNT_ANGLE_DEGREES + ty;
+ double angleToGoalRadians = Math.toRadians(angleToGoalDegrees);
+
+ return (Constants.TurretConstants.GOAL_HEIGHT_INCHES - Constants.TurretConstants.CAMERA_HEIGHT_INCHES) / Math.tan(angleToGoalRadians);
+ }
+ else if (robotPose != null) {
+ double targetX = (alliance == 2) ? Constants.TurretConstants.GOAL_RED_X : Constants.TurretConstants.GOAL_BLUE_X;
+ double targetY = (alliance == 2) ? Constants.TurretConstants.GOAL_RED_Y : Constants.TurretConstants.GOAL_BLUE_Y;
+
+ return Math.hypot(targetX - robotPose.getX(), targetY - robotPose.getY());
+ }
+ return 0.0;
+ }
+
+ private double calculateHoodOffset(double target, double current) {
+ return (current - target) * 0.0002;
+ }
+
+ private double getFlywheelVelocity(double dist) {
+ return (0.0000119972 * Math.pow((dist), 4) - 0.00249603 * Math.pow(dist, 3) + 0.179513 * Math.pow(dist, 2) + (0.0519688 * dist) + 1489.8908);
+ }
+
+ private double getHoodPOS(double dist) {
+ return ((-(3.62429 * Math.pow(10, -8)) * Math.pow(dist, 4)) + 0.00000638975 * Math.pow(dist, 3) - (0.000153252 * Math.pow(dist, 2)) + (-0.0197027 * dist) + 1.40511);
+ }
+
+ public void setHoodPos(double pos) {
+ hardware.setHoodPos(Range.clip(pos, Constants.TurretConstants.SERVO_MIN, Constants.TurretConstants.SERVO_MAX));
+ }
+
+ public void setLaunchPower(double power) {
+ this.turretPower = power;
+ }
+
+ public void updateOffsetTicks(double change) {
+ offsetTicks += change;
+ }
+
+ public double getTurretPower() {
+ return turretPower;
+ }
+
+ public double getTargetVelocity() {
+ return targetVelocity;
+ }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleOp/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleOp/Constants.java
new file mode 100755
index 0000000..c402f6e
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleOp/Constants.java
@@ -0,0 +1,156 @@
+package org.firstinspires.ftc.teamcode.teleOp;
+
+import com.bylazar.configurables.annotations.Configurable;
+import com.pedropathing.geometry.Pose;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
+import com.qualcomm.robotcore.hardware.Servo;
+@Configurable
+public class Constants {
+
+ public static class FieldConstants {
+ // Red Basket (Top Right)
+ public static final Pose RED_BASKET = new Pose(136, 139, 0);
+ // Blue Basket (Top Left)
+ public static final Pose BLUE_BASKET = new Pose(8, 139, 0);
+ }
+
+ // Drive Constants
+ public static class DriveConstants {
+ public static final String FRONT_LEFT_MOTOR = "fl";
+ public static final String FRONT_RIGHT_MOTOR = "fr";
+ public static final String BACK_LEFT_MOTOR = "bl";
+ public static final String BACK_RIGHT_MOTOR = "br";
+ public static final DcMotorSimple.Direction FRONT_LEFT_DIRECTION = DcMotorSimple.Direction.REVERSE;
+ public static final DcMotorSimple.Direction FRONT_RIGHT_DIRECTION = DcMotorSimple.Direction.FORWARD;
+ public static final DcMotorSimple.Direction BACK_LEFT_DIRECTION = DcMotorSimple.Direction.REVERSE;
+ public static final DcMotorSimple.Direction BACK_RIGHT_DIRECTION = DcMotorSimple.Direction.FORWARD;
+ public static final double TRACK_WIDTH = 13.5;
+ public static final double WHEEL_BASE = 13.5;
+ public static final double WHEEL_DIAMETER = 4.0;
+ public static final double WHEEL_CIRCUMFERENCE = WHEEL_DIAMETER * Math.PI;
+ public static final double TICKS_PER_REVOLUTION = 537.6;
+
+ // Drive characteristics
+ public static final double MAX_VELOCITY = 30.0; // inches per second
+ public static final double MAX_ANGULAR_VELOCITY = Math.toRadians(180.0); // radians per second
+ public static final double MAX_ACCELERATION = 30.0; // inches per second squared
+
+ public static final double STOP_SPEED_MULTIPLIER = 0.0;
+ public static final double PRECISION_SPEED_MULTIPLIER = 0.3;
+ public static final double NORMAL_SPEED_MULTIPLIER = 0.6;
+ public static final double TURBO_SPEED_MULTIPLIER = 1.0;
+ }
+
+ public static class IntakeConstants {
+ public static final String FRONT_INTAKE_MOTOR = "intake";
+
+ public static final DcMotorSimple.Direction INTAKE_DIRECTION = DcMotorSimple.Direction.REVERSE;
+
+ public static DcMotor.ZeroPowerBehavior INTAKE_ZERO_POWER_BEHAVIOR = DcMotor.ZeroPowerBehavior.BRAKE;
+ public static DcMotor.RunMode INTAKE_RUNMODE = DcMotor.RunMode.RUN_WITHOUT_ENCODER;
+
+ public static final double INTAKE_POWER = 1;
+ public static final double EXTAKE_POWER = -0.8;
+ public static final double LAUNCH_POWER = 1;
+ public static final double IDLE_POWER = 0;
+ }
+
+ public static class LiftConstants {
+ // Hardware Map Names
+ public static final String PTO_LEFT = "ptoLeft";
+ public static final String PTO_RIGHT = "ptoRight";
+
+ // Directions
+ public static final Servo.Direction PTO_LEFT_DIR = Servo.Direction.FORWARD;
+ public static final Servo.Direction PTO_RIGHT_DIR = Servo.Direction.REVERSE;
+
+ public static final double POS_ENGAGED = 0.8;
+ public static final double POS_DISENGAGED = 0.2;
+
+ // Motor Powers
+ public static final double POWER_LIFTING = 0.9;
+ public static final double POWER_HOLDING = 0.1;
+ }
+
+ public static class TransferConstants {
+ public static final String LIMITER_SERVO = "activeTransfer";
+
+ public static final Servo.Direction LIMITER_SERVO_DIR = Servo.Direction.FORWARD;
+
+
+ public static final String INDEX_SENSOR_A = "colorA";
+ public static final String INDEX_SENSOR_B = "colorB";
+ public static final String INDEX_SENSOR_C = "colorC";
+
+ public static final double LIMIT_POS = 0.99;
+
+ public static final double UNLOCK_POS = 0.81;
+
+ }
+
+ public static class TurretConstants {
+ public static final double TICKS_PER_REV_MOTOR = 384.5;
+ public static final double EXTERNAL_GEAR_RATIO = 1.315994798439532;
+ public static final double TICKS_PER_DEGREE = (TICKS_PER_REV_MOTOR * EXTERNAL_GEAR_RATIO) / 360.0;
+
+ public static final double KP = 0.028;
+ public static final double KI = 0.0;
+ public static final double KD = 0.00055;
+ public static final double MAX_POWER = 0.6;
+ public static final double MAX_INTEGRAL = 0.5;
+
+ public static final double GOAL_RED_X = 138;
+ public static final double GOAL_RED_Y = 138;
+ public static final double GOAL_BLUE_X = 6;
+ public static final double GOAL_BLUE_Y = 138;
+
+ public static final double RED_TARGET_OFFSET_DEGREES = 14;
+ public static final double BLUE_TARGET_OFFSET_DEGREES = 17;
+ public static final double LL_TARGET_OFFSET_DEGREES = -6;
+ public static final double LL_LOGIC_MULTIPLIER = -1.0;
+ public static final double OFFSET_SMOOTHING = 0.3;
+
+ public static final double SOFT_LIMIT_NEG = -100.0;
+ public static final double SOFT_LIMIT_POS = 250.0;
+
+ public static final double HOOD_POS_CLOSE = 0.3;
+ public static final double HOOD_POS_FAR = 0.7;
+ public static final double HOOD_POS_MID = 0.5;
+
+ public static double HOOD_OFFSET = 0.3;
+
+ public static final double MANUAL_TURRET_SPEED_DEG = 1.0;
+
+ public static final double TURRET_POWER_MID = -0.84;
+ public static final double TURRET_POWER_MAX = -1;
+ public static final double TURRET_POWER_LOW = -0.7;
+
+ public static final double EXTAKE_POWER = 0.3;
+ public static final double INTAKE_POWER = -0.04;
+
+ public static final String TURRET_ROTATION_MOTOR = "turretRotation";
+ public static final String HOOD_TURRET_SERVO = "hoodServo";
+ public static final Servo.Direction HOOD_SERVO_DIR = Servo.Direction.FORWARD;
+
+ public static final String TURRET_RIGHT_MOTOR = "rturret";
+ public static final String TURRET_LEFT_MOTOR = "lturret";
+
+ public static final DcMotorSimple.Direction TURRET_MOTOR_R_DIRECTION = DcMotorSimple.Direction.REVERSE;
+ public static final DcMotorSimple.Direction TURRET_MOTOR_L_DIRECTION = DcMotorSimple.Direction.REVERSE;
+
+ public static final int LIMELIGHT_PIPELINE_MOTIF = 0;
+ public static final int LIMELIGHT_PIPELINE_BLUE_TARGET = 1;
+ public static final int LIMELIGHT_PIPELINE_RED_TARGET = 2;
+
+ public static final double CAMERA_HEIGHT_INCHES = 15.5;
+ public static final double GOAL_HEIGHT_INCHES = 29.5;
+ public static final double CAMERA_MOUNT_ANGLE_DEGREES = 2.0;
+
+ public static final double SERVO_MAX = 0.7;
+ public static final double SERVO_MIN = 0.26;
+ }
+
+
+
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleOp/SOLOTeleOP.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleOp/SOLOTeleOP.java
new file mode 100755
index 0000000..1cc35e3
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleOp/SOLOTeleOP.java
@@ -0,0 +1,224 @@
+package org.firstinspires.ftc.teamcode.teleOp;
+
+import com.pedropathing.follower.Follower;
+import com.pedropathing.geometry.Pose;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import org.firstinspires.ftc.teamcode.Hware.hwMap;
+import org.firstinspires.ftc.teamcode.pedroPathing.Constants;
+import org.firstinspires.ftc.teamcode.subsystems.DriveTrain;
+import org.firstinspires.ftc.teamcode.subsystems.Intake;
+import org.firstinspires.ftc.teamcode.subsystems.TransferSys;
+import org.firstinspires.ftc.teamcode.util.AutoTransfer;
+import org.firstinspires.ftc.teamcode.util.FPSCounter;
+import com.qualcomm.hardware.lynx.LynxModule;
+import java.util.List;
+public class SOLOTeleOP {
+
+ @TeleOp(name = "Red SOLO", group = "FINAL")
+ public static class RedSOLO extends BaseSoloTeleOp {
+ public RedSOLO() {
+ super(2);
+ }
+ }
+ @TeleOp(name = "Blue SOLO", group = "FINAL")
+ public static class BlueSOLO extends BaseSoloTeleOp {
+ public BlueSOLO() {
+ super(1);
+ }
+ }
+
+ public static abstract class BaseSoloTeleOp extends LinearOpMode {
+
+ private StateMachine stateMachine;
+ private Follower follower;
+ private int currentAlliance;
+ private final int initialAlliance;
+ private boolean isScoringActive = false;
+ protected List allHubs;
+
+ private final Pose DEFAULT_RED_START = new Pose(129.25, 112, Math.toRadians(90));
+ private final Pose DEFAULT_BLUE_START = new Pose(14.75, 112, Math.toRadians(90));
+
+ protected FPSCounter fpsCounter = new FPSCounter();
+
+ public BaseSoloTeleOp(int alliance) {
+ this.initialAlliance = alliance;
+ this.currentAlliance = 0;
+ }
+
+ @Override
+ public void runOpMode() throws InterruptedException {
+ initializeRobot();
+
+ if (AutoTransfer.isAutonRan) {
+ currentAlliance = AutoTransfer.alliance;
+ if (currentAlliance != initialAlliance) {
+ telemetry.addLine("!!! WARNING: AUTON ALLIANCE DOES NOT MATCH TELEOP ALLIANCE!");
+ }
+ follower.setStartingPose(AutoTransfer.transferPose);
+ telemetry.addLine("AUTON DATA LOADED");
+ } else {
+ currentAlliance = initialAlliance;
+ if (currentAlliance == 2) follower.setStartingPose(DEFAULT_RED_START);
+ else follower.setStartingPose(DEFAULT_BLUE_START);
+ telemetry.addLine("MANUAL START - NO AUTON DATA");
+ }
+
+ stateMachine.getTurret().setAlliance(currentAlliance);
+
+ telemetry.addData("Alliance", currentAlliance == 2 ? "RED" : "BLUE");
+ telemetry.update();
+
+ waitForStart();
+ configureStartSettings();
+
+ fpsCounter.reset();
+
+ while (opModeIsActive()) {
+ fpsCounter.startLoop();
+
+ for (LynxModule hub : allHubs) {
+ hub.clearBulkCache();
+ }
+
+
+ stateMachine.getDriveTrain().teleopDrive(
+ gamepad1.left_stick_x, -gamepad1.left_stick_y, gamepad1.right_stick_x
+ );
+
+ follower.update();
+ Pose currentPose = follower.getPose();
+
+ //Pose healingPose = stateMachine.getTurret().getSelfHealingPose();
+ //if (healingPose != null) {
+ // follower.setPose(healingPose);
+ //}
+
+ handleGlobalLogic();
+ handleDriverControls();
+
+ stateMachine.update(false, false, currentPose);
+
+
+ updateTelemetry(currentPose);
+ }
+ stateMachine.setRobotState(RobotState.ESTOP);
+ }
+
+ private void initializeRobot() {
+ follower = Constants.createFollower(hardwareMap);
+
+ allHubs = hardwareMap.getAll(LynxModule.class);
+ for (LynxModule hub : allHubs) {
+ hub.setBulkCachingMode(LynxModule.BulkCachingMode.MANUAL);
+ }
+
+ stateMachine = new StateMachine(
+ new hwMap.DriveHwMap(hardwareMap),
+ new hwMap.IntakeHwMap(hardwareMap),
+ new hwMap.LiftHwMap(hardwareMap),
+ new hwMap.TransferHwMap(hardwareMap),
+ new hwMap.TurretHwMap(hardwareMap),
+ new hwMap.LedHwMap(hardwareMap),
+ AutoTransfer.motifPattern,
+ currentAlliance
+
+ );
+ stateMachine.getTurret().setAlliance(0);
+
+ stateMachine.getTransfer().setTransferState(TransferSys.TransferState.STOP);
+
+
+ telemetry.addLine("DSS (Single Player) Initialized.");
+ telemetry.update();
+ }
+
+ private void configureStartSettings() {
+ stateMachine.setRobotState(RobotState.TELEOP);
+ stateMachine.getDriveTrain().setDriveState(DriveTrain.DriveState.NORMAL);
+ stateMachine.getTurret().setLaunchPower(org.firstinspires.ftc.teamcode.teleOp.Constants.TurretConstants.TURRET_POWER_MID);
+ stateMachine.getTransfer().closeLimiter();
+ stateMachine.initUpdate();
+ }
+
+ private void handleGlobalLogic() {
+ if (currentAlliance == 0 && stateMachine.isMotifEdited()) {
+ currentAlliance = initialAlliance;
+ stateMachine.getTurret().setAlliance(currentAlliance);
+ }
+
+ if (gamepad1.startWasPressed() && currentAlliance != 0) {
+ currentAlliance = (currentAlliance == 1) ? 2 : 1;
+ stateMachine.getTurret().setAlliance(currentAlliance);
+ }
+
+ if (gamepad1.backWasPressed()) {
+ stateMachine.emergencyStop();
+ }
+
+ if (stateMachine.getCurrentGameState() != GameState.SCORING) {
+ if (!stateMachine.getTurret().hasReachedVelocity()) {
+ int rumbleDuration = (int) fpsCounter.getCurrentLoopTime() + 2;
+ gamepad1.rumble(rumbleDuration);
+ }
+ }
+
+ if (gamepad1.psWasPressed()) {
+ stateMachine.toggleAnchor();
+ }
+ }
+
+ private void handleDriverControls() {
+
+ if (gamepad1.dpad_up) stateMachine.getDriveTrain().setDriveState(DriveTrain.DriveState.TURBO);
+ else if (gamepad1.dpad_left) stateMachine.getDriveTrain().setDriveState(DriveTrain.DriveState.NORMAL);
+ else if (gamepad1.dpad_down) stateMachine.getDriveTrain().setDriveState(DriveTrain.DriveState.PRECISION);
+
+ if (stateMachine.getCurrentGameState() != GameState.SCORING) {
+ if (gamepad1.right_trigger > 0.1) {
+ stateMachine.getIntake().setIntakeState(Intake.IntakeState.INTAKE);
+ } else if (gamepad1.left_trigger > 0.1) {
+ stateMachine.getIntake().setIntakeState(Intake.IntakeState.EXTAKE);
+ } else {
+ if (stateMachine.getIntake().getCurrentState() == Intake.IntakeState.INTAKE ||
+ stateMachine.getIntake().getCurrentState() == Intake.IntakeState.EXTAKE) {
+ stateMachine.getIntake().setIntakeState(Intake.IntakeState.IDLE);
+ }
+ }
+ }
+
+ if (gamepad1.yWasPressed()) {
+ isScoringActive = !isScoringActive;
+ if (isScoringActive) stateMachine.setGameState(GameState.SCORING);
+ else if (stateMachine.getCurrentGameState() == GameState.SCORING) stateMachine.setGameState(GameState.IDLE);
+ }
+
+ if (gamepad1.right_bumper) stateMachine.getTurret().updateOffsetTicks(0.06);
+ if (gamepad1.left_bumper) stateMachine.getTurret().updateOffsetTicks(-0.06);
+
+
+ }
+
+ private void updateTelemetry(Pose p) {
+ telemetry.addData("OpMod ev", "SOLO TeleOp");
+ telemetry.addData("Avg FPS", "%.2f", fpsCounter.getAverageFps());
+
+ telemetry.addLine("▰▰▰▰▰▰▰ SYSTEMS ▰▰▰▰▰▰▰");
+ telemetry.addData("State", stateMachine.getCurrentRobotState());
+ telemetry.addData("Game Mode", stateMachine.getCurrentGameState());
+ telemetry.addData("Scoring", isScoringActive ? "ACTIVE" : "IDLE");
+ telemetry.addData("Anchor", stateMachine.getDriveTrain().isAnchorActive() ? "LOCKED" : "FREE");
+ telemetry.addData("Alliance", currentAlliance == 2 ? "RED" : "BLUE");
+ telemetry.addData("Drive Speed", stateMachine.getDriveTrain().getDriveState());
+
+ telemetry.addLine("▰▰▰▰▰▰▰ TRACKING ▰▰▰▰▰▰▰");
+ telemetry.addData("Pose", "X:%.1f Y:%.1f H:%.1f°", p.getX(), p.getY(), Math.toDegrees(p.getHeading()));
+ telemetry.addData("Distance", "%.1f in", stateMachine.getTurret().getDistance());
+ telemetry.addData("Turret Ready", stateMachine.getTurret().isTurretReady());
+
+ telemetry.update();
+ }
+ }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleOp/StateMachine.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleOp/StateMachine.java
new file mode 100755
index 0000000..8cacfc4
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleOp/StateMachine.java
@@ -0,0 +1,192 @@
+package org.firstinspires.ftc.teamcode.teleOp;
+
+import com.pedropathing.geometry.Pose; // ADDED
+import com.qualcomm.robotcore.hardware.HardwareMap;
+
+import org.firstinspires.ftc.teamcode.Hware.hwMap;
+import org.firstinspires.ftc.teamcode.subsystems.DriveTrain;
+import org.firstinspires.ftc.teamcode.subsystems.LED;
+import org.firstinspires.ftc.teamcode.subsystems.Lift;
+import org.firstinspires.ftc.teamcode.subsystems.TransferSys;
+import org.firstinspires.ftc.teamcode.subsystems.Intake;
+import org.firstinspires.ftc.teamcode.subsystems.Turret;
+
+enum RobotState {
+ INIT, TELEOP, ESTOP
+}
+enum GameState {
+ INTAKING, EXTAKING, SCORING, LIFTING, IDLE
+}
+
+public class StateMachine {
+
+ private RobotState currentRobotState = RobotState.INIT;
+ private GameState currentGameState = GameState.IDLE;
+ private final DriveTrain m_driveTrain;
+ private final Intake m_intake;
+ private final TransferSys m_transfer;
+ private final Turret m_turret;
+ private final Lift m_lift;
+
+ private final LED m_led;
+
+ boolean isMotifEdited = false;
+ private long lastIndexAllArtifactsTime = 0;
+
+ public boolean isIntakeLaunching = false;
+
+ public StateMachine(hwMap.DriveHwMap h_driveTrain, hwMap.IntakeHwMap h_intake, hwMap.LiftHwMap h_lift, hwMap.TransferHwMap h_transfer, hwMap.TurretHwMap h_turret, hwMap.LedHwMap h_led, int initialMotif, int alliance) {
+ this.m_driveTrain = new DriveTrain(h_driveTrain);
+ this.m_intake = new Intake(h_intake);
+ this.m_lift = new Lift(h_lift);
+ this.m_transfer = new TransferSys(h_transfer);
+ this.m_turret = new Turret(h_turret);
+ this.m_led = new LED(h_led);
+
+
+ setRobotState(RobotState.INIT);
+ setGameState(GameState.IDLE);
+ }
+
+ public void setRobotState(RobotState newState) {
+ if (this.currentRobotState == newState) return;
+ handleRobotStateExit(this.currentRobotState);
+ this.currentRobotState = newState;
+ handleRobotStateEntry(newState);
+ }
+
+ public void setGameState(GameState newState) {
+ if (this.currentGameState == newState) return;
+ handleGameStateExit(this.currentGameState);
+ this.currentGameState = newState;
+ handleGameStateEntry(newState);
+ }
+
+ private void handleGameStateExit(GameState oldState) {
+ switch (oldState) {
+ case SCORING:
+ m_transfer.setTransferState(TransferSys.TransferState.IDLE);
+ m_turret.setTurretState(Turret.TurretState.IDLE);
+ m_intake.setIntakeState(Intake.IntakeState.IDLE);
+ isIntakeLaunching = false;
+ //m_driveTrain.setDriveState(DriveTrain.DriveState.NORMAL);
+ m_driveTrain.stopAnchor();
+ break;
+ case LIFTING:
+ //m_driveTrain.setDriveState(DriveTrain.DriveState.NORMAL);
+ break;
+ }
+ }
+
+ public void update(boolean manualSAM, boolean manualTracking, Pose currentPose) {
+ m_driveTrain.update();
+
+ m_turret.update(manualTracking, manualSAM, currentPose);
+
+ if (currentGameState == GameState.SCORING) {
+ m_transfer.updateTurretReady(m_turret.hasReachedVelocity());
+ if (m_turret.hasReachedVelocity()) {
+ isIntakeLaunching = true;
+ m_intake.setIntakeState(Intake.IntakeState.LAUNCH);
+ }
+ }
+
+ m_transfer.update();
+
+ m_led.update(
+ currentGameState == GameState.SCORING,
+ m_turret.hasReachedVelocity(),
+ m_turret.hasTarget(),
+ m_driveTrain.isAnchorActive()
+ );
+
+ if (currentGameState == GameState.SCORING) {
+ if (m_transfer.getTransferState() != TransferSys.TransferState.LAUNCHING) {
+ setGameState(GameState.IDLE);
+ }
+ }
+ }
+
+ public void initUpdate() {
+ m_led.update(false, false, false, false);
+ }
+
+ private void handleGameStateEntry(GameState newState) {
+ switch (newState) {
+ case INTAKING:
+ //m_driveTrain.setDriveState(DriveTrain.DriveState.TURBO);
+ m_intake.setIntakeState(Intake.IntakeState.INTAKE);
+ break;
+ case EXTAKING:
+ //m_driveTrain.setDriveState(DriveTrain.DriveState.TURBO);
+ m_intake.setIntakeState(Intake.IntakeState.EXTAKE);
+ break;
+ case SCORING:
+ m_transfer.setTransferState(TransferSys.TransferState.LAUNCHING);
+ m_turret.setTurretState(Turret.TurretState.LAUNCH);
+ break;
+ case LIFTING:
+ //m_driveTrain.setDriveState(DriveTrain.DriveState.STOP);
+ m_intake.setIntakeState(Intake.IntakeState.IDLE);
+ m_transfer.setTransferState(TransferSys.TransferState.IDLE);
+ m_turret.setTurretState(Turret.TurretState.IDLE);
+ m_lift.setLiftState(Lift.LiftState.BEGIN_LIFT);
+ break;
+ case IDLE:
+ m_intake.setIntakeState(Intake.IntakeState.IDLE);
+ //m_driveTrain.setDriveState(DriveTrain.DriveState.NORMAL);
+ break;
+ }
+ }
+
+ public void holdLift() {
+ if (currentGameState == GameState.LIFTING) {
+ m_lift.setLiftState(Lift.LiftState.HOLD_LIFT);
+ }
+ }
+
+ public void toggleAnchor() {
+ if (m_driveTrain.isAnchorActive()) {
+ m_driveTrain.stopAnchor();
+ } else {
+ m_driveTrain.startAnchor();
+ }
+ }
+
+
+ private void handleRobotStateExit(RobotState oldState) {
+ switch (oldState) {
+ case TELEOP:
+ m_driveTrain.stop();
+ break;
+ case ESTOP:
+ break;
+ }
+ }
+
+ private void handleRobotStateEntry(RobotState newState) {
+ switch (newState) {
+ case TELEOP:
+ m_driveTrain.setDriveState(DriveTrain.DriveState.NORMAL);
+ m_intake.setIntakeState(Intake.IntakeState.IDLE);
+ break;
+ case ESTOP:
+ m_driveTrain.setDriveState(DriveTrain.DriveState.STOP);
+ m_intake.setIntakeState(Intake.IntakeState.STOP);
+ break;
+ }
+ }
+
+ public void emergencyStop() {
+ setRobotState(RobotState.ESTOP);
+ setGameState(GameState.IDLE);
+ }
+
+ public DriveTrain getDriveTrain() { return m_driveTrain; }
+ public Intake getIntake() { return m_intake; }
+ public Turret getTurret() { return m_turret; }
+ public TransferSys getTransfer() { return m_transfer; }
+ public RobotState getCurrentRobotState() { return currentRobotState; }
+ public GameState getCurrentGameState() { return currentGameState; }
+ public boolean isMotifEdited() { return isMotifEdited; }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleOp/finalTeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleOp/finalTeleOp.java
new file mode 100755
index 0000000..8c83bfa
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleOp/finalTeleOp.java
@@ -0,0 +1,289 @@
+package org.firstinspires.ftc.teamcode.teleOp;
+
+import com.pedropathing.follower.Follower;
+import com.pedropathing.geometry.Pose;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.util.Range;
+
+import org.firstinspires.ftc.teamcode.Hware.hwMap;
+import org.firstinspires.ftc.teamcode.pedroPathing.Constants;
+import org.firstinspires.ftc.teamcode.subsystems.DriveTrain;
+import org.firstinspires.ftc.teamcode.subsystems.Intake;
+import org.firstinspires.ftc.teamcode.util.AutoTransfer;
+import org.firstinspires.ftc.teamcode.util.FPSCounter;
+import com.qualcomm.hardware.lynx.LynxModule;
+import java.util.List;
+
+public class finalTeleOp {
+
+ @TeleOp(name = "Red Final", group = "FINAL")
+ public static class RedFinal extends BaseFinalTeleOp {
+ public RedFinal() {
+ super(2);
+ }
+ }
+
+ @TeleOp(name = "Blue Final", group = "FINAL")
+ public static class BlueFinal extends BaseFinalTeleOp {
+ public BlueFinal() {
+ super(1);
+ }
+ }
+
+ public static abstract class BaseFinalTeleOp extends LinearOpMode {
+
+ protected StateMachine stateMachine;
+ protected Follower follower;
+
+ protected int currentAlliance;
+ protected final int initialAlliance;
+ protected boolean launching = false;
+ protected boolean manualTracking = false;
+ protected boolean manualSAM = false;
+
+ protected double hoodPosition = 0.5;
+
+ protected List allHubs;
+
+ // Default Start Poses for manual testing (if Auton didn't run)
+
+ private final Pose DEFAULT_RED_START = new Pose(129.25, 112, Math.toRadians(90));
+ private final Pose DEFAULT_BLUE_START = new Pose(14.75, 112, Math.toRadians(90));
+
+ protected FPSCounter fpsCounter = new FPSCounter();
+
+ public BaseFinalTeleOp(int alliance) {
+ this.initialAlliance = alliance;
+ this.currentAlliance = 0;
+ }
+
+ @Override
+ public void runOpMode() throws InterruptedException {
+ initializeRobot();
+
+ if (AutoTransfer.isAutonRan) {
+ currentAlliance = AutoTransfer.alliance;
+ if (currentAlliance != initialAlliance) {
+ telemetry.addLine("WARNING: AUTON ALLIANCE DOES NOT MATCH TELEOP ALLIANCE!");
+ }
+ follower.setStartingPose(AutoTransfer.transferPose);
+ telemetry.addLine("AUTON DATA LOADED");
+ } else {
+ currentAlliance = initialAlliance;
+ if (currentAlliance == 2) follower.setStartingPose(DEFAULT_RED_START);
+ else follower.setStartingPose(DEFAULT_BLUE_START);
+ telemetry.addLine("MANUAL START - NO AUTON DATA");
+ }
+
+ stateMachine.getTurret().setAlliance(currentAlliance);
+
+ telemetry.addData("Alliance", currentAlliance == 2 ? "RED" : "BLUE");
+ telemetry.update();
+
+ waitForStart();
+ configureStartSettings();
+
+ fpsCounter.reset();
+
+ while (opModeIsActive()) {
+ fpsCounter.startLoop();
+
+ if (stateMachine.getCurrentGameState() != GameState.SCORING) {
+ stateMachine.getTransfer().closeLimiter();
+ } else if (stateMachine.getCurrentGameState() == GameState.SCORING && stateMachine.getTurret().isTurretReady()) {
+ stateMachine.getTransfer().openLimiter();
+ }
+
+ for (LynxModule hub : allHubs) {
+ hub.clearBulkCache();
+ }
+
+ follower.update();
+ Pose currentPose = follower.getPose();
+
+ handleGlobalLogic();
+ handleDriverInput();
+ handleOperatorInput();
+
+ stateMachine.update(manualSAM, manualTracking, currentPose);
+ //Pose healingPose = stateMachine.getTurret().getSelfHealingPose();
+
+ // if (healingPose != null) {
+ // follower.setPose(healingPose);
+ // }
+
+ if (gamepad2.a) {
+ stateMachine.getTransfer().openLimiter();
+ }
+ else {
+ if (stateMachine.getCurrentGameState() != GameState.SCORING) {
+ stateMachine.getTransfer().closeLimiter();
+ }
+ }
+
+
+ updateTelemetry(currentPose);
+ }
+
+ stateMachine.setRobotState(RobotState.ESTOP);
+ }
+
+ private void initializeRobot() {
+ follower = Constants.createFollower(hardwareMap);
+
+ allHubs = hardwareMap.getAll(LynxModule.class);
+ for (LynxModule hub : allHubs) {
+ hub.setBulkCachingMode(LynxModule.BulkCachingMode.MANUAL);
+ }
+
+ stateMachine = new StateMachine(
+ new hwMap.DriveHwMap(hardwareMap),
+ new hwMap.IntakeHwMap(hardwareMap),
+ new hwMap.LiftHwMap(hardwareMap),
+ new hwMap.TransferHwMap(hardwareMap),
+ new hwMap.TurretHwMap(hardwareMap),
+ new hwMap.LedHwMap(hardwareMap),
+ AutoTransfer.motifPattern,
+ currentAlliance
+ );
+
+
+ stateMachine.getTransfer().closeLimiter();
+
+ stateMachine.getTurret().setAlliance(currentAlliance);
+ }
+
+ private void configureStartSettings() {
+ stateMachine.setRobotState(RobotState.TELEOP);
+ stateMachine.getDriveTrain().setDriveState(DriveTrain.DriveState.NORMAL);
+ stateMachine.getTurret().setLaunchPower(org.firstinspires.ftc.teamcode.teleOp.Constants.TurretConstants.TURRET_POWER_MID);
+ stateMachine.getTransfer().closeLimiter();
+ stateMachine.initUpdate();
+ }
+
+ private void handleGlobalLogic() {
+
+ if (gamepad1.backWasPressed()) {
+ stateMachine.emergencyStop();
+ }
+
+ if (stateMachine.getCurrentGameState() != GameState.SCORING && !stateMachine.getTurret().hasReachedVelocity()) {
+ int rumbleDuration = (int) fpsCounter.getCurrentLoopTime() + 2;
+ gamepad2.rumble(rumbleDuration);
+ }
+
+ if (gamepad1.psWasPressed()) {
+ stateMachine.toggleAnchor();
+ }
+
+ if (gamepad2.psWasPressed()) {
+ if (currentAlliance == 2) { // RED
+ follower.setPose(new Pose(135, 9, 0));
+ } else if (currentAlliance == 1) {
+ follower.setPose(new Pose(9, 9, Math.toRadians(180)));
+
+ }
+ }
+
+ }
+
+ private void handleDriverInput() {
+ stateMachine.getDriveTrain().teleopDrive(
+ gamepad1.left_stick_x,
+ -gamepad1.left_stick_y,
+ gamepad1.right_stick_x
+ );
+
+ if (gamepad1.dpad_up) {
+ stateMachine.getDriveTrain().setDriveState(DriveTrain.DriveState.TURBO);
+ } else if (gamepad1.dpad_left) {
+ stateMachine.getDriveTrain().setDriveState(DriveTrain.DriveState.NORMAL);
+ } else if (gamepad1.dpad_down) {
+ stateMachine.getDriveTrain().setDriveState(DriveTrain.DriveState.PRECISION);
+ }
+
+ if (stateMachine.getCurrentGameState() != GameState.SCORING && !stateMachine.isIntakeLaunching) {
+ if (gamepad1.right_trigger > 0.1 || gamepad2.right_trigger > 0.1) {
+ stateMachine.getIntake().setIntakeState(Intake.IntakeState.INTAKE);
+ } else if (gamepad1.left_trigger > 0.1 || gamepad2.left_trigger > 0.1) {
+ stateMachine.getIntake().setIntakeState(Intake.IntakeState.EXTAKE);
+ } else {
+ if (stateMachine.getIntake().getCurrentState() == Intake.IntakeState.INTAKE ||
+ stateMachine.getIntake().getCurrentState() == Intake.IntakeState.EXTAKE) {
+ stateMachine.getIntake().setIntakeState(Intake.IntakeState.IDLE);
+ }
+ }
+ }
+
+
+
+ if (gamepad2.right_bumper) stateMachine.getTurret().updateOffsetTicks(-0.6);
+ if (gamepad2.left_bumper) stateMachine.getTurret().updateOffsetTicks(0.6);
+ }
+
+ private void handleOperatorInput() {
+ if (gamepad2.yWasPressed()) {
+ launching = !launching;
+ if (launching) {
+ stateMachine.setGameState(GameState.SCORING);
+ } else if (stateMachine.getCurrentGameState() == GameState.SCORING) {
+ stateMachine.setGameState(GameState.IDLE);
+ }
+ }
+
+ if (gamepad2.backWasPressed()) {
+ manualSAM = !manualSAM;
+ }
+
+ if (gamepad2.psWasPressed()) {
+ manualTracking = !manualTracking;
+ }
+
+ if (manualSAM) {
+ handleManualSAM();
+ }
+ }
+
+ private void handleManualSAM() {
+ if (gamepad2.dpad_up) {
+ stateMachine.getTurret().setLaunchPower(org.firstinspires.ftc.teamcode.teleOp.Constants.TurretConstants.TURRET_POWER_MAX);
+ stateMachine.getTurret().setHoodPos(org.firstinspires.ftc.teamcode.teleOp.Constants.TurretConstants.HOOD_POS_FAR);
+ } else if (gamepad2.dpad_left) {
+ stateMachine.getTurret().setLaunchPower(org.firstinspires.ftc.teamcode.teleOp.Constants.TurretConstants.TURRET_POWER_MID);
+ stateMachine.getTurret().setHoodPos(org.firstinspires.ftc.teamcode.teleOp.Constants.TurretConstants.HOOD_POS_MID);
+ } else if (gamepad2.dpad_down) {
+ stateMachine.getTurret().setLaunchPower(org.firstinspires.ftc.teamcode.teleOp.Constants.TurretConstants.TURRET_POWER_LOW);
+ stateMachine.getTurret().setHoodPos(org.firstinspires.ftc.teamcode.teleOp.Constants.TurretConstants.HOOD_POS_CLOSE);
+ }
+
+ if (Math.abs(gamepad2.left_stick_y) > 0.13) {
+ hoodPosition += -gamepad2.left_stick_y * 0.05;
+ hoodPosition = Range.clip(hoodPosition, 0.0, 1.0);
+ }
+ stateMachine.getTurret().setHoodPos(hoodPosition);
+ }
+
+ private void updateTelemetry(Pose p) {
+ telemetry.addData("Avg FPS", "%.2f", fpsCounter.getAverageFps());
+
+ telemetry.addLine("▰▰▰▰▰▰▰ SYSTEMS ▰▰▰▰▰▰▰");
+ telemetry.addData("State", stateMachine.getCurrentRobotState());
+ telemetry.addData("Game Mode", stateMachine.getCurrentGameState());
+ telemetry.addData("Scoring", stateMachine.getCurrentGameState() == GameState.SCORING ? "ACTIVE" : "IDLE");
+ telemetry.addData("Anchor", stateMachine.getDriveTrain().isAnchorActive() ? "LOCKED" : "FREE");
+ telemetry.addData("Alliance", currentAlliance == 2 ? "RED" : "BLUE");
+ telemetry.addData("Drive Speed", stateMachine.getDriveTrain().getDriveState());
+
+ telemetry.addLine("▰▰▰▰▰▰▰ TRACKING ▰▰▰▰▰▰▰");
+ telemetry.addData("Pose", "X:%.1f Y:%.1f H:%.1f°", p.getX(), p.getY(), Math.toDegrees(p.getHeading()));
+ telemetry.addData("Distance", "%.1f in", stateMachine.getTurret().getDistance());
+ telemetry.addData("Turret Ready", stateMachine.getTurret().isTurretReady());
+ telemetry.addData("Hood Offset", stateMachine.getTurret().offsetHood);
+
+
+
+ telemetry.update();
+ }
+ }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/ColorSensorTuning.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/ColorSensorTuning.java
new file mode 100755
index 0000000..89946f5
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/ColorSensorTuning.java
@@ -0,0 +1,113 @@
+package org.firstinspires.ftc.teamcode.tuning;
+
+import android.graphics.Color;
+
+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.NormalizedRGBA;
+import org.firstinspires.ftc.teamcode.Hware.hwMap;
+@TeleOp(name="Color Sensor Tuning", group="Diagnostics")
+public class ColorSensorTuning extends LinearOpMode {
+
+ hwMap.TransferHwMap transfer;
+
+ @Override
+ public void runOpMode() throws InterruptedException {
+ transfer = new hwMap.TransferHwMap(hardwareMap);
+
+ telemetry.addLine("═══════════════════════════════");
+ telemetry.addLine(" DUAL COLOR SENSOR TUNING");
+ telemetry.addLine("═══════════════════════════════");
+ telemetry.addLine("\nEach slot has 2 sensors for redundancy");
+ telemetry.addLine("Uses both sensors for best accuracy");
+ telemetry.update();
+
+ waitForStart();
+
+ while (opModeIsActive()) {
+ telemetry.clearAll();
+ telemetry.addLine("═══════════════════════════════");
+ telemetry.addLine(" DUAL SENSOR READINGS");
+ telemetry.addLine("═══════════════════════════════");
+
+ // Display all three slots with both sensors
+ for (int slot = 1; slot <= 3; slot++) {
+ displayDualSensorData(transfer, slot);
+ }
+
+ telemetry.addLine("\n═══════════════════════════════");
+ telemetry.addLine(" FINAL DETECTION RESULTS");
+ telemetry.addLine("═══════════════════════════════");
+ telemetry.addData("Slot A", getColorName(detectArtifactColor(1)));
+ telemetry.addData("Slot B", getColorName(detectArtifactColor(2)));
+ telemetry.addData("Slot C", getColorName(detectArtifactColor(3)));
+
+ telemetry.update();
+ sleep(100);
+ }
+ }
+
+ /**
+ * Detect color using BOTH sensors and pick the best result
+ */
+ public int detectArtifactColor(int index) {
+ if(index < 1 || index > 3) return 0;
+ int i = index - 1;
+
+ NormalizedRGBA colors2 = transfer.sensors[i].getNormalizedColors();
+
+ float[] hsv = new float[3];
+ Color.colorToHSV(colors2.toColor(), hsv);
+
+ int color = detectColor(hsv[0], hsv[1], hsv[2]);
+
+ double confidence = hsv[1] * hsv[2];
+
+ // Decision Matrix
+ if (color == 0 && confidence == 0) return 0;
+ else {
+ return color;
+ }
+
+ }
+
+ private int detectColor(float hue, float sat, float val) {
+ if (sat < 0.15 || val < 0.1) return 0;
+
+ if (hue > 170 && hue < 310) return 1;
+ if (hue > 80 && hue < 170) return 2;
+
+ return 0;
+ }
+ private void displayDualSensorData(hwMap.TransferHwMap transfer, int slot) {
+ int index = slot - 1;
+ String slotName = (slot == 1) ? "A" : (slot == 2) ? "B" : "C";
+
+ NormalizedRGBA colors = transfer.sensors[index].getNormalizedColors();
+
+ float[] hsv = new float[3];
+ Color.colorToHSV(colors.toColor(), hsv);
+
+ telemetry.addLine("\n───── Slot " + slotName + " ─────");
+
+ // Sensor 1 (Wall)
+ telemetry.addLine( "Sensors:");
+ telemetry.addData(" HSV", "H:%.0f S:%.2f V:%.2f", hsv[0], hsv[1], hsv[2]);
+ telemetry.addData(" Detects", getColorName(detectColor(hsv[0], hsv[1], hsv[2])));
+ telemetry.addData(" Confidence", String.format("%.2f", hsv[1] * hsv[2]));
+
+ // Show which sensor is being used
+ int color1 = detectColor(hsv[0], hsv[1], hsv[2]);
+ double conf1 = hsv[1] * hsv[2];
+ }
+
+ private String getColorName(int colorCode) {
+ switch (colorCode) {
+ case 0: return "NONE";
+ case 1: return "PURPLE";
+ case 2: return "GREEN";
+ default: return "UNKNOWN";
+ }
+ }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/GetDistanceTuning.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/GetDistanceTuning.java
new file mode 100755
index 0000000..05b48ad
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/GetDistanceTuning.java
@@ -0,0 +1,94 @@
+package org.firstinspires.ftc.teamcode.tuning;
+
+import com.bylazar.telemetry.TelemetryManager;
+import com.qualcomm.hardware.limelightvision.LLResult;
+import com.qualcomm.hardware.limelightvision.Limelight3A;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
+import com.qualcomm.robotcore.util.Range;
+import org.firstinspires.ftc.robotcore.external.navigation.Pose3D;
+import org.firstinspires.ftc.teamcode.teleOp.Constants;
+import com.bylazar.telemetry.PanelsTelemetry;
+
+import com.bylazar.configurables.annotations.Configurable;
+
+
+@Configurable
+@TeleOp(name = "Limelight Distance", group = "Sensor")
+public class GetDistanceTuning extends OpMode {
+
+ private Limelight3A limelight;
+ private double distance;
+ public static double CAMERA_HEIGHT_INCHES = 12.5;
+ public static double GOAL_HEIGHT_INCHES = 29.5;
+ public static int pipeline = 1;
+ public static double CAMERA_MOUNT_ANGLE_DEGREES = 14.3;
+
+ private TelemetryManager telemetryM;
+
+
+
+ @Override
+ public void init() {
+ limelight = hardwareMap.get(Limelight3A.class, "limelight");
+ limelight.pipelineSwitch(pipeline);
+
+ telemetryM = PanelsTelemetry.INSTANCE.getTelemetry();
+ limelight.start();
+
+ telemetryM.debug("Status", "Initialized");
+ telemetryM.update(telemetry);
+ }
+
+ @Override
+ public void loop() {
+ LLResult result = limelight.getLatestResult();
+
+ if (result != null && result.isValid()) {
+ double targetY = result.getTy();
+ double targetArea = result.getTa();
+
+
+ distance = getTrigBasedDistance(targetY);
+
+ double areaDistance = getDistanceFromTag(targetArea);
+
+ telemetryM.debug("Method", "Trigonometry");
+ telemetryM.debug("Target Y (ty)", targetY);
+ telemetryM.debug("Calculated Distance", distance);
+
+ telemetryM.debug("Area Algo Distance", areaDistance);
+
+ telemetryM.debug("Flywheel Velocity", getFlywheelVelocity(distance));
+ telemetryM.debug("Hood Position", getHoodPOS(distance));
+
+ } else {
+ telemetryM.debug("Limelight", "No Target Found");
+ }
+
+ telemetryM.update(telemetry);
+ }
+
+ private double getTrigBasedDistance(double targetOffsetAngleVertical) {
+
+ return (GOAL_HEIGHT_INCHES - CAMERA_HEIGHT_INCHES) / Math.tan(Math.toRadians(CAMERA_MOUNT_ANGLE_DEGREES + targetOffsetAngleVertical));
+
+ // Calculate distance
+ //double distanceFromLimelightToGoalInches = (GOAL_HEIGHT_INCHES - CAMERA_HEIGHT_INCHES) / Math.tan(angleToGoalRadians);
+
+ //return distanceFromLimelightToGoalInches;
+ }
+
+ private double getDistanceFromTag(double x) {
+ return (74.34095 * Math.pow(x, -0.518935));
+ }
+
+ private double getFlywheelVelocity(double dist) {
+ return (0.0000119972 * Math.pow((dist), 4) - 0.00249603 * Math.pow(dist, 3) + 0.179513 * Math.pow(dist, 2) + (0.0519688 * dist) + 1489.8908);
+ }
+
+ private double getHoodPOS(double dist) {
+ return Range.clip(((-(3.62429 * Math.pow(10, -8)) * Math.pow(dist, 4)) + 0.00000638975 * Math.pow(dist, 3) - (0.000153252 * Math.pow(dist, 2)) + (-0.0197027 * dist) + 1.40511), Constants.TurretConstants.SERVO_MIN, Constants.TurretConstants.SERVO_MAX);
+ }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/LimelightEncoderTurret.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/LimelightEncoderTurret.java
new file mode 100755
index 0000000..d154a20
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/LimelightEncoderTurret.java
@@ -0,0 +1,201 @@
+package org.firstinspires.ftc.teamcode.tuning;
+
+import com.qualcomm.hardware.limelightvision.LLResult;
+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 com.qualcomm.robotcore.hardware.CRServo;
+import com.qualcomm.robotcore.hardware.DcMotor;
+@Disabled
+@TeleOp(name="Limelight Turret", group="Tuning")
+public class LimelightEncoderTurret extends LinearOpMode {
+
+ private Limelight3A limelight;
+ private CRServo turretServo;
+ private DcMotor turretEncoder;
+
+ // Encoder specs
+ private final double ENCODER_TICKS_PER_REV = 8392; // Rev Through Bore
+ private final double ENCODER_TICKS_PER_DEGREE = ENCODER_TICKS_PER_REV / 360.0;
+
+ // ONLY tunable parameter - adjust this multiplier
+ private double rotationMultiplier = 1.0; // Tune this if turret over/under rotates
+
+ private double maxSpeed = 1.0; // Full speed
+ private double deadzone = 2.0; // Stop when within 2 degrees
+
+ private int targetTicks = 0;
+ private boolean trackingActive = false;
+
+ // Debouncing
+ private boolean lastDpadUp = false;
+ private boolean lastDpadDown = false;
+ private boolean lastA = false;
+ private boolean lastB = false;
+ private boolean lastX = false;
+
+ @Override
+ public void runOpMode() throws InterruptedException {
+ // Initialize hardware
+ limelight = hardwareMap.get(Limelight3A.class, "limelight");
+ turretServo = hardwareMap.get(CRServo.class, "turretservo");
+ turretEncoder = hardwareMap.get(DcMotor.class, "lturret");
+
+ // Setup encoder
+ turretEncoder.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+ turretEncoder.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+
+ // Start Limelight on pipeline 8
+ limelight.pipelineSwitch(8);
+ limelight.start();
+
+ telemetry.addLine("═══════════════════════════════");
+ telemetry.addLine(" SIMPLE LIMELIGHT TURRET");
+ telemetry.addLine("═══════════════════════════════");
+ telemetry.addLine("\nCamera → Calculate ticks → Spin");
+ telemetry.addLine("\nControls:");
+ telemetry.addLine(" D-Pad Up: Increase Multiplier");
+ telemetry.addLine(" D-Pad Down: Decrease Multiplier");
+ telemetry.addLine(" A: Aim at Target (One-Shot)");
+ telemetry.addLine(" B: Manual Control (Right Stick)");
+ telemetry.addLine(" X: Reset Encoder Zero");
+ telemetry.addLine("\nPress START when ready");
+ telemetry.update();
+
+ waitForStart();
+
+ while (opModeIsActive()) {
+
+ // Adjust rotation multiplier
+ if (gamepad1.dpad_up && !lastDpadUp) {
+ rotationMultiplier += 0.05;
+ }
+ lastDpadUp = gamepad1.dpad_up;
+
+ if (gamepad1.dpad_down && !lastDpadDown) {
+ rotationMultiplier -= 0.05;
+ if (rotationMultiplier < 0.1) rotationMultiplier = 0.1;
+ }
+ lastDpadDown = gamepad1.dpad_down;
+
+ // Trigger aiming
+ if (gamepad1.a && !lastA) {
+ aimAtTarget();
+ }
+ lastA = gamepad1.a;
+
+ // Manual control
+ if (gamepad1.b) {
+ turretServo.setPower(gamepad1.right_stick_x * maxSpeed);
+ trackingActive = false;
+ } else if (trackingActive) {
+ moveToTarget();
+ } else {
+ turretServo.setPower(0);
+ }
+
+ // Reset encoder
+ if (gamepad1.x && !lastX) {
+ turretEncoder.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+ turretEncoder.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+ targetTicks = 0;
+ trackingActive = false;
+ }
+ lastX = gamepad1.x;
+
+ // Display telemetry
+ displayTelemetry();
+ }
+
+ turretServo.setPower(0);
+ limelight.stop();
+ }
+
+ /**
+ * Use Limelight to calculate target and start moving
+ */
+ private void aimAtTarget() {
+ LLResult result = limelight.getLatestResult();
+
+ if (result == null || !result.isValid()) {
+ telemetry.addLine("\n✗ NO TARGET FOUND");
+ telemetry.update();
+ return;
+ }
+
+ // Get angle offset from camera
+ double txDegrees = result.getTx();
+
+ // Convert to ticks (apply multiplier for tuning)
+ double ticksToRotate = txDegrees * ENCODER_TICKS_PER_DEGREE * rotationMultiplier;
+
+ // Set target relative to current position
+ int currentTicks = turretEncoder.getCurrentPosition();
+ targetTicks = currentTicks + (int)ticksToRotate;
+
+ trackingActive = true;
+ }
+
+ private void moveToTarget() {
+ int currentTicks = turretEncoder.getCurrentPosition();
+ int error = targetTicks - currentTicks;
+
+ // Convert error to degrees for deadzone check
+ double errorDegrees = error / ENCODER_TICKS_PER_DEGREE;
+
+ if (Math.abs(errorDegrees) < deadzone) {
+ // Close enough - stop
+ turretServo.setPower(0);
+ trackingActive = false;
+ } else {
+ // Spin at max speed in correct direction
+ double power = (error > 0) ? maxSpeed : -maxSpeed;
+ turretServo.setPower(power); // Negative if servo is reversed
+ }
+ }
+
+ private void displayTelemetry() {
+ LLResult result = limelight.getLatestResult();
+ int currentTicks = turretEncoder.getCurrentPosition();
+ double currentDegrees = currentTicks / ENCODER_TICKS_PER_DEGREE;
+
+ telemetry.clearAll();
+ telemetry.addLine("═══════════════════════════════");
+ telemetry.addLine(" 🎯 SIMPLE TURRET AIMING");
+ telemetry.addLine("═══════════════════════════════");
+
+ telemetry.addLine("\n───── Status ─────");
+ telemetry.addData("Tracking", trackingActive ? "→ MOVING" : "⊙ IDLE");
+ telemetry.addData("Camera", (result != null && result.isValid()) ? "✓ TARGET" : "✗ NO TARGET");
+
+ if (result != null && result.isValid()) {
+ telemetry.addData("Camera Angle", String.format("%.2f°", result.getTx()));
+ }
+
+ telemetry.addLine("\n───── Encoder ─────");
+ telemetry.addData("Current Ticks", currentTicks);
+ telemetry.addData("Current Angle", String.format("%.1f°", currentDegrees));
+ telemetry.addData("Target Ticks", targetTicks);
+ telemetry.addData("Error", String.format("%d ticks", targetTicks - currentTicks));
+
+ telemetry.addLine("\n───── Tuning ─────");
+ telemetry.addData(">>> Rotation Multiplier", String.format("%.2f <<<", rotationMultiplier));
+ telemetry.addLine("");
+ telemetry.addLine("If turret OVERSHOOTS:");
+ telemetry.addLine(" → Press D-Pad DOWN");
+ telemetry.addLine("If turret UNDERSHOOTS:");
+ telemetry.addLine(" → Press D-Pad UP");
+
+ telemetry.addLine("\n───── How to Use ─────");
+ telemetry.addLine("1. Point camera at AprilTag");
+ telemetry.addLine("2. Press A to aim");
+ telemetry.addLine("3. Turret rotates at full speed");
+ telemetry.addLine("4. Stops when close enough");
+
+ telemetry.addLine("\n───── Copy to Code ─────");
+ telemetry.addLine(String.format("ROTATION_MULTIPLIER = %.3f;", rotationMultiplier));
+
+ telemetry.update();
+ }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/PIDFTuning.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/PIDFTuning.java
new file mode 100755
index 0000000..a0d4ea2
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/PIDFTuning.java
@@ -0,0 +1,84 @@
+package org.firstinspires.ftc.teamcode.tuning;
+
+import com.bylazar.configurables.annotations.Configurable;
+import com.qualcomm.hardware.limelightvision.LLResult;
+import com.qualcomm.hardware.limelightvision.Limelight3A;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
+import com.qualcomm.robotcore.hardware.PIDFCoefficients;
+import com.qualcomm.robotcore.hardware.Servo;
+import com.qualcomm.robotcore.util.Range;
+
+// Make sure your import points to your actual Constants file
+import org.firstinspires.ftc.teamcode.teleOp.Constants;
+@Configurable
+@TeleOp
+public class PIDFTuning extends OpMode {
+
+
+ private Limelight3A limelight;
+
+ public DcMotorEx flywheelMotorR;
+ public DcMotorEx flywheelMotorL;
+
+ public Servo hoodServo;
+
+ public static double curTargetVelocity = 0;
+ public static double targetPOS = 0;
+ public static int pipeline = 1;
+
+ double distance = 0;
+
+ @Override
+ public void init() {
+ flywheelMotorR = hardwareMap.get(DcMotorEx.class, Constants.TurretConstants.TURRET_RIGHT_MOTOR);
+ flywheelMotorL = hardwareMap.get(DcMotorEx.class, Constants.TurretConstants.TURRET_LEFT_MOTOR);
+
+ flywheelMotorR.setMode(DcMotorEx.RunMode.RUN_USING_ENCODER);
+ flywheelMotorR.setDirection(DcMotorSimple.Direction.REVERSE);
+ flywheelMotorL.setMode(DcMotorEx.RunMode.RUN_USING_ENCODER);
+
+ hoodServo = hardwareMap.servo.get("hoodServo");
+
+ hoodServo.setDirection(Servo.Direction.REVERSE);
+
+ limelight = hardwareMap.get(Limelight3A.class, "limelight");
+ limelight.pipelineSwitch(pipeline);
+ limelight.start();
+
+ telemetry.addLine("Init complete");
+ }
+
+ @Override
+ public void loop() {
+ limelight.pipelineSwitch(pipeline);
+
+ LLResult result = limelight.getLatestResult();
+
+ if (result.isValid()) {
+ double targetY = result.getTy();
+ telemetry.addData("Ty", targetY);
+ distance = getTrigBasedDistance(targetY);
+ }
+
+ if (!gamepad1.b) {flywheelMotorR.setVelocity(curTargetVelocity);
+ flywheelMotorL.setVelocity(curTargetVelocity);}
+
+ hoodServo.setPosition(Range.clip(targetPOS, Constants.TurretConstants.SERVO_MIN, Constants.TurretConstants.SERVO_MAX));
+
+
+
+ telemetry.addData("Current Velocity", "Current Velocity: %.2f", flywheelMotorL.getVelocity());
+ telemetry.addData("Target Velocity", "Target Velocity: %.2f", curTargetVelocity);
+ telemetry.addData("Target Position", "Target Velocity: %.2f", targetPOS);
+ telemetry.addData("Distance", distance);
+
+ telemetry.update();
+ }
+
+ private double getTrigBasedDistance(double targetOffsetAngleVertical) {
+ return (14) / Math.tan(Math.toRadians(2.0 + targetOffsetAngleVertical));
+ }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/Prism/GoBildaPrismArtboardExample.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/Prism/GoBildaPrismArtboardExample.java
new file mode 100755
index 0000000..ffa2271
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/Prism/GoBildaPrismArtboardExample.java
@@ -0,0 +1,105 @@
+/* MIT License
+ * Copyright (c) [2025] [Base 10 Assets, LLC]
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+package org.firstinspires.ftc.teamcode.tuning.Prism;
+
+
+import static org.firstinspires.ftc.teamcode.Prism.GoBildaPrismDriver.Artboard;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import org.firstinspires.ftc.teamcode.Prism.GoBildaPrismDriver;
+
+
+import java.util.concurrent.TimeUnit;
+
+/*
+ * This example shows how to recall previously created Artboards on the goBILDA Prism RGB Driver.
+ * Recalling these animations is all we recommend that your main robot OpMode do. We prefer to
+ * create these Artboards once and store them on the device, allowing for a short I²C write to
+ * set the current Artboard.
+ *
+ * It also shows how to enable or disable the default boot animation on power up. When enabled,
+ * as soon as the Prism gets power it will display the Artboard saved in Artboard slot 0. If you'd
+ * instead like to wait until you explicitly set an Artboard, disable this setting. This setting is
+ * saved and will only need to be set once.
+ *
+ */
+
+@TeleOp(name="Prism Artboard Example", group="Linear OpMode")
+//@Disabled
+
+public class GoBildaPrismArtboardExample extends LinearOpMode {
+
+ GoBildaPrismDriver prism;
+
+ @Override
+ public void runOpMode() {
+
+ /*
+ * Initialize the hardware variables. Note that the strings used here must correspond
+ * to the names assigned during the robot configuration step on the driver's station.
+ */
+ prism = hardwareMap.get(GoBildaPrismDriver.class, "prism");
+
+ telemetry.addData("Device ID: ", prism.getDeviceID());
+ telemetry.addData("Firmware Version: ", prism.getFirmwareVersionString());
+ telemetry.addData("Hardware Version: ", prism.getHardwareVersionString());
+ telemetry.addData("Power Cycle Count: ", prism.getPowerCycleCount());
+ telemetry.addData("Run Time (Minutes): ", prism.getRunTime(TimeUnit.MINUTES));
+ telemetry.update();
+
+ // Wait for the game to start (driver presses START)
+ waitForStart();
+ resetRuntime();
+
+ // run until the end of the match (driver presses STOP)
+ while (opModeIsActive()) {
+
+ if(gamepad1.aWasPressed()){
+ prism.loadAnimationsFromArtboard(Artboard.ARTBOARD_0);
+ } else if(gamepad1.bWasPressed()){
+ prism.loadAnimationsFromArtboard(Artboard.ARTBOARD_1);
+ } else if(gamepad1.yWasPressed()){
+ prism.loadAnimationsFromArtboard(Artboard.ARTBOARD_2);
+ } else if(gamepad1.xWasPressed()){
+ prism.loadAnimationsFromArtboard(Artboard.ARTBOARD_3);
+ }
+
+ if(gamepad1.dpadLeftWasPressed()){prism.enableDefaultBootArtboard(false);}
+ if(gamepad1.dpadRightWasPressed()){prism.enableDefaultBootArtboard(true);}
+
+ telemetry.addLine("Press A to recall Artboard #0");
+ telemetry.addLine("Press B to recall Artboard #1");
+ telemetry.addLine("Press Y to recall Artboard #2");
+ telemetry.addLine("Press X to recall Artboard #3");
+ telemetry.addLine("");
+ telemetry.addLine("By default the Prism will wait for an I²C signal to " +
+ " start showing an animation.The prism can enable a Default Boot animation" +
+ " which will play the animation stored at Artboard #0 automatically on power up.");
+ telemetry.addLine("Press D-Pad Right to enable the default boot animation." +
+ " Press D-Pad Left to disable it.");
+ telemetry.update();
+ sleep(20);
+
+ }
+ }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/Prism/GoBildaPrismConfigurator.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/Prism/GoBildaPrismConfigurator.java
new file mode 100755
index 0000000..22700ca
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/Prism/GoBildaPrismConfigurator.java
@@ -0,0 +1,1200 @@
+/* MIT License
+ * Copyright (c) [2025] [Base 10 Assets, LLC]
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+package org.firstinspires.ftc.teamcode.tuning.Prism;
+
+import static org.firstinspires.ftc.teamcode.Prism.GoBildaPrismDriver.LayerHeight;
+
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import org.firstinspires.ftc.teamcode.Prism.Color;
+import org.firstinspires.ftc.teamcode.Prism.GoBildaPrismDriver;
+import org.firstinspires.ftc.teamcode.Prism.PrismAnimations;
+import org.firstinspires.ftc.teamcode.Prism.PrismAnimations.AnimationType;
+import org.firstinspires.ftc.teamcode.Prism.PrismAnimations.PoliceLights;
+import org.firstinspires.ftc.teamcode.Prism.GoBildaPrismDriver.Artboard;
+
+import java.util.concurrent.TimeUnit;
+
+/*
+ * This code creates a "Configurator" UI which exposes a somewhat limited amount of the functionality
+ * available to create animations and artboards to users without needing to work with very much code.
+ *
+ * This file is not meant to serve as example code - unless you're trying to create a telemetry-based
+ * UI for the drivers station.
+ * For example code that your team can leverage in your autonomous/teleop code to recall artboards
+ * check out the GoBildaPrismArtboardExample. Run this code on your robot and use it to create
+ * the artboards you'd like to be able to switch between. This means that switching between animations
+ * can be very fast and easy, and the work of creating animations (and sending this information over
+ * I²C) only has to happen once!
+ *
+ */
+
+@TeleOp(name="Prism Configurator", group="Linear OpMode")
+//@Disabled
+
+public class GoBildaPrismConfigurator extends LinearOpMode {
+
+ GoBildaPrismDriver prism;
+
+ // an enum which is used in a mini state machine to allow the user to set different colors.
+ public enum AnimationColor{
+ PRIMARY_COLOR,
+ SECONDARY_COLOR,
+ TERTIARY_COLOR,
+ }
+ AnimationColor animationColor = AnimationColor.PRIMARY_COLOR;
+
+ // Set a default style for the Police Lights Animation.
+ PoliceLights.PoliceLightsStyle policeLightsStyle = PoliceLights.PoliceLightsStyle.Style1;
+
+ int startPoint = 0; // the start LED for any configured animation
+ int endPoint = 12; // the end LED for a configured animation
+ int brightness = 50; // the brightness of configured animation
+ int period = 1000; // the period of a configured animation
+ float speed = 0.5F; // the speed of a configured animation
+
+ int layerSelector = 0; // an integer used to create a cursor to select a layer
+ int animationSelector = 1; // animation cursor
+ int artboardSelector = 0; // artboard cursor
+
+ String hsbTelemetry; // string meant to send over telemetry that gets updated by hsbViaJoystick()
+ String hueTelemetry; // updated by hueViaJoystick()
+
+ /*
+ * An array of colors passed to the SingleFill animation.
+ */
+ Color[] singleFillColors = {
+ Color.RED, Color.WHITE, Color.BLUE
+ };
+
+ AnimationType selectedAnimation = AnimationType.SOLID; // store the animation that is being selected.
+
+ /*
+ * the enum powering the main state machine that this code moves through, each state represents
+ * a page the user can see displayed via telemetry on the driver's station.
+ */
+ public enum ConfigState{
+ WELCOME_SCREEN,
+ SELECT_LAYER,
+ SET_ENDPOINTS,
+ SELECT_ANIMATION,
+ CONFIGURE_ANIMATION,
+ SET_BRIGHTNESS,
+ SET_SPEED,
+ FORK_IN_THE_ROAD,
+ SAVE_TO_ARTBOARD,
+ COMPLETE;
+ }
+
+ ConfigState configState = ConfigState.WELCOME_SCREEN;
+
+ /*
+ * This is actually a bit of a duplicate of the LayerHeight found in PrismAnimations. This adds
+ * an animation slot which can be stored at each position in the enum. This is not required for
+ * the Prism side, but I want to be able to show a user what animation is stored at what layer
+ * after they've created their first animation.
+ */
+ public enum Layers{
+ LAYER_0 (AnimationType.NONE,0,LayerHeight.LAYER_0),
+ LAYER_1 (AnimationType.NONE,1,LayerHeight.LAYER_1),
+ LAYER_2 (AnimationType.NONE,2,LayerHeight.LAYER_2),
+ LAYER_3 (AnimationType.NONE,3,LayerHeight.LAYER_3),
+ LAYER_4 (AnimationType.NONE,4,LayerHeight.LAYER_4),
+ LAYER_5 (AnimationType.NONE,5,LayerHeight.LAYER_5),
+ LAYER_6 (AnimationType.NONE,6,LayerHeight.LAYER_6),
+ LAYER_7 (AnimationType.NONE,7,LayerHeight.LAYER_7),
+ LAYER_8 (AnimationType.NONE,8,LayerHeight.LAYER_8),
+ LAYER_9 (AnimationType.NONE,9,LayerHeight.LAYER_9);
+
+ private AnimationType animationType;
+ private final int index;
+ private final LayerHeight layerHeight;
+
+ Layers(AnimationType animationType, int index, LayerHeight layerHeight){
+ this.animationType = animationType;
+ this.index = index;
+ this.layerHeight = layerHeight;
+ }
+ }
+
+ /*
+ * This enum captures the kind of speed we can control on the animation.
+ */
+ public enum SpeedType{
+ NO_SPEED,
+ PERIOD_ONLY,
+ SPEED_ONLY,
+ PERIOD_AND_SPEED,
+ }
+
+ Layers selectedLayer = Layers.LAYER_0;
+
+ Artboard selectedArtboard = Artboard.ARTBOARD_0;
+
+ /*
+ * Create each Prism Animation which can be customized by the user.
+ */
+ PrismAnimations.Solid solid = new PrismAnimations.Solid();
+ PrismAnimations.Solid endpointsAnimation = new PrismAnimations.Solid();
+ PrismAnimations.Blink blink = new PrismAnimations.Blink();
+ PrismAnimations.Pulse pulse = new PrismAnimations.Pulse();
+ PrismAnimations.SineWave sineWave = new PrismAnimations.SineWave();
+ PrismAnimations.DroidScan droidScan = new PrismAnimations.DroidScan();
+ PrismAnimations.Rainbow rainbow = new PrismAnimations.Rainbow();
+ PrismAnimations.Snakes snakes = new PrismAnimations.Snakes();
+ PrismAnimations.Random random = new PrismAnimations.Random();
+ PrismAnimations.Sparkle sparkle = new PrismAnimations.Sparkle();
+ PrismAnimations.SingleFill singleFill = new PrismAnimations.SingleFill();
+ PrismAnimations.RainbowSnakes rainbowSnakes = new PrismAnimations.RainbowSnakes();
+ PoliceLights policeLights = new PoliceLights();
+
+
+ @Override
+ public void runOpMode() {
+ prism = hardwareMap.get(GoBildaPrismDriver.class,"prism");
+
+ telemetry.addLine("Welcome to the Prism Configurator, enjoy these fun stats " +
+ "and click the 'Play' button to continue");
+ telemetry.addLine("");
+
+ telemetry.addData("Device ID", prism.getDeviceID());
+ telemetry.addLine(prism.getFirmwareVersionString());
+ telemetry.addLine(prism.getHardwareVersionString());
+ telemetry.addData("Power Cycle Count", prism.getPowerCycleCount());
+ telemetry.addData("Run Time (Minutes)",prism.getRunTime(TimeUnit.MINUTES));
+ telemetry.addData("Run Time (Hours)",prism.getRunTime(TimeUnit.HOURS));
+ telemetry.update();
+
+ // Wait for the game to start (driver presses START)
+ waitForStart();
+ resetRuntime();
+
+ // run until the end of the match (driver presses STOP)
+ while (opModeIsActive()) {
+
+ switch (configState){
+ case WELCOME_SCREEN:
+ welcomeScreen();
+ if(gamepad1.aWasPressed()){
+ //if the user presses "a" we move onto the next state.
+ prism.clearAllAnimations();
+ configState = ConfigState.SELECT_LAYER;
+ }
+ break;
+ case SELECT_LAYER:
+ /*
+ * You can't convince me that this is the correct way to implement a cursor,
+ * but you also can't tell me this one doesn't work.
+ * We allow the user to increment the layerSelecter variable, before constraining
+ * it to within a valid range. We then pass that into our "selectLayer()"
+ * function which displays the UI elements via telemetry and saves the selected
+ * layer every loop.
+ */
+ if(gamepad1.dpadUpWasPressed()){
+ layerSelector -=1;
+ }
+ if(gamepad1.dpadDownWasPressed()){
+ layerSelector +=1;
+ }
+ layerSelector = Math.min(9, Math.max(0,layerSelector));
+ selectLayer(layerSelector);
+ if(gamepad1.aWasPressed()){
+ configureEndpointsAnimation(true);
+ configState = ConfigState.SET_ENDPOINTS;
+ }
+ break;
+ case SET_ENDPOINTS:
+ configureEndPoints();
+ if(gamepad1.aWasPressed()){
+ telemetry.clear();
+ selectAnimation(animationSelector);
+ configState = ConfigState.SELECT_ANIMATION;
+ }
+ if(gamepad1.bWasPressed()){
+ configState = ConfigState.SELECT_LAYER;
+ }
+ break;
+ case SELECT_ANIMATION:
+ if(gamepad1.dpadUpWasPressed()){
+ animationSelector -=1;
+ animationSelector = Math.min(12, Math.max(1,animationSelector));
+ selectAnimation(animationSelector);
+ }
+ if(gamepad1.dpadDownWasPressed()){
+ animationSelector +=1;
+ animationSelector = Math.min(12, Math.max(1,animationSelector));
+ selectAnimation(animationSelector);
+ }
+ if(gamepad1.aWasPressed()){
+ selectedLayer.animationType = selectedAnimation;
+ configState = ConfigState.CONFIGURE_ANIMATION;
+ }
+ if(gamepad1.bWasPressed()){
+ configureEndpointsAnimation(true);
+ configState = ConfigState.SET_ENDPOINTS;
+ }
+ break;
+ case CONFIGURE_ANIMATION:
+ configureAnimation(true,false);
+ if(gamepad1.aWasPressed()){
+ configState = ConfigState.SET_BRIGHTNESS;
+ }
+ if(gamepad1.bWasPressed()){
+ telemetry.clear();
+ selectAnimation(animationSelector);
+ configState = ConfigState.SELECT_ANIMATION;
+ }
+ break;
+ case SET_BRIGHTNESS:
+ configureBrightness();
+ if(gamepad1.aWasPressed()){
+ configState = ConfigState.SET_SPEED;
+ }
+ if(gamepad1.bWasPressed()){
+ configState = ConfigState.CONFIGURE_ANIMATION;
+ }
+ break;
+ case SET_SPEED:
+ if(configureSpeed()){
+ configState = ConfigState.FORK_IN_THE_ROAD;
+ }
+ if(gamepad1.aWasPressed()){
+ configState = ConfigState.FORK_IN_THE_ROAD;
+ }
+ if(gamepad1.bWasPressed()){
+ configState = ConfigState.SET_BRIGHTNESS;
+ }
+ break;
+ case FORK_IN_THE_ROAD:
+ forkInTheRoad();
+ if(gamepad1.aWasPressed()){
+ telemetry.clear();
+ configState = ConfigState.SAVE_TO_ARTBOARD;
+ selectArtboard(artboardSelector);
+ }
+ if(gamepad1.bWasPressed()){
+ if(configureSpeed()){
+ configState = ConfigState.SET_BRIGHTNESS;
+ } else configState = ConfigState.SET_SPEED;
+ }
+ if(gamepad1.yWasPressed()){
+ configState = ConfigState.SELECT_LAYER;
+ }
+ if(gamepad1.xWasPressed()){
+ configState = ConfigState.WELCOME_SCREEN;
+ }
+ break;
+ case SAVE_TO_ARTBOARD:
+ if(gamepad1.dpadUpWasPressed()){
+ artboardSelector -=1;
+ artboardSelector = Math.min(7, Math.max(0,artboardSelector));
+ selectArtboard(artboardSelector);
+ }
+ if(gamepad1.dpadDownWasPressed()){
+ artboardSelector +=1;
+ artboardSelector = Math.min(7, Math.max(0,artboardSelector));
+ selectArtboard(artboardSelector);
+ }
+ if(gamepad1.aWasPressed()){
+ prism.saveCurrentAnimationsToArtboard(selectedArtboard);
+ prism.enableDefaultBootArtboard(true);
+ configState = ConfigState.COMPLETE;
+ }
+ if(gamepad1.bWasPressed()){
+ configState = ConfigState.FORK_IN_THE_ROAD;
+ }
+ break;
+ case COMPLETE:
+ telemetry.addLine("Artboard saved!");
+ telemetry.addLine("");
+ telemetry.addLine("Press X to return to the beginning.");
+ if(gamepad1.xWasPressed()){
+ configState = ConfigState.WELCOME_SCREEN;
+ }
+ break;
+ }
+ telemetry.update();
+ sleep(20);
+ }
+ }
+
+ /*
+ * through this code I try and keep as much code as possible inside of functions, with how
+ * complex our main state machine is, stuff like telemetry is is nice to keep only where we need it.
+ */
+ public void welcomeScreen(){
+ telemetry.addLine("Welcome to the goBILDA Prism Configurator!");
+ telemetry.addLine("Hold on tight - we were cooking when we made this.");
+ telemetry.addLine("");
+ telemetry.addLine("Core to understanding how to use this product is knowing these three terms:");
+ telemetry.addLine("Animations: (Like RAINBOW or BLINK) - These have properties you can configure, " +
+ "like their color. And they can have unique start and end points.");
+ telemetry.addLine("");
+ telemetry.addLine("Layers: There are 10 layers, each of which can store an animation. " +
+ "These are hierarchical. So an Animation on layer 5 will cover an animation on layer 2 " +
+ "if they overlap. You can use start and end points to have layers overlap to create new patterns!" +
+ "Or show multiple animations at once on different LEDs.");
+ telemetry.addLine("");
+ telemetry.addLine("Artboards: An Artboard is a set of 10 layers which is stored on the Prism. " +
+ "you can have up to 8 unique Artboards. Artboards are easy and computationally fast to switch between. " +
+ "We recommend that you configure your Artboards for your robot once and then switch between them.");
+ telemetry.addLine("");
+ telemetry.addLine("Press A to continue");
+ resetStoredAnimations();
+ }
+
+ public void selectLayer(int selector){
+ telemetry.addLine("Select the Layer that you wish to save an Animation to.");
+ telemetry.addLine("Use D-Pad up and D-Pad down to navigate through the layers.");
+ telemetry.addLine("");
+ telemetry.addData("LAYER_0 Animation", Layers.LAYER_0.animationType+layerCursor(Layers.LAYER_0,selector));
+ telemetry.addData("LAYER_1 Animation", Layers.LAYER_1.animationType+layerCursor(Layers.LAYER_1,selector));
+ telemetry.addData("LAYER_2 Animation", Layers.LAYER_2.animationType+layerCursor(Layers.LAYER_2,selector));
+ telemetry.addData("LAYER_3 Animation", Layers.LAYER_3.animationType+layerCursor(Layers.LAYER_3,selector));
+ telemetry.addData("LAYER_4 Animation", Layers.LAYER_4.animationType+layerCursor(Layers.LAYER_4,selector));
+ telemetry.addData("LAYER_5 Animation", Layers.LAYER_5.animationType+layerCursor(Layers.LAYER_5,selector));
+ telemetry.addData("LAYER_6 Animation", Layers.LAYER_6.animationType+layerCursor(Layers.LAYER_6,selector));
+ telemetry.addData("LAYER_7 Animation", Layers.LAYER_7.animationType+layerCursor(Layers.LAYER_7,selector));
+ telemetry.addData("LAYER_8 Animation", Layers.LAYER_8.animationType+layerCursor(Layers.LAYER_8,selector));
+ telemetry.addData("LAYER_9 Animation", Layers.LAYER_9.animationType+layerCursor(Layers.LAYER_9,selector));
+ telemetry.addLine("");
+ telemetry.addLine("Press A to continue");
+ }
+
+ public String layerCursor(Layers layer, int selector){
+ if(layer.index == selector){
+ selectedLayer = layer;
+ return "<--";
+ }else return "";
+ }
+
+ public void selectAnimation(int animationSelector){
+ telemetry.addLine("Select the Animation that you wish to use");
+ telemetry.addLine("Use D-Pad up and D-Pad down to navigate through the Animations.");
+ telemetry.addLine("");
+ telemetry.addData("Solid",animationCursor(AnimationType.SOLID,animationSelector));
+ telemetry.addData("Blink",animationCursor(AnimationType.BLINK,animationSelector));
+ telemetry.addData("Pulse",animationCursor(AnimationType.PULSE,animationSelector));
+ telemetry.addData("Sine Wave",animationCursor(AnimationType.SINE_WAVE,animationSelector));
+ telemetry.addData("Droid Scan",animationCursor(AnimationType.DROID_SCAN,animationSelector));
+ telemetry.addData("Rainbow",animationCursor(AnimationType.RAINBOW,animationSelector));
+ telemetry.addData("Snakes",animationCursor(AnimationType.SNAKES,animationSelector));
+ telemetry.addData("Random",animationCursor(AnimationType.RANDOM,animationSelector));
+ telemetry.addData("Sparkle",animationCursor(AnimationType.SPARKLE,animationSelector));
+ telemetry.addData("Single Fill",animationCursor(AnimationType.SINGLE_FILL,animationSelector));
+ telemetry.addData("Rainbow Snakes",animationCursor(AnimationType.RAINBOW_SNAKES,animationSelector));
+ telemetry.addData("Police Lights",animationCursor(AnimationType.POLICE_LIGHTS,animationSelector));
+ telemetry.addLine("");
+ telemetry.addLine("Press A to continue");
+ telemetry.addLine("Press B to go back");
+ }
+
+ public String animationCursor(AnimationType animationType, int selector){
+ if(animationType.AnimationTypeIndex == selector){
+ selectedAnimation = animationType;
+ configureAnimation(false,true);
+ return "<--";
+ }else return "";
+ }
+
+ public void configureEndPoints(){
+ if(gamepad1.dpadLeftWasPressed()){
+ startPoint -= 1;
+ startPoint = Math.min(Math.min(255,Math.max(0,startPoint)),endPoint-1);
+ configureEndpointsAnimation(false);
+ }
+ if(gamepad1.dpadRightWasPressed()){
+ startPoint += 1;
+ startPoint = Math.min(Math.min(255,Math.max(0,startPoint)),endPoint-1);
+ configureEndpointsAnimation(false);
+ }
+ if(gamepad1.leftBumperWasPressed()){
+ endPoint -= 1;
+ endPoint = Math.max(Math.min(255,Math.max(0,endPoint)),startPoint+1);
+ configureEndpointsAnimation(false);
+ }
+ if(gamepad1.rightBumperWasPressed()){
+ endPoint += 1;
+ endPoint = Math.max(Math.min(255,Math.max(0,endPoint)),startPoint+1);
+ configureEndpointsAnimation(false);
+ }
+
+ telemetry.addLine("Set the start and stop point for each LED");
+ telemetry.addLine("");
+ telemetry.addLine("Use the d-pad to set the start point, d-pad right moves it further from the Prism. " +
+ "d-pad left moves it closer.");
+ telemetry.addLine("Bumpers move the endpoint, The left bumper moves it closer to the Prism, right moves it further.");
+ telemetry.addData("Start Point", startPoint);
+ telemetry.addData("End Point", endPoint);
+ telemetry.addLine("");
+ telemetry.addLine("Press A to Continue");
+ telemetry.addLine("Press B to go back");
+ }
+
+ public void configureBrightness(){
+ if(gamepad1.dpadDownWasPressed()){
+ brightness -= 10;
+ brightness = Math.min(100,Math.max(0,brightness));
+ configureAnimation(false,false);
+ }
+ if(gamepad1.dpadUpWasPressed()){
+ brightness += 10;
+ brightness = Math.min(100,Math.max(0,brightness));
+ configureAnimation(false,false);
+ }
+
+ telemetry.addLine("Set the brightness for this animation");
+ telemetry.addLine("");
+ telemetry.addLine("Use the d-pad to adjust the brightness, up increases, down decreases.");
+ telemetry.addLine("");
+ telemetry.addData("Brightness", brightness);
+ telemetry.addLine("");
+ telemetry.addLine("Press A to Continue");
+ telemetry.addLine("Press B to go back");
+ }
+
+ /**
+ * Returns: True if we should skip this animation type.
+ */
+ public boolean configureSpeed(){
+ switch (speedFromAnimation(selectedAnimation)){
+ case NO_SPEED: return true;
+ case SPEED_ONLY:
+ if(gamepad1.dpadUpWasPressed()){
+ speed += 0.1f;
+ speed = Math.min(1.0f, Math.max(0,speed));
+ configureAnimation(false,false);
+ }
+ if(gamepad1.dpadDownWasPressed()){
+ speed -= 0.1f;
+ speed = Math.min(1.0f, Math.max(0,speed));
+ configureAnimation(false,false);
+ }
+ telemetry.addLine("Set the speed for this animation from 0 to 1");
+ telemetry.addLine("");
+ telemetry.addLine("Use the d-pad to adjust the speed, up increases, down decreases.");
+ telemetry.addLine("");
+ telemetry.addData("Speed", speed);
+ telemetry.addLine("");
+ telemetry.addLine("Press A to Continue");
+ telemetry.addLine("Press B to go back");
+ break;
+ case PERIOD_ONLY:
+ if(gamepad1.dpadUpWasPressed()){
+ if(period < 401){
+ period += 100;
+ } else period += 500;
+ period = Math.min(300000, Math.max(0,period));
+ configureAnimation(false,false);
+ }
+ if(gamepad1.dpadDownWasPressed()){
+ if(period < 501){
+ period -= 100;
+ } else period -= 500;
+ period = Math.min(300000, Math.max(0,period));
+ configureAnimation(false,false);
+ }
+ telemetry.addLine("Set the period for this animation in Milliseconds");
+ telemetry.addLine("");
+ telemetry.addLine("Use the d-pad to adjust the period, up increases, down decreases.");
+ telemetry.addLine("");
+ telemetry.addData("Period", period);
+ telemetry.addLine("");
+ telemetry.addLine("Press A to Continue");
+ telemetry.addLine("Press B to go back");
+ break;
+ case PERIOD_AND_SPEED:
+ if(gamepad1.dpadUpWasPressed()){
+ if(period < 401){
+ period += 100;
+ } else period += 500;
+ period = Math.min(300000, Math.max(0,period));
+ configureAnimation(false,false);
+ }
+ if(gamepad1.dpadDownWasPressed()){
+ if(period < 501){
+ period -= 100;
+ } else period -= 500;
+ period = Math.min(300000, Math.max(0,period));
+ configureAnimation(false,false);
+ }
+ if(gamepad1.dpadRightWasPressed()){
+ speed += 0.1f;
+ speed = Math.min(1.0f, Math.max(0,speed));
+ configureAnimation(false,false);
+ }
+ if(gamepad1.dpadLeftWasPressed()){
+ speed -= 0.1f;
+ speed = Math.min(1.0f, Math.max(0,speed));
+ configureAnimation(false,false);
+ }
+ telemetry.addLine("Set the period for this animation in Milliseconds");
+ telemetry.addLine("");
+ telemetry.addLine("Set the speed for this animation from 0-1");
+ telemetry.addLine("");
+ telemetry.addLine("Use the d-pad to adjust the period, up increases, down decreases.");
+ telemetry.addLine("");
+ telemetry.addData("Period", period);
+ telemetry.addLine("");
+ telemetry.addLine("Use the d-pad to adjust the speed, right increases, left decreases.");
+ telemetry.addLine("");
+ telemetry.addData("Speed", speed);
+ telemetry.addLine("");
+ telemetry.addLine("Press A to Continue");
+ telemetry.addLine("Press B to go back");
+ break;
+ }
+ return false;
+ }
+
+ public SpeedType speedFromAnimation(AnimationType animationType){
+ if(animationType == AnimationType.BLINK || animationType == AnimationType.PULSE ||
+ animationType == AnimationType.SPARKLE || animationType == AnimationType.POLICE_LIGHTS){
+ return SpeedType.PERIOD_ONLY;
+ }
+ if (animationType == AnimationType.DROID_SCAN || animationType == AnimationType.RAINBOW ||
+ animationType == AnimationType.SNAKES || animationType == AnimationType.RANDOM ||
+ animationType == AnimationType.RAINBOW_SNAKES){
+ return SpeedType.SPEED_ONLY;
+ }
+ if(animationType == AnimationType.SINE_WAVE || animationType == AnimationType.SINGLE_FILL){
+ return SpeedType.PERIOD_AND_SPEED;
+ }
+ else return SpeedType.NO_SPEED;
+ }
+
+ public void forkInTheRoad(){
+ telemetry.addLine("If you are done with your creation, press A to save it to an Artboard.");
+ telemetry.addLine("");
+ telemetry.addLine("To go back, press B.");
+ telemetry.addLine("");
+ telemetry.addLine("If you'd instead like to layer another animation on top of this " +
+ "one, press Y.");
+ telemetry.addLine("");
+ telemetry.addLine("Press X to return to the start and clear currently set animations.");
+ }
+
+ public void selectArtboard(int artboardSelector){
+ telemetry.addLine("Select the Artboard that you wish to save to");
+ telemetry.addLine("Use D-Pad up and D-Pad down to navigate through the Artboards.");
+ telemetry.addLine("");
+ telemetry.addData("Artboard 0",artboardCursor(Artboard.ARTBOARD_0,artboardSelector));
+ telemetry.addData("Artboard 1",artboardCursor(Artboard.ARTBOARD_1,artboardSelector));
+ telemetry.addData("Artboard 2",artboardCursor(Artboard.ARTBOARD_2,artboardSelector));
+ telemetry.addData("Artboard 3",artboardCursor(Artboard.ARTBOARD_3,artboardSelector));
+ telemetry.addData("Artboard 4",artboardCursor(Artboard.ARTBOARD_4,artboardSelector));
+ telemetry.addData("Artboard 5",artboardCursor(Artboard.ARTBOARD_5,artboardSelector));
+ telemetry.addData("Artboard 6",artboardCursor(Artboard.ARTBOARD_6,artboardSelector));
+ telemetry.addData("Artboard 7",artboardCursor(Artboard.ARTBOARD_7,artboardSelector));
+ telemetry.addLine("");
+ telemetry.addLine("Press A to save");
+ telemetry.addLine("Press B to go back");
+ }
+
+ public String artboardCursor(Artboard artboard, int selector){
+ if(artboard.index == selector){
+ selectedArtboard = artboard;
+ return "<--";
+ }else return "";
+ }
+
+ public void configureAnimation(boolean showTelemetry, boolean isBeingInserted){
+ switch(selectedAnimation){
+ case SOLID:
+ configureSolid(showTelemetry, isBeingInserted);
+ break;
+ case BLINK:
+ configureBlink(showTelemetry, isBeingInserted);
+ break;
+ case PULSE:
+ configurePulse(showTelemetry, isBeingInserted);
+ break;
+ case SINE_WAVE:
+ configureSineWave(showTelemetry, isBeingInserted);
+ break;
+ case DROID_SCAN:
+ configureDroidScan(showTelemetry, isBeingInserted);
+ break;
+ case RAINBOW:
+ configureRainbow(showTelemetry, isBeingInserted);
+ break;
+ case SNAKES:
+ configureSnakes(showTelemetry, isBeingInserted);
+ break;
+ case RANDOM:
+ configureRandom(showTelemetry, isBeingInserted);
+ break;
+ case SPARKLE:
+ configureSparkle(showTelemetry, isBeingInserted);
+ break;
+ case SINGLE_FILL:
+ configureSingleFill(showTelemetry, isBeingInserted);
+ break;
+ case RAINBOW_SNAKES:
+ configureRainbowSnakes(showTelemetry, isBeingInserted);
+ break;
+ case POLICE_LIGHTS:
+ configurePoliceLights(showTelemetry, isBeingInserted);
+ break;
+ }
+ }
+
+ public void configureEndpointsAnimation(boolean isBeingInserted){
+ endpointsAnimation.setStartIndex(startPoint);
+ endpointsAnimation.setStopIndex(endPoint);
+ if(isBeingInserted){
+ prism.insertAndUpdateAnimation(selectedLayer.layerHeight, endpointsAnimation);
+ } else {
+ prism.updateAnimationFromIndex(selectedLayer.layerHeight);
+ }
+ }
+
+ /*
+ * Past this point, we have a function to create each type of animation and configure the
+ * specifics to it. As often as I could, you'll find that actions that multiple animations
+ * share (like setPrimaryColor) get abstracted into their own functions (here we have
+ * "hsbViaJoystick) to do the work. This avoids copy and pasting code.
+ */
+
+ public void configureSolid(boolean showTelemetry, boolean isBeingInserted){
+ solid.setPrimaryColor(hsbViaJoystick(solid.getPrimaryColor()));
+ solid.setStartIndex(startPoint);
+ solid.setStopIndex(endPoint);
+ solid.setBrightness(brightness);
+
+ if(isBeingInserted){
+ prism.insertAndUpdateAnimation(selectedLayer.layerHeight, solid);
+ } else {
+ prism.updateAnimationFromIndex(selectedLayer.layerHeight);
+ }
+
+ if(showTelemetry){
+ telemetry.addLine("Selected Animation: Solid");
+ showHsbTelemetry();
+ telemetry.addLine(hsbTelemetry);
+ telemetry.addLine("Press A to Continue");
+ telemetry.addLine("Press B to go back");
+ }
+ }
+
+ public void configureBlink(boolean showTelemetry, boolean isBeingInserted){
+ switch (animationColor){
+ case PRIMARY_COLOR:
+ blink.setPrimaryColor(hsbViaJoystick(blink.getPrimaryColor()));
+ break;
+ case SECONDARY_COLOR:
+ blink.setSecondaryColor(hsbViaJoystick(blink.getSecondaryColor()));
+ break;
+ }
+ toggleThroughColors(gamepad1.yWasPressed(),false);
+
+ blink.setStartIndex(startPoint);
+ blink.setStopIndex(endPoint);
+ blink.setBrightness(brightness);
+ blink.setPeriod(period);
+ blink.setPrimaryColorPeriod(period/2);
+
+ if(isBeingInserted){
+ prism.insertAndUpdateAnimation(selectedLayer.layerHeight, blink);
+ } else {
+ prism.updateAnimationFromIndex(selectedLayer.layerHeight);
+ }
+
+ if(showTelemetry){
+ telemetry.addLine("Selected Animation: Blink");
+ showHsbTelemetry();
+ telemetry.addLine(hsbTelemetry);
+ telemetry.addLine("");
+ telemetry.addLine("Click the Y button to switch between setting the primary and secondary color");
+ telemetry.addLine(animationColor.toString());
+ telemetry.addLine("");
+ telemetry.addLine("Press A to Continue");
+ telemetry.addLine("Press B to go back");
+ }
+ }
+
+ public void configurePulse(boolean showTelemetry, boolean isBeingInserted){
+ switch (animationColor){
+ case PRIMARY_COLOR:
+ pulse.setPrimaryColor(hsbViaJoystick(pulse.getPrimaryColor()));
+ break;
+ case SECONDARY_COLOR:
+ pulse.setSecondaryColor(hsbViaJoystick(pulse.getSecondaryColor()));
+ break;
+ }
+
+ toggleThroughColors(gamepad1.yWasPressed(),false);
+
+ pulse.setStartIndex(startPoint);
+ pulse.setStopIndex(endPoint);
+ pulse.setBrightness(brightness);
+ pulse.setPeriod(period);
+
+ if(isBeingInserted){
+ prism.insertAndUpdateAnimation(selectedLayer.layerHeight, pulse);
+ } else {
+ prism.updateAnimationFromIndex(selectedLayer.layerHeight);
+ }
+
+ if(showTelemetry){
+ telemetry.addLine("Selected Animation: Pulse");
+ showHsbTelemetry();
+ telemetry.addLine(hsbTelemetry);
+ telemetry.addLine("");
+ telemetry.addLine("Click the Y button to switch between setting the primary and secondary color");
+ telemetry.addLine(animationColor.toString());
+ telemetry.addLine("");
+ telemetry.addLine("Press A to Continue");
+ telemetry.addLine("Press B to go back");
+ }
+ }
+
+ public void configureSineWave(boolean showTelemetry, boolean isBeingInserted){
+ switch (animationColor){
+ case PRIMARY_COLOR:
+ sineWave.setPrimaryColor(hsbViaJoystick(sineWave.getPrimaryColor()));
+ break;
+ case SECONDARY_COLOR:
+ sineWave.setSecondaryColor(hsbViaJoystick(sineWave.getSecondaryColor()));
+ break;
+ }
+
+ toggleThroughColors(gamepad1.yWasPressed(),false);
+
+ sineWave.setStartIndex(startPoint);
+ sineWave.setStopIndex(endPoint);
+ sineWave.setBrightness(brightness);
+ sineWave.setPeriod(period);
+ sineWave.setSpeed(speed);
+
+ if(isBeingInserted){
+ prism.insertAndUpdateAnimation(selectedLayer.layerHeight, sineWave);
+ } else {
+ prism.updateAnimationFromIndex(selectedLayer.layerHeight);
+ }
+
+ if(showTelemetry){
+ telemetry.addLine("Selected Animation: Sine Wave");
+ showHsbTelemetry();
+ telemetry.addLine(hsbTelemetry);
+ telemetry.addLine("");
+ telemetry.addLine("Click the Y button to switch between setting the primary and secondary color");
+ telemetry.addLine(animationColor.toString());
+ telemetry.addLine("");
+ telemetry.addLine("Press A to Continue");
+ telemetry.addLine("Press B to go back");
+ }
+ }
+
+ public void configureDroidScan(boolean showTelemetry, boolean isBeingInserted){
+ switch (animationColor){
+ case PRIMARY_COLOR:
+ droidScan.setPrimaryColor(hsbViaJoystick(droidScan.getPrimaryColor()));
+ break;
+ case SECONDARY_COLOR:
+ droidScan.setSecondaryColor(hsbViaJoystick(droidScan.getSecondaryColor()));
+ break;
+ }
+
+ toggleThroughColors(gamepad1.yWasPressed(),false);
+
+ droidScan.setStartIndex(startPoint);
+ droidScan.setStopIndex(endPoint);
+ droidScan.setBrightness(brightness);
+ droidScan.setSpeed(speed);
+
+ if(isBeingInserted){
+ prism.insertAndUpdateAnimation(selectedLayer.layerHeight, droidScan);
+ } else {
+ prism.updateAnimationFromIndex(selectedLayer.layerHeight);
+ }
+
+ if(showTelemetry){
+ telemetry.addLine("Selected Animation: Droid Scan");
+ showHsbTelemetry();
+ telemetry.addLine(hsbTelemetry);
+ telemetry.addLine("");
+ telemetry.addLine("Click the Y button to switch between setting the primary and secondary color");
+ telemetry.addLine(animationColor.toString());
+ telemetry.addLine("");
+ telemetry.addLine("Press A to Continue");
+ telemetry.addLine("Press B to go back");
+ }
+ }
+
+ public void configureRainbow(boolean showTelemetry, boolean isBeingInserted){
+ float[] hues = hueViaJoystick(rainbow.getStartHue(),rainbow.getStopHue());
+ rainbow.setStartHue(hues[0]);
+ rainbow.setStopHue(hues[1]);
+
+ rainbow.setStartIndex(startPoint);
+ rainbow.setStopIndex(endPoint);
+ rainbow.setBrightness(brightness);
+ rainbow.setSpeed(speed);
+
+ if(isBeingInserted){
+ prism.insertAndUpdateAnimation(selectedLayer.layerHeight, rainbow);
+ } else {
+ prism.updateAnimationFromIndex(selectedLayer.layerHeight);
+ }
+ if(showTelemetry){
+ telemetry.addLine("Selected Animation: Rainbow");
+ telemetry.addLine("Move the joysticks to configure the range of the rainbow");
+ telemetry.addLine("Left Joystick X (side-to-side) changes the start hue");
+ telemetry.addLine("Right Joystick x (side-to-side) changes the end hue");
+ telemetry.addLine("");
+ telemetry.addLine(hueTelemetry);
+ telemetry.addLine("");
+ telemetry.addLine("Press A to Continue");
+ telemetry.addLine("Press B to go back");
+ }
+ }
+
+ public void configureSnakes(boolean showTelemetry, boolean isBeingInserted){
+ snakes.setColors(hsbViaJoystick(snakes.getColors()[0]));
+
+ snakes.setStartIndex(startPoint);
+ snakes.setStopIndex(endPoint);
+ snakes.setBrightness(brightness);
+ snakes.setSpeed(speed);
+
+ if(isBeingInserted){
+ prism.insertAndUpdateAnimation(selectedLayer.layerHeight, snakes);
+ } else {
+ prism.updateAnimationFromIndex(selectedLayer.layerHeight);
+ }
+
+ if(showTelemetry) {
+ telemetry.addLine("Selected Animation: Snakes");
+ showHsbTelemetry();
+ telemetry.addLine(hsbTelemetry);
+ telemetry.addLine("");
+ telemetry.addLine("Press A to Continue");
+ telemetry.addLine("Press B to go back");
+ }
+ }
+
+ public void configureRandom(boolean showTelemetry, boolean isBeingInserted){
+ float[] hues = hueViaJoystick(random.getStartHue(), random.getStopHue());
+ random.setStartHue(hues[0]);
+ random.setStopHue(hues[1]);
+
+ random.setStartIndex(startPoint);
+ random.setStopIndex(endPoint);
+ random.setBrightness(brightness);
+ random.setSpeed(speed);
+
+ if(isBeingInserted){
+ prism.insertAndUpdateAnimation(selectedLayer.layerHeight, random);
+ } else {
+ prism.updateAnimationFromIndex(selectedLayer.layerHeight);
+ }
+
+ if(showTelemetry){
+ telemetry.addLine("Selected Animation: Random");
+ telemetry.addLine("Move the joysticks to configure the range of the random colors");
+ telemetry.addLine("Left Joystick X (side-to-side) changes the start hue");
+ telemetry.addLine("Right Joystick x (side-to-side) changes the end hue");
+ telemetry.addLine("");
+ telemetry.addLine(hueTelemetry);
+ telemetry.addLine("");
+ telemetry.addLine("Press A to Continue");
+ telemetry.addLine("Press B to go back");
+ }
+ }
+
+ public void configureSparkle(boolean showTelemetry, boolean isBeingInserted){
+ switch (animationColor){
+ case PRIMARY_COLOR:
+ sparkle.setPrimaryColor(hsbViaJoystick(sparkle.getPrimaryColor()));
+ break;
+ case SECONDARY_COLOR:
+ sparkle.setSecondaryColor(hsbViaJoystick(sparkle.getSecondaryColor()));
+ break;
+ }
+
+ toggleThroughColors(gamepad1.yWasPressed(),false);
+
+ sparkle.setStartIndex(startPoint);
+ sparkle.setStopIndex(endPoint);
+ sparkle.setBrightness(brightness);
+ sparkle.setPeriod(period);
+
+ if(isBeingInserted){
+ prism.insertAndUpdateAnimation(selectedLayer.layerHeight, sparkle);
+ } else {
+ prism.updateAnimationFromIndex(selectedLayer.layerHeight);
+ }
+
+ if(showTelemetry){
+ telemetry.addLine("Selected Animation: Sparkle");
+ showHsbTelemetry();
+ telemetry.addLine(hsbTelemetry);
+ telemetry.addLine("");
+ telemetry.addLine("Click the Y button to switch between setting the primary and secondary color");
+ telemetry.addLine(animationColor.toString());
+ telemetry.addLine("");
+ telemetry.addLine("Press A to Continue");
+ telemetry.addLine("Press B to go back");
+ }
+ }
+
+ public void configureSingleFill(boolean showTelemetry, boolean isBeingInserted){
+ switch (animationColor){
+ case PRIMARY_COLOR:
+ singleFillColors[0] = hsbViaJoystick(singleFill.getColors()[0]);
+ break;
+ case SECONDARY_COLOR:
+ singleFillColors[1] = hsbViaJoystick(singleFill.getColors()[1]);
+ break;
+ case TERTIARY_COLOR:
+ singleFillColors[2] = hsbViaJoystick(singleFill.getColors()[2]);
+ break;
+ }
+
+ toggleThroughColors(gamepad1.yWasPressed(),true);
+
+ singleFill.setColors(singleFillColors);
+
+ singleFill.setStartIndex(startPoint);
+ singleFill.setStopIndex(endPoint);
+ singleFill.setBrightness(brightness);
+ singleFill.setSpeed(speed);
+ singleFill.setPeriod(period);
+
+ if(isBeingInserted){
+ prism.insertAndUpdateAnimation(selectedLayer.layerHeight, singleFill);
+ } else {
+ prism.updateAnimationFromIndex(selectedLayer.layerHeight);
+ }
+
+ if(showTelemetry){
+ telemetry.addLine("Selected Animation: Single Fill");
+ telemetry.addLine("");
+ showHsbTelemetry();
+ telemetry.addLine(hsbTelemetry);
+ telemetry.addLine("");
+ telemetry.addLine("Click the Y button to switch between setting the primary, secondary, and tertiary color");
+ telemetry.addLine(animationColor.toString());
+ telemetry.addLine("");
+ telemetry.addLine("Press A to Continue");
+ telemetry.addLine("Press B to go back");
+ }
+ }
+
+ public void configureRainbowSnakes(boolean showTelemetry, boolean isBeingInserted){
+ float[] hues = hueViaJoystick(rainbowSnakes.getStartHue(),rainbowSnakes.getStopHue());
+ rainbowSnakes.setStartHue(hues[0]);
+ rainbowSnakes.setStopHue(hues[1]);
+
+ rainbowSnakes.setStartIndex(startPoint);
+ rainbowSnakes.setStopIndex(endPoint);
+ rainbowSnakes.setBrightness(brightness);
+ rainbowSnakes.setSpeed(speed);
+
+ if(isBeingInserted){
+ prism.insertAndUpdateAnimation(selectedLayer.layerHeight, rainbowSnakes);
+ } else {
+ prism.updateAnimationFromIndex(selectedLayer.layerHeight);
+ }
+
+ if(showTelemetry){
+ telemetry.addLine("Selected Animation: Rainbow Snakes");
+ telemetry.addLine("Move the joysticks to configure the range of the rainbow");
+ telemetry.addLine("Left Joystick X (side-to-side) changes the start hue");
+ telemetry.addLine("Right Joystick x (side-to-side) changes the end hue");
+ telemetry.addLine("");
+ telemetry.addLine(hueTelemetry);
+ telemetry.addLine("");
+ telemetry.addLine("Press A to Continue");
+ telemetry.addLine("Press B to go back");
+ }
+ }
+
+ public void configurePoliceLights(boolean showTelemetry, boolean isBeingInserted){
+ switch (animationColor){
+ case PRIMARY_COLOR:
+ policeLights.setPrimaryColor(hsbViaJoystick(policeLights.getPrimaryColor()));
+ break;
+ case SECONDARY_COLOR:
+ policeLights.setSecondaryColor(hsbViaJoystick(policeLights.getSecondaryColor()));
+ break;
+ case TERTIARY_COLOR:
+ policeLights.setTertiaryColor(hsbViaJoystick(policeLights.getTertiaryColor()));
+ break;
+ }
+
+ toggleThroughColors(gamepad1.yWasPressed(),true);
+
+ switch (policeLightsStyle){
+ case Style1:
+ if(gamepad1.rightStickButtonWasPressed()){
+ policeLightsStyle = PoliceLights.PoliceLightsStyle.Style2;
+ }
+ break;
+ case Style2:
+ if(gamepad1.rightStickButtonWasPressed()){
+ policeLightsStyle = PoliceLights.PoliceLightsStyle.Style3;
+ }
+ break;
+ case Style3:
+ if(gamepad1.rightStickButtonWasPressed()){
+ policeLightsStyle = PoliceLights.PoliceLightsStyle.Style1;
+ }
+ break;
+ }
+
+ policeLights.setStartIndex(startPoint);
+ policeLights.setStopIndex(endPoint);
+ policeLights.setBrightness(brightness);
+ policeLights.setPoliceLightsStyle(policeLightsStyle);
+ policeLights.setPeriod(period);
+
+ if(isBeingInserted){
+ prism.insertAndUpdateAnimation(selectedLayer.layerHeight, policeLights);
+ } else {
+ prism.updateAnimationFromIndex(selectedLayer.layerHeight);
+ }
+
+ if(showTelemetry){
+ telemetry.addLine("Selected Animation: Police Lights");
+ telemetry.addLine("");
+ telemetry.addLine("Toggle between styles by clicking the right joystick");
+ telemetry.addData("Police Lights Style: ",policeLightsStyle);
+ telemetry.addLine("");
+ showHsbTelemetry();
+ telemetry.addLine(hsbTelemetry);
+ telemetry.addLine("");
+ telemetry.addLine("Click the Y button to switch between setting the primary, secondary, and tertiary color");
+ telemetry.addLine(animationColor.toString());
+ telemetry.addLine("");
+ telemetry.addLine("Press A to Continue");
+ telemetry.addLine("Press B to go back");
+ }
+ }
+
+ public void resetStoredAnimations(){
+ Layers.LAYER_0.animationType = AnimationType.NONE;
+ Layers.LAYER_1.animationType = AnimationType.NONE;
+ Layers.LAYER_2.animationType = AnimationType.NONE;
+ Layers.LAYER_3.animationType = AnimationType.NONE;
+ Layers.LAYER_4.animationType = AnimationType.NONE;
+ Layers.LAYER_5.animationType = AnimationType.NONE;
+ Layers.LAYER_6.animationType = AnimationType.NONE;
+ Layers.LAYER_7.animationType = AnimationType.NONE;
+ Layers.LAYER_8.animationType = AnimationType.NONE;
+ Layers.LAYER_9.animationType = AnimationType.NONE;
+ }
+
+ /*
+ * Reasonably, you might be wondering why I'm configuring the colors in HSB/HSL instead
+ * of RGB - The sane choice. And it's all to create a more intuitive user experience.
+ * Having the user create a color by combining sliders of red, green, and blue is very true
+ * to the colorspace we are actually working in, but folks I tried this on found it very
+ * difficult. Hue/Saturation/Brightness allows us to create one slider for each three intuitive
+ * variables. Hue changes the color, saturation changes the intensity of the color, and brightness
+ * changes, well the brightness.
+ * Actually implementing this isn't very clean, but the result for the user is a better experience.
+ */
+ public Color hsbViaJoystick(Color previousColor){
+ final float HUE_JOYSTICK_SCALAR = 5;
+ final float SATURATION_JOYSTICK_SCALAR = 0.05F;
+ final float BRIGHTNESS_JOYSTICK_SCALAR = 0.05f;
+
+ float[] hsb = new float[3]; // Android graphics library wants an array containing RGB values.
+ android.graphics.Color.RGBToHSV(previousColor.red,previousColor.green,previousColor.blue,hsb);
+
+ /*
+ * Here we let the user increase or decrease H, S, or B with the joystick.
+ * Pushing the stick more moves the value more.
+ */
+ hsb[0] = Math.max(Math.min(hsb[0] +(gamepad1.left_stick_x*HUE_JOYSTICK_SCALAR), 360),0);
+ hsb[1] = Math.max(Math.min(hsb[1] +(gamepad1.right_stick_x*SATURATION_JOYSTICK_SCALAR), 1),0);
+ hsb[2] = Math.max(Math.min(hsb[2] +(-gamepad1.right_stick_y*BRIGHTNESS_JOYSTICK_SCALAR), 0.98f),0);
+
+ /*
+ * Here we create an integer where the Android graphics library will store each component of
+ * our RGB color one-after-another. I hope we can agree that this is cursed.
+ */
+ int colorInt = android.graphics.Color.HSVToColor(hsb);
+ Color color = new Color(0,0,0); // Create a new color to return.
+
+ /*
+ * One of the big downsides in this multi-color model is that some behavior isn't very
+ * intuitive. In this case as you approach a saturation of 1, the number of hues you can
+ * display are limited to very pure versions of red, green, and blue. To avoid this we
+ * limit the maximum saturation to 0.98 (this limit happens above) and to reduce confusion,
+ * we display this to the user over telemetry as 1.0.
+ */
+ if(hsb[2] > 0.97){
+ hsb[2] = 1.0f;
+ }
+ hsbTelemetry = String.format("Hue/Saturation/Brightness: %4.2f %4.2f %4.2f", hsb[0], hsb[1], hsb[2]);
+
+ color.red = android.graphics.Color.red(colorInt);
+ color.green = android.graphics.Color.green(colorInt);
+ color.blue = android.graphics.Color.blue(colorInt);
+ return color;
+ }
+
+ public float[] hueViaJoystick(float previousStartHue, float previousEndHue){
+ final float HUE_JOYSTICK_SCALAR = 5;
+ float[] hues = new float[2];
+
+ hues[0] = Math.max(Math.min(previousStartHue +(gamepad1.left_stick_x*HUE_JOYSTICK_SCALAR), 360),0);
+ hues[1] = Math.max(Math.min(previousEndHue +(gamepad1.right_stick_x*HUE_JOYSTICK_SCALAR),360),0);
+
+ hueTelemetry = String.format("Start hue/end hue: %4.2f %4.2f", hues[0], hues[1]);
+
+ return hues;
+ }
+
+ public void showHsbTelemetry(){
+ telemetry.addLine("Move the joysticks to configure the color of the LEDs");
+ telemetry.addLine("Left Joystick X (side-to-side) Changes the Hue of a color");
+ telemetry.addLine("Right Joystick X (side-to-side) changes the Saturation");
+ telemetry.addLine("Right Joystick Y (up-and-down) changes the Brightness");
+ telemetry.addLine("");
+ }
+
+ public void toggleThroughColors(boolean button, boolean thirdColor){
+ if(button){
+ switch (animationColor){
+ case PRIMARY_COLOR:
+ animationColor = AnimationColor.SECONDARY_COLOR;
+ break;
+ case SECONDARY_COLOR:
+ if(thirdColor){
+ animationColor = AnimationColor.TERTIARY_COLOR;
+ } else{
+ animationColor = AnimationColor.PRIMARY_COLOR;
+ }
+ break;
+ case TERTIARY_COLOR:
+ animationColor = AnimationColor.PRIMARY_COLOR;
+ break;
+ }
+ }
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/Prism/GoBildaPrismExample.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/Prism/GoBildaPrismExample.java
new file mode 100755
index 0000000..a59c55f
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/Prism/GoBildaPrismExample.java
@@ -0,0 +1,148 @@
+/* MIT License
+ * Copyright (c) [2025] [Base 10 Assets, LLC]
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+package org.firstinspires.ftc.teamcode.tuning.Prism;
+
+
+import static org.firstinspires.ftc.teamcode.Prism.GoBildaPrismDriver.LayerHeight;
+
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import org.firstinspires.ftc.teamcode.Prism.Color;
+import org.firstinspires.ftc.teamcode.Prism.GoBildaPrismDriver;
+import org.firstinspires.ftc.teamcode.Prism.PrismAnimations;
+
+import java.util.concurrent.TimeUnit;
+
+/*
+ * This example file shows how to create a couple of different Animations on the Prism, and save
+ * them to an Artboard on the device.
+ * The example file called "GoBildaPrismConfigurator" is designed to create a user-interface
+ * through your driver's station. This is a great way to create Artboards from Animations, but
+ * doesn't let you control quite everything about the available Animations. If you'd like to
+ * do something more complex, or would just prefer to create your Artboard in Java, this
+ * program shows you how.
+ * The example file called "GoBildaPrismArtboardExample" shows how to recall different Artboards
+ * that you have already saved to the device. That file shows the code you should consider adding
+ * to your OpMode if you would like to dynamically change the Artboard shown by your LEDs during
+ * the match.
+ *
+ * Core to understanding how to use this product is knowing these three terms:
+ * Animations: (Like RAINBOW or BLINK) - These have properties you can configure, like their color.
+ * They can have unique start and end points!
+ * Layers: There are 10 layers, each of which can store an animation. These are hierarchical.
+ * So an Animation on layer 5 will cover an animation on layer 2 if they overlap.
+ * You can use start and end points to have layers overlap to create new patterns! Or show multiple
+ * animations at once on different LEDs.
+ * Artboards: An Artboard is a set of 10 layers which is stored on the Prism.
+ * you can have up to 8 unique Artboards. Artboards are easy and computationally fast to switch between.
+ */
+
+@TeleOp(name="Prism Animations Example", group="Linear OpMode")
+//@Disabled
+
+public class GoBildaPrismExample extends LinearOpMode {
+
+ GoBildaPrismDriver prism;
+
+ PrismAnimations.Solid solid = new PrismAnimations.Solid(Color.BLUE);
+ PrismAnimations.RainbowSnakes rainbowSnakes = new PrismAnimations.RainbowSnakes();
+
+ @Override
+ public void runOpMode() {
+ /*
+ * Initialize the hardware variables. Note that the strings used here must correspond
+ * to the names assigned during the robot configuration step on the driver's station.
+ */
+ prism = hardwareMap.get(GoBildaPrismDriver.class,"prism");
+
+ /*
+ * Here you can customize the specifics of different animations. Each animation has it's
+ * own set of parameters that you can customize to create something unique! Each animation
+ * has carefully selected default parameters. So you do not need to set each parameter
+ * for every animation!
+ */
+ solid.setBrightness(50);
+ solid.setStartIndex(0);
+ solid.setStopIndex(12);
+
+ rainbowSnakes.setNumberOfSnakes(2);
+ rainbowSnakes.setSnakeLength(3);
+ rainbowSnakes.setSpacingBetween(6);
+ rainbowSnakes.setSpeed(0.5f);
+
+ telemetry.addData("Device ID: ", prism.getDeviceID());
+ telemetry.addData("Firmware Version: ", prism.getFirmwareVersionString());
+ telemetry.addData("Hardware Version: ", prism.getHardwareVersionString());
+ telemetry.addData("Power Cycle Count: ", prism.getPowerCycleCount());
+ telemetry.update();
+
+ // Wait for the game to start (driver presses START)
+ waitForStart();
+ resetRuntime();
+
+ // run until the end of the match (driver presses STOP)
+ while (opModeIsActive()) {
+
+
+ if(gamepad1.aWasPressed()){
+ /*
+ * Here we insert and update the animation to the Prism, this by default does not
+ * save it to an Artboard, it just starts the Animation playing. If you have
+ * already inserted an animation at a layer height, you can instead call
+ * .updateAnimationFromIndex(LayerHeight.LAYER_0) to update an animation at a
+ * specific layer height without overwriting it completely.
+ */
+ prism.insertAndUpdateAnimation(LayerHeight.LAYER_0, solid);
+ prism.insertAndUpdateAnimation(LayerHeight.LAYER_1,rainbowSnakes);
+ }
+
+ if(gamepad1.xWasPressed()){
+ /*
+ * Clearing the animation doesn't erase any saved Artboards, but it removes all the
+ * currently displayed animations.
+ */
+ prism.clearAllAnimations();
+ }
+
+ if(gamepad1.dpadDownWasPressed()){
+ /*
+ * Here we save the animation we are currently displaying to Artboard 0.
+ */
+ prism.saveCurrentAnimationsToArtboard(GoBildaPrismDriver.Artboard.ARTBOARD_0);
+ }
+
+ telemetry.addLine("Press A to insert and update the created animations.");
+ telemetry.addLine("Press X to clear current animations.");
+ telemetry.addLine("Press D-Pad Down to save current animations to Artboard #0");
+ telemetry.addLine();
+ telemetry.addData("Run Time (Hours): ",prism.getRunTime(TimeUnit.HOURS));
+ telemetry.addData("Run Time (Minutes): ",prism.getRunTime(TimeUnit.MINUTES));
+ telemetry.addData("Number of LEDS: ", prism.getNumberOfLEDs());
+ telemetry.addData("Current FPS: ", prism.getCurrentFPS());
+ telemetry.update();
+ sleep(50);
+ }
+ }
+
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TestBench.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TestBench.java
new file mode 100755
index 0000000..1dfa95f
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TestBench.java
@@ -0,0 +1,291 @@
+package org.firstinspires.ftc.teamcode.tuning;
+
+import com.bylazar.telemetry.PanelsTelemetry;
+import com.bylazar.telemetry.TelemetryManager;
+import com.bylazar.configurables.annotations.Configurable;
+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.DcMotorEx;
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
+import com.qualcomm.robotcore.hardware.Servo;
+
+@Configurable
+@TeleOp(name = "TestBench", group = "Maintenance")
+public class TestBench extends LinearOpMode {
+
+ public static String SERVO_NAME = "servo";
+ public static boolean IS_CR_SERVO = false;
+
+ public static String MOTOR_NAME = "motor";
+ public static boolean IS_MOTOR_EX = false;
+
+ public static String ODO_LEFT_NAME = "intake";
+ public static String ODO_RIGHT_NAME = "fl";
+ public static String ODO_STRAFE_NAME = "fr";
+ public static boolean REVERSE_LEFT_ENCODER = false;
+ public static boolean REVERSE_RIGHT_ENCODER = false;
+ public static boolean REVERSE_STRAFE_ENCODER = false;
+
+ private enum Mode {
+ SERVO_TESTER,
+ MOTOR_TESTER,
+ ODOMETRY_TUNER
+ }
+
+ @Override
+ public void runOpMode() {
+ Mode[] modes = Mode.values();
+ int modeIndex = 0;
+ boolean locked = false;
+ boolean lastUp = false;
+ boolean lastDown = false;
+
+ while (!isStopRequested() && !locked) {
+ boolean currentUp = gamepad1.dpad_up;
+ boolean currentDown = gamepad1.dpad_down;
+
+ if (currentUp && !lastUp) {
+ modeIndex--;
+ if (modeIndex < 0) modeIndex = modes.length - 1;
+ }
+ if (currentDown && !lastDown) {
+ modeIndex++;
+ if (modeIndex >= modes.length) modeIndex = 0;
+ }
+ lastUp = currentUp;
+ lastDown = currentDown;
+
+ if (gamepad1.a) {
+ locked = true;
+ }
+
+ telemetry.addLine("▰▰▰▰▰▰▰ SYSTEM CONFIG MENU ▰▰▰▰▰▰▰");
+ telemetry.addLine("Use DPAD to scroll, A to select");
+ telemetry.addLine();
+
+ for (int i = 0; i < modes.length; i++) {
+ if (i == modeIndex) {
+ telemetry.addData(">>", modes[i].name());
+ } else {
+ telemetry.addData(" ", modes[i].name());
+ }
+ }
+
+ telemetry.addLine();
+ telemetry.addLine("Current Settings:");
+ if (modes[modeIndex] == Mode.SERVO_TESTER) {
+ telemetry.addData("Target", SERVO_NAME);
+ telemetry.addData("Type", IS_CR_SERVO ? "Continuous" : "Positional");
+ } else if (modes[modeIndex] == Mode.MOTOR_TESTER) {
+ telemetry.addData("Target", MOTOR_NAME);
+ telemetry.addData("Mode", IS_MOTOR_EX ? "Velocity (Ex)" : "Power (Std)");
+ } else {
+ telemetry.addData("Left", ODO_LEFT_NAME);
+ telemetry.addData("Right", ODO_RIGHT_NAME);
+ telemetry.addData("Strafe", ODO_STRAFE_NAME);
+ }
+ telemetry.update();
+ }
+
+ if (isStopRequested()) return;
+
+ Mode selectedMode = modes[modeIndex];
+ Servo posServo = null;
+ CRServo crServo = null;
+ DcMotor simpleMotor = null;
+ DcMotorEx exMotor = null;
+ DcMotor leftOdo = null, rightOdo = null, strafeOdo = null;
+
+ try {
+ switch (selectedMode) {
+ case SERVO_TESTER:
+ if (IS_CR_SERVO) {
+ crServo = hardwareMap.get(CRServo.class, SERVO_NAME);
+ } else {
+ posServo = hardwareMap.get(Servo.class, SERVO_NAME);
+ }
+ break;
+ case MOTOR_TESTER:
+ if (IS_MOTOR_EX) {
+ exMotor = hardwareMap.get(DcMotorEx.class, MOTOR_NAME);
+ exMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+ exMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+ } else {
+ simpleMotor = hardwareMap.get(DcMotor.class, MOTOR_NAME);
+ simpleMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+ simpleMotor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+ }
+ break;
+ case ODOMETRY_TUNER:
+ leftOdo = hardwareMap.get(DcMotor.class, ODO_LEFT_NAME);
+ rightOdo = hardwareMap.get(DcMotor.class, ODO_RIGHT_NAME);
+ strafeOdo = hardwareMap.get(DcMotor.class, ODO_STRAFE_NAME);
+
+ leftOdo.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+ rightOdo.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+ strafeOdo.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+
+ leftOdo.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+ rightOdo.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+ strafeOdo.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+ break;
+ }
+ } catch (Exception e) {
+ telemetry.addData("INIT ERROR", "Hardware not found. Check names.");
+ telemetry.update();
+ sleep(5000);
+ return;
+ }
+
+ waitForStart();
+
+ double servoPos = 0.5;
+ double velocityTarget = 0.0;
+ double velocityIncrement = 10.0;
+ boolean directionReverse = false;
+ boolean lastB = false;
+ boolean lastLeft = false, lastRight = false, lastUpVal = false, lastDownVal = false;
+
+ double prevLeft = 0, prevRight = 0, prevStrafe = 0;
+
+ while (opModeIsActive()) {
+ boolean currentB = gamepad1.b;
+ if (currentB && !lastB) {
+ directionReverse = !directionReverse;
+ }
+ lastB = currentB;
+
+ switch (selectedMode) {
+ case SERVO_TESTER:
+ if (IS_CR_SERVO && crServo != null) {
+ double power = gamepad1.right_trigger - gamepad1.left_trigger;
+ if (directionReverse) power *= -1;
+ crServo.setPower(power);
+ telemetry.addData("Type", "CR Servo");
+ telemetry.addData("Power", "%.2f", power);
+ telemetry.addData("Direction", directionReverse ? "REVERSED" : "FORWARD");
+ } else if (posServo != null) {
+ boolean dUp = gamepad1.dpad_up;
+ boolean dDown = gamepad1.dpad_down;
+
+ if (dUp && !lastUpVal) servoPos = Math.min(1.0, servoPos + 0.01);
+ if (dDown && !lastDownVal) servoPos = Math.max(0.0, servoPos - 0.01);
+
+ lastUpVal = dUp;
+ lastDownVal = dDown;
+
+ double outputPos = servoPos;
+ if (directionReverse) {
+ posServo.setDirection(Servo.Direction.REVERSE);
+ } else {
+ posServo.setDirection(Servo.Direction.FORWARD);
+ }
+
+ posServo.setPosition(outputPos);
+ telemetry.addData("Type", "Position Servo");
+ telemetry.addData("Target Pos", "%.2f", servoPos);
+ telemetry.addData("Direction", directionReverse ? "REVERSED" : "FORWARD");
+ }
+ break;
+
+ case MOTOR_TESTER:
+ if (IS_MOTOR_EX && exMotor != null) {
+ boolean dLeft = gamepad1.dpad_left;
+ boolean dRight = gamepad1.dpad_right;
+ boolean dUp = gamepad1.dpad_up;
+ boolean dDown = gamepad1.dpad_down;
+
+ if (dRight && !lastRight) {
+ if (velocityIncrement == 0.1) velocityIncrement = 1;
+ else if (velocityIncrement == 1) velocityIncrement = 10;
+ else if (velocityIncrement == 10) velocityIncrement = 100;
+ }
+ if (dLeft && !lastLeft) {
+ if (velocityIncrement == 100) velocityIncrement = 10;
+ else if (velocityIncrement == 10) velocityIncrement = 1;
+ else if (velocityIncrement == 1) velocityIncrement = 0.1;
+ }
+
+ if (dUp && !lastUpVal) velocityTarget += velocityIncrement;
+ if (dDown && !lastDownVal) velocityTarget -= velocityIncrement;
+
+ lastRight = dRight;
+ lastLeft = dLeft;
+ lastUpVal = dUp;
+ lastDownVal = dDown;
+
+ double finalVel = directionReverse ? -velocityTarget : velocityTarget;
+ exMotor.setVelocity(finalVel);
+
+ telemetry.addData("Type", "DcMotor Ex");
+ telemetry.addData("Target Vel", "%.1f", finalVel);
+ telemetry.addData("Actual Vel", "%.1f", exMotor.getVelocity());
+ telemetry.addData("Position", exMotor.getCurrentPosition());
+ telemetry.addData("Increment", velocityIncrement);
+ } else if (simpleMotor != null) {
+ double power = gamepad1.right_trigger - gamepad1.left_trigger;
+ if (Math.abs(power) > 1.0) power = Math.signum(power);
+ if (directionReverse) power *= -1;
+
+ simpleMotor.setPower(power);
+
+ telemetry.addData("Type", "DcMotor Standard");
+ telemetry.addData("Power", "%.2f", power);
+ telemetry.addData("Direction", directionReverse ? "REVERSED" : "FORWARD");
+ telemetry.addData("Position", simpleMotor.getCurrentPosition());
+ }
+ break;
+
+ case ODOMETRY_TUNER:
+ double rLeft = leftOdo.getCurrentPosition();
+ double rRight = rightOdo.getCurrentPosition();
+ double rStrafe = strafeOdo.getCurrentPosition();
+
+ double vLeft = REVERSE_LEFT_ENCODER ? -rLeft : rLeft;
+ double vRight = REVERSE_RIGHT_ENCODER ? -rRight : rRight;
+ double vStrafe = REVERSE_STRAFE_ENCODER ? -rStrafe : rStrafe;
+
+ double dLeft = vLeft - prevLeft;
+ double dRight = vRight - prevRight;
+ double dStrafe = vStrafe - prevStrafe;
+
+ prevLeft = vLeft;
+ prevRight = vRight;
+ prevStrafe = vStrafe;
+
+ telemetry.addLine("▰▰▰▰▰▰▰ READINGS ▰▰▰▰▰▰▰");
+ telemetry.addData("L", "%.0f (Raw: %.0f)", vLeft, rLeft);
+ telemetry.addData("R", "%.0f (Raw: %.0f)", vRight, rRight);
+ telemetry.addData("S", "%.0f (Raw: %.0f)", vStrafe, rStrafe);
+
+ telemetry.addLine("\n▰▰▰▰▰▰▰ DELTAS ▰▰▰▰▰▰▰");
+ telemetry.addData("L_Delta", "%.0f", dLeft);
+ telemetry.addData("R_Delta", "%.0f", dRight);
+ telemetry.addData("S_Delta", "%.0f", dStrafe);
+
+ telemetry.addLine("\n▰▰▰▰▰▰▰ ANALYSIS ▰▰▰▰▰▰▰");
+ if (Math.abs(dLeft) > 5 && Math.abs(dRight) > 5) {
+ if (Math.signum(dLeft) == Math.signum(dRight)) {
+ telemetry.addData("Motion", "FORWARD/BACK");
+ if (Math.abs(dStrafe) > Math.abs(dLeft) * 0.5) {
+ telemetry.addData("WARNING", "Strafe Pod Leaking/Crooked");
+ }
+ } else {
+ telemetry.addData("Motion", "TURNING");
+ }
+ } else if (Math.abs(dStrafe) > 5) {
+ telemetry.addData("Motion", "STRAFING");
+ if (Math.abs(dLeft) > 5 || Math.abs(dRight) > 5) {
+ telemetry.addData("WARNING", "Drive Pods Leaking");
+ }
+ } else {
+ telemetry.addData("Motion", "STATIONARY");
+ }
+ break;
+ }
+ telemetry.update();
+ }
+ }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TurretRotationTuning.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TurretRotationTuning.java
new file mode 100755
index 0000000..80e03f2
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TurretRotationTuning.java
@@ -0,0 +1,225 @@
+package org.firstinspires.ftc.teamcode.tuning;
+
+import com.bylazar.configurables.annotations.Configurable;
+import com.bylazar.telemetry.PanelsTelemetry;
+import com.bylazar.telemetry.TelemetryManager;
+import com.qualcomm.hardware.limelightvision.LLResult;
+import com.qualcomm.hardware.limelightvision.Limelight3A;
+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.util.ElapsedTime;
+
+import org.firstinspires.ftc.teamcode.util.FPSCounter;
+
+@Configurable
+@TeleOp(name = "Turret Motor Tuning", group = "Turret")
+public class TurretRotationTuning extends LinearOpMode {
+
+ // --- PID GAINS ---
+ public static double kP = 0.02;
+ public static double kI = 0;
+ public static double kD = 0.001;
+
+ public static double MAX_POWER = 0.5;
+ public static double MAX_INTEGRAL = 0.5;
+
+ // --- MECHANICAL CONSTANTS ---
+ public static double TICKS_PER_REV_MOTOR = 384.5;
+ public static double EXTERNAL_GEAR_RATIO = 2.68888889;
+ public static boolean MOTOR_REVERSED = true;
+
+ // --- CALIBRATION SETTINGS ---
+ public static boolean ENABLE_AUTO_RATIO = false;
+ public static double CALIBRATION_SWEEP_DEGREES = 180.0;
+
+ // --- TARGETING SETTINGS ---
+ public static double TARGET_OFFSET_DEGREES = -2;
+ public static double SOFT_LIMIT_NEG = -230;
+ public static double SOFT_LIMIT_POS = 230;
+ public static double WRAP_THRESHOLD_DEGREES = 180.0;
+
+ public static int targetFPS = 140;
+
+ private Limelight3A limelight;
+ private DcMotorEx turretMotor;
+ private TelemetryManager telemetryM;
+
+ private double integralSum = 0;
+ private double lastError = 0;
+
+ private double zeroOffsetTicks = 0;
+ private double startSweepTicks = 0;
+ private boolean isCalibratingSweep = false;
+ private boolean lastReverseState = false;
+
+ private double targetTicks = 0;
+ private double ticksPerDegree = 0;
+
+ private enum TuningMode {
+ CALIBRATION,
+ PID_ACTIVE
+ }
+
+ private TuningMode currentMode = TuningMode.PID_ACTIVE;
+ private final ElapsedTime timer = new ElapsedTime();
+ FPSCounter fps = new FPSCounter();
+
+ @Override
+ public void runOpMode() {
+
+ telemetryM = PanelsTelemetry.INSTANCE.getTelemetry();
+ turretMotor = hardwareMap.get(DcMotorEx.class, "turretRotation");
+ limelight = hardwareMap.get(Limelight3A.class, "limelight");
+
+ turretMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+ turretMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+ turretMotor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+
+ applyMotorDirection();
+
+ // Initial calculation based on provided static ratio
+ ticksPerDegree = (TICKS_PER_REV_MOTOR * EXTERNAL_GEAR_RATIO) / 360.0;
+
+ limelight.pipelineSwitch(1);
+ limelight.start();
+
+ telemetryM.debug("Status", "Initialized");
+ telemetryM.update(telemetry);
+
+ waitForStart();
+ timer.reset();
+
+ while (opModeIsActive()) {
+ fps.startLoop();
+ double dt = timer.seconds();
+ timer.reset();
+ if (dt < 0.001) dt = 0.001;
+
+ if (MOTOR_REVERSED != lastReverseState) {
+ applyMotorDirection();
+ }
+
+ double rawTicks = turretMotor.getCurrentPosition();
+ double currentTicks = rawTicks - zeroOffsetTicks;
+ double currentDegrees = currentTicks / ticksPerDegree;
+
+ switch (currentMode) {
+ case CALIBRATION:
+ handleCalibrationMode(rawTicks);
+ targetTicks = currentTicks; // Keep target at current position to avoid jumps
+ break;
+
+ case PID_ACTIVE:
+ handlePidMode(currentTicks, dt);
+ break;
+ }
+
+ sleep(fps.getSyncTime(targetFPS));
+
+ // --- TELEMETRY ---
+ telemetryM.debug("▰▰▰▰▰ MODE ▰▰▰▰▰", currentMode.toString());
+ telemetryM.debug("Gear Ratio", EXTERNAL_GEAR_RATIO);
+ telemetryM.debug("Ticks/Deg", ticksPerDegree);
+ telemetryM.debug("Adj Ticks", currentTicks);
+ telemetryM.debug("Degrees", currentDegrees);
+ telemetryM.debug("Target Deg", targetTicks / ticksPerDegree);
+ telemetry.addData("FPS", fps.getAverageFps());
+ telemetryM.update(telemetry);
+ }
+ limelight.stop();
+ }
+
+ private void handleCalibrationMode(double rawTicks) {
+ // Slow manual control for alignment
+ double manualPower = -gamepad1.left_stick_y * 0.4;
+ turretMotor.setPower(manualPower);
+
+ // 1. Reset Local Zero
+ if (gamepad1.x) {
+ zeroOffsetTicks = rawTicks;
+ telemetryM.debug("ACTION", "Zero Point Reset!");
+ }
+
+ // 2. Start 180 Degree Sweep
+ if (gamepad1.y && ENABLE_AUTO_RATIO && !isCalibratingSweep) {
+ startSweepTicks = rawTicks;
+ isCalibratingSweep = true;
+ }
+
+ // 3. End Sweep and Calculate Ratio
+ if (gamepad1.b && isCalibratingSweep) {
+ double deltaTicks = Math.abs(rawTicks - startSweepTicks);
+ double measuredTicksPerDegree = deltaTicks / CALIBRATION_SWEEP_DEGREES;
+ EXTERNAL_GEAR_RATIO = (measuredTicksPerDegree * 360.0) / TICKS_PER_REV_MOTOR;
+
+ ticksPerDegree = measuredTicksPerDegree;
+ isCalibratingSweep = false;
+ telemetryM.debug("ACTION", "New Ratio Calculated!");
+ }
+
+ // 4. Switch back to PID
+ if (gamepad1.a) {
+ currentMode = TuningMode.PID_ACTIVE;
+ }
+
+ telemetryM.debug("STATUS", isCalibratingSweep ? "RECORDING SWEEP..." : "IDLE");
+ telemetryM.debug("INSTRUCTIONS", "LS: Move | X: Zero | Y: Start Sweep | B: Stop Sweep | A: Done");
+ }
+
+ private void handlePidMode(double currentTicks, double dt) {
+ // Emergency stop/switch to manual
+ if (gamepad1.back || (gamepad1.b && !isCalibratingSweep)) {
+ turretMotor.setPower(0);
+ currentMode = TuningMode.CALIBRATION;
+ return;
+ }
+
+ double wrapLimitTicks = WRAP_THRESHOLD_DEGREES * ticksPerDegree;
+ LLResult result = limelight.getLatestResult();
+
+ if (result != null && result.isValid()) {
+ double tx = result.getTx();
+ double offsetTicks = (tx + TARGET_OFFSET_DEGREES) * ticksPerDegree;
+ double potentialTarget = currentTicks + offsetTicks;
+
+ // Anti-Wrap and Soft Limit Logic
+ if (potentialTarget < -wrapLimitTicks) {
+ targetTicks = SOFT_LIMIT_POS;
+ } else if (potentialTarget > wrapLimitTicks) {
+ targetTicks = SOFT_LIMIT_NEG;
+ } else {
+ targetTicks = Math.max(SOFT_LIMIT_NEG * ticksPerDegree,
+ Math.min(SOFT_LIMIT_POS * ticksPerDegree, potentialTarget));
+ }
+ }
+
+ double error = targetTicks - currentTicks;
+ double derivative = (error - lastError) / dt;
+
+ if (Math.abs(error) < (15 * ticksPerDegree)) {
+ integralSum += (error * dt);
+ } else {
+ integralSum = 0;
+ }
+
+ double integralTerm = kI * integralSum;
+ if (Math.abs(integralTerm) > MAX_INTEGRAL) {
+ integralTerm = Math.signum(integralTerm) * MAX_INTEGRAL;
+ }
+
+ double output = (kP * error) + integralTerm + (kD * derivative);
+ output = Math.max(-MAX_POWER, Math.min(MAX_POWER, output));
+
+ turretMotor.setPower(output);
+ lastError = error;
+ }
+
+ private void applyMotorDirection() {
+ turretMotor.setDirection(MOTOR_REVERSED ?
+ DcMotorSimple.Direction.REVERSE : DcMotorSimple.Direction.FORWARD);
+ lastReverseState = MOTOR_REVERSED;
+ }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/allInOne.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/allInOne.java
new file mode 100755
index 0000000..93a2362
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/allInOne.java
@@ -0,0 +1,465 @@
+package org.firstinspires.ftc.teamcode.tuning;
+
+import android.graphics.Color;
+import com.bylazar.configurables.annotations.Configurable;
+import com.bylazar.telemetry.PanelsTelemetry;
+import com.bylazar.telemetry.TelemetryManager;
+import com.pedropathing.follower.Follower;
+import com.pedropathing.geometry.Pose;
+import com.qualcomm.hardware.limelightvision.LLResult;
+import com.qualcomm.hardware.limelightvision.Limelight3A;
+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.NormalizedColorSensor;
+import com.qualcomm.robotcore.hardware.NormalizedRGBA;
+import com.qualcomm.robotcore.hardware.PIDFCoefficients;
+import com.qualcomm.robotcore.hardware.Servo;
+import com.qualcomm.robotcore.hardware.SwitchableLight;
+import com.qualcomm.robotcore.util.ElapsedTime;
+import com.qualcomm.robotcore.util.Range;
+
+import org.firstinspires.ftc.teamcode.pedroPathing.Constants;
+import com.qualcomm.hardware.lynx.LynxModule;
+
+import java.util.List;
+
+
+@Configurable
+@TeleOp(name = "HOLY GODFATHER OF TESTING", group = "All")
+public class allInOne extends LinearOpMode {
+
+ // --- PID Constants ---
+ public static double F = 15.6;
+ public static double P = 22.5;
+ public static double D = 0.001;
+
+ // Turret PID
+ public static double kP_LL = 0.02;
+ public static double kP_ODO = 0.035;
+ public static double kP = 0.035; // Default to ODO roughly
+ public static double kI = 0.0;
+ public static double kD_LL = 0.001;
+ public static double kD_ODO = 0.0007;
+
+ public static double MAX_POWER = 0.5;
+ public static double MAX_INTEGRAL = 0.5;
+
+ public static double TICKS_PER_REV_MOTOR = 384.5;
+ public static double EXTERNAL_GEAR_RATIO = 1.315994798439532;
+
+ public static boolean MOTOR_REVERSED = false;
+
+ public static double LL_LOGIC_MULTIPLIER = -1.0;
+
+ public static double LL_TARGET_OFFSET_DEGREES = 4;
+ public static double RED_TARGET_OFFSET_DEGREES = 4;
+ public static double BLUE_TARGET_OFFSET_DEGREES = 4;
+
+ private double TARGET_OFFSET_DEGREES = BLUE_TARGET_OFFSET_DEGREES;
+
+ public static double SOFT_LIMIT_NEG = -250.0;
+ public static double SOFT_LIMIT_POS = 250.0;
+ public static double WRAP_THRESHOLD_DEGREES = 180.0;
+
+ public static double GOAL_RED_X = 138;
+ public static double GOAL_RED_Y = 138;
+ public static double GOAL_BLUE_X = 6;
+ public static double GOAL_BLUE_Y = 138;
+
+ private double targetTicks = 0;
+ private double ticksPerDegree = 0;
+
+ public static double SERVO_MIN = 0.48;
+ public static double SERVO_MAX = 1.0;
+
+ public static double OFFSET_GAIN = -0.0005;
+
+ private Limelight3A limelight;
+ private DcMotorEx turretRotation;
+ private DcMotorEx leftTurret, rightTurret;
+ private Servo hoodServo;
+ private DcMotor frontLeft, frontRight, backLeft, backRight;
+ private Follower follower;
+
+ public DcMotor frontIntakeMotor;
+ private NormalizedColorSensor indexA, indexB, indexC;
+ private NormalizedColorSensor[] sensors;
+
+ private TelemetryManager telemetryM;
+
+ private double integralSum = 0;
+ private double lastError = 0;
+ private double targetAngle = 0;
+
+ private boolean launch = false;
+ private int pipelineIndex = 1;
+
+ private final ElapsedTime loopTimer = new ElapsedTime();
+ private final ElapsedTime limelightFailureTimer = new ElapsedTime();
+
+ private double loopTimeSum = 0;
+ private int loopCount = 0;
+
+ private long lastIndexAllArtifactsTime = 0;
+ private int[] artifactColors = {0, 0, 0};
+
+ List allHubs;
+
+ private enum Alliance { RED, BLUE }
+ private Alliance currentAlliance = Alliance.BLUE;
+
+ // Tracking State
+ private enum TrackingSource { LIMELIGHT, ODOMETRY }
+ private TrackingSource activeSource = TrackingSource.ODOMETRY;
+ private TrackingSource lastActiveSource = TrackingSource.ODOMETRY;
+
+ public static double start_x = 72;
+ public static double start_y = 8.5;
+ public static double start_heading = 90;
+
+ @Override
+ public void runOpMode() {
+ telemetryM = PanelsTelemetry.INSTANCE.getTelemetry();
+ allHubs = hardwareMap.getAll(LynxModule.class);
+
+ for (LynxModule hub : allHubs) {
+ hub.setBulkCachingMode(LynxModule.BulkCachingMode.MANUAL);
+ }
+
+ // --- Hardware Map ---
+ turretRotation = hardwareMap.get(DcMotorEx.class, "turretRotation");
+
+ // STATIC DIRECTION CONFIGURATION
+ turretRotation.setDirection(MOTOR_REVERSED ? DcMotorSimple.Direction.REVERSE : DcMotorSimple.Direction.FORWARD);
+
+ turretRotation.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+ turretRotation.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+ turretRotation.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+
+ leftTurret = hardwareMap.get(DcMotorEx.class, "lturret");
+ rightTurret = hardwareMap.get(DcMotorEx.class, "rturret");
+ hoodServo = hardwareMap.get(Servo.class, "hoodServo");
+
+ frontLeft = hardwareMap.get(DcMotor.class, "fl");
+ frontRight = hardwareMap.get(DcMotor.class, "fr");
+ backLeft = hardwareMap.get(DcMotor.class, "bl");
+ backRight = hardwareMap.get(DcMotor.class, "br");
+
+ frontIntakeMotor = hardwareMap.dcMotor.get("intake");
+ frontIntakeMotor.setDirection(org.firstinspires.ftc.teamcode.teleOp.Constants.IntakeConstants.INTAKE_DIRECTION);
+ frontIntakeMotor.setZeroPowerBehavior(org.firstinspires.ftc.teamcode.teleOp.Constants.IntakeConstants.INTAKE_ZERO_POWER_BEHAVIOR);
+ frontIntakeMotor.setMode(org.firstinspires.ftc.teamcode.teleOp.Constants.IntakeConstants.INTAKE_RUNMODE);
+
+ // --- Sensor Setup ---
+ indexA = hardwareMap.get(NormalizedColorSensor.class, org.firstinspires.ftc.teamcode.teleOp.Constants.TransferConstants.INDEX_SENSOR_A);
+ indexB = hardwareMap.get(NormalizedColorSensor.class, org.firstinspires.ftc.teamcode.teleOp.Constants.TransferConstants.INDEX_SENSOR_B);
+ indexC = hardwareMap.get(NormalizedColorSensor.class, org.firstinspires.ftc.teamcode.teleOp.Constants.TransferConstants.INDEX_SENSOR_C);
+ sensors = new NormalizedColorSensor[]{indexA, indexB, indexC};
+
+ for (NormalizedColorSensor s : sensors) {
+ if (s instanceof SwitchableLight) ((SwitchableLight) s).enableLight(true);
+ }
+
+ // PedroPathing Follower Setup
+ follower = Constants.createFollower(hardwareMap);
+ follower.setStartingPose(new Pose(start_x, start_y, Math.toRadians(start_heading)));
+
+ limelight = hardwareMap.get(Limelight3A.class, "limelight");
+
+ leftTurret.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+ rightTurret.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+ rightTurret.setDirection(DcMotorSimple.Direction.REVERSE);
+
+ PIDFCoefficients pidfCoefficients = new PIDFCoefficients(P, 0, D, F);
+ rightTurret.setPIDFCoefficients(DcMotorEx.RunMode.RUN_USING_ENCODER, pidfCoefficients);
+ leftTurret.setPIDFCoefficients(DcMotorEx.RunMode.RUN_USING_ENCODER, pidfCoefficients);
+
+ frontLeft.setDirection(DcMotor.Direction.REVERSE);
+ backLeft.setDirection(DcMotor.Direction.REVERSE);
+
+ recalculateTicksPerDegree();
+
+ limelight.pipelineSwitch(pipelineIndex);
+ limelight.start();
+
+ telemetryM.debug("Status", "Initialized");
+ telemetryM.update(telemetry);
+
+ waitForStart();
+ loopTimer.reset();
+ limelightFailureTimer.reset();
+
+ follower.startTeleopDrive(true);
+ follower.update();
+
+ loopTimeSum = 0;
+ loopCount = 0;
+
+ while (opModeIsActive()) {
+ update();
+ }
+ limelight.stop();
+ }
+
+ private void update() {
+ for (LynxModule hub : allHubs) {
+ hub.clearBulkCache();
+ }
+
+ follower.update();
+
+ double dt = loopTimer.seconds();
+ loopTimer.reset();
+ loopTimeSum += dt;
+ loopCount++;
+ if (dt < 0.001) dt = 0.001;
+
+ // Drive Control
+ double y = -gamepad1.left_stick_y;
+ double x = gamepad1.left_stick_x * 1.1;
+ double rx = gamepad1.right_stick_x;
+ double denominator = Math.max(Math.abs(y) + Math.abs(x) + Math.abs(rx), 1);
+
+ frontLeft.setPower((y + x + rx) / denominator);
+ backLeft.setPower((y - x + rx) / denominator);
+ frontRight.setPower((y - x - rx) / denominator);
+ backRight.setPower((y + x - rx) / denominator);
+
+ // Pass movement to follower for pose updates
+ follower.setTeleOpDrive(y, x, rx, true);
+
+ // Alliance Toggle
+ if (gamepad1.psWasPressed()) {
+ currentAlliance = (currentAlliance == Alliance.RED) ? Alliance.BLUE : Alliance.RED;
+ }
+
+ if (gamepad1.startWasPressed()) {
+ pipelineIndex = (pipelineIndex + 1) % 3;
+ limelight.pipelineSwitch(pipelineIndex);
+ }
+
+ if (gamepad1.yWasPressed()) launch = !launch;
+
+ // Intake Logic
+ if (gamepad1.right_trigger > 0.01) {
+ frontIntakeMotor.setPower(org.firstinspires.ftc.teamcode.teleOp.Constants.IntakeConstants.INTAKE_POWER);
+ } else if (gamepad1.left_trigger > 0.01) {
+ frontIntakeMotor.setPower(-org.firstinspires.ftc.teamcode.teleOp.Constants.IntakeConstants.INTAKE_POWER);
+ } else {
+ if (!launch) frontIntakeMotor.setPower(0);
+ }
+
+ // Color Sensor Logic
+ long currentTime = System.currentTimeMillis();
+ if (currentTime - lastIndexAllArtifactsTime >= 350) {
+ detectArtifacts();
+ lastIndexAllArtifactsTime = currentTime;
+ }
+
+ // --- TURRET TRACKING LOGIC ---
+
+ // Update Pipeline based on Alliance
+ if (currentAlliance == Alliance.BLUE) limelight.pipelineSwitch(1);
+ else limelight.pipelineSwitch(2);
+
+ LLResult result = limelight.getLatestResult();
+
+ double rawTicks = turretRotation.getCurrentPosition();
+ double currentDegrees = rawTicks / ticksPerDegree;
+
+ double calculatedTargetTicks = Double.NaN;
+
+ // Hybrid Logic: Default to LL, fall back to ODO
+ calculatedTargetTicks = calculateLimelightTargetTicks(result, currentDegrees, rawTicks);
+
+ if (Double.isNaN(calculatedTargetTicks)) {
+ // Limelight failed/no target
+ if (limelightFailureTimer.seconds() > 0.5) {
+ // If failed for > 0.5s, use ODO
+ calculatedTargetTicks = calculateOdometryTargetTicks();
+ activeSource = TrackingSource.ODOMETRY;
+ } else {
+ // Hold last valid position (short flicker)
+ calculatedTargetTicks = targetTicks;
+ }
+ } else {
+ // Limelight valid
+ limelightFailureTimer.reset();
+ activeSource = TrackingSource.LIMELIGHT;
+ }
+
+ // Apply Target
+ if (!Double.isNaN(calculatedTargetTicks)) {
+ targetTicks = calculatedTargetTicks;
+ }
+
+ // PID Reset on Source Switch
+ if (activeSource != lastActiveSource) {
+ integralSum = 0;
+ lastError = 0;
+ }
+ lastActiveSource = activeSource;
+
+ // PID Calculation
+ double kP_use = (activeSource == TrackingSource.ODOMETRY) ? kP_ODO : kP_LL;
+ double kD_use = (activeSource == TrackingSource.ODOMETRY) ? kD_ODO : kD_LL;
+
+ double error = targetTicks - rawTicks;
+ double derivative = (error - lastError) / dt;
+
+ if (Math.abs(error) < (15 * ticksPerDegree)) {
+ integralSum += (error * dt);
+ } else {
+ integralSum = 0;
+ }
+
+ double integralTerm = kI * integralSum;
+ if (Math.abs(integralTerm) > MAX_INTEGRAL) {
+ integralTerm = Math.signum(integralTerm) * MAX_INTEGRAL;
+ }
+
+ double output = (kP_use * error) + integralTerm + (kD_use * derivative);
+ output = Range.clip(output, -MAX_POWER, MAX_POWER);
+ turretRotation.setPower(output);
+ lastError = error;
+
+ // --- SHOOTER CALCULATIONS ---
+ // Uses raw LL result if valid, regardless of whether it's controlling the turret
+ double calculatedDistance = 0;
+ double calculatedVelocity = 0;
+ double calculatedHoodPos = 0;
+
+ if (result != null && result.isValid()) {
+ double ta = result.getTa();
+ calculatedDistance = getDistanceFromTag(ta);
+ calculatedVelocity = getFlywheelVelocity(calculatedDistance);
+ calculatedHoodPos = getHoodPOS(calculatedDistance, calculatedVelocity);
+
+ if (launch) {
+ leftTurret.setVelocity(calculatedVelocity);
+ rightTurret.setVelocity(calculatedVelocity);
+ hoodServo.setPosition(calculatedHoodPos);
+ }
+ }
+
+ if (launch) {
+ if (leftTurret.getVelocity() * 0.9 <= calculatedVelocity) {
+ frontIntakeMotor.setPower(org.firstinspires.ftc.teamcode.teleOp.Constants.IntakeConstants.LAUNCH_POWER);
+ }
+ }
+
+ // Telemetry
+ telemetryM.debug("▰▰▰▰▰▰▰ SYSTEMS ▰▰▰▰▰▰▰");
+ telemetryM.debug("Alliance", currentAlliance);
+ telemetryM.debug("Source", activeSource);
+ telemetryM.debug("Detected", getArtifactString());
+ telemetryM.debug("Turret Angle", currentDegrees);
+ telemetryM.debug("Target Angle", targetTicks / ticksPerDegree);
+ telemetryM.debug("Hood Target", calculatedHoodPos);
+ telemetryM.debug("Distance", calculatedDistance);
+
+ if (loopCount > 0) {
+ telemetryM.debug("▰▰▰▰▰▰▰ FPS ▰▰▰▰▰▰▰");
+ telemetryM.debug("FPS", "%.2f", 1000 / (dt * 1000));
+ }
+
+ telemetryM.update(telemetry);
+ }
+
+ private double calculateLimelightTargetTicks(LLResult result, double currentDegrees, double currentTicks) {
+ if (result != null && result.isValid()) {
+ double tx = result.getTx();
+
+ // Logic Multiplier handles coordinate system differences without switching motor direction
+ double calculatedTargetAngle = currentDegrees + (tx * LL_LOGIC_MULTIPLIER);
+
+ TARGET_OFFSET_DEGREES = LL_TARGET_OFFSET_DEGREES;
+ double finalTargetDeg = calculatedTargetAngle + TARGET_OFFSET_DEGREES;
+ double potentialTargetTicks = finalTargetDeg * ticksPerDegree;
+
+ // Teleport Wrap Logic
+ double wrapLimitTicks = WRAP_THRESHOLD_DEGREES * ticksPerDegree;
+ if (potentialTargetTicks < -wrapLimitTicks) {
+ potentialTargetTicks = SOFT_LIMIT_POS;
+ } else if (potentialTargetTicks > wrapLimitTicks) {
+ potentialTargetTicks = SOFT_LIMIT_NEG;
+ }
+
+ return Range.clip(potentialTargetTicks, SOFT_LIMIT_NEG, SOFT_LIMIT_POS);
+ }
+ return Double.NaN;
+ }
+
+ private double calculateOdometryTargetTicks() {
+ Pose robotPose = follower.getPose();
+ double targetX = (currentAlliance == Alliance.RED) ? GOAL_RED_X : GOAL_BLUE_X;
+ double targetY = (currentAlliance == Alliance.RED) ? GOAL_RED_Y : GOAL_BLUE_Y;
+
+ double dx = targetX - robotPose.getX();
+ double dy = targetY - robotPose.getY();
+
+ double targetFieldHeading = Math.atan2(dy, dx);
+ double robotHeading = robotPose.getHeading();
+
+ TARGET_OFFSET_DEGREES = (currentAlliance == Alliance.RED) ? RED_TARGET_OFFSET_DEGREES : BLUE_TARGET_OFFSET_DEGREES;
+
+ double relativeRad = targetFieldHeading - robotHeading;
+
+ while (relativeRad > Math.PI) relativeRad -= (2 * Math.PI);
+ while (relativeRad < -Math.PI) relativeRad += (2 * Math.PI);
+
+ double calculatedTargetAngle = Math.toDegrees(relativeRad);
+ double finalTargetDeg = calculatedTargetAngle + TARGET_OFFSET_DEGREES;
+ double potentialTargetTicks = finalTargetDeg * ticksPerDegree;
+
+ return Range.clip(potentialTargetTicks, SOFT_LIMIT_NEG, SOFT_LIMIT_POS);
+ }
+
+ private void recalculateTicksPerDegree() {
+ ticksPerDegree = (TICKS_PER_REV_MOTOR * EXTERNAL_GEAR_RATIO) / 360.0;
+ }
+
+ private void detectArtifacts() {
+ for (int i = 0; i < 3; i++) {
+ NormalizedRGBA c1 = sensors[i].getNormalizedColors();
+ float[] hsv = new float[3];
+ Color.colorToHSV(c1.toColor(), hsv);
+ int color1 = detectColor(hsv[0], hsv[1], hsv[2]);
+ artifactColors[i] = color1;
+ }
+ }
+
+ private int detectColor(float hue, float sat, float val) {
+ if (hue > 170 && hue < 310) return 1; // Purple
+ if (hue > 80 && hue < 170) return 2; // Green
+ if (sat < 0.1 || val < 0.1) return 0;
+ return 0;
+ }
+
+ private String getArtifactString() {
+ StringBuilder sb = new StringBuilder();
+ for (int i : artifactColors) {
+ sb.append(i == 2 ? "G " : (i == 1 ? "P " : "- "));
+ }
+ return sb.toString();
+ }
+
+ private double getDistanceFromTag(double x) {
+ return (74.34095 * Math.pow(x, -0.518935));
+ }
+
+ private double calculateHoodOffset(double target) {
+ return (target - leftTurret.getVelocity()) * OFFSET_GAIN;
+ }
+
+ private double getFlywheelVelocity(double dist) {
+ return (0.0000119972 * Math.pow((dist), 4) - 0.00249603 * Math.pow(dist, 3) + 0.179513 * Math.pow(dist, 2) + (0.0519688 * dist) + 1489.8908);
+ }
+
+ private double getHoodPOS(double dist, double targetVelocity) {
+ return Range.clip(((-(5.15893 * Math.pow(10, -8)) * Math.pow(dist, 4)) + 0.00000954719 * Math.pow(dist, 3) - (0.000349083 * Math.pow(dist, 2)) + (-0.0153896 * dist) + 1.37618 + calculateHoodOffset(targetVelocity)), SERVO_MIN, SERVO_MAX);
+ }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/anchorTuning.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/anchorTuning.java
new file mode 100755
index 0000000..c2e59ba
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/anchorTuning.java
@@ -0,0 +1,86 @@
+package org.firstinspires.ftc.teamcode.tuning;
+
+import com.bylazar.configurables.annotations.Configurable;
+import com.bylazar.telemetry.PanelsTelemetry;
+import com.bylazar.telemetry.TelemetryManager;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import org.firstinspires.ftc.teamcode.Hware.hwMap;
+import org.firstinspires.ftc.teamcode.subsystems.DriveTrain;
+
+@Configurable
+@TeleOp(name = "Anchor Mode Tuner", group = "Tuning")
+public class anchorTuning extends LinearOpMode {
+
+ public static double driveP = -0.001;
+ public static double driveI = 0.0;
+ public static double driveD = -0.00015;
+
+ public static double strafeP = -0.002;
+ public static double strafeI = 0.0;
+ public static double strafeD = -0.00008;
+
+ public static double turnP = -0.08;
+ public static double turnI = 0.0;
+ public static double turnD = -0.0001; // Tuned for Degrees
+
+
+ public static double MAX_POWER = 0.2;
+
+
+ private DriveTrain driveTrain;
+ private TelemetryManager telemetryM;
+ private boolean lastPS = false;
+
+ @Override
+ public void runOpMode() throws InterruptedException {
+ // Initialize Telemetry
+ telemetryM = PanelsTelemetry.INSTANCE.getTelemetry();
+
+ // Initialize Hardware (This now includes the IMU from hwMap)
+ hwMap.DriveHwMap driveHw = new hwMap.DriveHwMap(hardwareMap);
+ driveTrain = new DriveTrain(driveHw);
+
+ telemetryM.debug("Status", "Initialized. Press PS/Guide to Toggle Anchor.");
+ telemetryM.update(telemetry);
+
+ waitForStart();
+
+ while (opModeIsActive()) {
+ // Update PID values from Dashboard to Subsystem in real-time
+ driveTrain.updateAnchorPID(
+ driveP, driveI, driveD,
+ strafeP, strafeI, strafeD,
+ turnP, turnI, turnD,
+ MAX_POWER
+ );
+
+ // Toggle Anchor with PS Button
+ if (gamepad1.ps && !lastPS) {
+ if (driveTrain.isAnchorActive()) {
+ driveTrain.stopAnchor();
+ gamepad1.rumble(200);
+ } else {
+ driveTrain.startAnchor();
+ gamepad1.rumble(500);
+ }
+ }
+ lastPS = gamepad1.ps;
+
+ driveTrain.update();
+
+ if (!driveTrain.isAnchorActive()) {
+ driveTrain.teleopDrive(gamepad1.left_stick_x, -gamepad1.left_stick_y, gamepad1.right_stick_x);
+ }
+
+ telemetryM.debug("Anchor Active", driveTrain.isAnchorActive());
+
+ telemetryM.debug("Error FWD (Ticks)", driveTrain.getLastForwardError());
+ telemetryM.debug("Error STR (Ticks)", driveTrain.getLastStrafeError());
+ telemetryM.debug("Error TRN (Degs)", driveTrain.getLastTurnError());
+
+ telemetryM.update(telemetry);
+ }
+ }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/encoderTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/encoderTest.java
new file mode 100755
index 0000000..82a60b8
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/encoderTest.java
@@ -0,0 +1,130 @@
+package org.firstinspires.ftc.teamcode.tuning;
+
+import com.bylazar.telemetry.TelemetryManager;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.hardware.DcMotor;
+
+
+import com.bylazar.telemetry.PanelsTelemetry;
+
+
+@TeleOp(name = "Encoders Test", group = "Test")
+public class encoderTest extends OpMode {
+
+ // Define Motor Objects
+ private DcMotor leftPodMotor;
+ private DcMotor rightPodMotor;
+ private DcMotor strafePodMotor;
+
+ // Previous values for calculating delta (speed)
+ private double prevLeft = 0;
+ private double prevRight = 0;
+ private double prevStrafe = 0;
+
+ private TelemetryManager telemetryM;
+
+ @Override
+ public void init() {
+
+ telemetryM = PanelsTelemetry.INSTANCE.getTelemetry();
+ try {
+ leftPodMotor = hardwareMap.get(DcMotor.class, "fl");
+ } catch (Exception e) {
+ telemetryM.debug("Error", "Could not find 'frontLeft' in config");
+ }
+
+ try {
+ rightPodMotor = hardwareMap.get(DcMotor.class, "fr");
+ } catch (Exception e) {
+ telemetryM.debug("Error", "Could not find 'backRight' in config");
+ }
+
+ try {
+ strafePodMotor = hardwareMap.get(DcMotor.class, "br");
+ } catch (Exception e) {
+ telemetryM.debug("Error", "Could not find 'backLeft' in config");
+ }
+
+ if (leftPodMotor != null) {
+ leftPodMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+ leftPodMotor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+ }
+ if (rightPodMotor != null) {
+ rightPodMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+ rightPodMotor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+ }
+ if (strafePodMotor != null) {
+ strafePodMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+ strafePodMotor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+ }
+
+ telemetryM.debug("Status", "Initialized");
+ telemetryM.debug("Note", "Manually move robot to check directions");
+ telemetryM.update(telemetry);
+ }
+
+ @Override
+ public void loop() {
+ // Read current positions (Safeguard against null if config is wrong)
+ double rawLeft = (leftPodMotor != null) ? leftPodMotor.getCurrentPosition() : 0;
+ double rawRight = (rightPodMotor != null) ? rightPodMotor.getCurrentPosition() : 0;
+ double rawStrafe = (strafePodMotor != null) ? strafePodMotor.getCurrentPosition() : 0;
+
+ // Apply the negation logic found in your DriveTrain file
+ // Your Anchor code used: -currentPosition
+ double anchorLeft = -rawLeft;
+ double anchorRight = -rawRight;
+ double anchorStrafe = -rawStrafe;
+
+ // Calculate Deltas (Movement since last loop)
+ double deltaLeft = anchorLeft - prevLeft;
+ double deltaRight = anchorRight - prevRight;
+ double deltaStrafe = anchorStrafe - prevStrafe;
+
+ // Update Previous
+ prevLeft = anchorLeft;
+ prevRight = anchorRight;
+ prevStrafe = anchorStrafe;
+
+ // ----------------------------------
+ // TELEMETRY DISPLAY
+ // ----------------------------------
+ telemetryM.debug("--- RAW ENCODER TICKS ---");
+ telemetryM.debug("Left Pod (FL Port)", "%.0f", rawLeft);
+ telemetryM.debug("Right Pod (BR Port)", "%.0f", rawRight);
+ telemetryM.debug("Strafe Pod (BL Port)", "%.0f", rawStrafe);
+
+ telemetryM.debug("\n--- ANCHOR LOGIC (Negated) ---");
+ telemetryM.debug("Left", "%.0f", anchorLeft);
+ telemetryM.debug("Right", "%.0f", anchorRight);
+ telemetryM.debug("Strafe", "%.0f", anchorStrafe);
+
+ telemetry.addLine("\n--- DIAGNOSTICS ---");
+
+ // Check Forward Movement
+ if (Math.abs(deltaLeft) > 5 && Math.abs(deltaRight) > 5) {
+ if (Math.signum(deltaLeft) == Math.signum(deltaRight)) {
+ telemetryM.debug("Motion", "FORWARD / BACKWARD");
+ if (Math.abs(anchorStrafe) > 1000) {
+ telemetryM.debug("WARNING: Strafe pod is increasing while moving forward.");
+ telemetryM.debug("Check if Strafe pod is crooked.");
+ }
+ } else {
+ telemetryM.debug("Motion", "TURNING");
+ }
+ }
+ // Check Strafe Movement
+ else if (Math.abs(deltaStrafe) > 5) {
+ telemetryM.debug("Motion", "STRAFING");
+ if (Math.abs(deltaLeft) < 5 && Math.abs(deltaRight) < 5) {
+ telemetryM.debug("GOOD: Left/Right pods stationary during strafe.");
+ }
+ } else {
+ telemetryM.debug("Motion", "STATIONARY");
+ }
+
+ telemetryM.update(telemetry);
+ }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/flywheelTesting.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/flywheelTesting.java
new file mode 100755
index 0000000..29886ce
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/flywheelTesting.java
@@ -0,0 +1,370 @@
+package org.firstinspires.ftc.teamcode.tuning;
+
+import static org.firstinspires.ftc.teamcode.tuning.GetDistanceTuning.CAMERA_HEIGHT_INCHES;
+import static org.firstinspires.ftc.teamcode.tuning.GetDistanceTuning.CAMERA_MOUNT_ANGLE_DEGREES;
+import static org.firstinspires.ftc.teamcode.tuning.GetDistanceTuning.GOAL_HEIGHT_INCHES;
+
+import com.bylazar.configurables.annotations.Configurable;
+import com.bylazar.telemetry.PanelsTelemetry;
+import com.bylazar.telemetry.TelemetryManager;
+import com.pedropathing.follower.Follower;
+import com.pedropathing.geometry.Pose;
+import com.qualcomm.hardware.limelightvision.LLResult;
+import com.qualcomm.hardware.limelightvision.Limelight3A;
+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.PIDFCoefficients;
+import com.qualcomm.robotcore.hardware.Servo;
+import com.qualcomm.robotcore.util.ElapsedTime;
+import com.qualcomm.robotcore.util.Range;
+
+import org.firstinspires.ftc.teamcode.pedroPathing.Constants;
+import com.qualcomm.hardware.lynx.LynxModule;
+
+import java.util.List;
+
+@Configurable
+@TeleOp(name = "Flywheel Testing", group = "All")
+public class flywheelTesting extends LinearOpMode {
+
+ // --- PID Constants ---
+ public static double F = 15.6;
+ public static double P = 22.5;
+ public static double D = 0.001;
+
+ // Turret PID
+ public static double kP_LL = 0.02;
+ public static double kP_ODO = 0.035;
+ public static double kI = 0.0;
+ public static double kD_LL = 0.001;
+ public static double kD_ODO = 0.0007;
+
+ public static double MAX_POWER = 0.5;
+ public static double MAX_INTEGRAL = 0.5;
+
+ public static double TICKS_PER_REV_MOTOR = 384.5;
+ public static double EXTERNAL_GEAR_RATIO = 1.315994798439532;
+
+ // FIX: Static Motor Direction & Logic Multiplier
+ public static boolean MOTOR_REVERSED = false;
+ public static double LL_LOGIC_MULTIPLIER = -1.0;
+
+ public static double LL_TARGET_OFFSET_DEGREES = 4;
+ public static double RED_TARGET_OFFSET_DEGREES = 14;
+ public static double BLUE_TARGET_OFFSET_DEGREES = 17;
+
+ private double TARGET_OFFSET_DEGREES = BLUE_TARGET_OFFSET_DEGREES;
+
+ public static double SOFT_LIMIT_NEG = -250.0;
+ public static double SOFT_LIMIT_POS = 250.0;
+ public static double WRAP_THRESHOLD_DEGREES = 180.0;
+
+ // --- Goal Coordinates for Odometry ---
+ public static double GOAL_RED_X = 138;
+ public static double GOAL_RED_Y = 138;
+ public static double GOAL_BLUE_X = 6;
+ public static double GOAL_BLUE_Y = 138;
+
+ private double targetTicks = 0;
+ private double ticksPerDegree = 0;
+
+ // --- Hardware Constants ---
+ public static double SERVO_MIN = 0.4;
+ public static double SERVO_MAX = 1.0;
+ public static double OFFSET_GAIN = -0.0005;
+
+ private Limelight3A limelight;
+ private DcMotorEx turretRotation;
+ private DcMotorEx leftTurret, rightTurret;
+ private Servo hoodServo;
+ private Follower follower;
+
+ private TelemetryManager telemetryM;
+
+ private double integralSum = 0;
+ private double lastError = 0;
+
+ private boolean launch = false;
+ private int pipelineIndex = 1;
+
+ private final ElapsedTime loopTimer = new ElapsedTime();
+ private final ElapsedTime limelightFailureTimer = new ElapsedTime();
+
+ private double loopTimeSum = 0;
+ private int loopCount = 0;
+
+ private enum Alliance { RED, BLUE }
+ private Alliance currentAlliance = Alliance.BLUE;
+
+ private enum TrackingSource { LIMELIGHT, ODOMETRY }
+ private TrackingSource activeSource = TrackingSource.ODOMETRY;
+ private TrackingSource lastActiveSource = TrackingSource.ODOMETRY;
+
+ List allHubs;
+
+ public static double start_x = 72;
+ public static double start_y = 8.5;
+ public static double start_heading = 90;
+
+ @Override
+ public void runOpMode() {
+ telemetryM = PanelsTelemetry.INSTANCE.getTelemetry();
+ allHubs = hardwareMap.getAll(LynxModule.class);
+
+ for (LynxModule hub : allHubs) {
+ hub.setBulkCachingMode(LynxModule.BulkCachingMode.MANUAL);
+ }
+
+ // --- Hardware Map ---
+ turretRotation = hardwareMap.get(DcMotorEx.class, "turretRotation");
+
+ // STATIC DIRECTION CONFIGURATION
+ turretRotation.setDirection(MOTOR_REVERSED ? DcMotorSimple.Direction.REVERSE : DcMotorSimple.Direction.FORWARD);
+
+ turretRotation.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+ turretRotation.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+ turretRotation.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+
+ leftTurret = hardwareMap.get(DcMotorEx.class, "lturret");
+ rightTurret = hardwareMap.get(DcMotorEx.class, "rturret");
+ hoodServo = hardwareMap.get(Servo.class, "hoodServo");
+
+ // PedroPathing Follower Setup
+ follower = Constants.createFollower(hardwareMap);
+ follower.setStartingPose(new Pose(start_x, start_y, Math.toRadians(start_heading)));
+
+ limelight = hardwareMap.get(Limelight3A.class, "limelight");
+
+ leftTurret.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+ rightTurret.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+ rightTurret.setDirection(DcMotorSimple.Direction.REVERSE);
+
+ PIDFCoefficients pidfCoefficients = new PIDFCoefficients(P, 0, D, F);
+ rightTurret.setPIDFCoefficients(DcMotorEx.RunMode.RUN_USING_ENCODER, pidfCoefficients);
+ leftTurret.setPIDFCoefficients(DcMotorEx.RunMode.RUN_USING_ENCODER, pidfCoefficients);
+
+ recalculateTicksPerDegree();
+
+ limelight.pipelineSwitch(pipelineIndex);
+ limelight.start();
+
+ telemetryM.debug("Status", "Initialized");
+ telemetryM.update(telemetry);
+
+ waitForStart();
+ loopTimer.reset();
+ limelightFailureTimer.reset();
+
+ follower.startTeleopDrive(true);
+ follower.update();
+
+ loopTimeSum = 0;
+ loopCount = 0;
+
+ while (opModeIsActive()) {
+ update();
+ }
+ limelight.stop();
+ }
+
+ private void update() {
+ for (LynxModule hub : allHubs) {
+ hub.clearBulkCache();
+ }
+
+ follower.update();
+
+ double dt = loopTimer.seconds();
+ loopTimer.reset();
+ loopTimeSum += dt;
+ loopCount++;
+ if (dt < 0.001) dt = 0.001;
+
+ // Pass movement to follower for pose updates (Robot Centric)
+ follower.setTeleOpDrive(
+ -gamepad1.left_stick_y,
+ -gamepad1.left_stick_x,
+ -gamepad1.right_stick_x,
+ true
+ );
+
+ if (gamepad1.startWasPressed()) {
+ pipelineIndex = (pipelineIndex + 1) % 3;
+ limelight.pipelineSwitch(pipelineIndex);
+ }
+
+ if (gamepad1.psWasPressed()) {
+ currentAlliance = (currentAlliance == Alliance.RED) ? Alliance.BLUE : Alliance.RED;
+ }
+
+ if (gamepad1.yWasPressed()) launch = !launch;
+
+ // --- TURRET TRACKING LOGIC ---
+ if (currentAlliance == Alliance.BLUE) limelight.pipelineSwitch(1);
+ else limelight.pipelineSwitch(2);
+
+ LLResult result = limelight.getLatestResult();
+
+ double rawTicks = turretRotation.getCurrentPosition();
+ double currentDegrees = rawTicks / ticksPerDegree;
+
+ double calculatedTargetTicks = Double.NaN;
+
+ // Hybrid Logic
+ calculatedTargetTicks = calculateLimelightTargetTicks(result, currentDegrees, rawTicks);
+
+ if (Double.isNaN(calculatedTargetTicks)) {
+ if (limelightFailureTimer.seconds() > 0.5) {
+ calculatedTargetTicks = calculateOdometryTargetTicks();
+ activeSource = TrackingSource.ODOMETRY;
+ } else {
+ calculatedTargetTicks = targetTicks;
+ }
+ } else {
+ limelightFailureTimer.reset();
+ activeSource = TrackingSource.LIMELIGHT;
+ }
+
+ if (!Double.isNaN(calculatedTargetTicks)) {
+ targetTicks = calculatedTargetTicks;
+ }
+
+ if (activeSource != lastActiveSource) {
+ integralSum = 0;
+ lastError = 0;
+ }
+ lastActiveSource = activeSource;
+
+ double kP_use = (activeSource == TrackingSource.ODOMETRY) ? kP_ODO : kP_LL;
+ double kD_use = (activeSource == TrackingSource.ODOMETRY) ? kD_ODO : kD_LL;
+
+ double error = targetTicks - rawTicks;
+ double derivative = (error - lastError) / dt;
+
+ if (Math.abs(error) < (15 * ticksPerDegree)) {
+ integralSum += (error * dt);
+ } else {
+ integralSum = 0;
+ }
+
+ double integralTerm = kI * integralSum;
+ if (Math.abs(integralTerm) > MAX_INTEGRAL) {
+ integralTerm = Math.signum(integralTerm) * MAX_INTEGRAL;
+ }
+
+ double output = (kP_use * error) + integralTerm + (kD_use * derivative);
+ output = Range.clip(output, -MAX_POWER, MAX_POWER);
+
+ turretRotation.setPower(output);
+ lastError = error;
+
+ // --- SHOOTER CALCULATIONS (Using TY) ---
+ double calculatedDistance = 0;
+ double calculatedVelocity = 0;
+ double calculatedHoodPos = 0;
+
+ if (result != null && result.isValid()) {
+ double ty = result.getTy();
+ calculatedDistance = getDistanceFromTag(ty);
+ calculatedVelocity = getFlywheelVelocity(calculatedDistance);
+ calculatedHoodPos = getHoodPOS(calculatedDistance, calculatedVelocity);
+
+ if (launch) {
+ leftTurret.setVelocity(calculatedVelocity);
+ rightTurret.setVelocity(calculatedVelocity);
+ hoodServo.setPosition(calculatedHoodPos);
+ }
+ }
+
+ if (launch) {
+ if (leftTurret.getVelocity() * 0.9 <= calculatedVelocity) {
+ // unlockTurret();
+ }
+ }
+
+ // Telemetry
+ telemetryM.debug("▰▰▰▰▰▰▰ SYSTEMS ▰▰▰▰▰▰▰");
+ telemetryM.debug("Source", activeSource);
+ telemetryM.debug("Turret Angle", currentDegrees);
+ telemetryM.debug("Hood Target", calculatedHoodPos);
+ telemetryM.debug("Distance", calculatedDistance);
+ telemetryM.debug("Velocity", calculatedVelocity);
+
+ if (loopCount > 0) {
+ telemetryM.debug("▰▰▰▰▰▰▰ FPS ▰▰▰▰▰▰▰");
+ telemetryM.debug("FPS", "%.2f", 1000 / (dt * 1000));
+ }
+
+ telemetryM.update(telemetry);
+ }
+
+ private double calculateLimelightTargetTicks(LLResult result, double currentDegrees, double currentTicks) {
+ if (result != null && result.isValid()) {
+ double tx = result.getTx();
+ double calculatedTargetAngle = currentDegrees + (tx * LL_LOGIC_MULTIPLIER);
+
+ TARGET_OFFSET_DEGREES = LL_TARGET_OFFSET_DEGREES;
+ double finalTargetDeg = calculatedTargetAngle + TARGET_OFFSET_DEGREES;
+ double potentialTargetTicks = finalTargetDeg * ticksPerDegree;
+
+ double wrapLimitTicks = WRAP_THRESHOLD_DEGREES * ticksPerDegree;
+ if (potentialTargetTicks < -wrapLimitTicks) {
+ potentialTargetTicks = SOFT_LIMIT_POS;
+ } else if (potentialTargetTicks > wrapLimitTicks) {
+ potentialTargetTicks = SOFT_LIMIT_NEG;
+ }
+
+ return Range.clip(potentialTargetTicks, SOFT_LIMIT_NEG, SOFT_LIMIT_POS);
+ }
+ return Double.NaN;
+ }
+
+ private double calculateOdometryTargetTicks() {
+ Pose robotPose = follower.getPose();
+ double targetX = (currentAlliance == Alliance.RED) ? GOAL_RED_X : GOAL_BLUE_X;
+ double targetY = (currentAlliance == Alliance.RED) ? GOAL_RED_Y : GOAL_BLUE_Y;
+
+ double dx = targetX - robotPose.getX();
+ double dy = targetY - robotPose.getY();
+
+ double targetFieldHeading = Math.atan2(dy, dx);
+ double robotHeading = robotPose.getHeading();
+
+ TARGET_OFFSET_DEGREES = (currentAlliance == Alliance.RED) ? RED_TARGET_OFFSET_DEGREES : BLUE_TARGET_OFFSET_DEGREES;
+
+ double relativeRad = targetFieldHeading - robotHeading;
+
+ while (relativeRad > Math.PI) relativeRad -= (2 * Math.PI);
+ while (relativeRad < -Math.PI) relativeRad += (2 * Math.PI);
+
+ double calculatedTargetAngle = Math.toDegrees(relativeRad);
+ double finalTargetDeg = calculatedTargetAngle + TARGET_OFFSET_DEGREES;
+ double potentialTargetTicks = finalTargetDeg * ticksPerDegree;
+
+ return Range.clip(potentialTargetTicks, SOFT_LIMIT_NEG, SOFT_LIMIT_POS);
+ }
+
+ private void recalculateTicksPerDegree() {
+ ticksPerDegree = (TICKS_PER_REV_MOTOR * EXTERNAL_GEAR_RATIO) / 360.0;
+ }
+
+ private double getDistanceFromTag(double x) {
+ // Uses Trigonometry (TY) instead of Area (TA)
+ return (GOAL_HEIGHT_INCHES - CAMERA_HEIGHT_INCHES) / Math.tan(Math.toRadians(CAMERA_MOUNT_ANGLE_DEGREES + x));
+ }
+
+ private double calculateHoodOffset(double target) {
+ return (target - leftTurret.getVelocity()) * OFFSET_GAIN;
+ }
+
+ private double getFlywheelVelocity(double dist) {
+ return (0.0000119972 * Math.pow((dist), 4) - 0.00249603 * Math.pow(dist, 3) + 0.179513 * Math.pow(dist, 2) + (0.0519688 * dist) + 1489.8908);
+ }
+
+ private double getHoodPOS(double dist, double targetVelocity) {
+ return Range.clip(((-(5.15893 * Math.pow(10, -8)) * Math.pow(dist, 4)) + 0.00000954719 * Math.pow(dist, 3) - (0.000349083 * Math.pow(dist, 2)) + (-0.0153896 * dist) + 1.37618 + calculateHoodOffset(targetVelocity)), SERVO_MIN, SERVO_MAX);
+ }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/odoTracking.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/odoTracking.java
new file mode 100755
index 0000000..8244839
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/odoTracking.java
@@ -0,0 +1,302 @@
+package org.firstinspires.ftc.teamcode.tuning;
+
+import com.bylazar.configurables.annotations.Configurable;
+import com.pedropathing.follower.Follower;
+import com.pedropathing.geometry.Pose;
+import com.qualcomm.hardware.limelightvision.LLResult;
+import com.qualcomm.hardware.limelightvision.Limelight3A;
+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.util.ElapsedTime;
+import com.qualcomm.robotcore.util.Range;
+
+import org.firstinspires.ftc.teamcode.pedroPathing.Constants;
+import org.firstinspires.ftc.teamcode.util.FPSCounter;
+
+import com.qualcomm.hardware.lynx.LynxModule;
+import java.util.List;
+
+@Configurable
+@TeleOp(name = "Odo Turret Tracking", group = "Turret")
+public class odoTracking extends LinearOpMode {
+
+ public static double kP = 0.02;
+ public static double kI = 0.0;
+ public static double kD = 0.001;
+ public static double MAX_POWER = 0.6;
+ public static double MAX_INTEGRAL = 0.5;
+
+ // ================= Hardware Constants =================
+ public static double TICKS_PER_REV_MOTOR = 384.5;
+ public static double EXTERNAL_GEAR_RATIO = 2.68888889;
+ public static boolean MOTOR_REVERSED = true;
+
+ // ================= Limits & Logic =================
+ public static double LL_LOGIC_MULTIPLIER = 1.0;
+ public static double SOFT_LIMIT_NEG = -230;
+ public static double SOFT_LIMIT_POS = 230;
+
+ // ================= Targeting Offsets =================
+ public static double RED_TARGET_OFFSET_DEGREES = 14;
+ public static double BLUE_TARGET_OFFSET_DEGREES = 17;
+ public static double LL_TARGET_OFFSET_DEGREES = -2;
+
+ // ================= Field Coordinates =================
+ public static double GOAL_RED_X = 138;
+ public static double GOAL_RED_Y = 138;
+ public static double GOAL_BLUE_X = 6;
+ public static double GOAL_BLUE_Y = 138;
+
+ // ================= Sensor Fusion Settings =================
+ public static double OFFSET_SMOOTHING = 0.2;
+
+ public static int targetFPS = 100;
+
+ // ================= Hardware Objects =================
+ private Limelight3A limelight;
+ private DcMotorEx turretMotor;
+ private Follower follower;
+
+ // ================= State Variables =================
+ private double integralSum = 0;
+ private double lastError = 0;
+ private double zeroOffsetTicks = 0;
+ private double ticksPerDegree = 0;
+
+ private double targetCorrectionOffsetTicks = 0;
+ private double fusedTargetTicks = 0;
+
+ private enum TuningMode { CALIBRATION, TRACKING }
+ private TuningMode currentMode = TuningMode.CALIBRATION;
+
+ private enum Alliance { RED, BLUE }
+ private Alliance currentAlliance = Alliance.BLUE;
+ private Alliance lastAlliance = null;
+
+ private final ElapsedTime timer = new ElapsedTime();
+ protected FPSCounter fpsCounter = new FPSCounter();
+
+ public static double start_x = 72;
+ public static double start_y = 8.5;
+ public static double start_heading = 90;
+
+
+ protected List allHubs;
+
+ @Override
+ public void runOpMode() {
+ turretMotor = hardwareMap.get(DcMotorEx.class, "turretRotation");
+ limelight = hardwareMap.get(Limelight3A.class, "limelight");
+
+ follower = Constants.createFollower(hardwareMap);
+ follower.setStartingPose(new Pose(start_x, start_y, Math.toRadians(start_heading)));
+
+ turretMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+ turretMotor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+ turretMotor.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+
+ allHubs = hardwareMap.getAll(LynxModule.class);
+ for (LynxModule hub : allHubs) {
+ hub.setBulkCachingMode(LynxModule.BulkCachingMode.MANUAL);
+ }
+
+ applyMotorDirection();
+ recalculateTicksPerDegree();
+
+ limelight.pipelineSwitch(1);
+ limelight.start();
+
+ telemetry.addData("Status", "Initialized");
+ telemetry.update();
+
+ waitForStart();
+ timer.reset();
+
+ follower.startTeleopDrive(true);
+ fpsCounter.reset();
+
+ while (opModeIsActive()) {
+ for (LynxModule hub : allHubs) {
+ hub.clearBulkCache();
+ }
+ fpsCounter.startLoop();
+
+ follower.update();
+
+ double dt = timer.seconds();
+ timer.reset();
+ if (dt < 0.001) dt = 0.001;
+
+ handleInput();
+ updateAlliancePipeline();
+
+ double rawTicks = turretMotor.getCurrentPosition();
+ double currentTicks = rawTicks - zeroOffsetTicks;
+ double currentDegrees = currentTicks / ticksPerDegree;
+
+ switch (currentMode) {
+ case CALIBRATION:
+ handleCalibrationMode(rawTicks);
+ targetCorrectionOffsetTicks = 0;
+ fusedTargetTicks = currentTicks;
+ break;
+
+ case TRACKING:
+ calculateHybridTarget(currentTicks, currentDegrees);
+ runPidControl(currentTicks, dt);
+
+ follower.setTeleOpDrive(
+ -gamepad1.left_stick_y,
+ -gamepad1.left_stick_x,
+ -gamepad1.right_stick_x,
+ true
+ );
+ break;
+ }
+
+ sleep(fpsCounter.getSyncTime(targetFPS));
+ updateTelemetry(currentTicks, currentDegrees);
+ }
+ limelight.stop();
+ }
+
+ private void calculateHybridTarget(double currentTicks, double currentDegrees) {
+ double odoTargetTicks = calculateOdometryTargetTicks();
+
+ LLResult result = limelight.getLatestResult();
+
+ if (result != null && result.isValid()) {
+ double rawErrorTicks = getErrorTicks(currentDegrees, result, odoTargetTicks);
+
+ targetCorrectionOffsetTicks = (targetCorrectionOffsetTicks * (1.0 - OFFSET_SMOOTHING))
+ + (rawErrorTicks * OFFSET_SMOOTHING);
+ }
+
+ double rawTarget = odoTargetTicks + targetCorrectionOffsetTicks;
+
+ fusedTargetTicks = Range.clip(rawTarget, SOFT_LIMIT_NEG, SOFT_LIMIT_POS);
+ }
+
+ private double getErrorTicks(double currentDegrees, LLResult result, double odoTargetTicks) {
+ double tx = result.getTx();
+
+ double visionTargetDegrees = currentDegrees + (tx * LL_LOGIC_MULTIPLIER);
+
+ double finalVisionDegrees = visionTargetDegrees + LL_TARGET_OFFSET_DEGREES;
+ double visionTargetTicks = finalVisionDegrees * ticksPerDegree;
+
+ double rawErrorTicks = visionTargetTicks - odoTargetTicks;
+
+ double ticksPerRev = 360.0 * ticksPerDegree;
+
+ while (rawErrorTicks > (ticksPerRev / 2)) rawErrorTicks -= ticksPerRev;
+ while (rawErrorTicks < -(ticksPerRev / 2)) rawErrorTicks += ticksPerRev;
+ return rawErrorTicks;
+ }
+
+ private double calculateOdometryTargetTicks() {
+ Pose robotPose = follower.getPose();
+ double targetX = (currentAlliance == Alliance.RED) ? GOAL_RED_X : GOAL_BLUE_X;
+ double targetY = (currentAlliance == Alliance.RED) ? GOAL_RED_Y : GOAL_BLUE_Y;
+
+ double dx = targetX - robotPose.getX();
+ double dy = targetY - robotPose.getY();
+
+ double targetFieldHeading = Math.atan2(dy, dx);
+ double robotHeading = robotPose.getHeading();
+
+ double relativeRad = targetFieldHeading - robotHeading;
+
+ while (relativeRad > Math.PI) relativeRad -= (2 * Math.PI);
+ while (relativeRad < -Math.PI) relativeRad += (2 * Math.PI);
+
+ double relativeDegrees = Math.toDegrees(relativeRad);
+
+ double staticOffset = (currentAlliance == Alliance.RED) ? RED_TARGET_OFFSET_DEGREES : BLUE_TARGET_OFFSET_DEGREES;
+
+ return (relativeDegrees + staticOffset) * ticksPerDegree;
+ }
+
+ private void runPidControl(double currentTicks, double dt) {
+ double error = fusedTargetTicks - currentTicks;
+ double derivative = (error - lastError) / dt;
+
+ if (Math.abs(error) < (15 * ticksPerDegree)) {
+ integralSum += (error * dt);
+ } else {
+ integralSum = 0;
+ }
+
+ double integralTerm = kI * integralSum;
+ if (Math.abs(integralTerm) > MAX_INTEGRAL) {
+ integralTerm = Math.signum(integralTerm) * MAX_INTEGRAL;
+ }
+
+ double output = (kP * error) + integralTerm + (kD * derivative);
+ output = Range.clip(output, -MAX_POWER, MAX_POWER);
+
+ turretMotor.setPower(output);
+ lastError = error;
+ }
+
+ private void handleInput() {
+ if (gamepad1.x) {
+ fusedTargetTicks = 0;
+ return;
+ }
+
+ if (gamepad1.psWasPressed()) {
+ currentAlliance = (currentAlliance == Alliance.RED) ? Alliance.BLUE : Alliance.RED;
+ }
+
+ if (gamepad1.aWasPressed() && currentMode == TuningMode.CALIBRATION) {
+ currentMode = TuningMode.TRACKING;
+ } else if (gamepad1.bWasPressed() && currentMode == TuningMode.TRACKING) {
+ turretMotor.setPower(0);
+ currentMode = TuningMode.CALIBRATION;
+ }
+ }
+
+ private void updateAlliancePipeline() {
+ if (lastAlliance != currentAlliance) {
+ if (currentAlliance == Alliance.RED) {
+ limelight.pipelineSwitch(2);
+ } else {
+ limelight.pipelineSwitch(1);
+ }
+ lastAlliance = currentAlliance;
+ }
+ }
+
+ private void handleCalibrationMode(double rawTicks) {
+ double manualPower = -gamepad1.left_stick_y * 0.3;
+ turretMotor.setPower(manualPower);
+
+ }
+
+ private void updateTelemetry(double currentTicks, double currentDegrees) {
+ telemetry.addData("Mode", currentMode);
+ telemetry.addData("Alliance", currentAlliance);
+ telemetry.addLine("------------------");
+ telemetry.addData("Target Ticks", fusedTargetTicks);
+ telemetry.addData("Current Ticks", currentTicks);
+ telemetry.addData("Error Ticks", fusedTargetTicks - currentTicks);
+ telemetry.addLine("------------------");
+ telemetry.addData("Applied Offset (Ticks)", targetCorrectionOffsetTicks);
+ telemetry.addData("Applied Offset (Deg)", targetCorrectionOffsetTicks / ticksPerDegree);
+ telemetry.addLine("------------------");
+ telemetry.addData("FPS", "%.2f", fpsCounter.getAverageFps());
+ telemetry.update();
+ }
+
+ private void applyMotorDirection() {
+ turretMotor.setDirection(MOTOR_REVERSED ? DcMotorSimple.Direction.REVERSE : DcMotorSimple.Direction.FORWARD);
+ }
+
+ private void recalculateTicksPerDegree() {
+ ticksPerDegree = (TICKS_PER_REV_MOTOR * EXTERNAL_GEAR_RATIO) / 360.0;
+ }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/AutoTransfer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/AutoTransfer.java
new file mode 100755
index 0000000..fc91998
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/AutoTransfer.java
@@ -0,0 +1,55 @@
+package org.firstinspires.ftc.teamcode.util;
+
+import com.pedropathing.geometry.Pose;
+
+public class AutoTransfer {
+
+ public static Pose transferPose = new Pose(0, 0, 0);
+
+ public static int alliance = 0;
+
+ public static int motifPattern = 0;
+
+ public static boolean isAutonRan = false;
+
+ public static void reset() {
+ transferPose = new Pose(0, 0, 0);
+ alliance = 0;
+ motifPattern = 0;
+ isAutonRan = false;
+ }
+
+ public static void initAuton(int allianceColor) {
+ reset();
+ alliance = allianceColor;
+ isAutonRan = true;
+ }
+
+ public static void setMotifPattern(int pattern) {
+ motifPattern = pattern;
+ }
+
+ public static void updatePose(Pose pose) {
+ transferPose = pose;
+ }
+
+ public static String getAllianceString() {
+ switch (alliance) {
+ case 1: return "BLUE";
+ case 2: return "RED";
+ default: return "UNKNOWN";
+ }
+ }
+ public static String getMotifString() {
+ switch (motifPattern) {
+ case 1: return "Pattern 1 (GPP)";
+ case 2: return "Pattern 2 (PGP)";
+ case 3: return "Pattern 3 (PPG)";
+ default: return "UNKNOWN";
+ }
+ }
+
+ public static boolean hasValidData() {
+ return isAutonRan && alliance != 0;
+ }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/FPSCounter.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/FPSCounter.java
new file mode 100755
index 0000000..d932853
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/FPSCounter.java
@@ -0,0 +1,60 @@
+package org.firstinspires.ftc.teamcode.util;
+
+import com.qualcomm.robotcore.util.ElapsedTime;
+
+public class FPSCounter {
+ private ElapsedTime loopTimer = new ElapsedTime();
+ private double loopTimeSum = 0;
+ private int loopCount = 0;
+ private double currentLoopTime = 0;
+ private double lastTime = 0;
+
+ public FPSCounter() {
+ reset();
+ }
+
+ public void startLoop() {
+ double now = loopTimer.milliseconds();
+ if (loopCount > 0) {
+ currentLoopTime = now - lastTime;
+ loopTimeSum += currentLoopTime;
+ }
+ lastTime = now;
+ loopCount++;
+ }
+
+ public long getSyncTime(int targetFps) {
+ double targetMs = 1000.0 / targetFps;
+
+ double elapsedWorkMs = loopTimer.milliseconds() - lastTime;
+ double remainingMs = targetMs - elapsedWorkMs;
+
+ return (remainingMs > 0) ? (long) remainingMs : 0;
+ }
+
+ public double getCurrentFps() {
+ return (currentLoopTime > 0) ? 1000.0 / currentLoopTime : 0;
+ }
+
+ public double getAverageFps() {
+ if (loopCount <= 1) return 0;
+ return 1000.0 / (loopTimeSum / (loopCount - 1));
+ }
+
+ public double getCurrentLoopTime() {
+ return currentLoopTime;
+ }
+
+ public double getAverageLoopTime() {
+ if (loopCount <= 1) return 0;
+ return loopTimeSum / (loopCount - 1);
+ }
+
+ public void reset() {
+ loopTimer.reset();
+ lastTime = 0;
+ loopTimeSum = 0;
+ loopCount = 0;
+ currentLoopTime = 0;
+ }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/OATDrawing.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/OATDrawing.java
new file mode 100755
index 0000000..0c42eb0
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/OATDrawing.java
@@ -0,0 +1,58 @@
+package org.firstinspires.ftc.teamcode.util;
+
+import com.bylazar.field.FieldManager;
+import com.bylazar.field.PanelsField;
+import com.bylazar.field.Style;
+import com.pedropathing.geometry.*;
+import com.pedropathing.math.*;
+import com.pedropathing.paths.*;
+
+public class OATDrawing {
+ public static final double ROBOT_RADIUS = 9;
+ private static final FieldManager panelsField = PanelsField.INSTANCE.getField();
+
+ private static final Style robotLook = new Style("", "#3F51B5", 0.75);
+
+ private static final Style goalLook = new Style("", "#F44336", 0.9);
+
+ private static final Style vectorLook = new Style("", "#4CAF50", 0.8);
+
+ private static final Style visionLook = new Style("", "#FFD700", 0.9);
+
+ public static void init() {
+ panelsField.setOffsets(PanelsField.INSTANCE.getPresets().getPEDRO_PATHING());
+ }
+ public static void drawOATState(Pose robotPose, Pose goalPose, boolean hasVisionLock) {
+ if (robotPose == null || goalPose == null) return;
+
+ drawRobot(robotPose, robotLook);
+
+ panelsField.setStyle(goalLook);
+ panelsField.moveCursor(goalPose.getX(), goalPose.getY());
+ panelsField.circle(4);
+
+ panelsField.setStyle(hasVisionLock ? visionLook : vectorLook);
+ panelsField.moveCursor(robotPose.getX(), robotPose.getY());
+ panelsField.line(goalPose.getX(), goalPose.getY());
+
+ panelsField.update();
+ }
+
+ private static void drawRobot(Pose pose, Style style) {
+ if (pose == null || Double.isNaN(pose.getX()) || Double.isNaN(pose.getY()) || Double.isNaN(pose.getHeading())) {
+ return;
+ }
+
+ panelsField.setStyle(style);
+ panelsField.moveCursor(pose.getX(), pose.getY());
+ panelsField.circle(ROBOT_RADIUS);
+
+ Vector v = pose.getHeadingAsUnitVector();
+ v.setMagnitude(v.getMagnitude() * ROBOT_RADIUS);
+ double x2 = pose.getX() + v.getXComponent();
+ double y2 = pose.getY() + v.getYComponent();
+
+ panelsField.moveCursor(pose.getX(), pose.getY());
+ panelsField.line(x2, y2);
+ }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/res/raw/readme.md b/TeamCode/src/main/res/raw/readme.md
new file mode 100644
index 0000000..355a3c4
--- /dev/null
+++ b/TeamCode/src/main/res/raw/readme.md
@@ -0,0 +1 @@
+Place your sound files in this folder to use them as project resources.
\ No newline at end of file
diff --git a/TeamCode/src/main/res/values/strings.xml b/TeamCode/src/main/res/values/strings.xml
new file mode 100644
index 0000000..d781ec5
--- /dev/null
+++ b/TeamCode/src/main/res/values/strings.xml
@@ -0,0 +1,4 @@
+
+
+
+
diff --git a/TeamCode/src/main/res/xml/teamwebcamcalibrations.xml b/TeamCode/src/main/res/xml/teamwebcamcalibrations.xml
new file mode 100644
index 0000000..22ae7a8
--- /dev/null
+++ b/TeamCode/src/main/res/xml/teamwebcamcalibrations.xml
@@ -0,0 +1,161 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/build.common.gradle b/build.common.gradle
new file mode 100644
index 0000000..160f84f
--- /dev/null
+++ b/build.common.gradle
@@ -0,0 +1,117 @@
+/**
+ * build.common.gradle
+ *
+ * Try to avoid editing this file, as it may be updated from time to time as the FTC SDK
+ * evolves. Rather, if it is necessary to customize the build process, do those edits in
+ * the build.gradle file in TeamCode.
+ *
+ * This file contains the necessary content of the 'build.gradle' files for robot controller
+ * applications built using the FTC SDK. Each individual 'build.gradle' in those applications
+ * can simply contain the one line:
+ *
+ * apply from: '../build.common.gradle'
+ *
+ * which will pick up this file here. This approach allows makes it easier to integrate
+ * updates to the FTC SDK into your code.
+ */
+
+import java.util.regex.Pattern
+
+apply plugin: 'com.android.application'
+
+android {
+
+ compileSdk 34
+
+ signingConfigs {
+ release {
+ def apkStoreFile = System.getenv("APK_SIGNING_STORE_FILE")
+ if (apkStoreFile != null) {
+ keyAlias System.getenv("APK_SIGNING_KEY_ALIAS")
+ keyPassword System.getenv("APK_SIGNING_KEY_PASSWORD")
+ storeFile file(System.getenv("APK_SIGNING_STORE_FILE"))
+ storePassword System.getenv("APK_SIGNING_STORE_PASSWORD")
+ } else {
+ keyAlias 'androiddebugkey'
+ keyPassword 'android'
+ storeFile rootProject.file('libs/ftc.debug.keystore')
+ storePassword 'android'
+ }
+ }
+
+ debug {
+ keyAlias 'androiddebugkey'
+ keyPassword 'android'
+ storeFile rootProject.file('libs/ftc.debug.keystore')
+ storePassword 'android'
+ }
+ }
+
+ defaultConfig {
+ signingConfig signingConfigs.debug
+ applicationId 'com.qualcomm.ftcrobotcontroller'
+ minSdkVersion 24
+ //noinspection ExpiredTargetSdkVersion
+ targetSdkVersion 28
+
+ /**
+ * We keep the versionCode and versionName of robot controller applications in sync with
+ * the master information published in the AndroidManifest.xml file of the FtcRobotController
+ * module. This helps avoid confusion that might arise from having multiple versions of
+ * a robot controller app simultaneously installed on a robot controller device.
+ *
+ * We accomplish this with the help of a funky little Groovy script that maintains that
+ * correspondence automatically.
+ *
+ * @see Configure Your Build
+ * @see Versioning Your App
+ */
+ def manifestFile = project(':FtcRobotController').file('src/main/AndroidManifest.xml');
+ def manifestText = manifestFile.getText()
+ //
+ def vCodePattern = Pattern.compile("versionCode=\"(\\d+(\\.\\d+)*)\"")
+ def matcher = vCodePattern.matcher(manifestText)
+ matcher.find()
+ def vCode = Integer.parseInt(matcher.group(1))
+ //
+ def vNamePattern = Pattern.compile("versionName=\"(.*)\"")
+ matcher = vNamePattern.matcher(manifestText);
+ matcher.find()
+ def vName = matcher.group(1)
+ //
+ versionCode vCode
+ versionName vName
+ }
+
+ // http://google.github.io/android-gradle-dsl/current/com.android.build.gradle.internal.dsl.BuildType.html
+ buildTypes {
+ release {
+ signingConfig signingConfigs.release
+
+ ndk {
+ abiFilters "armeabi-v7a", "arm64-v8a"
+ }
+ }
+ debug {
+ debuggable true
+ jniDebuggable true
+ ndk {
+ abiFilters "armeabi-v7a", "arm64-v8a"
+ }
+ }
+ }
+
+ compileOptions {
+ sourceCompatibility JavaVersion.VERSION_1_8
+ targetCompatibility JavaVersion.VERSION_1_8
+ }
+
+ packagingOptions {
+ pickFirst '**/*.so'
+ }
+ ndkVersion '21.3.6528147'
+}
+
+repositories {
+}
+
diff --git a/build.dependencies.gradle b/build.dependencies.gradle
new file mode 100644
index 0000000..f14d25b
--- /dev/null
+++ b/build.dependencies.gradle
@@ -0,0 +1,22 @@
+repositories {
+ mavenCentral()
+ google() // Needed for androidx
+ maven { url = "https://mymaven.bylazar.com/releases" }
+}
+
+dependencies {
+ implementation 'org.firstinspires.ftc:Inspection:11.1.0'
+ implementation 'org.firstinspires.ftc:Blocks:11.1.0'
+ implementation 'org.firstinspires.ftc:RobotCore:11.1.0'
+ implementation 'org.firstinspires.ftc:RobotServer:11.1.0'
+ implementation 'org.firstinspires.ftc:OnBotJava:11.1.0'
+ implementation 'org.firstinspires.ftc:Hardware:11.1.0'
+ implementation 'org.firstinspires.ftc:FtcCommon:11.1.0'
+ implementation 'org.firstinspires.ftc:Vision:11.1.0'
+ implementation 'androidx.appcompat:appcompat:1.2.0'
+
+ implementation 'com.pedropathing:ftc:2.0.6'
+ implementation 'com.pedropathing:telemetry:1.0.0'
+ //implementation 'com.bylazar:fullpanels:1.0.9'
+}
+
diff --git a/build.dependencies.gradle.old b/build.dependencies.gradle.old
new file mode 100644
index 0000000..216984d
--- /dev/null
+++ b/build.dependencies.gradle.old
@@ -0,0 +1,22 @@
+repositories {
+ mavenCentral()
+ google() // Needed for androidx
+ maven { url = "https://mymaven.bylazar.com/releases" }
+}
+
+dependencies {
+ implementation 'org.firstinspires.ftc:Inspection:11.1.0'
+ implementation 'org.firstinspires.ftc:Blocks:11.1.0'
+ implementation 'org.firstinspires.ftc:RobotCore:11.1.0'
+ implementation 'org.firstinspires.ftc:RobotServer:11.1.0'
+ implementation 'org.firstinspires.ftc:OnBotJava:11.1.0'
+ implementation 'org.firstinspires.ftc:Hardware:11.1.0'
+ implementation 'org.firstinspires.ftc:FtcCommon:11.1.0'
+ implementation 'org.firstinspires.ftc:Vision:11.1.0'
+ implementation 'androidx.appcompat:appcompat:1.2.0'
+
+ implementation 'com.pedropathing:ftc:2.0.6'
+ implementation 'com.pedropathing:telemetry:1.0.0'
+ implementation 'com.bylazar:fullpanels:1.0.9'
+}
+
diff --git a/build.gradle b/build.gradle
new file mode 100644
index 0000000..e70f209
--- /dev/null
+++ b/build.gradle
@@ -0,0 +1,29 @@
+/**
+ * Top-level build file for ftc_app project.
+ *
+ * It is extraordinarily rare that you will ever need to edit this file.
+ */
+
+buildscript {
+ repositories {
+ mavenCentral()
+ google()
+ }
+ dependencies {
+ // Note for FTC Teams: Do not modify this yourself.
+ classpath 'com.android.tools.build:gradle:8.7.0'
+ }
+}
+
+// This is now required because aapt2 has to be downloaded from the
+// google() repository beginning with version 3.2 of the Android Gradle Plugin
+allprojects {
+ repositories {
+ mavenCentral()
+ google()
+ }
+}
+
+repositories {
+ mavenCentral()
+}
diff --git a/doc/legal/AudioBlocksSounds.txt b/doc/legal/AudioBlocksSounds.txt
new file mode 100644
index 0000000..4eab3bc
--- /dev/null
+++ b/doc/legal/AudioBlocksSounds.txt
@@ -0,0 +1,21 @@
+The sound files listed below in this SDK were purchased from www.audioblocks.com under the
+following license.
+
+ http://support.audioblocks.com/customer/en/portal/topics/610636-licensing-faq-s/articles
+
+ How am I allowed to use your content?
+ Last Updated: Aug 11, 2016 01:51PM EDT
+ Our content may be used for nearly any project, commercial or otherwise, including feature
+ films, broadcast, commercial, industrial, educational video, print projects, multimedia,
+ games, and the internet, so long as substantial value is added to the content. (For example,
+ incorporating an audio clip into a commercial qualifies, while reposting our audio clip on
+ YouTube with no modification or no visual component does not.) Once you download a file it is
+ yours to keep and use forever, royalty- free, even if you change your subscription or cancel
+ your account.
+
+List of applicable sound files
+
+ chimeconnect.wav
+ chimedisconnect.wav
+ errormessage.wav
+ warningmessage.wav
\ No newline at end of file
diff --git a/doc/legal/Exhibit A - LEGO Open Source License Agreement.txt b/doc/legal/Exhibit A - LEGO Open Source License Agreement.txt
new file mode 100644
index 0000000..10c13b9
--- /dev/null
+++ b/doc/legal/Exhibit A - LEGO Open Source License Agreement.txt
@@ -0,0 +1,15 @@
+EXHIBIT A - LEGO® Open Source License Agreement
+
+The contents of the file 'nxtstartupsound.wav' contained in this SDK are subject to the
+LEGO® Open Source License Agreement Version 1.0 (the "License"); you may not use this
+file except in compliance with the License. You may obtain a copy of the License
+at "LEGO Open Source License.pdf" contained in the same directory as this exhibit.
+
+Software distributed under the License is distributed on an "AS IS" basis, WITHOUT
+WARRANTY OF ANY KIND, either express or implied. See the License for the specific
+language governing rights and limitations under the License.
+
+The Original Code is \AT91SAM7S256\Resource\SOUNDS\!Startup.rso.
+LEGO is the owner of the Original Code. Portions created by Robert Atkinson are
+Copyright (C) 2015. All Rights Reserved.
+Contributor(s): Robert Atkinson.
\ No newline at end of file
diff --git a/doc/legal/LEGO Open Source License.pdf b/doc/legal/LEGO Open Source License.pdf
new file mode 100644
index 0000000..9188498
Binary files /dev/null and b/doc/legal/LEGO Open Source License.pdf differ
diff --git a/doc/media/PullRequest.PNG b/doc/media/PullRequest.PNG
new file mode 100644
index 0000000..8cba3a9
Binary files /dev/null and b/doc/media/PullRequest.PNG differ
diff --git a/gradle.properties b/gradle.properties
new file mode 100644
index 0000000..9dce7ee
--- /dev/null
+++ b/gradle.properties
@@ -0,0 +1,13 @@
+# AndroidX package structure to make it clearer which packages are bundled with the
+# Android operating system, and which are packaged with your app's APK
+# https://developer.android.com/topic/libraries/support-library/androidx-rn
+android.useAndroidX=true
+
+# We no longer need to auto-convert third-party libraries to use AndroidX, which slowed down the build
+android.enableJetifier=false
+
+# Allow Gradle to use up to 1 GB of RAM
+org.gradle.jvmargs=-Xmx1024M
+
+android.nonTransitiveRClass=false
+org.gradle.configuration-cache=true
\ No newline at end of file
diff --git a/gradle/wrapper/gradle-wrapper.jar b/gradle/wrapper/gradle-wrapper.jar
new file mode 100644
index 0000000..f3d88b1
Binary files /dev/null and b/gradle/wrapper/gradle-wrapper.jar differ
diff --git a/gradle/wrapper/gradle-wrapper.properties b/gradle/wrapper/gradle-wrapper.properties
new file mode 100644
index 0000000..19cfad9
--- /dev/null
+++ b/gradle/wrapper/gradle-wrapper.properties
@@ -0,0 +1,5 @@
+distributionBase=GRADLE_USER_HOME
+distributionPath=wrapper/dists
+distributionUrl=https\://services.gradle.org/distributions/gradle-8.9-bin.zip
+zipStoreBase=GRADLE_USER_HOME
+zipStorePath=wrapper/dists
diff --git a/gradlew b/gradlew
new file mode 100755
index 0000000..91a7e26
--- /dev/null
+++ b/gradlew
@@ -0,0 +1,164 @@
+#!/usr/bin/env bash
+
+##############################################################################
+##
+## Gradle start up script for UN*X
+##
+##############################################################################
+
+# Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script.
+DEFAULT_JVM_OPTS=""
+
+APP_NAME="Gradle"
+APP_BASE_NAME=`basename "$0"`
+
+# Use the maximum available, or set MAX_FD != -1 to use that value.
+MAX_FD="maximum"
+
+warn ( ) {
+ echo "$*"
+}
+
+die ( ) {
+ echo
+ echo "$*"
+ echo
+ exit 1
+}
+
+# OS specific support (must be 'true' or 'false').
+cygwin=false
+msys=false
+darwin=false
+case "`uname`" in
+ CYGWIN* )
+ cygwin=true
+ ;;
+ Darwin* )
+ darwin=true
+ ;;
+ MINGW* )
+ msys=true
+ ;;
+esac
+
+# For Cygwin, ensure paths are in UNIX format before anything is touched.
+if $cygwin ; then
+ [ -n "$JAVA_HOME" ] && JAVA_HOME=`cygpath --unix "$JAVA_HOME"`
+fi
+
+# Attempt to set APP_HOME
+# Resolve links: $0 may be a link
+PRG="$0"
+# Need this for relative symlinks.
+while [ -h "$PRG" ] ; do
+ ls=`ls -ld "$PRG"`
+ link=`expr "$ls" : '.*-> \(.*\)$'`
+ if expr "$link" : '/.*' > /dev/null; then
+ PRG="$link"
+ else
+ PRG=`dirname "$PRG"`"/$link"
+ fi
+done
+SAVED="`pwd`"
+cd "`dirname \"$PRG\"`/" >&-
+APP_HOME="`pwd -P`"
+cd "$SAVED" >&-
+
+CLASSPATH=$APP_HOME/gradle/wrapper/gradle-wrapper.jar
+
+# Determine the Java command to use to start the JVM.
+if [ -n "$JAVA_HOME" ] ; then
+ if [ -x "$JAVA_HOME/jre/sh/java" ] ; then
+ # IBM's JDK on AIX uses strange locations for the executables
+ JAVACMD="$JAVA_HOME/jre/sh/java"
+ else
+ JAVACMD="$JAVA_HOME/bin/java"
+ fi
+ if [ ! -x "$JAVACMD" ] ; then
+ die "ERROR: JAVA_HOME is set to an invalid directory: $JAVA_HOME
+
+Please set the JAVA_HOME variable in your environment to match the
+location of your Java installation."
+ fi
+else
+ JAVACMD="java"
+ which java >/dev/null 2>&1 || die "ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH.
+
+Please set the JAVA_HOME variable in your environment to match the
+location of your Java installation."
+fi
+
+# Increase the maximum file descriptors if we can.
+if [ "$cygwin" = "false" -a "$darwin" = "false" ] ; then
+ MAX_FD_LIMIT=`ulimit -H -n`
+ if [ $? -eq 0 ] ; then
+ if [ "$MAX_FD" = "maximum" -o "$MAX_FD" = "max" ] ; then
+ MAX_FD="$MAX_FD_LIMIT"
+ fi
+ ulimit -n $MAX_FD
+ if [ $? -ne 0 ] ; then
+ warn "Could not set maximum file descriptor limit: $MAX_FD"
+ fi
+ else
+ warn "Could not query maximum file descriptor limit: $MAX_FD_LIMIT"
+ fi
+fi
+
+# For Darwin, add options to specify how the application appears in the dock
+if $darwin; then
+ GRADLE_OPTS="$GRADLE_OPTS \"-Xdock:name=$APP_NAME\" \"-Xdock:icon=$APP_HOME/media/gradle.icns\""
+fi
+
+# For Cygwin, switch paths to Windows format before running java
+if $cygwin ; then
+ APP_HOME=`cygpath --path --mixed "$APP_HOME"`
+ CLASSPATH=`cygpath --path --mixed "$CLASSPATH"`
+
+ # We build the pattern for arguments to be converted via cygpath
+ ROOTDIRSRAW=`find -L / -maxdepth 1 -mindepth 1 -type d 2>/dev/null`
+ SEP=""
+ for dir in $ROOTDIRSRAW ; do
+ ROOTDIRS="$ROOTDIRS$SEP$dir"
+ SEP="|"
+ done
+ OURCYGPATTERN="(^($ROOTDIRS))"
+ # Add a user-defined pattern to the cygpath arguments
+ if [ "$GRADLE_CYGPATTERN" != "" ] ; then
+ OURCYGPATTERN="$OURCYGPATTERN|($GRADLE_CYGPATTERN)"
+ fi
+ # Now convert the arguments - kludge to limit ourselves to /bin/sh
+ i=0
+ for arg in "$@" ; do
+ CHECK=`echo "$arg"|egrep -c "$OURCYGPATTERN" -`
+ CHECK2=`echo "$arg"|egrep -c "^-"` ### Determine if an option
+
+ if [ $CHECK -ne 0 ] && [ $CHECK2 -eq 0 ] ; then ### Added a condition
+ eval `echo args$i`=`cygpath --path --ignore --mixed "$arg"`
+ else
+ eval `echo args$i`="\"$arg\""
+ fi
+ i=$((i+1))
+ done
+ case $i in
+ (0) set -- ;;
+ (1) set -- "$args0" ;;
+ (2) set -- "$args0" "$args1" ;;
+ (3) set -- "$args0" "$args1" "$args2" ;;
+ (4) set -- "$args0" "$args1" "$args2" "$args3" ;;
+ (5) set -- "$args0" "$args1" "$args2" "$args3" "$args4" ;;
+ (6) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" ;;
+ (7) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" ;;
+ (8) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" "$args7" ;;
+ (9) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" "$args7" "$args8" ;;
+ esac
+fi
+
+# Split up the JVM_OPTS And GRADLE_OPTS values into an array, following the shell quoting and substitution rules
+function splitJvmOpts() {
+ JVM_OPTS=("$@")
+}
+eval splitJvmOpts $DEFAULT_JVM_OPTS $JAVA_OPTS $GRADLE_OPTS
+JVM_OPTS[${#JVM_OPTS[*]}]="-Dorg.gradle.appname=$APP_BASE_NAME"
+
+exec "$JAVACMD" "${JVM_OPTS[@]}" -classpath "$CLASSPATH" org.gradle.wrapper.GradleWrapperMain "$@"
diff --git a/gradlew.bat b/gradlew.bat
new file mode 100644
index 0000000..8a0b282
--- /dev/null
+++ b/gradlew.bat
@@ -0,0 +1,90 @@
+@if "%DEBUG%" == "" @echo off
+@rem ##########################################################################
+@rem
+@rem Gradle startup script for Windows
+@rem
+@rem ##########################################################################
+
+@rem Set local scope for the variables with windows NT shell
+if "%OS%"=="Windows_NT" setlocal
+
+@rem Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script.
+set DEFAULT_JVM_OPTS=
+
+set DIRNAME=%~dp0
+if "%DIRNAME%" == "" set DIRNAME=.
+set APP_BASE_NAME=%~n0
+set APP_HOME=%DIRNAME%
+
+@rem Find java.exe
+if defined JAVA_HOME goto findJavaFromJavaHome
+
+set JAVA_EXE=java.exe
+%JAVA_EXE% -version >NUL 2>&1
+if "%ERRORLEVEL%" == "0" goto init
+
+echo.
+echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH.
+echo.
+echo Please set the JAVA_HOME variable in your environment to match the
+echo location of your Java installation.
+
+goto fail
+
+:findJavaFromJavaHome
+set JAVA_HOME=%JAVA_HOME:"=%
+set JAVA_EXE=%JAVA_HOME%/bin/java.exe
+
+if exist "%JAVA_EXE%" goto init
+
+echo.
+echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME%
+echo.
+echo Please set the JAVA_HOME variable in your environment to match the
+echo location of your Java installation.
+
+goto fail
+
+:init
+@rem Get command-line arguments, handling Windowz variants
+
+if not "%OS%" == "Windows_NT" goto win9xME_args
+if "%@eval[2+2]" == "4" goto 4NT_args
+
+:win9xME_args
+@rem Slurp the command line arguments.
+set CMD_LINE_ARGS=
+set _SKIP=2
+
+:win9xME_args_slurp
+if "x%~1" == "x" goto execute
+
+set CMD_LINE_ARGS=%*
+goto execute
+
+:4NT_args
+@rem Get arguments from the 4NT Shell from JP Software
+set CMD_LINE_ARGS=%$
+
+:execute
+@rem Setup the command line
+
+set CLASSPATH=%APP_HOME%\gradle\wrapper\gradle-wrapper.jar
+
+@rem Execute Gradle
+"%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.appname=%APP_BASE_NAME%" -classpath "%CLASSPATH%" org.gradle.wrapper.GradleWrapperMain %CMD_LINE_ARGS%
+
+:end
+@rem End local scope for the variables with windows NT shell
+if "%ERRORLEVEL%"=="0" goto mainEnd
+
+:fail
+rem Set variable GRADLE_EXIT_CONSOLE if you need the _script_ return code instead of
+rem the _cmd.exe /c_ return code!
+if not "" == "%GRADLE_EXIT_CONSOLE%" exit 1
+exit /b 1
+
+:mainEnd
+if "%OS%"=="Windows_NT" endlocal
+
+:omega
diff --git a/libs/README.txt b/libs/README.txt
new file mode 100644
index 0000000..3d34852
--- /dev/null
+++ b/libs/README.txt
@@ -0,0 +1 @@
+Location of external libs
diff --git a/libs/ftc.debug.keystore b/libs/ftc.debug.keystore
new file mode 100644
index 0000000..a7204e6
Binary files /dev/null and b/libs/ftc.debug.keystore differ
diff --git a/settings.gradle b/settings.gradle
new file mode 100644
index 0000000..9e2cfb3
--- /dev/null
+++ b/settings.gradle
@@ -0,0 +1,2 @@
+include ':FtcRobotController'
+include ':TeamCode'