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..16f91e8
--- /dev/null
+++ b/TeamCode/build.gradle
@@ -0,0 +1,33 @@
+//
+// 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')
+ implementation 'org.ftclib.ftclib:core:2.1.1' // core
+ implementation 'org.ftclib.ftclib:vision:2.1.0' // vision
+ implementation 'org.apache.commons:commons-math3:3.6.1'
+ implementation 'com.fasterxml.jackson.core:jackson-databind:2.12.7'
+ implementation 'com.acmerobotics.roadrunner:core:0.5.6'
+}
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/AlignmentTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AlignmentTest.java
new file mode 100644
index 0000000..3bdaf28
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AlignmentTest.java
@@ -0,0 +1,154 @@
+package org.firstinspires.ftc.teamcode;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.roadrunner.control.PIDFController;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.geometry.Vector2d;
+import com.arcrobotics.ftclib.drivebase.MecanumDrive;
+import com.arcrobotics.ftclib.gamepad.GamepadEx;
+import com.arcrobotics.ftclib.gamepad.GamepadKeys;
+import com.arcrobotics.ftclib.hardware.motors.Motor;
+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.Gamepad;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.teamcode.drive.DriveConstants;
+import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
+import org.firstinspires.ftc.teamcode.drive.advanced.PoseStorage;
+
+
+@TeleOp (name = "DO NOT RUN")
+@Config
+public class AlignmentTest extends LinearOpMode {
+
+ // Define 2 states, driver control or alignment control
+ enum Mode {
+ NORMAL_CONTROL,
+ ALIGN_TO_POINT
+ }
+
+ private Mode currentMode = Mode.NORMAL_CONTROL;
+
+ // Declare a PIDF Controller to regulate heading
+ // Use the same gains as SampleMecanumDrive's heading controller
+ private PIDFController headingController = new PIDFController(SampleMecanumDrive.HEADING_PID);
+
+ // Declare a target vector you'd like your bot to align with
+ // Can be any x/y coordinate of your choosing
+ private Vector2d targetPosition = new Vector2d(0, 90);
+
+
+ @Override
+ public void runOpMode() throws InterruptedException {
+
+ /** SETUP **/
+
+ // Hardware Map Setup
+ Hware robot = new Hware();
+ robot.initialize(hardwareMap);
+
+ // Gamepad Setup
+ GamepadEx driverControl = new GamepadEx(gamepad1);
+ GamepadEx armControl = new GamepadEx(gamepad2);
+
+ // DriveBase Setup
+ SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
+
+ double SPEEDCONTROL = 1;
+
+ // Retrieve pose
+ drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+ drive.getLocalizer().setPoseEstimate(PoseStorage.currentPose);
+ headingController.setInputBounds(-Math.PI, Math.PI);
+
+ waitForStart();
+
+ while (opModeIsActive()) {
+
+ /** DRIVER CONTROLS **/
+
+ if (driverControl.getButton(GamepadKeys.Button.RIGHT_BUMPER) || driverControl.getButton(GamepadKeys.Button.LEFT_BUMPER)) { SPEEDCONTROL = .5; } else { SPEEDCONTROL = 1; }
+
+ // Read pose
+ Pose2d poseEstimate = drive.getLocalizer().getPoseEstimate();
+
+ // Declare a drive direction
+ // Pose representing desired x, y, and angular velocity
+ Pose2d driveDirection = new Pose2d();
+
+ telemetry.addData("mode", currentMode);
+
+ switch (currentMode) {
+ case NORMAL_CONTROL:
+ // Switch into alignment mode if `a` is pressed
+ if (gamepad1.triangle) {
+ currentMode = Mode.ALIGN_TO_POINT;
+ }
+
+ // Standard teleop control
+ // Convert gamepad input into desired pose velocity
+ driveDirection = new Pose2d(
+ -gamepad1.left_stick_y * SPEEDCONTROL,
+ -gamepad1.left_stick_x * SPEEDCONTROL,
+ -gamepad1.right_stick_x * SPEEDCONTROL
+ );
+ break;
+ case ALIGN_TO_POINT:
+ // Switch back into normal driver control mode if `b` is pressed
+ if (gamepad1.circle) {
+ currentMode = Mode.NORMAL_CONTROL;
+ }
+
+ // Create a vector from the gamepad x/y inputs which is the field relative movement
+ // Then, rotate that vector by the inverse of that heading for field centric control
+ Vector2d fieldFrameInput = new Vector2d(
+ -gamepad1.left_stick_y * SPEEDCONTROL,
+ -gamepad1.left_stick_x * SPEEDCONTROL
+ );
+ Vector2d robotFrameInput = fieldFrameInput.rotated(-poseEstimate.getHeading());
+
+ // Difference between the target vector and the bot's position
+ Vector2d difference = targetPosition.minus(poseEstimate.vec());
+ // Obtain the target angle for feedback and derivative for feedforward
+ double theta = difference.angle();
+
+ // Not technically omega because its power. This is the derivative of atan2
+ double thetaFF = -fieldFrameInput.rotated(-Math.PI / 2).dot(difference) / (difference.norm() * difference.norm());
+
+ // Set the target heading for the heading controller to our desired angle
+ headingController.setTargetPosition(theta);
+
+ // Set desired angular velocity to the heading controller output + angular
+ // velocity feedforward
+ double headingInput = (headingController.update(poseEstimate.getHeading())
+ * DriveConstants.kV + thetaFF)
+ * DriveConstants.TRACK_WIDTH;
+
+ // Combine the field centric x/y velocity with our derived angular velocity
+ driveDirection = new Pose2d(
+ robotFrameInput,
+ headingInput
+ );
+ break;
+ }
+
+ drive.setWeightedDrivePower(driveDirection);
+
+ // Update the heading controller with our current heading
+ headingController.update(poseEstimate.getHeading());
+
+ // Update the localizer
+ drive.getLocalizer().update();
+ // Print pose to telemetry
+ telemetry.addData("x", poseEstimate.getX());
+ telemetry.addData("y", poseEstimate.getY());
+ telemetry.addData("heading", poseEstimate.getHeading());
+ telemetry.update();
+
+ }
+
+ while (!isStopRequested() && opModeIsActive());
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoLock.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoLock.java
new file mode 100644
index 0000000..f566318
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoLock.java
@@ -0,0 +1,341 @@
+package org.firstinspires.ftc.teamcode;
+
+import android.util.Size;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.arcrobotics.ftclib.drivebase.MecanumDrive;
+import com.arcrobotics.ftclib.gamepad.GamepadEx;
+import com.arcrobotics.ftclib.gamepad.GamepadKeys;
+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.DcMotorEx;
+import com.qualcomm.robotcore.util.ElapsedTime;
+
+import org.firstinspires.ftc.robotcore.external.hardware.camera.BuiltinCameraDirection;
+import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+
+import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
+import org.firstinspires.ftc.robotcore.external.navigation.Position;
+import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles;
+import org.firstinspires.ftc.vision.VisionPortal;
+import org.firstinspires.ftc.vision.apriltag.AprilTagDetection;
+import org.firstinspires.ftc.vision.apriltag.AprilTagGameDatabase;
+import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor;
+
+import java.util.List;
+@Config
+@TeleOp(name = "Auto-aiming turret (Webcam AprilTag)", group = "Iterative Opmode")
+public class AutoLock extends OpMode {
+
+ // Hardware
+ private DcMotorEx turretMotor;
+
+ // Vision
+ private VisionPortal visionPortal;
+ private AprilTagProcessor aprilTag;
+
+ // PID gains
+ public double kP = 0.0012;
+ public static double kI = 0.001;
+ public static double kD = 0.002;
+
+ // Target center (degrees). 0 means center of camera.
+ private double targetX = 0.0;
+
+ private double integral = 0.0;
+ private double lastError = 0.0;
+
+ GamepadEx driverControl = null;
+ GamepadEx armControl = null;
+
+ private final ElapsedTime loopTimer = new ElapsedTime();
+ private final ElapsedTime targetLostTimer = new ElapsedTime();
+
+ // Tuning
+ private static final double POSITION_TOLERANCE_DEG = 1.5;
+ private static final double MIN_POWER = 0.05;
+ private static final double MAX_POWER = 0.4;
+ private static final double TARGET_LOST_TIMEOUT_SEC = 0.5;
+
+ // Flip if turret moves the wrong way
+ private static final boolean INVERT_MOTOR = true;
+
+ // OPTIONAL: only lock onto certain tag IDs. Leave empty to accept any.
+ // Example: new int[]{1,2,3}
+ private static final int[] DESIRED_TAG_IDS = new int[]{23, 24}; // empty = any
+
+ private boolean targetWasVisible = false;
+ private Position cameraPosition = new Position(DistanceUnit.INCH,
+ 0, 0, 0, 0);
+ private YawPitchRollAngles cameraOrientation = new YawPitchRollAngles(AngleUnit.DEGREES,
+ 0, -90, 0, 0);
+
+ // Drive Variables
+ double x, y, xy;
+ double SPEEDCONTROL;
+ boolean driveMode = true;
+ MecanumDrive drive = null;
+ Hware robot = null;
+
+ @Override
+ public void init() {
+ try {
+
+ // Hardware Map Setup
+ robot = new Hware();
+ robot.initialize(hardwareMap);
+
+ driverControl = new GamepadEx(gamepad1);
+ armControl = new GamepadEx(gamepad2);
+
+ // DriveBase Setup
+ drive = new MecanumDrive(true, robot.frontLeft, robot.frontRight, robot.backLeft, robot.backRight);
+
+
+ turretMotor = hardwareMap.get(DcMotorEx.class, "turret");
+ turretMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+
+ // AprilTag processor
+ aprilTag = new AprilTagProcessor.Builder()
+ .setTagFamily(AprilTagProcessor.TagFamily.TAG_36h11)
+ .setTagLibrary(AprilTagGameDatabase.getDecodeTagLibrary())
+ .setOutputUnits(DistanceUnit.INCH, AngleUnit.DEGREES)
+ // If you have a Logitech C920/C270 etc, you can usually leave defaults.
+ // If you have calibrated intrinsics, you can set them here (advanced).
+ .build();
+
+ // Vision portal with webcam
+ visionPortal = new VisionPortal.Builder()
+ .setCamera(hardwareMap.get(WebcamName.class, "Webcam 1"))
+ .addProcessor(aprilTag)
+ .build();
+
+
+ loopTimer.reset();
+ targetLostTimer.reset();
+
+ telemetry.addData("Status", "Initialized (Webcam + AprilTag)");
+ telemetry.addData("Motor Inverted", INVERT_MOTOR);
+ } catch (Exception e) {
+ telemetry.addData("Init Error", e.getMessage());
+ }
+ }
+
+ @Override
+ public void loop() {
+ try {
+ // Get current detections
+ List detections = aprilTag.getDetections();
+
+ AprilTagDetection best = pickBestDetection(detections);
+
+ telemetry.addData("Detections", detections.size());
+ for (AprilTagDetection d : detections) {
+ telemetry.addData("ID", d.id);
+ telemetry.addData("Metadata null?", d.metadata == null);
+ telemetry.addData("Pose null?", d.ftcPose == null);
+
+ }
+
+ /** DRIVER CONTROLS **/
+
+ if (gamepad2.x) { driveMode = true; } else if (gamepad2.y) { driveMode = false;}
+
+ // Chassis
+ if (!driveMode) { SPEEDCONTROL = 0;} else if (driverControl.getButton(GamepadKeys.Button.RIGHT_BUMPER) || driverControl.getButton(GamepadKeys.Button.LEFT_BUMPER)) { SPEEDCONTROL = .5; } else { SPEEDCONTROL = 1; }
+ x = driverControl.getLeftX();
+ y = driverControl.getLeftY();
+ xy = driverControl.getRightX();
+
+ drive.driveRobotCentric(
+ -x * SPEEDCONTROL,
+ -y * SPEEDCONTROL,
+ -xy * SPEEDCONTROL
+ );
+
+ // Intake
+ if (driverControl.getTrigger(GamepadKeys.Trigger.RIGHT_TRIGGER) > 0.5) { robot.intake.set(1); } else if (driverControl.getTrigger(GamepadKeys.Trigger.LEFT_TRIGGER) > 0.5) { robot.intake.set(-1); } else { robot.intake.set(0); }
+
+ /** Arm Controls **/
+// if (armControl.getTrigger(GamepadKeys.Trigger.RIGHT_TRIGGER) > 0.5) { robot.turret.set(.3); } else if (armControl.getTrigger(GamepadKeys.Trigger.LEFT_TRIGGER) > 0.5) { robot.turret.set(-0.3); } else { robot.turret.set(0); }
+// if (armControl.getButton(GamepadKeys.Button.X)) { robot.launchLeft.set(1); robot.launchRight.set(1); } else {robot.launchLeft.set(0); robot.launchRight.set(0);}
+// if (!driveMode) {robot.rightPTO.setPosition(1); robot.leftPTO.setPosition(1);} else {robot.rightPTO.setPosition(0); robot.rightPTO.setPosition(0);}
+
+
+ if (best != null) {
+ targetWasVisible = true;
+ targetLostTimer.reset();
+
+ // ----- "tx" equivalent -----
+ // AprilTag gives pose relative to camera. We want horizontal angle offset.
+ // Use bearing/yaw style value if available from metadata or pose.
+ //
+ // Most reliable: compute from pose X/Z (left/right vs forward):
+ // angle = atan2(x, z)
+ //
+ // In FTC pose:
+ // - pose.x: left/right (inches or meters depending on config; typically inches)
+ // - pose.z: forward distance
+ //
+ // If your coordinate axes differ, flip sign once using INVERT_MOTOR or sign flip here.
+ double x = best.ftcPose.x; // left(+) / right(-) depending on convention
+ double z = best.ftcPose.z; // forward distance
+ double txDeg = Math.toDegrees(Math.atan2(x, z));
+
+ // Timing
+ double dt = loopTimer.seconds();
+ loopTimer.reset();
+
+ if (dt < 0.001) dt = 0.001;
+ if (dt > 1.0) dt = 1.0;
+
+ // Error (positive tx means tag is to one side)
+ double error = txDeg - targetX;
+
+ // PID
+ integral += error * dt;
+ integral = Math.max(-50, Math.min(50, integral)); // anti-windup
+
+ double derivative = (error - lastError) / dt;
+
+ double pidOutput = (kP * error) + (kI * integral) + (kD * derivative);
+
+ // Deadband
+ if (Math.abs(error) < POSITION_TOLERANCE_DEG) {
+ pidOutput = 0;
+ integral = 0;
+ }
+
+ // Min power to overcome friction
+ if (pidOutput != 0 && Math.abs(pidOutput) < MIN_POWER) {
+ pidOutput = MIN_POWER * Math.signum(pidOutput);
+ }
+
+ // Clamp
+ double motorPower = clamp(pidOutput, -MAX_POWER, MAX_POWER);
+
+ if (INVERT_MOTOR) motorPower = -motorPower;
+
+ if (!Double.isFinite(motorPower)) {
+ motorPower = 0;
+ resetPID();
+ }
+
+ turretMotor.setPower(motorPower);
+ lastError = error;
+
+ telemetry.addData("Status", "LOCKED ON");
+ telemetry.addData("Tag ID", best.id);
+ telemetry.addData("tx (deg)", "%.2f", txDeg);
+ telemetry.addData("Error", "%.2f", error);
+ telemetry.addData("PID Output", "%.3f", pidOutput);
+ telemetry.addData("Motor Power", "%.3f", motorPower);
+
+ // Extra pose telemetry (helpful for debugging)
+ telemetry.addData("Pose x", "%.2f", best.ftcPose.x);
+ telemetry.addData("Pose z", "%.2f", best.ftcPose.z);
+ telemetry.addData("Range: ", best.ftcPose.range);
+ telemetry.addData("Bearing", "%.2f deg", best.ftcPose.bearing);
+ telemetry.addData("Yaw", "%.2f deg", best.ftcPose.yaw);
+
+ if(best.ftcPose.range < 40){
+ kP = 0.008;
+ } else if (best.ftcPose.range < 70) {
+ kP = 0.005;
+ } else if (best.ftcPose.range < 90){
+ kP = 0.004;
+ } else if (best.ftcPose.range < 110) {
+ kP = 0.003;
+ } else if (best.ftcPose.range < 130) {
+ kP = 0.002;
+ } else if (best.ftcPose.range < 150) {
+ kP = 0.001;
+ } else {
+ kP = 0.0005;
+ }
+ telemetry.addData("kP: ", kP);
+
+ } else {
+ handleNoTarget();
+ }
+
+ } catch (Exception e) {
+ telemetry.addData("Error", e.getMessage());
+ telemetry.addData("Error Type", e.getClass().getSimpleName());
+ stopTurret();
+ }
+
+ telemetry.update();
+ }
+
+ /**
+ * Pick "best" detection. Simple approach:
+ * - If DESIRED_TAG_IDS set: only accept those
+ * - Choose closest (smallest range) among candidates
+ */
+ private AprilTagDetection pickBestDetection(List detections) {
+ if (detections == null || detections.isEmpty()) return null;
+
+ AprilTagDetection best = null;
+ double bestRange = Double.POSITIVE_INFINITY;
+
+ for (AprilTagDetection d : detections) {
+ if (d == null) continue;
+
+ if (!isDesiredId(d.id)) continue;
+
+ // Use range as "quality" metric
+ double r = d.ftcPose.range;
+ if (r < bestRange) {
+ bestRange = r;
+ best = d;
+ }
+ }
+ return best;
+ }
+
+ private boolean isDesiredId(int id) {
+ if (DESIRED_TAG_IDS == null || DESIRED_TAG_IDS.length == 0) return true;
+ for (int x : DESIRED_TAG_IDS) {
+ if (x == id) return true;
+ }
+ return false;
+ }
+
+ private void handleNoTarget() {
+ if (targetWasVisible && targetLostTimer.seconds() < TARGET_LOST_TIMEOUT_SEC) {
+ telemetry.addData("Status", "TRACKING LOST - Coasting");
+ telemetry.addData("Time Lost", "%.2f s", targetLostTimer.seconds());
+ // You could also keep last motor power for a brief time if you store it.
+ } else {
+ stopTurret();
+ telemetry.addData("Status", "SEARCHING");
+ telemetry.addData("Target Visible", "No");
+ }
+ }
+
+ private void stopTurret() {
+ if (turretMotor != null) turretMotor.setPower(0);
+ resetPID();
+ targetWasVisible = false;
+ }
+
+ private void resetPID() {
+ integral = 0;
+ lastError = 0;
+ }
+
+ private static double clamp(double v, double lo, double hi) {
+ return Math.max(lo, Math.min(hi, v));
+ }
+
+ @Override
+ public void stop() {
+ try {
+ if (turretMotor != null) turretMotor.setPower(0);
+ if (visionPortal != null) visionPortal.close();
+ } catch (Exception ignored) {}
+ }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auton/blueDepot.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auton/blueDepot.java
new file mode 100644
index 0000000..085fba8
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Auton/blueDepot.java
@@ -0,0 +1,138 @@
+package org.firstinspires.ftc.teamcode.Auton;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryVelocityConstraint;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.hardware.DcMotor;
+
+import org.firstinspires.ftc.teamcode.Hware;
+import org.firstinspires.ftc.teamcode.drive.DriveConstants;
+import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
+import org.firstinspires.ftc.teamcode.drive.advanced.PoseStorage;
+import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequence;
+
+@Config
+@Autonomous(name = "Blue Depot Cycle")
+public class blueDepot extends LinearOpMode {
+
+ Hware robot = new Hware();
+ double TICKS_PER_DEGREE = (384.5 * 9.26) / 360;
+
+ public void turretPower(double power) {
+ robot.turret.setPower(power);
+ }
+
+ public void turretPos(int position) {
+ turretPower(0);
+ robot.turret.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+ robot.turret.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+ robot.turret.setTargetPosition(position * (int) TICKS_PER_DEGREE);
+ robot.turret.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+ turretPower(1);
+ }
+
+ public void spintakeStart(){
+ robot.intake.set(1);
+ }
+
+ public void spintakeEnd(){
+ robot.intake.set(0);
+ }
+
+ public void shoot(){
+ robot.topFly.set(1);
+ robot.bottomFly.set(1);
+ robot.activeTransfer.setPosition(1);
+ }
+
+ public void reset(){
+ robot.intake.set(0);
+ robot.topFly.set(0);
+ robot.bottomFly.set(0);
+ robot.launchHood.setPosition(0.3);
+ robot.activeTransfer.setPosition(0);
+ }
+
+
+ public void runOpMode() throws InterruptedException {
+
+ //webcam1 = hardwareMap.get(WebcamName.class, "Webcam 2");
+ SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
+
+
+ Pose2d startPose = new Pose2d(0, 0, Math.toRadians(0));
+
+ drive.setPoseEstimate(startPose);
+
+ TrajectorySequence oneCycle = drive.trajectorySequenceBuilder(startPose)
+ .strafeLeft(25)
+ .UNSTABLE_addTemporalMarkerOffset(.3, () -> {
+ shoot();
+ })
+ .waitSeconds(1)
+ .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
+ reset();
+ })
+ .lineToLinearHeading(new Pose2d(0,0, Math.toRadians(0)))
+ .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
+ spintakeStart();
+ })
+ .forward(30, SampleMecanumDrive.getVelocityConstraint(30, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
+ SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
+ .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
+ spintakeEnd();
+ })
+ .lineToLinearHeading(new Pose2d(0,0, Math.toRadians(0)))
+ .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
+ shoot();
+ })
+ .waitSeconds(1)
+ .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
+ reset();
+ })
+ .lineToLinearHeading(new Pose2d(0,0, Math.toRadians(0)))
+ .forward(10)
+ .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
+ spintakeStart();
+ })
+ .strafeLeft(10)
+ .forward(3)
+ .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
+ spintakeEnd();
+ })
+ .lineToLinearHeading(new Pose2d(0,0, Math.toRadians(0)))
+ .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
+ shoot();
+ })
+ .waitSeconds(1)
+ .lineToLinearHeading(new Pose2d(0,0, Math.toRadians(0)))
+ .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
+ spintakeStart();
+ })
+ .forward(30, SampleMecanumDrive.getVelocityConstraint(30, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH),
+ SampleMecanumDrive.getAccelerationConstraint(DriveConstants.MAX_ACCEL))
+ .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
+ spintakeEnd();
+ })
+ .lineToLinearHeading(new Pose2d(0,0, Math.toRadians(0)))
+ .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
+ shoot();
+ })
+ .waitSeconds(1)
+ .UNSTABLE_addTemporalMarkerOffset(.1, () -> {
+ reset();
+ })
+ .lineToLinearHeading(new Pose2d(0,0, Math.toRadians(0)))
+ .build();
+
+ waitForStart();
+ if (opModeIsActive() && !isStopRequested()) {
+// while (opModeIsActive()) {
+ drive.followTrajectorySequence(oneCycle);
+ PoseStorage.currentPose = drive.getPoseEstimate();
+// }
+ }
+ }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DriverControlsDoubleV1.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DriverControlsDoubleV1.java
new file mode 100644
index 0000000..4db4b9a
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DriverControlsDoubleV1.java
@@ -0,0 +1,120 @@
+package org.firstinspires.ftc.teamcode;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.arcrobotics.ftclib.gamepad.GamepadEx;
+import com.arcrobotics.ftclib.gamepad.GamepadKeys;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
+import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequence;
+
+import java.lang.reflect.Method;
+
+@TeleOp
+@Config
+public class DriverControlsDoubleV1 extends LinearOpMode {
+
+ @Override
+ public void runOpMode() {
+
+ Hware robot = new Hware();
+ robot.initialize(hardwareMap);
+
+ GamepadEx driverControl = new GamepadEx(gamepad1);
+ GamepadEx armControl = new GamepadEx(gamepad2);
+
+ SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
+
+ boolean xPrev = false;
+ boolean bPrev = false;
+
+ final boolean[] shooting = { false };
+
+ Pose2d startPose = drive.getPoseEstimate();
+
+ TrajectorySequence shootSeq = drive.trajectorySequenceBuilder(startPose)
+ .addTemporalMarker(0.00, () -> {
+ robot.intake.set(0);
+ robot.activeTransfer.setPosition(0.7);
+ robot.topFly.set(1);
+ robot.bottomFly.set(1);
+ })
+ .waitSeconds(0.50)
+ .addTemporalMarker(0.00, () -> {
+ robot.activeTransfer.setPosition(1);
+ robot.intake.set(1);
+ })
+ .waitSeconds(0.70)
+ .addTemporalMarker(0.00, () -> {
+ stopAll(robot);
+ shooting[0] = false;
+ })
+ .build();
+
+ waitForStart();
+
+ while (opModeIsActive()) {
+
+ drive.update();
+
+ if (!shooting[0]) {
+ if (driverControl.getTrigger(GamepadKeys.Trigger.RIGHT_TRIGGER) > 0.5) {
+ robot.intake.set(1);
+ robot.activeTransfer.setPosition(0.7);
+ } else if (driverControl.getTrigger(GamepadKeys.Trigger.LEFT_TRIGGER) > 0.5) {
+ robot.intake.set(-1);
+ } else {
+ robot.intake.set(0);
+ }
+ }
+
+ boolean xNow = armControl.getButton(GamepadKeys.Button.X);
+ if (xNow && !xPrev && !shooting[0]) {
+ shooting[0] = true;
+ drive.followTrajectorySequenceAsync(shootSeq);
+ }
+ xPrev = xNow;
+
+ boolean bNow = armControl.getButton(GamepadKeys.Button.B);
+ if (bNow && !bPrev && shooting[0]) {
+ cancelTrajectorySequenceCompat(drive);
+ stopAll(robot);
+ shooting[0] = false;
+ }
+ bPrev = bNow;
+
+ telemetry.addData("Shooting", shooting[0]);
+ telemetry.addData("RR Busy", drive.isBusy());
+ telemetry.update();
+ }
+ }
+
+ private static void stopAll(Hware robot) {
+ robot.intake.set(0);
+ robot.topFly.set(0);
+ robot.bottomFly.set(0);
+ robot.activeTransfer.setPosition(0.7);
+ }
+
+ private static void cancelTrajectorySequenceCompat(Object drive) {
+ if (invokeIfExists(drive, "cancelTrajectorySequence")) return;
+ if (invokeIfExists(drive, "cancelFollowing")) return;
+ if (invokeIfExists(drive, "breakFollowing")) return;
+ try {
+ Method m = drive.getClass().getMethod("setDrivePower", Pose2d.class);
+ m.invoke(drive, new Pose2d(0, 0, 0));
+ } catch (Exception ignored) {}
+ }
+
+ private static boolean invokeIfExists(Object obj, String methodName) {
+ try {
+ Method m = obj.getClass().getMethod(methodName);
+ m.invoke(obj);
+ return true;
+ } catch (Exception e) {
+ return false;
+ }
+ }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Hware.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Hware.java
new file mode 100644
index 0000000..c6734fd
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Hware.java
@@ -0,0 +1,60 @@
+package org.firstinspires.ftc.teamcode;
+
+import com.arcrobotics.ftclib.hardware.ServoEx;
+import com.arcrobotics.ftclib.hardware.SimpleServo;
+import com.arcrobotics.ftclib.hardware.motors.Motor;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+
+import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
+
+public class Hware {
+
+ HardwareMap hardwareMap;
+ public Motor frontRight, frontLeft, backRight, backLeft, intake, bottomFly, topFly;
+ public DcMotor turret;
+ public SimpleServo activeTransfer, rightPTO, leftPTO, launchHood;
+ WebcamName webcam;
+ public void initialize(HardwareMap hwMap) {
+ hardwareMap = hwMap;
+
+ /** MOTORS **/
+
+// Chassis Motors
+ frontRight = new Motor(hardwareMap, "frontRight", Motor.GoBILDA.RPM_435);
+ frontLeft = new Motor(hardwareMap, "frontLeft", Motor.GoBILDA.RPM_435);
+ backRight = new Motor(hardwareMap, "backRight", Motor.GoBILDA.RPM_435);
+ backLeft = new Motor(hardwareMap, "backLeft", Motor.GoBILDA.RPM_435);
+
+// Intake
+ intake = new Motor(hardwareMap, "intake", Motor.GoBILDA.RPM_1620);
+
+// Launch
+ bottomFly = new Motor(hardwareMap, "bottomFly", Motor.GoBILDA.BARE);
+ topFly = new Motor(hardwareMap, "topFly", Motor.GoBILDA.BARE);
+
+// Turret
+ turret = hardwareMap.get(DcMotorEx.class, "turret");
+
+
+ /** SERVOS **/
+ activeTransfer = new SimpleServo(hardwareMap, "activeTransfer", 0, 100);
+ leftPTO = new SimpleServo(hardwareMap, "leftPTO", 0, 90);
+ rightPTO = new SimpleServo(hardwareMap, "rightPTO", 0, 90);
+ launchHood = new SimpleServo(hardwareMap, "launchHood", 0, 180);
+
+ /** Cam **/
+// webcam = hardwareMap.get(WebcamName.class, "cam");
+
+// Zero Power Behaviors
+ frontRight.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE);
+ frontLeft.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE);
+ backLeft.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE);
+ backRight.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE);
+ intake.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE);
+
+ bottomFly.setInverted(true);
+ }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OdometryAutoLock.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OdometryAutoLock.java
new file mode 100644
index 0000000..258d01f
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OdometryAutoLock.java
@@ -0,0 +1,262 @@
+package org.firstinspires.ftc.teamcode;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.roadrunner.control.PIDFController;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.geometry.Vector2d;
+import com.arcrobotics.ftclib.gamepad.GamepadEx;
+import com.arcrobotics.ftclib.gamepad.GamepadKeys;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.hardware.DcMotor;
+
+import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
+import org.firstinspires.ftc.teamcode.drive.SampleMecanumDriveRIGGED;
+import org.firstinspires.ftc.teamcode.drive.advanced.PoseStorage;
+import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequence;
+import org.firstinspires.ftc.teamcode.util.PersonalPID;
+
+import java.lang.reflect.Method;
+import java.util.concurrent.atomic.AtomicBoolean;
+
+
+@TeleOp (name = "OdoAutoLock")
+@Config
+public class OdometryAutoLock extends LinearOpMode {
+
+ // Declare a PIDF Controller to regulate heading
+ // Use the same gains as SampleMecanumDrive's heading controller
+ private PIDFController headingController = new PIDFController(SampleMecanumDrive.HEADING_PID);
+ public static double MAX_LOCK_ANGLE = Math.toRadians(90); // ±90°
+
+ // Declare a target vector you'd like your bot to align with
+ // Can be any x/y coordinate of your choosing
+ private Vector2d targetPosition = new Vector2d(0, 45);
+
+ private PersonalPID controller;
+
+ public static double p = 0.025, i = 0, d = 0.0001, f = 0.001;
+
+ public static int target = 0;
+
+ public static double TICKS_PER_DEGREE = (384.5 * 9.26) / 360;
+ public static int TURRET_MIN_TICKS = (int) (TICKS_PER_DEGREE * (-90));
+ public static int TURRET_MAX_TICKS = (int) (TICKS_PER_DEGREE * (90));
+
+
+ @Override
+ public void runOpMode() throws InterruptedException {
+
+ /** SETUP **/
+
+ // Hardware Map Setup
+ Hware robot = new Hware();
+ robot.initialize(hardwareMap);
+ controller = new PersonalPID(p, i, d, f);
+
+ // Gamepad Setup
+ GamepadEx driverControl = new GamepadEx(gamepad1);
+ GamepadEx armControl = new GamepadEx(gamepad2);
+
+
+ // DriveBase Setup
+ SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
+ SampleMecanumDriveRIGGED driveRIGGED = new SampleMecanumDriveRIGGED(hardwareMap);
+
+ double SPEEDCONTROL = 1;
+ Boolean auto = false;
+ // Retrieve pose
+ drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+ drive.getLocalizer().setPoseEstimate(PoseStorage.currentPose);
+ headingController.setInputBounds(-Math.PI, Math.PI);
+
+ boolean xPrev = false;
+ boolean bPrev = false;
+
+ final boolean[] shooting = { false };
+
+ Pose2d startPose = drive.getPoseEstimate();
+
+ TrajectorySequence shootSeq = driveRIGGED.trajectorySequenceBuilder(startPose)
+ .forward(.1)
+ .addDisplacementMarker(()-> {
+ robot.activeTransfer.setPosition(1);
+ robot.intake.set(1);
+ })
+ .build();
+
+ TrajectorySequence runUp = driveRIGGED.trajectorySequenceBuilder(startPose)
+ .forward(.1)
+ .addDisplacementMarker(()-> {
+ robot.topFly.set(1);
+ robot.bottomFly.set(1);
+ })
+ .forward(.1)
+ .waitSeconds(1.5)
+ .forward(.1)
+ .addDisplacementMarker(()-> {
+ gamepad1.rumbleBlips(2);
+ gamepad2.rumbleBlips(2);
+ })
+ .build();
+
+
+ waitForStart();
+
+ robot.turret.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+
+ while (opModeIsActive()) {
+
+ double distance = Math.hypot(drive.getPoseEstimate().getX() - targetPosition.getX(), drive.getPoseEstimate().getY() - targetPosition.getY());
+ double height = 62;
+ double launchAngle = Math.atan2(height, distance);
+ /** DRIVER CONTROLS **/
+
+ if (driverControl.getButton(GamepadKeys.Button.RIGHT_BUMPER) || driverControl.getButton(GamepadKeys.Button.LEFT_BUMPER)) { SPEEDCONTROL = .5; } else { SPEEDCONTROL = 1; }
+
+ // Read pose
+ Pose2d poseEstimate = drive.getLocalizer().getPoseEstimate();
+
+ // Declare a drive direction
+ // Pose representing desired x, y, and angular velocity
+ Pose2d driveDirection = new Pose2d();
+
+ // Standard teleop control
+ // Convert gamepad input into desired pose velocity
+ driveDirection = new Pose2d(
+ -gamepad1.left_stick_y * SPEEDCONTROL,
+ -gamepad1.left_stick_x * SPEEDCONTROL,
+ -gamepad1.right_stick_x * SPEEDCONTROL
+ );
+
+
+ drive.setWeightedDrivePower(driveDirection);
+
+ double headingPose = poseEstimate.getHeading();
+ //9.26
+
+ double totalHeading = ((Math.toRadians((robot.turret.getCurrentPosition() / TICKS_PER_DEGREE))) + headingPose) % (2 * Math.PI);
+//
+ double theta = Math.atan2(targetPosition.getY() - drive.getPoseEstimate().getY(), targetPosition.getX() - drive.getPoseEstimate().getX()) - drive.getPoseEstimate().getHeading();
+// theta = Math.atan2(Math.sin(theta), Math.cos(theta));
+
+ boolean withinAngle =
+ Math.abs(theta) < MAX_LOCK_ANGLE;
+
+// if((realTheta > (2 * Math.PI - 0.5)) || (realTheta < (-2 * Math.PI + 0.5))){
+// robot.turret.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+// }
+
+
+ controller.setPIDF(p, i, d, f);
+ int armPos = robot.turret.getCurrentPosition();
+ double pid = controller.calculate(armPos, target);
+
+
+ if (withinAngle) {
+
+ int desiredTarget = (int)(TICKS_PER_DEGREE * Math.toDegrees(theta));
+
+ // Clamp to turret mechanical limits
+ desiredTarget = Math.max(
+ TURRET_MIN_TICKS,
+ Math.min(TURRET_MAX_TICKS, desiredTarget)
+ );
+
+ target = desiredTarget;
+
+ controller.setPIDF(p, i, d, f);
+ pid = controller.calculate(armPos, target);
+
+ robot.turret.setPower(pid);
+
+ } else {
+ // Outside lock range → stop or hold
+ robot.turret.setPower(0);
+ }
+
+ telemetry.addData("armPos", armPos);
+ telemetry.addData("target", target);
+ telemetry.addData("pid", pid);
+ telemetry.update();
+
+ driveRIGGED.update();
+
+ if (driverControl.getTrigger(GamepadKeys.Trigger.RIGHT_TRIGGER) > 0.5) {
+ auto = false;
+ robot.intake.set(1);
+ robot.activeTransfer.setPosition(0.85);
+ } else if (driverControl.getTrigger(GamepadKeys.Trigger.LEFT_TRIGGER) > 0.5) {
+ auto = false;
+ robot.intake.set(-1);
+ } else {
+ if (!auto) {
+ robot.intake.set(0);
+ }
+ }
+
+ if(gamepad2.dpad_down) {
+ robot.topFly.set(0);
+ robot.bottomFly.set(0);
+ }
+
+
+ if (gamepad2.dpad_up) {
+ if(!auto) {
+ auto = true;
+ driveRIGGED.followTrajectorySequenceAsync(shootSeq);
+ }
+ }
+
+ if (gamepad2.dpad_right) {
+ driveRIGGED.followTrajectorySequenceAsync(runUp);
+ }
+
+ telemetry.addData("Shooting", shooting[0]);
+ telemetry.addData("RR Busy", drive.isBusy());
+
+ // Update the localizer
+ drive.getLocalizer().update();
+ // Print pose to telemetry
+ telemetry.addData("totalHeading", totalHeading);
+ telemetry.addData("theta", theta);
+ telemetry.addData("x", poseEstimate.getX());
+ telemetry.addData("y", poseEstimate.getY());
+ telemetry.addData("heading", poseEstimate.getHeading());
+ telemetry.addData("auton", auto);
+ telemetry.update();
+
+ }
+
+
+
+ while (!isStopRequested() && opModeIsActive());
+ }
+
+ private static void stopAll(Hware robot) {
+ robot.intake.set(0);
+ robot.topFly.set(0);
+ robot.bottomFly.set(0);
+ robot.activeTransfer.setPosition(0.85);
+ }
+
+ private static void cancelTrajectorySequenceCompat(Object drive) {
+ if (invokeIfExists(drive, "cancelTrajectorySequence")) return;
+ if (invokeIfExists(drive, "cancelFollowing")) return;
+ if (invokeIfExists(drive, "breakFollowing")) return;
+ try {
+ Method m = drive.getClass().getMethod("setDrivePower", Pose2d.class);
+ m.invoke(drive, new Pose2d(0, 0, 0));
+ } catch (Exception ignored) {}
+ }
+
+ private static boolean invokeIfExists(Object obj, String methodName) {
+ try {
+ Method m = obj.getClass().getMethod(methodName);
+ m.invoke(obj);
+ return true;
+ } catch (Exception e) {
+ return false;
+ }
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ServoTesting.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ServoTesting.java
new file mode 100644
index 0000000..adb3644
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ServoTesting.java
@@ -0,0 +1,62 @@
+package org.firstinspires.ftc.teamcode;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.roadrunner.control.PIDFController;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.geometry.Vector2d;
+import com.arcrobotics.ftclib.drivebase.MecanumDrive;
+import com.arcrobotics.ftclib.gamepad.GamepadEx;
+import com.arcrobotics.ftclib.gamepad.GamepadKeys;
+import com.arcrobotics.ftclib.hardware.motors.Motor;
+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.Gamepad;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.teamcode.drive.DriveConstants;
+import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
+import org.firstinspires.ftc.teamcode.drive.advanced.PoseStorage;
+import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequence;
+
+import java.lang.reflect.Method;
+
+
+@TeleOp (name = "Servo")
+@Config
+public class ServoTesting extends LinearOpMode {
+
+
+ @Override
+ public void runOpMode() throws InterruptedException {
+
+ /** SETUP **/
+
+ // Hardware Map Setup
+ Hware robot = new Hware();
+ robot.initialize(hardwareMap);
+
+ GamepadEx armControl = new GamepadEx(gamepad2);
+
+
+ waitForStart();
+ while (opModeIsActive()) {
+ if (armControl.getTrigger(GamepadKeys.Trigger.RIGHT_TRIGGER) > 0.5) {
+ robot.launchHood.setPosition(0.35);
+ } else if (armControl.getTrigger(GamepadKeys.Trigger.LEFT_TRIGGER) > 0.5) {
+ robot.launchHood.setPosition(0.85);
+ }
+
+ if (armControl.getButton(GamepadKeys.Button.DPAD_UP)) {
+ robot.activeTransfer.setPosition(1);
+ } else if (armControl.getButton(GamepadKeys.Button.DPAD_DOWN)) {
+ robot.activeTransfer.setPosition(0.85);
+ }
+
+ }
+
+
+
+ while (!isStopRequested() && opModeIsActive());
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/DriveConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/DriveConstants.java
new file mode 100644
index 0000000..80dd0e2
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/DriveConstants.java
@@ -0,0 +1,94 @@
+package org.firstinspires.ftc.teamcode.drive;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
+import com.qualcomm.robotcore.hardware.PIDFCoefficients;
+
+/*
+ * Constants shared between multiple drive types.
+ *
+ * TODO: Tune or adjust the following constants to fit your robot. Note that the non-final
+ * fields may also be edited through the dashboard (connect to the robot's WiFi network and
+ * navigate to https://192.168.49.1:8080/dash). Make sure to save the values here after you
+ * adjust them in the dashboard; **config variable changes don't persist between app restarts**.
+ *
+ * These are not the only parameters; some are located in the localizer classes, drive base classes,
+ * and op modes themselves.
+ */
+@Config
+public class DriveConstants {
+
+ /*
+ * These are motor constants that should be listed online for your motors.
+ */
+ public static final double TICKS_PER_REV = 384.5;
+ public static final double MAX_RPM = 435;
+
+ /*
+ * Set RUN_USING_ENCODER to true to enable built-in hub velocity control using drive encoders.
+ * Set this flag to false if drive encoders are not present and an alternative localization
+ * method is in use (e.g., tracking wheels).
+ *
+ * If using the built-in motor velocity PID, update MOTOR_VELO_PID with the tuned coefficients
+ * from DriveVelocityPIDTuner.
+ */
+ public static final boolean RUN_USING_ENCODER = false;
+ public static PIDFCoefficients MOTOR_VELO_PID = new PIDFCoefficients(0, 0, 0,
+ getMotorVelocityF(MAX_RPM / 60 * TICKS_PER_REV));
+
+ /*
+ * These are physical constants that can be determined from your robot (including the track
+ * width; it will be tune empirically later although a rough estimate is important). Users are
+ * free to chose whichever linear distance unit they would like so long as it is consistently
+ * used. The default values were selected with inches in mind. Road runner uses radians for
+ * angular distances although most angular parameters are wrapped in Math.toRadians() for
+ * convenience. Make sure to exclude any gear ratio included in MOTOR_CONFIG from GEAR_RATIO.
+ */
+ public static double WHEEL_RADIUS = 1.8898; // in
+ public static double GEAR_RATIO = (24 / 22); // output (wheel) speed / input (motor) speed
+ public static double TRACK_WIDTH = 12.3; // in
+
+ /*
+ * These are the feedforward parameters used to model the drive motor behavior. If you are using
+ * the built-in velocity PID, *these values are fine as is*. However, if you do not have drive
+ * motor encoders or have elected not to use them for velocity control, these values should be
+ * empirically tuned.
+ */
+ public static double kV = 1.0 / rpmToVelocity(MAX_RPM);
+ public static double kA = 0;
+ public static double kStatic = 0;
+
+ /*
+ * These values are used to generate the trajectories for you robot. To ensure proper operation,
+ * the constraints should never exceed ~80% of the robot's actual capabilities. While Road
+ * Runner is designed to enable faster autonomous motion, it is a good idea for testing to start
+ * small and gradually increase them later after everything is working. All distance units are
+ * inches.
+ */
+ public static double MAX_VEL = 60;
+ public static double MAX_ACCEL = 60;
+ public static double MAX_ANG_VEL = Math.toRadians(90);
+ public static double MAX_ANG_ACCEL = Math.toRadians(90);
+
+ /*
+ * Adjust the orientations here to match your robot. See the FTC SDK documentation for details.
+ */
+ public static RevHubOrientationOnRobot.LogoFacingDirection LOGO_FACING_DIR =
+ RevHubOrientationOnRobot.LogoFacingDirection.RIGHT;
+ public static RevHubOrientationOnRobot.UsbFacingDirection USB_FACING_DIR =
+ RevHubOrientationOnRobot.UsbFacingDirection.UP;
+
+
+ public static double encoderTicksToInches(double ticks) {
+ return WHEEL_RADIUS * 2 * Math.PI * GEAR_RATIO * ticks / TICKS_PER_REV;
+ }
+
+ public static double rpmToVelocity(double rpm) {
+ return rpm * GEAR_RATIO * 2 * Math.PI * WHEEL_RADIUS / 60.0;
+ }
+
+ public static double getMotorVelocityF(double ticksPerSecond) {
+ // see https://docs.google.com/document/d/1tyWrXDfMidwYyP_5H4mZyVgaEswhOC35gvdmP-V-5hA/edit#heading=h.61g9ixenznbx
+ return 32767 / ticksPerSecond;
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleMecanumDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleMecanumDrive.java
new file mode 100644
index 0000000..125af1f
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleMecanumDrive.java
@@ -0,0 +1,314 @@
+package org.firstinspires.ftc.teamcode.drive;
+
+import androidx.annotation.NonNull;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.roadrunner.control.PIDCoefficients;
+import com.acmerobotics.roadrunner.drive.DriveSignal;
+import com.acmerobotics.roadrunner.drive.MecanumDrive;
+import com.acmerobotics.roadrunner.followers.HolonomicPIDVAFollower;
+import com.acmerobotics.roadrunner.followers.TrajectoryFollower;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.trajectory.Trajectory;
+import com.acmerobotics.roadrunner.trajectory.TrajectoryBuilder;
+import com.acmerobotics.roadrunner.trajectory.constraints.AngularVelocityConstraint;
+import com.acmerobotics.roadrunner.trajectory.constraints.MecanumVelocityConstraint;
+import com.acmerobotics.roadrunner.trajectory.constraints.MinVelocityConstraint;
+import com.acmerobotics.roadrunner.trajectory.constraints.ProfileAccelerationConstraint;
+import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryAccelerationConstraint;
+import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryVelocityConstraint;
+import com.qualcomm.hardware.lynx.LynxModule;
+import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+import com.qualcomm.robotcore.hardware.IMU;
+import com.qualcomm.robotcore.hardware.PIDFCoefficients;
+import com.qualcomm.robotcore.hardware.VoltageSensor;
+import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType;
+
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequence;
+import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequenceBuilder;
+import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequenceRunner;
+import org.firstinspires.ftc.teamcode.util.LynxModuleUtil;
+
+import java.util.ArrayList;
+import java.util.Arrays;
+import java.util.List;
+
+import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_ACCEL;
+import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_ANG_ACCEL;
+import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_ANG_VEL;
+import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_VEL;
+import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MOTOR_VELO_PID;
+import static org.firstinspires.ftc.teamcode.drive.DriveConstants.RUN_USING_ENCODER;
+import static org.firstinspires.ftc.teamcode.drive.DriveConstants.TRACK_WIDTH;
+import static org.firstinspires.ftc.teamcode.drive.DriveConstants.encoderTicksToInches;
+import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kA;
+import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kStatic;
+import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kV;
+
+/*
+ * Simple mecanum drive hardware implementation for REV hardware.
+ */
+@Config
+public class SampleMecanumDrive extends MecanumDrive {
+ public static PIDCoefficients TRANSLATIONAL_PID = new PIDCoefficients(0, 0, 0);
+ public static PIDCoefficients HEADING_PID = new PIDCoefficients(0, 0, 0);
+
+ public static double LATERAL_MULTIPLIER = 1;
+
+ public static double VX_WEIGHT = 1;
+ public static double VY_WEIGHT = 1;
+ public static double OMEGA_WEIGHT = 1;
+
+ private TrajectorySequenceRunner trajectorySequenceRunner;
+
+ private static final TrajectoryVelocityConstraint VEL_CONSTRAINT = getVelocityConstraint(MAX_VEL, MAX_ANG_VEL, TRACK_WIDTH);
+ private static final TrajectoryAccelerationConstraint ACCEL_CONSTRAINT = getAccelerationConstraint(MAX_ACCEL);
+
+ private TrajectoryFollower follower;
+
+ private DcMotorEx leftFront, leftRear, rightRear, rightFront;
+ private List motors;
+
+ private IMU imu;
+ private VoltageSensor batteryVoltageSensor;
+
+ private List lastEncPositions = new ArrayList<>();
+ private List lastEncVels = new ArrayList<>();
+
+ public SampleMecanumDrive(HardwareMap hardwareMap) {
+ super(kV, kA, kStatic, TRACK_WIDTH, TRACK_WIDTH, LATERAL_MULTIPLIER);
+
+ follower = new HolonomicPIDVAFollower(TRANSLATIONAL_PID, TRANSLATIONAL_PID, HEADING_PID,
+ new Pose2d(0.5, 0.5, Math.toRadians(5.0)), 0.5);
+
+ LynxModuleUtil.ensureMinimumFirmwareVersion(hardwareMap);
+
+ batteryVoltageSensor = hardwareMap.voltageSensor.iterator().next();
+
+ for (LynxModule module : hardwareMap.getAll(LynxModule.class)) {
+ module.setBulkCachingMode(LynxModule.BulkCachingMode.AUTO);
+ }
+
+ // TODO: adjust the names of the following hardware devices to match your configuration
+// imu = hardwareMap.get(IMU.class, "imu");
+// IMU.Parameters parameters = new IMU.Parameters(new RevHubOrientationOnRobot(
+// DriveConstants.LOGO_FACING_DIR, DriveConstants.USB_FACING_DIR));
+// imu.initialize(parameters);
+
+ leftFront = hardwareMap.get(DcMotorEx.class, "frontLeft");
+ leftRear = hardwareMap.get(DcMotorEx.class, "backLeft");
+ rightRear = hardwareMap.get(DcMotorEx.class, "backRight");
+ rightFront = hardwareMap.get(DcMotorEx.class, "frontRight");
+
+ motors = Arrays.asList(leftFront, leftRear, rightRear, rightFront);
+
+ for (DcMotorEx motor : motors) {
+ MotorConfigurationType motorConfigurationType = motor.getMotorType().clone();
+ motorConfigurationType.setAchieveableMaxRPMFraction(1.0);
+ motor.setMotorType(motorConfigurationType);
+ }
+
+ if (RUN_USING_ENCODER) {
+ setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+ }
+
+ setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+
+ if (RUN_USING_ENCODER && MOTOR_VELO_PID != null) {
+ setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, MOTOR_VELO_PID);
+ }
+
+ // TODO: reverse any motors using DcMotor.setDirection()
+ leftFront.setDirection(DcMotorSimple.Direction.REVERSE);
+ leftRear.setDirection(DcMotorSimple.Direction.REVERSE);
+
+ List lastTrackingEncPositions = new ArrayList<>();
+ List lastTrackingEncVels = new ArrayList<>();
+
+ // TODO: if desired, use setLocalizer() to change the localization method
+ setLocalizer(new StandardTrackingWheelLocalizer(hardwareMap, lastTrackingEncPositions, lastTrackingEncVels));
+
+ trajectorySequenceRunner = new TrajectorySequenceRunner(
+ follower, HEADING_PID, batteryVoltageSensor,
+ lastEncPositions, lastEncVels, lastTrackingEncPositions, lastTrackingEncVels
+ );
+ }
+
+ public TrajectoryBuilder trajectoryBuilder(Pose2d startPose) {
+ return new TrajectoryBuilder(startPose, VEL_CONSTRAINT, ACCEL_CONSTRAINT);
+ }
+
+ public TrajectoryBuilder trajectoryBuilder(Pose2d startPose, boolean reversed) {
+ return new TrajectoryBuilder(startPose, reversed, VEL_CONSTRAINT, ACCEL_CONSTRAINT);
+ }
+
+ public TrajectoryBuilder trajectoryBuilder(Pose2d startPose, double startHeading) {
+ return new TrajectoryBuilder(startPose, startHeading, VEL_CONSTRAINT, ACCEL_CONSTRAINT);
+ }
+
+ public TrajectorySequenceBuilder trajectorySequenceBuilder(Pose2d startPose) {
+ return new TrajectorySequenceBuilder(
+ startPose,
+ VEL_CONSTRAINT, ACCEL_CONSTRAINT,
+ MAX_ANG_VEL, MAX_ANG_ACCEL
+ );
+ }
+
+ public void turnAsync(double angle) {
+ trajectorySequenceRunner.followTrajectorySequenceAsync(
+ trajectorySequenceBuilder(getPoseEstimate())
+ .turn(angle)
+ .build()
+ );
+ }
+
+ public void turn(double angle) {
+ turnAsync(angle);
+ waitForIdle();
+ }
+
+ public void followTrajectoryAsync(Trajectory trajectory) {
+ trajectorySequenceRunner.followTrajectorySequenceAsync(
+ trajectorySequenceBuilder(trajectory.start())
+ .addTrajectory(trajectory)
+ .build()
+ );
+ }
+
+ public void followTrajectory(Trajectory trajectory) {
+ followTrajectoryAsync(trajectory);
+ waitForIdle();
+ }
+
+ public void followTrajectorySequenceAsync(TrajectorySequence trajectorySequence) {
+ trajectorySequenceRunner.followTrajectorySequenceAsync(trajectorySequence);
+ }
+
+ public void followTrajectorySequence(TrajectorySequence trajectorySequence) {
+ followTrajectorySequenceAsync(trajectorySequence);
+ waitForIdle();
+ }
+
+ public Pose2d getLastError() {
+ return trajectorySequenceRunner.getLastPoseError();
+ }
+
+ public void update() {
+ updatePoseEstimate();
+ DriveSignal signal = trajectorySequenceRunner.update(getPoseEstimate(), getPoseVelocity());
+ if (signal != null) setDriveSignal(signal);
+ }
+
+ public void waitForIdle() {
+ while (!Thread.currentThread().isInterrupted() && isBusy())
+ update();
+ }
+
+ public boolean isBusy() {
+ return trajectorySequenceRunner.isBusy();
+ }
+
+ public void setMode(DcMotor.RunMode runMode) {
+ for (DcMotorEx motor : motors) {
+ motor.setMode(runMode);
+ }
+ }
+
+ public void setZeroPowerBehavior(DcMotor.ZeroPowerBehavior zeroPowerBehavior) {
+ for (DcMotorEx motor : motors) {
+ motor.setZeroPowerBehavior(zeroPowerBehavior);
+ }
+ }
+
+ public void setPIDFCoefficients(DcMotor.RunMode runMode, PIDFCoefficients coefficients) {
+ PIDFCoefficients compensatedCoefficients = new PIDFCoefficients(
+ coefficients.p, coefficients.i, coefficients.d,
+ coefficients.f * 12 / batteryVoltageSensor.getVoltage()
+ );
+
+ for (DcMotorEx motor : motors) {
+ motor.setPIDFCoefficients(runMode, compensatedCoefficients);
+ }
+ }
+
+ public void setWeightedDrivePower(Pose2d drivePower) {
+ Pose2d vel = drivePower;
+
+ if (Math.abs(drivePower.getX()) + Math.abs(drivePower.getY())
+ + Math.abs(drivePower.getHeading()) > 1) {
+ // re-normalize the powers according to the weights
+ double denom = VX_WEIGHT * Math.abs(drivePower.getX())
+ + VY_WEIGHT * Math.abs(drivePower.getY())
+ + OMEGA_WEIGHT * Math.abs(drivePower.getHeading());
+
+ vel = new Pose2d(
+ VX_WEIGHT * drivePower.getX(),
+ VY_WEIGHT * drivePower.getY(),
+ OMEGA_WEIGHT * drivePower.getHeading()
+ ).div(denom);
+ }
+
+ setDrivePower(vel);
+ }
+
+ @NonNull
+ @Override
+ public List getWheelPositions() {
+ lastEncPositions.clear();
+
+ List wheelPositions = new ArrayList<>();
+ for (DcMotorEx motor : motors) {
+ int position = motor.getCurrentPosition();
+ lastEncPositions.add(position);
+ wheelPositions.add(encoderTicksToInches(position));
+ }
+ return wheelPositions;
+ }
+
+ @Override
+ public List getWheelVelocities() {
+ lastEncVels.clear();
+
+ List wheelVelocities = new ArrayList<>();
+ for (DcMotorEx motor : motors) {
+ int vel = (int) motor.getVelocity();
+ lastEncVels.add(vel);
+ wheelVelocities.add(encoderTicksToInches(vel));
+ }
+ return wheelVelocities;
+ }
+
+ @Override
+ public void setMotorPowers(double v, double v1, double v2, double v3) {
+ leftFront.setPower(v);
+ leftRear.setPower(v1);
+ rightRear.setPower(v2);
+ rightFront.setPower(v3);
+ }
+
+ @Override
+ public double getRawExternalHeading() {
+ return imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS);
+ }
+
+ @Override
+ public Double getExternalHeadingVelocity() {
+ return (double) imu.getRobotAngularVelocity(AngleUnit.RADIANS).zRotationRate;
+ }
+
+ public static TrajectoryVelocityConstraint getVelocityConstraint(double maxVel, double maxAngularVel, double trackWidth) {
+ return new MinVelocityConstraint(Arrays.asList(
+ new AngularVelocityConstraint(maxAngularVel),
+ new MecanumVelocityConstraint(maxVel, trackWidth)
+ ));
+ }
+
+ public static TrajectoryAccelerationConstraint getAccelerationConstraint(double maxAccel) {
+ return new ProfileAccelerationConstraint(maxAccel);
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleMecanumDriveRIGGED.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleMecanumDriveRIGGED.java
new file mode 100644
index 0000000..121b4a0
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleMecanumDriveRIGGED.java
@@ -0,0 +1,310 @@
+package org.firstinspires.ftc.teamcode.drive;
+
+import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_ACCEL;
+import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_ANG_ACCEL;
+import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_ANG_VEL;
+import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_VEL;
+import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MOTOR_VELO_PID;
+import static org.firstinspires.ftc.teamcode.drive.DriveConstants.RUN_USING_ENCODER;
+import static org.firstinspires.ftc.teamcode.drive.DriveConstants.TRACK_WIDTH;
+import static org.firstinspires.ftc.teamcode.drive.DriveConstants.encoderTicksToInches;
+import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kA;
+import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kStatic;
+import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kV;
+
+import androidx.annotation.NonNull;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.roadrunner.control.PIDCoefficients;
+import com.acmerobotics.roadrunner.drive.DriveSignal;
+import com.acmerobotics.roadrunner.drive.MecanumDrive;
+import com.acmerobotics.roadrunner.followers.HolonomicPIDVAFollower;
+import com.acmerobotics.roadrunner.followers.TrajectoryFollower;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.trajectory.Trajectory;
+import com.acmerobotics.roadrunner.trajectory.TrajectoryBuilder;
+import com.acmerobotics.roadrunner.trajectory.constraints.AngularVelocityConstraint;
+import com.acmerobotics.roadrunner.trajectory.constraints.MecanumVelocityConstraint;
+import com.acmerobotics.roadrunner.trajectory.constraints.MinVelocityConstraint;
+import com.acmerobotics.roadrunner.trajectory.constraints.ProfileAccelerationConstraint;
+import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryAccelerationConstraint;
+import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryVelocityConstraint;
+import com.qualcomm.hardware.lynx.LynxModule;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+import com.qualcomm.robotcore.hardware.IMU;
+import com.qualcomm.robotcore.hardware.PIDFCoefficients;
+import com.qualcomm.robotcore.hardware.VoltageSensor;
+import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType;
+
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequence;
+import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequenceBuilder;
+import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequenceRunner;
+import org.firstinspires.ftc.teamcode.util.LynxModuleUtil;
+
+import java.util.ArrayList;
+import java.util.Arrays;
+import java.util.List;
+
+/*
+ * Simple mecanum drive hardware implementation for REV hardware.
+ */
+@Config
+public class SampleMecanumDriveRIGGED extends MecanumDrive {
+ public static PIDCoefficients TRANSLATIONAL_PID = new PIDCoefficients(0, 0, 0);
+ public static PIDCoefficients HEADING_PID = new PIDCoefficients(0, 0, 0);
+
+ public static double LATERAL_MULTIPLIER = 1;
+
+ public static double VX_WEIGHT = 1;
+ public static double VY_WEIGHT = 1;
+ public static double OMEGA_WEIGHT = 1;
+
+ private TrajectorySequenceRunner trajectorySequenceRunner;
+
+ private static final TrajectoryVelocityConstraint VEL_CONSTRAINT = getVelocityConstraint(MAX_VEL, MAX_ANG_VEL, TRACK_WIDTH);
+ private static final TrajectoryAccelerationConstraint ACCEL_CONSTRAINT = getAccelerationConstraint(MAX_ACCEL);
+
+ private TrajectoryFollower follower;
+
+ private DcMotorEx leftFront, leftRear, rightRear, rightFront;
+ private List motors;
+
+ private IMU imu;
+ private VoltageSensor batteryVoltageSensor;
+
+ private List lastEncPositions = new ArrayList<>();
+ private List lastEncVels = new ArrayList<>();
+
+ public SampleMecanumDriveRIGGED(HardwareMap hardwareMap) {
+ super(kV, kA, kStatic, TRACK_WIDTH, TRACK_WIDTH, LATERAL_MULTIPLIER);
+
+ follower = new HolonomicPIDVAFollower(TRANSLATIONAL_PID, TRANSLATIONAL_PID, HEADING_PID,
+ new Pose2d(0.5, 0.5, Math.toRadians(5.0)), 0.5);
+
+ LynxModuleUtil.ensureMinimumFirmwareVersion(hardwareMap);
+
+ batteryVoltageSensor = hardwareMap.voltageSensor.iterator().next();
+
+ for (LynxModule module : hardwareMap.getAll(LynxModule.class)) {
+ module.setBulkCachingMode(LynxModule.BulkCachingMode.AUTO);
+ }
+
+ // TODO: adjust the names of the following hardware devices to match your configuration
+// imu = hardwareMap.get(IMU.class, "imu");
+// IMU.Parameters parameters = new IMU.Parameters(new RevHubOrientationOnRobot(
+// DriveConstants.LOGO_FACING_DIR, DriveConstants.USB_FACING_DIR));
+// imu.initialize(parameters);
+
+ leftFront = hardwareMap.get(DcMotorEx.class, "frontLeft");
+ leftRear = hardwareMap.get(DcMotorEx.class, "backLeft");
+ rightRear = hardwareMap.get(DcMotorEx.class, "backRight");
+ rightFront = hardwareMap.get(DcMotorEx.class, "frontRight");
+
+ motors = Arrays.asList(leftFront, leftRear, rightRear, rightFront);
+
+ for (DcMotorEx motor : motors) {
+ MotorConfigurationType motorConfigurationType = motor.getMotorType().clone();
+ motorConfigurationType.setAchieveableMaxRPMFraction(1.0);
+ motor.setMotorType(motorConfigurationType);
+ }
+
+ if (RUN_USING_ENCODER) {
+ setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+ }
+
+ setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+
+ if (RUN_USING_ENCODER && MOTOR_VELO_PID != null) {
+ setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, MOTOR_VELO_PID);
+ }
+
+ // TODO: reverse any motors using DcMotor.setDirection()
+ leftFront.setDirection(DcMotorSimple.Direction.REVERSE);
+ leftRear.setDirection(DcMotorSimple.Direction.REVERSE);
+
+ List lastTrackingEncPositions = new ArrayList<>();
+ List lastTrackingEncVels = new ArrayList<>();
+
+ // TODO: if desired, use setLocalizer() to change the localization method
+ setLocalizer(new StandardTrackingWheelLocalizer(hardwareMap, lastTrackingEncPositions, lastTrackingEncVels));
+
+ trajectorySequenceRunner = new TrajectorySequenceRunner(
+ follower, HEADING_PID, batteryVoltageSensor,
+ lastEncPositions, lastEncVels, lastTrackingEncPositions, lastTrackingEncVels
+ );
+ }
+
+ public TrajectoryBuilder trajectoryBuilder(Pose2d startPose) {
+ return new TrajectoryBuilder(startPose, VEL_CONSTRAINT, ACCEL_CONSTRAINT);
+ }
+
+ public TrajectoryBuilder trajectoryBuilder(Pose2d startPose, boolean reversed) {
+ return new TrajectoryBuilder(startPose, reversed, VEL_CONSTRAINT, ACCEL_CONSTRAINT);
+ }
+
+ public TrajectoryBuilder trajectoryBuilder(Pose2d startPose, double startHeading) {
+ return new TrajectoryBuilder(startPose, startHeading, VEL_CONSTRAINT, ACCEL_CONSTRAINT);
+ }
+
+ public TrajectorySequenceBuilder trajectorySequenceBuilder(Pose2d startPose) {
+ return new TrajectorySequenceBuilder(
+ startPose,
+ VEL_CONSTRAINT, ACCEL_CONSTRAINT,
+ MAX_ANG_VEL, MAX_ANG_ACCEL
+ );
+ }
+
+ public void turnAsync(double angle) {
+ trajectorySequenceRunner.followTrajectorySequenceAsync(
+ trajectorySequenceBuilder(getPoseEstimate())
+ .turn(angle)
+ .build()
+ );
+ }
+
+ public void turn(double angle) {
+ turnAsync(angle);
+ waitForIdle();
+ }
+
+ public void followTrajectoryAsync(Trajectory trajectory) {
+ trajectorySequenceRunner.followTrajectorySequenceAsync(
+ trajectorySequenceBuilder(trajectory.start())
+ .addTrajectory(trajectory)
+ .build()
+ );
+ }
+
+ public void followTrajectory(Trajectory trajectory) {
+ followTrajectoryAsync(trajectory);
+ waitForIdle();
+ }
+
+ public void followTrajectorySequenceAsync(TrajectorySequence trajectorySequence) {
+ trajectorySequenceRunner.followTrajectorySequenceAsync(trajectorySequence);
+ }
+
+ public void followTrajectorySequence(TrajectorySequence trajectorySequence) {
+ followTrajectorySequenceAsync(trajectorySequence);
+ waitForIdle();
+ }
+
+ public Pose2d getLastError() {
+ return trajectorySequenceRunner.getLastPoseError();
+ }
+
+ public void update() {
+ updatePoseEstimate();
+ DriveSignal signal = trajectorySequenceRunner.update(getPoseEstimate(), getPoseVelocity());
+ if (signal != null) setDriveSignal(signal);
+ }
+
+ public void waitForIdle() {
+ while (!Thread.currentThread().isInterrupted() && isBusy())
+ update();
+ }
+
+ public boolean isBusy() {
+ return trajectorySequenceRunner.isBusy();
+ }
+
+ public void setMode(DcMotor.RunMode runMode) {
+ for (DcMotorEx motor : motors) {
+ motor.setMode(runMode);
+ }
+ }
+
+ public void setZeroPowerBehavior(DcMotor.ZeroPowerBehavior zeroPowerBehavior) {
+ for (DcMotorEx motor : motors) {
+ motor.setZeroPowerBehavior(zeroPowerBehavior);
+ }
+ }
+
+ public void setPIDFCoefficients(DcMotor.RunMode runMode, PIDFCoefficients coefficients) {
+ PIDFCoefficients compensatedCoefficients = new PIDFCoefficients(
+ coefficients.p, coefficients.i, coefficients.d,
+ coefficients.f * 12 / batteryVoltageSensor.getVoltage()
+ );
+
+ for (DcMotorEx motor : motors) {
+ motor.setPIDFCoefficients(runMode, compensatedCoefficients);
+ }
+ }
+
+ public void setWeightedDrivePower(Pose2d drivePower) {
+ Pose2d vel = drivePower;
+
+ if (Math.abs(drivePower.getX()) + Math.abs(drivePower.getY())
+ + Math.abs(drivePower.getHeading()) > 1) {
+ // re-normalize the powers according to the weights
+ double denom = VX_WEIGHT * Math.abs(drivePower.getX())
+ + VY_WEIGHT * Math.abs(drivePower.getY())
+ + OMEGA_WEIGHT * Math.abs(drivePower.getHeading());
+
+ vel = new Pose2d(
+ VX_WEIGHT * drivePower.getX(),
+ VY_WEIGHT * drivePower.getY(),
+ OMEGA_WEIGHT * drivePower.getHeading()
+ ).div(denom);
+ }
+
+ setDrivePower(vel);
+ }
+
+ @NonNull
+ @Override
+ public List getWheelPositions() {
+ lastEncPositions.clear();
+
+ List wheelPositions = new ArrayList<>();
+ for (DcMotorEx motor : motors) {
+ int position = motor.getCurrentPosition();
+ lastEncPositions.add(position);
+ wheelPositions.add(encoderTicksToInches(position));
+ }
+ return wheelPositions;
+ }
+
+ @Override
+ public List getWheelVelocities() {
+ lastEncVels.clear();
+
+ List wheelVelocities = new ArrayList<>();
+ for (DcMotorEx motor : motors) {
+ int vel = (int) motor.getVelocity();
+ lastEncVels.add(vel);
+ wheelVelocities.add(encoderTicksToInches(vel));
+ }
+ return wheelVelocities;
+ }
+
+ @Override
+ public void setMotorPowers(double v, double v1, double v2, double v3) {
+
+ }
+
+ @Override
+ public double getRawExternalHeading() {
+ return imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS);
+ }
+
+ @Override
+ public Double getExternalHeadingVelocity() {
+ return (double) imu.getRobotAngularVelocity(AngleUnit.RADIANS).zRotationRate;
+ }
+
+ public static TrajectoryVelocityConstraint getVelocityConstraint(double maxVel, double maxAngularVel, double trackWidth) {
+ return new MinVelocityConstraint(Arrays.asList(
+ new AngularVelocityConstraint(maxAngularVel),
+ new MecanumVelocityConstraint(maxVel, trackWidth)
+ ));
+ }
+
+ public static TrajectoryAccelerationConstraint getAccelerationConstraint(double maxAccel) {
+ return new ProfileAccelerationConstraint(maxAccel);
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleTankDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleTankDrive.java
new file mode 100644
index 0000000..55aeb3a
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/SampleTankDrive.java
@@ -0,0 +1,305 @@
+package org.firstinspires.ftc.teamcode.drive;
+
+import androidx.annotation.NonNull;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.roadrunner.control.PIDCoefficients;
+import com.acmerobotics.roadrunner.drive.DriveSignal;
+import com.acmerobotics.roadrunner.drive.TankDrive;
+import com.acmerobotics.roadrunner.followers.TankPIDVAFollower;
+import com.acmerobotics.roadrunner.followers.TrajectoryFollower;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.trajectory.Trajectory;
+import com.acmerobotics.roadrunner.trajectory.TrajectoryBuilder;
+import com.acmerobotics.roadrunner.trajectory.constraints.AngularVelocityConstraint;
+import com.acmerobotics.roadrunner.trajectory.constraints.MinVelocityConstraint;
+import com.acmerobotics.roadrunner.trajectory.constraints.ProfileAccelerationConstraint;
+import com.acmerobotics.roadrunner.trajectory.constraints.TankVelocityConstraint;
+import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryAccelerationConstraint;
+import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryVelocityConstraint;
+import com.qualcomm.hardware.lynx.LynxModule;
+import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+import com.qualcomm.robotcore.hardware.IMU;
+import com.qualcomm.robotcore.hardware.PIDFCoefficients;
+import com.qualcomm.robotcore.hardware.VoltageSensor;
+import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType;
+
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequence;
+import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequenceBuilder;
+import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequenceRunner;
+import org.firstinspires.ftc.teamcode.util.LynxModuleUtil;
+
+import java.util.ArrayList;
+import java.util.Arrays;
+import java.util.List;
+
+import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_ACCEL;
+import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_ANG_ACCEL;
+import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_ANG_VEL;
+import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_VEL;
+import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MOTOR_VELO_PID;
+import static org.firstinspires.ftc.teamcode.drive.DriveConstants.RUN_USING_ENCODER;
+import static org.firstinspires.ftc.teamcode.drive.DriveConstants.TRACK_WIDTH;
+import static org.firstinspires.ftc.teamcode.drive.DriveConstants.encoderTicksToInches;
+import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kA;
+import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kStatic;
+import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kV;
+
+/*
+ * Simple tank drive hardware implementation for REV hardware.
+ */
+@Config
+public class SampleTankDrive extends TankDrive {
+ public static PIDCoefficients AXIAL_PID = new PIDCoefficients(0, 0, 0);
+ public static PIDCoefficients CROSS_TRACK_PID = new PIDCoefficients(0, 0, 0);
+ public static PIDCoefficients HEADING_PID = new PIDCoefficients(0, 0, 0);
+
+ public static double VX_WEIGHT = 1;
+ public static double OMEGA_WEIGHT = 1;
+
+ private TrajectorySequenceRunner trajectorySequenceRunner;
+
+ private static final TrajectoryVelocityConstraint VEL_CONSTRAINT = getVelocityConstraint(MAX_VEL, MAX_ANG_VEL, TRACK_WIDTH);
+ private static final TrajectoryAccelerationConstraint accelConstraint = getAccelerationConstraint(MAX_ACCEL);
+
+ private TrajectoryFollower follower;
+
+ private List motors, leftMotors, rightMotors;
+ private IMU imu;
+
+ private VoltageSensor batteryVoltageSensor;
+
+ public SampleTankDrive(HardwareMap hardwareMap) {
+ super(kV, kA, kStatic, TRACK_WIDTH);
+
+ follower = new TankPIDVAFollower(AXIAL_PID, CROSS_TRACK_PID,
+ new Pose2d(0.5, 0.5, Math.toRadians(5.0)), 0.5);
+
+ LynxModuleUtil.ensureMinimumFirmwareVersion(hardwareMap);
+
+ batteryVoltageSensor = hardwareMap.voltageSensor.iterator().next();
+
+ for (LynxModule module : hardwareMap.getAll(LynxModule.class)) {
+ module.setBulkCachingMode(LynxModule.BulkCachingMode.AUTO);
+ }
+
+ // TODO: adjust the names of the following hardware devices to match your configuration
+ imu = hardwareMap.get(IMU.class, "imu");
+ IMU.Parameters parameters = new IMU.Parameters(new RevHubOrientationOnRobot(
+ DriveConstants.LOGO_FACING_DIR, DriveConstants.USB_FACING_DIR));
+ imu.initialize(parameters);
+
+ // add/remove motors depending on your robot (e.g., 6WD)
+ DcMotorEx leftFront = hardwareMap.get(DcMotorEx.class, "leftFront");
+ DcMotorEx leftRear = hardwareMap.get(DcMotorEx.class, "leftRear");
+ DcMotorEx rightRear = hardwareMap.get(DcMotorEx.class, "rightRear");
+ DcMotorEx rightFront = hardwareMap.get(DcMotorEx.class, "rightFront");
+
+ motors = Arrays.asList(leftFront, leftRear, rightRear, rightFront);
+ leftMotors = Arrays.asList(leftFront, leftRear);
+ rightMotors = Arrays.asList(rightFront, rightRear);
+
+ for (DcMotorEx motor : motors) {
+ MotorConfigurationType motorConfigurationType = motor.getMotorType().clone();
+ motorConfigurationType.setAchieveableMaxRPMFraction(1.0);
+ motor.setMotorType(motorConfigurationType);
+ }
+
+ if (RUN_USING_ENCODER) {
+ setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+ }
+
+ setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+
+ if (RUN_USING_ENCODER && MOTOR_VELO_PID != null) {
+ setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, MOTOR_VELO_PID);
+ }
+
+ // TODO: reverse any motors using DcMotor.setDirection()
+
+ // TODO: if desired, use setLocalizer() to change the localization method
+ // for instance, setLocalizer(new ThreeTrackingWheelLocalizer(...));
+
+ trajectorySequenceRunner = new TrajectorySequenceRunner(
+ follower, HEADING_PID, batteryVoltageSensor,
+ new ArrayList<>(), new ArrayList<>(), new ArrayList<>(), new ArrayList<>()
+ );
+ }
+
+ public TrajectoryBuilder trajectoryBuilder(Pose2d startPose) {
+ return new TrajectoryBuilder(startPose, VEL_CONSTRAINT, accelConstraint);
+ }
+
+ public TrajectoryBuilder trajectoryBuilder(Pose2d startPose, boolean reversed) {
+ return new TrajectoryBuilder(startPose, reversed, VEL_CONSTRAINT, accelConstraint);
+ }
+
+ public TrajectoryBuilder trajectoryBuilder(Pose2d startPose, double startHeading) {
+ return new TrajectoryBuilder(startPose, startHeading, VEL_CONSTRAINT, accelConstraint);
+ }
+
+ public TrajectorySequenceBuilder trajectorySequenceBuilder(Pose2d startPose) {
+ return new TrajectorySequenceBuilder(
+ startPose,
+ VEL_CONSTRAINT, accelConstraint,
+ MAX_ANG_VEL, MAX_ANG_ACCEL
+ );
+ }
+
+ public void turnAsync(double angle) {
+ trajectorySequenceRunner.followTrajectorySequenceAsync(
+ trajectorySequenceBuilder(getPoseEstimate())
+ .turn(angle)
+ .build()
+ );
+ }
+
+ public void turn(double angle) {
+ turnAsync(angle);
+ waitForIdle();
+ }
+
+ public void followTrajectoryAsync(Trajectory trajectory) {
+ trajectorySequenceRunner.followTrajectorySequenceAsync(
+ trajectorySequenceBuilder(trajectory.start())
+ .addTrajectory(trajectory)
+ .build()
+ );
+ }
+
+ public void followTrajectory(Trajectory trajectory) {
+ followTrajectoryAsync(trajectory);
+ waitForIdle();
+ }
+
+ public void followTrajectorySequenceAsync(TrajectorySequence trajectorySequence) {
+ trajectorySequenceRunner.followTrajectorySequenceAsync(trajectorySequence);
+ }
+
+ public void followTrajectorySequence(TrajectorySequence trajectorySequence) {
+ followTrajectorySequenceAsync(trajectorySequence);
+ waitForIdle();
+ }
+
+ public Pose2d getLastError() {
+ return trajectorySequenceRunner.getLastPoseError();
+ }
+
+
+ public void update() {
+ updatePoseEstimate();
+ DriveSignal signal = trajectorySequenceRunner.update(getPoseEstimate(), getPoseVelocity());
+ if (signal != null) setDriveSignal(signal);
+ }
+
+ public void waitForIdle() {
+ while (!Thread.currentThread().isInterrupted() && isBusy())
+ update();
+ }
+
+ public boolean isBusy() {
+ return trajectorySequenceRunner.isBusy();
+ }
+
+ public void setMode(DcMotor.RunMode runMode) {
+ for (DcMotorEx motor : motors) {
+ motor.setMode(runMode);
+ }
+ }
+
+ public void setZeroPowerBehavior(DcMotor.ZeroPowerBehavior zeroPowerBehavior) {
+ for (DcMotorEx motor : motors) {
+ motor.setZeroPowerBehavior(zeroPowerBehavior);
+ }
+ }
+
+ public void setPIDFCoefficients(DcMotor.RunMode runMode, PIDFCoefficients coefficients) {
+ PIDFCoefficients compensatedCoefficients = new PIDFCoefficients(
+ coefficients.p, coefficients.i, coefficients.d,
+ coefficients.f * 12 / batteryVoltageSensor.getVoltage()
+ );
+ for (DcMotorEx motor : motors) {
+ motor.setPIDFCoefficients(runMode, compensatedCoefficients);
+ }
+ }
+
+ public void setWeightedDrivePower(Pose2d drivePower) {
+ Pose2d vel = drivePower;
+
+ if (Math.abs(drivePower.getX()) + Math.abs(drivePower.getHeading()) > 1) {
+ // re-normalize the powers according to the weights
+ double denom = VX_WEIGHT * Math.abs(drivePower.getX())
+ + OMEGA_WEIGHT * Math.abs(drivePower.getHeading());
+
+ vel = new Pose2d(
+ VX_WEIGHT * drivePower.getX(),
+ 0,
+ OMEGA_WEIGHT * drivePower.getHeading()
+ ).div(denom);
+ } else {
+ // Ensure the y axis is zeroed out.
+ vel = new Pose2d(drivePower.getX(), 0, drivePower.getHeading());
+ }
+
+ setDrivePower(vel);
+ }
+
+ @NonNull
+ @Override
+ public List getWheelPositions() {
+ double leftSum = 0, rightSum = 0;
+ for (DcMotorEx leftMotor : leftMotors) {
+ leftSum += encoderTicksToInches(leftMotor.getCurrentPosition());
+ }
+ for (DcMotorEx rightMotor : rightMotors) {
+ rightSum += encoderTicksToInches(rightMotor.getCurrentPosition());
+ }
+ return Arrays.asList(leftSum / leftMotors.size(), rightSum / rightMotors.size());
+ }
+
+ public List getWheelVelocities() {
+ double leftSum = 0, rightSum = 0;
+ for (DcMotorEx leftMotor : leftMotors) {
+ leftSum += encoderTicksToInches(leftMotor.getVelocity());
+ }
+ for (DcMotorEx rightMotor : rightMotors) {
+ rightSum += encoderTicksToInches(rightMotor.getVelocity());
+ }
+ return Arrays.asList(leftSum / leftMotors.size(), rightSum / rightMotors.size());
+ }
+
+ @Override
+ public void setMotorPowers(double v, double v1) {
+ for (DcMotorEx leftMotor : leftMotors) {
+ leftMotor.setPower(v);
+ }
+ for (DcMotorEx rightMotor : rightMotors) {
+ rightMotor.setPower(v1);
+ }
+ }
+
+ @Override
+ public double getRawExternalHeading() {
+ return imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS);
+ }
+
+ @Override
+ public Double getExternalHeadingVelocity() {
+ return (double) imu.getRobotAngularVelocity(AngleUnit.RADIANS).zRotationRate;
+ }
+
+ public static TrajectoryVelocityConstraint getVelocityConstraint(double maxVel, double maxAngularVel, double trackWidth) {
+ return new MinVelocityConstraint(Arrays.asList(
+ new AngularVelocityConstraint(maxAngularVel),
+ new TankVelocityConstraint(maxVel, trackWidth)
+ ));
+ }
+
+ public static TrajectoryAccelerationConstraint getAccelerationConstraint(double maxAccel) {
+ return new ProfileAccelerationConstraint(maxAccel);
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/StandardTrackingWheelLocalizer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/StandardTrackingWheelLocalizer.java
new file mode 100644
index 0000000..261898d
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/StandardTrackingWheelLocalizer.java
@@ -0,0 +1,104 @@
+package org.firstinspires.ftc.teamcode.drive;
+
+import androidx.annotation.NonNull;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.localization.ThreeTrackingWheelLocalizer;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+import org.firstinspires.ftc.teamcode.util.Encoder;
+
+import java.util.Arrays;
+import java.util.List;
+
+/*
+ * Sample tracking wheel localizer implementation assuming the standard configuration:
+ *
+ * /--------------\
+ * | ____ |
+ * | ---- |
+ * | || || |
+ * | || || |
+ * | |
+ * | |
+ * \--------------/
+ *
+ */
+@Config
+public class StandardTrackingWheelLocalizer extends ThreeTrackingWheelLocalizer {
+ public static double TICKS_PER_REV = 2000;
+ public static double WHEEL_RADIUS = 0.944882; // in
+ public static double GEAR_RATIO = 1; // output (wheel) speed / input (encoder) speed
+
+ public static double LATERAL_DISTANCE = 6.2; // in; distance between the left and right wheels
+ public static double FORWARD_OFFSET = -6; // in; offset of the lateral wheel
+
+ private Encoder leftEncoder, rightEncoder, frontEncoder;
+
+ private List lastEncPositions, lastEncVels;
+
+
+ public static double X_MULTIPLIER = 1; // Multiplier in the X direction
+ public static double Y_MULTIPLIER = 1; // Multiplier in the Y direction
+
+ public StandardTrackingWheelLocalizer(HardwareMap hardwareMap, List lastTrackingEncPositions, List lastTrackingEncVels) {
+ super(Arrays.asList(
+ new Pose2d(0, LATERAL_DISTANCE / 2, 0), // left
+ new Pose2d(0, -LATERAL_DISTANCE / 2, 0), // right
+ new Pose2d(FORWARD_OFFSET, 0, Math.toRadians(90)) // front
+ ));
+
+ lastEncPositions = lastTrackingEncPositions;
+ lastEncVels = lastTrackingEncVels;
+
+ leftEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "intake"));
+ rightEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "topFly"));
+ frontEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "bottomFly"));
+
+ // TODO: reverse any encoders using Encoder.setDirection(Encoder.Direction.REVERSE)
+ // leftEncoder.setDirection(Encoder.Direction.REVERSE);
+ }
+
+ public static double encoderTicksToInches(double ticks) {
+ return WHEEL_RADIUS * 2 * Math.PI * GEAR_RATIO * ticks / TICKS_PER_REV;
+ }
+
+ @NonNull
+ @Override
+ public List getWheelPositions() {
+ int leftPos = leftEncoder.getCurrentPosition();
+ int rightPos = rightEncoder.getCurrentPosition();
+ int frontPos = frontEncoder.getCurrentPosition();
+
+ lastEncPositions.clear();
+ lastEncPositions.add(leftPos);
+ lastEncPositions.add(rightPos);
+ lastEncPositions.add(frontPos);
+
+ return Arrays.asList(
+ encoderTicksToInches(leftPos),
+ encoderTicksToInches(rightPos),
+ encoderTicksToInches(frontPos)
+ );
+ }
+
+ @NonNull
+ @Override
+ public List getWheelVelocities() {
+ int leftVel = (int) leftEncoder.getCorrectedVelocity();
+ int rightVel = (int) rightEncoder.getCorrectedVelocity();
+ int frontVel = (int) frontEncoder.getCorrectedVelocity();
+
+ lastEncVels.clear();
+ lastEncVels.add(leftVel);
+ lastEncVels.add(rightVel);
+ lastEncVels.add(frontVel);
+
+ return Arrays.asList(
+ encoderTicksToInches(leftVel) * X_MULTIPLIER,
+ encoderTicksToInches(rightVel) * X_MULTIPLIER,
+ encoderTicksToInches(frontVel) * Y_MULTIPLIER
+ );
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/advanced/PoseStorage.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/advanced/PoseStorage.java
new file mode 100644
index 0000000..ef935f0
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/advanced/PoseStorage.java
@@ -0,0 +1,12 @@
+package org.firstinspires.ftc.teamcode.drive.advanced;
+
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+
+/**
+ * Simple static field serving as a storage medium for the bot's pose.
+ * This allows different classes/opmodes to set and read from a central source of truth.
+ * A static field allows data to persist between opmodes.
+ */
+public class PoseStorage {
+ public static Pose2d currentPose = new Pose2d(0,0,0);
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/AutomaticFeedforwardTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/AutomaticFeedforwardTuner.java
new file mode 100644
index 0000000..b7187d9
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/AutomaticFeedforwardTuner.java
@@ -0,0 +1,221 @@
+package org.firstinspires.ftc.teamcode.drive.opmode;
+
+import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_RPM;
+import static org.firstinspires.ftc.teamcode.drive.DriveConstants.RUN_USING_ENCODER;
+import static org.firstinspires.ftc.teamcode.drive.DriveConstants.rpmToVelocity;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.util.NanoClock;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.util.RobotLog;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.robotcore.internal.system.Misc;
+import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
+import org.firstinspires.ftc.teamcode.util.LoggingUtil;
+import org.firstinspires.ftc.teamcode.util.RegressionUtil;
+
+import java.util.ArrayList;
+import java.util.List;
+
+/*
+ * Op mode for computing kV, kStatic, and kA from various drive routines. For the curious, here's an
+ * outline of the procedure:
+ * 1. Slowly ramp the motor power and record encoder values along the way.
+ * 2. Run a linear regression on the encoder velocity vs. motor power plot to obtain a slope (kV)
+ * and an optional intercept (kStatic).
+ * 3. Accelerate the robot (apply constant power) and record the encoder counts.
+ * 4. Adjust the encoder data based on the velocity tuning data and find kA with another linear
+ * regression.
+ */
+@Config
+@Autonomous(group = "drive")
+public class AutomaticFeedforwardTuner extends LinearOpMode {
+ public static double MAX_POWER = 0.7;
+ public static double DISTANCE = 40; // in
+
+ @Override
+ public void runOpMode() throws InterruptedException {
+ if (RUN_USING_ENCODER) {
+ RobotLog.setGlobalErrorMsg("Feedforward constants usually don't need to be tuned " +
+ "when using the built-in drive motor velocity PID.");
+ }
+
+ Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
+
+ SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
+
+ NanoClock clock = NanoClock.system();
+
+ telemetry.addLine("Press play to begin the feedforward tuning routine");
+ telemetry.update();
+
+ waitForStart();
+
+ if (isStopRequested()) return;
+
+ telemetry.clearAll();
+ telemetry.addLine("Would you like to fit kStatic?");
+ telemetry.addLine("Press (Y/Δ) for yes, (B/O) for no");
+ telemetry.update();
+
+ boolean fitIntercept = false;
+ while (!isStopRequested()) {
+ if (gamepad1.y) {
+ fitIntercept = true;
+ while (!isStopRequested() && gamepad1.y) {
+ idle();
+ }
+ break;
+ } else if (gamepad1.b) {
+ while (!isStopRequested() && gamepad1.b) {
+ idle();
+ }
+ break;
+ }
+ idle();
+ }
+
+ telemetry.clearAll();
+ telemetry.addLine(Misc.formatInvariant(
+ "Place your robot on the field with at least %.2f in of room in front", DISTANCE));
+ telemetry.addLine("Press (Y/Δ) to begin");
+ telemetry.update();
+
+ while (!isStopRequested() && !gamepad1.y) {
+ idle();
+ }
+ while (!isStopRequested() && gamepad1.y) {
+ idle();
+ }
+
+ telemetry.clearAll();
+ telemetry.addLine("Running...");
+ telemetry.update();
+
+ double maxVel = rpmToVelocity(MAX_RPM);
+ double finalVel = MAX_POWER * maxVel;
+ double accel = (finalVel * finalVel) / (2.0 * DISTANCE);
+ double rampTime = Math.sqrt(2.0 * DISTANCE / accel);
+
+ List timeSamples = new ArrayList<>();
+ List positionSamples = new ArrayList<>();
+ List powerSamples = new ArrayList<>();
+
+ drive.setPoseEstimate(new Pose2d());
+
+ double startTime = clock.seconds();
+ while (!isStopRequested()) {
+ double elapsedTime = clock.seconds() - startTime;
+ if (elapsedTime > rampTime) {
+ break;
+ }
+ double vel = accel * elapsedTime;
+ double power = vel / maxVel;
+
+ timeSamples.add(elapsedTime);
+ positionSamples.add(drive.getPoseEstimate().getX());
+ powerSamples.add(power);
+
+ drive.setDrivePower(new Pose2d(power, 0.0, 0.0));
+ drive.updatePoseEstimate();
+ }
+ drive.setDrivePower(new Pose2d(0.0, 0.0, 0.0));
+
+ RegressionUtil.RampResult rampResult = RegressionUtil.fitRampData(
+ timeSamples, positionSamples, powerSamples, fitIntercept,
+ LoggingUtil.getLogFile(Misc.formatInvariant(
+ "DriveRampRegression-%d.csv", System.currentTimeMillis())));
+
+ telemetry.clearAll();
+ telemetry.addLine("Quasi-static ramp up test complete");
+ if (fitIntercept) {
+ telemetry.addLine(Misc.formatInvariant("kV = %.5f, kStatic = %.5f (R^2 = %.2f)",
+ rampResult.kV, rampResult.kStatic, rampResult.rSquare));
+ } else {
+ telemetry.addLine(Misc.formatInvariant("kV = %.5f (R^2 = %.2f)",
+ rampResult.kStatic, rampResult.rSquare));
+ }
+ telemetry.addLine("Would you like to fit kA?");
+ telemetry.addLine("Press (Y/Δ) for yes, (B/O) for no");
+ telemetry.update();
+
+ boolean fitAccelFF = false;
+ while (!isStopRequested()) {
+ if (gamepad1.y) {
+ fitAccelFF = true;
+ while (!isStopRequested() && gamepad1.y) {
+ idle();
+ }
+ break;
+ } else if (gamepad1.b) {
+ while (!isStopRequested() && gamepad1.b) {
+ idle();
+ }
+ break;
+ }
+ idle();
+ }
+
+ if (fitAccelFF) {
+ telemetry.clearAll();
+ telemetry.addLine("Place the robot back in its starting position");
+ telemetry.addLine("Press (Y/Δ) to continue");
+ telemetry.update();
+
+ while (!isStopRequested() && !gamepad1.y) {
+ idle();
+ }
+ while (!isStopRequested() && gamepad1.y) {
+ idle();
+ }
+
+ telemetry.clearAll();
+ telemetry.addLine("Running...");
+ telemetry.update();
+
+ double maxPowerTime = DISTANCE / maxVel;
+
+ timeSamples.clear();
+ positionSamples.clear();
+ powerSamples.clear();
+
+ drive.setPoseEstimate(new Pose2d());
+ drive.setDrivePower(new Pose2d(MAX_POWER, 0.0, 0.0));
+
+ startTime = clock.seconds();
+ while (!isStopRequested()) {
+ double elapsedTime = clock.seconds() - startTime;
+ if (elapsedTime > maxPowerTime) {
+ break;
+ }
+
+ timeSamples.add(elapsedTime);
+ positionSamples.add(drive.getPoseEstimate().getX());
+ powerSamples.add(MAX_POWER);
+
+ drive.updatePoseEstimate();
+ }
+ drive.setDrivePower(new Pose2d(0.0, 0.0, 0.0));
+
+ RegressionUtil.AccelResult accelResult = RegressionUtil.fitAccelData(
+ timeSamples, positionSamples, powerSamples, rampResult,
+ LoggingUtil.getLogFile(Misc.formatInvariant(
+ "DriveAccelRegression-%d.csv", System.currentTimeMillis())));
+
+ telemetry.clearAll();
+ telemetry.addLine("Constant power test complete");
+ telemetry.addLine(Misc.formatInvariant("kA = %.5f (R^2 = %.2f)",
+ accelResult.kA, accelResult.rSquare));
+ telemetry.update();
+ }
+
+ while (!isStopRequested()) {
+ idle();
+ }
+ }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/BackAndForth.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/BackAndForth.java
new file mode 100644
index 0000000..a78ad37
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/BackAndForth.java
@@ -0,0 +1,52 @@
+package org.firstinspires.ftc.teamcode.drive.opmode;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.trajectory.Trajectory;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+
+import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
+
+/*
+ * Op mode for preliminary tuning of the follower PID coefficients (located in the drive base
+ * classes). The robot drives back and forth in a straight line indefinitely. Utilization of the
+ * dashboard is recommended for this tuning routine. To access the dashboard, connect your computer
+ * to the RC's WiFi network. In your browser, navigate to https://192.168.49.1:8080/dash if you're
+ * using the RC phone or https://192.168.43.1:8080/dash if you are using the Control Hub. Once
+ * you've successfully connected, start the program, and your robot will begin moving forward and
+ * backward. You should observe the target position (green) and your pose estimate (blue) and adjust
+ * your follower PID coefficients such that you follow the target position as accurately as possible.
+ * If you are using SampleMecanumDrive, you should be tuning TRANSLATIONAL_PID and HEADING_PID.
+ * If you are using SampleTankDrive, you should be tuning AXIAL_PID, CROSS_TRACK_PID, and HEADING_PID.
+ * These coefficients can be tuned live in dashboard.
+ *
+ * This opmode is designed as a convenient, coarse tuning for the follower PID coefficients. It
+ * is recommended that you use the FollowerPIDTuner opmode for further fine tuning.
+ */
+@Config
+@Autonomous(group = "drive")
+public class BackAndForth extends LinearOpMode {
+
+ public static double DISTANCE = 50;
+
+ @Override
+ public void runOpMode() throws InterruptedException {
+ SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
+
+ Trajectory trajectoryForward = drive.trajectoryBuilder(new Pose2d())
+ .forward(DISTANCE)
+ .build();
+
+ Trajectory trajectoryBackward = drive.trajectoryBuilder(trajectoryForward.end())
+ .back(DISTANCE)
+ .build();
+
+ waitForStart();
+
+ while (opModeIsActive() && !isStopRequested()) {
+ drive.followTrajectory(trajectoryForward);
+ drive.followTrajectory(trajectoryBackward);
+ }
+ }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/DriveVelocityPIDTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/DriveVelocityPIDTuner.java
new file mode 100644
index 0000000..931b742
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/DriveVelocityPIDTuner.java
@@ -0,0 +1,171 @@
+package org.firstinspires.ftc.teamcode.drive.opmode;
+
+import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_ACCEL;
+import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_VEL;
+import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MOTOR_VELO_PID;
+import static org.firstinspires.ftc.teamcode.drive.DriveConstants.RUN_USING_ENCODER;
+import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kV;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.profile.MotionProfile;
+import com.acmerobotics.roadrunner.profile.MotionProfileGenerator;
+import com.acmerobotics.roadrunner.profile.MotionState;
+import com.acmerobotics.roadrunner.util.NanoClock;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.util.RobotLog;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
+
+import java.util.List;
+
+/*
+ * This routine is designed to tune the PID coefficients used by the REV Expansion Hubs for closed-
+ * loop velocity control. Although it may seem unnecessary, tuning these coefficients is just as
+ * important as the positional parameters. Like the other manual tuning routines, this op mode
+ * relies heavily upon the dashboard. To access the dashboard, connect your computer to the RC's
+ * WiFi network. In your browser, navigate to https://192.168.49.1:8080/dash if you're using the RC
+ * phone or https://192.168.43.1:8080/dash if you are using the Control Hub. Once you've successfully
+ * connected, start the program, and your robot will begin moving forward and backward according to
+ * a motion profile. Your job is to graph the velocity errors over time and adjust the PID
+ * coefficients (note: the tuning variable will not appear until the op mode finishes initializing).
+ * Once you've found a satisfactory set of gains, add them to the DriveConstants.java file under the
+ * MOTOR_VELO_PID field.
+ *
+ * Recommended tuning process:
+ *
+ * 1. Increase kP until any phase lag is eliminated. Concurrently increase kD as necessary to
+ * mitigate oscillations.
+ * 2. Add kI (or adjust kF) until the steady state/constant velocity plateaus are reached.
+ * 3. Back off kP and kD a little until the response is less oscillatory (but without lag).
+ *
+ * Pressing Y/Δ (Xbox/PS4) will pause the tuning process and enter driver override, allowing the
+ * user to reset the position of the bot in the event that it drifts off the path.
+ * Pressing B/O (Xbox/PS4) will cede control back to the tuning process.
+ */
+@Config
+@Autonomous(group = "drive")
+public class DriveVelocityPIDTuner extends LinearOpMode {
+ public static double DISTANCE = 72; // in
+
+ enum Mode {
+ DRIVER_MODE,
+ TUNING_MODE
+ }
+
+ private static MotionProfile generateProfile(boolean movingForward) {
+ MotionState start = new MotionState(movingForward ? 0 : DISTANCE, 0, 0, 0);
+ MotionState goal = new MotionState(movingForward ? DISTANCE : 0, 0, 0, 0);
+ return MotionProfileGenerator.generateSimpleMotionProfile(start, goal, MAX_VEL, MAX_ACCEL);
+ }
+
+ @Override
+ public void runOpMode() {
+ if (!RUN_USING_ENCODER) {
+ RobotLog.setGlobalErrorMsg("%s does not need to be run if the built-in motor velocity" +
+ "PID is not in use", getClass().getSimpleName());
+ }
+
+ Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
+
+ SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
+
+ Mode mode = Mode.TUNING_MODE;
+
+ double lastKp = MOTOR_VELO_PID.p;
+ double lastKi = MOTOR_VELO_PID.i;
+ double lastKd = MOTOR_VELO_PID.d;
+ double lastKf = MOTOR_VELO_PID.f;
+
+ drive.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, MOTOR_VELO_PID);
+
+ NanoClock clock = NanoClock.system();
+
+ telemetry.addLine("Ready!");
+ telemetry.update();
+ telemetry.clearAll();
+
+ waitForStart();
+
+ if (isStopRequested()) return;
+
+ boolean movingForwards = true;
+ MotionProfile activeProfile = generateProfile(true);
+ double profileStart = clock.seconds();
+
+
+ while (!isStopRequested()) {
+ telemetry.addData("mode", mode);
+
+ switch (mode) {
+ case TUNING_MODE:
+ if (gamepad1.y) {
+ mode = Mode.DRIVER_MODE;
+ drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+ }
+
+ // calculate and set the motor power
+ double profileTime = clock.seconds() - profileStart;
+
+ if (profileTime > activeProfile.duration()) {
+ // generate a new profile
+ movingForwards = !movingForwards;
+ activeProfile = generateProfile(movingForwards);
+ profileStart = clock.seconds();
+ }
+
+ MotionState motionState = activeProfile.get(profileTime);
+ double targetPower = kV * motionState.getV();
+ drive.setDrivePower(new Pose2d(targetPower, 0, 0));
+
+ List velocities = drive.getWheelVelocities();
+
+ // update telemetry
+ telemetry.addData("targetVelocity", motionState.getV());
+ for (int i = 0; i < velocities.size(); i++) {
+ telemetry.addData("measuredVelocity" + i, velocities.get(i));
+ telemetry.addData(
+ "error" + i,
+ motionState.getV() - velocities.get(i)
+ );
+ }
+ break;
+ case DRIVER_MODE:
+ if (gamepad1.b) {
+ drive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+
+ mode = Mode.TUNING_MODE;
+ movingForwards = true;
+ activeProfile = generateProfile(movingForwards);
+ profileStart = clock.seconds();
+ }
+
+ drive.setWeightedDrivePower(
+ new Pose2d(
+ -gamepad1.left_stick_y,
+ -gamepad1.left_stick_x,
+ -gamepad1.right_stick_x
+ )
+ );
+ break;
+ }
+
+ if (lastKp != MOTOR_VELO_PID.p || lastKd != MOTOR_VELO_PID.d
+ || lastKi != MOTOR_VELO_PID.i || lastKf != MOTOR_VELO_PID.f) {
+ drive.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, MOTOR_VELO_PID);
+
+ lastKp = MOTOR_VELO_PID.p;
+ lastKi = MOTOR_VELO_PID.i;
+ lastKd = MOTOR_VELO_PID.d;
+ lastKf = MOTOR_VELO_PID.f;
+ }
+
+ telemetry.update();
+ }
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/FollowerPIDTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/FollowerPIDTuner.java
new file mode 100644
index 0000000..63f577e
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/FollowerPIDTuner.java
@@ -0,0 +1,55 @@
+package org.firstinspires.ftc.teamcode.drive.opmode;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+
+import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
+import org.firstinspires.ftc.teamcode.trajectorysequence.TrajectorySequence;
+
+/*
+ * Op mode for preliminary tuning of the follower PID coefficients (located in the drive base
+ * classes). The robot drives in a DISTANCE-by-DISTANCE square indefinitely. Utilization of the
+ * dashboard is recommended for this tuning routine. To access the dashboard, connect your computer
+ * to the RC's WiFi network. In your browser, navigate to https://192.168.49.1:8080/dash if you're
+ * using the RC phone or https://192.168.43.1:8080/dash if you are using the Control Hub. Once
+ * you've successfully connected, start the program, and your robot will begin driving in a square.
+ * You should observe the target position (green) and your pose estimate (blue) and adjust your
+ * follower PID coefficients such that you follow the target position as accurately as possible.
+ * If you are using SampleMecanumDrive, you should be tuning TRANSLATIONAL_PID and HEADING_PID.
+ * If you are using SampleTankDrive, you should be tuning AXIAL_PID, CROSS_TRACK_PID, and HEADING_PID.
+ * These coefficients can be tuned live in dashboard.
+ */
+@Config
+@Autonomous(group = "drive")
+public class FollowerPIDTuner extends LinearOpMode {
+ public static double DISTANCE = 48; // in
+
+ @Override
+ public void runOpMode() throws InterruptedException {
+ SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
+
+ Pose2d startPose = new Pose2d(-DISTANCE / 2, -DISTANCE / 2, 0);
+
+ drive.setPoseEstimate(startPose);
+
+ waitForStart();
+
+ if (isStopRequested()) return;
+
+ while (!isStopRequested()) {
+ TrajectorySequence trajSeq = drive.trajectorySequenceBuilder(startPose)
+ .forward(DISTANCE)
+ .turn(Math.toRadians(90))
+ .forward(DISTANCE)
+ .turn(Math.toRadians(90))
+ .forward(DISTANCE)
+ .turn(Math.toRadians(90))
+ .forward(DISTANCE)
+ .turn(Math.toRadians(90))
+ .build();
+ drive.followTrajectorySequence(trajSeq);
+ }
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/LocalizationTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/LocalizationTest.java
new file mode 100644
index 0000000..8411792
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/LocalizationTest.java
@@ -0,0 +1,45 @@
+package org.firstinspires.ftc.teamcode.drive.opmode;
+
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.hardware.DcMotor;
+
+import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
+
+/**
+ * This is a simple teleop routine for testing localization. Drive the robot around like a normal
+ * teleop routine and make sure the robot's estimated pose matches the robot's actual pose (slight
+ * errors are not out of the ordinary, especially with sudden drive motions). The goal of this
+ * exercise is to ascertain whether the localizer has been configured properly (note: the pure
+ * encoder localizer heading may be significantly off if the track width has not been tuned).
+ */
+@TeleOp(group = "drive")
+public class LocalizationTest extends LinearOpMode {
+ @Override
+ public void runOpMode() throws InterruptedException {
+ SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
+
+ drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+
+ waitForStart();
+
+ while (!isStopRequested()) {
+ drive.setWeightedDrivePower(
+ new Pose2d(
+ -gamepad1.left_stick_y,
+ -gamepad1.left_stick_x,
+ -gamepad1.right_stick_x
+ )
+ );
+
+ drive.update();
+
+ Pose2d poseEstimate = drive.getPoseEstimate();
+ telemetry.addData("x", poseEstimate.getX());
+ telemetry.addData("y", poseEstimate.getY());
+ telemetry.addData("heading", poseEstimate.getHeading());
+ telemetry.update();
+ }
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/ManualFeedforwardTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/ManualFeedforwardTuner.java
new file mode 100644
index 0000000..0d01bcb
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/ManualFeedforwardTuner.java
@@ -0,0 +1,154 @@
+package org.firstinspires.ftc.teamcode.drive.opmode;
+
+import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_ACCEL;
+import static org.firstinspires.ftc.teamcode.drive.DriveConstants.MAX_VEL;
+import static org.firstinspires.ftc.teamcode.drive.DriveConstants.RUN_USING_ENCODER;
+import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kA;
+import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kStatic;
+import static org.firstinspires.ftc.teamcode.drive.DriveConstants.kV;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.acmerobotics.roadrunner.drive.DriveSignal;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.kinematics.Kinematics;
+import com.acmerobotics.roadrunner.profile.MotionProfile;
+import com.acmerobotics.roadrunner.profile.MotionProfileGenerator;
+import com.acmerobotics.roadrunner.profile.MotionState;
+import com.acmerobotics.roadrunner.util.NanoClock;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.hardware.VoltageSensor;
+import com.qualcomm.robotcore.util.RobotLog;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.teamcode.drive.DriveConstants;
+import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
+
+import java.util.Objects;
+
+/*
+ * This routine is designed to tune the open-loop feedforward coefficients. Although it may seem unnecessary,
+ * tuning these coefficients is just as important as the positional parameters. Like the other
+ * manual tuning routines, this op mode relies heavily upon the dashboard. To access the dashboard,
+ * connect your computer to the RC's WiFi network. In your browser, navigate to
+ * https://192.168.49.1:8080/dash if you're using the RC phone or https://192.168.43.1:8080/dash if
+ * you are using the Control Hub. Once you've successfully connected, start the program, and your
+ * robot will begin moving forward and backward according to a motion profile. Your job is to graph
+ * the velocity errors over time and adjust the feedforward coefficients. Once you've found a
+ * satisfactory set of gains, add them to the appropriate fields in the DriveConstants.java file.
+ *
+ * Pressing Y/Δ (Xbox/PS4) will pause the tuning process and enter driver override, allowing the
+ * user to reset the position of the bot in the event that it drifts off the path.
+ * Pressing B/O (Xbox/PS4) will cede control back to the tuning process.
+ */
+@Config
+@Autonomous(group = "drive")
+public class ManualFeedforwardTuner extends LinearOpMode {
+ public static double DISTANCE = 72; // in
+
+ private FtcDashboard dashboard = FtcDashboard.getInstance();
+
+ private SampleMecanumDrive drive;
+
+ enum Mode {
+ DRIVER_MODE,
+ TUNING_MODE
+ }
+
+ private Mode mode;
+
+ private static MotionProfile generateProfile(boolean movingForward) {
+ MotionState start = new MotionState(movingForward ? 0 : DISTANCE, 0, 0, 0);
+ MotionState goal = new MotionState(movingForward ? DISTANCE : 0, 0, 0, 0);
+ return MotionProfileGenerator.generateSimpleMotionProfile(start, goal, MAX_VEL, MAX_ACCEL);
+ }
+
+ @Override
+ public void runOpMode() {
+ if (RUN_USING_ENCODER) {
+ RobotLog.setGlobalErrorMsg("Feedforward constants usually don't need to be tuned " +
+ "when using the built-in drive motor velocity PID.");
+ }
+
+ Telemetry telemetry = new MultipleTelemetry(this.telemetry, dashboard.getTelemetry());
+
+ drive = new SampleMecanumDrive(hardwareMap);
+
+ final VoltageSensor voltageSensor = hardwareMap.voltageSensor.iterator().next();
+
+ mode = Mode.TUNING_MODE;
+
+ NanoClock clock = NanoClock.system();
+
+ telemetry.addLine("Ready!");
+ telemetry.update();
+ telemetry.clearAll();
+
+ waitForStart();
+
+ if (isStopRequested()) return;
+
+ boolean movingForwards = true;
+ MotionProfile activeProfile = generateProfile(true);
+ double profileStart = clock.seconds();
+
+
+ while (!isStopRequested()) {
+ telemetry.addData("mode", mode);
+
+ switch (mode) {
+ case TUNING_MODE:
+ if (gamepad1.y) {
+ mode = Mode.DRIVER_MODE;
+ }
+
+ // calculate and set the motor power
+ double profileTime = clock.seconds() - profileStart;
+
+ if (profileTime > activeProfile.duration()) {
+ // generate a new profile
+ movingForwards = !movingForwards;
+ activeProfile = generateProfile(movingForwards);
+ profileStart = clock.seconds();
+ }
+
+ MotionState motionState = activeProfile.get(profileTime);
+ double targetPower = Kinematics.calculateMotorFeedforward(motionState.getV(), motionState.getA(), kV, kA, kStatic);
+
+ final double NOMINAL_VOLTAGE = 12.0;
+ final double voltage = voltageSensor.getVoltage();
+ drive.setDrivePower(new Pose2d(NOMINAL_VOLTAGE / voltage * targetPower, 0, 0));
+ drive.updatePoseEstimate();
+
+ Pose2d poseVelo = Objects.requireNonNull(drive.getPoseVelocity(), "poseVelocity() must not be null. Ensure that the getWheelVelocities() method has been overridden in your localizer.");
+ double currentVelo = poseVelo.getX();
+
+ // update telemetry
+ telemetry.addData("targetVelocity", motionState.getV());
+ telemetry.addData("measuredVelocity", currentVelo);
+ telemetry.addData("error", motionState.getV() - currentVelo);
+ break;
+ case DRIVER_MODE:
+ if (gamepad1.b) {
+ mode = Mode.TUNING_MODE;
+ movingForwards = true;
+ activeProfile = generateProfile(movingForwards);
+ profileStart = clock.seconds();
+ }
+
+ drive.setWeightedDrivePower(
+ new Pose2d(
+ -gamepad1.left_stick_y,
+ -gamepad1.left_stick_x,
+ -gamepad1.right_stick_x
+ )
+ );
+ break;
+ }
+
+ telemetry.update();
+ }
+ }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/MaxAngularVeloTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/MaxAngularVeloTuner.java
new file mode 100644
index 0000000..05d8265
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/MaxAngularVeloTuner.java
@@ -0,0 +1,73 @@
+package org.firstinspires.ftc.teamcode.drive.opmode;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.util.ElapsedTime;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
+
+import java.util.Objects;
+
+/**
+ * This routine is designed to calculate the maximum angular velocity your bot can achieve under load.
+ *
+ * Upon pressing start, your bot will turn at max power for RUNTIME seconds.
+ *
+ * Further fine tuning of MAX_ANG_VEL may be desired.
+ */
+
+@Config
+@Autonomous(group = "drive")
+public class MaxAngularVeloTuner extends LinearOpMode {
+ public static double RUNTIME = 4.0;
+
+ private ElapsedTime timer;
+ private double maxAngVelocity = 0.0;
+
+ @Override
+ public void runOpMode() throws InterruptedException {
+ SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
+
+ drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+
+ Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
+
+ telemetry.addLine("Your bot will turn at full speed for " + RUNTIME + " seconds.");
+ telemetry.addLine("Please ensure you have enough space cleared.");
+ telemetry.addLine("");
+ telemetry.addLine("Press start when ready.");
+ telemetry.update();
+
+ waitForStart();
+
+ telemetry.clearAll();
+ telemetry.update();
+
+ drive.setDrivePower(new Pose2d(0, 0, 1));
+ timer = new ElapsedTime();
+
+ while (!isStopRequested() && timer.seconds() < RUNTIME) {
+ drive.updatePoseEstimate();
+
+ Pose2d poseVelo = Objects.requireNonNull(drive.getPoseVelocity(), "poseVelocity() must not be null. Ensure that the getWheelVelocities() method has been overridden in your localizer.");
+
+ maxAngVelocity = Math.max(poseVelo.getHeading(), maxAngVelocity);
+ }
+
+ drive.setDrivePower(new Pose2d());
+
+ telemetry.addData("Max Angular Velocity (rad)", maxAngVelocity);
+ telemetry.addData("Max Angular Velocity (deg)", Math.toDegrees(maxAngVelocity));
+ telemetry.addData("Max Recommended Angular Velocity (rad)", maxAngVelocity * 0.8);
+ telemetry.addData("Max Recommended Angular Velocity (deg)", Math.toDegrees(maxAngVelocity * 0.8));
+ telemetry.update();
+
+ while (!isStopRequested()) idle();
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/MaxVelocityTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/MaxVelocityTuner.java
new file mode 100644
index 0000000..ddca6cd
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/MaxVelocityTuner.java
@@ -0,0 +1,84 @@
+package org.firstinspires.ftc.teamcode.drive.opmode;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.VoltageSensor;
+import com.qualcomm.robotcore.util.ElapsedTime;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.teamcode.drive.DriveConstants;
+import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
+
+import java.util.Objects;
+
+/**
+ * This routine is designed to calculate the maximum velocity your bot can achieve under load. It
+ * will also calculate the effective kF value for your velocity PID.
+ *
+ * Upon pressing start, your bot will run at max power for RUNTIME seconds.
+ *
+ * Further fine tuning of kF may be desired.
+ */
+@Config
+@Autonomous(group = "drive")
+public class MaxVelocityTuner extends LinearOpMode {
+ public static double RUNTIME = 2.0;
+
+ private ElapsedTime timer;
+ private double maxVelocity = 0.0;
+
+ private VoltageSensor batteryVoltageSensor;
+
+ @Override
+ public void runOpMode() throws InterruptedException {
+ SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
+
+ drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
+
+ batteryVoltageSensor = hardwareMap.voltageSensor.iterator().next();
+
+ Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
+
+ telemetry.addLine("Your bot will go at full speed for " + RUNTIME + " seconds.");
+ telemetry.addLine("Please ensure you have enough space cleared.");
+ telemetry.addLine("");
+ telemetry.addLine("Press start when ready.");
+ telemetry.update();
+
+ waitForStart();
+
+ telemetry.clearAll();
+ telemetry.update();
+
+ drive.setDrivePower(new Pose2d(1, 0, 0));
+ timer = new ElapsedTime();
+
+ while (!isStopRequested() && timer.seconds() < RUNTIME) {
+ drive.updatePoseEstimate();
+
+ Pose2d poseVelo = Objects.requireNonNull(drive.getPoseVelocity(), "poseVelocity() must not be null. Ensure that the getWheelVelocities() method has been overridden in your localizer.");
+
+ maxVelocity = Math.max(poseVelo.vec().norm(), maxVelocity);
+ }
+
+ drive.setDrivePower(new Pose2d());
+
+ double effectiveKf = DriveConstants.getMotorVelocityF(veloInchesToTicks(maxVelocity));
+
+ telemetry.addData("Max Velocity", maxVelocity);
+ telemetry.addData("Max Recommended Velocity", maxVelocity * 0.8);
+ telemetry.addData("Voltage Compensated kF", effectiveKf * batteryVoltageSensor.getVoltage() / 12);
+ telemetry.update();
+
+ while (!isStopRequested() && opModeIsActive()) idle();
+ }
+
+ private double veloInchesToTicks(double inchesPerSec) {
+ return inchesPerSec / (2 * Math.PI * DriveConstants.WHEEL_RADIUS) / DriveConstants.GEAR_RATIO * DriveConstants.TICKS_PER_REV;
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/MotorDirectionDebugger.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/MotorDirectionDebugger.java
new file mode 100644
index 0000000..023cfcc
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/MotorDirectionDebugger.java
@@ -0,0 +1,93 @@
+package org.firstinspires.ftc.teamcode.drive.opmode;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+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.teamcode.drive.SampleMecanumDrive;
+
+/**
+ * This is a simple teleop routine for debugging your motor configuration.
+ * Pressing each of the buttons will power its respective motor.
+ *
+ * Button Mappings:
+ *
+ * Xbox/PS4 Button - Motor
+ * X / ▢ - Front Left
+ * Y / Δ - Front Right
+ * B / O - Rear Right
+ * A / X - Rear Left
+ * The buttons are mapped to match the wheels spatially if you
+ * were to rotate the gamepad 45deg°. x/square is the front left
+ * ________ and each button corresponds to the wheel as you go clockwise
+ * / ______ \
+ * ------------.-' _ '-..+ Front of Bot
+ * / _ ( Y ) _ \ ^
+ * | ( X ) _ ( B ) | Front Left \ Front Right
+ * ___ '. ( A ) /| Wheel \ Wheel
+ * .' '. '-._____.-' .' (x/▢) \ (Y/Δ)
+ * | | | \
+ * '.___.' '. | Rear Left \ Rear Right
+ * '. / Wheel \ Wheel
+ * \. .' (A/X) \ (B/O)
+ * \________/
+ *
+ * Uncomment the @Disabled tag below to use this opmode.
+ */
+@Disabled
+@Config
+@TeleOp(group = "drive")
+public class MotorDirectionDebugger extends LinearOpMode {
+ public static double MOTOR_POWER = 0.7;
+
+ @Override
+ public void runOpMode() throws InterruptedException {
+ Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
+
+ SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
+
+ telemetry.addLine("Press play to begin the debugging opmode");
+ telemetry.update();
+
+ waitForStart();
+
+ if (isStopRequested()) return;
+
+ telemetry.clearAll();
+ telemetry.setDisplayFormat(Telemetry.DisplayFormat.HTML);
+
+ while (!isStopRequested()) {
+ telemetry.addLine("Press each button to turn on its respective motor");
+ telemetry.addLine();
+ telemetry.addLine("Xbox/PS4 Button - Motor");
+ telemetry.addLine(" X / ▢ - Front Left");
+ telemetry.addLine(" Y / Δ - Front Right");
+ telemetry.addLine(" B / O - Rear Right");
+ telemetry.addLine(" A / X - Rear Left");
+ telemetry.addLine();
+
+ if(gamepad1.x) {
+ drive.setMotorPowers(MOTOR_POWER, 0, 0, 0);
+ telemetry.addLine("Running Motor: Front Left");
+ } else if(gamepad1.y) {
+ drive.setMotorPowers(0, 0, 0, MOTOR_POWER);
+ telemetry.addLine("Running Motor: Front Right");
+ } else if(gamepad1.b) {
+ drive.setMotorPowers(0, 0, MOTOR_POWER, 0);
+ telemetry.addLine("Running Motor: Rear Right");
+ } else if(gamepad1.a) {
+ drive.setMotorPowers(0, MOTOR_POWER, 0, 0);
+ telemetry.addLine("Running Motor: Rear Left");
+ } else {
+ drive.setMotorPowers(0, 0, 0, 0);
+ telemetry.addLine("Running Motor: None");
+ }
+
+ telemetry.update();
+ }
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/SplineTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/SplineTest.java
new file mode 100644
index 0000000..d731676
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/SplineTest.java
@@ -0,0 +1,38 @@
+package org.firstinspires.ftc.teamcode.drive.opmode;
+
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.geometry.Vector2d;
+import com.acmerobotics.roadrunner.trajectory.Trajectory;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+
+import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
+
+/*
+ * This is an example of a more complex path to really test the tuning.
+ */
+@Autonomous(group = "drive")
+public class SplineTest extends LinearOpMode {
+ @Override
+ public void runOpMode() throws InterruptedException {
+ SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
+
+ waitForStart();
+
+ if (isStopRequested()) return;
+
+ Trajectory traj = drive.trajectoryBuilder(new Pose2d())
+ .splineTo(new Vector2d(30, 30), 0)
+ .build();
+
+ drive.followTrajectory(traj);
+
+ sleep(2000);
+
+ drive.followTrajectory(
+ drive.trajectoryBuilder(traj.end(), true)
+ .splineTo(new Vector2d(0, 0), Math.toRadians(180))
+ .build()
+ );
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/StrafeTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/StrafeTest.java
new file mode 100644
index 0000000..992d03a
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/StrafeTest.java
@@ -0,0 +1,46 @@
+package org.firstinspires.ftc.teamcode.drive.opmode;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.trajectory.Trajectory;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
+
+/*
+ * This is a simple routine to test translational drive capabilities.
+ */
+@Config
+@Autonomous(group = "drive")
+public class StrafeTest extends LinearOpMode {
+ public static double DISTANCE = 60; // in
+
+ @Override
+ public void runOpMode() throws InterruptedException {
+ Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
+
+ SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
+
+ Trajectory trajectory = drive.trajectoryBuilder(new Pose2d())
+ .strafeRight(DISTANCE)
+ .build();
+
+ waitForStart();
+
+ if (isStopRequested()) return;
+
+ drive.followTrajectory(trajectory);
+
+ Pose2d poseEstimate = drive.getPoseEstimate();
+ telemetry.addData("finalX", poseEstimate.getX());
+ telemetry.addData("finalY", poseEstimate.getY());
+ telemetry.addData("finalHeading", poseEstimate.getHeading());
+ telemetry.update();
+
+ while (!isStopRequested() && opModeIsActive()) ;
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/StraightTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/StraightTest.java
new file mode 100644
index 0000000..e1b27e1
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/StraightTest.java
@@ -0,0 +1,46 @@
+package org.firstinspires.ftc.teamcode.drive.opmode;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.trajectory.Trajectory;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
+
+/*
+ * This is a simple routine to test translational drive capabilities.
+ */
+@Config
+@Autonomous(group = "drive")
+public class StraightTest extends LinearOpMode {
+ public static double DISTANCE = 60; // in
+
+ @Override
+ public void runOpMode() throws InterruptedException {
+ Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
+
+ SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
+
+ Trajectory trajectory = drive.trajectoryBuilder(new Pose2d())
+ .forward(DISTANCE)
+ .build();
+
+ waitForStart();
+
+ if (isStopRequested()) return;
+
+ drive.followTrajectory(trajectory);
+
+ Pose2d poseEstimate = drive.getPoseEstimate();
+ telemetry.addData("finalX", poseEstimate.getX());
+ telemetry.addData("finalY", poseEstimate.getY());
+ telemetry.addData("finalHeading", poseEstimate.getHeading());
+ telemetry.update();
+
+ while (!isStopRequested() && opModeIsActive()) ;
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/TrackWidthTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/TrackWidthTuner.java
new file mode 100644
index 0000000..ffce023
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/TrackWidthTuner.java
@@ -0,0 +1,88 @@
+package org.firstinspires.ftc.teamcode.drive.opmode;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.util.Angle;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.util.MovingStatistics;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.robotcore.internal.system.Misc;
+import org.firstinspires.ftc.teamcode.drive.DriveConstants;
+import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
+
+/*
+ * This routine determines the effective track width. The procedure works by executing a point turn
+ * with a given angle and measuring the difference between that angle and the actual angle (as
+ * indicated by an external IMU/gyro, track wheels, or some other localizer). The quotient
+ * given angle / actual angle gives a multiplicative adjustment to the estimated track width
+ * (effective track width = estimated track width * given angle / actual angle). The routine repeats
+ * this procedure a few times and averages the values for additional accuracy. Note: a relatively
+ * accurate track width estimate is important or else the angular constraints will be thrown off.
+ */
+@Config
+@Autonomous(group = "drive")
+public class TrackWidthTuner extends LinearOpMode {
+ public static double ANGLE = 180; // deg
+ public static int NUM_TRIALS = 5;
+ public static int DELAY = 1000; // ms
+
+ @Override
+ public void runOpMode() throws InterruptedException {
+ Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
+
+ SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
+ // TODO: if you haven't already, set the localizer to something that doesn't depend on
+ // drive encoders for computing the heading
+
+ telemetry.addLine("Press play to begin the track width tuner routine");
+ telemetry.addLine("Make sure your robot has enough clearance to turn smoothly");
+ telemetry.update();
+
+ waitForStart();
+
+ if (isStopRequested()) return;
+
+ telemetry.clearAll();
+ telemetry.addLine("Running...");
+ telemetry.update();
+
+ MovingStatistics trackWidthStats = new MovingStatistics(NUM_TRIALS);
+ for (int i = 0; i < NUM_TRIALS; i++) {
+ drive.setPoseEstimate(new Pose2d());
+
+ // it is important to handle heading wraparounds
+ double headingAccumulator = 0;
+ double lastHeading = 0;
+
+ drive.turnAsync(Math.toRadians(ANGLE));
+
+ while (!isStopRequested() && drive.isBusy()) {
+ double heading = drive.getPoseEstimate().getHeading();
+ headingAccumulator += Angle.normDelta(heading - lastHeading);
+ lastHeading = heading;
+
+ drive.update();
+ }
+
+ double trackWidth = DriveConstants.TRACK_WIDTH * Math.toRadians(ANGLE) / headingAccumulator;
+ trackWidthStats.add(trackWidth);
+
+ sleep(DELAY);
+ }
+
+ telemetry.clearAll();
+ telemetry.addLine("Tuning complete");
+ telemetry.addLine(Misc.formatInvariant("Effective track width = %.2f (SE = %.3f)",
+ trackWidthStats.getMean(),
+ trackWidthStats.getStandardDeviation() / Math.sqrt(NUM_TRIALS)));
+ telemetry.update();
+
+ while (!isStopRequested()) {
+ idle();
+ }
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/TrackingWheelForwardOffsetTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/TrackingWheelForwardOffsetTuner.java
new file mode 100644
index 0000000..ebe97f2
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/TrackingWheelForwardOffsetTuner.java
@@ -0,0 +1,104 @@
+package org.firstinspires.ftc.teamcode.drive.opmode;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.util.Angle;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.util.MovingStatistics;
+import com.qualcomm.robotcore.util.RobotLog;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.robotcore.internal.system.Misc;
+import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
+import org.firstinspires.ftc.teamcode.drive.StandardTrackingWheelLocalizer;
+
+/**
+ * This routine determines the effective forward offset for the lateral tracking wheel.
+ * The procedure executes a point turn at a given angle for a certain number of trials,
+ * along with a specified delay in milliseconds. The purpose of this is to track the
+ * change in the y position during the turn. The offset, or distance, of the lateral tracking
+ * wheel from the center or rotation allows the wheel to spin during a point turn, leading
+ * to an incorrect measurement for the y position. This creates an arc around around
+ * the center of rotation with an arc length of change in y and a radius equal to the forward
+ * offset. We can compute this offset by calculating (change in y position) / (change in heading)
+ * which returns the radius if the angle (change in heading) is in radians. This is based
+ * on the arc length formula of length = theta * radius.
+ *
+ * To run this routine, simply adjust the desired angle and specify the number of trials
+ * and the desired delay. Then, run the procedure. Once it finishes, it will print the
+ * average of all the calculated forward offsets derived from the calculation. This calculated
+ * forward offset is then added onto the current forward offset to produce an overall estimate
+ * for the forward offset. You can run this procedure as many times as necessary until a
+ * satisfactory result is produced.
+ */
+@Config
+@Autonomous(group="drive")
+public class TrackingWheelForwardOffsetTuner extends LinearOpMode {
+ public static double ANGLE = 180; // deg
+ public static int NUM_TRIALS = 5;
+ public static int DELAY = 1000; // ms
+
+ @Override
+ public void runOpMode() throws InterruptedException {
+ Telemetry telemetry = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry());
+
+ SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
+
+ if (!(drive.getLocalizer() instanceof StandardTrackingWheelLocalizer)) {
+ RobotLog.setGlobalErrorMsg("StandardTrackingWheelLocalizer is not being set in the "
+ + "drive class. Ensure that \"setLocalizer(new StandardTrackingWheelLocalizer"
+ + "(hardwareMap));\" is called in SampleMecanumDrive.java");
+ }
+
+ telemetry.addLine("Press play to begin the forward offset tuner");
+ telemetry.addLine("Make sure your robot has enough clearance to turn smoothly");
+ telemetry.update();
+
+ waitForStart();
+
+ if (isStopRequested()) return;
+
+ telemetry.clearAll();
+ telemetry.addLine("Running...");
+ telemetry.update();
+
+ MovingStatistics forwardOffsetStats = new MovingStatistics(NUM_TRIALS);
+ for (int i = 0; i < NUM_TRIALS; i++) {
+ drive.setPoseEstimate(new Pose2d());
+
+ // it is important to handle heading wraparounds
+ double headingAccumulator = 0;
+ double lastHeading = 0;
+
+ drive.turnAsync(Math.toRadians(ANGLE));
+
+ while (!isStopRequested() && drive.isBusy()) {
+ double heading = drive.getPoseEstimate().getHeading();
+ headingAccumulator += Angle.norm(heading - lastHeading);
+ lastHeading = heading;
+
+ drive.update();
+ }
+
+ double forwardOffset = StandardTrackingWheelLocalizer.FORWARD_OFFSET +
+ drive.getPoseEstimate().getY() / headingAccumulator;
+ forwardOffsetStats.add(forwardOffset);
+
+ sleep(DELAY);
+ }
+
+ telemetry.clearAll();
+ telemetry.addLine("Tuning complete");
+ telemetry.addLine(Misc.formatInvariant("Effective forward offset = %.2f (SE = %.3f)",
+ forwardOffsetStats.getMean(),
+ forwardOffsetStats.getStandardDeviation() / Math.sqrt(NUM_TRIALS)));
+ telemetry.update();
+
+ while (!isStopRequested()) {
+ idle();
+ }
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/TrackingWheelLateralDistanceTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/TrackingWheelLateralDistanceTuner.java
new file mode 100644
index 0000000..a11059c
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/TrackingWheelLateralDistanceTuner.java
@@ -0,0 +1,130 @@
+package org.firstinspires.ftc.teamcode.drive.opmode;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.util.Angle;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.util.RobotLog;
+
+import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
+import org.firstinspires.ftc.teamcode.drive.StandardTrackingWheelLocalizer;
+
+/**
+ * Opmode designed to assist the user in tuning the `StandardTrackingWheelLocalizer`'s
+ * LATERAL_DISTANCE value. The LATERAL_DISTANCE is the center-to-center distance of the parallel
+ * wheels.
+ *
+ * Tuning Routine:
+ *
+ * 1. Set the LATERAL_DISTANCE value in StandardTrackingWheelLocalizer.java to the physical
+ * measured value. This need only be an estimated value as you will be tuning it anyways.
+ *
+ * 2. Make a mark on the bot (with a piece of tape or sharpie or however you wish) and make an
+ * similar mark right below the indicator on your bot. This will be your reference point to
+ * ensure you've turned exactly 360°.
+ *
+ * 3. Although not entirely necessary, having the bot's pose being drawn in dashbooard does help
+ * identify discrepancies in the LATERAL_DISTANCE value. To access the dashboard,
+ * connect your computer to the RC's WiFi network. In your browser, navigate to
+ * https://192.168.49.1:8080/dash if you're using the RC phone or https://192.168.43.1:8080/dash
+ * if you are using the Control Hub.
+ * Ensure the field is showing (select the field view in top right of the page).
+ *
+ * 4. Press play to begin the tuning routine.
+ *
+ * 5. Use the right joystick on gamepad 1 to turn the bot counterclockwise.
+ *
+ * 6. Spin the bot 10 times, counterclockwise. Make sure to keep track of these turns.
+ *
+ * 7. Once the bot has finished spinning 10 times, press A to finishing the routine. The indicators
+ * on the bot and on the ground you created earlier should be lined up.
+ *
+ * 8. Your effective LATERAL_DISTANCE will be given. Stick this value into your
+ * StandardTrackingWheelLocalizer.java class.
+ *
+ * 9. If this value is incorrect, run the routine again while adjusting the LATERAL_DISTANCE value
+ * yourself. Read the heading output and follow the advice stated in the note below to manually
+ * nudge the values yourself.
+ *
+ * Note:
+ * It helps to pay attention to how the pose on the field is drawn in dashboard. A blue circle with
+ * a line from the circumference to the center should be present, representing the bot. The line
+ * indicates forward. If your LATERAL_DISTANCE value is tuned currently, the pose drawn in
+ * dashboard should keep track with the pose of your actual bot. If the drawn bot turns slower than
+ * the actual bot, the LATERAL_DISTANCE should be decreased. If the drawn bot turns faster than the
+ * actual bot, the LATERAL_DISTANCE should be increased.
+ *
+ * If your drawn bot oscillates around a point in dashboard, don't worry. This is because the
+ * position of the perpendicular wheel isn't perfectly set and causes a discrepancy in the
+ * effective center of rotation. You can ignore this effect. The center of rotation will be offset
+ * slightly but your heading will still be fine. This does not affect your overall tracking
+ * precision. The heading should still line up.
+ */
+@Config
+@TeleOp(group = "drive")
+public class TrackingWheelLateralDistanceTuner extends LinearOpMode {
+ public static int NUM_TURNS = 10;
+
+ @Override
+ public void runOpMode() throws InterruptedException {
+ SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
+
+ if (!(drive.getLocalizer() instanceof StandardTrackingWheelLocalizer)) {
+ RobotLog.setGlobalErrorMsg("StandardTrackingWheelLocalizer is not being set in the "
+ + "drive class. Ensure that \"setLocalizer(new StandardTrackingWheelLocalizer"
+ + "(hardwareMap));\" is called in SampleMecanumDrive.java");
+ }
+
+ telemetry.addLine("Prior to beginning the routine, please read the directions "
+ + "located in the comments of the opmode file.");
+ telemetry.addLine("Press play to begin the tuning routine.");
+ telemetry.addLine("");
+ telemetry.addLine("Press Y/△ to stop the routine.");
+ telemetry.update();
+
+ waitForStart();
+
+ if (isStopRequested()) return;
+
+ telemetry.clearAll();
+ telemetry.update();
+
+ double headingAccumulator = 0;
+ double lastHeading = 0;
+
+ boolean tuningFinished = false;
+
+ while (!isStopRequested() && !tuningFinished) {
+ Pose2d vel = new Pose2d(0, 0, -gamepad1.right_stick_x);
+ drive.setDrivePower(vel);
+
+ drive.update();
+
+ double heading = drive.getPoseEstimate().getHeading();
+ double deltaHeading = heading - lastHeading;
+
+ headingAccumulator += Angle.normDelta(deltaHeading);
+ lastHeading = heading;
+
+ telemetry.clearAll();
+ telemetry.addLine("Total Heading (deg): " + Math.toDegrees(headingAccumulator));
+ telemetry.addLine("Raw Heading (deg): " + Math.toDegrees(heading));
+ telemetry.addLine();
+ telemetry.addLine("Press Y/△ to conclude routine");
+ telemetry.update();
+
+ if (gamepad1.y)
+ tuningFinished = true;
+ }
+
+ telemetry.clearAll();
+ telemetry.addLine("Localizer's total heading: " + Math.toDegrees(headingAccumulator) + "°");
+ telemetry.addLine("Effective LATERAL_DISTANCE: " +
+ (headingAccumulator / (NUM_TURNS * Math.PI * 2)) * StandardTrackingWheelLocalizer.LATERAL_DISTANCE);
+
+ telemetry.update();
+
+ while (!isStopRequested()) idle();
+ }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/TurnTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/TurnTest.java
new file mode 100644
index 0000000..0637d19
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/drive/opmode/TurnTest.java
@@ -0,0 +1,27 @@
+package org.firstinspires.ftc.teamcode.drive.opmode;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+
+import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
+
+/*
+ * This is a simple routine to test turning capabilities.
+ */
+@Config
+@Autonomous(group = "drive")
+public class TurnTest extends LinearOpMode {
+ public static double ANGLE = 90; // deg
+
+ @Override
+ public void runOpMode() throws InterruptedException {
+ SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
+
+ waitForStart();
+
+ if (isStopRequested()) return;
+
+ drive.turn(Math.toRadians(ANGLE));
+ }
+}
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 100644
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/trajectorysequence/EmptySequenceException.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/EmptySequenceException.java
new file mode 100644
index 0000000..081d695
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/EmptySequenceException.java
@@ -0,0 +1,4 @@
+package org.firstinspires.ftc.teamcode.trajectorysequence;
+
+
+public class EmptySequenceException extends RuntimeException { }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/TrajectorySequence.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/TrajectorySequence.java
new file mode 100644
index 0000000..29032e6
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/TrajectorySequence.java
@@ -0,0 +1,44 @@
+package org.firstinspires.ftc.teamcode.trajectorysequence;
+
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+
+import org.firstinspires.ftc.teamcode.trajectorysequence.sequencesegment.SequenceSegment;
+
+import java.util.Collections;
+import java.util.List;
+
+public class TrajectorySequence {
+ private final List sequenceList;
+
+ public TrajectorySequence(List sequenceList) {
+ if (sequenceList.size() == 0) throw new EmptySequenceException();
+
+ this.sequenceList = Collections.unmodifiableList(sequenceList);
+ }
+
+ public Pose2d start() {
+ return sequenceList.get(0).getStartPose();
+ }
+
+ public Pose2d end() {
+ return sequenceList.get(sequenceList.size() - 1).getEndPose();
+ }
+
+ public double duration() {
+ double total = 0.0;
+
+ for (SequenceSegment segment : sequenceList) {
+ total += segment.getDuration();
+ }
+
+ return total;
+ }
+
+ public SequenceSegment get(int i) {
+ return sequenceList.get(i);
+ }
+
+ public int size() {
+ return sequenceList.size();
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/TrajectorySequenceBuilder.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/TrajectorySequenceBuilder.java
new file mode 100644
index 0000000..8166db3
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/TrajectorySequenceBuilder.java
@@ -0,0 +1,679 @@
+package org.firstinspires.ftc.teamcode.trajectorysequence;
+
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.geometry.Vector2d;
+import com.acmerobotics.roadrunner.path.PathContinuityViolationException;
+import com.acmerobotics.roadrunner.profile.MotionProfile;
+import com.acmerobotics.roadrunner.profile.MotionProfileGenerator;
+import com.acmerobotics.roadrunner.profile.MotionState;
+import com.acmerobotics.roadrunner.trajectory.DisplacementMarker;
+import com.acmerobotics.roadrunner.trajectory.DisplacementProducer;
+import com.acmerobotics.roadrunner.trajectory.MarkerCallback;
+import com.acmerobotics.roadrunner.trajectory.SpatialMarker;
+import com.acmerobotics.roadrunner.trajectory.TemporalMarker;
+import com.acmerobotics.roadrunner.trajectory.TimeProducer;
+import com.acmerobotics.roadrunner.trajectory.Trajectory;
+import com.acmerobotics.roadrunner.trajectory.TrajectoryBuilder;
+import com.acmerobotics.roadrunner.trajectory.TrajectoryMarker;
+import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryAccelerationConstraint;
+import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryVelocityConstraint;
+import com.acmerobotics.roadrunner.util.Angle;
+
+import org.firstinspires.ftc.teamcode.trajectorysequence.sequencesegment.SequenceSegment;
+import org.firstinspires.ftc.teamcode.trajectorysequence.sequencesegment.TrajectorySegment;
+import org.firstinspires.ftc.teamcode.trajectorysequence.sequencesegment.TurnSegment;
+import org.firstinspires.ftc.teamcode.trajectorysequence.sequencesegment.WaitSegment;
+
+import java.util.ArrayList;
+import java.util.Collections;
+import java.util.Comparator;
+import java.util.List;
+
+public class TrajectorySequenceBuilder {
+ private final double resolution = 0.25;
+
+ private final TrajectoryVelocityConstraint baseVelConstraint;
+ private final TrajectoryAccelerationConstraint baseAccelConstraint;
+
+ private TrajectoryVelocityConstraint currentVelConstraint;
+ private TrajectoryAccelerationConstraint currentAccelConstraint;
+
+ private final double baseTurnConstraintMaxAngVel;
+ private final double baseTurnConstraintMaxAngAccel;
+
+ private double currentTurnConstraintMaxAngVel;
+ private double currentTurnConstraintMaxAngAccel;
+
+ private final List sequenceSegments;
+
+ private final List temporalMarkers;
+ private final List displacementMarkers;
+ private final List spatialMarkers;
+
+ private Pose2d lastPose;
+
+ private double tangentOffset;
+
+ private boolean setAbsoluteTangent;
+ private double absoluteTangent;
+
+ private TrajectoryBuilder currentTrajectoryBuilder;
+
+ private double currentDuration;
+ private double currentDisplacement;
+
+ private double lastDurationTraj;
+ private double lastDisplacementTraj;
+
+ public TrajectorySequenceBuilder(
+ Pose2d startPose,
+ Double startTangent,
+ TrajectoryVelocityConstraint baseVelConstraint,
+ TrajectoryAccelerationConstraint baseAccelConstraint,
+ double baseTurnConstraintMaxAngVel,
+ double baseTurnConstraintMaxAngAccel
+ ) {
+ this.baseVelConstraint = baseVelConstraint;
+ this.baseAccelConstraint = baseAccelConstraint;
+
+ this.currentVelConstraint = baseVelConstraint;
+ this.currentAccelConstraint = baseAccelConstraint;
+
+ this.baseTurnConstraintMaxAngVel = baseTurnConstraintMaxAngVel;
+ this.baseTurnConstraintMaxAngAccel = baseTurnConstraintMaxAngAccel;
+
+ this.currentTurnConstraintMaxAngVel = baseTurnConstraintMaxAngVel;
+ this.currentTurnConstraintMaxAngAccel = baseTurnConstraintMaxAngAccel;
+
+ sequenceSegments = new ArrayList<>();
+
+ temporalMarkers = new ArrayList<>();
+ displacementMarkers = new ArrayList<>();
+ spatialMarkers = new ArrayList<>();
+
+ lastPose = startPose;
+
+ tangentOffset = 0.0;
+
+ setAbsoluteTangent = (startTangent != null);
+ absoluteTangent = startTangent != null ? startTangent : 0.0;
+
+ currentTrajectoryBuilder = null;
+
+ currentDuration = 0.0;
+ currentDisplacement = 0.0;
+
+ lastDurationTraj = 0.0;
+ lastDisplacementTraj = 0.0;
+ }
+
+ public TrajectorySequenceBuilder(
+ Pose2d startPose,
+ TrajectoryVelocityConstraint baseVelConstraint,
+ TrajectoryAccelerationConstraint baseAccelConstraint,
+ double baseTurnConstraintMaxAngVel,
+ double baseTurnConstraintMaxAngAccel
+ ) {
+ this(
+ startPose, null,
+ baseVelConstraint, baseAccelConstraint,
+ baseTurnConstraintMaxAngVel, baseTurnConstraintMaxAngAccel
+ );
+ }
+
+ public TrajectorySequenceBuilder lineTo(Vector2d endPosition) {
+ return addPath(() -> currentTrajectoryBuilder.lineTo(endPosition, currentVelConstraint, currentAccelConstraint));
+ }
+
+ public TrajectorySequenceBuilder lineTo(
+ Vector2d endPosition,
+ TrajectoryVelocityConstraint velConstraint,
+ TrajectoryAccelerationConstraint accelConstraint
+ ) {
+ return addPath(() -> currentTrajectoryBuilder.lineTo(endPosition, velConstraint, accelConstraint));
+ }
+
+ public TrajectorySequenceBuilder lineToConstantHeading(Vector2d endPosition) {
+ return addPath(() -> currentTrajectoryBuilder.lineToConstantHeading(endPosition, currentVelConstraint, currentAccelConstraint));
+ }
+
+ public TrajectorySequenceBuilder lineToConstantHeading(
+ Vector2d endPosition,
+ TrajectoryVelocityConstraint velConstraint,
+ TrajectoryAccelerationConstraint accelConstraint
+ ) {
+ return addPath(() -> currentTrajectoryBuilder.lineToConstantHeading(endPosition, velConstraint, accelConstraint));
+ }
+
+ public TrajectorySequenceBuilder lineToLinearHeading(Pose2d endPose) {
+ return addPath(() -> currentTrajectoryBuilder.lineToLinearHeading(endPose, currentVelConstraint, currentAccelConstraint));
+ }
+
+ public TrajectorySequenceBuilder lineToLinearHeading(
+ Pose2d endPose,
+ TrajectoryVelocityConstraint velConstraint,
+ TrajectoryAccelerationConstraint accelConstraint
+ ) {
+ return addPath(() -> currentTrajectoryBuilder.lineToLinearHeading(endPose, velConstraint, accelConstraint));
+ }
+
+ public TrajectorySequenceBuilder lineToSplineHeading(Pose2d endPose) {
+ return addPath(() -> currentTrajectoryBuilder.lineToSplineHeading(endPose, currentVelConstraint, currentAccelConstraint));
+ }
+
+ public TrajectorySequenceBuilder lineToSplineHeading(
+ Pose2d endPose,
+ TrajectoryVelocityConstraint velConstraint,
+ TrajectoryAccelerationConstraint accelConstraint
+ ) {
+ return addPath(() -> currentTrajectoryBuilder.lineToSplineHeading(endPose, velConstraint, accelConstraint));
+ }
+
+ public TrajectorySequenceBuilder strafeTo(Vector2d endPosition) {
+ return addPath(() -> currentTrajectoryBuilder.strafeTo(endPosition, currentVelConstraint, currentAccelConstraint));
+ }
+
+ public TrajectorySequenceBuilder strafeTo(
+ Vector2d endPosition,
+ TrajectoryVelocityConstraint velConstraint,
+ TrajectoryAccelerationConstraint accelConstraint
+ ) {
+ return addPath(() -> currentTrajectoryBuilder.strafeTo(endPosition, velConstraint, accelConstraint));
+ }
+
+ public TrajectorySequenceBuilder forward(double distance) {
+ return addPath(() -> currentTrajectoryBuilder.forward(distance, currentVelConstraint, currentAccelConstraint));
+ }
+
+ public TrajectorySequenceBuilder forward(
+ double distance,
+ TrajectoryVelocityConstraint velConstraint,
+ TrajectoryAccelerationConstraint accelConstraint
+ ) {
+ return addPath(() -> currentTrajectoryBuilder.forward(distance, velConstraint, accelConstraint));
+ }
+
+ public TrajectorySequenceBuilder back(double distance) {
+ return addPath(() -> currentTrajectoryBuilder.back(distance, currentVelConstraint, currentAccelConstraint));
+ }
+
+ public TrajectorySequenceBuilder back(
+ double distance,
+ TrajectoryVelocityConstraint velConstraint,
+ TrajectoryAccelerationConstraint accelConstraint
+ ) {
+ return addPath(() -> currentTrajectoryBuilder.back(distance, velConstraint, accelConstraint));
+ }
+
+ public TrajectorySequenceBuilder strafeLeft(double distance) {
+ return addPath(() -> currentTrajectoryBuilder.strafeLeft(distance, currentVelConstraint, currentAccelConstraint));
+ }
+
+ public TrajectorySequenceBuilder strafeLeft(
+ double distance,
+ TrajectoryVelocityConstraint velConstraint,
+ TrajectoryAccelerationConstraint accelConstraint
+ ) {
+ return addPath(() -> currentTrajectoryBuilder.strafeLeft(distance, velConstraint, accelConstraint));
+ }
+
+ public TrajectorySequenceBuilder strafeRight(double distance) {
+ return addPath(() -> currentTrajectoryBuilder.strafeRight(distance, currentVelConstraint, currentAccelConstraint));
+ }
+
+ public TrajectorySequenceBuilder strafeRight(
+ double distance,
+ TrajectoryVelocityConstraint velConstraint,
+ TrajectoryAccelerationConstraint accelConstraint
+ ) {
+ return addPath(() -> currentTrajectoryBuilder.strafeRight(distance, velConstraint, accelConstraint));
+ }
+
+ public TrajectorySequenceBuilder splineTo(Vector2d endPosition, double endHeading) {
+ return addPath(() -> currentTrajectoryBuilder.splineTo(endPosition, endHeading, currentVelConstraint, currentAccelConstraint));
+ }
+
+ public TrajectorySequenceBuilder splineTo(
+ Vector2d endPosition,
+ double endHeading,
+ TrajectoryVelocityConstraint velConstraint,
+ TrajectoryAccelerationConstraint accelConstraint
+ ) {
+ return addPath(() -> currentTrajectoryBuilder.splineTo(endPosition, endHeading, velConstraint, accelConstraint));
+ }
+
+ public TrajectorySequenceBuilder splineToConstantHeading(Vector2d endPosition, double endHeading) {
+ return addPath(() -> currentTrajectoryBuilder.splineToConstantHeading(endPosition, endHeading, currentVelConstraint, currentAccelConstraint));
+ }
+
+ public TrajectorySequenceBuilder splineToConstantHeading(
+ Vector2d endPosition,
+ double endHeading,
+ TrajectoryVelocityConstraint velConstraint,
+ TrajectoryAccelerationConstraint accelConstraint
+ ) {
+ return addPath(() -> currentTrajectoryBuilder.splineToConstantHeading(endPosition, endHeading, velConstraint, accelConstraint));
+ }
+
+ public TrajectorySequenceBuilder splineToLinearHeading(Pose2d endPose, double endHeading) {
+ return addPath(() -> currentTrajectoryBuilder.splineToLinearHeading(endPose, endHeading, currentVelConstraint, currentAccelConstraint));
+ }
+
+ public TrajectorySequenceBuilder splineToLinearHeading(
+ Pose2d endPose,
+ double endHeading,
+ TrajectoryVelocityConstraint velConstraint,
+ TrajectoryAccelerationConstraint accelConstraint
+ ) {
+ return addPath(() -> currentTrajectoryBuilder.splineToLinearHeading(endPose, endHeading, velConstraint, accelConstraint));
+ }
+
+ public TrajectorySequenceBuilder splineToSplineHeading(Pose2d endPose, double endHeading) {
+ return addPath(() -> currentTrajectoryBuilder.splineToSplineHeading(endPose, endHeading, currentVelConstraint, currentAccelConstraint));
+ }
+
+ public TrajectorySequenceBuilder splineToSplineHeading(
+ Pose2d endPose,
+ double endHeading,
+ TrajectoryVelocityConstraint velConstraint,
+ TrajectoryAccelerationConstraint accelConstraint
+ ) {
+ return addPath(() -> currentTrajectoryBuilder.splineToSplineHeading(endPose, endHeading, velConstraint, accelConstraint));
+ }
+
+ private TrajectorySequenceBuilder addPath(AddPathCallback callback) {
+ if (currentTrajectoryBuilder == null) newPath();
+
+ try {
+ callback.run();
+ } catch (PathContinuityViolationException e) {
+ newPath();
+ callback.run();
+ }
+
+ Trajectory builtTraj = currentTrajectoryBuilder.build();
+
+ double durationDifference = builtTraj.duration() - lastDurationTraj;
+ double displacementDifference = builtTraj.getPath().length() - lastDisplacementTraj;
+
+ lastPose = builtTraj.end();
+ currentDuration += durationDifference;
+ currentDisplacement += displacementDifference;
+
+ lastDurationTraj = builtTraj.duration();
+ lastDisplacementTraj = builtTraj.getPath().length();
+
+ return this;
+ }
+
+ public TrajectorySequenceBuilder setTangent(double tangent) {
+ setAbsoluteTangent = true;
+ absoluteTangent = tangent;
+
+ pushPath();
+
+ return this;
+ }
+
+ private TrajectorySequenceBuilder setTangentOffset(double offset) {
+ setAbsoluteTangent = false;
+
+ this.tangentOffset = offset;
+ this.pushPath();
+
+ return this;
+ }
+
+ public TrajectorySequenceBuilder setReversed(boolean reversed) {
+ return reversed ? this.setTangentOffset(Math.toRadians(180.0)) : this.setTangentOffset(0.0);
+ }
+
+ public TrajectorySequenceBuilder setConstraints(
+ TrajectoryVelocityConstraint velConstraint,
+ TrajectoryAccelerationConstraint accelConstraint
+ ) {
+ this.currentVelConstraint = velConstraint;
+ this.currentAccelConstraint = accelConstraint;
+
+ return this;
+ }
+
+ public TrajectorySequenceBuilder resetConstraints() {
+ this.currentVelConstraint = this.baseVelConstraint;
+ this.currentAccelConstraint = this.baseAccelConstraint;
+
+ return this;
+ }
+
+ public TrajectorySequenceBuilder setVelConstraint(TrajectoryVelocityConstraint velConstraint) {
+ this.currentVelConstraint = velConstraint;
+
+ return this;
+ }
+
+ public TrajectorySequenceBuilder resetVelConstraint() {
+ this.currentVelConstraint = this.baseVelConstraint;
+
+ return this;
+ }
+
+ public TrajectorySequenceBuilder setAccelConstraint(TrajectoryAccelerationConstraint accelConstraint) {
+ this.currentAccelConstraint = accelConstraint;
+
+ return this;
+ }
+
+ public TrajectorySequenceBuilder resetAccelConstraint() {
+ this.currentAccelConstraint = this.baseAccelConstraint;
+
+ return this;
+ }
+
+ public TrajectorySequenceBuilder setTurnConstraint(double maxAngVel, double maxAngAccel) {
+ this.currentTurnConstraintMaxAngVel = maxAngVel;
+ this.currentTurnConstraintMaxAngAccel = maxAngAccel;
+
+ return this;
+ }
+
+ public TrajectorySequenceBuilder resetTurnConstraint() {
+ this.currentTurnConstraintMaxAngVel = baseTurnConstraintMaxAngVel;
+ this.currentTurnConstraintMaxAngAccel = baseTurnConstraintMaxAngAccel;
+
+ return this;
+ }
+
+ public TrajectorySequenceBuilder addTemporalMarker(MarkerCallback callback) {
+ return this.addTemporalMarker(currentDuration, callback);
+ }
+
+ public TrajectorySequenceBuilder UNSTABLE_addTemporalMarkerOffset(double offset, MarkerCallback callback) {
+ return this.addTemporalMarker(currentDuration + offset, callback);
+ }
+
+ public TrajectorySequenceBuilder addTemporalMarker(double time, MarkerCallback callback) {
+ return this.addTemporalMarker(0.0, time, callback);
+ }
+
+ public TrajectorySequenceBuilder addTemporalMarker(double scale, double offset, MarkerCallback callback) {
+ return this.addTemporalMarker(time -> scale * time + offset, callback);
+ }
+
+ public TrajectorySequenceBuilder addTemporalMarker(TimeProducer time, MarkerCallback callback) {
+ this.temporalMarkers.add(new TemporalMarker(time, callback));
+ return this;
+ }
+
+ public TrajectorySequenceBuilder addSpatialMarker(Vector2d point, MarkerCallback callback) {
+ this.spatialMarkers.add(new SpatialMarker(point, callback));
+ return this;
+ }
+
+ public TrajectorySequenceBuilder addDisplacementMarker(MarkerCallback callback) {
+ return this.addDisplacementMarker(currentDisplacement, callback);
+ }
+
+ public TrajectorySequenceBuilder UNSTABLE_addDisplacementMarkerOffset(double offset, MarkerCallback callback) {
+ return this.addDisplacementMarker(currentDisplacement + offset, callback);
+ }
+
+ public TrajectorySequenceBuilder addDisplacementMarker(double displacement, MarkerCallback callback) {
+ return this.addDisplacementMarker(0.0, displacement, callback);
+ }
+
+ public TrajectorySequenceBuilder addDisplacementMarker(double scale, double offset, MarkerCallback callback) {
+ return addDisplacementMarker((displacement -> scale * displacement + offset), callback);
+ }
+
+ public TrajectorySequenceBuilder addDisplacementMarker(DisplacementProducer displacement, MarkerCallback callback) {
+ displacementMarkers.add(new DisplacementMarker(displacement, callback));
+
+ return this;
+ }
+
+ public TrajectorySequenceBuilder turn(double angle) {
+ return turn(angle, currentTurnConstraintMaxAngVel, currentTurnConstraintMaxAngAccel);
+ }
+
+ public TrajectorySequenceBuilder turn(double angle, double maxAngVel, double maxAngAccel) {
+ pushPath();
+
+ MotionProfile turnProfile = MotionProfileGenerator.generateSimpleMotionProfile(
+ new MotionState(lastPose.getHeading(), 0.0, 0.0, 0.0),
+ new MotionState(lastPose.getHeading() + angle, 0.0, 0.0, 0.0),
+ maxAngVel,
+ maxAngAccel
+ );
+
+ sequenceSegments.add(new TurnSegment(lastPose, angle, turnProfile, Collections.emptyList()));
+
+ lastPose = new Pose2d(
+ lastPose.getX(), lastPose.getY(),
+ Angle.norm(lastPose.getHeading() + angle)
+ );
+
+ currentDuration += turnProfile.duration();
+
+ return this;
+ }
+
+ public TrajectorySequenceBuilder waitSeconds(double seconds) {
+ pushPath();
+ sequenceSegments.add(new WaitSegment(lastPose, seconds, Collections.emptyList()));
+
+ currentDuration += seconds;
+ return this;
+ }
+
+ public TrajectorySequenceBuilder addTrajectory(Trajectory trajectory) {
+ pushPath();
+
+ sequenceSegments.add(new TrajectorySegment(trajectory));
+ return this;
+ }
+
+ private void pushPath() {
+ if (currentTrajectoryBuilder != null) {
+ Trajectory builtTraj = currentTrajectoryBuilder.build();
+ sequenceSegments.add(new TrajectorySegment(builtTraj));
+ }
+
+ currentTrajectoryBuilder = null;
+ }
+
+ private void newPath() {
+ if (currentTrajectoryBuilder != null)
+ pushPath();
+
+ lastDurationTraj = 0.0;
+ lastDisplacementTraj = 0.0;
+
+ double tangent = setAbsoluteTangent ? absoluteTangent : Angle.norm(lastPose.getHeading() + tangentOffset);
+
+ currentTrajectoryBuilder = new TrajectoryBuilder(lastPose, tangent, currentVelConstraint, currentAccelConstraint, resolution);
+ }
+
+ public TrajectorySequence build() {
+ pushPath();
+
+ List globalMarkers = convertMarkersToGlobal(
+ sequenceSegments,
+ temporalMarkers, displacementMarkers, spatialMarkers
+ );
+ projectGlobalMarkersToLocalSegments(globalMarkers, sequenceSegments);
+
+ return new TrajectorySequence(sequenceSegments);
+ }
+
+ private List convertMarkersToGlobal(
+ List sequenceSegments,
+ List temporalMarkers,
+ List displacementMarkers,
+ List spatialMarkers
+ ) {
+ ArrayList trajectoryMarkers = new ArrayList<>();
+
+ // Convert temporal markers
+ for (TemporalMarker marker : temporalMarkers) {
+ trajectoryMarkers.add(
+ new TrajectoryMarker(marker.getProducer().produce(currentDuration), marker.getCallback())
+ );
+ }
+
+ // Convert displacement markers
+ for (DisplacementMarker marker : displacementMarkers) {
+ double time = displacementToTime(
+ sequenceSegments,
+ marker.getProducer().produce(currentDisplacement)
+ );
+
+ trajectoryMarkers.add(
+ new TrajectoryMarker(
+ time,
+ marker.getCallback()
+ )
+ );
+ }
+
+ // Convert spatial markers
+ for (SpatialMarker marker : spatialMarkers) {
+ trajectoryMarkers.add(
+ new TrajectoryMarker(
+ pointToTime(sequenceSegments, marker.getPoint()),
+ marker.getCallback()
+ )
+ );
+ }
+
+ return trajectoryMarkers;
+ }
+
+ private void projectGlobalMarkersToLocalSegments(List markers, List sequenceSegments) {
+ if (sequenceSegments.isEmpty()) return;
+
+ markers.sort(Comparator.comparingDouble(TrajectoryMarker::getTime));
+
+ double timeOffset = 0.0;
+ int markerIndex = 0;
+ for (SequenceSegment segment : sequenceSegments) {
+ while (markerIndex < markers.size()) {
+ TrajectoryMarker marker = markers.get(markerIndex);
+ if (marker.getTime() >= timeOffset + segment.getDuration()) {
+ break;
+ }
+
+ segment.getMarkers().add(new TrajectoryMarker(
+ Math.max(0.0, marker.getTime()) - timeOffset, marker.getCallback()));
+ ++markerIndex;
+ }
+
+ timeOffset += segment.getDuration();
+ }
+
+ SequenceSegment segment = sequenceSegments.get(sequenceSegments.size() - 1);
+ while (markerIndex < markers.size()) {
+ TrajectoryMarker marker = markers.get(markerIndex);
+ segment.getMarkers().add(new TrajectoryMarker(segment.getDuration(), marker.getCallback()));
+ ++markerIndex;
+ }
+ }
+
+ // Taken from Road Runner's TrajectoryGenerator.displacementToTime() since it's private
+ // note: this assumes that the profile position is monotonic increasing
+ private Double motionProfileDisplacementToTime(MotionProfile profile, double s) {
+ double tLo = 0.0;
+ double tHi = profile.duration();
+ while (!(Math.abs(tLo - tHi) < 1e-6)) {
+ double tMid = 0.5 * (tLo + tHi);
+ if (profile.get(tMid).getX() > s) {
+ tHi = tMid;
+ } else {
+ tLo = tMid;
+ }
+ }
+ return 0.5 * (tLo + tHi);
+ }
+
+ private Double displacementToTime(List sequenceSegments, double s) {
+ double currentTime = 0.0;
+ double currentDisplacement = 0.0;
+
+ for (SequenceSegment segment : sequenceSegments) {
+ if (segment instanceof TrajectorySegment) {
+ TrajectorySegment thisSegment = (TrajectorySegment) segment;
+
+ double segmentLength = thisSegment.getTrajectory().getPath().length();
+
+ if (currentDisplacement + segmentLength > s) {
+ double target = s - currentDisplacement;
+ double timeInSegment = motionProfileDisplacementToTime(
+ thisSegment.getTrajectory().getProfile(),
+ target
+ );
+
+ return currentTime + timeInSegment;
+ } else {
+ currentDisplacement += segmentLength;
+ }
+ }
+
+ currentTime += segment.getDuration();
+ }
+
+ return currentTime;
+ }
+
+ private Double pointToTime(List sequenceSegments, Vector2d point) {
+ class ComparingPoints {
+ private final double distanceToPoint;
+ private final double totalDisplacement;
+ private final double thisPathDisplacement;
+
+ public ComparingPoints(double distanceToPoint, double totalDisplacement, double thisPathDisplacement) {
+ this.distanceToPoint = distanceToPoint;
+ this.totalDisplacement = totalDisplacement;
+ this.thisPathDisplacement = thisPathDisplacement;
+ }
+ }
+
+ List projectedPoints = new ArrayList<>();
+
+ for (SequenceSegment segment : sequenceSegments) {
+ if (segment instanceof TrajectorySegment) {
+ TrajectorySegment thisSegment = (TrajectorySegment) segment;
+
+ double displacement = thisSegment.getTrajectory().getPath().project(point, 0.25);
+ Vector2d projectedPoint = thisSegment.getTrajectory().getPath().get(displacement).vec();
+ double distanceToPoint = point.minus(projectedPoint).norm();
+
+ double totalDisplacement = 0.0;
+
+ for (ComparingPoints comparingPoint : projectedPoints) {
+ totalDisplacement += comparingPoint.totalDisplacement;
+ }
+
+ totalDisplacement += displacement;
+
+ projectedPoints.add(new ComparingPoints(distanceToPoint, displacement, totalDisplacement));
+ }
+ }
+
+ ComparingPoints closestPoint = null;
+
+ for (ComparingPoints comparingPoint : projectedPoints) {
+ if (closestPoint == null) {
+ closestPoint = comparingPoint;
+ continue;
+ }
+
+ if (comparingPoint.distanceToPoint < closestPoint.distanceToPoint)
+ closestPoint = comparingPoint;
+ }
+
+ return displacementToTime(sequenceSegments, closestPoint.thisPathDisplacement);
+ }
+
+ private interface AddPathCallback {
+ void run();
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/TrajectorySequenceRunner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/TrajectorySequenceRunner.java
new file mode 100644
index 0000000..e05af9c
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/TrajectorySequenceRunner.java
@@ -0,0 +1,306 @@
+package org.firstinspires.ftc.teamcode.trajectorysequence;
+
+import androidx.annotation.Nullable;
+
+import com.acmerobotics.dashboard.FtcDashboard;
+import com.acmerobotics.dashboard.canvas.Canvas;
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
+import com.acmerobotics.roadrunner.control.PIDCoefficients;
+import com.acmerobotics.roadrunner.control.PIDFController;
+import com.acmerobotics.roadrunner.drive.DriveSignal;
+import com.acmerobotics.roadrunner.followers.TrajectoryFollower;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.profile.MotionState;
+import com.acmerobotics.roadrunner.trajectory.Trajectory;
+import com.acmerobotics.roadrunner.trajectory.TrajectoryMarker;
+import com.acmerobotics.roadrunner.util.NanoClock;
+import com.qualcomm.robotcore.hardware.VoltageSensor;
+
+import org.firstinspires.ftc.teamcode.drive.DriveConstants;
+import org.firstinspires.ftc.teamcode.trajectorysequence.sequencesegment.SequenceSegment;
+import org.firstinspires.ftc.teamcode.trajectorysequence.sequencesegment.TrajectorySegment;
+import org.firstinspires.ftc.teamcode.trajectorysequence.sequencesegment.TurnSegment;
+import org.firstinspires.ftc.teamcode.trajectorysequence.sequencesegment.WaitSegment;
+import org.firstinspires.ftc.teamcode.util.DashboardUtil;
+import org.firstinspires.ftc.teamcode.util.LogFiles;
+
+import java.util.ArrayList;
+import java.util.Collections;
+import java.util.LinkedList;
+import java.util.List;
+
+@Config
+public class TrajectorySequenceRunner {
+ public static String COLOR_INACTIVE_TRAJECTORY = "#4caf507a";
+ public static String COLOR_INACTIVE_TURN = "#7c4dff7a";
+ public static String COLOR_INACTIVE_WAIT = "#dd2c007a";
+
+ public static String COLOR_ACTIVE_TRAJECTORY = "#4CAF50";
+ public static String COLOR_ACTIVE_TURN = "#7c4dff";
+ public static String COLOR_ACTIVE_WAIT = "#dd2c00";
+
+ public static int POSE_HISTORY_LIMIT = 100;
+
+ private final TrajectoryFollower follower;
+
+ private final PIDFController turnController;
+
+ private final NanoClock clock;
+
+ private TrajectorySequence currentTrajectorySequence;
+ private double currentSegmentStartTime;
+ private int currentSegmentIndex;
+ private int lastSegmentIndex;
+
+ private Pose2d lastPoseError = new Pose2d();
+
+ List remainingMarkers = new ArrayList<>();
+
+ private final FtcDashboard dashboard;
+ private final LinkedList poseHistory = new LinkedList<>();
+
+ private VoltageSensor voltageSensor;
+
+ private List lastDriveEncPositions, lastDriveEncVels, lastTrackingEncPositions, lastTrackingEncVels;
+
+ public TrajectorySequenceRunner(
+ TrajectoryFollower follower, PIDCoefficients headingPIDCoefficients, VoltageSensor voltageSensor,
+ List lastDriveEncPositions, List lastDriveEncVels, List lastTrackingEncPositions, List lastTrackingEncVels
+ ) {
+ this.follower = follower;
+
+ turnController = new PIDFController(headingPIDCoefficients);
+ turnController.setInputBounds(0, 2 * Math.PI);
+
+ this.voltageSensor = voltageSensor;
+
+ this.lastDriveEncPositions = lastDriveEncPositions;
+ this.lastDriveEncVels = lastDriveEncVels;
+ this.lastTrackingEncPositions = lastTrackingEncPositions;
+ this.lastTrackingEncVels = lastTrackingEncVels;
+
+ clock = NanoClock.system();
+
+ dashboard = FtcDashboard.getInstance();
+ dashboard.setTelemetryTransmissionInterval(25);
+ }
+
+ public void followTrajectorySequenceAsync(TrajectorySequence trajectorySequence) {
+ currentTrajectorySequence = trajectorySequence;
+ currentSegmentStartTime = clock.seconds();
+ currentSegmentIndex = 0;
+ lastSegmentIndex = -1;
+ }
+
+ public @Nullable
+ DriveSignal update(Pose2d poseEstimate, Pose2d poseVelocity) {
+ Pose2d targetPose = null;
+ DriveSignal driveSignal = null;
+
+ TelemetryPacket packet = new TelemetryPacket();
+ Canvas fieldOverlay = packet.fieldOverlay();
+
+ SequenceSegment currentSegment = null;
+
+ if (currentTrajectorySequence != null) {
+ if (currentSegmentIndex >= currentTrajectorySequence.size()) {
+ for (TrajectoryMarker marker : remainingMarkers) {
+ marker.getCallback().onMarkerReached();
+ }
+
+ remainingMarkers.clear();
+
+ currentTrajectorySequence = null;
+ }
+
+ if (currentTrajectorySequence == null)
+ return new DriveSignal();
+
+ double now = clock.seconds();
+ boolean isNewTransition = currentSegmentIndex != lastSegmentIndex;
+
+ currentSegment = currentTrajectorySequence.get(currentSegmentIndex);
+
+ if (isNewTransition) {
+ currentSegmentStartTime = now;
+ lastSegmentIndex = currentSegmentIndex;
+
+ for (TrajectoryMarker marker : remainingMarkers) {
+ marker.getCallback().onMarkerReached();
+ }
+
+ remainingMarkers.clear();
+
+ remainingMarkers.addAll(currentSegment.getMarkers());
+ Collections.sort(remainingMarkers, (t1, t2) -> Double.compare(t1.getTime(), t2.getTime()));
+ }
+
+ double deltaTime = now - currentSegmentStartTime;
+
+ if (currentSegment instanceof TrajectorySegment) {
+ Trajectory currentTrajectory = ((TrajectorySegment) currentSegment).getTrajectory();
+
+ if (isNewTransition)
+ follower.followTrajectory(currentTrajectory);
+
+ if (!follower.isFollowing()) {
+ currentSegmentIndex++;
+
+ driveSignal = new DriveSignal();
+ } else {
+ driveSignal = follower.update(poseEstimate, poseVelocity);
+ lastPoseError = follower.getLastError();
+ }
+
+ targetPose = currentTrajectory.get(deltaTime);
+ } else if (currentSegment instanceof TurnSegment) {
+ MotionState targetState = ((TurnSegment) currentSegment).getMotionProfile().get(deltaTime);
+
+ turnController.setTargetPosition(targetState.getX());
+
+ double correction = turnController.update(poseEstimate.getHeading());
+
+ double targetOmega = targetState.getV();
+ double targetAlpha = targetState.getA();
+
+ lastPoseError = new Pose2d(0, 0, turnController.getLastError());
+
+ Pose2d startPose = currentSegment.getStartPose();
+ targetPose = startPose.copy(startPose.getX(), startPose.getY(), targetState.getX());
+
+ driveSignal = new DriveSignal(
+ new Pose2d(0, 0, targetOmega + correction),
+ new Pose2d(0, 0, targetAlpha)
+ );
+
+ if (deltaTime >= currentSegment.getDuration()) {
+ currentSegmentIndex++;
+ driveSignal = new DriveSignal();
+ }
+ } else if (currentSegment instanceof WaitSegment) {
+ lastPoseError = new Pose2d();
+
+ targetPose = currentSegment.getStartPose();
+ driveSignal = new DriveSignal();
+
+ if (deltaTime >= currentSegment.getDuration()) {
+ currentSegmentIndex++;
+ }
+ }
+
+ while (remainingMarkers.size() > 0 && deltaTime > remainingMarkers.get(0).getTime()) {
+ remainingMarkers.get(0).getCallback().onMarkerReached();
+ remainingMarkers.remove(0);
+ }
+ }
+
+ poseHistory.add(poseEstimate);
+
+ if (POSE_HISTORY_LIMIT > -1 && poseHistory.size() > POSE_HISTORY_LIMIT) {
+ poseHistory.removeFirst();
+ }
+
+ final double NOMINAL_VOLTAGE = 12.0;
+ double voltage = voltageSensor.getVoltage();
+ if (driveSignal != null && !DriveConstants.RUN_USING_ENCODER) {
+ driveSignal = new DriveSignal(
+ driveSignal.getVel().times(NOMINAL_VOLTAGE / voltage),
+ driveSignal.getAccel().times(NOMINAL_VOLTAGE / voltage)
+ );
+ }
+
+ if (targetPose != null) {
+ LogFiles.record(
+ targetPose, poseEstimate, voltage,
+ lastDriveEncPositions, lastDriveEncVels, lastTrackingEncPositions, lastTrackingEncVels
+ );
+ }
+
+ packet.put("x", poseEstimate.getX());
+ packet.put("y", poseEstimate.getY());
+ packet.put("heading (deg)", Math.toDegrees(poseEstimate.getHeading()));
+
+ packet.put("xError", getLastPoseError().getX());
+ packet.put("yError", getLastPoseError().getY());
+ packet.put("headingError (deg)", Math.toDegrees(getLastPoseError().getHeading()));
+
+ draw(fieldOverlay, currentTrajectorySequence, currentSegment, targetPose, poseEstimate);
+
+ dashboard.sendTelemetryPacket(packet);
+
+ return driveSignal;
+ }
+
+ private void draw(
+ Canvas fieldOverlay,
+ TrajectorySequence sequence, SequenceSegment currentSegment,
+ Pose2d targetPose, Pose2d poseEstimate
+ ) {
+ if (sequence != null) {
+ for (int i = 0; i < sequence.size(); i++) {
+ SequenceSegment segment = sequence.get(i);
+
+ if (segment instanceof TrajectorySegment) {
+ fieldOverlay.setStrokeWidth(1);
+ fieldOverlay.setStroke(COLOR_INACTIVE_TRAJECTORY);
+
+ DashboardUtil.drawSampledPath(fieldOverlay, ((TrajectorySegment) segment).getTrajectory().getPath());
+ } else if (segment instanceof TurnSegment) {
+ Pose2d pose = segment.getStartPose();
+
+ fieldOverlay.setFill(COLOR_INACTIVE_TURN);
+ fieldOverlay.fillCircle(pose.getX(), pose.getY(), 2);
+ } else if (segment instanceof WaitSegment) {
+ Pose2d pose = segment.getStartPose();
+
+ fieldOverlay.setStrokeWidth(1);
+ fieldOverlay.setStroke(COLOR_INACTIVE_WAIT);
+ fieldOverlay.strokeCircle(pose.getX(), pose.getY(), 3);
+ }
+ }
+ }
+
+ if (currentSegment != null) {
+ if (currentSegment instanceof TrajectorySegment) {
+ Trajectory currentTrajectory = ((TrajectorySegment) currentSegment).getTrajectory();
+
+ fieldOverlay.setStrokeWidth(1);
+ fieldOverlay.setStroke(COLOR_ACTIVE_TRAJECTORY);
+
+ DashboardUtil.drawSampledPath(fieldOverlay, currentTrajectory.getPath());
+ } else if (currentSegment instanceof TurnSegment) {
+ Pose2d pose = currentSegment.getStartPose();
+
+ fieldOverlay.setFill(COLOR_ACTIVE_TURN);
+ fieldOverlay.fillCircle(pose.getX(), pose.getY(), 3);
+ } else if (currentSegment instanceof WaitSegment) {
+ Pose2d pose = currentSegment.getStartPose();
+
+ fieldOverlay.setStrokeWidth(1);
+ fieldOverlay.setStroke(COLOR_ACTIVE_WAIT);
+ fieldOverlay.strokeCircle(pose.getX(), pose.getY(), 3);
+ }
+ }
+
+ if (targetPose != null) {
+ fieldOverlay.setStrokeWidth(1);
+ fieldOverlay.setStroke("#4CAF50");
+ DashboardUtil.drawRobot(fieldOverlay, targetPose);
+ }
+
+ fieldOverlay.setStroke("#3F51B5");
+ DashboardUtil.drawPoseHistory(fieldOverlay, poseHistory);
+
+ fieldOverlay.setStroke("#3F51B5");
+ DashboardUtil.drawRobot(fieldOverlay, poseEstimate);
+ }
+
+ public Pose2d getLastPoseError() {
+ return lastPoseError;
+ }
+
+ public boolean isBusy() {
+ return currentTrajectorySequence != null;
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/sequencesegment/SequenceSegment.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/sequencesegment/SequenceSegment.java
new file mode 100644
index 0000000..84bccfe
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/sequencesegment/SequenceSegment.java
@@ -0,0 +1,41 @@
+package org.firstinspires.ftc.teamcode.trajectorysequence.sequencesegment;
+
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.trajectory.TrajectoryMarker;
+
+import java.util.ArrayList;
+import java.util.List;
+
+public abstract class SequenceSegment {
+ private final double duration;
+ private final Pose2d startPose;
+ private final Pose2d endPose;
+ private final List markers;
+
+ protected SequenceSegment(
+ double duration,
+ Pose2d startPose, Pose2d endPose,
+ List markers
+ ) {
+ this.duration = duration;
+ this.startPose = startPose;
+ this.endPose = endPose;
+ this.markers = new ArrayList<>(markers);
+ }
+
+ public double getDuration() {
+ return this.duration;
+ }
+
+ public Pose2d getStartPose() {
+ return startPose;
+ }
+
+ public Pose2d getEndPose() {
+ return endPose;
+ }
+
+ public List getMarkers() {
+ return markers;
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/sequencesegment/TrajectorySegment.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/sequencesegment/TrajectorySegment.java
new file mode 100644
index 0000000..2e735d4
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/sequencesegment/TrajectorySegment.java
@@ -0,0 +1,20 @@
+package org.firstinspires.ftc.teamcode.trajectorysequence.sequencesegment;
+
+import com.acmerobotics.roadrunner.trajectory.Trajectory;
+
+import java.util.Collections;
+
+public final class TrajectorySegment extends SequenceSegment {
+ private final Trajectory trajectory;
+
+ public TrajectorySegment(Trajectory trajectory) {
+ // Note: Markers are already stored in the `Trajectory` itself.
+ // This class should not hold any markers
+ super(trajectory.duration(), trajectory.start(), trajectory.end(), Collections.emptyList());
+ this.trajectory = trajectory;
+ }
+
+ public Trajectory getTrajectory() {
+ return this.trajectory;
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/sequencesegment/TurnSegment.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/sequencesegment/TurnSegment.java
new file mode 100644
index 0000000..11995db
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/sequencesegment/TurnSegment.java
@@ -0,0 +1,36 @@
+package org.firstinspires.ftc.teamcode.trajectorysequence.sequencesegment;
+
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.profile.MotionProfile;
+import com.acmerobotics.roadrunner.trajectory.TrajectoryMarker;
+import com.acmerobotics.roadrunner.util.Angle;
+
+import java.util.List;
+
+public final class TurnSegment extends SequenceSegment {
+ private final double totalRotation;
+ private final MotionProfile motionProfile;
+
+ public TurnSegment(Pose2d startPose, double totalRotation, MotionProfile motionProfile, List markers) {
+ super(
+ motionProfile.duration(),
+ startPose,
+ new Pose2d(
+ startPose.getX(), startPose.getY(),
+ Angle.norm(startPose.getHeading() + totalRotation)
+ ),
+ markers
+ );
+
+ this.totalRotation = totalRotation;
+ this.motionProfile = motionProfile;
+ }
+
+ public final double getTotalRotation() {
+ return this.totalRotation;
+ }
+
+ public final MotionProfile getMotionProfile() {
+ return this.motionProfile;
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/sequencesegment/WaitSegment.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/sequencesegment/WaitSegment.java
new file mode 100644
index 0000000..62d2ba4
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/trajectorysequence/sequencesegment/WaitSegment.java
@@ -0,0 +1,12 @@
+package org.firstinspires.ftc.teamcode.trajectorysequence.sequencesegment;
+
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.trajectory.TrajectoryMarker;
+
+import java.util.List;
+
+public final class WaitSegment extends SequenceSegment {
+ public WaitSegment(Pose2d pose, double seconds, List markers) {
+ super(seconds, pose, pose, markers);
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/AssetsTrajectoryManager.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/AssetsTrajectoryManager.java
new file mode 100644
index 0000000..1b34a95
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/AssetsTrajectoryManager.java
@@ -0,0 +1,70 @@
+package org.firstinspires.ftc.teamcode.util;
+
+import androidx.annotation.Nullable;
+
+import com.acmerobotics.roadrunner.trajectory.Trajectory;
+import com.acmerobotics.roadrunner.trajectory.TrajectoryBuilder;
+import com.acmerobotics.roadrunner.trajectory.config.TrajectoryConfig;
+import com.acmerobotics.roadrunner.trajectory.config.TrajectoryConfigManager;
+import com.acmerobotics.roadrunner.trajectory.config.TrajectoryGroupConfig;
+
+import org.firstinspires.ftc.robotcore.internal.system.AppUtil;
+
+import java.io.IOException;
+import java.io.InputStream;
+
+/**
+ * Set of utilities for loading trajectories from assets (the plugin save location).
+ */
+public class AssetsTrajectoryManager {
+
+ /**
+ * Loads the group config.
+ */
+ public static @Nullable
+ TrajectoryGroupConfig loadGroupConfig() {
+ try {
+ InputStream inputStream = AppUtil.getDefContext().getAssets().open(
+ "trajectory/" + TrajectoryConfigManager.GROUP_FILENAME);
+ return TrajectoryConfigManager.loadGroupConfig(inputStream);
+ } catch (IOException e) {
+ return null;
+ }
+ }
+
+ /**
+ * Loads a trajectory config with the given name.
+ */
+ public static @Nullable TrajectoryConfig loadConfig(String name) {
+ try {
+ InputStream inputStream = AppUtil.getDefContext().getAssets().open(
+ "trajectory/" + name + ".yaml");
+ return TrajectoryConfigManager.loadConfig(inputStream);
+ } catch (IOException e) {
+ return null;
+ }
+ }
+
+ /**
+ * Loads a trajectory builder with the given name.
+ */
+ public static @Nullable TrajectoryBuilder loadBuilder(String name) {
+ TrajectoryGroupConfig groupConfig = loadGroupConfig();
+ TrajectoryConfig config = loadConfig(name);
+ if (groupConfig == null || config == null) {
+ return null;
+ }
+ return config.toTrajectoryBuilder(groupConfig);
+ }
+
+ /**
+ * Loads a trajectory with the given name.
+ */
+ public static @Nullable Trajectory load(String name) {
+ TrajectoryBuilder builder = loadBuilder(name);
+ if (builder == null) {
+ return null;
+ }
+ return builder.build();
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/AxesSigns.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/AxesSigns.java
new file mode 100644
index 0000000..42271d6
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/AxesSigns.java
@@ -0,0 +1,45 @@
+package org.firstinspires.ftc.teamcode.util;
+
+/**
+ * IMU axes signs in the order XYZ (after remapping).
+ */
+public enum AxesSigns {
+ PPP(0b000),
+ PPN(0b001),
+ PNP(0b010),
+ PNN(0b011),
+ NPP(0b100),
+ NPN(0b101),
+ NNP(0b110),
+ NNN(0b111);
+
+ public final int bVal;
+
+ AxesSigns(int bVal) {
+ this.bVal = bVal;
+ }
+
+ public static AxesSigns fromBinaryValue(int bVal) {
+ int maskedVal = bVal & 0x07;
+ switch (maskedVal) {
+ case 0b000:
+ return AxesSigns.PPP;
+ case 0b001:
+ return AxesSigns.PPN;
+ case 0b010:
+ return AxesSigns.PNP;
+ case 0b011:
+ return AxesSigns.PNN;
+ case 0b100:
+ return AxesSigns.NPP;
+ case 0b101:
+ return AxesSigns.NPN;
+ case 0b110:
+ return AxesSigns.NNP;
+ case 0b111:
+ return AxesSigns.NNN;
+ default:
+ throw new IllegalStateException("Unexpected value for maskedVal: " + maskedVal);
+ }
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/AxisDirection.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/AxisDirection.java
new file mode 100644
index 0000000..5b8279b
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/AxisDirection.java
@@ -0,0 +1,8 @@
+package org.firstinspires.ftc.teamcode.util;
+
+/**
+ * A direction for an axis to be remapped to
+ */
+public enum AxisDirection {
+ POS_X, NEG_X, POS_Y, NEG_Y, POS_Z, NEG_Z
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/DashboardUtil.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/DashboardUtil.java
new file mode 100644
index 0000000..ce11d8d
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/DashboardUtil.java
@@ -0,0 +1,54 @@
+package org.firstinspires.ftc.teamcode.util;
+
+import com.acmerobotics.dashboard.canvas.Canvas;
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.acmerobotics.roadrunner.geometry.Vector2d;
+import com.acmerobotics.roadrunner.path.Path;
+
+import java.util.List;
+
+/**
+ * Set of helper functions for drawing Road Runner paths and trajectories on dashboard canvases.
+ */
+public class DashboardUtil {
+ private static final double DEFAULT_RESOLUTION = 2.0; // distance units; presumed inches
+ private static final double ROBOT_RADIUS = 9; // in
+
+
+ public static void drawPoseHistory(Canvas canvas, List poseHistory) {
+ double[] xPoints = new double[poseHistory.size()];
+ double[] yPoints = new double[poseHistory.size()];
+ for (int i = 0; i < poseHistory.size(); i++) {
+ Pose2d pose = poseHistory.get(i);
+ xPoints[i] = pose.getX();
+ yPoints[i] = pose.getY();
+ }
+ canvas.strokePolyline(xPoints, yPoints);
+ }
+
+ public static void drawSampledPath(Canvas canvas, Path path, double resolution) {
+ int samples = (int) Math.ceil(path.length() / resolution);
+ double[] xPoints = new double[samples];
+ double[] yPoints = new double[samples];
+ double dx = path.length() / (samples - 1);
+ for (int i = 0; i < samples; i++) {
+ double displacement = i * dx;
+ Pose2d pose = path.get(displacement);
+ xPoints[i] = pose.getX();
+ yPoints[i] = pose.getY();
+ }
+ canvas.strokePolyline(xPoints, yPoints);
+ }
+
+ public static void drawSampledPath(Canvas canvas, Path path) {
+ drawSampledPath(canvas, path, DEFAULT_RESOLUTION);
+ }
+
+ public static void drawRobot(Canvas canvas, Pose2d pose) {
+ canvas.strokeCircle(pose.getX(), pose.getY(), ROBOT_RADIUS);
+ Vector2d v = pose.headingVec().times(ROBOT_RADIUS);
+ double x1 = pose.getX() + v.getX() / 2, y1 = pose.getY() + v.getY() / 2;
+ double x2 = pose.getX() + v.getX(), y2 = pose.getY() + v.getY();
+ canvas.strokeLine(x1, y1, x2, y2);
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Encoder.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Encoder.java
new file mode 100644
index 0000000..f724c52
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Encoder.java
@@ -0,0 +1,125 @@
+package org.firstinspires.ftc.teamcode.util;
+
+import com.acmerobotics.roadrunner.util.NanoClock;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
+import com.qualcomm.robotcore.hardware.DcMotorSimple;
+
+/**
+ * Wraps a motor instance to provide corrected velocity counts and allow reversing independently of the corresponding
+ * slot's motor direction
+ */
+public class Encoder {
+ private final static int CPS_STEP = 0x10000;
+
+ private static double inverseOverflow(double input, double estimate) {
+ // convert to uint16
+ int real = (int) input & 0xffff;
+ // initial, modulo-based correction: it can recover the remainder of 5 of the upper 16 bits
+ // because the velocity is always a multiple of 20 cps due to Expansion Hub's 50ms measurement window
+ real += ((real % 20) / 4) * CPS_STEP;
+ // estimate-based correction: it finds the nearest multiple of 5 to correct the upper bits by
+ real += Math.round((estimate - real) / (5 * CPS_STEP)) * 5 * CPS_STEP;
+ return real;
+ }
+
+ public enum Direction {
+ FORWARD(1),
+ REVERSE(-1);
+
+ private int multiplier;
+
+ Direction(int multiplier) {
+ this.multiplier = multiplier;
+ }
+
+ public int getMultiplier() {
+ return multiplier;
+ }
+ }
+
+ private DcMotorEx motor;
+ private NanoClock clock;
+
+ private Direction direction;
+
+ private int lastPosition;
+ private int velocityEstimateIdx;
+ private double[] velocityEstimates;
+ private double lastUpdateTime;
+
+ public Encoder(DcMotorEx motor, NanoClock clock) {
+ this.motor = motor;
+ this.clock = clock;
+
+ this.direction = Direction.FORWARD;
+
+ this.lastPosition = 0;
+ this.velocityEstimates = new double[3];
+ this.lastUpdateTime = clock.seconds();
+ }
+
+ public Encoder(DcMotorEx motor) {
+ this(motor, NanoClock.system());
+ }
+
+ public Direction getDirection() {
+ return direction;
+ }
+
+ private int getMultiplier() {
+ return getDirection().getMultiplier() * (motor.getDirection() == DcMotorSimple.Direction.FORWARD ? 1 : -1);
+ }
+
+ /**
+ * Allows you to set the direction of the counts and velocity without modifying the motor's direction state
+ * @param direction either reverse or forward depending on if encoder counts should be negated
+ */
+ public void setDirection(Direction direction) {
+ this.direction = direction;
+ }
+
+ /**
+ * Gets the position from the underlying motor and adjusts for the set direction.
+ * Additionally, this method updates the velocity estimates used for compensated velocity
+ *
+ * @return encoder position
+ */
+ public int getCurrentPosition() {
+ int multiplier = getMultiplier();
+ int currentPosition = motor.getCurrentPosition() * multiplier;
+ if (currentPosition != lastPosition) {
+ double currentTime = clock.seconds();
+ double dt = currentTime - lastUpdateTime;
+ velocityEstimates[velocityEstimateIdx] = (currentPosition - lastPosition) / dt;
+ velocityEstimateIdx = (velocityEstimateIdx + 1) % 3;
+ lastPosition = currentPosition;
+ lastUpdateTime = currentTime;
+ }
+ return currentPosition;
+ }
+
+ /**
+ * Gets the velocity directly from the underlying motor and compensates for the direction
+ * See {@link #getCorrectedVelocity} for high (>2^15) counts per second velocities (such as on REV Through Bore)
+ *
+ * @return raw velocity
+ */
+ public double getRawVelocity() {
+ int multiplier = getMultiplier();
+ return motor.getVelocity() * multiplier;
+ }
+
+ /**
+ * Uses velocity estimates gathered in {@link #getCurrentPosition} to estimate the upper bits of velocity
+ * that are lost in overflow due to velocity being transmitted as 16 bits.
+ * CAVEAT: must regularly call {@link #getCurrentPosition} for the compensation to work correctly.
+ *
+ * @return corrected velocity
+ */
+ public double getCorrectedVelocity() {
+ double median = velocityEstimates[0] > velocityEstimates[1]
+ ? Math.max(velocityEstimates[1], Math.min(velocityEstimates[0], velocityEstimates[2]))
+ : Math.max(velocityEstimates[0], Math.min(velocityEstimates[1], velocityEstimates[2]));
+ return inverseOverflow(getRawVelocity(), median);
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/LogFiles.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/LogFiles.java
new file mode 100644
index 0000000..8338eb1
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/LogFiles.java
@@ -0,0 +1,273 @@
+package org.firstinspires.ftc.teamcode.util;
+
+import android.annotation.SuppressLint;
+import android.content.Context;
+
+import com.acmerobotics.roadrunner.geometry.Pose2d;
+import com.fasterxml.jackson.core.JsonFactory;
+import com.fasterxml.jackson.databind.ObjectMapper;
+import com.fasterxml.jackson.databind.ObjectWriter;
+import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.eventloop.opmode.OpModeManagerImpl;
+import com.qualcomm.robotcore.eventloop.opmode.OpModeManagerNotifier;
+import com.qualcomm.robotcore.util.RobotLog;
+import com.qualcomm.robotcore.util.WebHandlerManager;
+
+import org.firstinspires.ftc.ftccommon.external.WebHandlerRegistrar;
+import org.firstinspires.ftc.robotcore.internal.system.AppUtil;
+import org.firstinspires.ftc.teamcode.drive.DriveConstants;
+import org.firstinspires.ftc.teamcode.drive.SampleMecanumDrive;
+import org.firstinspires.ftc.teamcode.drive.SampleTankDrive;
+import org.firstinspires.ftc.teamcode.drive.StandardTrackingWheelLocalizer;
+
+import java.io.File;
+import java.io.FileInputStream;
+import java.io.IOException;
+import java.text.DateFormat;
+import java.text.SimpleDateFormat;
+import java.util.ArrayList;
+import java.util.Arrays;
+import java.util.Date;
+import java.util.List;
+import java.util.Objects;
+
+import fi.iki.elonen.NanoHTTPD;
+
+public final class LogFiles {
+ private static final File ROOT =
+ new File(AppUtil.ROOT_FOLDER + "/RoadRunner/logs/");
+
+ public static LogFile log = new LogFile("uninitialized");
+
+ public static class LogFile {
+ public String version = "quickstart1 v2";
+
+ public String opModeName;
+ public long msInit = System.currentTimeMillis();
+ public long nsInit = System.nanoTime();
+ public long nsStart, nsStop;
+
+ public double ticksPerRev = DriveConstants.TICKS_PER_REV;
+ public double maxRpm = DriveConstants.MAX_RPM;
+ public boolean runUsingEncoder = DriveConstants.RUN_USING_ENCODER;
+ public double motorP = DriveConstants.MOTOR_VELO_PID.p;
+ public double motorI = DriveConstants.MOTOR_VELO_PID.i;
+ public double motorD = DriveConstants.MOTOR_VELO_PID.d;
+ public double motorF = DriveConstants.MOTOR_VELO_PID.f;
+ public double wheelRadius = DriveConstants.WHEEL_RADIUS;
+ public double gearRatio = DriveConstants.GEAR_RATIO;
+ public double trackWidth = DriveConstants.TRACK_WIDTH;
+ public double kV = DriveConstants.kV;
+ public double kA = DriveConstants.kA;
+ public double kStatic = DriveConstants.kStatic;
+ public double maxVel = DriveConstants.MAX_VEL;
+ public double maxAccel = DriveConstants.MAX_ACCEL;
+ public double maxAngVel = DriveConstants.MAX_ANG_VEL;
+ public double maxAngAccel = DriveConstants.MAX_ANG_ACCEL;
+
+ public double mecTransP = SampleMecanumDrive.TRANSLATIONAL_PID.kP;
+ public double mecTransI = SampleMecanumDrive.TRANSLATIONAL_PID.kI;
+ public double mecTransD = SampleMecanumDrive.TRANSLATIONAL_PID.kD;
+ public double mecHeadingP = SampleMecanumDrive.HEADING_PID.kP;
+ public double mecHeadingI = SampleMecanumDrive.HEADING_PID.kI;
+ public double mecHeadingD = SampleMecanumDrive.HEADING_PID.kD;
+ public double mecLateralMultiplier = SampleMecanumDrive.LATERAL_MULTIPLIER;
+
+ public double tankAxialP = SampleTankDrive.AXIAL_PID.kP;
+ public double tankAxialI = SampleTankDrive.AXIAL_PID.kI;
+ public double tankAxialD = SampleTankDrive.AXIAL_PID.kD;
+ public double tankCrossTrackP = SampleTankDrive.CROSS_TRACK_PID.kP;
+ public double tankCrossTrackI = SampleTankDrive.CROSS_TRACK_PID.kI;
+ public double tankCrossTrackD = SampleTankDrive.CROSS_TRACK_PID.kD;
+ public double tankHeadingP = SampleTankDrive.HEADING_PID.kP;
+ public double tankHeadingI = SampleTankDrive.HEADING_PID.kI;
+ public double tankHeadingD = SampleTankDrive.HEADING_PID.kD;
+
+ public double trackingTicksPerRev = StandardTrackingWheelLocalizer.TICKS_PER_REV;
+ public double trackingWheelRadius = StandardTrackingWheelLocalizer.WHEEL_RADIUS;
+ public double trackingGearRatio = StandardTrackingWheelLocalizer.GEAR_RATIO;
+ public double trackingLateralDistance = StandardTrackingWheelLocalizer.LATERAL_DISTANCE;
+ public double trackingForwardOffset = StandardTrackingWheelLocalizer.FORWARD_OFFSET;
+
+ public RevHubOrientationOnRobot.LogoFacingDirection LOGO_FACING_DIR = DriveConstants.LOGO_FACING_DIR;
+ public RevHubOrientationOnRobot.UsbFacingDirection USB_FACING_DIR = DriveConstants.USB_FACING_DIR;
+
+ public List nsTimes = new ArrayList<>();
+
+ public List targetXs = new ArrayList<>();
+ public List targetYs = new ArrayList<>();
+ public List targetHeadings = new ArrayList<>();
+
+ public List xs = new ArrayList<>();
+ public List ys = new ArrayList<>();
+ public List headings = new ArrayList<>();
+
+ public List voltages = new ArrayList<>();
+
+ public List> driveEncPositions = new ArrayList<>();
+ public List> driveEncVels = new ArrayList<>();
+ public List> trackingEncPositions = new ArrayList<>();
+ public List> trackingEncVels = new ArrayList<>();
+
+ public LogFile(String opModeName) {
+ this.opModeName = opModeName;
+ }
+ }
+
+ public static void record(
+ Pose2d targetPose, Pose2d pose, double voltage,
+ List lastDriveEncPositions, List lastDriveEncVels, List lastTrackingEncPositions, List lastTrackingEncVels
+ ) {
+ long nsTime = System.nanoTime();
+ if (nsTime - log.nsStart > 3 * 60 * 1_000_000_000L) {
+ return;
+ }
+
+ log.nsTimes.add(nsTime);
+
+ log.targetXs.add(targetPose.getX());
+ log.targetYs.add(targetPose.getY());
+ log.targetHeadings.add(targetPose.getHeading());
+
+ log.xs.add(pose.getX());
+ log.ys.add(pose.getY());
+ log.headings.add(pose.getHeading());
+
+ log.voltages.add(voltage);
+
+ while (log.driveEncPositions.size() < lastDriveEncPositions.size()) {
+ log.driveEncPositions.add(new ArrayList<>());
+ }
+ while (log.driveEncVels.size() < lastDriveEncVels.size()) {
+ log.driveEncVels.add(new ArrayList<>());
+ }
+ while (log.trackingEncPositions.size() < lastTrackingEncPositions.size()) {
+ log.trackingEncPositions.add(new ArrayList<>());
+ }
+ while (log.trackingEncVels.size() < lastTrackingEncVels.size()) {
+ log.trackingEncVels.add(new ArrayList<>());
+ }
+
+ for (int i = 0; i < lastDriveEncPositions.size(); i++) {
+ log.driveEncPositions.get(i).add(lastDriveEncPositions.get(i));
+ }
+ for (int i = 0; i < lastDriveEncVels.size(); i++) {
+ log.driveEncVels.get(i).add(lastDriveEncVels.get(i));
+ }
+ for (int i = 0; i < lastTrackingEncPositions.size(); i++) {
+ log.trackingEncPositions.get(i).add(lastTrackingEncPositions.get(i));
+ }
+ for (int i = 0; i < lastTrackingEncVels.size(); i++) {
+ log.trackingEncVels.get(i).add(lastTrackingEncVels.get(i));
+ }
+ }
+
+ private static final OpModeManagerNotifier.Notifications notifHandler = new OpModeManagerNotifier.Notifications() {
+ @SuppressLint("SimpleDateFormat")
+ final DateFormat dateFormat = new SimpleDateFormat("yyyy_MM_dd__HH_mm_ss_SSS");
+
+ final ObjectWriter jsonWriter = new ObjectMapper(new JsonFactory())
+ .writerWithDefaultPrettyPrinter();
+
+ @Override
+ public void onOpModePreInit(OpMode opMode) {
+ log = new LogFile(opMode.getClass().getCanonicalName());
+
+ // clean up old files
+ File[] fs = Objects.requireNonNull(ROOT.listFiles());
+ Arrays.sort(fs, (a, b) -> Long.compare(a.lastModified(), b.lastModified()));
+ long totalSizeBytes = 0;
+ for (File f : fs) {
+ totalSizeBytes += f.length();
+ }
+
+ int i = 0;
+ while (i < fs.length && totalSizeBytes >= 32 * 1000 * 1000) {
+ totalSizeBytes -= fs[i].length();
+ if (!fs[i].delete()) {
+ RobotLog.setGlobalErrorMsg("Unable to delete file " + fs[i].getAbsolutePath());
+ }
+ ++i;
+ }
+ }
+
+ @Override
+ public void onOpModePreStart(OpMode opMode) {
+ log.nsStart = System.nanoTime();
+ }
+
+ @Override
+ public void onOpModePostStop(OpMode opMode) {
+ log.nsStop = System.nanoTime();
+
+ if (!(opMode instanceof OpModeManagerImpl.DefaultOpMode)) {
+ //noinspection ResultOfMethodCallIgnored
+ ROOT.mkdirs();
+
+ String filename = dateFormat.format(new Date(log.msInit)) + "__" + opMode.getClass().getSimpleName() + ".json";
+ File file = new File(ROOT, filename);
+ try {
+ jsonWriter.writeValue(file, log);
+ } catch (IOException e) {
+ RobotLog.setGlobalErrorMsg(new RuntimeException(e),
+ "Unable to write data to " + file.getAbsolutePath());
+ }
+ }
+ }
+ };
+
+ @WebHandlerRegistrar
+ public static void registerRoutes(Context context, WebHandlerManager manager) {
+ //noinspection ResultOfMethodCallIgnored
+ ROOT.mkdirs();
+
+ // op mode manager only stores a weak reference, so we need to keep notifHandler alive ourselves
+ // don't use @OnCreateEventLoop because it's unreliable
+ OpModeManagerImpl.getOpModeManagerOfActivity(
+ AppUtil.getInstance().getActivity()
+ ).registerListener(notifHandler);
+
+ manager.register("/logs", session -> {
+ final StringBuilder sb = new StringBuilder();
+ sb.append("Logs");
+ File[] fs = Objects.requireNonNull(ROOT.listFiles());
+ Arrays.sort(fs, (a, b) -> Long.compare(b.lastModified(), a.lastModified()));
+ for (File f : fs) {
+ sb.append("- ");
+ sb.append(f.getName());
+ sb.append("
");
+ }
+ sb.append("
");
+ return NanoHTTPD.newFixedLengthResponse(NanoHTTPD.Response.Status.OK,
+ NanoHTTPD.MIME_HTML, sb.toString());
+ });
+
+ manager.register("/logs/download", session -> {
+ final String[] pairs = session.getQueryParameterString().split("&");
+ if (pairs.length != 1) {
+ return NanoHTTPD.newFixedLengthResponse(NanoHTTPD.Response.Status.BAD_REQUEST,
+ NanoHTTPD.MIME_PLAINTEXT, "expected one query parameter, got " + pairs.length);
+ }
+
+ final String[] parts = pairs[0].split("=");
+ if (!parts[0].equals("file")) {
+ return NanoHTTPD.newFixedLengthResponse(NanoHTTPD.Response.Status.BAD_REQUEST,
+ NanoHTTPD.MIME_PLAINTEXT, "expected file query parameter, got " + parts[0]);
+ }
+
+ File f = new File(ROOT, parts[1]);
+ if (!f.exists()) {
+ return NanoHTTPD.newFixedLengthResponse(NanoHTTPD.Response.Status.NOT_FOUND,
+ NanoHTTPD.MIME_PLAINTEXT, "file " + f + " doesn't exist");
+ }
+
+ return NanoHTTPD.newChunkedResponse(NanoHTTPD.Response.Status.OK,
+ "application/json", new FileInputStream(f));
+ });
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/LoggingUtil.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/LoggingUtil.java
new file mode 100644
index 0000000..2c558f1
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/LoggingUtil.java
@@ -0,0 +1,60 @@
+package org.firstinspires.ftc.teamcode.util;
+
+import org.firstinspires.ftc.robotcore.internal.system.AppUtil;
+
+import java.io.File;
+import java.util.ArrayList;
+import java.util.Collections;
+import java.util.List;
+
+/**
+ * Utility functions for log files.
+ */
+public class LoggingUtil {
+ public static final File ROAD_RUNNER_FOLDER =
+ new File(AppUtil.ROOT_FOLDER + "/RoadRunner/");
+
+ private static final long LOG_QUOTA = 25 * 1024 * 1024; // 25MB log quota for now
+
+ private static void buildLogList(List logFiles, File dir) {
+ for (File file : dir.listFiles()) {
+ if (file.isDirectory()) {
+ buildLogList(logFiles, file);
+ } else {
+ logFiles.add(file);
+ }
+ }
+ }
+
+ private static void pruneLogsIfNecessary() {
+ List logFiles = new ArrayList<>();
+ buildLogList(logFiles, ROAD_RUNNER_FOLDER);
+ Collections.sort(logFiles, (lhs, rhs) ->
+ Long.compare(lhs.lastModified(), rhs.lastModified()));
+
+ long dirSize = 0;
+ for (File file: logFiles) {
+ dirSize += file.length();
+ }
+
+ while (dirSize > LOG_QUOTA) {
+ if (logFiles.size() == 0) break;
+ File fileToRemove = logFiles.remove(0);
+ dirSize -= fileToRemove.length();
+ //noinspection ResultOfMethodCallIgnored
+ fileToRemove.delete();
+ }
+ }
+
+ /**
+ * Obtain a log file with the provided name
+ */
+ public static File getLogFile(String name) {
+ //noinspection ResultOfMethodCallIgnored
+ ROAD_RUNNER_FOLDER.mkdirs();
+
+ pruneLogsIfNecessary();
+
+ return new File(ROAD_RUNNER_FOLDER, name);
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/LynxModuleUtil.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/LynxModuleUtil.java
new file mode 100644
index 0000000..243dd7e
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/LynxModuleUtil.java
@@ -0,0 +1,124 @@
+package org.firstinspires.ftc.teamcode.util;
+
+import com.qualcomm.hardware.lynx.LynxModule;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+
+import org.firstinspires.ftc.robotcore.internal.system.Misc;
+
+import java.util.HashMap;
+import java.util.Map;
+
+/**
+ * Collection of utilites for interacting with Lynx modules.
+ */
+public class LynxModuleUtil {
+
+ private static final LynxFirmwareVersion MIN_VERSION = new LynxFirmwareVersion(1, 8, 2);
+
+ /**
+ * Parsed representation of a Lynx module firmware version.
+ */
+ public static class LynxFirmwareVersion implements Comparable {
+ public final int major;
+ public final int minor;
+ public final int eng;
+
+ public LynxFirmwareVersion(int major, int minor, int eng) {
+ this.major = major;
+ this.minor = minor;
+ this.eng = eng;
+ }
+
+ @Override
+ public boolean equals(Object other) {
+ if (other instanceof LynxFirmwareVersion) {
+ LynxFirmwareVersion otherVersion = (LynxFirmwareVersion) other;
+ return major == otherVersion.major && minor == otherVersion.minor &&
+ eng == otherVersion.eng;
+ } else {
+ return false;
+ }
+ }
+
+ @Override
+ public int compareTo(LynxFirmwareVersion other) {
+ int majorComp = Integer.compare(major, other.major);
+ if (majorComp == 0) {
+ int minorComp = Integer.compare(minor, other.minor);
+ if (minorComp == 0) {
+ return Integer.compare(eng, other.eng);
+ } else {
+ return minorComp;
+ }
+ } else {
+ return majorComp;
+ }
+ }
+
+ @Override
+ public String toString() {
+ return Misc.formatInvariant("%d.%d.%d", major, minor, eng);
+ }
+ }
+
+ /**
+ * Retrieve and parse Lynx module firmware version.
+ * @param module Lynx module
+ * @return parsed firmware version
+ */
+ public static LynxFirmwareVersion getFirmwareVersion(LynxModule module) {
+ String versionString = module.getNullableFirmwareVersionString();
+ if (versionString == null) {
+ return null;
+ }
+
+ String[] parts = versionString.split("[ :,]+");
+ try {
+ // note: for now, we ignore the hardware entry
+ return new LynxFirmwareVersion(
+ Integer.parseInt(parts[3]),
+ Integer.parseInt(parts[5]),
+ Integer.parseInt(parts[7])
+ );
+ } catch (NumberFormatException e) {
+ return null;
+ }
+ }
+
+ /**
+ * Exception indicating an outdated Lynx firmware version.
+ */
+ public static class LynxFirmwareVersionException extends RuntimeException {
+ public LynxFirmwareVersionException(String detailMessage) {
+ super(detailMessage);
+ }
+ }
+
+ /**
+ * Ensure all of the Lynx modules attached to the robot satisfy the minimum requirement.
+ * @param hardwareMap hardware map containing Lynx modules
+ */
+ public static void ensureMinimumFirmwareVersion(HardwareMap hardwareMap) {
+ Map outdatedModules = new HashMap<>();
+ for (LynxModule module : hardwareMap.getAll(LynxModule.class)) {
+ LynxFirmwareVersion version = getFirmwareVersion(module);
+ if (version == null || version.compareTo(MIN_VERSION) < 0) {
+ for (String name : hardwareMap.getNamesOf(module)) {
+ outdatedModules.put(name, version);
+ }
+ }
+ }
+ if (outdatedModules.size() > 0) {
+ StringBuilder msgBuilder = new StringBuilder();
+ msgBuilder.append("One or more of the attached Lynx modules has outdated firmware\n");
+ msgBuilder.append(Misc.formatInvariant("Mandatory minimum firmware version for Road Runner: %s\n",
+ MIN_VERSION.toString()));
+ for (Map.Entry entry : outdatedModules.entrySet()) {
+ msgBuilder.append(Misc.formatInvariant(
+ "\t%s: %s\n", entry.getKey(),
+ entry.getValue() == null ? "Unknown" : entry.getValue().toString()));
+ }
+ throw new LynxFirmwareVersionException(msgBuilder.toString());
+ }
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/PersonalPID.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/PersonalPID.java
new file mode 100644
index 0000000..0104499
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/PersonalPID.java
@@ -0,0 +1,268 @@
+package org.firstinspires.ftc.teamcode.util;
+
+/**
+ * This is a PID controller (https://en.wikipedia.org/wiki/PID_controller)
+ * for your robot. Internally, it performs all the calculations for you.
+ * You need to tune your values to the appropriate amounts in order
+ * to properly utilize these calculations.
+ *
+ * The equation we will use is:
+ * u(t) = kP * e(t) + kI * int(0,t)[e(t')dt'] + kD * e'(t) + kF
+ * where e(t) = r(t) - y(t) and r(t) is the setpoint and y(t) is the
+ * measured value. If we consider e(t) the positional error, then
+ * int(0,t)[e(t')dt'] is the total error and e'(t) is the velocity error.
+ */
+public class PersonalPID {
+
+ private double kP, kI, kD, kF;
+ private double setPoint;
+ private double measuredValue;
+ private double minIntegral, maxIntegral;
+
+ private double errorVal_p;
+ private double errorVal_v;
+
+ private double totalError;
+ private double prevErrorVal;
+
+ private double errorTolerance_p = 0.05;
+ private double errorTolerance_v = Double.POSITIVE_INFINITY;
+
+ private double lastTimeStamp;
+ private double period;
+
+ /**
+ * The base constructor for the PIDF controller
+ */
+ public PersonalPID(double kp, double ki, double kd, double kf) {
+ this(kp, ki, kd, kf, 0, 0);
+ }
+
+ /**
+ * This is the full constructor for the PIDF controller. Our PIDF controller
+ * includes a feed-forward value which is useful for fighting friction and gravity.
+ * Our errorVal represents the return of e(t) and prevErrorVal is the previous error.
+ *
+ * @param sp The setpoint of the pid control loop.
+ * @param pv The measured value of he pid control loop. We want sp = pv, or to the degree
+ * such that sp - pv, or e(t) < tolerance.
+ */
+ public PersonalPID(double kp, double ki, double kd, double kf, double sp, double pv) {
+ kP = kp;
+ kI = ki;
+ kD = kd;
+ kF = kf;
+
+ setPoint = sp;
+ measuredValue = pv;
+
+ minIntegral = -1.0;
+ maxIntegral = 1.0;
+
+ lastTimeStamp = 0;
+ period = 0;
+
+ errorVal_p = setPoint - measuredValue;
+ reset();
+ }
+
+ public void reset() {
+ totalError = 0;
+ prevErrorVal = 0;
+ lastTimeStamp = 0;
+ }
+
+ /**
+ * Sets the error which is considered tolerable for use with {@link #atSetPoint()}.
+ *
+ * @param positionTolerance Position error which is tolerable.
+ */
+ public void setTolerance(double positionTolerance) {
+ setTolerance(positionTolerance, Double.POSITIVE_INFINITY);
+ }
+
+ /**
+ * Sets the error which is considered tolerable for use with {@link #atSetPoint()}.
+ *
+ * @param positionTolerance Position error which is tolerable.
+ * @param velocityTolerance Velocity error which is tolerable.
+ */
+ public void setTolerance(double positionTolerance, double velocityTolerance) {
+ errorTolerance_p = positionTolerance;
+ errorTolerance_v = velocityTolerance;
+ }
+
+ /**
+ * Returns the current setpoint of the PIDFController.
+ *
+ * @return The current setpoint.
+ */
+ public double getSetPoint() {
+ return setPoint;
+ }
+
+ /**
+ * Sets the setpoint for the PIDFController
+ *
+ * @param sp The desired setpoint.
+ */
+ public void setSetPoint(double sp) {
+ setPoint = sp;
+ errorVal_p = setPoint - measuredValue;
+ errorVal_v = (errorVal_p - prevErrorVal) / period;
+ }
+
+ /**
+ * Returns true if the error is within the percentage of the total input range, determined by
+ * {@link #setTolerance}.
+ *
+ * @return Whether the error is within the acceptable bounds.
+ */
+ public boolean atSetPoint() {
+ return Math.abs(errorVal_p) < errorTolerance_p
+ && Math.abs(errorVal_v) < errorTolerance_v;
+ }
+
+ /**
+ * @return the PIDF coefficients
+ */
+ public double[] getCoefficients() {
+ return new double[]{kP, kI, kD, kF};
+ }
+
+ /**
+ * @return the positional error e(t)
+ */
+ public double getPositionError() {
+ return errorVal_p;
+ }
+
+ /**
+ * @return the tolerances of the controller
+ */
+ public double[] getTolerance() {
+ return new double[]{errorTolerance_p, errorTolerance_v};
+ }
+
+ /**
+ * @return the velocity error e'(t)
+ */
+ public double getVelocityError() {
+ return errorVal_v;
+ }
+
+ /**
+ * Calculates the next output of the PIDF controller.
+ *
+ * @return the next output using the current measured value via
+ * {@link #calculate(double)}.
+ */
+ public double calculate() {
+ return calculate(measuredValue);
+ }
+
+ /**
+ * Calculates the next output of the PIDF controller.
+ *
+ * @param pv The given measured value.
+ * @param sp The given setpoint.
+ * @return the next output using the given measurd value via
+ * {@link #calculate(double)}.
+ */
+ public double calculate(double pv, double sp) {
+ // set the setpoint to the provided value
+ setSetPoint(sp);
+ return calculate(pv);
+ }
+
+ /**
+ * Calculates the control value, u(t).
+ *
+ * @param pv The current measurement of the process variable.
+ * @return the value produced by u(t).
+ */
+ public double calculate(double pv) {
+ prevErrorVal = errorVal_p;
+
+ double currentTimeStamp = (double) System.nanoTime() / 1E9;
+ if (lastTimeStamp == 0) lastTimeStamp = currentTimeStamp;
+ period = currentTimeStamp - lastTimeStamp;
+ lastTimeStamp = currentTimeStamp;
+
+ if (measuredValue == pv) {
+ errorVal_p = setPoint - measuredValue;
+ } else {
+ errorVal_p = setPoint - pv;
+ measuredValue = pv;
+ }
+
+ if (Math.abs(period) > 1E-6) {
+ errorVal_v = (errorVal_p - prevErrorVal) / period;
+ } else {
+ errorVal_v = 0;
+ }
+
+ /*
+ if total error is the integral from 0 to t of e(t')dt', and
+ e(t) = sp - pv, then the total error, E(t), equals sp*t - pv*t.
+ */
+ totalError += period * (setPoint - measuredValue);
+ totalError = totalError < minIntegral ? minIntegral : Math.min(maxIntegral, totalError);
+
+ // returns u(t)
+ return kP * errorVal_p + kI * totalError + kD * errorVal_v + kF * setPoint;
+ }
+
+ public void setPIDF(double kp, double ki, double kd, double kf) {
+ kP = kp;
+ kI = ki;
+ kD = kd;
+ kF = kf;
+ }
+
+ public void setIntegrationBounds(double integralMin, double integralMax) {
+ minIntegral = integralMin;
+ maxIntegral = integralMax;
+ }
+
+ public void clearTotalError() {
+ totalError = 0;
+ }
+
+ public void setP(double kp) {
+ kP = kp;
+ }
+
+ public void setI(double ki) {
+ kI = ki;
+ }
+
+ public void setD(double kd) {
+ kD = kd;
+ }
+
+ public void setF(double kf) {
+ kF = kf;
+ }
+
+ public double getP() {
+ return kP;
+ }
+
+ public double getI() {
+ return kI;
+ }
+
+ public double getD() {
+ return kD;
+ }
+
+ public double getF() {
+ return kF;
+ }
+
+ public double getPeriod() {
+ return period;
+ }
+
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/RegressionUtil.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/RegressionUtil.java
new file mode 100644
index 0000000..c3bf488
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/RegressionUtil.java
@@ -0,0 +1,156 @@
+package org.firstinspires.ftc.teamcode.util;
+
+import androidx.annotation.Nullable;
+
+import com.acmerobotics.roadrunner.kinematics.Kinematics;
+
+import org.apache.commons.math3.stat.regression.SimpleRegression;
+
+import java.io.File;
+import java.io.FileNotFoundException;
+import java.io.PrintWriter;
+import java.util.ArrayList;
+import java.util.List;
+
+/**
+ * Various regression utilities.
+ */
+public class RegressionUtil {
+
+ /**
+ * Feedforward parameter estimates from the ramp regression and additional summary statistics
+ */
+ public static class RampResult {
+ public final double kV, kStatic, rSquare;
+
+ public RampResult(double kV, double kStatic, double rSquare) {
+ this.kV = kV;
+ this.kStatic = kStatic;
+ this.rSquare = rSquare;
+ }
+ }
+
+ /**
+ * Feedforward parameter estimates from the ramp regression and additional summary statistics
+ */
+ public static class AccelResult {
+ public final double kA, rSquare;
+
+ public AccelResult(double kA, double rSquare) {
+ this.kA = kA;
+ this.rSquare = rSquare;
+ }
+ }
+
+ /**
+ * Numerically compute dy/dx from the given x and y values. The returned list is padded to match
+ * the length of the original sequences.
+ *
+ * @param x x-values
+ * @param y y-values
+ * @return derivative values
+ */
+ private static List numericalDerivative(List x, List y) {
+ List deriv = new ArrayList<>(x.size());
+ for (int i = 1; i < x.size() - 1; i++) {
+ deriv.add(
+ (y.get(i + 1) - y.get(i - 1)) /
+ (x.get(i + 1) - x.get(i - 1))
+ );
+ }
+ // copy endpoints to pad output
+ deriv.add(0, deriv.get(0));
+ deriv.add(deriv.get(deriv.size() - 1));
+ return deriv;
+ }
+
+ /**
+ * Run regression to compute velocity and static feedforward from ramp test data.
+ *
+ * Here's the general procedure for gathering the requisite data:
+ * 1. Slowly ramp the motor power/voltage and record encoder values along the way.
+ * 2. Run a linear regression on the encoder velocity vs. motor power plot to obtain a slope
+ * (kV) and an optional intercept (kStatic).
+ *
+ * @param timeSamples time samples
+ * @param positionSamples position samples
+ * @param powerSamples power samples
+ * @param fitStatic fit kStatic
+ * @param file log file
+ */
+ public static RampResult fitRampData(List timeSamples, List positionSamples,
+ List powerSamples, boolean fitStatic,
+ @Nullable File file) {
+ if (file != null) {
+ try (PrintWriter pw = new PrintWriter(file)) {
+ pw.println("time,position,power");
+ for (int i = 0; i < timeSamples.size(); i++) {
+ double time = timeSamples.get(i);
+ double pos = positionSamples.get(i);
+ double power = powerSamples.get(i);
+ pw.println(time + "," + pos + "," + power);
+ }
+ } catch (FileNotFoundException e) {
+ // ignore
+ }
+ }
+
+ List velSamples = numericalDerivative(timeSamples, positionSamples);
+
+ SimpleRegression rampReg = new SimpleRegression(fitStatic);
+ for (int i = 0; i < timeSamples.size(); i++) {
+ double vel = velSamples.get(i);
+ double power = powerSamples.get(i);
+
+ rampReg.addData(vel, power);
+ }
+
+ return new RampResult(Math.abs(rampReg.getSlope()), Math.abs(rampReg.getIntercept()),
+ rampReg.getRSquare());
+ }
+
+ /**
+ * Run regression to compute acceleration feedforward.
+ *
+ * @param timeSamples time samples
+ * @param positionSamples position samples
+ * @param powerSamples power samples
+ * @param rampResult ramp result
+ * @param file log file
+ */
+ public static AccelResult fitAccelData(List timeSamples, List positionSamples,
+ List powerSamples, RampResult rampResult,
+ @Nullable File file) {
+ if (file != null) {
+ try (PrintWriter pw = new PrintWriter(file)) {
+ pw.println("time,position,power");
+ for (int i = 0; i < timeSamples.size(); i++) {
+ double time = timeSamples.get(i);
+ double pos = positionSamples.get(i);
+ double power = powerSamples.get(i);
+ pw.println(time + "," + pos + "," + power);
+ }
+ } catch (FileNotFoundException e) {
+ // ignore
+ }
+ }
+
+ List velSamples = numericalDerivative(timeSamples, positionSamples);
+ List accelSamples = numericalDerivative(timeSamples, velSamples);
+
+ SimpleRegression accelReg = new SimpleRegression(false);
+ for (int i = 0; i < timeSamples.size(); i++) {
+ double vel = velSamples.get(i);
+ double accel = accelSamples.get(i);
+ double power = powerSamples.get(i);
+
+ double powerFromVel = Kinematics.calculateMotorFeedforward(
+ vel, 0.0, rampResult.kV, 0.0, rampResult.kStatic);
+ double powerFromAccel = power - powerFromVel;
+
+ accelReg.addData(accel, powerFromAccel);
+ }
+
+ return new AccelResult(Math.abs(accelReg.getSlope()), accelReg.getRSquare());
+ }
+}
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..9213e2e
--- /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 {
+
+ compileSdkVersion 30
+
+ 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"
+ }
+ }
+ debug {
+ debuggable true
+ jniDebuggable true
+ ndk {
+ abiFilters "armeabi-v7a"
+ }
+ }
+ }
+
+ 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..6b55475
--- /dev/null
+++ b/build.dependencies.gradle
@@ -0,0 +1,19 @@
+repositories {
+ mavenCentral()
+ google() // Needed for androidx
+ maven { url = 'https://maven.brott.dev/' }
+}
+
+dependencies {
+ implementation 'org.firstinspires.ftc:Inspection:11.0.0'
+ implementation 'org.firstinspires.ftc:Blocks:11.0.0'
+ implementation 'org.firstinspires.ftc:RobotCore:11.0.0'
+ implementation 'org.firstinspires.ftc:RobotServer:11.0.0'
+ implementation 'org.firstinspires.ftc:OnBotJava:11.0.0'
+ implementation 'org.firstinspires.ftc:Hardware:11.0.0'
+ implementation 'org.firstinspires.ftc:FtcCommon:11.0.0'
+ implementation 'org.firstinspires.ftc:Vision:11.0.0'
+ implementation 'androidx.appcompat:appcompat:1.2.0'
+ implementation 'com.acmerobotics.dashboard:dashboard:0.4.15'
+}
+
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..f5935e9
--- /dev/null
+++ b/gradle.properties
@@ -0,0 +1,12 @@
+# 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
\ 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 100644
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'