allHubs = hardwareMap.getAll(LynxModule.class);
+
+ ElapsedTime timer = new ElapsedTime();
+
+ telemetry.addData(">", "Press play 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..7ea4683
--- /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 play button is pressed (or the
+ * OpMode is stopped).
+ */
+ @Override
+ public void init_loop() {
+ }
+
+ /**
+ * This method will be called once, when the play button is pressed.
+ */
+ @Override
+ public void start() {
+ runtime.reset();
+ }
+
+ /**
+ * This method will be called repeatedly during the period between when
+ * the play 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/ConceptRevSPARKMini.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptRevSPARKMini.java
new file mode 100644
index 0000000..e710662
--- /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 executes a basic Tank Drive Teleop for a two wheeled robot using two REV SPARKminis.
+ * To use this example, connect two REV SPARKminis into servo ports on the Expansion 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 the robot configuration
+ // step (using the FTC Robot Controller app on the phone).
+ 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 PLAY)
+ 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..f0a855f
--- /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 PLAY)
+ 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/RobotAutoDriveByEncoder_Linear.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveByEncoder_Linear.java
new file mode 100644
index 0000000..be2e218
--- /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 PLAY)
+ 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..7885fe4
--- /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 corrects 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 it's 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..b449258
--- /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 PLAY)
+ waitForStart();
+
+ // Step through each leg of the path, ensuring that the Auto mode 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..ab21e96
--- /dev/null
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveToAprilTagOmni.java
@@ -0,0 +1,297 @@
+/* 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.
+ *
+ * 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: leftfront_drive and rightfront_drive, leftback_drive and rightback_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 (usually starting at 0)
+ *
+ * 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". eg: Ramp up to 50% power at a 25 inch error. (0.50 / 25.0)
+ final double STRAFE_GAIN = 0.015 ; // Strafe Speed Control "Gain". eg: Ramp up to 25% power at a 25 degree Yaw error. (0.25 / 25.0)
+ final double TURN_GAIN = 0.01 ; // Turn Control "Gain". eg: 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 approach 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 leftFrontDrive = null; // Used to control the left front drive wheel
+ private DcMotor rightFrontDrive = null; // Used to control the right front drive wheel
+ private DcMotor leftBackDrive = null; // Used to control the left back drive wheel
+ private DcMotor rightBackDrive = 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 = 0; // 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).
+ leftFrontDrive = hardwareMap.get(DcMotor.class, "leftfront_drive");
+ rightFrontDrive = hardwareMap.get(DcMotor.class, "rightfront_drive");
+ leftBackDrive = hardwareMap.get(DcMotor.class, "leftback_drive");
+ rightBackDrive = hardwareMap.get(DcMotor.class, "rightback_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
+ leftFrontDrive.setDirection(DcMotor.Direction.REVERSE);
+ leftBackDrive.setDirection(DcMotor.Direction.REVERSE);
+ rightFrontDrive.setDirection(DcMotor.Direction.FORWARD);
+ rightBackDrive.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 Play 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) {
+ if ((detection.metadata != null) &&
+ ((DESIRED_TAG_ID < 0) || (detection.id == DESIRED_TAG_ID)) ){
+ targetFound = true;
+ desiredTag = detection;
+ break; // don't look any further.
+ } else {
+ telemetry.addData("Unknown Target", "Tag ID %d is not in TagLibrary\n", detection.id);
+ }
+ }
+
+ // Tell the driver what we see, and what to do.
+ if (targetFound) {
+ telemetry.addData(">","HOLD Left-Bumper to Drive to Target\n");
+ telemetry.addData("Target", "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(">","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 leftFrontPower = x -y -yaw;
+ double rightFrontPower = x +y +yaw;
+ double leftBackPower = x +y -yaw;
+ double rightBackPower = x -y +yaw;
+
+ // Normalize wheel powers to be less than 1.0
+ double max = Math.max(Math.abs(leftFrontPower), Math.abs(rightFrontPower));
+ max = Math.max(max, Math.abs(leftBackPower));
+ max = Math.max(max, Math.abs(rightBackPower));
+
+ if (max > 1.0) {
+ leftFrontPower /= max;
+ rightFrontPower /= max;
+ leftBackPower /= max;
+ rightBackPower /= max;
+ }
+
+ // Send powers to the wheels.
+ leftFrontDrive.setPower(leftFrontPower);
+ rightFrontDrive.setPower(rightFrontPower);
+ leftBackDrive.setPower(leftBackPower);
+ rightBackDrive.setPower(rightBackPower);
+ }
+
+ /**
+ * Initialize the AprilTag processor.
+ */
+ private void initAprilTag() {
+ // Create the AprilTag processor by using a builder.
+ aprilTag = new AprilTagProcessor.Builder().build();
+
+ // 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..657bc3b
--- /dev/null
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveToAprilTagTank.java
@@ -0,0 +1,274 @@
+/* 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
+ *
+ * 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 (usually starting at 0)
+ *
+ * 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". eg: Ramp up to 50% power at a 25 inch error. (0.50 / 25.0)
+ final double TURN_GAIN = 0.01 ; // Turn Control "Gain". eg: 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 = 0; // 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 Play 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) {
+ if ((detection.metadata != null) &&
+ ((DESIRED_TAG_ID < 0) || (detection.id == DESIRED_TAG_ID)) ){
+ targetFound = true;
+ desiredTag = detection;
+ break; // don't look any further.
+ } else {
+ telemetry.addData("Unknown Target", "Tag ID %d is not in TagLibrary\n", detection.id);
+ }
+ }
+
+ // Tell the driver what we see, and what to do.
+ if (targetFound) {
+ telemetry.addData(">","HOLD Left-Bumper to Drive to Target\n");
+ telemetry.addData("Target", "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(">","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();
+
+ // 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..e5076c0
--- /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 PLAY)
+ // 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/RobotHardware.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotHardware.java
new file mode 100644
index 0000000..b1c8de6
--- /dev/null
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/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/RobotTeleopPOV_Linear.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotTeleopPOV_Linear.java
new file mode 100644
index 0000000..454d5a7
--- /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 Play."); //
+ telemetry.update();
+
+ // Wait for the game to start (driver presses PLAY)
+ 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..b9f4fcf
--- /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 Play."); //
+ }
+
+ /*
+ * Code to run REPEATEDLY after the driver hits INIT, but before they hit PLAY
+ */
+ @Override
+ public void init_loop() {
+ }
+
+ /*
+ * Code to run ONCE when the driver hits PLAY
+ */
+ @Override
+ public void start() {
+ }
+
+ /*
+ * Code to run REPEATEDLY after the driver hits PLAY 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/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..7546c9d
--- /dev/null
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorColor.java
@@ -0,0 +1,221 @@
+/* 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.
+ *
+ * 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.
+ *
+ * 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.
+ *
+ * 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 2020,
+ * the only color sensors that support this are the ones from REV Robotics. 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/SensorHuskyLens.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorHuskyLens.java
new file mode 100644
index 0000000..1ec1f55
--- /dev/null
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorHuskyLens.java
@@ -0,0 +1,149 @@
+/*
+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.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;
+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
+ *
+ * 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.
+ */
+ 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());
+ }
+
+ 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..eb1c7ca
--- /dev/null
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorIMUNonOrthogonal.java
@@ -0,0 +1,181 @@
+/* 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.
+ *
+ * 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.
+ * 1) 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..d92ce3e
--- /dev/null
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorIMUOrthogonal.java
@@ -0,0 +1,143 @@
+/* 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.
+ */
+
+ /* 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/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/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/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/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..45968ef
--- /dev/null
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/sample_conventions.md
@@ -0,0 +1,108 @@
+## 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.
+
+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..8c6ea59
--- /dev/null
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/internal/FtcOpModeRegister.java
@@ -0,0 +1,70 @@
+/* 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;
+
+import org.firstinspires.ftc.robotcontroller.external.samples.ConceptNullOp;
+
+/**
+ * {@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 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/PathPlanning/.gitignore b/PathPlanning/.gitignore
new file mode 100644
index 0000000..42afabf
--- /dev/null
+++ b/PathPlanning/.gitignore
@@ -0,0 +1 @@
+/build
\ No newline at end of file
diff --git a/PathPlanning/build.gradle b/PathPlanning/build.gradle
new file mode 100644
index 0000000..c1fa0fe
--- /dev/null
+++ b/PathPlanning/build.gradle
@@ -0,0 +1,20 @@
+plugins {
+ id 'java-library'
+ id 'org.jetbrains.kotlin.jvm'
+}
+
+java {
+ toolchain {
+ languageVersion = JavaLanguageVersion.of(21)
+ }
+}
+
+repositories {
+ maven { url = 'https://jitpack.io' }
+ maven { url = 'https://maven.brott.dev/' }
+}
+
+dependencies {
+ implementation 'com.github.NoahBres:MeepMeep:2.0.3'
+ implementation 'com.acmerobotics.roadrunner:core:0.5.6'
+}
\ No newline at end of file
diff --git a/PathPlanning/src/main/resources/temp_centerstage_field.png b/PathPlanning/src/main/resources/temp_centerstage_field.png
new file mode 100644
index 0000000..3e0eb58
Binary files /dev/null and b/PathPlanning/src/main/resources/temp_centerstage_field.png differ
diff --git a/README.md b/README.md
new file mode 100644
index 0000000..0460511
--- /dev/null
+++ b/README.md
@@ -0,0 +1 @@
+Public code repository for FTC Team 13100 47 Beavers for Into The Deep
\ No newline at end of file
diff --git a/TeamCode/build.gradle b/TeamCode/build.gradle
new file mode 100644
index 0000000..5b8887f
--- /dev/null
+++ b/TeamCode/build.gradle
@@ -0,0 +1,43 @@
+//
+// 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'
+
+apply plugin: 'kotlin-android'
+
+kotlin {
+ jvmToolchain(17)
+}
+
+android {
+ namespace = 'org.firstinspires.ftc.teamcode'
+
+ packagingOptions {
+ jniLibs {
+ pickFirsts += ['**/*.so']
+ }
+ }
+}
+
+dependencies {
+ implementation project(':FtcRobotController')
+ implementation 'androidx.core:core-ktx:1.13.1'
+ annotationProcessor files('lib/OpModeAnnotationProcessor.jar')
+
+ implementation 'org.apache.commons:commons-math3:3.6.1'
+ implementation 'com.acmerobotics.roadrunner:core:0.5.6'
+ implementation 'org.ftclib.ftclib:core:2.1.1' // core
+}
diff --git a/TeamCode/eocvsim_workspace.json b/TeamCode/eocvsim_workspace.json
new file mode 100644
index 0000000..8ff3266
--- /dev/null
+++ b/TeamCode/eocvsim_workspace.json
@@ -0,0 +1,7 @@
+{
+ "sourcesPath": ".",
+ "resourcesPath": ".",
+ "excludedPaths": [],
+ "excludedFileExtensions": [],
+ "eocvSimVersion": "3.5.1"
+}
\ No newline at end of file
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..87b7178
--- /dev/null
+++ b/TeamCode/src/main/AndroidManifest.xml
@@ -0,0 +1,11 @@
+
+
+
+
+
+
+
+
+
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/drive/DriveCommand.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/drive/DriveCommand.kt
new file mode 100644
index 0000000..8f87198
--- /dev/null
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/drive/DriveCommand.kt
@@ -0,0 +1,34 @@
+package org.firstinspires.ftc.teamcode.commands.drive
+
+import com.arcrobotics.ftclib.command.CommandBase
+import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem
+import java.util.function.DoubleSupplier
+
+class DriveCommand(
+ private val subsystem: DriveSubsystem,
+ private val leftX: DoubleSupplier,
+ private val leftY: DoubleSupplier,
+ private val rightX: DoubleSupplier,
+ private val zoneVal: Double,
+) : CommandBase() {
+ init {
+ addRequirements(subsystem)
+ }
+
+ override fun execute() {
+ subsystem.drive(
+ leftY = zonedDrive(-leftY.asDouble, zoneVal),
+ leftX = zonedDrive(leftX.asDouble, zoneVal),
+ rightX = zonedDrive(rightX.asDouble, zoneVal),
+ )
+ }
+
+ private fun zonedDrive(drive: Double, zoneVal: Double) =
+ if (drive in -zoneVal..zoneVal) {
+ 0.0
+ } else if (drive > zoneVal) {
+ drive / (1 - zoneVal) - zoneVal / (1 - zoneVal)
+ } else {
+ drive / (1 - zoneVal) + zoneVal / (1 - zoneVal)
+ }
+}
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/constants/ControlBoard.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/constants/ControlBoard.kt
new file mode 100644
index 0000000..ecee851
--- /dev/null
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/constants/ControlBoard.kt
@@ -0,0 +1,24 @@
+package org.firstinspires.ftc.teamcode.constants
+
+
+enum class ControlBoard(val deviceName: String) {
+ // Drive motors
+ DRIVE_LEFT_FRONT("leftFront"),
+ DRIVE_RIGHT_FRONT("rightFront"),
+ DRIVE_LEFT_REAR("leftRear"),
+ DRIVE_RIGHT_REAR("rightRear"),
+
+ // Odometry
+ ODO_LEFT_ENCODER(""),
+ ODO_RIGHT_ENCODER(""),
+ ODO_STRAFE_ENCODER(""),
+
+ // Arm
+ ARM_LEFT(""),
+ ARM_RIGHT(""),
+ SLIDES_LEFT(""),
+ SLIDES_RIGHT(""),
+
+ // Camera
+ CAMERA("lifecam")
+}
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/constants/PoseStorage.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/constants/PoseStorage.kt
new file mode 100644
index 0000000..037f691
--- /dev/null
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/constants/PoseStorage.kt
@@ -0,0 +1,7 @@
+package org.firstinspires.ftc.teamcode.constants
+
+import com.acmerobotics.roadrunner.geometry.Pose2d
+
+object PoseStorage {
+ var poseEstimate = Pose2d()
+}
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/teleOp/MainTeleOp.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/teleOp/MainTeleOp.kt
new file mode 100644
index 0000000..6ff176c
--- /dev/null
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/teleOp/MainTeleOp.kt
@@ -0,0 +1,34 @@
+package org.firstinspires.ftc.teamcode.opModes.teleOp
+
+import com.arcrobotics.ftclib.command.CommandOpMode
+import com.arcrobotics.ftclib.gamepad.GamepadEx
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp
+import org.firstinspires.ftc.teamcode.commands.drive.DriveCommand
+import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem
+
+@TeleOp
+class MainTeleOp : CommandOpMode() {
+ private lateinit var driveSubsystem: DriveSubsystem
+
+ private lateinit var driveCommand: DriveCommand
+
+ private lateinit var driver: GamepadEx
+ override fun initialize() {
+
+ driveSubsystem = DriveSubsystem(hardwareMap)
+
+ driver = GamepadEx(gamepad1)
+
+ driveCommand = DriveCommand(
+ driveSubsystem,
+ leftX = driver::getLeftX,
+ leftY = driver::getLeftY,
+ rightX = driver::getRightX,
+ zoneVal = 0.15
+ )
+
+ register(driveSubsystem)
+
+ driveSubsystem.defaultCommand = driveCommand
+ }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/AutomaticFeedforwardTuner.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/AutomaticFeedforwardTuner.kt
new file mode 100644
index 0000000..1328568
--- /dev/null
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/AutomaticFeedforwardTuner.kt
@@ -0,0 +1,211 @@
+package org.firstinspires.ftc.teamcode.opModes.tuning.roadrunner
+
+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.Disabled
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode
+import com.qualcomm.robotcore.util.RobotLog
+import org.firstinspires.ftc.robotcore.internal.system.Misc
+import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem
+import org.firstinspires.ftc.teamcode.utils.roadrunner.drive.DriveConstants
+import org.firstinspires.ftc.teamcode.utils.roadrunner.drive.DriveConstants.rpmToVelocity
+import org.firstinspires.ftc.teamcode.utils.roadrunner.util.LoggingUtil
+import org.firstinspires.ftc.teamcode.utils.roadrunner.util.RegressionUtil
+
+/*
+ * 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.
+ */
+@Disabled
+@Config
+@Autonomous(group = "drive")
+class AutomaticFeedforwardTuner : LinearOpMode() {
+ @Throws(InterruptedException::class)
+ override fun runOpMode() {
+ if (DriveConstants.RUN_USING_ENCODER) {
+ RobotLog.setGlobalErrorMsg(
+ "Feedforward constants usually don't need to be tuned " +
+ "when using the built-in drive motor velocity PID."
+ )
+ }
+ telemetry = MultipleTelemetry(telemetry, FtcDashboard.getInstance().telemetry)
+ val drive = DriveSubsystem(hardwareMap)
+ val 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()
+ var 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()
+ val maxVel = rpmToVelocity(DriveConstants.MAX_RPM)
+ val finalVel = MAX_POWER * maxVel
+ val accel = finalVel * finalVel / (2.0 * DISTANCE)
+ val rampTime = Math.sqrt(2.0 * DISTANCE / accel)
+ val timeSamples: MutableList = ArrayList()
+ val positionSamples: MutableList = ArrayList()
+ val powerSamples: MutableList = ArrayList()
+ drive.poseEstimate = Pose2d()
+ var startTime = clock.seconds()
+ while (!isStopRequested) {
+ val elapsedTime = clock.seconds() - startTime
+ if (elapsedTime > rampTime) {
+ break
+ }
+ val vel = accel * elapsedTime
+ val power = vel / maxVel
+ timeSamples.add(elapsedTime)
+ positionSamples.add(drive.poseEstimate.x)
+ powerSamples.add(power)
+ drive.setDrivePower(Pose2d(power, 0.0, 0.0))
+ drive.updatePoseEstimate()
+ }
+ drive.setDrivePower(Pose2d(0.0, 0.0, 0.0))
+ val 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()
+ var 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()
+ val maxPowerTime = DISTANCE / maxVel
+ timeSamples.clear()
+ positionSamples.clear()
+ powerSamples.clear()
+ drive.poseEstimate = Pose2d()
+ drive.setDrivePower(Pose2d(MAX_POWER, 0.0, 0.0))
+ startTime = clock.seconds()
+ while (!isStopRequested) {
+ val elapsedTime = clock.seconds() - startTime
+ if (elapsedTime > maxPowerTime) {
+ break
+ }
+ timeSamples.add(elapsedTime)
+ positionSamples.add(drive.poseEstimate.x)
+ powerSamples.add(MAX_POWER)
+ drive.updatePoseEstimate()
+ }
+ drive.setDrivePower(Pose2d(0.0, 0.0, 0.0))
+ val 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()
+ }
+ }
+
+ companion object {
+ var MAX_POWER = 0.7
+ var DISTANCE = 100.0 // in
+ }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/BackAndForth.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/BackAndForth.kt
new file mode 100644
index 0000000..ac49ef0
--- /dev/null
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/BackAndForth.kt
@@ -0,0 +1,49 @@
+package org.firstinspires.ftc.teamcode.opModes.tuning.roadrunner
+
+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.Disabled
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode
+import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem
+
+/*
+ * 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.
+ */
+@Disabled
+@Config
+@Autonomous(group = "drive")
+class BackAndForth : LinearOpMode() {
+ @Throws(InterruptedException::class)
+ override fun runOpMode() {
+ val drive = DriveSubsystem(hardwareMap)
+ val trajectoryForward = drive.trajectoryBuilder(Pose2d())
+ .forward(DISTANCE)
+ .build()
+ val trajectoryBackward = drive.trajectoryBuilder(trajectoryForward.end())
+ .back(DISTANCE)
+ .build()
+ waitForStart()
+ while (opModeIsActive() && !isStopRequested) {
+ drive.followTrajectory(trajectoryForward)
+ drive.followTrajectory(trajectoryBackward)
+ }
+ }
+
+ companion object {
+ var DISTANCE = 50.0
+ }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/DriveVelocityPIDTuner.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/DriveVelocityPIDTuner.kt
new file mode 100644
index 0000000..5843449
--- /dev/null
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/DriveVelocityPIDTuner.kt
@@ -0,0 +1,154 @@
+package org.firstinspires.ftc.teamcode.opModes.tuning.roadrunner
+
+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.generateSimpleMotionProfile
+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.Disabled
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode
+import com.qualcomm.robotcore.hardware.DcMotor
+import com.qualcomm.robotcore.util.RobotLog
+import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem
+import org.firstinspires.ftc.teamcode.utils.roadrunner.drive.DriveConstants
+
+/*
+ * 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.
+ */
+@Disabled
+@Config
+@Autonomous(group = "drive")
+class DriveVelocityPIDTuner : LinearOpMode() {
+ override fun runOpMode() {
+ if (!DriveConstants.RUN_USING_ENCODER) {
+ RobotLog.setGlobalErrorMsg(
+ "%s does not need to be run if the built-in motor velocity" +
+ "PID is not in use", javaClass.simpleName
+ )
+ }
+ telemetry = MultipleTelemetry(telemetry, FtcDashboard.getInstance().telemetry)
+ val drive = DriveSubsystem(hardwareMap)
+ var mode = Mode.TUNING_MODE
+ var lastKp = DriveConstants.MOTOR_VELO_PID.p
+ var lastKi = DriveConstants.MOTOR_VELO_PID.i
+ var lastKd = DriveConstants.MOTOR_VELO_PID.d
+ var lastKf = DriveConstants.MOTOR_VELO_PID.f
+ drive.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, DriveConstants.MOTOR_VELO_PID)
+ val clock = NanoClock.system()
+ telemetry.addLine("Ready!")
+ telemetry.update()
+ telemetry.clearAll()
+ waitForStart()
+ if (isStopRequested) return
+ var movingForwards = true
+ var activeProfile = generateProfile(true)
+ var profileStart = clock.seconds()
+ while (!isStopRequested) {
+ telemetry.addData("mode", mode)
+ when (mode) {
+ Mode.TUNING_MODE -> {
+ if (gamepad1.y) {
+ mode = Mode.DRIVER_MODE
+ drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER)
+ }
+
+ // calculate and set the motor power
+ val profileTime = clock.seconds() - profileStart
+ if (profileTime > activeProfile.duration()) {
+ // generate a new profile
+ movingForwards = !movingForwards
+ activeProfile = generateProfile(movingForwards)
+ profileStart = clock.seconds()
+ }
+ val motionState = activeProfile[profileTime]
+ val targetPower = DriveConstants.kV * motionState.v
+ drive.setDrivePower(Pose2d(targetPower, 0.0, 0.0))
+ val velocities = drive.getWheelVelocities()
+
+ // update telemetry
+ telemetry.addData("targetVelocity", motionState.v)
+ var i = 0
+ while (i < velocities.size) {
+ telemetry.addData("measuredVelocity$i", velocities[i])
+ telemetry.addData(
+ "error$i",
+ motionState.v - velocities[i]
+ )
+ i++
+ }
+ }
+
+ Mode.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(
+ Pose2d(
+ -gamepad1.left_stick_y.toDouble(),
+ -gamepad1.left_stick_x.toDouble(),
+ -gamepad1.right_stick_x.toDouble()
+ )
+ )
+ }
+ }
+ if (lastKp != DriveConstants.MOTOR_VELO_PID.p || lastKd != DriveConstants.MOTOR_VELO_PID.d || lastKi != DriveConstants.MOTOR_VELO_PID.i || lastKf != DriveConstants.MOTOR_VELO_PID.f) {
+ drive.setPIDFCoefficients(
+ DcMotor.RunMode.RUN_USING_ENCODER,
+ DriveConstants.MOTOR_VELO_PID
+ )
+ lastKp = DriveConstants.MOTOR_VELO_PID.p
+ lastKi = DriveConstants.MOTOR_VELO_PID.i
+ lastKd = DriveConstants.MOTOR_VELO_PID.d
+ lastKf = DriveConstants.MOTOR_VELO_PID.f
+ }
+ telemetry.update()
+ }
+ }
+
+ internal enum class Mode {
+ DRIVER_MODE, TUNING_MODE
+ }
+
+ companion object {
+ var DISTANCE = 72.0 // in
+ private fun generateProfile(movingForward: Boolean): MotionProfile {
+ val start = MotionState(if (movingForward) 0.0 else DISTANCE, 0.0, 0.0, 0.0)
+ val goal = MotionState(if (movingForward) DISTANCE else 0.0, 0.0, 0.0, 0.0)
+ return generateSimpleMotionProfile(
+ start,
+ goal,
+ DriveConstants.MAX_VEL,
+ DriveConstants.MAX_ACCEL
+ )
+ }
+ }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/FollowerPIDTuner.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/FollowerPIDTuner.kt
new file mode 100644
index 0000000..328c703
--- /dev/null
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/FollowerPIDTuner.kt
@@ -0,0 +1,52 @@
+package org.firstinspires.ftc.teamcode.opModes.tuning.roadrunner
+
+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.Disabled
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode
+import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem
+
+/*
+ * 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.
+ */
+@Disabled
+@Config
+@Autonomous(group = "drive")
+class FollowerPIDTuner : LinearOpMode() {
+ @Throws(InterruptedException::class)
+ override fun runOpMode() {
+ val drive = DriveSubsystem(hardwareMap)
+ val startPose = Pose2d(-DISTANCE / 2, -DISTANCE / 2, 0.0)
+ drive.poseEstimate = startPose
+ waitForStart()
+ if (isStopRequested) return
+ while (!isStopRequested) {
+ val trajSeq = drive.trajectorySequenceBuilder(startPose)
+ .forward(DISTANCE)
+ .turn(Math.toRadians(90.0))
+ .forward(DISTANCE)
+ .turn(Math.toRadians(90.0))
+ .forward(DISTANCE)
+ .turn(Math.toRadians(90.0))
+ .forward(DISTANCE)
+ .turn(Math.toRadians(90.0))
+ .build()
+ drive.followTrajectorySequence(trajSeq)
+ }
+ }
+
+ companion object {
+ var DISTANCE = 48.0 // in
+ }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/LocalizationTest.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/LocalizationTest.kt
new file mode 100644
index 0000000..da6a08a
--- /dev/null
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/LocalizationTest.kt
@@ -0,0 +1,41 @@
+package org.firstinspires.ftc.teamcode.opModes.tuning.roadrunner
+
+import com.acmerobotics.roadrunner.geometry.Pose2d
+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 org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem
+
+/**
+ * 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).
+ */
+@Disabled
+@TeleOp(group = "drive")
+class LocalizationTest : LinearOpMode() {
+ @Throws(InterruptedException::class)
+ override fun runOpMode() {
+ val drive = DriveSubsystem(hardwareMap)
+ drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER)
+ waitForStart()
+ while (!isStopRequested) {
+ drive.setWeightedDrivePower(
+ Pose2d(
+ -gamepad1.left_stick_y.toDouble(),
+ -gamepad1.left_stick_x.toDouble(),
+ -gamepad1.right_stick_x.toDouble()
+ )
+ )
+ drive.update()
+ val (x, y, heading) = drive.poseEstimate
+ telemetry.addData("x", x)
+ telemetry.addData("y", y)
+ telemetry.addData("heading", Math.toDegrees(heading))
+ telemetry.update()
+ }
+ }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/ManualFeedforwardTuner.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/ManualFeedforwardTuner.kt
new file mode 100644
index 0000000..501d22a
--- /dev/null
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/ManualFeedforwardTuner.kt
@@ -0,0 +1,131 @@
+package org.firstinspires.ftc.teamcode.opModes.tuning.roadrunner
+
+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.kinematics.Kinematics.calculateMotorFeedforward
+import com.acmerobotics.roadrunner.profile.MotionProfile
+import com.acmerobotics.roadrunner.profile.MotionProfileGenerator.generateSimpleMotionProfile
+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.Disabled
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode
+import com.qualcomm.robotcore.util.RobotLog
+import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem
+import org.firstinspires.ftc.teamcode.utils.roadrunner.drive.DriveConstants.MAX_ACCEL
+import org.firstinspires.ftc.teamcode.utils.roadrunner.drive.DriveConstants.MAX_VEL
+import org.firstinspires.ftc.teamcode.utils.roadrunner.drive.DriveConstants.RUN_USING_ENCODER
+import org.firstinspires.ftc.teamcode.utils.roadrunner.drive.DriveConstants.kA
+import org.firstinspires.ftc.teamcode.utils.roadrunner.drive.DriveConstants.kStatic
+import org.firstinspires.ftc.teamcode.utils.roadrunner.drive.DriveConstants.kV
+
+/*
+ * 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.
+ */
+@Disabled
+@Config
+@Autonomous(group = "drive")
+class ManualFeedforwardTuner : LinearOpMode() {
+ private val dashboard = FtcDashboard.getInstance()
+ private lateinit var drive: DriveSubsystem
+ private lateinit var mode: Mode
+ override fun 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 = MultipleTelemetry(telemetry, dashboard.telemetry)
+ drive = DriveSubsystem(hardwareMap)
+ mode = Mode.TUNING_MODE
+ val clock = NanoClock.system()
+ telemetry.addLine("Ready!")
+ telemetry.update()
+ telemetry.clearAll()
+ waitForStart()
+ if (isStopRequested) return
+ var movingForwards = true
+ var activeProfile = generateProfile(true)
+ var profileStart = clock.seconds()
+ while (!isStopRequested) {
+ telemetry.addData("mode", mode)
+ when (mode) {
+ Mode.TUNING_MODE -> {
+ if (gamepad1.y) {
+ mode = Mode.DRIVER_MODE
+ }
+
+ // calculate and set the motor power
+ val profileTime = clock.seconds() - profileStart
+ if (profileTime > activeProfile.duration()) {
+ // generate a new profile
+ movingForwards = !movingForwards
+ activeProfile = generateProfile(movingForwards)
+ profileStart = clock.seconds()
+ }
+
+ val motionState = activeProfile[profileTime]
+ val targetPower =
+ calculateMotorFeedforward(motionState.v, motionState.a, kV, kA, kStatic)
+
+ drive.setDrivePower(Pose2d(targetPower, 0.0, 0.0))
+ drive.updatePoseEstimate()
+
+ val currentVelo = requireNotNull(
+ drive.poseVelocity
+ ) { "poseVelocity() must not be null. Ensure that the getWheelVelocities() method has been overridden in your localizer." }.x
+
+ // update telemetry
+ telemetry.addData("targetVelocity", motionState.v)
+ telemetry.addData("measuredVelocity", currentVelo)
+ telemetry.addData("error", motionState.v - currentVelo)
+ }
+
+ Mode.DRIVER_MODE -> {
+ if (gamepad1.b) {
+ mode = Mode.TUNING_MODE
+ movingForwards = true
+ activeProfile = generateProfile(movingForwards)
+ profileStart = clock.seconds()
+ }
+ drive.setWeightedDrivePower(
+ Pose2d(
+ -gamepad1.left_stick_y.toDouble(),
+ -gamepad1.left_stick_x.toDouble(),
+ -gamepad1.right_stick_x.toDouble()
+ )
+ )
+ }
+ }
+ telemetry.update()
+ }
+ }
+
+ internal enum class Mode {
+ DRIVER_MODE, TUNING_MODE
+ }
+
+ companion object {
+ private var DISTANCE = 72.0 // in
+ private fun generateProfile(movingForward: Boolean): MotionProfile {
+ val start = MotionState(if (movingForward) 0.0 else DISTANCE, 0.0, 0.0, 0.0)
+ val goal = MotionState(if (movingForward) DISTANCE else 0.0, 0.0, 0.0, 0.0)
+ return generateSimpleMotionProfile(start, goal, MAX_VEL, MAX_ACCEL)
+ }
+ }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/MaxAngularVeloTuner.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/MaxAngularVeloTuner.kt
new file mode 100644
index 0000000..f99e549
--- /dev/null
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/MaxAngularVeloTuner.kt
@@ -0,0 +1,70 @@
+package org.firstinspires.ftc.teamcode.opModes.tuning.roadrunner
+
+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.Disabled
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode
+import com.qualcomm.robotcore.hardware.DcMotor
+import com.qualcomm.robotcore.util.ElapsedTime
+import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem
+
+/**
+ * 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.
+ */
+@Disabled
+@Config
+@Autonomous(group = "drive")
+class MaxAngularVeloTuner : LinearOpMode() {
+ private var timer: ElapsedTime? = null
+ private var maxAngVelocity = 0.0
+
+ @Throws(InterruptedException::class)
+ override fun runOpMode() {
+ val drive = DriveSubsystem(hardwareMap)
+ drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER)
+
+ telemetry = MultipleTelemetry(telemetry, FtcDashboard.getInstance().telemetry)
+ 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(Pose2d(0.0, 0.0, 1.0))
+
+ timer = ElapsedTime()
+
+ while (!isStopRequested && timer!!.seconds() < RUNTIME) {
+ drive.updatePoseEstimate()
+
+ val (_, _, heading) = requireNotNull(
+ drive.poseVelocity
+ ) { "poseVelocity() must not be null. Ensure that the getWheelVelocities() method has been overridden in your localizer." }
+
+ maxAngVelocity = heading.coerceAtLeast(maxAngVelocity)
+ }
+ drive.setDrivePower(Pose2d())
+ telemetry.addData("Max Angular Velocity (rad)", maxAngVelocity)
+ telemetry.addData("Max Angular Velocity (deg)", Math.toDegrees(maxAngVelocity))
+ telemetry.update()
+ while (!isStopRequested) idle()
+ }
+
+ companion object {
+ var RUNTIME = 4.0
+ }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/MaxVelocityTuner.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/MaxVelocityTuner.kt
new file mode 100644
index 0000000..b679e3f
--- /dev/null
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/MaxVelocityTuner.kt
@@ -0,0 +1,87 @@
+package org.firstinspires.ftc.teamcode.opModes.tuning.roadrunner
+
+import com.acmerobotics.dashboard.FtcDashboard
+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.Disabled
+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.teamcode.subsystems.drive.DriveSubsystem
+import org.firstinspires.ftc.teamcode.utils.roadrunner.drive.DriveConstants
+import org.firstinspires.ftc.teamcode.utils.roadrunner.drive.DriveConstants.getMotorVelocityF
+
+/**
+ * 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.
+ */
+@Disabled //@Config
+@Autonomous(group = "drive")
+class MaxVelocityTuner : LinearOpMode() {
+ private lateinit var timer: ElapsedTime
+ private var maxVelocity = 0.0
+ private lateinit var batteryVoltageSensor: VoltageSensor
+
+ @Throws(InterruptedException::class)
+ override fun runOpMode() {
+ val drive = DriveSubsystem(hardwareMap)
+ drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER)
+
+ batteryVoltageSensor = hardwareMap.voltageSensor.iterator().next()
+
+ telemetry = MultipleTelemetry(telemetry, FtcDashboard.getInstance().telemetry)
+ 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(Pose2d(1.0, 0.0, 0.0))
+
+ timer = ElapsedTime()
+
+ while (!isStopRequested && timer.seconds() < RUNTIME) {
+ drive.updatePoseEstimate()
+ val poseVelo = requireNotNull(
+ drive.poseVelocity
+ ) { "poseVelocity() must not be null. Ensure that the getWheelVelocities() method has been overridden in your localizer." }
+
+ maxVelocity = poseVelo.vec().norm().coerceAtLeast(maxVelocity)
+ }
+
+ drive.setDrivePower(Pose2d())
+
+ val effectiveKf = getMotorVelocityF(veloInchesToTicks(maxVelocity))
+
+ telemetry.addData("Max Velocity", maxVelocity)
+ telemetry.addData(
+ "Voltage Compensated kF",
+ effectiveKf * batteryVoltageSensor.getVoltage() / 12
+ )
+
+ telemetry.update()
+
+ while (!isStopRequested && opModeIsActive()) idle()
+ }
+
+ private fun veloInchesToTicks(inchesPerSec: Double): Double {
+ return inchesPerSec / (2 * Math.PI * DriveConstants.WHEEL_RADIUS) / DriveConstants.GEAR_RATIO * DriveConstants.TICKS_PER_REV
+ }
+
+ companion object {
+ var RUNTIME = 2.0
+ }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/MotorDirectionDebugger.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/MotorDirectionDebugger.kt
new file mode 100644
index 0000000..c1226bf
--- /dev/null
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/MotorDirectionDebugger.kt
@@ -0,0 +1,79 @@
+package org.firstinspires.ftc.teamcode.opModes.tuning.roadrunner
+
+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.subsystems.drive.DriveSubsystem
+
+/**
+ * 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)
+ * \________/
+ *
+ */
+@Disabled
+@Config
+@TeleOp(group = "drive")
+class MotorDirectionDebugger : LinearOpMode() {
+ @Throws(InterruptedException::class)
+ override fun runOpMode() {
+ telemetry = MultipleTelemetry(telemetry, FtcDashboard.getInstance().telemetry)
+ val drive = DriveSubsystem(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 - MotorPDController")
+ 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.0, 0.0)
+ telemetry.addLine("Running MotorPDController: Front Left")
+ } else if (gamepad1.y) {
+ drive.setMotorPowers(0.0, 0.0, 0.0, MOTOR_POWER)
+ telemetry.addLine("Running MotorPDController: Front Right")
+ } else if (gamepad1.b) {
+ drive.setMotorPowers(0.0, 0.0, MOTOR_POWER, 0.0)
+ telemetry.addLine("Running MotorPDController: Rear Right")
+ } else if (gamepad1.a) {
+ drive.setMotorPowers(0.0, MOTOR_POWER, 0.0, 0.0)
+ telemetry.addLine("Running MotorPDController: Rear Left")
+ } else {
+ drive.setMotorPowers(0.0, 0.0, 0.0, 0.0)
+ telemetry.addLine("Running MotorPDController: None")
+ }
+ telemetry.update()
+ }
+ }
+
+ companion object {
+ var MOTOR_POWER = 0.7
+ }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/SplineTest.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/SplineTest.kt
new file mode 100644
index 0000000..9aaa610
--- /dev/null
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/SplineTest.kt
@@ -0,0 +1,38 @@
+package org.firstinspires.ftc.teamcode.opModes.tuning.roadrunner
+
+import com.acmerobotics.roadrunner.geometry.Pose2d
+import com.acmerobotics.roadrunner.geometry.Vector2d
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous
+import com.qualcomm.robotcore.eventloop.opmode.Disabled
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode
+import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem
+
+/*
+ * This is an example of a more complex path to really test the tuning.
+ */
+@Disabled
+@Autonomous(group = "drive")
+class SplineTest : LinearOpMode() {
+ @Throws(InterruptedException::class)
+ override fun runOpMode() {
+ val drive = DriveSubsystem(hardwareMap)
+
+ waitForStart()
+
+ if (isStopRequested) return
+
+ val traj = drive.trajectoryBuilder(Pose2d())
+ .splineTo(Vector2d(30.0, 30.0), 0.0)
+ .build()
+
+ drive.followTrajectory(traj)
+
+ sleep(2000)
+
+ drive.followTrajectory(
+ drive.trajectoryBuilder(traj.end(), true)
+ .splineTo(Vector2d(0.0, 0.0), Math.toRadians(180.0))
+ .build()
+ )
+ }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/StrafeTest.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/StrafeTest.kt
new file mode 100644
index 0000000..c319420
--- /dev/null
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/StrafeTest.kt
@@ -0,0 +1,47 @@
+package org.firstinspires.ftc.teamcode.opModes.tuning.roadrunner
+
+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.Disabled
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode
+import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem
+
+/*
+ * This is a simple routine to test translational drive capabilities.
+ */
+@Disabled
+@Config
+@Autonomous(group = "drive")
+class StrafeTest : LinearOpMode() {
+ @Throws(InterruptedException::class)
+ override fun runOpMode() {
+ telemetry = MultipleTelemetry(telemetry, FtcDashboard.getInstance().telemetry)
+
+ val drive = DriveSubsystem(hardwareMap)
+ val trajectory = drive.trajectoryBuilder(Pose2d())
+ .strafeRight(DISTANCE)
+ .build()
+
+ waitForStart()
+
+ if (isStopRequested) return
+
+ drive.followTrajectory(trajectory)
+
+ val (x, y, heading) = drive.poseEstimate
+
+ telemetry.addData("finalX", x)
+ telemetry.addData("finalY", y)
+ telemetry.addData("finalHeading", heading)
+ telemetry.update()
+
+ while (!isStopRequested && opModeIsActive());
+ }
+
+ companion object {
+ var DISTANCE = 60.0 // in
+ }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/StraightTest.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/StraightTest.kt
new file mode 100644
index 0000000..dd4c9e7
--- /dev/null
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/StraightTest.kt
@@ -0,0 +1,40 @@
+package org.firstinspires.ftc.teamcode.opModes.tuning.roadrunner
+
+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.Disabled
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode
+import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem
+
+/*
+ * This is a simple routine to test translational drive capabilities.
+ */
+@Disabled
+@Config
+@Autonomous(group = "drive")
+class StraightTest : LinearOpMode() {
+ @Throws(InterruptedException::class)
+ override fun runOpMode() {
+ telemetry = MultipleTelemetry(telemetry, FtcDashboard.getInstance().telemetry)
+ val drive = DriveSubsystem(hardwareMap)
+ val trajectory = drive.trajectoryBuilder(Pose2d())
+ .forward(DISTANCE)
+ .build()
+ waitForStart()
+ if (isStopRequested) return
+ drive.followTrajectory(trajectory)
+ val (x, y, heading) = drive.poseEstimate
+ telemetry.addData("finalX", x)
+ telemetry.addData("finalY", y)
+ telemetry.addData("finalHeading", heading)
+ telemetry.update()
+ while (!isStopRequested && opModeIsActive());
+ }
+
+ companion object {
+ var DISTANCE = 60.0 // in
+ }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/TrackWidthTuner.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/TrackWidthTuner.kt
new file mode 100644
index 0000000..59c5b29
--- /dev/null
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/TrackWidthTuner.kt
@@ -0,0 +1,81 @@
+package org.firstinspires.ftc.teamcode.opModes.tuning.roadrunner
+
+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.norm
+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.util.MovingStatistics
+import org.firstinspires.ftc.robotcore.internal.system.Misc
+import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem
+import org.firstinspires.ftc.teamcode.utils.roadrunner.drive.DriveConstants
+
+/*
+ * 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.
+ */
+@Disabled
+@Config
+@Autonomous(group = "drive")
+class TrackWidthTuner : LinearOpMode() {
+ @Throws(InterruptedException::class)
+ override fun runOpMode() {
+ telemetry = MultipleTelemetry(telemetry, FtcDashboard.getInstance().telemetry)
+ val drive = DriveSubsystem(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()
+ val trackWidthStats = MovingStatistics(NUM_TRIALS)
+ for (i in 0 until NUM_TRIALS) {
+ drive.poseEstimate = Pose2d()
+
+ // it is important to handle heading wraparounds
+ var headingAccumulator = 0.0
+ var lastHeading = 0.0
+ drive.turnAsync(Math.toRadians(ANGLE))
+ while (!isStopRequested && drive.isBusy) {
+ val heading = drive.poseEstimate.heading
+ headingAccumulator += norm(heading - lastHeading)
+ lastHeading = heading
+ drive.update()
+ }
+ val trackWidth = DriveConstants.TRACK_WIDTH * Math.toRadians(ANGLE) / headingAccumulator
+ trackWidthStats.add(trackWidth)
+ sleep(DELAY.toLong())
+ }
+ telemetry.clearAll()
+ telemetry.addLine("Tuning complete")
+ telemetry.addLine(
+ Misc.formatInvariant(
+ "Effective track width = %.2f (SE = %.3f)",
+ trackWidthStats.mean,
+ trackWidthStats.standardDeviation / Math.sqrt(NUM_TRIALS.toDouble())
+ )
+ )
+ telemetry.update()
+ while (!isStopRequested) {
+ idle()
+ }
+ }
+
+ companion object {
+ var ANGLE = 180.0 // deg
+ var NUM_TRIALS = 5
+ var DELAY = 1000 // ms
+ }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/TrackingWheelForwardOffsetTuner.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/TrackingWheelForwardOffsetTuner.kt
new file mode 100644
index 0000000..c5cc5a3
--- /dev/null
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/TrackingWheelForwardOffsetTuner.kt
@@ -0,0 +1,99 @@
+package org.firstinspires.ftc.teamcode.opModes.tuning.roadrunner
+
+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.norm
+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.util.MovingStatistics
+import com.qualcomm.robotcore.util.RobotLog
+import org.firstinspires.ftc.robotcore.internal.system.Misc
+import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem
+import org.firstinspires.ftc.teamcode.utils.roadrunner.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.
+ */
+@Disabled
+@Config
+@Autonomous(group = "drive")
+class TrackingWheelForwardOffsetTuner : LinearOpMode() {
+ @Throws(InterruptedException::class)
+ override fun runOpMode() {
+ telemetry = MultipleTelemetry(telemetry, FtcDashboard.getInstance().telemetry)
+ val drive = DriveSubsystem(hardwareMap)
+ if (drive.localizer !is 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()
+ val forwardOffsetStats = MovingStatistics(NUM_TRIALS)
+ for (i in 0 until NUM_TRIALS) {
+ drive.poseEstimate = Pose2d()
+
+ // it is important to handle heading wraparounds
+ var headingAccumulator = 0.0
+ var lastHeading = 0.0
+ drive.turnAsync(Math.toRadians(ANGLE))
+ while (!isStopRequested && drive.isBusy) {
+ val heading = drive.poseEstimate.heading
+ headingAccumulator += norm(heading - lastHeading)
+ lastHeading = heading
+ drive.update()
+ }
+ val forwardOffset = StandardTrackingWheelLocalizer.FORWARD_OFFSET +
+ drive.poseEstimate.y / headingAccumulator
+ forwardOffsetStats.add(forwardOffset)
+ sleep(DELAY.toLong())
+ }
+ telemetry.clearAll()
+ telemetry.addLine("Tuning complete")
+ telemetry.addLine(
+ Misc.formatInvariant(
+ "Effective forward offset = %.2f (SE = %.3f)",
+ forwardOffsetStats.mean,
+ forwardOffsetStats.standardDeviation / Math.sqrt(NUM_TRIALS.toDouble())
+ )
+ )
+ telemetry.update()
+ while (!isStopRequested) {
+ idle()
+ }
+ }
+
+ companion object {
+ var ANGLE = 180.0 // deg
+ var NUM_TRIALS = 5
+ var DELAY = 1000 // ms
+ }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/TrackingWheelLateralDistanceTuner.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/TrackingWheelLateralDistanceTuner.kt
new file mode 100644
index 0000000..e1fa00b
--- /dev/null
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/TrackingWheelLateralDistanceTuner.kt
@@ -0,0 +1,134 @@
+package org.firstinspires.ftc.teamcode.opModes.tuning.roadrunner
+
+import com.acmerobotics.dashboard.config.Config
+import com.acmerobotics.roadrunner.geometry.Pose2d
+import com.acmerobotics.roadrunner.util.Angle.normDelta
+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.RobotLog
+import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem
+import org.firstinspires.ftc.teamcode.utils.roadrunner.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.
+ */
+@Disabled
+@Config
+@TeleOp(group = "drive")
+class TrackingWheelLateralDistanceTuner : LinearOpMode() {
+ @Throws(InterruptedException::class)
+ override fun runOpMode() {
+ val drive = DriveSubsystem(hardwareMap)
+ if (drive.localizer !is 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()
+ var headingAccumulator = 0.0
+ var lastHeading = 0.0
+ var tuningFinished = false
+ while (!isStopRequested && !tuningFinished) {
+ val vel = Pose2d(0.0, 0.0, -gamepad1.right_stick_x.toDouble())
+ drive.setDrivePower(vel)
+ drive.update()
+ val heading = drive.poseEstimate.heading
+ val deltaHeading = heading - lastHeading
+ headingAccumulator += 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()
+ }
+
+ companion object {
+ var NUM_TURNS = 10
+ }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/TurnTest.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/TurnTest.kt
new file mode 100644
index 0000000..d58b044
--- /dev/null
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/TurnTest.kt
@@ -0,0 +1,27 @@
+package org.firstinspires.ftc.teamcode.opModes.tuning.roadrunner
+
+import com.acmerobotics.dashboard.config.Config
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous
+import com.qualcomm.robotcore.eventloop.opmode.Disabled
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode
+import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem
+
+/*
+ * This is a simple routine to test turning capabilities.
+ */
+@Disabled
+@Config
+@Autonomous(group = "drive")
+class TurnTest : LinearOpMode() {
+ @Throws(InterruptedException::class)
+ override fun runOpMode() {
+ val drive = DriveSubsystem(hardwareMap)
+ waitForStart()
+ if (isStopRequested) return
+ drive.turn(Math.toRadians(ANGLE))
+ }
+
+ companion object {
+ var ANGLE = 90.0 // deg
+ }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/readme.md b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/readme.md
new file mode 100644
index 0000000..64177c6
--- /dev/null
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/readme.md
@@ -0,0 +1,130 @@
+## 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/kotlin/org/firstinspires/ftc/teamcode/subsystems/drive/DriveSubsystem.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/drive/DriveSubsystem.kt
new file mode 100644
index 0000000..4aed5b7
--- /dev/null
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/drive/DriveSubsystem.kt
@@ -0,0 +1,305 @@
+package org.firstinspires.ftc.teamcode.subsystems.drive
+
+import com.acmerobotics.dashboard.config.Config
+import com.acmerobotics.roadrunner.control.PIDCoefficients
+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.geometry.Vector2d
+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.acmerobotics.roadrunner.util.Angle
+import com.arcrobotics.ftclib.command.CommandScheduler
+import com.arcrobotics.ftclib.command.Subsystem
+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.PIDFCoefficients
+import com.qualcomm.robotcore.hardware.VoltageSensor
+import org.firstinspires.ftc.teamcode.constants.ControlBoard
+import org.firstinspires.ftc.teamcode.constants.PoseStorage
+import org.firstinspires.ftc.teamcode.utils.roadrunner.drive.DriveConstants
+import org.firstinspires.ftc.teamcode.utils.roadrunner.drive.StandardTrackingWheelLocalizer
+import org.firstinspires.ftc.teamcode.utils.roadrunner.trajectorysequence.TrajectorySequence
+import org.firstinspires.ftc.teamcode.utils.roadrunner.trajectorysequence.TrajectorySequenceBuilder
+import org.firstinspires.ftc.teamcode.utils.roadrunner.trajectorysequence.TrajectorySequenceRunner
+import org.firstinspires.ftc.teamcode.utils.roadrunner.util.LynxModuleUtil
+import kotlin.math.PI
+import kotlin.math.abs
+
+/*
+ * Simple mecanum drive hardware implementation for REV hardware.
+ */
+@Config
+class DriveSubsystem @JvmOverloads constructor(
+ hardwareMap: HardwareMap,
+ private val fieldCentric: Boolean = false,
+) : MecanumDrive(
+ DriveConstants.kV,
+ DriveConstants.kA,
+ DriveConstants.kStatic,
+ DriveConstants.TRACK_WIDTH,
+ DriveConstants.TRACK_WIDTH,
+ LATERAL_MULTIPLIER
+), Subsystem {
+ private val trajectorySequenceRunner: TrajectorySequenceRunner
+ private val leftFront: DcMotorEx
+ private val leftRear: DcMotorEx
+ private val rightRear: DcMotorEx
+ private val rightFront: DcMotorEx
+ private val motors: List
+ private val batteryVoltageSensor: VoltageSensor
+
+ var m_name = this.javaClass.simpleName
+
+ init {
+ val follower: TrajectoryFollower = HolonomicPIDVAFollower(
+ TRANSLATIONAL_PID, TRANSLATIONAL_PID, HEADING_PID,
+ Pose2d(0.5, 0.5, Math.toRadians(5.0)), 0.5
+ )
+
+ LynxModuleUtil.ensureMinimumFirmwareVersion(hardwareMap)
+
+ batteryVoltageSensor = hardwareMap.voltageSensor.iterator().next()
+
+ for (module in hardwareMap.getAll(LynxModule::class.java)) {
+ module.bulkCachingMode = LynxModule.BulkCachingMode.AUTO
+ }
+
+ leftFront = hardwareMap.get(DcMotorEx::class.java, ControlBoard.DRIVE_LEFT_FRONT.deviceName)
+ leftRear = hardwareMap.get(DcMotorEx::class.java, ControlBoard.DRIVE_LEFT_REAR.deviceName)
+ rightRear = hardwareMap.get(DcMotorEx::class.java, ControlBoard.DRIVE_RIGHT_REAR.deviceName)
+ rightFront =
+ hardwareMap.get(DcMotorEx::class.java, ControlBoard.DRIVE_RIGHT_FRONT.deviceName)
+ motors = listOf(leftFront, leftRear, rightRear, rightFront)
+ for (motor in motors) {
+ val motorConfigurationType = motor.motorType.clone()
+ motorConfigurationType.achieveableMaxRPMFraction = 1.0
+ motor.motorType = motorConfigurationType
+ }
+
+ if (DriveConstants.RUN_USING_ENCODER) {
+ setMode(DcMotor.RunMode.RUN_USING_ENCODER)
+ }
+
+ setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE)
+
+ if (DriveConstants.RUN_USING_ENCODER) {
+ setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, DriveConstants.MOTOR_VELO_PID)
+ }
+
+ leftFront.direction = DcMotorSimple.Direction.REVERSE
+ leftRear.direction = DcMotorSimple.Direction.REVERSE
+
+ localizer = StandardTrackingWheelLocalizer(hardwareMap)
+
+ trajectorySequenceRunner = TrajectorySequenceRunner(follower, HEADING_PID)
+
+ CommandScheduler.getInstance().registerSubsystem(this)
+ }
+
+ @JvmOverloads
+ fun trajectoryBuilder(
+ startPose: Pose2d,
+ reversed: Boolean = false,
+ startHeading: Double = startPose.heading,
+ ): TrajectoryBuilder {
+ return TrajectoryBuilder(
+ startPose,
+ Angle.norm(startHeading + if (reversed) PI else 0.0),
+ VEL_CONSTRAINT,
+ ACCEL_CONSTRAINT
+ )
+ }
+
+ fun drive(leftY: Double, leftX: Double, rightX: Double) {
+ val (_, _, heading) = poseEstimate
+
+ val (x, y) = Vector2d(
+ -leftY,
+ -leftX
+ ).rotated(
+ if (fieldCentric) -heading else 0.0
+ )
+
+ setWeightedDrivePower(Pose2d(x, y, -rightX))
+
+ update()
+ PoseStorage.poseEstimate = poseEstimate
+ }
+
+ fun trajectorySequenceBuilder(startPose: Pose2d): TrajectorySequenceBuilder {
+ return TrajectorySequenceBuilder(
+ startPose,
+ VEL_CONSTRAINT, ACCEL_CONSTRAINT,
+ DriveConstants.MAX_ANG_VEL, DriveConstants.MAX_ANG_ACCEL
+ )
+ }
+
+ fun turnAsync(angle: Double) {
+ trajectorySequenceRunner.followTrajectorySequenceAsync(
+ trajectorySequenceBuilder(poseEstimate)
+ .turn(angle)
+ .build()
+ )
+ }
+
+ fun turn(angle: Double) {
+ turnAsync(angle)
+ waitForIdle()
+ }
+
+ fun followTrajectoryAsync(trajectory: Trajectory) {
+ trajectorySequenceRunner.followTrajectorySequenceAsync(
+ trajectorySequenceBuilder(trajectory.start())
+ .addTrajectory(trajectory)
+ .build()
+ )
+ }
+
+ fun followTrajectory(trajectory: Trajectory) {
+ followTrajectoryAsync(trajectory)
+ waitForIdle()
+ }
+
+ fun followTrajectorySequenceAsync(trajectorySequence: TrajectorySequence) {
+ trajectorySequenceRunner.followTrajectorySequenceAsync(trajectorySequence)
+ }
+
+ fun followTrajectorySequence(trajectorySequence: TrajectorySequence) {
+ followTrajectorySequenceAsync(trajectorySequence)
+ waitForIdle()
+ }
+
+ @Suppress("unused", "MemberVisibilityCanBePrivate")
+ val lastError: Pose2d
+ get() = trajectorySequenceRunner.lastPoseError
+
+ fun update() {
+ updatePoseEstimate()
+ val signal = trajectorySequenceRunner.update(poseEstimate, poseVelocity)
+ signal?.let { setDriveSignal(it) }
+ }
+
+ fun waitForIdle() {
+ while (!Thread.currentThread().isInterrupted && isBusy) update()
+ }
+
+ val isBusy: Boolean
+ get() = trajectorySequenceRunner.isBusy
+
+ fun setMode(runMode: DcMotor.RunMode?) {
+ for (motor in motors) {
+ motor.mode = runMode
+ }
+ }
+
+ fun setZeroPowerBehavior(zeroPowerBehavior: DcMotor.ZeroPowerBehavior) {
+ for (motor in motors) {
+ motor.zeroPowerBehavior = zeroPowerBehavior
+ }
+ }
+
+ fun setPIDFCoefficients(runMode: DcMotor.RunMode?, coefficients: PIDFCoefficients) {
+ val compensatedCoefficients = PIDFCoefficients(
+ coefficients.p, coefficients.i, coefficients.d,
+ coefficients.f * 12 / batteryVoltageSensor.voltage
+ )
+ for (motor in motors) {
+ motor.setPIDFCoefficients(runMode, compensatedCoefficients)
+ }
+ }
+
+ fun setWeightedDrivePower(drivePower: Pose2d) {
+ var vel = drivePower
+ if ((abs(drivePower.x) + abs(drivePower.y)
+ + abs(drivePower.heading)) > 1
+ ) {
+ // re-normalize the powers according to the weights
+ val denom =
+ VX_WEIGHT * abs(drivePower.x) + VY_WEIGHT * abs(drivePower.y) + OMEGA_WEIGHT * abs(
+ drivePower.heading
+ )
+ vel = Pose2d(
+ VX_WEIGHT * drivePower.x,
+ VY_WEIGHT * drivePower.y,
+ OMEGA_WEIGHT * drivePower.heading
+ ).div(denom)
+ }
+ setDrivePower(vel)
+ }
+
+ override fun getWheelPositions(): List {
+ val wheelPositions: MutableList = ArrayList()
+ for (motor in motors) {
+ wheelPositions.add(DriveConstants.encoderTicksToInches(motor.currentPosition.toDouble()))
+ }
+ return wheelPositions
+ }
+
+ @Suppress("unused")
+ override fun getWheelVelocities(): List {
+ val wheelVelocities: MutableList = ArrayList()
+ for (motor in motors) {
+ wheelVelocities.add(DriveConstants.encoderTicksToInches(motor.velocity))
+ }
+ return wheelVelocities
+ }
+
+ override fun setMotorPowers(
+ frontLeft: Double,
+ rearLeft: Double,
+ rearRight: Double,
+ frontRight: Double,
+ ) {
+ leftFront.power = frontLeft
+ leftRear.power = rearLeft
+ rightRear.power = rearRight
+ rightFront.power = frontRight
+ }
+
+ override val rawExternalHeading: Double
+ get() = 0.0
+
+ override fun getExternalHeadingVelocity() = 0.0
+
+ companion object {
+ var TRANSLATIONAL_PID = PIDCoefficients(8.0, 0.0, 0.0)
+ var HEADING_PID = PIDCoefficients(8.0, 0.0, 0.0)
+ var LATERAL_MULTIPLIER = 1.0
+ var VX_WEIGHT = 1.0
+ var VY_WEIGHT = 1.0
+ var OMEGA_WEIGHT = 1.0
+ private val VEL_CONSTRAINT = getVelocityConstraint(
+ DriveConstants.MAX_VEL,
+ DriveConstants.MAX_ANG_VEL,
+ DriveConstants.TRACK_WIDTH
+ )
+ private val ACCEL_CONSTRAINT = getAccelerationConstraint(DriveConstants.MAX_ACCEL)
+ fun getVelocityConstraint(
+ maxVel: Double,
+ maxAngularVel: Double,
+ trackWidth: Double,
+ ): TrajectoryVelocityConstraint {
+ return MinVelocityConstraint(
+ listOf(
+ AngularVelocityConstraint(maxAngularVel),
+ MecanumVelocityConstraint(maxVel, trackWidth)
+ )
+ )
+ }
+
+ fun getAccelerationConstraint(maxAccel: Double): TrajectoryAccelerationConstraint {
+ return ProfileAccelerationConstraint(maxAccel)
+ }
+ }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/PIDSubsystem.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/PIDSubsystem.kt
new file mode 100644
index 0000000..9192ae8
--- /dev/null
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/PIDSubsystem.kt
@@ -0,0 +1,39 @@
+package org.firstinspires.ftc.teamcode.utils
+
+import com.arcrobotics.ftclib.command.SubsystemBase
+import com.arcrobotics.ftclib.controller.PIDFController
+
+abstract class PIDSubsystem(
+ private val controller: PIDFController,
+) : SubsystemBase() {
+ private var enabled: Boolean = true
+ private var setpoint: Double = controller.setPoint
+ set(value) {
+ controller.setPoint = value
+ field = value
+ }
+
+ protected abstract fun useOutput(output: Double, setpoint: Double)
+
+ protected abstract fun getMeasurement(): Double
+
+ override fun periodic() {
+ if (enabled) {
+ useOutput(controller.calculate(getMeasurement()), setpoint)
+ }
+ }
+
+ fun enable() {
+ enabled = true
+ controller.reset()
+ }
+
+ fun disable() {
+ enabled = false
+ useOutput(0.0, 0.0)
+ }
+
+ fun isEnabled(): Boolean {
+ return enabled
+ }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/drive/AprilTagThreeWheelLocalizer.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/drive/AprilTagThreeWheelLocalizer.kt
new file mode 100644
index 0000000..8885d3d
--- /dev/null
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/drive/AprilTagThreeWheelLocalizer.kt
@@ -0,0 +1,24 @@
+package org.firstinspires.ftc.teamcode.utils.roadrunner.drive
+
+import com.acmerobotics.roadrunner.geometry.Pose2d
+import com.acmerobotics.roadrunner.localization.Localizer
+
+class AprilTagThreeWheelLocalizer(
+ private val odometry: StandardTrackingWheelLocalizer,
+) : Localizer {
+ private var _poseEstimate: Pose2d = Pose2d()
+
+ override var poseEstimate: Pose2d
+ get() = _poseEstimate
+ set(value) {
+ _poseEstimate = value
+ }
+
+ override val poseVelocity: Pose2d? = null
+
+ override fun update() {
+ // Return a kalman filter that takes the pose estimate from the odometry and april tags to combine
+ odometry.update()
+ TODO("Not yet implemented")
+ }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/drive/DriveConstants.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/drive/DriveConstants.kt
new file mode 100644
index 0000000..cbb3589
--- /dev/null
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/drive/DriveConstants.kt
@@ -0,0 +1,130 @@
+package org.firstinspires.ftc.teamcode.utils.roadrunner.drive
+
+import com.acmerobotics.dashboard.config.Config
+import com.qualcomm.robotcore.hardware.PIDFCoefficients
+
+/*
+ * Constants shared between multiple drive types.
+ *
+ * Constants generated by LearnRoadRunner.com/drive-constants
+ *
+ * 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
+object DriveConstants {
+ /*
+ * These are motor constants that should be listed online for your motors.
+ */
+ const val TICKS_PER_REV = 383.6
+ const val MAX_RPM = 435.0
+
+ /*
+ * 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.
+ */
+ const val RUN_USING_ENCODER = false
+
+ @JvmField
+ var MOTOR_VELO_PID = PIDFCoefficients(
+ 0.0, 0.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.
+ */
+ @JvmField
+ var WHEEL_RADIUS = 1.8898 // in
+
+ @JvmField
+ var GEAR_RATIO = 0.5 // output (wheel) speed / input (motor) speed
+
+ @JvmField
+ var TRACK_WIDTH = 15.75 // 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.
+ */
+ @JvmField
+ var kV = 0.025
+
+ @JvmField
+ var kA = 0.005
+
+ @JvmField
+ var kStatic = 0.01
+
+ /*
+ * 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.
+ */
+ /*
+ * Note from LearnRoadRunner.com:
+ * The velocity and acceleration constraints were calculated based on the following equation:
+ * ((MAX_RPM / 60) * GEAR_RATIO * WHEEL_RADIUS * 2 * Math.PI) * 0.85
+ * Resulting in 36.58665032249647 in/s.
+ * This is only 85% of the theoretical maximum velocity of the bot, following the recommendation above.
+ * This is capped at 85% because there are a number of variables that will prevent your bot from actually
+ * reaching this maximum velocity: voltage dropping over the game, bot weight, general mechanical inefficiencies, etc.
+ * However, you can push this higher yourself if you'd like. Perhaps raise it to 90-95% of the theoretically
+ * max velocity. The theoretically maximum velocity is 43.04311802646643 in/s.
+ * Just make sure that your bot can actually reach this maximum velocity. Path following will be detrimentally
+ * affected if it is aiming for a velocity not actually possible.
+ *
+ * The maximum acceleration is somewhat arbitrary and it is recommended that you tweak this yourself based on
+ * actual testing. Just set it at a reasonable value and keep increasing until your path following starts
+ * to degrade. As of now, it simply mirrors the velocity, resulting in 36.58665032249647 in/s/s
+ *
+ * Maximum Angular Velocity is calculated as: maximum velocity / trackWidth * (180 / Math.PI) but capped at 360°/s.
+ * You are free to raise this on your own if you would like. It is best determined through experimentation.
+
+ */
+ @JvmField
+ var MAX_VEL = 36.58665032249647
+
+ @JvmField
+ var MAX_ACCEL = 36.58665032249647
+
+ @JvmField
+ var MAX_ANG_VEL = Math.toRadians(163.028007)
+
+ @JvmField
+ var MAX_ANG_ACCEL = Math.toRadians(123.30945)
+
+ @JvmStatic
+ fun encoderTicksToInches(ticks: Double): Double {
+ return WHEEL_RADIUS * 2 * Math.PI * GEAR_RATIO * ticks / TICKS_PER_REV
+ }
+
+ @JvmStatic
+ fun rpmToVelocity(rpm: Double): Double {
+ return rpm * GEAR_RATIO * 2 * Math.PI * WHEEL_RADIUS / 60.0
+ }
+
+ @JvmStatic
+ fun getMotorVelocityF(ticksPerSecond: Double): Double {
+ // see https://docs.google.com/document/d/1tyWrXDfMidwYyP_5H4mZyVgaEswhOC35gvdmP-V-5hA/edit#heading=h.61g9ixenznbx
+ return 32767 / ticksPerSecond
+ }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/drive/StandardTrackingWheelLocalizer.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/drive/StandardTrackingWheelLocalizer.kt
new file mode 100644
index 0000000..897808e
--- /dev/null
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/drive/StandardTrackingWheelLocalizer.kt
@@ -0,0 +1,83 @@
+package org.firstinspires.ftc.teamcode.utils.roadrunner.drive
+
+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.constants.ControlBoard
+import org.firstinspires.ftc.teamcode.utils.roadrunner.util.Encoder
+
+/*
+ * Sample tracking wheel localizer implementation assuming the standard configuration:
+ *
+ * /--------------\
+ * | ____ |
+ * | ---- |
+ * | || || |
+ * | || || |
+ * | |
+ * | |
+ * \--------------/
+ *
+ */
+@Config
+class StandardTrackingWheelLocalizer(hardwareMap: HardwareMap) : ThreeTrackingWheelLocalizer(
+ listOf(
+ Pose2d(0.0, LATERAL_DISTANCE / 2, 0.0), // left
+ Pose2d(0.0, -LATERAL_DISTANCE / 2, 0.0), // right
+ Pose2d(FORWARD_OFFSET, 0.0, Math.toRadians(90.0)) // front
+ )
+) {
+ private val leftEncoder: Encoder
+ private val rightEncoder: Encoder
+ private val strafeEncoder: Encoder
+
+ init {
+ leftEncoder = Encoder(
+ hardwareMap.get(
+ DcMotorEx::class.java, ControlBoard.ODO_LEFT_ENCODER.deviceName
+ )
+ )
+ rightEncoder = Encoder(
+ hardwareMap.get(
+ DcMotorEx::class.java, ControlBoard.ODO_RIGHT_ENCODER.deviceName
+ )
+ )
+ strafeEncoder = Encoder(
+ hardwareMap.get(
+ DcMotorEx::class.java, ControlBoard.ODO_STRAFE_ENCODER.deviceName
+ )
+ )
+ }
+
+ override fun getWheelPositions() = listOf(
+ encoderTicksToInches(leftEncoder.currentPosition.toDouble()) * X_MULTIPLIER,
+ encoderTicksToInches(rightEncoder.currentPosition.toDouble()) * X_MULTIPLIER,
+ encoderTicksToInches(strafeEncoder.currentPosition.toDouble()) * Y_MULTIPLIER
+ )
+
+ override fun getWheelVelocities() = listOf(
+ encoderTicksToInches(leftEncoder.correctedVelocity) * X_MULTIPLIER,
+ encoderTicksToInches(rightEncoder.correctedVelocity) * X_MULTIPLIER,
+ encoderTicksToInches(strafeEncoder.correctedVelocity) * Y_MULTIPLIER
+ )
+
+ companion object {
+ val TICKS_PER_REV = 8192.0
+ val WHEEL_RADIUS = 0.74803 // in
+ val GEAR_RATIO = 1.0 // output (wheel) speed / input (encoder) speed
+
+ @JvmField
+ var LATERAL_DISTANCE = 15.48986087647907 // in; distance between the left and right wheels
+
+ @JvmField
+ var FORWARD_OFFSET = 0.0 // in; offset of the lateral wheel
+
+ var X_MULTIPLIER = 0.9943625369724665 // Multiplier in the X direction
+ var Y_MULTIPLIER = 0.9973390034543611 // Multiplier in the Y direction
+ fun encoderTicksToInches(ticks: Double): Double {
+ return WHEEL_RADIUS * 2 * Math.PI * GEAR_RATIO * ticks / TICKS_PER_REV
+ }
+ }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/trajectorysequence/EmptySequenceException.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/trajectorysequence/EmptySequenceException.kt
new file mode 100644
index 0000000..9f18020
--- /dev/null
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/trajectorysequence/EmptySequenceException.kt
@@ -0,0 +1,3 @@
+package org.firstinspires.ftc.teamcode.utils.roadrunner.trajectorysequence
+
+class EmptySequenceException : RuntimeException()
\ No newline at end of file
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/trajectorysequence/TrajectorySequence.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/trajectorysequence/TrajectorySequence.kt
new file mode 100644
index 0000000..11a191e
--- /dev/null
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/trajectorysequence/TrajectorySequence.kt
@@ -0,0 +1,38 @@
+package org.firstinspires.ftc.teamcode.utils.roadrunner.trajectorysequence
+
+import com.acmerobotics.roadrunner.geometry.Pose2d
+import org.firstinspires.ftc.teamcode.utils.roadrunner.trajectorysequence.sequencesegment.SequenceSegment
+import java.util.Collections
+
+class TrajectorySequence(sequenceList: List) {
+ private val sequenceList: List
+
+ init {
+ if (sequenceList.isEmpty()) throw EmptySequenceException()
+ this.sequenceList = Collections.unmodifiableList(sequenceList)
+ }
+
+ fun start(): Pose2d {
+ return sequenceList[0].startPose
+ }
+
+ fun end(): Pose2d {
+ return sequenceList[sequenceList.size - 1].endPose
+ }
+
+ fun duration(): Double {
+ var total = 0.0
+ for (segment in sequenceList) {
+ total += segment.duration
+ }
+ return total
+ }
+
+ operator fun get(i: Int): SequenceSegment {
+ return sequenceList[i]
+ }
+
+ fun size(): Int {
+ return sequenceList.size
+ }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/trajectorysequence/TrajectorySequenceBuilder.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/trajectorysequence/TrajectorySequenceBuilder.kt
new file mode 100644
index 0000000..191cd1d
--- /dev/null
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/trajectorysequence/TrajectorySequenceBuilder.kt
@@ -0,0 +1,809 @@
+package org.firstinspires.ftc.teamcode.utils.roadrunner.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.generateSimpleMotionProfile
+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.norm
+import org.firstinspires.ftc.teamcode.utils.roadrunner.trajectorysequence.TrajectorySequenceBuilder.AddPathCallback
+import org.firstinspires.ftc.teamcode.utils.roadrunner.trajectorysequence.sequencesegment.SequenceSegment
+import org.firstinspires.ftc.teamcode.utils.roadrunner.trajectorysequence.sequencesegment.TrajectorySegment
+import org.firstinspires.ftc.teamcode.utils.roadrunner.trajectorysequence.sequencesegment.TurnSegment
+import org.firstinspires.ftc.teamcode.utils.roadrunner.trajectorysequence.sequencesegment.WaitSegment
+import kotlin.math.abs
+
+class TrajectorySequenceBuilder(
+ startPose: Pose2d,
+ startTangent: Double?,
+ private val baseVelConstraint: TrajectoryVelocityConstraint,
+ private val baseAccelConstraint: TrajectoryAccelerationConstraint,
+ baseTurnConstraintMaxAngVel: Double,
+ baseTurnConstraintMaxAngAccel: Double,
+) {
+ private val resolution = 0.25
+ private val baseTurnConstraintMaxAngVel: Double
+ private val baseTurnConstraintMaxAngAccel: Double
+ private val sequenceSegments: MutableList
+ private val temporalMarkers: MutableList
+ private val displacementMarkers: MutableList
+ private val spatialMarkers: MutableList
+ private var currentVelConstraint: TrajectoryVelocityConstraint
+ private var currentAccelConstraint: TrajectoryAccelerationConstraint
+ private var currentTurnConstraintMaxAngVel: Double
+ private var currentTurnConstraintMaxAngAccel: Double
+ private var lastPose: Pose2d
+ private var tangentOffset: Double
+ private var setAbsoluteTangent: Boolean
+ private var absoluteTangent: Double
+ private var currentTrajectoryBuilder: TrajectoryBuilder?
+ private var currentDuration: Double
+ private var currentDisplacement: Double
+ private var lastDurationTraj: Double
+ private var lastDisplacementTraj: Double
+
+ init {
+ currentVelConstraint = baseVelConstraint
+ currentAccelConstraint = baseAccelConstraint
+ this.baseTurnConstraintMaxAngVel = baseTurnConstraintMaxAngVel
+ this.baseTurnConstraintMaxAngAccel = baseTurnConstraintMaxAngAccel
+ currentTurnConstraintMaxAngVel = baseTurnConstraintMaxAngVel
+ currentTurnConstraintMaxAngAccel = baseTurnConstraintMaxAngAccel
+ sequenceSegments = ArrayList()
+ temporalMarkers = ArrayList()
+ displacementMarkers = ArrayList()
+ spatialMarkers = ArrayList()
+ lastPose = startPose
+ tangentOffset = 0.0
+ setAbsoluteTangent = startTangent != null
+ absoluteTangent = startTangent ?: 0.0
+ currentTrajectoryBuilder = null
+ currentDuration = 0.0
+ currentDisplacement = 0.0
+ lastDurationTraj = 0.0
+ lastDisplacementTraj = 0.0
+ }
+
+ constructor(
+ startPose: Pose2d,
+ baseVelConstraint: TrajectoryVelocityConstraint,
+ baseAccelConstraint: TrajectoryAccelerationConstraint,
+ baseTurnConstraintMaxAngVel: Double,
+ baseTurnConstraintMaxAngAccel: Double,
+ ) : this(
+ startPose, null,
+ baseVelConstraint, baseAccelConstraint,
+ baseTurnConstraintMaxAngVel, baseTurnConstraintMaxAngAccel
+ )
+
+ fun lineTo(endPosition: Vector2d): TrajectorySequenceBuilder {
+ return addPath {
+ currentTrajectoryBuilder!!.lineTo(
+ endPosition, currentVelConstraint, currentAccelConstraint
+ )
+ }
+ }
+
+ fun lineTo(
+ endPosition: Vector2d,
+ velConstraint: TrajectoryVelocityConstraint?,
+ accelConstraint: TrajectoryAccelerationConstraint?,
+ ): TrajectorySequenceBuilder {
+ return addPath {
+ currentTrajectoryBuilder!!.lineTo(
+ endPosition, velConstraint, accelConstraint
+ )
+ }
+ }
+
+ fun lineToConstantHeading(endPosition: Vector2d): TrajectorySequenceBuilder {
+ return addPath {
+ currentTrajectoryBuilder!!.lineToConstantHeading(
+ endPosition, currentVelConstraint, currentAccelConstraint
+ )
+ }
+ }
+
+ fun lineToConstantHeading(
+ endPosition: Vector2d,
+ velConstraint: TrajectoryVelocityConstraint?,
+ accelConstraint: TrajectoryAccelerationConstraint?,
+ ): TrajectorySequenceBuilder {
+ return addPath {
+ currentTrajectoryBuilder!!.lineToConstantHeading(
+ endPosition, velConstraint, accelConstraint
+ )
+ }
+ }
+
+ fun lineToLinearHeading(endPose: Pose2d): TrajectorySequenceBuilder {
+ return addPath {
+ currentTrajectoryBuilder!!.lineToLinearHeading(
+ endPose, currentVelConstraint, currentAccelConstraint
+ )
+ }
+ }
+
+ fun lineToLinearHeading(
+ endPose: Pose2d,
+ velConstraint: TrajectoryVelocityConstraint?,
+ accelConstraint: TrajectoryAccelerationConstraint?,
+ ): TrajectorySequenceBuilder {
+ return addPath {
+ currentTrajectoryBuilder!!.lineToLinearHeading(
+ endPose, velConstraint, accelConstraint
+ )
+ }
+ }
+
+ fun lineToSplineHeading(endPose: Pose2d): TrajectorySequenceBuilder {
+ return addPath(AddPathCallback {
+ currentTrajectoryBuilder!!.lineToSplineHeading(
+ endPose, currentVelConstraint, currentAccelConstraint
+ )
+ })
+ }
+
+ fun lineToSplineHeading(
+ endPose: Pose2d,
+ velConstraint: TrajectoryVelocityConstraint?,
+ accelConstraint: TrajectoryAccelerationConstraint?,
+ ): TrajectorySequenceBuilder {
+ return addPath {
+ currentTrajectoryBuilder!!.lineToSplineHeading(
+ endPose, velConstraint, accelConstraint
+ )
+ }
+ }
+
+ fun strafeTo(endPosition: Vector2d): TrajectorySequenceBuilder {
+ return addPath {
+ currentTrajectoryBuilder!!.strafeTo(
+ endPosition, currentVelConstraint, currentAccelConstraint
+ )
+ }
+ }
+
+ fun strafeTo(
+ endPosition: Vector2d,
+ velConstraint: TrajectoryVelocityConstraint?,
+ accelConstraint: TrajectoryAccelerationConstraint?,
+ ): TrajectorySequenceBuilder {
+ return addPath {
+ currentTrajectoryBuilder!!.strafeTo(
+ endPosition, velConstraint, accelConstraint
+ )
+ }
+ }
+
+ fun forward(distance: Double): TrajectorySequenceBuilder {
+ return addPath {
+ currentTrajectoryBuilder!!.forward(
+ distance,
+ currentVelConstraint,
+ currentAccelConstraint
+ )
+ }
+ }
+
+ fun forward(
+ distance: Double,
+ velConstraint: TrajectoryVelocityConstraint?,
+ accelConstraint: TrajectoryAccelerationConstraint?,
+ ): TrajectorySequenceBuilder {
+ return addPath {
+ currentTrajectoryBuilder!!.forward(
+ distance,
+ velConstraint,
+ accelConstraint
+ )
+ }
+ }
+
+ fun back(distance: Double): TrajectorySequenceBuilder {
+ return addPath {
+ currentTrajectoryBuilder!!.back(
+ distance,
+ currentVelConstraint,
+ currentAccelConstraint
+ )
+ }
+ }
+
+ fun back(
+ distance: Double,
+ velConstraint: TrajectoryVelocityConstraint?,
+ accelConstraint: TrajectoryAccelerationConstraint?,
+ ): TrajectorySequenceBuilder {
+ return addPath(AddPathCallback {
+ currentTrajectoryBuilder!!.back(
+ distance,
+ velConstraint,
+ accelConstraint
+ )
+ })
+ }
+
+ fun strafeLeft(distance: Double): TrajectorySequenceBuilder {
+ return addPath(AddPathCallback {
+ currentTrajectoryBuilder!!.strafeLeft(
+ distance,
+ currentVelConstraint,
+ currentAccelConstraint
+ )
+ })
+ }
+
+ fun strafeLeft(
+ distance: Double,
+ velConstraint: TrajectoryVelocityConstraint?,
+ accelConstraint: TrajectoryAccelerationConstraint?,
+ ): TrajectorySequenceBuilder {
+ return addPath(AddPathCallback {
+ currentTrajectoryBuilder!!.strafeLeft(
+ distance,
+ velConstraint,
+ accelConstraint
+ )
+ })
+ }
+
+ fun strafeRight(distance: Double): TrajectorySequenceBuilder {
+ return addPath(AddPathCallback {
+ currentTrajectoryBuilder!!.strafeRight(
+ distance,
+ currentVelConstraint,
+ currentAccelConstraint
+ )
+ })
+ }
+
+ fun strafeRight(
+ distance: Double,
+ velConstraint: TrajectoryVelocityConstraint?,
+ accelConstraint: TrajectoryAccelerationConstraint?,
+ ): TrajectorySequenceBuilder {
+ return addPath(AddPathCallback {
+ currentTrajectoryBuilder!!.strafeRight(
+ distance,
+ velConstraint,
+ accelConstraint
+ )
+ })
+ }
+
+ fun splineTo(endPosition: Vector2d, endHeading: Double): TrajectorySequenceBuilder {
+ return addPath {
+ currentTrajectoryBuilder!!.splineTo(
+ endPosition, endHeading, currentVelConstraint, currentAccelConstraint
+ )
+ }
+ }
+
+ fun splineTo(
+ endPosition: Vector2d,
+ endHeading: Double,
+ velConstraint: TrajectoryVelocityConstraint?,
+ accelConstraint: TrajectoryAccelerationConstraint?,
+ ): TrajectorySequenceBuilder {
+ return addPath {
+ currentTrajectoryBuilder!!.splineTo(
+ endPosition, endHeading, velConstraint, accelConstraint
+ )
+ }
+ }
+
+ fun splineToConstantHeading(
+ endPosition: Vector2d,
+ endHeading: Double,
+ ): TrajectorySequenceBuilder {
+ return addPath {
+ currentTrajectoryBuilder!!.splineToConstantHeading(
+ endPosition, endHeading, currentVelConstraint, currentAccelConstraint
+ )
+ }
+ }
+
+ fun splineToConstantHeading(
+ endPosition: Vector2d,
+ endHeading: Double,
+ velConstraint: TrajectoryVelocityConstraint?,
+ accelConstraint: TrajectoryAccelerationConstraint?,
+ ): TrajectorySequenceBuilder {
+ return addPath {
+ currentTrajectoryBuilder!!.splineToConstantHeading(
+ endPosition, endHeading, velConstraint, accelConstraint
+ )
+ }
+ }
+
+ fun splineToLinearHeading(endPose: Pose2d, endHeading: Double): TrajectorySequenceBuilder {
+ return addPath {
+ currentTrajectoryBuilder!!.splineToLinearHeading(
+ endPose, endHeading, currentVelConstraint, currentAccelConstraint
+ )
+ }
+ }
+
+ fun splineToLinearHeading(
+ endPose: Pose2d,
+ endHeading: Double,
+ velConstraint: TrajectoryVelocityConstraint?,
+ accelConstraint: TrajectoryAccelerationConstraint?,
+ ): TrajectorySequenceBuilder {
+ return addPath {
+ currentTrajectoryBuilder!!.splineToLinearHeading(
+ endPose, endHeading, velConstraint, accelConstraint
+ )
+ }
+ }
+
+ fun splineToSplineHeading(endPose: Pose2d, endHeading: Double): TrajectorySequenceBuilder {
+ return addPath {
+ currentTrajectoryBuilder!!.splineToSplineHeading(
+ endPose, endHeading, currentVelConstraint, currentAccelConstraint
+ )
+ }
+ }
+
+ fun splineToSplineHeading(
+ endPose: Pose2d,
+ endHeading: Double,
+ velConstraint: TrajectoryVelocityConstraint?,
+ accelConstraint: TrajectoryAccelerationConstraint?,
+ ): TrajectorySequenceBuilder {
+ return addPath {
+ currentTrajectoryBuilder!!.splineToSplineHeading(
+ endPose, endHeading, velConstraint, accelConstraint
+ )
+ }
+ }
+
+ private fun addPath(callback: AddPathCallback): TrajectorySequenceBuilder {
+ if (currentTrajectoryBuilder == null) newPath()
+ try {
+ callback.run()
+ } catch (e: PathContinuityViolationException) {
+ newPath()
+ callback.run()
+ }
+ val builtTraj = currentTrajectoryBuilder!!.build()
+ val durationDifference = builtTraj.duration() - lastDurationTraj
+ val displacementDifference = builtTraj.path.length() - lastDisplacementTraj
+ lastPose = builtTraj.end()
+ currentDuration += durationDifference
+ currentDisplacement += displacementDifference
+ lastDurationTraj = builtTraj.duration()
+ lastDisplacementTraj = builtTraj.path.length()
+ return this
+ }
+
+ fun setTangent(tangent: Double): TrajectorySequenceBuilder {
+ setAbsoluteTangent = true
+ absoluteTangent = tangent
+ pushPath()
+ return this
+ }
+
+ private fun setTangentOffset(offset: Double): TrajectorySequenceBuilder {
+ setAbsoluteTangent = false
+ tangentOffset = offset
+ pushPath()
+ return this
+ }
+
+ fun setReversed(reversed: Boolean): TrajectorySequenceBuilder {
+ return if (reversed) setTangentOffset(Math.toRadians(180.0)) else setTangentOffset(0.0)
+ }
+
+ fun setConstraints(
+ velConstraint: TrajectoryVelocityConstraint,
+ accelConstraint: TrajectoryAccelerationConstraint,
+ ): TrajectorySequenceBuilder {
+ currentVelConstraint = velConstraint
+ currentAccelConstraint = accelConstraint
+ return this
+ }
+
+ fun resetConstraints(): TrajectorySequenceBuilder {
+ currentVelConstraint = baseVelConstraint
+ currentAccelConstraint = baseAccelConstraint
+ return this
+ }
+
+ fun setVelConstraint(velConstraint: TrajectoryVelocityConstraint): TrajectorySequenceBuilder {
+ currentVelConstraint = velConstraint
+ return this
+ }
+
+ fun resetVelConstraint(): TrajectorySequenceBuilder {
+ currentVelConstraint = baseVelConstraint
+ return this
+ }
+
+ fun setAccelConstraint(accelConstraint: TrajectoryAccelerationConstraint): TrajectorySequenceBuilder {
+ currentAccelConstraint = accelConstraint
+ return this
+ }
+
+ fun resetAccelConstraint(): TrajectorySequenceBuilder {
+ currentAccelConstraint = baseAccelConstraint
+ return this
+ }
+
+ fun setTurnConstraint(maxAngVel: Double, maxAngAccel: Double): TrajectorySequenceBuilder {
+ currentTurnConstraintMaxAngVel = maxAngVel
+ currentTurnConstraintMaxAngAccel = maxAngAccel
+ return this
+ }
+
+ fun resetTurnConstraint(): TrajectorySequenceBuilder {
+ currentTurnConstraintMaxAngVel = baseTurnConstraintMaxAngVel
+ currentTurnConstraintMaxAngAccel = baseTurnConstraintMaxAngAccel
+ return this
+ }
+
+ fun addTemporalMarker(callback: MarkerCallback?): TrajectorySequenceBuilder {
+ return this.addTemporalMarker(currentDuration, callback)
+ }
+
+ fun UNSTABLE_addTemporalMarkerOffset(
+ offset: Double,
+ callback: MarkerCallback?,
+ ): TrajectorySequenceBuilder {
+ return this.addTemporalMarker(currentDuration + offset, callback)
+ }
+
+ fun addTemporalMarker(time: Double, callback: MarkerCallback?): TrajectorySequenceBuilder {
+ return this.addTemporalMarker(0.0, time, callback)
+ }
+
+ fun addTemporalMarker(
+ scale: Double,
+ offset: Double,
+ callback: MarkerCallback?,
+ ): TrajectorySequenceBuilder {
+ return this.addTemporalMarker({ time: Double -> scale * time + offset }, callback)
+ }
+
+ fun addTemporalMarker(
+ time: TimeProducer?,
+ callback: MarkerCallback?,
+ ): TrajectorySequenceBuilder {
+ temporalMarkers.add(TemporalMarker(time!!, callback!!))
+ return this
+ }
+
+ fun addSpatialMarker(point: Vector2d?, callback: MarkerCallback?): TrajectorySequenceBuilder {
+ spatialMarkers.add(SpatialMarker(point!!, callback!!))
+ return this
+ }
+
+ fun addDisplacementMarker(callback: MarkerCallback?): TrajectorySequenceBuilder {
+ return this.addDisplacementMarker(currentDisplacement, callback)
+ }
+
+ fun UNSTABLE_addDisplacementMarkerOffset(
+ offset: Double,
+ callback: MarkerCallback?,
+ ): TrajectorySequenceBuilder {
+ return this.addDisplacementMarker(currentDisplacement + offset, callback)
+ }
+
+ fun addDisplacementMarker(
+ displacement: Double,
+ callback: MarkerCallback?,
+ ): TrajectorySequenceBuilder {
+ return this.addDisplacementMarker(0.0, displacement, callback)
+ }
+
+ fun addDisplacementMarker(
+ scale: Double,
+ offset: Double,
+ callback: MarkerCallback?,
+ ): TrajectorySequenceBuilder {
+ return addDisplacementMarker(
+ { displacement: Double -> scale * displacement + offset },
+ callback
+ )
+ }
+
+ fun addDisplacementMarker(
+ displacement: DisplacementProducer?,
+ callback: MarkerCallback?,
+ ): TrajectorySequenceBuilder {
+ displacementMarkers.add(DisplacementMarker(displacement!!, callback!!))
+ return this
+ }
+
+ @JvmOverloads
+ fun turn(
+ angle: Double,
+ maxAngVel: Double = currentTurnConstraintMaxAngVel,
+ maxAngAccel: Double = currentTurnConstraintMaxAngAccel,
+ ): TrajectorySequenceBuilder {
+ pushPath()
+ val turnProfile = generateSimpleMotionProfile(
+ MotionState(lastPose.heading, 0.0, 0.0, 0.0),
+ MotionState(lastPose.heading + angle, 0.0, 0.0, 0.0),
+ maxAngVel,
+ maxAngAccel
+ )
+ sequenceSegments.add(TurnSegment(lastPose, angle, turnProfile, emptyList()))
+ lastPose = Pose2d(
+ lastPose.x, lastPose.y,
+ norm(lastPose.heading + angle)
+ )
+ currentDuration += turnProfile.duration()
+ return this
+ }
+
+ fun waitSeconds(seconds: Double): TrajectorySequenceBuilder {
+ pushPath()
+ sequenceSegments.add(WaitSegment(lastPose, seconds, emptyList()))
+ currentDuration += seconds
+ return this
+ }
+
+ fun addTrajectory(trajectory: Trajectory): TrajectorySequenceBuilder {
+ pushPath()
+ sequenceSegments.add(TrajectorySegment(trajectory))
+ return this
+ }
+
+ private fun pushPath() {
+ if (currentTrajectoryBuilder != null) {
+ val builtTraj = currentTrajectoryBuilder!!.build()
+ sequenceSegments.add(TrajectorySegment(builtTraj))
+ }
+ currentTrajectoryBuilder = null
+ }
+
+ private fun newPath() {
+ if (currentTrajectoryBuilder != null) pushPath()
+ lastDurationTraj = 0.0
+ lastDisplacementTraj = 0.0
+ val tangent =
+ if (setAbsoluteTangent) absoluteTangent else norm(lastPose.heading + tangentOffset)
+ currentTrajectoryBuilder = TrajectoryBuilder(
+ lastPose,
+ tangent,
+ currentVelConstraint,
+ currentAccelConstraint,
+ resolution
+ )
+ }
+
+ fun build(): TrajectorySequence {
+ pushPath()
+ val globalMarkers = convertMarkersToGlobal(
+ sequenceSegments,
+ temporalMarkers, displacementMarkers, spatialMarkers
+ )
+ return TrajectorySequence(
+ projectGlobalMarkersToLocalSegments(
+ globalMarkers,
+ sequenceSegments
+ )
+ )
+ }
+
+ private fun convertMarkersToGlobal(
+ sequenceSegments: List,
+ temporalMarkers: List,
+ displacementMarkers: List,
+ spatialMarkers: List,
+ ): List {
+ val trajectoryMarkers = ArrayList()
+
+ // Convert temporal markers
+ for ((producer, callback) in temporalMarkers) {
+ trajectoryMarkers.add(
+ TrajectoryMarker(producer.produce(currentDuration), callback)
+ )
+ }
+
+ // Convert displacement markers
+ for ((producer, callback) in displacementMarkers) {
+ val time = displacementToTime(
+ sequenceSegments,
+ producer.produce(currentDisplacement)
+ )
+ trajectoryMarkers.add(
+ TrajectoryMarker(
+ time,
+ callback
+ )
+ )
+ }
+
+ // Convert spatial markers
+ for ((point, callback) in spatialMarkers) {
+ trajectoryMarkers.add(
+ TrajectoryMarker(
+ pointToTime(sequenceSegments, point),
+ callback
+ )
+ )
+ }
+ return trajectoryMarkers
+ }
+
+ private fun projectGlobalMarkersToLocalSegments(
+ markers: List,
+ sequenceSegments: MutableList,
+ ): List {
+ if (sequenceSegments.isEmpty()) return emptyList()
+
+ var totalSequenceDuration = 0.0
+ for (segment in sequenceSegments) {
+ totalSequenceDuration += segment.duration
+ }
+
+ for ((time, callback) in markers) {
+ lateinit var segment: SequenceSegment
+ var segmentIndex = 0
+ var segmentOffsetTime = 0.0
+ var currentTime = 0.0
+
+ for (i in sequenceSegments.indices) {
+ val seg = sequenceSegments[i]
+ val markerTime = Math.min(time, totalSequenceDuration)
+ if (currentTime + seg.duration >= markerTime) {
+ segment = seg
+ segmentIndex = i
+ segmentOffsetTime = markerTime - currentTime
+ break
+ } else {
+ currentTime += seg.duration
+ }
+ }
+ lateinit var newSegment: SequenceSegment
+
+ when (segment) {
+ is WaitSegment -> {
+ val newMarkers: MutableList = ArrayList(segment.markers)
+
+ newMarkers.addAll(sequenceSegments[segmentIndex].markers)
+ newMarkers.add(TrajectoryMarker(segmentOffsetTime, callback))
+
+ val thisSegment = segment
+
+ newSegment =
+ WaitSegment(thisSegment.startPose, thisSegment.duration, newMarkers)
+ }
+
+ is TurnSegment -> {
+ val newMarkers: MutableList = ArrayList(segment.markers)
+
+ newMarkers.addAll(sequenceSegments[segmentIndex].markers)
+ newMarkers.add(TrajectoryMarker(segmentOffsetTime, callback))
+
+ val thisSegment = segment
+
+ newSegment = TurnSegment(
+ thisSegment.startPose,
+ thisSegment.totalRotation,
+ thisSegment.motionProfile,
+ newMarkers
+ )
+ }
+
+ is TrajectorySegment -> {
+ val thisSegment = segment
+
+ val newMarkers: MutableList =
+ ArrayList(thisSegment.trajectory.markers)
+ newMarkers.add(TrajectoryMarker(segmentOffsetTime, callback))
+
+ newSegment = TrajectorySegment(
+ Trajectory(
+ thisSegment.trajectory.path,
+ thisSegment.trajectory.profile,
+ newMarkers
+ )
+ )
+ }
+ }
+
+ sequenceSegments[segmentIndex] = newSegment
+ }
+ return sequenceSegments
+ }
+
+ // Taken from Road Runner's TrajectoryGenerator.displacementToTime() since it's private
+ // note: this assumes that the profile position is monotonic increasing
+ private fun motionProfileDisplacementToTime(profile: MotionProfile, s: Double): Double {
+ var tLo = 0.0
+ var tHi = profile.duration()
+ while (abs(tLo - tHi) >= 1e-6) {
+ val tMid = 0.5 * (tLo + tHi)
+ if (profile[tMid].x > s) {
+ tHi = tMid
+ } else {
+ tLo = tMid
+ }
+ }
+ return 0.5 * (tLo + tHi)
+ }
+
+ private fun displacementToTime(sequenceSegments: List, s: Double): Double {
+ var currentTime = 0.0
+ var currentDisplacement = 0.0
+ for (segment in sequenceSegments) {
+ if (segment is TrajectorySegment) {
+ val segmentLength = segment.trajectory.path.length()
+ if (currentDisplacement + segmentLength > s) {
+ val target = s - currentDisplacement
+ val timeInSegment = motionProfileDisplacementToTime(
+ segment.trajectory.profile,
+ target
+ )
+ return currentTime + timeInSegment
+ } else {
+ currentDisplacement += segmentLength
+ currentTime += segment.trajectory.duration()
+ }
+ } else {
+ currentTime += segment.duration
+ }
+ }
+ return 0.0
+ }
+
+ private fun pointToTime(sequenceSegments: List, point: Vector2d): Double {
+ class ComparingPoints(
+ val distanceToPoint: Double,
+ val totalDisplacement: Double,
+ val thisPathDisplacement: Double,
+ )
+
+ val projectedPoints: MutableList = ArrayList()
+ for (segment in sequenceSegments) {
+ if (segment is TrajectorySegment) {
+ val thisSegment = segment
+ val displacement = thisSegment.trajectory.path.project(point, 0.25)
+ val projectedPoint = thisSegment.trajectory.path[displacement].vec()
+ val distanceToPoint = point.minus(projectedPoint).norm()
+ var totalDisplacement = 0.0
+ for (comparingPoint in projectedPoints) {
+ totalDisplacement += comparingPoint.totalDisplacement
+ }
+ totalDisplacement += displacement
+ projectedPoints.add(
+ ComparingPoints(
+ distanceToPoint,
+ displacement,
+ totalDisplacement
+ )
+ )
+ }
+ }
+ var closestPoint: ComparingPoints? = null
+ for (comparingPoint in projectedPoints) {
+ if (closestPoint == null) {
+ closestPoint = comparingPoint
+ continue
+ }
+ if (comparingPoint.distanceToPoint < closestPoint.distanceToPoint) closestPoint =
+ comparingPoint
+ }
+ return displacementToTime(sequenceSegments, closestPoint!!.thisPathDisplacement)
+ }
+
+ private fun interface AddPathCallback {
+ fun run()
+ }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/trajectorysequence/TrajectorySequenceRunner.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/trajectorysequence/TrajectorySequenceRunner.kt
new file mode 100644
index 0000000..3311474
--- /dev/null
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/trajectorysequence/TrajectorySequenceRunner.kt
@@ -0,0 +1,237 @@
+package org.firstinspires.ftc.teamcode.utils.roadrunner.trajectorysequence
+
+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.trajectory.TrajectoryMarker
+import com.acmerobotics.roadrunner.util.NanoClock
+import org.firstinspires.ftc.teamcode.utils.roadrunner.trajectorysequence.sequencesegment.SequenceSegment
+import org.firstinspires.ftc.teamcode.utils.roadrunner.trajectorysequence.sequencesegment.TrajectorySegment
+import org.firstinspires.ftc.teamcode.utils.roadrunner.trajectorysequence.sequencesegment.TurnSegment
+import org.firstinspires.ftc.teamcode.utils.roadrunner.trajectorysequence.sequencesegment.WaitSegment
+import org.firstinspires.ftc.teamcode.utils.roadrunner.util.DashboardUtil
+import java.util.LinkedList
+
+@Config
+class TrajectorySequenceRunner(
+ private val follower: TrajectoryFollower,
+ headingPIDCoefficients: PIDCoefficients,
+) {
+ private val turnController: PIDFController
+ private val clock: NanoClock
+ private val dashboard: FtcDashboard
+ private val poseHistory = LinkedList()
+ private var remainingMarkers: MutableList = ArrayList()
+ private var currentTrajectorySequence: TrajectorySequence? = null
+ private var currentSegmentStartTime = 0.0
+ private var currentSegmentIndex = 0
+ private var lastSegmentIndex = 0
+ var lastPoseError = Pose2d()
+ private set
+
+ init {
+ turnController = PIDFController(headingPIDCoefficients)
+ turnController.setInputBounds(0.0, 2 * Math.PI)
+ clock = NanoClock.system()
+ dashboard = FtcDashboard.getInstance()
+ dashboard.telemetryTransmissionInterval = 25
+ }
+
+ fun followTrajectorySequenceAsync(trajectorySequence: TrajectorySequence) {
+ currentTrajectorySequence = trajectorySequence
+ currentSegmentStartTime = clock.seconds()
+ currentSegmentIndex = 0
+ lastSegmentIndex = -1
+ }
+
+ fun update(poseEstimate: Pose2d, poseVelocity: Pose2d?): DriveSignal? {
+ var targetPose: Pose2d? = null
+ var driveSignal: DriveSignal? = null
+ val packet = TelemetryPacket()
+ val fieldOverlay = packet.fieldOverlay()
+ var currentSegment: SequenceSegment? = null
+
+ if (currentTrajectorySequence != null) {
+ if (currentSegmentIndex >= currentTrajectorySequence!!.size()) {
+ for (marker in remainingMarkers) {
+ marker.callback.onMarkerReached()
+ }
+
+ remainingMarkers.clear()
+
+ currentTrajectorySequence = null
+ }
+
+ if (currentTrajectorySequence == null) return DriveSignal()
+
+ val now = clock.seconds()
+
+ val isNewTransition = currentSegmentIndex != lastSegmentIndex
+
+ currentSegment = currentTrajectorySequence!![currentSegmentIndex]
+
+ if (isNewTransition) {
+ currentSegmentStartTime = now
+ lastSegmentIndex = currentSegmentIndex
+ for (marker in remainingMarkers) {
+ marker.callback.onMarkerReached()
+ }
+ remainingMarkers.clear()
+ remainingMarkers.addAll(currentSegment.markers)
+ remainingMarkers.sortWith { t1: TrajectoryMarker, t2: TrajectoryMarker ->
+ t1.time.compareTo(t2.time)
+ }
+ }
+ val deltaTime = now - currentSegmentStartTime
+
+ when (currentSegment) {
+ is TrajectorySegment -> {
+ val currentTrajectory = currentSegment.trajectory
+ if (isNewTransition) follower.followTrajectory(currentTrajectory)
+ if (!follower.isFollowing()) {
+ currentSegmentIndex++
+ driveSignal = DriveSignal()
+ } else {
+ driveSignal = follower.update(poseEstimate, poseVelocity)
+ lastPoseError = follower.lastError
+ }
+ targetPose = currentTrajectory[deltaTime]
+ }
+
+ is TurnSegment -> {
+ val targetState = currentSegment.motionProfile[deltaTime]
+
+ turnController.targetPosition = targetState.x
+
+ val correction = turnController.update(poseEstimate.heading)
+ val targetOmega = targetState.v
+ val targetAlpha = targetState.a
+
+ lastPoseError = Pose2d(0.0, 0.0, turnController.lastError)
+
+ val startPose = currentSegment.startPose
+
+ targetPose = startPose.copy(startPose.x, startPose.y, targetState.x)
+
+ driveSignal = DriveSignal(
+ Pose2d(0.0, 0.0, targetOmega + correction),
+ Pose2d(0.0, 0.0, targetAlpha)
+ )
+
+ if (deltaTime >= currentSegment.duration) {
+ currentSegmentIndex++
+ driveSignal = DriveSignal()
+ }
+ }
+
+ is WaitSegment -> {
+ lastPoseError = Pose2d()
+ targetPose = currentSegment.startPose
+ driveSignal = DriveSignal()
+ if (deltaTime >= currentSegment.duration) {
+ currentSegmentIndex++
+ }
+ }
+ }
+ while (remainingMarkers.size > 0 && deltaTime > remainingMarkers[0].time) {
+ remainingMarkers[0].callback.onMarkerReached()
+ remainingMarkers.removeAt(0)
+ }
+ }
+ poseHistory.add(poseEstimate)
+ if (POSE_HISTORY_LIMIT > -1 && poseHistory.size > POSE_HISTORY_LIMIT) {
+ poseHistory.removeFirst()
+ }
+ packet.put("x", poseEstimate.x)
+ packet.put("y", poseEstimate.y)
+ packet.put("heading (deg)", Math.toDegrees(poseEstimate.heading))
+ packet.put("xError", lastPoseError.x)
+ packet.put("yError", lastPoseError.y)
+ packet.put("headingError (deg)", Math.toDegrees(lastPoseError.heading))
+ draw(fieldOverlay, currentTrajectorySequence, currentSegment, targetPose, poseEstimate)
+ dashboard.sendTelemetryPacket(packet)
+ return driveSignal
+ }
+
+ private fun draw(
+ fieldOverlay: Canvas,
+ sequence: TrajectorySequence?, currentSegment: SequenceSegment?,
+ targetPose: Pose2d?, poseEstimate: Pose2d,
+ ) {
+ if (sequence != null) {
+ for (i in 0 until sequence.size()) {
+ when (val segment = sequence[i]) {
+ is TrajectorySegment -> {
+ fieldOverlay.setStrokeWidth(1)
+ fieldOverlay.setStroke(COLOR_INACTIVE_TRAJECTORY)
+ DashboardUtil.drawSampledPath(fieldOverlay, segment.trajectory.path)
+ }
+
+ is TurnSegment -> {
+ val pose = segment.startPose
+ fieldOverlay.setFill(COLOR_INACTIVE_TURN)
+ fieldOverlay.fillCircle(pose.x, pose.y, 2.0)
+ }
+
+ is WaitSegment -> {
+ val pose = segment.startPose
+ fieldOverlay.setStrokeWidth(1)
+ fieldOverlay.setStroke(COLOR_INACTIVE_WAIT)
+ fieldOverlay.strokeCircle(pose.x, pose.y, 3.0)
+ }
+ }
+ }
+ }
+ if (currentSegment != null) {
+ when (currentSegment) {
+ is TrajectorySegment -> {
+ val currentTrajectory = currentSegment.trajectory
+ fieldOverlay.setStrokeWidth(1)
+ fieldOverlay.setStroke(COLOR_ACTIVE_TRAJECTORY)
+ DashboardUtil.drawSampledPath(fieldOverlay, currentTrajectory.path)
+ }
+
+ is TurnSegment -> {
+ val pose = currentSegment.startPose
+ fieldOverlay.setFill(COLOR_ACTIVE_TURN)
+ fieldOverlay.fillCircle(pose.x, pose.y, 3.0)
+ }
+
+ is WaitSegment -> {
+ val pose = currentSegment.startPose
+ fieldOverlay.setStrokeWidth(1)
+ fieldOverlay.setStroke(COLOR_ACTIVE_WAIT)
+ fieldOverlay.strokeCircle(pose.x, pose.y, 3.0)
+ }
+ }
+ }
+ 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)
+ }
+
+ val isBusy: Boolean
+ get() = currentTrajectorySequence != null
+
+ companion object {
+ var COLOR_INACTIVE_TRAJECTORY = "#4caf507a"
+ var COLOR_INACTIVE_TURN = "#7c4dff7a"
+ var COLOR_INACTIVE_WAIT = "#dd2c007a"
+ var COLOR_ACTIVE_TRAJECTORY = "#4CAF50"
+ var COLOR_ACTIVE_TURN = "#7c4dff"
+ var COLOR_ACTIVE_WAIT = "#dd2c00"
+ var POSE_HISTORY_LIMIT = -1
+ }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/trajectorysequence/sequencesegment/SequenceSegment.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/trajectorysequence/sequencesegment/SequenceSegment.kt
new file mode 100644
index 0000000..73f0fcc
--- /dev/null
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/trajectorysequence/sequencesegment/SequenceSegment.kt
@@ -0,0 +1,10 @@
+package org.firstinspires.ftc.teamcode.utils.roadrunner.trajectorysequence.sequencesegment
+
+import com.acmerobotics.roadrunner.geometry.Pose2d
+import com.acmerobotics.roadrunner.trajectory.TrajectoryMarker
+
+abstract class SequenceSegment protected constructor(
+ val duration: Double,
+ val startPose: Pose2d, val endPose: Pose2d,
+ val markers: List,
+)
\ No newline at end of file
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/trajectorysequence/sequencesegment/TrajectorySegment.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/trajectorysequence/sequencesegment/TrajectorySegment.kt
new file mode 100644
index 0000000..50c708f
--- /dev/null
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/trajectorysequence/sequencesegment/TrajectorySegment.kt
@@ -0,0 +1,9 @@
+package org.firstinspires.ftc.teamcode.utils.roadrunner.trajectorysequence.sequencesegment
+
+import com.acmerobotics.roadrunner.trajectory.Trajectory
+
+class TrajectorySegment // Note: Markers are already stored in the `Trajectory` itself.
+// This class should not hold any markers
+ (val trajectory: Trajectory) : SequenceSegment(
+ trajectory.duration(), trajectory.start(), trajectory.end(), emptyList()
+)
\ No newline at end of file
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/trajectorysequence/sequencesegment/TurnSegment.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/trajectorysequence/sequencesegment/TurnSegment.kt
new file mode 100644
index 0000000..94326c0
--- /dev/null
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/trajectorysequence/sequencesegment/TurnSegment.kt
@@ -0,0 +1,21 @@
+package org.firstinspires.ftc.teamcode.utils.roadrunner.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.norm
+
+class TurnSegment(
+ startPose: Pose2d,
+ val totalRotation: Double,
+ val motionProfile: MotionProfile,
+ markers: List,
+) : SequenceSegment(
+ motionProfile.duration(),
+ startPose,
+ Pose2d(
+ startPose.x, startPose.y,
+ norm(startPose.heading + totalRotation)
+ ),
+ markers
+)
\ No newline at end of file
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/trajectorysequence/sequencesegment/WaitSegment.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/trajectorysequence/sequencesegment/WaitSegment.kt
new file mode 100644
index 0000000..7d5d248
--- /dev/null
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/trajectorysequence/sequencesegment/WaitSegment.kt
@@ -0,0 +1,7 @@
+package org.firstinspires.ftc.teamcode.utils.roadrunner.trajectorysequence.sequencesegment
+
+import com.acmerobotics.roadrunner.geometry.Pose2d
+import com.acmerobotics.roadrunner.trajectory.TrajectoryMarker
+
+class WaitSegment(pose: Pose2d, seconds: Double, markers: List) :
+ SequenceSegment(seconds, pose, pose, markers)
\ No newline at end of file
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/util/AssetsTrajectoryManager.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/util/AssetsTrajectoryManager.kt
new file mode 100644
index 0000000..b5b6a15
--- /dev/null
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/util/AssetsTrajectoryManager.kt
@@ -0,0 +1,65 @@
+package org.firstinspires.ftc.teamcode.utils.roadrunner.util
+
+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.GROUP_FILENAME
+import com.acmerobotics.roadrunner.trajectory.config.TrajectoryConfigManager.loadConfig
+import com.acmerobotics.roadrunner.trajectory.config.TrajectoryConfigManager.loadGroupConfig
+import com.acmerobotics.roadrunner.trajectory.config.TrajectoryGroupConfig
+import org.firstinspires.ftc.robotcore.internal.system.AppUtil
+import java.io.IOException
+
+/**
+ * Set of utilities for loading trajectories from assets (the plugin save location).
+ */
+object AssetsTrajectoryManager {
+ /**
+ * Loads the group config.
+ */
+ fun loadGroupConfig(): TrajectoryGroupConfig? {
+ return try {
+ val inputStream = AppUtil.getDefContext().assets.open(
+ "trajectory/$GROUP_FILENAME"
+ )
+ loadGroupConfig(inputStream)
+ } catch (e: IOException) {
+ null
+ }
+ }
+
+ /**
+ * Loads a trajectory config with the given name.
+ */
+ fun loadConfig(name: String): TrajectoryConfig? {
+ return try {
+ val inputStream = AppUtil.getDefContext().assets.open(
+ "trajectory/$name.yaml"
+ )
+ loadConfig(inputStream)
+ } catch (e: IOException) {
+ null
+ }
+ }
+
+ /**
+ * Loads a trajectory builder with the given name.
+ */
+ fun loadBuilder(name: String): TrajectoryBuilder? {
+ val groupConfig = loadGroupConfig()
+ val config = loadConfig(name)
+ return if (groupConfig == null || config == null) {
+ null
+ } else config.toTrajectoryBuilder(
+ groupConfig
+ )
+ }
+
+ /**
+ * Loads a trajectory with the given name.
+ */
+ fun load(name: String): Trajectory? {
+ val builder = loadBuilder(name) ?: return null
+ return builder.build()
+ }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/util/AxesSigns.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/util/AxesSigns.kt
new file mode 100644
index 0000000..91d496c
--- /dev/null
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/util/AxesSigns.kt
@@ -0,0 +1,8 @@
+package org.firstinspires.ftc.teamcode.utils.roadrunner.util
+
+/**
+ * IMU axes signs in the order XYZ (after remapping).
+ */
+enum class AxesSigns(val bVal: Int) {
+ PPP(0), PPN(1), PNP(2), PNN(3), NPP(4), NPN(5), NNP(6), NNN(7)
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/util/BNO055IMUUtil.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/util/BNO055IMUUtil.kt
new file mode 100644
index 0000000..fbeedd2
--- /dev/null
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/util/BNO055IMUUtil.kt
@@ -0,0 +1,54 @@
+package org.firstinspires.ftc.teamcode.utils.roadrunner.util
+
+import com.qualcomm.hardware.bosch.BNO055IMU
+import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder
+
+/**
+ * Various utility functions for the BNO055 IMU.
+ */
+object BNO055IMUUtil {
+ /**
+ * Remap BNO055 IMU axes and signs. For reference, the default order is [AxesOrder.ZYX].
+ * Call after [BNO055IMU.initialize]. Although this isn't
+ * mentioned in the datasheet, the axes order appears to affect the onboard sensor fusion.
+ *
+ *
+ * Adapted from [this post](https://ftcforum.usfirst.org/forum/ftc-technology/53812-mounting-the-revhub-vertically?p=56587#post56587).
+ *
+ * @param imu IMU
+ * @param order axes order
+ * @param signs axes signs
+ */
+ fun remapAxes(imu: BNO055IMU, order: AxesOrder, signs: AxesSigns) {
+ try {
+ // the indices correspond with the 2-bit encodings specified in the datasheet
+ val indices = order.indices()
+ var axisMapConfig = 0
+ axisMapConfig = axisMapConfig or (indices[0] shl 4)
+ axisMapConfig = axisMapConfig or (indices[1] shl 2)
+ axisMapConfig = axisMapConfig or (indices[2] shl 0)
+
+ // the BNO055 driver flips the first orientation vector so we also flip here
+ val axisMapSign = signs.bVal xor (4 shr indices[0])
+
+ // Enter CONFIG mode
+ imu.write8(
+ BNO055IMU.Register.OPR_MODE,
+ BNO055IMU.SensorMode.CONFIG.bVal.toInt() and 0x0F
+ )
+ Thread.sleep(100)
+
+ // Write the AXIS_MAP_CONFIG register
+ imu.write8(BNO055IMU.Register.AXIS_MAP_CONFIG, axisMapConfig and 0x3F)
+
+ // Write the AXIS_MAP_SIGN register
+ imu.write8(BNO055IMU.Register.AXIS_MAP_SIGN, axisMapSign and 0x07)
+
+ // Switch back to the previous mode
+ imu.write8(BNO055IMU.Register.OPR_MODE, imu.parameters.mode.bVal.toInt() and 0x0F)
+ Thread.sleep(100)
+ } catch (e: InterruptedException) {
+ Thread.currentThread().interrupt()
+ }
+ }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/util/DashboardUtil.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/util/DashboardUtil.kt
new file mode 100644
index 0000000..15f3a7a
--- /dev/null
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/util/DashboardUtil.kt
@@ -0,0 +1,48 @@
+package org.firstinspires.ftc.teamcode.utils.roadrunner.util
+
+import com.acmerobotics.dashboard.canvas.Canvas
+import com.acmerobotics.roadrunner.geometry.Pose2d
+import com.acmerobotics.roadrunner.path.Path
+
+/**
+ * Set of helper functions for drawing Road Runner paths and trajectories on dashboard canvases.
+ */
+object DashboardUtil {
+ private const val DEFAULT_RESOLUTION = 2.0 // distance units; presumed inches
+ private const val ROBOT_RADIUS = 9.0 // in
+ fun drawPoseHistory(canvas: Canvas, poseHistory: List) {
+ val xPoints = DoubleArray(poseHistory.size)
+ val yPoints = DoubleArray(poseHistory.size)
+ for (i in poseHistory.indices) {
+ val (x, y) = poseHistory[i]
+ xPoints[i] = x
+ yPoints[i] = y
+ }
+ canvas.strokePolyline(xPoints, yPoints)
+ }
+
+ @JvmOverloads
+ fun drawSampledPath(canvas: Canvas, path: Path, resolution: Double = DEFAULT_RESOLUTION) {
+ val samples = Math.ceil(path.length() / resolution).toInt()
+ val xPoints = DoubleArray(samples)
+ val yPoints = DoubleArray(samples)
+ val dx = path.length() / (samples - 1)
+ for (i in 0 until samples) {
+ val displacement = i * dx
+ val (x, y) = path[displacement]
+ xPoints[i] = x
+ yPoints[i] = y
+ }
+ canvas.strokePolyline(xPoints, yPoints)
+ }
+
+ fun drawRobot(canvas: Canvas, pose: Pose2d) {
+ canvas.strokeCircle(pose.x, pose.y, ROBOT_RADIUS)
+ val (x, y) = pose.headingVec().times(ROBOT_RADIUS)
+ val x1 = pose.x + x / 2
+ val y1 = pose.y + y / 2
+ val x2 = pose.x + x
+ val y2 = pose.y + y
+ canvas.strokeLine(x1, y1, x2, y2)
+ }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/util/Encoder.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/util/Encoder.kt
new file mode 100644
index 0000000..13b01b4
--- /dev/null
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/util/Encoder.kt
@@ -0,0 +1,72 @@
+package org.firstinspires.ftc.teamcode.utils.roadrunner.util
+
+import com.acmerobotics.roadrunner.util.NanoClock
+import com.qualcomm.robotcore.hardware.DcMotorEx
+import com.qualcomm.robotcore.hardware.DcMotorSimple
+import kotlin.math.abs
+import kotlin.math.sign
+
+/**
+ * Wraps a motor instance to provide corrected velocity counts and allow reversing independently of the corresponding
+ * slot's motor direction
+ */
+class Encoder @JvmOverloads constructor(
+ private val motor: DcMotorEx,
+ private val clock: NanoClock = NanoClock.system(),
+) {
+ /**
+ * 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
+ */
+ private var direction: Direction
+ private var lastPosition = 0
+ private var velocityEstimate = 0.0
+ private var lastUpdateTime: Double
+
+ init {
+ direction = Direction.FORWARD
+ lastUpdateTime = clock.seconds()
+ }
+
+ private val multiplier: Int
+ get() = direction.multiplier * if (motor.direction == DcMotorSimple.Direction.FORWARD) 1 else -1
+ val currentPosition: Int
+ get() {
+ val multiplier = multiplier
+ val currentPosition = motor.currentPosition * multiplier
+ if (currentPosition != lastPosition) {
+ val currentTime = clock.seconds()
+ val dt = currentTime - lastUpdateTime
+ velocityEstimate = (currentPosition - lastPosition) / dt
+ lastPosition = currentPosition
+ lastUpdateTime = currentTime
+ }
+ return currentPosition
+ }
+ private val rawVelocity: Double
+ get() {
+ val multiplier = multiplier
+ return motor.velocity * multiplier
+ }
+ val correctedVelocity: Double
+ get() = inverseOverflow(
+ rawVelocity, velocityEstimate
+ )
+
+ enum class Direction(val multiplier: Int) {
+ FORWARD(1), REVERSE(-1)
+
+ }
+
+ companion object {
+ private const val CPS_STEP = 0x10000
+ private fun inverseOverflow(input: Double, estimate: Double): Double {
+ var real = input
+ while (abs(estimate - real) > CPS_STEP / 2.0) {
+ real += sign(estimate - real) * CPS_STEP
+ }
+ return real
+ }
+ }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/util/LoggingUtil.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/util/LoggingUtil.kt
new file mode 100644
index 0000000..e125dd6
--- /dev/null
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/util/LoggingUtil.kt
@@ -0,0 +1,50 @@
+package org.firstinspires.ftc.teamcode.utils.roadrunner.util
+
+import org.firstinspires.ftc.robotcore.internal.system.AppUtil
+import java.io.File
+
+/**
+ * Utility functions for log files.
+ */
+object LoggingUtil {
+ private val ROAD_RUNNER_FOLDER = File(AppUtil.ROOT_FOLDER.toString() + "/RoadRunner/")
+ private const val LOG_QUOTA = (25 * 1024 * 1024 // 25MB log quota for now
+ ).toLong()
+
+ private fun buildLogList(logFiles: MutableList, dir: File) {
+ for (file in dir.listFiles()!!) {
+ if (file.isDirectory) {
+ buildLogList(logFiles, file)
+ } else {
+ logFiles.add(file)
+ }
+ }
+ }
+
+ private fun pruneLogsIfNecessary() {
+ val logFiles: MutableList = ArrayList()
+ buildLogList(logFiles, ROAD_RUNNER_FOLDER)
+ logFiles.sortWith { lhs: File, rhs: File ->
+ lhs.lastModified().compareTo(rhs.lastModified())
+ }
+ var dirSize: Long = 0
+ for (file in logFiles) {
+ dirSize += file.length()
+ }
+ while (dirSize > LOG_QUOTA) {
+ if (logFiles.size == 0) break
+ val fileToRemove = logFiles.removeAt(0)
+ dirSize -= fileToRemove.length()
+ fileToRemove.delete()
+ }
+ }
+
+ /**
+ * Obtain a log file with the provided name
+ */
+ fun getLogFile(name: String): File {
+ ROAD_RUNNER_FOLDER.mkdirs()
+ pruneLogsIfNecessary()
+ return File(ROAD_RUNNER_FOLDER, name)
+ }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/util/LynxModuleUtil.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/util/LynxModuleUtil.kt
new file mode 100644
index 0000000..4c49c45
--- /dev/null
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/util/LynxModuleUtil.kt
@@ -0,0 +1,103 @@
+package org.firstinspires.ftc.teamcode.utils.roadrunner.util
+
+import com.qualcomm.hardware.lynx.LynxModule
+import com.qualcomm.robotcore.hardware.HardwareMap
+import org.firstinspires.ftc.robotcore.internal.system.Misc
+
+/**
+ * Collection of utilites for interacting with Lynx modules.
+ */
+object LynxModuleUtil {
+ private val MIN_VERSION = LynxFirmwareVersion(1, 8, 2)
+
+ /**
+ * Retrieve and parse Lynx module firmware version.
+ *
+ * @param module Lynx module
+ * @return parsed firmware version
+ */
+ fun getFirmwareVersion(module: LynxModule): LynxFirmwareVersion? {
+ val versionString = module.nullableFirmwareVersionString ?: return null
+ val parts =
+ versionString.split("[ :,]+".toRegex()).dropLastWhile { it.isEmpty() }.toTypedArray()
+ return try {
+ // note: for now, we ignore the hardware entry
+ LynxFirmwareVersion(parts[3].toInt(), parts[5].toInt(), parts[7].toInt())
+ } catch (e: NumberFormatException) {
+ null
+ }
+ }
+
+ /**
+ * Ensure all of the Lynx modules attached to the robot satisfy the minimum requirement.
+ *
+ * @param hardwareMap hardware map containing Lynx modules
+ */
+ fun ensureMinimumFirmwareVersion(hardwareMap: HardwareMap) {
+ val outdatedModules: MutableMap = HashMap()
+ for (module in hardwareMap.getAll(LynxModule::class.java)) {
+ val version = getFirmwareVersion(module)
+ if (version == null || version < MIN_VERSION) {
+ for (name in hardwareMap.getNamesOf(module)) {
+ outdatedModules[name] = version
+ }
+ }
+ }
+ if (outdatedModules.isNotEmpty()) {
+ val msgBuilder = 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 ((key, value) in outdatedModules) {
+ msgBuilder.append(
+ Misc.formatInvariant(
+ "\t%s: %s\n", key,
+ value?.toString() ?: "Unknown"
+ )
+ )
+ }
+ throw LynxFirmwareVersionException(msgBuilder.toString())
+ }
+ }
+
+ /**
+ * Parsed representation of a Lynx module firmware version.
+ */
+ class LynxFirmwareVersion(val major: Int, val minor: Int, val eng: Int) :
+ Comparable {
+ override fun equals(other: Any?): Boolean {
+ return if (other is LynxFirmwareVersion) {
+ major == other.major && minor == other.minor && eng == other.eng
+ } else {
+ false
+ }
+ }
+
+ override fun compareTo(other: LynxFirmwareVersion): Int {
+ val majorComp = major.compareTo(other.major)
+ return if (majorComp == 0) {
+ val minorComp = minor.compareTo(other.minor)
+ if (minorComp == 0) {
+ eng.compareTo(other.eng)
+ } else {
+ minorComp
+ }
+ } else {
+ majorComp
+ }
+ }
+
+ override fun toString(): String {
+ return Misc.formatInvariant("%d.%d.%d", major, minor, eng)
+ }
+ }
+
+ /**
+ * Exception indicating an outdated Lynx firmware version.
+ */
+ class LynxFirmwareVersionException(detailMessage: String?) : RuntimeException(detailMessage)
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/util/RegressionUtil.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/util/RegressionUtil.kt
new file mode 100644
index 0000000..138ed16
--- /dev/null
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/util/RegressionUtil.kt
@@ -0,0 +1,138 @@
+package org.firstinspires.ftc.teamcode.utils.roadrunner.util
+
+import com.acmerobotics.roadrunner.kinematics.Kinematics.calculateMotorFeedforward
+import org.apache.commons.math3.stat.regression.SimpleRegression
+import java.io.File
+import java.io.FileNotFoundException
+import java.io.PrintWriter
+import kotlin.math.abs
+
+/**
+ * Various regression utilities.
+ */
+object RegressionUtil {
+ /**
+ * 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 fun numericalDerivative(x: List, y: List): List {
+ val deriv: MutableList = ArrayList(x.size)
+ for (i in 1 until x.size - 1) {
+ deriv.add(
+ (y[i + 1] - y[i - 1]) /
+ (x[i + 1] - x[i - 1])
+ )
+ }
+ // copy endpoints to pad output
+ deriv.add(0, deriv[0])
+ deriv.add(deriv[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
+ */
+ fun fitRampData(
+ timeSamples: List, positionSamples: List,
+ powerSamples: List, fitStatic: Boolean,
+ file: File?,
+ ): RampResult {
+ if (file != null) {
+ try {
+ PrintWriter(file).use { pw ->
+ pw.println("time,position,power")
+ for (i in timeSamples.indices) {
+ val time = timeSamples[i]
+ val pos = positionSamples[i]
+ val power = powerSamples[i]
+ pw.println("$time,$pos,$power")
+ }
+ }
+ } catch (e: FileNotFoundException) {
+ // ignore
+ }
+ }
+ val velSamples = numericalDerivative(timeSamples, positionSamples)
+ val rampReg = SimpleRegression(fitStatic)
+ for (i in timeSamples.indices) {
+ val vel = velSamples[i]
+ val power = powerSamples[i]
+ rampReg.addData(vel, power)
+ }
+ return RampResult(
+ abs(rampReg.slope), abs(rampReg.intercept),
+ rampReg.rSquare
+ )
+ }
+
+ /**
+ * 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
+ */
+ fun fitAccelData(
+ timeSamples: List, positionSamples: List,
+ powerSamples: List, rampResult: RampResult,
+ file: File?,
+ ): AccelResult {
+ if (file != null) {
+ try {
+ PrintWriter(file).use { pw ->
+ pw.println("time,position,power")
+ for (i in timeSamples.indices) {
+ val time = timeSamples[i]
+ val pos = positionSamples[i]
+ val power = powerSamples[i]
+ pw.println("$time,$pos,$power")
+ }
+ }
+ } catch (e: FileNotFoundException) {
+ // ignore
+ }
+ }
+ val velSamples = numericalDerivative(timeSamples, positionSamples)
+ val accelSamples = numericalDerivative(timeSamples, velSamples)
+ val accelReg = SimpleRegression(false)
+ for (i in timeSamples.indices) {
+ val vel = velSamples[i]
+ val accel = accelSamples[i]
+ val power = powerSamples[i]
+ val powerFromVel = calculateMotorFeedforward(
+ vel, 0.0, rampResult.kV, 0.0, rampResult.kStatic
+ )
+ val powerFromAccel = power - powerFromVel
+ accelReg.addData(accel, powerFromAccel)
+ }
+ return AccelResult(Math.abs(accelReg.slope), accelReg.rSquare)
+ }
+
+ /**
+ * Feedforward parameter estimates from the ramp regression and additional summary statistics
+ */
+ class RampResult(val kV: Double, val kStatic: Double, val rSquare: Double)
+
+ /**
+ * Feedforward parameter estimates from the ramp regression and additional summary statistics
+ */
+ class AccelResult(val kA: Double, val rSquare: Double)
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/util/TimedAction.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/util/TimedAction.kt
new file mode 100644
index 0000000..2296629
--- /dev/null
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/roadrunner/util/TimedAction.kt
@@ -0,0 +1,88 @@
+package org.firstinspires.ftc.teamcode.utils.roadrunner.util
+
+import com.qualcomm.robotcore.util.ElapsedTime
+
+/**
+ * Too many people use sleep(...) where it doesn't need to be.
+ * This is a simple util that runs an action until a time is reached and then performs a specified stop action.
+ * This is basically the same as a chain of commands with a deadline WaitCommand but without command-based.
+ *
+ * @author Jackson
+ */
+class TimedAction @JvmOverloads constructor(
+ onRun: Runnable,
+ onEnd: Runnable,
+ milliseconds: Double,
+ symmetric: Boolean = false,
+) {
+ private val onRun: Runnable
+ private val onEnd: Runnable
+ private val waitTime: Double
+ private val symmetric: Boolean
+ private val timer: ElapsedTime = ElapsedTime()
+ private var state = State.IDLE
+ /**
+ * Sets up a timed action that runs two actions depending on the time
+ * and is non-blocking.
+ *
+ * @param onRun the action to run during the time period
+ * @param onEnd the action to run after the time period is up
+ * @param milliseconds the wait time
+ * @param symmetric true if the timed action runs symmetric
+ */
+ /**
+ * Sets up an asymmetric timed action.
+ *
+ * @param onRun the action to run during the time period
+ * @param onEnd the action to run after the time period is up
+ * @param milliseconds the wait time
+ */
+ init {
+ this.onRun = onRun
+ this.onEnd = onEnd
+ waitTime = milliseconds
+ this.symmetric = symmetric
+ }
+
+ /**
+ * @return true if the timed action is currently running
+ */
+ fun running(): Boolean {
+ return state != State.IDLE
+ }
+
+ /**
+ * Resets the timed action to the RUNNING state
+ */
+ fun reset() {
+ timer.reset()
+ state = State.RUNNING
+ }
+
+ /**
+ * Runs the timed action in a non-blocking FSM
+ */
+ fun run() {
+ when (state) {
+ State.IDLE -> {}
+ State.RUNNING -> if (timer.milliseconds() <= waitTime) {
+ onRun.run()
+ } else {
+ timer.reset()
+ onEnd.run()
+ state = if (symmetric) State.SYMMETRIC else State.IDLE
+ }
+
+ State.SYMMETRIC -> if (timer.milliseconds() <= waitTime) {
+ onEnd.run()
+ } else {
+ timer.reset()
+ state = State.IDLE
+ }
+ }
+ }
+
+ internal enum class State {
+ IDLE, RUNNING, SYMMETRIC
+ }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/res/raw/readme.md b/TeamCode/src/main/res/raw/readme.md
new file mode 100644
index 0000000..355a3c4
--- /dev/null
+++ b/TeamCode/src/main/res/raw/readme.md
@@ -0,0 +1 @@
+Place your sound files in this folder to use them as project resources.
\ No newline at end of file
diff --git a/TeamCode/src/main/res/values/strings.xml b/TeamCode/src/main/res/values/strings.xml
new file mode 100644
index 0000000..d781ec5
--- /dev/null
+++ b/TeamCode/src/main/res/values/strings.xml
@@ -0,0 +1,4 @@
+
+
+
+
diff --git a/TeamCode/src/main/res/xml/teamwebcamcalibrations.xml b/TeamCode/src/main/res/xml/teamwebcamcalibrations.xml
new file mode 100644
index 0000000..9764eae
--- /dev/null
+++ b/TeamCode/src/main/res/xml/teamwebcamcalibrations.xml
@@ -0,0 +1,160 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/build.common.gradle b/build.common.gradle
new file mode 100644
index 0000000..771116d
--- /dev/null
+++ b/build.common.gradle
@@ -0,0 +1,113 @@
+/**
+ * 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 34
+
+ signingConfigs {
+ release {
+ def apkStoreFile = System.getenv("APK_SIGNING_STORE_FILE")
+ if (apkStoreFile != null) {
+ keyAlias System.getenv("APK_SIGNING_KEY_ALIAS")
+ keyPassword System.getenv("APK_SIGNING_KEY_PASSWORD")
+ storeFile file(System.getenv("APK_SIGNING_STORE_FILE"))
+ storePassword System.getenv("APK_SIGNING_STORE_PASSWORD")
+ } else {
+ keyAlias 'androiddebugkey'
+ keyPassword 'android'
+ storeFile rootProject.file('libs/ftc.debug.keystore')
+ storePassword 'android'
+ }
+ }
+
+ debug {
+ keyAlias 'androiddebugkey'
+ keyPassword 'android'
+ storeFile rootProject.file('libs/ftc.debug.keystore')
+ storePassword 'android'
+ }
+ }
+
+ defaultConfig {
+ signingConfig signingConfigs.debug
+ applicationId 'com.qualcomm.ftcrobotcontroller'
+ minSdkVersion 24
+ //noinspection ExpiredTargetSdkVersion
+ targetSdkVersion 28
+
+ /**
+ * We keep the versionCode and versionName of robot controller applications in sync with
+ * the master information published in the AndroidManifest.xml file of the FtcRobotController
+ * module. This helps avoid confusion that might arise from having multiple versions of
+ * a robot controller app simultaneously installed on a robot controller device.
+ *
+ * We accomplish this with the help of a funky little Groovy script that maintains that
+ * correspondence automatically.
+ *
+ * @see Configure Your Build
+ * @see Versioning Your App
+ */
+ def manifestFile = project(':FtcRobotController').file('src/main/AndroidManifest.xml');
+ def manifestText = manifestFile.getText()
+ //
+ def vCodePattern = Pattern.compile("versionCode=\"(\\d+(\\.\\d+)*)\"")
+ def matcher = vCodePattern.matcher(manifestText)
+ matcher.find()
+ def vCode = Integer.parseInt(matcher.group(1))
+ //
+ def vNamePattern = Pattern.compile("versionName=\"(.*)\"")
+ matcher = vNamePattern.matcher(manifestText);
+ matcher.find()
+ def vName = matcher.group(1)
+ //
+ versionCode vCode
+ versionName vName
+ }
+
+ // http://google.github.io/android-gradle-dsl/current/com.android.build.gradle.internal.dsl.BuildType.html
+ buildTypes {
+ release {
+ signingConfig signingConfigs.release
+
+ ndk {
+ abiFilters "armeabi-v7a", "arm64-v8a"
+ }
+ }
+ debug {
+ debuggable true
+ jniDebuggable true
+ ndk {
+ abiFilters "armeabi-v7a", "arm64-v8a"
+ }
+ }
+ }
+
+
+ packagingOptions {
+ pickFirst '**/*.so'
+ }
+ sourceSets.main {
+ jni.srcDirs = []
+ jniLibs.srcDir rootProject.file('libs')
+ }
+ ndkVersion '21.3.6528147'
+}
+
diff --git a/build.dependencies.gradle b/build.dependencies.gradle
new file mode 100644
index 0000000..951adb2
--- /dev/null
+++ b/build.dependencies.gradle
@@ -0,0 +1,20 @@
+repositories {
+ mavenCentral()
+ maven { url = 'https://maven.brott.dev/' }
+ google() // Needed for androidx
+}
+
+dependencies {
+ implementation 'org.firstinspires.ftc:Inspection:10.1.0'
+ implementation 'org.firstinspires.ftc:Blocks:10.1.0'
+ implementation 'org.firstinspires.ftc:RobotCore:10.1.0'
+ implementation 'org.firstinspires.ftc:RobotServer:10.1.0'
+ implementation 'org.firstinspires.ftc:OnBotJava:10.1.0'
+ implementation 'org.firstinspires.ftc:Hardware:10.1.0'
+ implementation 'org.firstinspires.ftc:FtcCommon:10.1.0'
+ implementation 'org.firstinspires.ftc:Vision:10.1.0'
+ //noinspection GradleDependency
+ implementation 'androidx.appcompat:appcompat:1.2.0'
+
+ implementation 'com.acmerobotics.dashboard:dashboard:0.4.12'
+}
diff --git a/build.gradle b/build.gradle
new file mode 100644
index 0000000..77978ce
--- /dev/null
+++ b/build.gradle
@@ -0,0 +1,28 @@
+/**
+ * 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 {
+ classpath 'com.android.tools.build:gradle:8.7.1'
+ classpath 'org.jetbrains.kotlin:kotlin-gradle-plugin:1.8.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..0f284f8
--- /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
+android.nonFinalResIds=false
diff --git a/gradle/wrapper/gradle-wrapper.jar b/gradle/wrapper/gradle-wrapper.jar
new file mode 100644
index 0000000..2c35211
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..09523c0
--- /dev/null
+++ b/gradle/wrapper/gradle-wrapper.properties
@@ -0,0 +1,7 @@
+distributionBase=GRADLE_USER_HOME
+distributionPath=wrapper/dists
+distributionUrl=https\://services.gradle.org/distributions/gradle-8.9-bin.zip
+networkTimeout=10000
+validateDistributionUrl=true
+zipStoreBase=GRADLE_USER_HOME
+zipStorePath=wrapper/dists
diff --git a/gradlew b/gradlew
new file mode 100755
index 0000000..f5feea6
--- /dev/null
+++ b/gradlew
@@ -0,0 +1,252 @@
+#!/bin/sh
+
+#
+# Copyright © 2015-2021 the original authors.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# https://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+#
+# SPDX-License-Identifier: Apache-2.0
+#
+
+##############################################################################
+#
+# Gradle start up script for POSIX generated by Gradle.
+#
+# Important for running:
+#
+# (1) You need a POSIX-compliant shell to run this script. If your /bin/sh is
+# noncompliant, but you have some other compliant shell such as ksh or
+# bash, then to run this script, type that shell name before the whole
+# command line, like:
+#
+# ksh Gradle
+#
+# Busybox and similar reduced shells will NOT work, because this script
+# requires all of these POSIX shell features:
+# * functions;
+# * expansions «$var», «${var}», «${var:-default}», «${var+SET}»,
+# «${var#prefix}», «${var%suffix}», and «$( cmd )»;
+# * compound commands having a testable exit status, especially «case»;
+# * various built-in commands including «command», «set», and «ulimit».
+#
+# Important for patching:
+#
+# (2) This script targets any POSIX shell, so it avoids extensions provided
+# by Bash, Ksh, etc; in particular arrays are avoided.
+#
+# The "traditional" practice of packing multiple parameters into a
+# space-separated string is a well documented source of bugs and security
+# problems, so this is (mostly) avoided, by progressively accumulating
+# options in "$@", and eventually passing that to Java.
+#
+# Where the inherited environment variables (DEFAULT_JVM_OPTS, JAVA_OPTS,
+# and GRADLE_OPTS) rely on word-splitting, this is performed explicitly;
+# see the in-line comments for details.
+#
+# There are tweaks for specific operating systems such as AIX, CygWin,
+# Darwin, MinGW, and NonStop.
+#
+# (3) This script is generated from the Groovy template
+# https://github.com/gradle/gradle/blob/HEAD/platforms/jvm/plugins-application/src/main/resources/org/gradle/api/internal/plugins/unixStartScript.txt
+# within the Gradle project.
+#
+# You can find Gradle at https://github.com/gradle/gradle/.
+#
+##############################################################################
+
+# Attempt to set APP_HOME
+
+# Resolve links: $0 may be a link
+app_path=$0
+
+# Need this for daisy-chained symlinks.
+while
+ APP_HOME=${app_path%"${app_path##*/}"} # leaves a trailing /; empty if no leading path
+ [ -h "$app_path" ]
+do
+ ls=$( ls -ld "$app_path" )
+ link=${ls#*' -> '}
+ case $link in #(
+ /*) app_path=$link ;; #(
+ *) app_path=$APP_HOME$link ;;
+ esac
+done
+
+# This is normally unused
+# shellcheck disable=SC2034
+APP_BASE_NAME=${0##*/}
+# Discard cd standard output in case $CDPATH is set (https://github.com/gradle/gradle/issues/25036)
+APP_HOME=$( cd -P "${APP_HOME:-./}" > /dev/null && printf '%s
+' "$PWD" ) || exit
+
+# Use the maximum available, or set MAX_FD != -1 to use that value.
+MAX_FD=maximum
+
+warn () {
+ echo "$*"
+} >&2
+
+die () {
+ echo
+ echo "$*"
+ echo
+ exit 1
+} >&2
+
+# OS specific support (must be 'true' or 'false').
+cygwin=false
+msys=false
+darwin=false
+nonstop=false
+case "$( uname )" in #(
+ CYGWIN* ) cygwin=true ;; #(
+ Darwin* ) darwin=true ;; #(
+ MSYS* | MINGW* ) msys=true ;; #(
+ NONSTOP* ) nonstop=true ;;
+esac
+
+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
+ if ! command -v java >/dev/null 2>&1
+ then
+ 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
+fi
+
+# Increase the maximum file descriptors if we can.
+if ! "$cygwin" && ! "$darwin" && ! "$nonstop" ; then
+ case $MAX_FD in #(
+ max*)
+ # In POSIX sh, ulimit -H is undefined. That's why the result is checked to see if it worked.
+ # shellcheck disable=SC2039,SC3045
+ MAX_FD=$( ulimit -H -n ) ||
+ warn "Could not query maximum file descriptor limit"
+ esac
+ case $MAX_FD in #(
+ '' | soft) :;; #(
+ *)
+ # In POSIX sh, ulimit -n is undefined. That's why the result is checked to see if it worked.
+ # shellcheck disable=SC2039,SC3045
+ ulimit -n "$MAX_FD" ||
+ warn "Could not set maximum file descriptor limit to $MAX_FD"
+ esac
+fi
+
+# Collect all arguments for the java command, stacking in reverse order:
+# * args from the command line
+# * the main class name
+# * -classpath
+# * -D...appname settings
+# * --module-path (only if needed)
+# * DEFAULT_JVM_OPTS, JAVA_OPTS, and GRADLE_OPTS environment variables.
+
+# For Cygwin or MSYS, switch paths to Windows format before running java
+if "$cygwin" || "$msys" ; then
+ APP_HOME=$( cygpath --path --mixed "$APP_HOME" )
+ CLASSPATH=$( cygpath --path --mixed "$CLASSPATH" )
+
+ JAVACMD=$( cygpath --unix "$JAVACMD" )
+
+ # Now convert the arguments - kludge to limit ourselves to /bin/sh
+ for arg do
+ if
+ case $arg in #(
+ -*) false ;; # don't mess with options #(
+ /?*) t=${arg#/} t=/${t%%/*} # looks like a POSIX filepath
+ [ -e "$t" ] ;; #(
+ *) false ;;
+ esac
+ then
+ arg=$( cygpath --path --ignore --mixed "$arg" )
+ fi
+ # Roll the args list around exactly as many times as the number of
+ # args, so each arg winds up back in the position where it started, but
+ # possibly modified.
+ #
+ # NB: a `for` loop captures its iteration list before it begins, so
+ # changing the positional parameters here affects neither the number of
+ # iterations, nor the values presented in `arg`.
+ shift # remove old arg
+ set -- "$@" "$arg" # push replacement arg
+ done
+fi
+
+
+# Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script.
+DEFAULT_JVM_OPTS='"-Xmx64m" "-Xms64m"'
+
+# Collect all arguments for the java command:
+# * DEFAULT_JVM_OPTS, JAVA_OPTS, JAVA_OPTS, and optsEnvironmentVar are not allowed to contain shell fragments,
+# and any embedded shellness will be escaped.
+# * For example: A user cannot expect ${Hostname} to be expanded, as it is an environment variable and will be
+# treated as '${Hostname}' itself on the command line.
+
+set -- \
+ "-Dorg.gradle.appname=$APP_BASE_NAME" \
+ -classpath "$CLASSPATH" \
+ org.gradle.wrapper.GradleWrapperMain \
+ "$@"
+
+# Stop when "xargs" is not available.
+if ! command -v xargs >/dev/null 2>&1
+then
+ die "xargs is not available"
+fi
+
+# Use "xargs" to parse quoted args.
+#
+# With -n1 it outputs one arg per line, with the quotes and backslashes removed.
+#
+# In Bash we could simply go:
+#
+# readarray ARGS < <( xargs -n1 <<<"$var" ) &&
+# set -- "${ARGS[@]}" "$@"
+#
+# but POSIX shell has neither arrays nor command substitution, so instead we
+# post-process each arg (as a line of input to sed) to backslash-escape any
+# character that might be a shell metacharacter, then use eval to reverse
+# that process (while maintaining the separation between arguments), and wrap
+# the whole thing up as a single "set" statement.
+#
+# This will of course break if any of these variables contains a newline or
+# an unmatched quote.
+#
+
+eval "set -- $(
+ printf '%s\n' "$DEFAULT_JVM_OPTS $JAVA_OPTS $GRADLE_OPTS" |
+ xargs -n1 |
+ sed ' s~[^-[:alnum:]+,./:=@_]~\\&~g; ' |
+ tr '\n' ' '
+ )" '"$@"'
+
+exec "$JAVACMD" "$@"
diff --git a/gradlew.bat b/gradlew.bat
new file mode 100644
index 0000000..9b42019
--- /dev/null
+++ b/gradlew.bat
@@ -0,0 +1,94 @@
+@rem
+@rem Copyright 2015 the original author or authors.
+@rem
+@rem Licensed under the Apache License, Version 2.0 (the "License");
+@rem you may not use this file except in compliance with the License.
+@rem You may obtain a copy of the License at
+@rem
+@rem https://www.apache.org/licenses/LICENSE-2.0
+@rem
+@rem Unless required by applicable law or agreed to in writing, software
+@rem distributed under the License is distributed on an "AS IS" BASIS,
+@rem WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+@rem See the License for the specific language governing permissions and
+@rem limitations under the License.
+@rem
+@rem SPDX-License-Identifier: Apache-2.0
+@rem
+
+@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
+
+set DIRNAME=%~dp0
+if "%DIRNAME%"=="" set DIRNAME=.
+@rem This is normally unused
+set APP_BASE_NAME=%~n0
+set APP_HOME=%DIRNAME%
+
+@rem Resolve any "." and ".." in APP_HOME to make it shorter.
+for %%i in ("%APP_HOME%") do set APP_HOME=%%~fi
+
+@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="-Xmx64m" "-Xms64m"
+
+@rem Find java.exe
+if defined JAVA_HOME goto findJavaFromJavaHome
+
+set JAVA_EXE=java.exe
+%JAVA_EXE% -version >NUL 2>&1
+if %ERRORLEVEL% equ 0 goto execute
+
+echo. 1>&2
+echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. 1>&2
+echo. 1>&2
+echo Please set the JAVA_HOME variable in your environment to match the 1>&2
+echo location of your Java installation. 1>&2
+
+goto fail
+
+:findJavaFromJavaHome
+set JAVA_HOME=%JAVA_HOME:"=%
+set JAVA_EXE=%JAVA_HOME%/bin/java.exe
+
+if exist "%JAVA_EXE%" goto execute
+
+echo. 1>&2
+echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME% 1>&2
+echo. 1>&2
+echo Please set the JAVA_HOME variable in your environment to match the 1>&2
+echo location of your Java installation. 1>&2
+
+goto fail
+
+: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 %*
+
+:end
+@rem End local scope for the variables with windows NT shell
+if %ERRORLEVEL% equ 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!
+set EXIT_CODE=%ERRORLEVEL%
+if %EXIT_CODE% equ 0 set EXIT_CODE=1
+if not ""=="%GRADLE_EXIT_CONSOLE%" exit %EXIT_CODE%
+exit /b %EXIT_CODE%
+
+: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..b35396e
--- /dev/null
+++ b/settings.gradle
@@ -0,0 +1,7 @@
+plugins {
+ id 'org.gradle.toolchains.foojay-resolver-convention' version '0.8.0'
+}
+
+include ':FtcRobotController'
+include ':TeamCode'
+include ':PathPlanning'
\ No newline at end of file