currentDetections = aprilTag.getDetections();
+ telemetry.addData("# AprilTags Detected", currentDetections.size());
+
+ // Step through the list of detections and display info for each one.
+ for (AprilTagDetection detection : currentDetections) {
+ if (detection.metadata != null) {
+ telemetry.addLine(String.format("\n==== (ID %d) %s", detection.id, detection.metadata.name));
+ telemetry.addLine(String.format("XYZ %6.1f %6.1f %6.1f (inch)", detection.ftcPose.x, detection.ftcPose.y, detection.ftcPose.z));
+ telemetry.addLine(String.format("PRY %6.1f %6.1f %6.1f (deg)", detection.ftcPose.pitch, detection.ftcPose.roll, detection.ftcPose.yaw));
+ telemetry.addLine(String.format("RBE %6.1f %6.1f %6.1f (inch, deg, deg)", detection.ftcPose.range, detection.ftcPose.bearing, detection.ftcPose.elevation));
+ } else {
+ telemetry.addLine(String.format("\n==== (ID %d) Unknown", detection.id));
+ telemetry.addLine(String.format("Center %6.0f %6.0f (pixels)", detection.center.x, detection.center.y));
+ }
+ } // end for() loop
+
+ // Add "key" information to telemetry
+ telemetry.addLine("\nkey:\nXYZ = X (Right), Y (Forward), Z (Up) dist.");
+ telemetry.addLine("PRY = Pitch, Roll & Yaw (XYZ Rotation)");
+ telemetry.addLine("RBE = Range, Bearing & Elevation");
+
+ } // end method telemetryAprilTag()
+
+ /**
+ * Set the active camera according to input from the gamepad.
+ */
+ private void doCameraSwitching() {
+ if (visionPortal.getCameraState() == CameraState.STREAMING) {
+ // If the left bumper is pressed, use Webcam 1.
+ // If the right bumper is pressed, use Webcam 2.
+ boolean newLeftBumper = gamepad1.left_bumper;
+ boolean newRightBumper = gamepad1.right_bumper;
+ if (newLeftBumper && !oldLeftBumper) {
+ visionPortal.setActiveCamera(webcam1);
+ } else if (newRightBumper && !oldRightBumper) {
+ visionPortal.setActiveCamera(webcam2);
+ }
+ oldLeftBumper = newLeftBumper;
+ oldRightBumper = newRightBumper;
+ }
+
+ } // end method doCameraSwitching()
+
+} // end class
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptCompassCalibration.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptCompassCalibration.java
deleted file mode 100644
index cd1da0a..0000000
--- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptCompassCalibration.java
+++ /dev/null
@@ -1,121 +0,0 @@
-/* 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.CompassSensor;
-import com.qualcomm.robotcore.util.ElapsedTime;
-
-/**
- * This file illustrates the concept of calibrating a MR Compass
- * It uses the common Pushbot hardware class to define the drive on the robot.
- * The code is structured as a LinearOpMode
- *
- * This code assumes there is a compass configured with the name "compass"
- *
- * This code will put the compass into calibration mode, wait three seconds and then attempt
- * to rotate two full turns clockwise. This will allow the compass to do a magnetic calibration.
- *
- * Once compete, the program will put the compass back into measurement mode and check to see if the
- * calibration was successful.
- *
- * 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: Compass Calibration", group="Concept")
-@Disabled
-public class ConceptCompassCalibration extends LinearOpMode {
-
- /* Declare OpMode members. */
- HardwarePushbot robot = new HardwarePushbot(); // Use a Pushbot's hardware
- private ElapsedTime runtime = new ElapsedTime();
- CompassSensor compass;
-
- final static double MOTOR_POWER = 0.2; // scale from 0 to 1
- static final long HOLD_TIME_MS = 3000;
- static final double CAL_TIME_SEC = 20;
-
- @Override
- public void runOpMode() {
-
- /* Initialize the drive system variables.
- * The init() method of the hardware class does all the work here
- */
- robot.init(hardwareMap);
-
- // get a reference to our Compass Sensor object.
- compass = hardwareMap.get(CompassSensor.class, "compass");
-
- // Send telemetry message to signify robot waiting;
- telemetry.addData("Status", "Ready to cal"); //
- telemetry.update();
-
- // Wait for the game to start (driver presses PLAY)
- waitForStart();
-
- // Set the compass to calibration mode
- compass.setMode(CompassSensor.CompassMode.CALIBRATION_MODE);
- telemetry.addData("Compass", "Compass in calibration mode");
- telemetry.update();
-
- sleep(HOLD_TIME_MS); // Just do a sleep while we switch modes
-
- // Start the robot rotating clockwise
- telemetry.addData("Compass", "Calibration mode. Turning the robot...");
- telemetry.update();
- robot.leftDrive.setPower(MOTOR_POWER);
- robot.rightDrive.setPower(-MOTOR_POWER);
-
- // run until time expires OR the driver presses STOP;
- runtime.reset();
- while (opModeIsActive() && (runtime.time() < CAL_TIME_SEC)) {
- idle();
- }
-
- // Stop all motors and turn off claibration
- robot.leftDrive.setPower(0);
- robot.rightDrive.setPower(0);
- compass.setMode(CompassSensor.CompassMode.MEASUREMENT_MODE);
- telemetry.addData("Compass", "Returning to measurement mode");
- telemetry.update();
-
- sleep(HOLD_TIME_MS); // Just do a sleep while we switch modes
-
- // Report whether the Calibration was successful or not.
- if (compass.calibrationFailed())
- telemetry.addData("Compass", "Calibrate Failed. Try Again!");
- else
- telemetry.addData("Compass", "Calibrate Passed.");
- telemetry.update();
- }
-}
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptDIMAsIndicator.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptDIMAsIndicator.java
deleted file mode 100644
index 75b6ad4..0000000
--- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptDIMAsIndicator.java
+++ /dev/null
@@ -1,103 +0,0 @@
-/* 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.DeviceInterfaceModule;
-import com.qualcomm.robotcore.util.ElapsedTime;
-
-/**
- * This OpMode illustrates using the Device Interface Module as a signalling device.
- * The code is structured as a LinearOpMode
- *
- * This code assumes a DIM name "dim".
- *
- * There are many examples where the robot might like to signal the driver, without requiring them
- * to look at the driver station. This might be something like a "ball in hopper" condition or a
- * "ready to shoot" condition.
- *
- * The DIM has two user settable indicator LEDs (one red one blue). These can be controlled
- * directly from your program.
- *
- * Use Android Studios 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: DIM As Indicator", group = "Concept")
-@Disabled
-public class ConceptDIMAsIndicator extends LinearOpMode {
-
- static final int BLUE_LED = 0; // Blue LED channel on DIM
- static final int RED_LED = 1; // Red LED Channel on DIM
-
- // Create timer to toggle LEDs
- private ElapsedTime runtime = new ElapsedTime();
-
- // Define class members
- DeviceInterfaceModule dim;
-
- @Override
- public void runOpMode() {
-
- // Connect to motor (Assume standard left wheel)
- // Change the text in quotes to match any motor name on your robot.
- dim = hardwareMap.get(DeviceInterfaceModule.class, "dim");
-
- // Toggle LEDs while Waiting for the start button
- telemetry.addData(">", "Press Play to test LEDs." );
- telemetry.update();
-
- while (!isStarted()) {
- // Determine if we are on an odd or even second
- boolean even = (((int)(runtime.time()) & 0x01) == 0);
- dim.setLED(RED_LED, even); // Red for even
- dim.setLED(BLUE_LED, !even); // Blue for odd
- idle();
- }
-
- // Running now
- telemetry.addData(">", "Press X for Blue, B for Red." );
- telemetry.update();
-
- // Now just use red and blue buttons to set red and blue LEDs
- while(opModeIsActive()){
- dim.setLED(BLUE_LED, gamepad1.x);
- dim.setLED(RED_LED, gamepad1.b);
- idle();
- }
-
- // Turn off LEDs;
- dim.setLED(BLUE_LED, false);
- dim.setLED(RED_LED, false);
- telemetry.addData(">", "Done");
- telemetry.update();
- }
-}
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptExploringIMUOrientation.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptExploringIMUOrientation.java
new file mode 100644
index 0000000..751d54f
--- /dev/null
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptExploringIMUOrientation.java
@@ -0,0 +1,184 @@
+/*
+Copyright (c) 2022 REV Robotics, FIRST
+
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without modification,
+are permitted (subject to the limitations in the disclaimer below) provided that
+the following conditions are met:
+
+Redistributions of source code must retain the above copyright notice, this list
+of conditions and the following disclaimer.
+
+Redistributions in binary form must reproduce the above copyright notice, this
+list of conditions and the following disclaimer in the documentation and/or
+other materials provided with the distribution.
+
+Neither the name of REV Robotics nor the names of its contributors may be used to
+endorse or promote products derived from this software without specific prior
+written permission.
+
+NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
+LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR
+TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
+THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+package org.firstinspires.ftc.robotcontroller.external.samples;
+
+import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.hardware.IMU;
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+import org.firstinspires.ftc.robotcore.external.navigation.AngularVelocity;
+import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles;
+
+/*
+ * This OpMode demonstrates the impact of setting the IMU orientation correctly or incorrectly. This
+ * code assumes there is an IMU configured with the name "imu".
+ *
+ * Note: This OpMode is more of a tool than a code sample. The User Interface portion of this code
+ * goes beyond simply showing how to interface to the IMU.
+ * For a minimal example of interfacing to an IMU, please see the SensorIMUOrthogonal or SensorIMUNonOrthogonal sample OpModes.
+ *
+ * This OpMode enables you to re-specify the Hub Mounting orientation dynamically by using gamepad controls.
+ * While doing so, the sample will display how Pitch, Roll and Yaw angles change as the hub is moved.
+ *
+ * The gamepad controls let you change the two parameters that specify how the Control/Expansion Hub is mounted.
+ * The first parameter specifies which direction the printed logo on the Hub is pointing.
+ * The second parameter specifies which direction the USB connector on the Hub is pointing.
+ * All directions are relative to the robot, and left/right is as viewed from behind the robot.
+ *
+ * How will you know if you have chosen the correct Orientation? With the correct orientation
+ * parameters selected, pitch/roll/yaw should act as follows:
+ *
+ * Pitch value should INCREASE as the robot is tipped UP at the front. (Rotation about X)
+ * Roll value should INCREASE as the robot is tipped UP at the left side. (Rotation about Y)
+ * Yaw value should INCREASE as the robot is rotated Counter Clockwise. (Rotation about Z)
+ *
+ * The Yaw can be reset (to zero) by pressing the Y button on the gamepad (Triangle on a PS4 controller)
+ *
+ * The rotational velocities should follow the change in corresponding axes.
+ */
+
+@TeleOp(name="Concept: IMU Orientation", group="Concept")
+@Disabled
+public class ConceptExploringIMUOrientation extends LinearOpMode {
+ static RevHubOrientationOnRobot.LogoFacingDirection[] logoFacingDirections
+ = RevHubOrientationOnRobot.LogoFacingDirection.values();
+ static RevHubOrientationOnRobot.UsbFacingDirection[] usbFacingDirections
+ = RevHubOrientationOnRobot.UsbFacingDirection.values();
+ static int LAST_DIRECTION = logoFacingDirections.length - 1;
+ static float TRIGGER_THRESHOLD = 0.2f;
+
+ IMU imu;
+ int logoFacingDirectionPosition;
+ int usbFacingDirectionPosition;
+ boolean orientationIsValid = true;
+
+ @Override public void runOpMode() throws InterruptedException {
+ imu = hardwareMap.get(IMU.class, "imu");
+ logoFacingDirectionPosition = 0; // Up
+ usbFacingDirectionPosition = 2; // Forward
+
+ updateOrientation();
+
+ boolean justChangedLogoDirection = false;
+ boolean justChangedUsbDirection = false;
+
+ // Loop until stop requested
+ while (!isStopRequested()) {
+
+ // Check to see if Yaw reset is requested (Y button)
+ if (gamepad1.y) {
+ telemetry.addData("Yaw", "Resetting\n");
+ imu.resetYaw();
+ } else {
+ telemetry.addData("Yaw", "Press Y (triangle) on Gamepad to reset.\n");
+ }
+
+ // Check to see if new Logo Direction is requested
+ if (gamepad1.left_bumper || gamepad1.right_bumper) {
+ if (!justChangedLogoDirection) {
+ justChangedLogoDirection = true;
+ if (gamepad1.left_bumper) {
+ logoFacingDirectionPosition--;
+ if (logoFacingDirectionPosition < 0) {
+ logoFacingDirectionPosition = LAST_DIRECTION;
+ }
+ } else {
+ logoFacingDirectionPosition++;
+ if (logoFacingDirectionPosition > LAST_DIRECTION) {
+ logoFacingDirectionPosition = 0;
+ }
+ }
+ updateOrientation();
+ }
+ } else {
+ justChangedLogoDirection = false;
+ }
+
+ // Check to see if new USB Direction is requested
+ if (gamepad1.left_trigger > TRIGGER_THRESHOLD || gamepad1.right_trigger > TRIGGER_THRESHOLD) {
+ if (!justChangedUsbDirection) {
+ justChangedUsbDirection = true;
+ if (gamepad1.left_trigger > TRIGGER_THRESHOLD) {
+ usbFacingDirectionPosition--;
+ if (usbFacingDirectionPosition < 0) {
+ usbFacingDirectionPosition = LAST_DIRECTION;
+ }
+ } else {
+ usbFacingDirectionPosition++;
+ if (usbFacingDirectionPosition > LAST_DIRECTION) {
+ usbFacingDirectionPosition = 0;
+ }
+ }
+ updateOrientation();
+ }
+ } else {
+ justChangedUsbDirection = false;
+ }
+
+ // Display User instructions and IMU data
+ telemetry.addData("logo Direction (set with bumpers)", logoFacingDirections[logoFacingDirectionPosition]);
+ telemetry.addData("usb Direction (set with triggers)", usbFacingDirections[usbFacingDirectionPosition] + "\n");
+
+ if (orientationIsValid) {
+ YawPitchRollAngles orientation = imu.getRobotYawPitchRollAngles();
+ AngularVelocity angularVelocity = imu.getRobotAngularVelocity(AngleUnit.DEGREES);
+
+ telemetry.addData("Yaw (Z)", "%.2f Deg. (Heading)", orientation.getYaw(AngleUnit.DEGREES));
+ telemetry.addData("Pitch (X)", "%.2f Deg.", orientation.getPitch(AngleUnit.DEGREES));
+ telemetry.addData("Roll (Y)", "%.2f Deg.\n", orientation.getRoll(AngleUnit.DEGREES));
+ telemetry.addData("Yaw (Z) velocity", "%.2f Deg/Sec", angularVelocity.zRotationRate);
+ telemetry.addData("Pitch (X) velocity", "%.2f Deg/Sec", angularVelocity.xRotationRate);
+ telemetry.addData("Roll (Y) velocity", "%.2f Deg/Sec", angularVelocity.yRotationRate);
+ } else {
+ telemetry.addData("Error", "Selected orientation on robot is invalid");
+ }
+
+ telemetry.update();
+ }
+ }
+
+ // apply any requested orientation changes.
+ void updateOrientation() {
+ RevHubOrientationOnRobot.LogoFacingDirection logo = logoFacingDirections[logoFacingDirectionPosition];
+ RevHubOrientationOnRobot.UsbFacingDirection usb = usbFacingDirections[usbFacingDirectionPosition];
+ try {
+ RevHubOrientationOnRobot orientationOnRobot = new RevHubOrientationOnRobot(logo, usb);
+ imu.initialize(new IMU.Parameters(orientationOnRobot));
+ orientationIsValid = true;
+ } catch (IllegalArgumentException e) {
+ orientationIsValid = false;
+ }
+ }
+}
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptExternalHardwareClass.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptExternalHardwareClass.java
new file mode 100644
index 0000000..7a721fe
--- /dev/null
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptExternalHardwareClass.java
@@ -0,0 +1,142 @@
+/* Copyright (c) 2022 FIRST. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted (subject to the limitations in the disclaimer below) provided that
+ * the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this list
+ * of conditions and the following disclaimer.
+ *
+ * Redistributions in binary form must reproduce the above copyright notice, this
+ * list of conditions and the following disclaimer in the documentation and/or
+ * other materials provided with the distribution.
+ *
+ * Neither the name of FIRST nor the names of its contributors may be used to endorse or
+ * promote products derived from this software without specific prior written permission.
+ *
+ * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
+ * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+ * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+ * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+package org.firstinspires.ftc.robotcontroller.external.samples;
+
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.util.Range;
+
+/*
+ * This OpMode illustrates how to use an external "hardware" class to modularize all the robot's sensors and actuators.
+ * This approach is very efficient because the same hardware class can be used by all of your teleop and autonomous OpModes
+ * without requiring many copy & paste operations. Once you have defined and tested the hardware class with one OpMode,
+ * it is instantly available to other OpModes.
+ *
+ * The real benefit of this approach is that as you tweak your robot hardware, you only need to make changes in ONE place (the Hardware Class).
+ * So, to be effective you should put as much or your hardware setup and access code as possible in the hardware class.
+ * Essentially anything you do with hardware in BOTH Teleop and Auto should likely go in the hardware class.
+ *
+ * The Hardware Class is created in a separate file, and then an "instance" of this class is created in each OpMode.
+ * In order for the class to do typical OpMode things (like send telemetry data) it must be passed a reference to the
+ * OpMode object when it's created, so it can access all core OpMode functions. This is illustrated below.
+ *
+ * In this concept sample, the hardware class file is called RobotHardware.java and it must accompany this sample OpMode.
+ * So, if you copy ConceptExternalHardwareClass.java into TeamCode (using Android Studio or OnBotJava) then RobotHardware.java
+ * must also be copied to the same location (maintaining its name).
+ *
+ * For comparison purposes, this sample and its accompanying hardware class duplicates the functionality of the
+ * RobotTelopPOV_Linear OpMode. It assumes three motors (left_drive, right_drive and arm) and two servos (left_hand and right_hand)
+ *
+ * View the RobotHardware.java class file for more details
+ *
+ * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name.
+ * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list
+ *
+ * In OnBot Java, add a new OpMode, select this sample, and select TeleOp.
+ * Also add another new file named RobotHardware.java, select the sample with that name, and select Not an OpMode.
+ */
+
+@TeleOp(name="Concept: Robot Hardware Class", group="Robot")
+@Disabled
+public class ConceptExternalHardwareClass extends LinearOpMode {
+
+ // Create a RobotHardware object to be used to access robot hardware.
+ // Prefix any hardware functions with "robot." to access this class.
+ RobotHardware robot = new RobotHardware(this);
+
+ @Override
+ public void runOpMode() {
+ double drive = 0;
+ double turn = 0;
+ double arm = 0;
+ double handOffset = 0;
+
+ // initialize all the hardware, using the hardware class. See how clean and simple this is?
+ robot.init();
+
+ // Send telemetry message to signify robot waiting;
+ // Wait for the game to start (driver presses START)
+ waitForStart();
+
+ // run until the end of the match (driver presses STOP)
+ while (opModeIsActive()) {
+
+ // Run wheels in POV mode (note: The joystick goes negative when pushed forward, so negate it)
+ // In this mode the Left stick moves the robot fwd and back, the Right stick turns left and right.
+ // This way it's also easy to just drive straight, or just turn.
+ drive = -gamepad1.left_stick_y;
+ turn = gamepad1.right_stick_x;
+
+ // Combine drive and turn for blended motion. Use RobotHardware class
+ robot.driveRobot(drive, turn);
+
+ // Use gamepad left & right Bumpers to open and close the claw
+ // Use the SERVO constants defined in RobotHardware class.
+ // Each time around the loop, the servos will move by a small amount.
+ // Limit the total offset to half of the full travel range
+ if (gamepad1.right_bumper)
+ handOffset += robot.HAND_SPEED;
+ else if (gamepad1.left_bumper)
+ handOffset -= robot.HAND_SPEED;
+ handOffset = Range.clip(handOffset, -0.5, 0.5);
+
+ // Move both servos to new position. Use RobotHardware class
+ robot.setHandPositions(handOffset);
+
+ // Use gamepad buttons to move arm up (Y) and down (A)
+ // Use the MOTOR constants defined in RobotHardware class.
+ if (gamepad1.y)
+ arm = robot.ARM_UP_POWER;
+ else if (gamepad1.a)
+ arm = robot.ARM_DOWN_POWER;
+ else
+ arm = 0;
+
+ robot.setArmPower(arm);
+
+ // Send telemetry messages to explain controls and show robot status
+ telemetry.addData("Drive", "Left Stick");
+ telemetry.addData("Turn", "Right Stick");
+ telemetry.addData("Arm Up/Down", "Y & A Buttons");
+ telemetry.addData("Hand Open/Closed", "Left and Right Bumpers");
+ telemetry.addData("-", "-------");
+
+ telemetry.addData("Drive Power", "%.2f", drive);
+ telemetry.addData("Turn Power", "%.2f", turn);
+ telemetry.addData("Arm Power", "%.2f", arm);
+ telemetry.addData("Hand Position", "Offset = %.2f", handOffset);
+ telemetry.update();
+
+ // Pace this loop so hands move at a reasonable speed.
+ sleep(50);
+ }
+ }
+}
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptGamepadRumble.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptGamepadRumble.java
index 929fdaa..cf846e1 100644
--- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptGamepadRumble.java
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptGamepadRumble.java
@@ -6,13 +6,13 @@
import com.qualcomm.robotcore.hardware.Gamepad;
import com.qualcomm.robotcore.util.ElapsedTime;
-/**
- * This sample illustrates using the rumble feature of many gamepads.
+/*
+ * This OpMode illustrates using the rumble feature of many gamepads.
*
* Note: Some gamepads "rumble" better than others.
* The Xbox & PS4 have a left (rumble1) and right (rumble2) rumble motor.
* These two gamepads have two distinct rumble modes: Large on the left, and small on the right
- * The ETpark gamepad may only respond to rumble1, and may only run at full power.
+ * The Etpark gamepad may only respond to rumble1, and may only run at full power.
* The Logitech F310 gamepad does not have *any* rumble ability.
*
* Moral: You should use this sample to experiment with your specific gamepads to explore their rumble features.
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptGamepadTouchpad.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptGamepadTouchpad.java
index dc1795a..84d8cec 100644
--- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptGamepadTouchpad.java
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptGamepadTouchpad.java
@@ -1,13 +1,12 @@
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.eventloop.opmode.Disabled;
-
import org.firstinspires.ftc.robotcore.external.Telemetry;
-/**
- * This sample illustrates using the touchpad feature found on some gamepads.
+/*
+ * This OpMode illustrates using the touchpad feature found on some gamepads.
*
* The Sony PS4 gamepad can detect two distinct touches on the central touchpad.
* Other gamepads with different touchpads may provide mixed results.
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptI2cAddressChange.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptI2cAddressChange.java
deleted file mode 100644
index 4deef91..0000000
--- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptI2cAddressChange.java
+++ /dev/null
@@ -1,223 +0,0 @@
-/* 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.ModernRoboticsUsbDeviceInterfaceModule;
-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.DeviceInterfaceModule;
-import com.qualcomm.robotcore.hardware.I2cAddr;
-import com.qualcomm.robotcore.util.RobotLog;
-import com.qualcomm.robotcore.util.TypeConversion;
-
-import java.util.concurrent.locks.Lock;
-
-/**
- * An example of a linear op mode that shows how to change the I2C address.
- */
-@TeleOp(name = "Concept: I2c Address Change", group = "Concept")
-@Disabled
-public class ConceptI2cAddressChange extends LinearOpMode {
-
- public static final int ADDRESS_SET_NEW_I2C_ADDRESS = 0x70;
- // trigger bytes used to change I2C address on ModernRobotics sensors.
- public static final byte TRIGGER_BYTE_1 = 0x55;
- public static final byte TRIGGER_BYTE_2 = (byte) 0xaa;
-
- // Expected bytes from the Modern Robotics IR Seeker V3 memory map
- public static final byte IR_SEEKER_V3_FIRMWARE_REV = 0x12;
- public static final byte IR_SEEKER_V3_SENSOR_ID = 0x49;
- public static final I2cAddr IR_SEEKER_V3_ORIGINAL_ADDRESS = I2cAddr.create8bit(0x38);
-
- // Expected bytes from the Modern Robotics Color Sensor memory map
- public static final byte COLOR_SENSOR_FIRMWARE_REV = 0x10;
- public static final byte COLOR_SENSOR_SENSOR_ID = 0x43;
- public static final byte COLOR_SENSOR_ORIGINAL_ADDRESS = 0x3C;
-
- public static final byte MANUFACTURER_CODE = 0x4d;
- // Currently, this is set to expect the bytes from the IR Seeker.
- // If you change these values so you're setting "FIRMWARE_REV" to
- // COLOR_SENSOR_FIRMWARE_REV, and "SENSOR_ID" to "COLOR_SENSOR_SENSOR_ID",
- // you'll be able to change the I2C address of the ModernRoboticsColorSensor.
- // If the bytes you're expecting are different than what this op mode finds,
- // a comparison will be printed out into the logfile.
- public static final byte FIRMWARE_REV = IR_SEEKER_V3_FIRMWARE_REV;
- public static final byte SENSOR_ID = IR_SEEKER_V3_SENSOR_ID;
-
- // These byte values are common with most Modern Robotics sensors.
- public static final int READ_MODE = 0x80;
- public static final int ADDRESS_MEMORY_START = 0x0;
- public static final int TOTAL_MEMORY_LENGTH = 0x0c;
- public static final int BUFFER_CHANGE_ADDRESS_LENGTH = 0x03;
-
- // The port where your sensor is connected.
- int port = 5;
-
- byte[] readCache;
- Lock readLock;
- byte[] writeCache;
- Lock writeLock;
-
- I2cAddr currentAddress = IR_SEEKER_V3_ORIGINAL_ADDRESS;
- // I2c addresses on Modern Robotics devices must be divisible by 2, and between 0x7e and 0x10
- // Different hardware may have different rules.
- // Be sure to read the requirements for the hardware you're using!
- // If you use an invalid address, you may make your device completely unusable.
- I2cAddr newAddress = I2cAddr.create8bit(0x42);
-
- DeviceInterfaceModule dim;
-
- @Override
- public void runOpMode() {
-
- // set up the hardware devices we are going to use
- dim = hardwareMap.get(DeviceInterfaceModule.class, "dim");
-
- readCache = dim.getI2cReadCache(port);
- readLock = dim.getI2cReadCacheLock(port);
- writeCache = dim.getI2cWriteCache(port);
- writeLock = dim.getI2cWriteCacheLock(port);
-
- // I2c addresses on Modern Robotics devices must be divisible by 2, and between 0x7e and 0x10
- // Different hardware may have different rules.
- // Be sure to read the requirements for the hardware you're using!
- ModernRoboticsUsbDeviceInterfaceModule.throwIfModernRoboticsI2cAddressIsInvalid(newAddress);
-
- // wait for the start button to be pressed
- waitForStart();
-
- performAction("read", port, currentAddress, ADDRESS_MEMORY_START, TOTAL_MEMORY_LENGTH);
-
- while(!dim.isI2cPortReady(port)) {
- telemetry.addData("I2cAddressChange", "waiting for the port to be ready...");
- telemetry.update();
- sleep(1000);
- }
-
- // update the local cache
- dim.readI2cCacheFromController(port);
-
- // make sure the first bytes are what we think they should be.
- int count = 0;
- int[] initialArray = {READ_MODE, currentAddress.get8Bit(), ADDRESS_MEMORY_START, TOTAL_MEMORY_LENGTH, FIRMWARE_REV, MANUFACTURER_CODE, SENSOR_ID};
- while (!foundExpectedBytes(initialArray, readLock, readCache)) {
- telemetry.addData("I2cAddressChange", "Confirming that we're reading the correct bytes...");
- telemetry.update();
- dim.readI2cCacheFromController(port);
- sleep(1000);
- count++;
- // if we go too long with failure, we probably are expecting the wrong bytes.
- if (count >= 10) {
- telemetry.addData("I2cAddressChange", String.format("Looping too long with no change, probably have the wrong address. Current address: 8bit=0x%02x", currentAddress.get8Bit()));
- hardwareMap.irSeekerSensor.get(String.format("Looping too long with no change, probably have the wrong address. Current address: 8bit=0x%02x", currentAddress.get8Bit()));
- telemetry.update();
- }
- }
-
- // Enable writes to the correct segment of the memory map.
- performAction("write", port, currentAddress, ADDRESS_SET_NEW_I2C_ADDRESS, BUFFER_CHANGE_ADDRESS_LENGTH);
-
- // Write out the trigger bytes, and the new desired address.
- writeNewAddress();
- dim.setI2cPortActionFlag(port);
- dim.writeI2cCacheToController(port);
-
- telemetry.addData("I2cAddressChange", "Giving the hardware 60 seconds to make the change...");
- telemetry.update();
-
- // Changing the I2C address takes some time.
- sleep(60000);
-
- // Query the new address and see if we can get the bytes we expect.
- dim.enableI2cReadMode(port, newAddress, ADDRESS_MEMORY_START, TOTAL_MEMORY_LENGTH);
- dim.setI2cPortActionFlag(port);
- dim.writeI2cCacheToController(port);
-
- int[] confirmArray = {READ_MODE, newAddress.get8Bit(), ADDRESS_MEMORY_START, TOTAL_MEMORY_LENGTH, FIRMWARE_REV, MANUFACTURER_CODE, SENSOR_ID};
- while (!foundExpectedBytes(confirmArray, readLock, readCache)) {
- telemetry.addData("I2cAddressChange", "Have not confirmed the changes yet...");
- telemetry.update();
- dim.readI2cCacheFromController(port);
- sleep(1000);
- }
-
- telemetry.addData("I2cAddressChange", "Successfully changed the I2C address. New address: 8bit=0x%02x", newAddress.get8Bit());
- telemetry.update();
- RobotLog.i("Successfully changed the I2C address." + String.format("New address: 8bit=0x%02x", newAddress.get8Bit()));
-
- /**** IMPORTANT NOTE ******/
- // You need to add a line like this at the top of your op mode
- // to update the I2cAddress in the driver.
- //irSeeker.setI2cAddress(newAddress);
- /***************************/
-
- }
-
- private boolean foundExpectedBytes(int[] byteArray, Lock lock, byte[] cache) {
- try {
- lock.lock();
- boolean allMatch = true;
- StringBuilder s = new StringBuilder(300 * 4);
- String mismatch = "";
- for (int i = 0; i < byteArray.length; i++) {
- s.append(String.format("expected: %02x, got: %02x \n", TypeConversion.unsignedByteToInt( (byte) byteArray[i]), cache[i]));
- if (TypeConversion.unsignedByteToInt(cache[i]) != TypeConversion.unsignedByteToInt( (byte) byteArray[i])) {
- mismatch = String.format("i: %d, byteArray[i]: %02x, cache[i]: %02x", i, byteArray[i], cache[i]);
- allMatch = false;
- }
- }
- RobotLog.e(s.toString() + "\n allMatch: " + allMatch + ", mismatch: " + mismatch);
- return allMatch;
- } finally {
- lock.unlock();
- }
- }
-
- private void performAction(String actionName, int port, I2cAddr i2cAddress, int memAddress, int memLength) {
- if (actionName.equalsIgnoreCase("read")) dim.enableI2cReadMode(port, i2cAddress, memAddress, memLength);
- if (actionName.equalsIgnoreCase("write")) dim.enableI2cWriteMode(port, i2cAddress, memAddress, memLength);
-
- dim.setI2cPortActionFlag(port);
- dim.writeI2cCacheToController(port);
- dim.readI2cCacheFromController(port);
- }
-
- private void writeNewAddress() {
- try {
- writeLock.lock();
- writeCache[4] = (byte) newAddress.get8Bit();
- writeCache[5] = TRIGGER_BYTE_1;
- writeCache[6] = TRIGGER_BYTE_2;
- } finally {
- writeLock.unlock();
- }
- }
-}
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptLEDStick.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptLEDStick.java
new file mode 100644
index 0000000..01729bb
--- /dev/null
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptLEDStick.java
@@ -0,0 +1,123 @@
+package org.firstinspires.ftc.robotcontroller.external.samples;
+/*
+ Copyright (c) 2021-24 Alan Smith
+
+ All rights reserved.
+
+ Redistribution and use in source and binary forms, with or without modification,
+ are permitted (subject to the limitations in the disclaimer below) provided that
+ the following conditions are met:
+
+ Redistributions of source code must retain the above copyright notice, this list
+ of conditions and the following disclaimer.
+
+ Redistributions in binary form must reproduce the above copyright notice, this
+ list of conditions and the following disclaimer in the documentation and/or
+ other materials provided with the distribution.
+
+ Neither the name of Alan Smith nor the names of its contributors may be used to
+ endorse or promote products derived from this software without specific prior
+ written permission.
+
+ NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
+ LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESSFOR A PARTICULAR PURPOSE
+ ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
+ FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+ SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR
+ TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
+ THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+import android.graphics.Color;
+
+import com.qualcomm.hardware.sparkfun.SparkFunLEDStick;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.util.Range;
+
+/*
+ * This OpMode illustrates how to use the SparkFun QWIIC LED Strip
+ *
+ * This is a simple way to add a strip of 10 LEDs to your robot where you can set the color of each
+ * LED or the whole strip. This allows for driver feedback or even just fun ways to show your team
+ * colors.
+ *
+ * Why?
+ * Because more LEDs == more fun!!
+ *
+ * This OpMode assumes that the QWIIC LED Stick is attached to an I2C interface named "back_leds" in the robot configuration.
+ *
+ * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name.
+ * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list
+ *
+ * You can buy this product here: https://www.sparkfun.com/products/18354
+ * Don't forget to also buy this to make it easy to connect to your Control or Expansion Hub:
+ * https://www.sparkfun.com/products/25596
+ */
+@TeleOp(name = "Concept: LED Stick", group = "Concept")
+@Disabled
+public class ConceptLEDStick extends OpMode {
+ private boolean wasUp;
+ private boolean wasDown;
+ private int brightness = 5; // this needs to be between 0 and 31
+ private final static double END_GAME_TIME = 120 - 30;
+
+ private SparkFunLEDStick ledStick;
+
+ @Override
+ public void init() {
+ ledStick = hardwareMap.get(SparkFunLEDStick.class, "back_leds");
+ ledStick.setBrightness(brightness);
+ ledStick.setColor(Color.GREEN);
+ }
+
+ @Override
+ public void start() {
+ resetRuntime();
+ }
+
+ @Override
+ public void loop() {
+ telemetry.addLine("Hold the A button to turn blue");
+ telemetry.addLine("Hold the B button to turn red");
+ telemetry.addLine("Hold the left bumper to turn off");
+ telemetry.addLine("Use DPAD Up/Down to change brightness");
+
+ if (getRuntime() > END_GAME_TIME) {
+ int[] ledColors = {Color.RED, Color.YELLOW, Color.RED, Color.YELLOW, Color.RED,
+ Color.YELLOW, Color.RED, Color.YELLOW, Color.RED, Color.YELLOW};
+ ledStick.setColors(ledColors);
+ } else if (gamepad1.a) {
+ ledStick.setColor(Color.BLUE);
+ } else if (gamepad1.b) {
+ ledStick.setColor(Color.RED);
+ } else if (gamepad1.left_bumper) {
+ ledStick.turnAllOff();
+ } else {
+ ledStick.setColor(Color.GREEN);
+ }
+
+ /*
+ * Use DPAD up and down to change brightness
+ */
+ int newBrightness = brightness;
+ if (gamepad1.dpad_up && !wasUp) {
+ newBrightness = brightness + 1;
+ } else if (gamepad1.dpad_down && !wasDown) {
+ newBrightness = brightness - 1;
+ }
+ if (newBrightness != brightness) {
+ brightness = Range.clip(newBrightness, 0, 31);
+ ledStick.setBrightness(brightness);
+ }
+ telemetry.addData("Brightness", brightness);
+
+ wasDown = gamepad1.dpad_down;
+ wasUp = gamepad1.dpad_up;
+ }
+}
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptMotorBulkRead.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptMotorBulkRead.java
index 4e6f52f..5439f78 100644
--- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptMotorBulkRead.java
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptMotorBulkRead.java
@@ -36,44 +36,49 @@
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.util.ElapsedTime;
-import java.util.Iterator;
import java.util.List;
- /*
- This sample illustrates how to use the Expansion Hub's Bulk-Read feature to speed up control cycle times.
- In this example there are 4 motors that need their encoder positions, and velocities read.
- The sample is written to work with one or two expansion hubs, with no assumption as to where the motors are located.
-
- Three scenarios are tested:
- Cache Mode = OFF This is the normal default, where no cache is used, and every read produces a discrete transaction with
- an expansion hub, which is the slowest approach.
- Cache Mode = AUTO This mode will attempt to minimize the number of discrete read commands, by performing bulk-reads
- and then returning values that have been cached. The cache is updated automatically whenever a specific read operation is repeated.
- This mode will always return fresh data, but it may perform more bulk-reads than absolutely required.
- Extra reads will be performed if multiple identical encoder/velocity reads are performed in one control cycle.
- This mode is a good compromise between the OFF and MANUAL modes.
- Cache Mode = MANUAL This mode enables the user's code to determine the best time to refresh the cached bulk-read data.
- Well organized code can place all the sensor reads in one location, and then just reset the cache once per control cycle.
- The approach will produce the shortest cycle times, but it does require the user to manually clear the cache.
-
- -------------------------------------
-
- General tip to speed up your control cycles:
- No matter what method you use to read encoders and other inputs, you should try to
- avoid reading the same input multiple times around a control loop.
- Under normal conditions, this will slow down the control loop.
- The preferred method is to read all the required inputs ONCE at the beginning of the loop,
- and save the values in variable that can be used by other parts of the control code.
-
- eg: if you are sending encoder positions to your telemetry display, putting a getCurrentPosition()
- call in the telemetry statement will force the code to go and get another copy which will take time.
- It's much better read the position into a variable once, and use that variable for control AND display.
- Reading saved variables takes no time at all.
-
- Once you put all your sensor reads at the beginning of the control cycle, it's very easy to use
- the bulk-read AUTO mode to streamline your cycle timing.
-
- */
+/*
+ * This OpMode illustrates how to use the Expansion Hub's Bulk-Read feature to speed up control cycle times.
+ * In this example there are 4 motors that need their encoder positions, and velocities read.
+ * The sample is written to work with one or two expansion hubs, with no assumption as to where the motors are located.
+ *
+ * Three scenarios are tested:
+ * Cache Mode = OFF This is the normal default, where no cache is used, and every read produces a discrete transaction with
+ * an expansion hub, which is the slowest approach, but guarentees that the value is as fresh (recent) as possible..
+ *
+ * Cache Mode = AUTO This mode will attempt to minimize the number of discrete read commands, by performing bulk-reads
+ * and then returning values that have been cached. The cache is updated automatically whenever any specific encoder is re-read.
+ * This mode will always return new data, but it may perform more bulk-reads than absolutely required.
+ * Extra reads will be performed if multiple encoder/velocity reads are performed on the same encoder in one control cycle.
+ * This mode is a good compromise between the OFF and MANUAL modes.
+ * Note: If there are significant user-program delays between encoder reads, the cached value may not be fresh (recent).
+ * You can issue a clearBulkCache() call at any time force a fresh bulk-read on the next encoder read.
+ *
+ * Cache Mode = MANUAL This mode requires the user's code to determine the best time to clear the cached bulk-read data.
+ * Well organized code will reset the cache once at the beginning of the control cycle, and then immediately read and store all the encoder values.
+ * This approach will produce the shortest cycle times, but it does require the user to manually clear the cache.
+ * Since NO automatic Bulk-Reads are performed, neglecting to clear the bulk cache will result in the same values being returned
+ * each time an encoder read is performed.
+ *
+ * -------------------------------------
+ *
+ * General tip to speed up your control cycles:
+ *
+ * No matter what method you use to read encoders and other inputs, you should try to
+ * avoid reading the same encoder input multiple times around a control loop.
+ * Under normal conditions, this will slow down the control loop.
+ * The preferred method is to read all the required inputs ONCE at the beginning of the loop,
+ * and save the values in variable that can be used by other parts of the control code.
+ *
+ * eg: if you are sending encoder positions to your telemetry display, putting a getCurrentPosition()
+ * call in the telemetry statement will force the code to go and get another copy which will take time.
+ * It's much better read the position into a variable once, and use that variable for control AND display.
+ * Reading saved variables takes no time at all.
+ *
+ * Once you put all your sensor reads at the beginning of the control cycle, it's very easy to use
+ * the bulk-read AUTO mode to streamline your cycle timing.
+ */
@TeleOp (name = "Motor Bulk Reads", group = "Tests")
@Disabled
public class ConceptMotorBulkRead extends LinearOpMode {
@@ -105,7 +110,7 @@ public void runOpMode() {
ElapsedTime timer = new ElapsedTime();
- telemetry.addData(">", "Press play to start tests");
+ telemetry.addData(">", "Press START to start tests");
telemetry.addData(">", "Test results will update for each access method.");
telemetry.update();
waitForStart();
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
index 2f84695..4a887bd 100644
--- 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
@@ -34,11 +34,8 @@
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.util.ElapsedTime;
-import java.text.SimpleDateFormat;
-import java.util.Date;
-
-/**
- * Demonstrates empty OpMode
+/*
+ * Demonstrates an empty iterative OpMode
*/
@TeleOp(name = "Concept: NullOp", group = "Concept")
@Disabled
@@ -46,34 +43,47 @@ 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");
}
- /*
- * Code to run when the op mode is first enabled goes here
- * @see com.qualcomm.robotcore.eventloop.opmode.OpMode#start()
- */
+ /**
+ * This method will be called repeatedly during the period between when
+ * the INIT button is pressed and when the START button is pressed (or the
+ * OpMode is stopped).
+ */
@Override
public void init_loop() {
}
- /*
- * This method will be called ONCE when start is pressed
- * @see com.qualcomm.robotcore.eventloop.opmode.OpMode#loop()
+ /**
+ * This method will be called once, when the START button is pressed.
*/
@Override
public void start() {
runtime.reset();
}
- /*
- * This method will be called repeatedly in a loop
- * @see com.qualcomm.robotcore.eventloop.opmode.OpMode#loop()
+ /**
+ * This method will be called repeatedly during the period between when
+ * the START button is pressed and when the OpMode is stopped.
*/
@Override
public void loop() {
telemetry.addData("Status", "Run Time: " + runtime.toString());
}
+
+ /**
+ * This method will be called once, when this OpMode is stopped.
+ *
+ * Your ability to control hardware from this method will be limited.
+ */
+ @Override
+ public void stop() {
+
+ }
}
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptRampMotorSpeed.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptRampMotorSpeed.java
index 0a8f3dc..6e0be37 100644
--- 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
@@ -34,17 +34,17 @@
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 pushbot.
+ * 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
+ * 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
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptRevLED.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptRevLED.java
new file mode 100644
index 0000000..9c168d5
--- /dev/null
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptRevLED.java
@@ -0,0 +1,78 @@
+/*
+ Copyright (c) 2024 Alan Smith
+ All rights reserved.
+ Redistribution and use in source and binary forms, with or without modification,
+ are permitted (subject to the limitations in the disclaimer below) provided that
+ the following conditions are met:
+ Redistributions of source code must retain the above copyright notice, this list
+ of conditions and the following disclaimer.
+ Redistributions in binary form must reproduce the above copyright notice, this
+ list of conditions and the following disclaimer in the documentation and/or
+ other materials provided with the distribution.
+ Neither the name of Alan Smith nor the names of its contributors may be used to
+ endorse or promote products derived from this software without specific prior
+ written permission.
+ NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
+ LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESSFOR A PARTICULAR PURPOSE
+ ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
+ FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+ SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR
+ TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
+ THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+package org.firstinspires.ftc.robotcontroller.external.samples;
+
+/*
+ * This OpMode illustrates how to use the REV Digital Indicator
+ *
+ * This is a simple way to add the REV Digital Indicator which has a red and green LED.
+ * (and as you may remember, red + green = yellow so when they are both on you can get yellow)
+ *
+ * Why?
+ * You can use this to show information to the driver. For example, green might be that you can
+ * pick up more game elements and red would be that you already have the possession limit.
+ *
+ * This OpMode assumes that the REV Digital Indicator is setup as 2 Digital Channels named
+ * front_led_green and front_led_red. (the green should be the lower of the 2 channels it is plugged
+ * into and the red should be the higher)
+ *
+ * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name.
+ * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list
+ *
+ * You can buy this product here: https://www.revrobotics.com/rev-31-2010/
+ */
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.hardware.LED;
+
+@TeleOp(name = "Concept: RevLED", group = "Concept")
+@Disabled
+public class ConceptRevLED extends OpMode {
+ LED frontLED_red;
+ LED frontLED_green;
+
+ @Override
+ public void init() {
+ frontLED_green = hardwareMap.get(LED.class, "front_led_green");
+ frontLED_red = hardwareMap.get(LED.class, "front_led_red");
+ }
+
+ @Override
+ public void loop() {
+ if (gamepad1.a) {
+ frontLED_red.on();
+ } else {
+ frontLED_red.off();
+ }
+ if (gamepad1.b) {
+ frontLED_green.on();
+ } else {
+ frontLED_green.off();
+ }
+ }
+}
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptRevSPARKMini.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptRevSPARKMini.java
index 1b1ecc3..798d689 100644
--- 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
@@ -37,15 +37,15 @@
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
+/*
+ * This OpMode demonstrates a POV Drive system, with commented-out code for a Tank Drive system,
+ * for a two wheeled robot using two REV SPARKminis.
+ * To use this example, connect two REV SPARKminis into servo ports on the Control Hub. On the
* robot configuration, use the drop down list under 'Servos' to select 'REV SPARKmini Controller'
* and name them 'left_drive' and 'right_drive'.
*
- * Use Android Studios 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
+ * 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")
@@ -63,17 +63,16 @@ public void runOpMode() {
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).
+ // to 'get' must correspond to the names assigned during robot configuration.
leftDrive = hardwareMap.get(DcMotorSimple.class, "left_drive");
rightDrive = hardwareMap.get(DcMotorSimple.class, "right_drive");
// Most robots need the motor on one side to be reversed to drive forward
- // Reverse the motor that runs backwards when connected directly to the battery
+ // 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)
+ // Wait for the game to start (driver presses START)
waitForStart();
runtime.reset();
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
index 8e9c899..2b8ad33 100644
--- 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
@@ -34,19 +34,19 @@
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.Servo;
-/**
- * This OpMode scans a single servo back and forwards until Stop is pressed.
+/*
+ * 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 pushbot.
+ * 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
+ * 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
@@ -66,7 +66,7 @@ public class ConceptScanServo extends LinearOpMode {
@Override
public void runOpMode() {
- // Connect to servo (Assume PushBot Left Hand)
+ // 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");
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
index 51d99d2..1ceaa17 100644
--- 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
@@ -34,17 +34,15 @@
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
-import java.io.File;
-
-/**
- * This file demonstrates how to play simple sounds on both the RC and DS phones.
+/*
+ * 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 Studios 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
+ * 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:
*
@@ -102,7 +100,7 @@ public void runOpMode() {
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)
+ // Wait for the game to start (driver presses START)
telemetry.addData(">", "Press Start to continue");
telemetry.update();
waitForStart();
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
index bacb66e..fbb7416 100644
--- 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
@@ -33,10 +33,11 @@
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 file demonstrates how to play simple sounds on both the RC and DS phones.
+/*
+ * 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.
*
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
index e8f3dde..983e434 100644
--- 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
@@ -30,19 +30,18 @@
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;
-import com.qualcomm.robotcore.eventloop.opmode.Disabled;
-/**
- * This file demonstrates how to play one of the several SKYSTONE/Star Wars sounds loaded into the SDK.
+/*
+ * 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 Studios 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
+ * 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.
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
index d3b4682..f2c6097 100644
--- 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
@@ -34,26 +34,25 @@
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;
-/**
- * {@link ConceptTelemetry} illustrates various ways in which telemetry can be
+/*
+ * 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 {@link Telemetry#log() log} is illustrated by scrolling a poem
+ * information. The telemetry log is illustrated by scrolling a poem
* to the driver station.
*
- * @see Telemetry
+ * 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 */
+ /** 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 */
+ /** 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[] {
@@ -94,10 +93,10 @@ public class ConceptTelemetry extends LinearOpMode {
// 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 {@link #waitForStart()}.
+ * we waited, we would just call waitForStart().
*/
while (!isStarted()) {
telemetry.addData("time", "%.1f seconds", opmodeRunTime.seconds());
@@ -107,10 +106,10 @@ public class ConceptTelemetry extends LinearOpMode {
// 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
+ * 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.
*
@@ -122,7 +121,7 @@ public class ConceptTelemetry extends LinearOpMode {
}
});
- // Reset to keep some timing stats for the post-'start' part of the opmode
+ // Reset to keep some timing stats for the post-'start' part of the OpMode
opmodeRunTime.reset();
int loopCount = 1;
@@ -146,13 +145,13 @@ public class ConceptTelemetry extends LinearOpMode {
.addData("x", gamepad1.right_stick_x)
.addData("y", gamepad1.right_stick_y);
- /**
+ /*
* Transmit the telemetry to the driver station, subject to throttling.
- * @see Telemetry#getMsTransmissionInterval()
+ * See the documentation for Telemetry.getMsTransmissionInterval() for more information.
*/
telemetry.update();
- /** Update loop info and play nice with the rest of the {@link Thread}s in the system */
+ // Update loop info
loopCount++;
}
}
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptTensorFlowObjectDetection.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptTensorFlowObjectDetection.java
deleted file mode 100644
index 06436eb..0000000
--- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptTensorFlowObjectDetection.java
+++ /dev/null
@@ -1,186 +0,0 @@
-/* Copyright (c) 2019 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 java.util.List;
-import org.firstinspires.ftc.robotcore.external.ClassFactory;
-import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer;
-import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer.CameraDirection;
-import org.firstinspires.ftc.robotcore.external.tfod.TFObjectDetector;
-import org.firstinspires.ftc.robotcore.external.tfod.Recognition;
-
-/**
- * This 2020-2021 OpMode illustrates the basics of using the TensorFlow Object Detection API to
- * determine the position of the Freight Frenzy game elements.
- *
- * 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.
- *
- * IMPORTANT: In order to use this OpMode, you need to obtain your own Vuforia license key as
- * is explained below.
- */
-@TeleOp(name = "Concept: TensorFlow Object Detection", group = "Concept")
-@Disabled
-public class ConceptTensorFlowObjectDetection extends LinearOpMode {
- /* Note: This sample uses the all-objects Tensor Flow model (FreightFrenzy_BCDM.tflite), which contains
- * the following 4 detectable objects
- * 0: Ball,
- * 1: Cube,
- * 2: Duck,
- * 3: Marker (duck location tape marker)
- *
- * Two additional model assets are available which only contain a subset of the objects:
- * FreightFrenzy_BC.tflite 0: Ball, 1: Cube
- * FreightFrenzy_DM.tflite 0: Duck, 1: Marker
- */
- private static final String TFOD_MODEL_ASSET = "FreightFrenzy_BCDM.tflite";
- private static final String[] LABELS = {
- "Ball",
- "Cube",
- "Duck",
- "Marker"
- };
-
- /*
- * IMPORTANT: You need to obtain your own license key to use Vuforia. The string below with which
- * 'parameters.vuforiaLicenseKey' is initialized is for illustration only, and will not function.
- * A Vuforia 'Development' license key, can be obtained free of charge from the Vuforia developer
- * web site at https://developer.vuforia.com/license-manager.
- *
- * Vuforia license keys are always 380 characters long, and look as if they contain mostly
- * random data. As an example, here is a example of a fragment of a valid key:
- * ... yIgIzTqZ4mWjk9wd3cZO9T1axEqzuhxoGlfOOI2dRzKS4T0hQ8kT ...
- * Once you've obtained a license key, copy the string from the Vuforia web site
- * and paste it in to your code on the next line, between the double quotes.
- */
- private static final String VUFORIA_KEY =
- " -- YOUR NEW VUFORIA KEY GOES HERE --- ";
-
- /**
- * {@link #vuforia} is the variable we will use to store our instance of the Vuforia
- * localization engine.
- */
- private VuforiaLocalizer vuforia;
-
- /**
- * {@link #tfod} is the variable we will use to store our instance of the TensorFlow Object
- * Detection engine.
- */
- private TFObjectDetector tfod;
-
- @Override
- public void runOpMode() {
- // The TFObjectDetector uses the camera frames from the VuforiaLocalizer, so we create that
- // first.
- initVuforia();
- initTfod();
-
- /**
- * Activate TensorFlow Object Detection before we wait for the start command.
- * Do it here so that the Camera Stream window will have the TensorFlow annotations visible.
- **/
- if (tfod != null) {
- tfod.activate();
-
- // The TensorFlow software will scale the input images from the camera to a lower resolution.
- // This can result in lower detection accuracy at longer distances (> 55cm or 22").
- // If your target is at distance greater than 50 cm (20") you can adjust the magnification value
- // to artificially zoom in to the center of image. For best results, the "aspectRatio" argument
- // should be set to the value of the images used to create the TensorFlow Object Detection model
- // (typically 16/9).
- tfod.setZoom(2.5, 16.0/9.0);
- }
-
- /** Wait for the game to begin */
- telemetry.addData(">", "Press Play to start op mode");
- telemetry.update();
- waitForStart();
-
- if (opModeIsActive()) {
- while (opModeIsActive()) {
- if (tfod != null) {
- // getUpdatedRecognitions() will return null if no new information is available since
- // the last time that call was made.
- List updatedRecognitions = tfod.getUpdatedRecognitions();
- if (updatedRecognitions != null) {
- telemetry.addData("# Object Detected", updatedRecognitions.size());
-
- // step through the list of recognitions and display boundary info.
- int i = 0;
- for (Recognition recognition : updatedRecognitions) {
- telemetry.addData(String.format("label (%d)", i), recognition.getLabel());
- telemetry.addData(String.format(" left,top (%d)", i), "%.03f , %.03f",
- recognition.getLeft(), recognition.getTop());
- telemetry.addData(String.format(" right,bottom (%d)", i), "%.03f , %.03f",
- recognition.getRight(), recognition.getBottom());
- i++;
- }
- telemetry.update();
- }
- }
- }
- }
- }
-
- /**
- * Initialize the Vuforia localization engine.
- */
- private void initVuforia() {
- /*
- * Configure Vuforia by creating a Parameter object, and passing it to the Vuforia engine.
- */
- VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters();
-
- parameters.vuforiaLicenseKey = VUFORIA_KEY;
- parameters.cameraDirection = CameraDirection.BACK;
-
- // Instantiate the Vuforia engine
- vuforia = ClassFactory.getInstance().createVuforia(parameters);
-
- // Loading trackables is not necessary for the TensorFlow Object Detection engine.
- }
-
- /**
- * Initialize the TensorFlow Object Detection engine.
- */
- private void initTfod() {
- int tfodMonitorViewId = hardwareMap.appContext.getResources().getIdentifier(
- "tfodMonitorViewId", "id", hardwareMap.appContext.getPackageName());
- TFObjectDetector.Parameters tfodParameters = new TFObjectDetector.Parameters(tfodMonitorViewId);
- tfodParameters.minResultConfidence = 0.8f;
- tfodParameters.isModelTensorFlow2 = true;
- tfodParameters.inputSize = 320;
- tfod = ClassFactory.getInstance().createTFObjectDetector(tfodParameters, vuforia);
- tfod.loadModelFromAsset(TFOD_MODEL_ASSET, LABELS);
- }
-}
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptTensorFlowObjectDetectionSwitchableCameras.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptTensorFlowObjectDetectionSwitchableCameras.java
deleted file mode 100644
index f8c7df1..0000000
--- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptTensorFlowObjectDetectionSwitchableCameras.java
+++ /dev/null
@@ -1,221 +0,0 @@
-/* Copyright (c) 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 com.qualcomm.robotcore.eventloop.opmode.Disabled;
-import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
-import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
-import java.util.List;
-import org.firstinspires.ftc.robotcore.external.ClassFactory;
-import org.firstinspires.ftc.robotcore.external.hardware.camera.SwitchableCamera;
-import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
-import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer;
-import org.firstinspires.ftc.robotcore.external.tfod.TFObjectDetector;
-import org.firstinspires.ftc.robotcore.external.tfod.Recognition;
-
-/**
- * This 2020-2021 OpMode illustrates the basics of using the TensorFlow Object Detection API to
- * determine the position of the Freight Frenzy game elements.
- *
- * 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.
- *
- * IMPORTANT: In order to use this OpMode, you need to obtain your own Vuforia license key as
- * is explained below.
- */
-@TeleOp(name = "Concept: TensorFlow Object Detection Switchable Cameras", group = "Concept")
-@Disabled
-public class ConceptTensorFlowObjectDetectionSwitchableCameras extends LinearOpMode {
- /* Note: This sample uses the all-objects Tensor Flow model (FreightFrenzy_BCDM.tflite), which contains
- * the following 4 detectable objects
- * 0: Ball,
- * 1: Cube,
- * 2: Duck,
- * 3: Marker (duck location tape marker)
- *
- * Two additional model assets are available which only contain a subset of the objects:
- * FreightFrenzy_BC.tflite 0: Ball, 1: Cube
- * FreightFrenzy_DM.tflite 0: Duck, 1: Marker
- */
- private static final String TFOD_MODEL_ASSET = "FreightFrenzy_BCDM.tflite";
- private static final String[] LABELS = {
- "Ball",
- "Cube",
- "Duck",
- "Marker"
- };
-
- /*
- * IMPORTANT: You need to obtain your own license key to use Vuforia. The string below with which
- * 'parameters.vuforiaLicenseKey' is initialized is for illustration only, and will not function.
- * A Vuforia 'Development' license key, can be obtained free of charge from the Vuforia developer
- * web site at https://developer.vuforia.com/license-manager.
- *
- * Vuforia license keys are always 380 characters long, and look as if they contain mostly
- * random data. As an example, here is a example of a fragment of a valid key:
- * ... yIgIzTqZ4mWjk9wd3cZO9T1axEqzuhxoGlfOOI2dRzKS4T0hQ8kT ...
- * Once you've obtained a license key, copy the string from the Vuforia web site
- * and paste it in to your code on the next line, between the double quotes.
- */
- private static final String VUFORIA_KEY =
- " -- YOUR NEW VUFORIA KEY GOES HERE --- ";
-
- /**
- * {@link #vuforia} is the variable we will use to store our instance of the Vuforia
- * localization engine.
- */
- private VuforiaLocalizer vuforia;
-
- /**
- * Variables used for switching cameras.
- */
- private WebcamName webcam1, webcam2;
- private SwitchableCamera switchableCamera;
- private boolean oldLeftBumper;
- private boolean oldRightBumper;
-
- /**
- * {@link #tfod} is the variable we will use to store our instance of the TensorFlow Object
- * Detection engine.
- */
- private TFObjectDetector tfod;
-
- @Override
- public void runOpMode() {
- // The TFObjectDetector uses the camera frames from the VuforiaLocalizer, so we create that
- // first.
- initVuforia();
- initTfod();
-
- /**
- * Activate TensorFlow Object Detection before we wait for the start command.
- * Do it here so that the Camera Stream window will have the TensorFlow annotations visible.
- **/
- if (tfod != null) {
- tfod.activate();
-
- // The TensorFlow software will scale the input images from the camera to a lower resolution.
- // This can result in lower detection accuracy at longer distances (> 55cm or 22").
- // If your target is at distance greater than 50 cm (20") you can adjust the magnification value
- // to artificially zoom in to the center of image. For best results, the "aspectRatio" argument
- // should be set to the value of the images used to create the TensorFlow Object Detection model
- // (typically 16/9).
- tfod.setZoom(2.5, 16.0/9.0);
- }
-
- /** Wait for the game to begin */
- telemetry.addData(">", "Press Play to start op mode");
- telemetry.update();
- waitForStart();
-
- if (opModeIsActive()) {
- while (opModeIsActive()) {
- if (tfod != null) {
- doCameraSwitching();
- List recognitions = tfod.getRecognitions();
- telemetry.addData("# Object Detected", recognitions.size());
- // step through the list of recognitions and display boundary info.
- int i = 0;
- for (Recognition recognition : recognitions) {
- telemetry.addData(String.format("label (%d)", i), recognition.getLabel());
- telemetry.addData(String.format(" left,top (%d)", i), "%.03f , %.03f",
- recognition.getLeft(), recognition.getTop());
- telemetry.addData(String.format(" right,bottom (%d)", i), "%.03f , %.03f",
- recognition.getRight(), recognition.getBottom());
- i++;
- }
- telemetry.update();
- }
- }
- }
- }
-
- /**
- * Initialize the Vuforia localization engine.
- */
- private void initVuforia() {
- /*
- * Configure Vuforia by creating a Parameter object, and passing it to the Vuforia engine.
- */
- VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters();
-
- parameters.vuforiaLicenseKey = VUFORIA_KEY;
-
- // Indicate that we wish to be able to switch cameras.
- webcam1 = hardwareMap.get(WebcamName.class, "Webcam 1");
- webcam2 = hardwareMap.get(WebcamName.class, "Webcam 2");
- parameters.cameraName = ClassFactory.getInstance().getCameraManager().nameForSwitchableCamera(webcam1, webcam2);
-
- // Instantiate the Vuforia engine
- vuforia = ClassFactory.getInstance().createVuforia(parameters);
-
- // Set the active camera to Webcam 1.
- switchableCamera = (SwitchableCamera) vuforia.getCamera();
- switchableCamera.setActiveCamera(webcam1);
-
- // Loading trackables is not necessary for the TensorFlow Object Detection engine.
- }
-
- /**
- * Initialize the TensorFlow Object Detection engine.
- */
- private void initTfod() {
- int tfodMonitorViewId = hardwareMap.appContext.getResources().getIdentifier(
- "tfodMonitorViewId", "id", hardwareMap.appContext.getPackageName());
- TFObjectDetector.Parameters tfodParameters = new TFObjectDetector.Parameters(tfodMonitorViewId);
- tfodParameters.minResultConfidence = 0.8f;
- tfodParameters.isModelTensorFlow2 = true;
- tfodParameters.inputSize = 320;
- tfod = ClassFactory.getInstance().createTFObjectDetector(tfodParameters, vuforia);
- tfod.loadModelFromAsset(TFOD_MODEL_ASSET, LABELS);
- }
-
- private void doCameraSwitching() {
- // If the left bumper is pressed, use Webcam 1.
- // If the right bumper is pressed, use Webcam 2.
- boolean newLeftBumper = gamepad1.left_bumper;
- boolean newRightBumper = gamepad1.right_bumper;
- if (newLeftBumper && !oldLeftBumper) {
- switchableCamera.setActiveCamera(webcam1);
- } else if (newRightBumper && !oldRightBumper) {
- switchableCamera.setActiveCamera(webcam2);
- }
- oldLeftBumper = newLeftBumper;
- oldRightBumper = newRightBumper;
-
- if (switchableCamera.getActiveCamera().equals(webcam1)) {
- telemetry.addData("activeCamera", "Webcam 1");
- telemetry.addData("Press RightBumper", "to switch to Webcam 2");
- } else {
- telemetry.addData("activeCamera", "Webcam 2");
- telemetry.addData("Press LeftBumper", "to switch to Webcam 1");
- }
- }
-}
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptTensorFlowObjectDetectionWebcam.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptTensorFlowObjectDetectionWebcam.java
deleted file mode 100644
index 0820863..0000000
--- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptTensorFlowObjectDetectionWebcam.java
+++ /dev/null
@@ -1,185 +0,0 @@
-/* Copyright (c) 2019 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 java.util.List;
-import org.firstinspires.ftc.robotcore.external.ClassFactory;
-import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
-import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer;
-import org.firstinspires.ftc.robotcore.external.tfod.TFObjectDetector;
-import org.firstinspires.ftc.robotcore.external.tfod.Recognition;
-
-/**
- * This 2020-2021 OpMode illustrates the basics of using the TensorFlow Object Detection API to
- * determine the position of the Freight Frenzy game elements.
- *
- * 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.
- *
- * IMPORTANT: In order to use this OpMode, you need to obtain your own Vuforia license key as
- * is explained below.
- */
-@TeleOp(name = "Concept: TensorFlow Object Detection Webcam", group = "Concept")
-@Disabled
-public class ConceptTensorFlowObjectDetectionWebcam extends LinearOpMode {
- /* Note: This sample uses the all-objects Tensor Flow model (FreightFrenzy_BCDM.tflite), which contains
- * the following 4 detectable objects
- * 0: Ball,
- * 1: Cube,
- * 2: Duck,
- * 3: Marker (duck location tape marker)
- *
- * Two additional model assets are available which only contain a subset of the objects:
- * FreightFrenzy_BC.tflite 0: Ball, 1: Cube
- * FreightFrenzy_DM.tflite 0: Duck, 1: Marker
- */
- private static final String TFOD_MODEL_ASSET = "FreightFrenzy_BCDM.tflite";
- private static final String[] LABELS = {
- "Ball",
- "Cube",
- "Duck",
- "Marker"
- };
-
- /*
- * IMPORTANT: You need to obtain your own license key to use Vuforia. The string below with which
- * 'parameters.vuforiaLicenseKey' is initialized is for illustration only, and will not function.
- * A Vuforia 'Development' license key, can be obtained free of charge from the Vuforia developer
- * web site at https://developer.vuforia.com/license-manager.
- *
- * Vuforia license keys are always 380 characters long, and look as if they contain mostly
- * random data. As an example, here is a example of a fragment of a valid key:
- * ... yIgIzTqZ4mWjk9wd3cZO9T1axEqzuhxoGlfOOI2dRzKS4T0hQ8kT ...
- * Once you've obtained a license key, copy the string from the Vuforia web site
- * and paste it in to your code on the next line, between the double quotes.
- */
- private static final String VUFORIA_KEY =
- " -- YOUR NEW VUFORIA KEY GOES HERE --- ";
-
- /**
- * {@link #vuforia} is the variable we will use to store our instance of the Vuforia
- * localization engine.
- */
- private VuforiaLocalizer vuforia;
-
- /**
- * {@link #tfod} is the variable we will use to store our instance of the TensorFlow Object
- * Detection engine.
- */
- private TFObjectDetector tfod;
-
- @Override
- public void runOpMode() {
- // The TFObjectDetector uses the camera frames from the VuforiaLocalizer, so we create that
- // first.
- initVuforia();
- initTfod();
-
- /**
- * Activate TensorFlow Object Detection before we wait for the start command.
- * Do it here so that the Camera Stream window will have the TensorFlow annotations visible.
- **/
- if (tfod != null) {
- tfod.activate();
-
- // The TensorFlow software will scale the input images from the camera to a lower resolution.
- // This can result in lower detection accuracy at longer distances (> 55cm or 22").
- // If your target is at distance greater than 50 cm (20") you can adjust the magnification value
- // to artificially zoom in to the center of image. For best results, the "aspectRatio" argument
- // should be set to the value of the images used to create the TensorFlow Object Detection model
- // (typically 16/9).
- tfod.setZoom(2.5, 16.0/9.0);
- }
-
- /** Wait for the game to begin */
- telemetry.addData(">", "Press Play to start op mode");
- telemetry.update();
- waitForStart();
-
- if (opModeIsActive()) {
- while (opModeIsActive()) {
- if (tfod != null) {
- // getUpdatedRecognitions() will return null if no new information is available since
- // the last time that call was made.
- List updatedRecognitions = tfod.getUpdatedRecognitions();
- if (updatedRecognitions != null) {
- telemetry.addData("# Object Detected", updatedRecognitions.size());
- // step through the list of recognitions and display boundary info.
- int i = 0;
- for (Recognition recognition : updatedRecognitions) {
- telemetry.addData(String.format("label (%d)", i), recognition.getLabel());
- telemetry.addData(String.format(" left,top (%d)", i), "%.03f , %.03f",
- recognition.getLeft(), recognition.getTop());
- telemetry.addData(String.format(" right,bottom (%d)", i), "%.03f , %.03f",
- recognition.getRight(), recognition.getBottom());
- i++;
- }
- telemetry.update();
- }
- }
- }
- }
- }
-
- /**
- * Initialize the Vuforia localization engine.
- */
- private void initVuforia() {
- /*
- * Configure Vuforia by creating a Parameter object, and passing it to the Vuforia engine.
- */
- VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters();
-
- parameters.vuforiaLicenseKey = VUFORIA_KEY;
- parameters.cameraName = hardwareMap.get(WebcamName.class, "Webcam 1");
-
- // Instantiate the Vuforia engine
- vuforia = ClassFactory.getInstance().createVuforia(parameters);
-
- // Loading trackables is not necessary for the TensorFlow Object Detection engine.
- }
-
- /**
- * Initialize the TensorFlow Object Detection engine.
- */
- private void initTfod() {
- int tfodMonitorViewId = hardwareMap.appContext.getResources().getIdentifier(
- "tfodMonitorViewId", "id", hardwareMap.appContext.getPackageName());
- TFObjectDetector.Parameters tfodParameters = new TFObjectDetector.Parameters(tfodMonitorViewId);
- tfodParameters.minResultConfidence = 0.8f;
- tfodParameters.isModelTensorFlow2 = true;
- tfodParameters.inputSize = 320;
- tfod = ClassFactory.getInstance().createTFObjectDetector(tfodParameters, vuforia);
- tfod.loadModelFromAsset(TFOD_MODEL_ASSET, LABELS);
- }
-}
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVisionColorLocator.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVisionColorLocator.java
new file mode 100644
index 0000000..987694d
--- /dev/null
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVisionColorLocator.java
@@ -0,0 +1,191 @@
+/*
+ * Copyright (c) 2024 Phil Malone
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+package org.firstinspires.ftc.robotcontroller.external.samples;
+
+import android.util.Size;
+
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.util.SortOrder;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
+import org.firstinspires.ftc.vision.VisionPortal;
+import org.firstinspires.ftc.vision.opencv.ColorBlobLocatorProcessor;
+import org.firstinspires.ftc.vision.opencv.ColorRange;
+import org.firstinspires.ftc.vision.opencv.ImageRegion;
+import org.opencv.core.RotatedRect;
+
+import java.util.List;
+
+/*
+ * This OpMode illustrates how to use a video source (camera) to locate specifically colored regions
+ *
+ * Unlike a "color sensor" which determines the color of an object in the field of view, this "color locator"
+ * will search the Region Of Interest (ROI) in a camera image, and find any "blobs" of color that match the requested color range.
+ * These blobs can be further filtered and sorted to find the one most likely to be the item the user is looking for.
+ *
+ * To perform this function, a VisionPortal runs a ColorBlobLocatorProcessor process.
+ * The ColorBlobLocatorProcessor process is created first, and then the VisionPortal is built to use this process.
+ * The ColorBlobLocatorProcessor analyses the ROI and locates pixels that match the ColorRange to form a "mask".
+ * The matching pixels are then collected into contiguous "blobs" of pixels. The outer boundaries of these blobs are called its "contour".
+ * For each blob, the process then creates the smallest possible rectangle "boxFit" that will fully encase the contour.
+ * The user can then call getBlobs() to retrieve the list of Blobs, where each Blob contains the contour and the boxFit data.
+ * Note: The default sort order for Blobs is ContourArea, in descending order, so the biggest contours are listed first.
+ *
+ * To aid the user, a colored boxFit rectangle is drawn on the camera preview to show the location of each Blob
+ * The original Blob contour can also be added to the preview. This is helpful when configuring the ColorBlobLocatorProcessor parameters.
+ *
+ * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name.
+ * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list
+ */
+
+@Disabled
+@TeleOp(name = "Concept: Vision Color-Locator", group = "Concept")
+public class ConceptVisionColorLocator extends LinearOpMode
+{
+ @Override
+ public void runOpMode()
+ {
+ /* Build a "Color Locator" vision processor based on the ColorBlobLocatorProcessor class.
+ * - Specify the color range you are looking for. You can use a predefined color, or create you own color range
+ * .setTargetColorRange(ColorRange.BLUE) // use a predefined color match
+ * Available predefined colors are: RED, BLUE YELLOW GREEN
+ * .setTargetColorRange(new ColorRange(ColorSpace.YCrCb, // or define your own color match
+ * new Scalar( 32, 176, 0),
+ * new Scalar(255, 255, 132)))
+ *
+ * - Focus the color locator by defining a RegionOfInterest (ROI) which you want to search.
+ * This can be the entire frame, or a sub-region defined using:
+ * 1) standard image coordinates or 2) a normalized +/- 1.0 coordinate system.
+ * Use one form of the ImageRegion class to define the ROI.
+ * ImageRegion.entireFrame()
+ * ImageRegion.asImageCoordinates(50, 50, 150, 150) 100x100 pixel square near the upper left corner
+ * ImageRegion.asUnityCenterCoordinates(-0.5, 0.5, 0.5, -0.5) 50% width/height square centered on screen
+ *
+ * - Define which contours are included.
+ * You can get ALL the contours, or you can skip any contours that are completely inside another contour.
+ * .setContourMode(ColorBlobLocatorProcessor.ContourMode.ALL_FLATTENED_HIERARCHY) // return all contours
+ * .setContourMode(ColorBlobLocatorProcessor.ContourMode.EXTERNAL_ONLY) // exclude contours inside other contours
+ * note: EXTERNAL_ONLY helps to avoid bright reflection spots from breaking up areas of solid color.
+ *
+ * - turn the display of contours ON or OFF. Turning this on helps debugging but takes up valuable CPU time.
+ * .setDrawContours(true)
+ *
+ * - include any pre-processing of the image or mask before looking for Blobs.
+ * There are some extra processing you can include to improve the formation of blobs. Using these features requires
+ * an understanding of how they may effect the final blobs. The "pixels" argument sets the NxN kernel size.
+ * .setBlurSize(int pixels) Blurring an image helps to provide a smooth color transition between objects, and smoother contours.
+ * The higher the number of pixels, the more blurred the image becomes.
+ * Note: Even "pixels" values will be incremented to satisfy the "odd number" requirement.
+ * Blurring too much may hide smaller features. A "pixels" size of 5 is good for a 320x240 image.
+ * .setErodeSize(int pixels) Erosion removes floating pixels and thin lines so that only substantive objects remain.
+ * Erosion can grow holes inside regions, and also shrink objects.
+ * "pixels" in the range of 2-4 are suitable for low res images.
+ * .setDilateSize(int pixels) Dilation makes objects more visible by filling in small holes, making lines appear thicker,
+ * and making filled shapes appear larger. Dilation is useful for joining broken parts of an
+ * object, such as when removing noise from an image.
+ * "pixels" in the range of 2-4 are suitable for low res images.
+ */
+ ColorBlobLocatorProcessor colorLocator = new ColorBlobLocatorProcessor.Builder()
+ .setTargetColorRange(ColorRange.BLUE) // use a predefined color match
+ .setContourMode(ColorBlobLocatorProcessor.ContourMode.EXTERNAL_ONLY) // exclude blobs inside blobs
+ .setRoi(ImageRegion.asUnityCenterCoordinates(-0.5, 0.5, 0.5, -0.5)) // search central 1/4 of camera view
+ .setDrawContours(true) // Show contours on the Stream Preview
+ .setBlurSize(5) // Smooth the transitions between different colors in image
+ .build();
+
+ /*
+ * Build a vision portal to run the Color Locator process.
+ *
+ * - Add the colorLocator process created above.
+ * - Set the desired video resolution.
+ * Since a high resolution will not improve this process, choose a lower resolution that is
+ * supported by your camera. This will improve overall performance and reduce latency.
+ * - Choose your video source. This may be
+ * .setCamera(hardwareMap.get(WebcamName.class, "Webcam 1")) ..... for a webcam
+ * or
+ * .setCamera(BuiltinCameraDirection.BACK) ... for a Phone Camera
+ */
+ VisionPortal portal = new VisionPortal.Builder()
+ .addProcessor(colorLocator)
+ .setCameraResolution(new Size(320, 240))
+ .setCamera(hardwareMap.get(WebcamName.class, "Webcam 1"))
+ .build();
+
+ telemetry.setMsTransmissionInterval(50); // Speed up telemetry updates, Just use for debugging.
+ telemetry.setDisplayFormat(Telemetry.DisplayFormat.MONOSPACE);
+
+ // WARNING: To be able to view the stream preview on the Driver Station, this code runs in INIT mode.
+ while (opModeIsActive() || opModeInInit())
+ {
+ telemetry.addData("preview on/off", "... Camera Stream\n");
+
+ // Read the current list
+ List blobs = colorLocator.getBlobs();
+
+ /*
+ * The list of Blobs can be filtered to remove unwanted Blobs.
+ * Note: All contours will be still displayed on the Stream Preview, but only those that satisfy the filter
+ * conditions will remain in the current list of "blobs". Multiple filters may be used.
+ *
+ * Use any of the following filters.
+ *
+ * ColorBlobLocatorProcessor.Util.filterByArea(minArea, maxArea, blobs);
+ * A Blob's area is the number of pixels contained within the Contour. Filter out any that are too big or small.
+ * Start with a large range and then refine the range based on the likely size of the desired object in the viewfinder.
+ *
+ * ColorBlobLocatorProcessor.Util.filterByDensity(minDensity, maxDensity, blobs);
+ * A blob's density is an indication of how "full" the contour is.
+ * If you put a rubber band around the contour you would get the "Convex Hull" of the contour.
+ * The density is the ratio of Contour-area to Convex Hull-area.
+ *
+ * ColorBlobLocatorProcessor.Util.filterByAspectRatio(minAspect, maxAspect, blobs);
+ * A blob's Aspect ratio is the ratio of boxFit long side to short side.
+ * A perfect Square has an aspect ratio of 1. All others are > 1
+ */
+ ColorBlobLocatorProcessor.Util.filterByArea(50, 20000, blobs); // filter out very small blobs.
+
+ /*
+ * The list of Blobs can be sorted using the same Blob attributes as listed above.
+ * No more than one sort call should be made. Sorting can use ascending or descending order.
+ * ColorBlobLocatorProcessor.Util.sortByArea(SortOrder.DESCENDING, blobs); // Default
+ * ColorBlobLocatorProcessor.Util.sortByDensity(SortOrder.DESCENDING, blobs);
+ * ColorBlobLocatorProcessor.Util.sortByAspectRatio(SortOrder.DESCENDING, blobs);
+ */
+
+ telemetry.addLine(" Area Density Aspect Center");
+
+ // Display the size (area) and center location for each Blob.
+ for(ColorBlobLocatorProcessor.Blob b : blobs)
+ {
+ RotatedRect boxFit = b.getBoxFit();
+ telemetry.addLine(String.format("%5d %4.2f %5.2f (%3d,%3d)",
+ b.getContourArea(), b.getDensity(), b.getAspectRatio(), (int) boxFit.center.x, (int) boxFit.center.y));
+ }
+
+ telemetry.update();
+ sleep(50);
+ }
+ }
+}
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVisionColorSensor.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVisionColorSensor.java
new file mode 100644
index 0000000..6be2bc4
--- /dev/null
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVisionColorSensor.java
@@ -0,0 +1,136 @@
+/*
+ * Copyright (c) 2024 Phil Malone
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+package org.firstinspires.ftc.robotcontroller.external.samples;
+
+import android.graphics.Color;
+import android.util.Size;
+
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
+import org.firstinspires.ftc.vision.VisionPortal;
+import org.firstinspires.ftc.vision.opencv.ImageRegion;
+import org.firstinspires.ftc.vision.opencv.PredominantColorProcessor;
+
+/*
+ * This OpMode illustrates how to use a video source (camera) as a color sensor
+ *
+ * A "color sensor" will typically determine the color of the object that it is pointed at.
+ *
+ * This sample performs the same function, except it uses a video camera to inspect an object or scene.
+ * The user may choose to inspect all, or just a Region of Interest (ROI), of the active camera view.
+ * The user must also provide a list of "acceptable colors" (Swatches) from which the closest matching color will be selected.
+ *
+ * To perform this function, a VisionPortal runs a PredominantColorProcessor process.
+ * The PredominantColorProcessor process is created first, and then the VisionPortal is built to use this process.
+ * The PredominantColorProcessor analyses the ROI and splits the colored pixels into several color-clusters.
+ * The largest of these clusters is then considered to be the "Predominant Color"
+ * The process then matches the Predominant Color with the closest Swatch and returns that match.
+ *
+ * To aid the user, a colored rectangle is drawn on the camera preview to show the RegionOfInterest,
+ * The Predominant Color is used to paint the rectangle border, so the user can verify that the color is reasonable.
+ *
+ * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name.
+ * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list
+ */
+
+@Disabled
+@TeleOp(name = "Concept: Vision Color-Sensor", group = "Concept")
+public class ConceptVisionColorSensor extends LinearOpMode
+{
+ @Override
+ public void runOpMode()
+ {
+ /* Build a "Color Sensor" vision processor based on the PredominantColorProcessor class.
+ *
+ * - Focus the color sensor by defining a RegionOfInterest (ROI) which you want to inspect.
+ * This can be the entire frame, or a sub-region defined using:
+ * 1) standard image coordinates or 2) a normalized +/- 1.0 coordinate system.
+ * Use one form of the ImageRegion class to define the ROI.
+ * ImageRegion.entireFrame()
+ * ImageRegion.asImageCoordinates(50, 50, 150, 150) 100x100 pixel square near the upper left corner
+ * ImageRegion.asUnityCenterCoordinates(-0.1, 0.1, 0.1, -0.1) 10% width/height square centered on screen
+ *
+ * - Set the list of "acceptable" color swatches (matches).
+ * Only colors that you assign here will be returned.
+ * If you know the sensor will be pointing to one of a few specific colors, enter them here.
+ * Or, if the sensor may be pointed randomly, provide some additional colors that may match the surrounding.
+ * Possible choices are:
+ * RED, ORANGE, YELLOW, GREEN, CYAN, BLUE, PURPLE, MAGENTA, BLACK, WHITE;
+ *
+ * Note that in the example shown below, only some of the available colors are included.
+ * This will force any other colored region into one of these colors.
+ * eg: Green may be reported as YELLOW, as this may be the "closest" match.
+ */
+ PredominantColorProcessor colorSensor = new PredominantColorProcessor.Builder()
+ .setRoi(ImageRegion.asUnityCenterCoordinates(-0.1, 0.1, 0.1, -0.1))
+ .setSwatches(
+ PredominantColorProcessor.Swatch.RED,
+ PredominantColorProcessor.Swatch.BLUE,
+ PredominantColorProcessor.Swatch.YELLOW,
+ PredominantColorProcessor.Swatch.BLACK,
+ PredominantColorProcessor.Swatch.WHITE)
+ .build();
+
+ /*
+ * Build a vision portal to run the Color Sensor process.
+ *
+ * - Add the colorSensor process created above.
+ * - Set the desired video resolution.
+ * Since a high resolution will not improve this process, choose a lower resolution that is
+ * supported by your camera. This will improve overall performance and reduce latency.
+ * - Choose your video source. This may be
+ * .setCamera(hardwareMap.get(WebcamName.class, "Webcam 1")) ..... for a webcam
+ * or
+ * .setCamera(BuiltinCameraDirection.BACK) ... for a Phone Camera
+ */
+ VisionPortal portal = new VisionPortal.Builder()
+ .addProcessor(colorSensor)
+ .setCameraResolution(new Size(320, 240))
+ .setCamera(hardwareMap.get(WebcamName.class, "Webcam 1"))
+ .build();
+
+ telemetry.setMsTransmissionInterval(50); // Speed up telemetry updates, Just use for debugging.
+
+ // WARNING: To be able to view the stream preview on the Driver Station, this code runs in INIT mode.
+ while (opModeIsActive() || opModeInInit())
+ {
+ telemetry.addData("DS preview on/off", "3 dots, Camera Stream\n");
+
+ // Request the most recent color analysis.
+ // This will return the closest matching colorSwatch and the predominant RGB color.
+ // Note: to take actions based on the detected color, simply use the colorSwatch in a comparison or switch.
+ // eg:
+ // if (result.closestSwatch == PredominantColorProcessor.Swatch.RED) {... some code ...}
+ PredominantColorProcessor.Result result = colorSensor.getAnalysis();
+
+ // Display the Color Sensor result.
+ telemetry.addData("Best Match:", result.closestSwatch);
+ telemetry.addLine(String.format("R %3d, G %3d, B %3d", Color.red(result.rgb), Color.green(result.rgb), Color.blue(result.rgb)));
+ telemetry.update();
+
+ sleep(20);
+ }
+ }
+}
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVuMarkIdentification.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVuMarkIdentification.java
deleted file mode 100644
index f0d735b..0000000
--- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVuMarkIdentification.java
+++ /dev/null
@@ -1,186 +0,0 @@
-/* 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.TeleOp;
-import com.qualcomm.robotcore.eventloop.opmode.Disabled;
-import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
-
-import org.firstinspires.ftc.robotcore.external.ClassFactory;
-import org.firstinspires.ftc.robotcore.external.matrices.OpenGLMatrix;
-import org.firstinspires.ftc.robotcore.external.matrices.VectorF;
-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.RelicRecoveryVuMark;
-import org.firstinspires.ftc.robotcore.external.navigation.VuMarkInstanceId;
-import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer;
-import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackable;
-import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackableDefaultListener;
-import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackables;
-
-/**
- * This OpMode illustrates the basics of using the Vuforia engine to determine
- * the identity of Vuforia VuMarks encountered on the field. The code is structured as
- * a LinearOpMode. It shares much structure with {@link ConceptVuforiaNavigation}; we do not here
- * duplicate the core Vuforia documentation found there, but rather instead focus on the
- * differences between the use of Vuforia for navigation vs VuMark identification.
- *
- * @see ConceptVuforiaNavigation
- * @see VuforiaLocalizer
- * @see VuforiaTrackableDefaultListener
- * see ftc_app/doc/tutorial/FTC_FieldCoordinateSystemDefinition.pdf
- *
- * 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.
- *
- * IMPORTANT: In order to use this OpMode, you need to obtain your own Vuforia license key as
- * is explained in {@link ConceptVuforiaNavigation}.
- */
-
-@TeleOp(name="Concept: VuMark Id", group ="Concept")
-@Disabled
-public class ConceptVuMarkIdentification extends LinearOpMode {
-
- public static final String TAG = "Vuforia VuMark Sample";
-
- OpenGLMatrix lastLocation = null;
-
- /**
- * {@link #vuforia} is the variable we will use to store our instance of the Vuforia
- * localization engine.
- */
- VuforiaLocalizer vuforia;
-
- @Override public void runOpMode() {
-
- /*
- * To start up Vuforia, tell it the view that we wish to use for camera monitor (on the RC phone);
- */
- int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName());
- VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters(cameraMonitorViewId);
-
- // OR... Do Not Activate the Camera Monitor View, to save power
- // VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters();
-
- /*
- * IMPORTANT: You need to obtain your own license key to use Vuforia. The string below with which
- * 'parameters.vuforiaLicenseKey' is initialized is for illustration only, and will not function.
- * A Vuforia 'Development' license key, can be obtained free of charge from the Vuforia developer
- * web site at https://developer.vuforia.com/license-manager.
- *
- * Vuforia license keys are always 380 characters long, and look as if they contain mostly
- * random data. As an example, here is a example of a fragment of a valid key:
- * ... yIgIzTqZ4mWjk9wd3cZO9T1axEqzuhxoGlfOOI2dRzKS4T0hQ8kT ...
- * Once you've obtained a license key, copy the string from the Vuforia web site
- * and paste it in to your code on the next line, between the double quotes.
- */
- parameters.vuforiaLicenseKey = " -- YOUR NEW VUFORIA KEY GOES HERE --- ";
-
- /*
- * We also indicate which camera on the RC that we wish to use.
- * Here we chose the back (HiRes) camera (for greater range), but
- * for a competition robot, the front camera might be more convenient.
- */
- parameters.cameraDirection = VuforiaLocalizer.CameraDirection.BACK;
-
- /**
- * Instantiate the Vuforia engine
- */
- vuforia = ClassFactory.getInstance().createVuforia(parameters);
-
-
- /**
- * Load the data set containing the VuMarks for Relic Recovery. There's only one trackable
- * in this data set: all three of the VuMarks in the game were created from this one template,
- * but differ in their instance id information.
- * @see VuMarkInstanceId
- */
- VuforiaTrackables relicTrackables = this.vuforia.loadTrackablesFromAsset("RelicVuMark");
- VuforiaTrackable relicTemplate = relicTrackables.get(0);
- relicTemplate.setName("relicVuMarkTemplate"); // can help in debugging; otherwise not necessary
-
- telemetry.addData(">", "Press Play to start");
- telemetry.update();
- waitForStart();
-
- relicTrackables.activate();
-
- while (opModeIsActive()) {
-
- /**
- * See if any of the instances of {@link relicTemplate} are currently visible.
- * {@link RelicRecoveryVuMark} is an enum which can have the following values:
- * UNKNOWN, LEFT, CENTER, and RIGHT. When a VuMark is visible, something other than
- * UNKNOWN will be returned by {@link RelicRecoveryVuMark#from(VuforiaTrackable)}.
- */
- RelicRecoveryVuMark vuMark = RelicRecoveryVuMark.from(relicTemplate);
- if (vuMark != RelicRecoveryVuMark.UNKNOWN) {
-
- /* Found an instance of the template. In the actual game, you will probably
- * loop until this condition occurs, then move on to act accordingly depending
- * on which VuMark was visible. */
- telemetry.addData("VuMark", "%s visible", vuMark);
-
- /* For fun, we also exhibit the navigational pose. In the Relic Recovery game,
- * it is perhaps unlikely that you will actually need to act on this pose information, but
- * we illustrate it nevertheless, for completeness. */
- OpenGLMatrix pose = ((VuforiaTrackableDefaultListener)relicTemplate.getListener()).getPose();
- telemetry.addData("Pose", format(pose));
-
- /* We further illustrate how to decompose the pose into useful rotational and
- * translational components */
- if (pose != null) {
- VectorF trans = pose.getTranslation();
- Orientation rot = Orientation.getOrientation(pose, AxesReference.EXTRINSIC, AxesOrder.XYZ, AngleUnit.DEGREES);
-
- // Extract the X, Y, and Z components of the offset of the target relative to the robot
- double tX = trans.get(0);
- double tY = trans.get(1);
- double tZ = trans.get(2);
-
- // Extract the rotational components of the target relative to the robot
- double rX = rot.firstAngle;
- double rY = rot.secondAngle;
- double rZ = rot.thirdAngle;
- }
- }
- else {
- telemetry.addData("VuMark", "not visible");
- }
-
- telemetry.update();
- }
- }
-
- String format(OpenGLMatrix transformationMatrix) {
- return (transformationMatrix != null) ? transformationMatrix.formatAsTransform() : "null";
- }
-}
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVuMarkIdentificationWebcam.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVuMarkIdentificationWebcam.java
deleted file mode 100644
index 866261f..0000000
--- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVuMarkIdentificationWebcam.java
+++ /dev/null
@@ -1,194 +0,0 @@
-/* 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.TeleOp;
-import com.qualcomm.robotcore.eventloop.opmode.Disabled;
-import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
-
-import org.firstinspires.ftc.robotcore.external.ClassFactory;
-import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
-import org.firstinspires.ftc.robotcore.external.matrices.OpenGLMatrix;
-import org.firstinspires.ftc.robotcore.external.matrices.VectorF;
-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.RelicRecoveryVuMark;
-import org.firstinspires.ftc.robotcore.external.navigation.VuMarkInstanceId;
-import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer;
-import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackable;
-import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackableDefaultListener;
-import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackables;
-
-/**
- * This OpMode illustrates the basics of using the Vuforia engine to determine
- * the identity of Vuforia VuMarks encountered on the field. The code is structured as
- * a LinearOpMode. It shares much structure with {@link ConceptVuforiaNavigationWebcam}; we do not here
- * duplicate the core Vuforia documentation found there, but rather instead focus on the
- * differences between the use of Vuforia for navigation vs VuMark identification.
- *
- * @see ConceptVuforiaNavigationWebcam
- * @see VuforiaLocalizer
- * @see VuforiaTrackableDefaultListener
- * see ftc_app/doc/tutorial/FTC_FieldCoordinateSystemDefinition.pdf
- *
- * 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.
- *
- * IMPORTANT: In order to use this OpMode, you need to obtain your own Vuforia license key as
- * is explained in {@link ConceptVuforiaNavigationWebcam}.
- */
-
-@TeleOp(name="Concept: VuMark Id Webcam", group ="Concept")
-@Disabled
-public class ConceptVuMarkIdentificationWebcam extends LinearOpMode {
-
- public static final String TAG = "Vuforia VuMark Sample";
-
- OpenGLMatrix lastLocation = null;
-
- /**
- * {@link #vuforia} is the variable we will use to store our instance of the Vuforia
- * localization engine.
- */
- VuforiaLocalizer vuforia;
-
- /**
- * This is the webcam we are to use. As with other hardware devices such as motors and
- * servos, this device is identified using the robot configuration tool in the FTC application.
- */
- WebcamName webcamName;
-
- @Override public void runOpMode() {
-
- /*
- * Retrieve the camera we are to use.
- */
- webcamName = hardwareMap.get(WebcamName.class, "Webcam 1");
-
- /*
- * To start up Vuforia, tell it the view that we wish to use for camera monitor (on the RC phone);
- * If no camera monitor is desired, use the parameterless constructor instead (commented out below).
- */
- int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName());
- VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters(cameraMonitorViewId);
-
- // OR... Do Not Activate the Camera Monitor View, to save power
- // VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters();
-
- /*
- * IMPORTANT: You need to obtain your own license key to use Vuforia. The string below with which
- * 'parameters.vuforiaLicenseKey' is initialized is for illustration only, and will not function.
- * A Vuforia 'Development' license key, can be obtained free of charge from the Vuforia developer
- * web site at https://developer.vuforia.com/license-manager.
- *
- * Vuforia license keys are always 380 characters long, and look as if they contain mostly
- * random data. As an example, here is a example of a fragment of a valid key:
- * ... yIgIzTqZ4mWjk9wd3cZO9T1axEqzuhxoGlfOOI2dRzKS4T0hQ8kT ...
- * Once you've obtained a license key, copy the string from the Vuforia web site
- * and paste it in to your code on the next line, between the double quotes.
- */
- parameters.vuforiaLicenseKey = " -- YOUR NEW VUFORIA KEY GOES HERE --- ";
-
-
- /**
- * We also indicate which camera on the RC we wish to use. For pedagogical purposes,
- * we use the same logic as in {@link ConceptVuforiaNavigationWebcam}.
- */
- parameters.cameraName = webcamName;
- this.vuforia = ClassFactory.getInstance().createVuforia(parameters);
-
- /**
- * Load the data set containing the VuMarks for Relic Recovery. There's only one trackable
- * in this data set: all three of the VuMarks in the game were created from this one template,
- * but differ in their instance id information.
- * @see VuMarkInstanceId
- */
- VuforiaTrackables relicTrackables = this.vuforia.loadTrackablesFromAsset("RelicVuMark");
- VuforiaTrackable relicTemplate = relicTrackables.get(0);
- relicTemplate.setName("relicVuMarkTemplate"); // can help in debugging; otherwise not necessary
-
- telemetry.addData(">", "Press Play to start");
- telemetry.update();
- waitForStart();
-
- relicTrackables.activate();
-
- while (opModeIsActive()) {
-
- /**
- * See if any of the instances of {@link relicTemplate} are currently visible.
- * {@link RelicRecoveryVuMark} is an enum which can have the following values:
- * UNKNOWN, LEFT, CENTER, and RIGHT. When a VuMark is visible, something other than
- * UNKNOWN will be returned by {@link RelicRecoveryVuMark#from(VuforiaTrackable)}.
- */
- RelicRecoveryVuMark vuMark = RelicRecoveryVuMark.from(relicTemplate);
- if (vuMark != RelicRecoveryVuMark.UNKNOWN) {
-
- /* Found an instance of the template. In the actual game, you will probably
- * loop until this condition occurs, then move on to act accordingly depending
- * on which VuMark was visible. */
- telemetry.addData("VuMark", "%s visible", vuMark);
-
- /* For fun, we also exhibit the navigational pose. In the Relic Recovery game,
- * it is perhaps unlikely that you will actually need to act on this pose information, but
- * we illustrate it nevertheless, for completeness. */
- OpenGLMatrix pose = ((VuforiaTrackableDefaultListener)relicTemplate.getListener()).getFtcCameraFromTarget();
- telemetry.addData("Pose", format(pose));
-
- /* We further illustrate how to decompose the pose into useful rotational and
- * translational components */
- if (pose != null) {
- VectorF trans = pose.getTranslation();
- Orientation rot = Orientation.getOrientation(pose, AxesReference.EXTRINSIC, AxesOrder.XYZ, AngleUnit.DEGREES);
-
- // Extract the X, Y, and Z components of the offset of the target relative to the robot
- double tX = trans.get(0);
- double tY = trans.get(1);
- double tZ = trans.get(2);
-
- // Extract the rotational components of the target relative to the robot
- double rX = rot.firstAngle;
- double rY = rot.secondAngle;
- double rZ = rot.thirdAngle;
- }
- }
- else {
- telemetry.addData("VuMark", "not visible");
- }
-
- telemetry.update();
- }
- }
-
- String format(OpenGLMatrix transformationMatrix) {
- return (transformationMatrix != null) ? transformationMatrix.formatAsTransform() : "null";
- }
-}
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVuforiaDriveToTargetWebcam.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVuforiaDriveToTargetWebcam.java
deleted file mode 100644
index 0175964..0000000
--- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVuforiaDriveToTargetWebcam.java
+++ /dev/null
@@ -1,202 +0,0 @@
-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.ClassFactory;
-import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
-import org.firstinspires.ftc.robotcore.external.matrices.OpenGLMatrix;
-import org.firstinspires.ftc.robotcore.external.matrices.VectorF;
-import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer;
-import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackable;
-import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackableDefaultListener;
-import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackables;
-
-/**
- * This OpMode illustrates using a webcam to locate and drive towards ANY Vuforia target.
- * The code assumes a basic two-wheel Robot Configuration with motors named left_drive and right_drive.
- * The motor directions must be set so a positive drive goes forward and a positive turn rotates to the right.
- *
- * Under manual control, the left stick will move forward/back, and the right stick will turn left/right.
- * 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.
- *
- * 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.
- *
- * For more Vuforia details, or to adapt this OpMode for a phone camera, view the
- * ConceptVuforiaFieldNavigation and ConceptVuforiaFieldNavigationWebcam samples.
- *
- * 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.
- *
- * IMPORTANT: In order to use this OpMode, you need to obtain your own Vuforia license key as
- * is explained below.
- */
-
-@TeleOp(name="Drive To Target", group = "Concept")
-@Disabled
-public class ConceptVuforiaDriveToTargetWebcam extends LinearOpMode
-{
- // Adjust these numbers to suit your robot.
- final double DESIRED_DISTANCE = 8.0; // this is how close the camera should get to the target (inches)
- // The GAIN constants set the relationship between the measured position error,
- // and how much power is applied to the drive motors. Drive = Error * Gain
- // Make these values smaller for smoother control.
- 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 MM_PER_INCH = 25.40 ; // Metric conversion
-
- /*
- * IMPORTANT: You need to obtain your own license key to use Vuforia. The string below with which
- * 'parameters.vuforiaLicenseKey' is initialized is for illustration only, and will not function.
- * A Vuforia 'Development' license key, can be obtained free of charge from the Vuforia developer
- * web site at https://developer.vuforia.com/license-manager.
- *
- * Vuforia license keys are always 380 characters long, and look as if they contain mostly
- * random data. As an example, here is a example of a fragment of a valid key:
- * ... yIgIzTqZ4mWjk9wd3cZO9T1axEqzuhxoGlfOOI2dRzKS4T0hQ8kT ...
- * Once you've obtained a license key, copy the string from the Vuforia web site
- * and paste it in to your code on the next line, between the double quotes.
- */
- private static final String VUFORIA_KEY =
- " --- YOUR NEW VUFORIA KEY GOES HERE --- ";
-
- VuforiaLocalizer vuforia = null;
- OpenGLMatrix targetPose = null;
- String targetName = "";
-
- private DcMotor leftDrive = null;
- private DcMotor rightDrive = null;
-
- @Override public void runOpMode()
- {
- /*
- * Configure Vuforia by creating a Parameter object, and passing it to the Vuforia engine.
- * To get an on-phone camera preview, use the code below.
- * If no camera preview is desired, use the parameter-less constructor instead (commented out below).
- */
- int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName());
- VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters(cameraMonitorViewId);
- // VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters();
-
- parameters.vuforiaLicenseKey = VUFORIA_KEY;
-
- // Turn off Extended tracking. Set this true if you want Vuforia to track beyond the target.
- parameters.useExtendedTracking = false;
-
- // Connect to the camera we are to use. This name must match what is set up in Robot Configuration
- parameters.cameraName = hardwareMap.get(WebcamName.class, "Webcam 1");
- this.vuforia = ClassFactory.getInstance().createVuforia(parameters);
-
- // Load the trackable objects from the Assets file, and give them meaningful names
- VuforiaTrackables targetsFreightFrenzy = this.vuforia.loadTrackablesFromAsset("FreightFrenzy");
- targetsFreightFrenzy.get(0).setName("Blue Storage");
- targetsFreightFrenzy.get(1).setName("Blue Alliance Wall");
- targetsFreightFrenzy.get(2).setName("Red Storage");
- targetsFreightFrenzy.get(3).setName("Red Alliance Wall");
-
- // Start tracking targets in the background
- targetsFreightFrenzy.activate();
-
- // 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(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.
- // Pushing the left stick forward MUST make robot go forward. So adjust these two lines based on your first test drive.
- leftDrive.setDirection(DcMotor.Direction.FORWARD);
- rightDrive.setDirection(DcMotor.Direction.REVERSE);
-
- telemetry.addData(">", "Press Play to start");
- telemetry.update();
-
- waitForStart();
-
- boolean targetFound = false; // Set to true when a target is detected by Vuforia
- double targetRange = 0; // Distance from camera to target in Inches
- double targetBearing = 0; // Robot Heading, relative to target. Positive degrees means target is to the right.
- double drive = 0; // Desired forward power (-1 to +1)
- double turn = 0; // Desired turning power (-1 to +1)
-
- while (opModeIsActive())
- {
- // Look for first visible target, and save its pose.
- targetFound = false;
- for (VuforiaTrackable trackable : targetsFreightFrenzy)
- {
- if (((VuforiaTrackableDefaultListener) trackable.getListener()).isVisible())
- {
- targetPose = ((VuforiaTrackableDefaultListener)trackable.getListener()).getVuforiaCameraFromTarget();
-
- // if we have a target, process the "pose" to determine the position of the target relative to the robot.
- if (targetPose != null)
- {
- targetFound = true;
- targetName = trackable.getName();
- VectorF trans = targetPose.getTranslation();
-
- // Extract the X & Y components of the offset of the target relative to the robot
- double targetX = trans.get(0) / MM_PER_INCH; // Image X axis
- double targetY = trans.get(2) / MM_PER_INCH; // Image Z axis
-
- // target range is based on distance from robot position to origin (right triangle).
- targetRange = Math.hypot(targetX, targetY);
-
- // target bearing is based on angle formed between the X axis and the target range line
- targetBearing = Math.toDegrees(Math.asin(targetX / targetRange));
-
- break; // jump out of target tracking loop if we find a target.
- }
- }
- }
-
- // 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", " %s", targetName);
- telemetry.addData("Range", "%5.1f inches", targetRange);
- telemetry.addData("Bearing","%3.0f degrees", targetBearing);
- } else {
- telemetry.addData(">","Drive using joystick to find target\n");
- }
-
- // Drive to target Automatically if Left Bumper is being pressed, AND we have found a target.
- if (gamepad1.left_bumper && targetFound) {
-
- // Determine heading and range error so we can use them to control the robot automatically.
- double rangeError = (targetRange - DESIRED_DISTANCE);
- double headingError = targetBearing;
-
- // Use the speed and turn "gains" to calculate how we want the robot to move.
- drive = rangeError * SPEED_GAIN;
- turn = headingError * TURN_GAIN ;
-
- 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();
-
- // Calculate left and right wheel powers and send to them to the motors.
- double leftPower = Range.clip(drive + turn, -1.0, 1.0) ;
- double rightPower = Range.clip(drive - turn, -1.0, 1.0) ;
- leftDrive.setPower(leftPower);
- rightDrive.setPower(rightPower);
-
- sleep(10);
- }
- }
-}
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVuforiaFieldNavigation.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVuforiaFieldNavigation.java
deleted file mode 100644
index d9d13dc..0000000
--- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVuforiaFieldNavigation.java
+++ /dev/null
@@ -1,284 +0,0 @@
-/* Copyright (c) 2019 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 org.firstinspires.ftc.robotcore.external.ClassFactory;
-import org.firstinspires.ftc.robotcore.external.matrices.OpenGLMatrix;
-import org.firstinspires.ftc.robotcore.external.matrices.VectorF;
-import org.firstinspires.ftc.robotcore.external.navigation.Orientation;
-import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer;
-import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackable;
-import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackableDefaultListener;
-import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackables;
-
-import java.util.ArrayList;
-import java.util.List;
-
-import static org.firstinspires.ftc.robotcore.external.navigation.AngleUnit.DEGREES;
-import static org.firstinspires.ftc.robotcore.external.navigation.AxesOrder.XYZ;
-import static org.firstinspires.ftc.robotcore.external.navigation.AxesOrder.YZX;
-import static org.firstinspires.ftc.robotcore.external.navigation.AxesReference.EXTRINSIC;
-import static org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer.CameraDirection.BACK;
-
-/**
- * This OpMode illustrates using the Vuforia localizer to determine positioning and orientation of
- * robot on the FTC field using the RC phone's camera. The code is structured as a LinearOpMode
- *
- * Note: If you are using a WEBCAM see ConceptVuforiaFieldNavigationWebcam.java
- *
- * When images are located, Vuforia is able to determine the position and orientation of the
- * image relative to the camera. This sample code then combines that information with a
- * knowledge of where the target images are on the field, to determine the location of the camera.
- *
- * Finally, the location of the camera on the robot is used to determine the
- * robot's location and orientation on the field.
- *
- * To learn more about the FTC field coordinate model, see FTC_FieldCoordinateSystemDefinition.pdf in this folder
- *
- * 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.
- *
- * IMPORTANT: In order to use this OpMode, you need to obtain your own Vuforia license key as
- * is explained below.
- */
-
-@TeleOp(name="Vuforia Field Nav", group ="Concept")
-@Disabled
-public class ConceptVuforiaFieldNavigation extends LinearOpMode {
-
- // IMPORTANT: For Phone Camera, set 1) the camera source and 2) the orientation, based on how your phone is mounted:
- // 1) Camera Source. Valid choices are: BACK (behind screen) or FRONT (selfie side)
- // 2) Phone Orientation. Choices are: PHONE_IS_PORTRAIT = true (portrait) or PHONE_IS_PORTRAIT = false (landscape)
-
- private static final VuforiaLocalizer.CameraDirection CAMERA_CHOICE = BACK;
- private static final boolean PHONE_IS_PORTRAIT = false ;
-
- /*
- * IMPORTANT: You need to obtain your own license key to use Vuforia. The string below with which
- * 'parameters.vuforiaLicenseKey' is initialized is for illustration only, and will not function.
- * A Vuforia 'Development' license key, can be obtained free of charge from the Vuforia developer
- * web site at https://developer.vuforia.com/license-manager.
- *
- * Vuforia license keys are always 380 characters long, and look as if they contain mostly
- * random data. As an example, here is a example of a fragment of a valid key:
- * ... yIgIzTqZ4mWjk9wd3cZO9T1axEqzuhxoGlfOOI2dRzKS4T0hQ8kT ...
- * Once you've obtained a license key, copy the string from the Vuforia web site
- * and paste it in to your code on the next line, between the double quotes.
- */
- private static final String VUFORIA_KEY =
- " -- YOUR NEW VUFORIA KEY GOES HERE --- ";
-
- // Since ImageTarget trackables use mm to specifiy their dimensions, we must use mm for all the physical dimension.
- // We will define some constants and conversions here. These are useful for the Freight Frenzy field.
- private static final float mmPerInch = 25.4f;
- private static final float mmTargetHeight = 6 * mmPerInch; // the height of the center of the target image above the floor
- private static final float halfField = 72 * mmPerInch;
- private static final float halfTile = 12 * mmPerInch;
- private static final float oneAndHalfTile = 36 * mmPerInch;
-
- // Class Members
- private OpenGLMatrix lastLocation = null;
- private VuforiaLocalizer vuforia = null;
- private VuforiaTrackables targets = null ;
-
- private boolean targetVisible = false;
- private float phoneXRotate = 0;
- private float phoneYRotate = 0;
- private float phoneZRotate = 0;
-
- @Override public void runOpMode() {
- /*
- * Configure Vuforia by creating a Parameter object, and passing it to the Vuforia engine.
- * To get an on-phone camera preview, use the code below.
- * If no camera preview is desired, use the parameter-less constructor instead (commented out below).
- */
- int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName());
- VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters(cameraMonitorViewId);
- // VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters();
-
- parameters.vuforiaLicenseKey = VUFORIA_KEY;
- parameters.cameraDirection = CAMERA_CHOICE;
-
- // Turn off Extended tracking. Set this true if you want Vuforia to track beyond the target.
- parameters.useExtendedTracking = false;
-
- // Instantiate the Vuforia engine
- vuforia = ClassFactory.getInstance().createVuforia(parameters);
-
- // Load the data sets for the trackable objects. These particular data
- // sets are stored in the 'assets' part of our application.
- targets = this.vuforia.loadTrackablesFromAsset("FreightFrenzy");
-
- // For convenience, gather together all the trackable objects in one easily-iterable collection */
- List allTrackables = new ArrayList();
- allTrackables.addAll(targets);
-
- /**
- * In order for localization to work, we need to tell the system where each target is on the field, and
- * where the phone resides on the robot. These specifications are in the form of transformation matrices.
- * Transformation matrices are a central, important concept in the math here involved in localization.
- * See Transformation Matrix
- * for detailed information. Commonly, you'll encounter transformation matrices as instances
- * of the {@link OpenGLMatrix} class.
- *
- * If you are standing in the Red Alliance Station looking towards the center of the field,
- * - The X axis runs from your left to the right. (positive from the center to the right)
- * - The Y axis runs from the Red Alliance Station towards the other side of the field
- * where the Blue Alliance Station is. (Positive is from the center, towards the BlueAlliance station)
- * - The Z axis runs from the floor, upwards towards the ceiling. (Positive is above the floor)
- *
- * Before being transformed, each target image is conceptually located at the origin of the field's
- * coordinate system (the center of the field), facing up.
- */
-
- // Name and locate each trackable object
- identifyTarget(0, "Blue Storage", -halfField, oneAndHalfTile, mmTargetHeight, 90, 0, 90);
- identifyTarget(1, "Blue Alliance Wall", halfTile, halfField, mmTargetHeight, 90, 0, 0);
- identifyTarget(2, "Red Storage", -halfField, -oneAndHalfTile, mmTargetHeight, 90, 0, 90);
- identifyTarget(3, "Red Alliance Wall", halfTile, -halfField, mmTargetHeight, 90, 0, 180);
-
- /*
- * Create a transformation matrix describing where the phone is on the robot.
- *
- * NOTE !!!! It's very important that you turn OFF your phone's Auto-Screen-Rotation option.
- * Lock it into Portrait for these numbers to work.
- *
- * Info: The coordinate frame for the robot looks the same as the field.
- * The robot's "forward" direction is facing out along X axis, with the LEFT side facing out along the Y axis.
- * Z is UP on the robot. This equates to a heading angle of Zero degrees.
- *
- * The phone starts out lying flat, with the screen facing Up and with the physical top of the phone
- * pointing to the LEFT side of the Robot.
- * The two examples below assume that the camera is facing forward out the front of the robot.
- */
-
- // We need to rotate the camera around its long axis to bring the correct camera forward.
- if (CAMERA_CHOICE == BACK) {
- phoneYRotate = -90;
- } else {
- phoneYRotate = 90;
- }
-
- // Rotate the phone vertical about the X axis if it's in portrait mode
- if (PHONE_IS_PORTRAIT) {
- phoneXRotate = 90 ;
- }
-
- // Next, translate the camera lens to where it is on the robot.
- // In this example, it is centered on the robot (left-to-right and front-to-back), and 6 inches above ground level.
- final float CAMERA_FORWARD_DISPLACEMENT = 0.0f * mmPerInch; // eg: Enter the forward distance from the center of the robot to the camera lens
- final float CAMERA_VERTICAL_DISPLACEMENT = 6.0f * mmPerInch; // eg: Camera is 6 Inches above ground
- final float CAMERA_LEFT_DISPLACEMENT = 0.0f * mmPerInch; // eg: Enter the left distance from the center of the robot to the camera lens
-
- OpenGLMatrix robotFromCamera = OpenGLMatrix
- .translation(CAMERA_FORWARD_DISPLACEMENT, CAMERA_LEFT_DISPLACEMENT, CAMERA_VERTICAL_DISPLACEMENT)
- .multiplied(Orientation.getRotationMatrix(EXTRINSIC, YZX, DEGREES, phoneYRotate, phoneZRotate, phoneXRotate));
-
- /** Let all the trackable listeners know where the phone is. */
- for (VuforiaTrackable trackable : allTrackables) {
- ((VuforiaTrackableDefaultListener) trackable.getListener()).setPhoneInformation(robotFromCamera, parameters.cameraDirection);
- }
-
- /*
- * WARNING:
- * In this sample, we do not wait for PLAY to be pressed. Target Tracking is started immediately when INIT is pressed.
- * This sequence is used to enable the new remote DS Camera Stream feature to be used with this sample.
- * CONSEQUENTLY do not put any driving commands in this loop.
- * To restore the normal opmode structure, just un-comment the following line:
- */
-
- // waitForStart();
-
- /* Note: To use the remote camera preview:
- * AFTER you hit Init on the Driver Station, use the "options menu" to select "Camera Stream"
- * Tap the preview window to receive a fresh image.
- * It is not permitted to transition to RUN while the camera preview window is active.
- * Either press STOP to exit the OpMode, or use the "options menu" again, and select "Camera Stream" to close the preview window.
- */
-
- targets.activate();
- while (!isStopRequested()) {
-
- // check all the trackable targets to see which one (if any) is visible.
- targetVisible = false;
- for (VuforiaTrackable trackable : allTrackables) {
- if (((VuforiaTrackableDefaultListener)trackable.getListener()).isVisible()) {
- telemetry.addData("Visible Target", trackable.getName());
- targetVisible = true;
-
- // getUpdatedRobotLocation() will return null if no new information is available since
- // the last time that call was made, or if the trackable is not currently visible.
- OpenGLMatrix robotLocationTransform = ((VuforiaTrackableDefaultListener)trackable.getListener()).getUpdatedRobotLocation();
- if (robotLocationTransform != null) {
- lastLocation = robotLocationTransform;
- }
- break;
- }
- }
-
- // Provide feedback as to where the robot is located (if we know).
- if (targetVisible) {
- // express position (translation) of robot in inches.
- VectorF translation = lastLocation.getTranslation();
- telemetry.addData("Pos (inches)", "{X, Y, Z} = %.1f, %.1f, %.1f",
- translation.get(0) / mmPerInch, translation.get(1) / mmPerInch, translation.get(2) / mmPerInch);
-
- // express the rotation of the robot in degrees.
- Orientation rotation = Orientation.getOrientation(lastLocation, EXTRINSIC, XYZ, DEGREES);
- telemetry.addData("Rot (deg)", "{Roll, Pitch, Heading} = %.0f, %.0f, %.0f", rotation.firstAngle, rotation.secondAngle, rotation.thirdAngle);
- }
- else {
- telemetry.addData("Visible Target", "none");
- }
- telemetry.update();
- }
-
- // Disable Tracking when we are done;
- targets.deactivate();
- }
-
- /***
- * Identify a target by naming it, and setting its position and orientation on the field
- * @param targetIndex
- * @param targetName
- * @param dx, dy, dz Target offsets in x,y,z axes
- * @param rx, ry, rz Target rotations in x,y,z axes
- */
- void identifyTarget(int targetIndex, String targetName, float dx, float dy, float dz, float rx, float ry, float rz) {
- VuforiaTrackable aTarget = targets.get(targetIndex);
- aTarget.setName(targetName);
- aTarget.setLocation(OpenGLMatrix.translation(dx, dy, dz)
- .multiplied(Orientation.getRotationMatrix(EXTRINSIC, XYZ, DEGREES, rx, ry, rz)));
- }
-}
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVuforiaFieldNavigationWebcam.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVuforiaFieldNavigationWebcam.java
deleted file mode 100644
index c19ee22..0000000
--- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVuforiaFieldNavigationWebcam.java
+++ /dev/null
@@ -1,274 +0,0 @@
-/* Copyright (c) 2019 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 org.firstinspires.ftc.robotcore.external.ClassFactory;
-import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
-import org.firstinspires.ftc.robotcore.external.matrices.OpenGLMatrix;
-import org.firstinspires.ftc.robotcore.external.matrices.VectorF;
-import org.firstinspires.ftc.robotcore.external.navigation.Orientation;
-import org.firstinspires.ftc.robotcore.external.navigation.VuforiaLocalizer;
-import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackable;
-import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackableDefaultListener;
-import org.firstinspires.ftc.robotcore.external.navigation.VuforiaTrackables;
-
-import java.util.ArrayList;
-import java.util.List;
-
-import static org.firstinspires.ftc.robotcore.external.navigation.AngleUnit.DEGREES;
-import static org.firstinspires.ftc.robotcore.external.navigation.AxesOrder.XYZ;
-import static org.firstinspires.ftc.robotcore.external.navigation.AxesOrder.XZY;
-import static org.firstinspires.ftc.robotcore.external.navigation.AxesReference.EXTRINSIC;
-
-/**
- * This OpMode illustrates using the Vuforia localizer to determine positioning and orientation of
- * robot on the FTC field using a WEBCAM. The code is structured as a LinearOpMode
- *
- * NOTE: If you are running on a Phone with a built-in camera, use the ConceptVuforiaFieldNavigation example instead of this one.
- * NOTE: It is possible to switch between multiple WebCams (eg: one for the left side and one for the right).
- * For a related example of how to do this, see ConceptTensorFlowObjectDetectionSwitchableCameras
- *
- * When images are located, Vuforia is able to determine the position and orientation of the
- * image relative to the camera. This sample code then combines that information with a
- * knowledge of where the target images are on the field, to determine the location of the camera.
- *
- * Finally, the location of the camera on the robot is used to determine the
- * robot's location and orientation on the field.
- *
- * To learn more about the FTC field coordinate model, see FTC_FieldCoordinateSystemDefinition.pdf in this folder
- *
- * 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.
- *
- * IMPORTANT: In order to use this OpMode, you need to obtain your own Vuforia license key as
- * is explained below.
- */
-
-@TeleOp(name="Vuforia Field Nav Webcam", group ="Concept")
-@Disabled
-public class ConceptVuforiaFieldNavigationWebcam extends LinearOpMode {
-
- /*
- * IMPORTANT: You need to obtain your own license key to use Vuforia. The string below with which
- * 'parameters.vuforiaLicenseKey' is initialized is for illustration only, and will not function.
- * A Vuforia 'Development' license key, can be obtained free of charge from the Vuforia developer
- * web site at https://developer.vuforia.com/license-manager.
- *
- * Vuforia license keys are always 380 characters long, and look as if they contain mostly
- * random data. As an example, here is a example of a fragment of a valid key:
- * ... yIgIzTqZ4mWjk9wd3cZO9T1axEqzuhxoGlfOOI2dRzKS4T0hQ8kT ...
- * Once you've obtained a license key, copy the string from the Vuforia web site
- * and paste it in to your code on the next line, between the double quotes.
- */
- private static final String VUFORIA_KEY =
- " --- YOUR NEW VUFORIA KEY GOES HERE --- ";
-
- // Since ImageTarget trackables use mm to specifiy their dimensions, we must use mm for all the physical dimension.
- // We will define some constants and conversions here
- private static final float mmPerInch = 25.4f;
- private static final float mmTargetHeight = 6 * mmPerInch; // the height of the center of the target image above the floor
- private static final float halfField = 72 * mmPerInch;
- private static final float halfTile = 12 * mmPerInch;
- private static final float oneAndHalfTile = 36 * mmPerInch;
-
- // Class Members
- private OpenGLMatrix lastLocation = null;
- private VuforiaLocalizer vuforia = null;
- private VuforiaTrackables targets = null ;
- private WebcamName webcamName = null;
-
- private boolean targetVisible = false;
-
- @Override public void runOpMode() {
- // Connect to the camera we are to use. This name must match what is set up in Robot Configuration
- webcamName = hardwareMap.get(WebcamName.class, "Webcam 1");
-
- /*
- * Configure Vuforia by creating a Parameter object, and passing it to the Vuforia engine.
- * We can pass Vuforia the handle to a camera preview resource (on the RC screen);
- * If no camera-preview is desired, use the parameter-less constructor instead (commented out below).
- * Note: A preview window is required if you want to view the camera stream on the Driver Station Phone.
- */
- int cameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName());
- VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters(cameraMonitorViewId);
- // VuforiaLocalizer.Parameters parameters = new VuforiaLocalizer.Parameters();
-
- parameters.vuforiaLicenseKey = VUFORIA_KEY;
-
- // We also indicate which camera we wish to use.
- parameters.cameraName = webcamName;
-
- // Turn off Extended tracking. Set this true if you want Vuforia to track beyond the target.
- parameters.useExtendedTracking = false;
-
- // Instantiate the Vuforia engine
- vuforia = ClassFactory.getInstance().createVuforia(parameters);
-
- // Load the data sets for the trackable objects. These particular data
- // sets are stored in the 'assets' part of our application.
- targets = this.vuforia.loadTrackablesFromAsset("FreightFrenzy");
-
- // For convenience, gather together all the trackable objects in one easily-iterable collection */
- List allTrackables = new ArrayList();
- allTrackables.addAll(targets);
-
- /**
- * In order for localization to work, we need to tell the system where each target is on the field, and
- * where the phone resides on the robot. These specifications are in the form of transformation matrices.
- * Transformation matrices are a central, important concept in the math here involved in localization.
- * See Transformation Matrix
- * for detailed information. Commonly, you'll encounter transformation matrices as instances
- * of the {@link OpenGLMatrix} class.
- *
- * If you are standing in the Red Alliance Station looking towards the center of the field,
- * - The X axis runs from your left to the right. (positive from the center to the right)
- * - The Y axis runs from the Red Alliance Station towards the other side of the field
- * where the Blue Alliance Station is. (Positive is from the center, towards the BlueAlliance station)
- * - The Z axis runs from the floor, upwards towards the ceiling. (Positive is above the floor)
- *
- * Before being transformed, each target image is conceptually located at the origin of the field's
- * coordinate system (the center of the field), facing up.
- */
-
- // Name and locate each trackable object
- identifyTarget(0, "Blue Storage", -halfField, oneAndHalfTile, mmTargetHeight, 90, 0, 90);
- identifyTarget(1, "Blue Alliance Wall", halfTile, halfField, mmTargetHeight, 90, 0, 0);
- identifyTarget(2, "Red Storage", -halfField, -oneAndHalfTile, mmTargetHeight, 90, 0, 90);
- identifyTarget(3, "Red Alliance Wall", halfTile, -halfField, mmTargetHeight, 90, 0, 180);
-
- /*
- * Create a transformation matrix describing where the camera is on the robot.
- *
- * Info: The coordinate frame for the robot looks the same as the field.
- * The robot's "forward" direction is facing out along X axis, with the LEFT side facing out along the Y axis.
- * Z is UP on the robot. This equates to a bearing angle of Zero degrees.
- *
- * For a WebCam, the default starting orientation of the camera is looking UP (pointing in the Z direction),
- * with the wide (horizontal) axis of the camera aligned with the X axis, and
- * the Narrow (vertical) axis of the camera aligned with the Y axis
- *
- * But, this example assumes that the camera is actually facing forward out the front of the robot.
- * So, the "default" camera position requires two rotations to get it oriented correctly.
- * 1) First it must be rotated +90 degrees around the X axis to get it horizontal (its now facing out the right side of the robot)
- * 2) Next it must be be rotated +90 degrees (counter-clockwise) around the Z axis to face forward.
- *
- * Finally the camera can be translated to its actual mounting position on the robot.
- * In this example, it is centered on the robot (left-to-right and front-to-back), and 6 inches above ground level.
- */
-
- final float CAMERA_FORWARD_DISPLACEMENT = 0.0f * mmPerInch; // eg: Enter the forward distance from the center of the robot to the camera lens
- final float CAMERA_VERTICAL_DISPLACEMENT = 6.0f * mmPerInch; // eg: Camera is 6 Inches above ground
- final float CAMERA_LEFT_DISPLACEMENT = 0.0f * mmPerInch; // eg: Enter the left distance from the center of the robot to the camera lens
-
- OpenGLMatrix cameraLocationOnRobot = OpenGLMatrix
- .translation(CAMERA_FORWARD_DISPLACEMENT, CAMERA_LEFT_DISPLACEMENT, CAMERA_VERTICAL_DISPLACEMENT)
- .multiplied(Orientation.getRotationMatrix(EXTRINSIC, XZY, DEGREES, 90, 90, 0));
-
- /** Let all the trackable listeners know where the camera is. */
- for (VuforiaTrackable trackable : allTrackables) {
- ((VuforiaTrackableDefaultListener) trackable.getListener()).setCameraLocationOnRobot(parameters.cameraName, cameraLocationOnRobot);
- }
-
- /*
- * WARNING:
- * In this sample, we do not wait for PLAY to be pressed. Target Tracking is started immediately when INIT is pressed.
- * This sequence is used to enable the new remote DS Camera Preview feature to be used with this sample.
- * CONSEQUENTLY do not put any driving commands in this loop.
- * To restore the normal opmode structure, just un-comment the following line:
- */
-
- // waitForStart();
-
- /* Note: To use the remote camera preview:
- * AFTER you hit Init on the Driver Station, use the "options menu" to select "Camera Stream"
- * Tap the preview window to receive a fresh image.
- * It is not permitted to transition to RUN while the camera preview window is active.
- * Either press STOP to exit the OpMode, or use the "options menu" again, and select "Camera Stream" to close the preview window.
- */
-
- targets.activate();
- while (!isStopRequested()) {
-
- // check all the trackable targets to see which one (if any) is visible.
- targetVisible = false;
- for (VuforiaTrackable trackable : allTrackables) {
- if (((VuforiaTrackableDefaultListener)trackable.getListener()).isVisible()) {
- telemetry.addData("Visible Target", trackable.getName());
- targetVisible = true;
-
- // getUpdatedRobotLocation() will return null if no new information is available since
- // the last time that call was made, or if the trackable is not currently visible.
- OpenGLMatrix robotLocationTransform = ((VuforiaTrackableDefaultListener)trackable.getListener()).getUpdatedRobotLocation();
- if (robotLocationTransform != null) {
- lastLocation = robotLocationTransform;
- }
- break;
- }
- }
-
- // Provide feedback as to where the robot is located (if we know).
- if (targetVisible) {
- // express position (translation) of robot in inches.
- VectorF translation = lastLocation.getTranslation();
- telemetry.addData("Pos (inches)", "{X, Y, Z} = %.1f, %.1f, %.1f",
- translation.get(0) / mmPerInch, translation.get(1) / mmPerInch, translation.get(2) / mmPerInch);
-
- // express the rotation of the robot in degrees.
- Orientation rotation = Orientation.getOrientation(lastLocation, EXTRINSIC, XYZ, DEGREES);
- telemetry.addData("Rot (deg)", "{Roll, Pitch, Heading} = %.0f, %.0f, %.0f", rotation.firstAngle, rotation.secondAngle, rotation.thirdAngle);
- }
- else {
- telemetry.addData("Visible Target", "none");
- }
- telemetry.update();
- }
-
- // Disable Tracking when we are done;
- targets.deactivate();
- }
-
- /***
- * Identify a target by naming it, and setting its position and orientation on the field
- * @param targetIndex
- * @param targetName
- * @param dx, dy, dz Target offsets in x,y,z axes
- * @param rx, ry, rz Target rotations in x,y,z axes
- */
- void identifyTarget(int targetIndex, String targetName, float dx, float dy, float dz, float rx, float ry, float rz) {
- VuforiaTrackable aTarget = targets.get(targetIndex);
- aTarget.setName(targetName);
- aTarget.setLocation(OpenGLMatrix.translation(dx, dy, dz)
- .multiplied(Orientation.getRotationMatrix(EXTRINSIC, XYZ, DEGREES, rx, ry, rz)));
- }
-}
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptWebcam.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptWebcam.java
deleted file mode 100644
index 41f3dac..0000000
--- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptWebcam.java
+++ /dev/null
@@ -1,310 +0,0 @@
-/* Copyright (c) 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.graphics.Bitmap;
-import android.graphics.ImageFormat;
-import android.os.Handler;
-
-import androidx.annotation.NonNull;
-
-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.robotcore.external.ClassFactory;
-import org.firstinspires.ftc.robotcore.external.android.util.Size;
-import org.firstinspires.ftc.robotcore.external.function.Consumer;
-import org.firstinspires.ftc.robotcore.external.function.Continuation;
-import org.firstinspires.ftc.robotcore.external.hardware.camera.Camera;
-import org.firstinspires.ftc.robotcore.external.hardware.camera.CameraCaptureRequest;
-import org.firstinspires.ftc.robotcore.external.hardware.camera.CameraCaptureSequenceId;
-import org.firstinspires.ftc.robotcore.external.hardware.camera.CameraCaptureSession;
-import org.firstinspires.ftc.robotcore.external.hardware.camera.CameraCharacteristics;
-import org.firstinspires.ftc.robotcore.external.hardware.camera.CameraException;
-import org.firstinspires.ftc.robotcore.external.hardware.camera.CameraFrame;
-import org.firstinspires.ftc.robotcore.external.hardware.camera.CameraManager;
-import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
-import org.firstinspires.ftc.robotcore.internal.collections.EvictingBlockingQueue;
-import org.firstinspires.ftc.robotcore.internal.network.CallbackLooper;
-import org.firstinspires.ftc.robotcore.internal.system.AppUtil;
-import org.firstinspires.ftc.robotcore.internal.system.ContinuationSynchronizer;
-import org.firstinspires.ftc.robotcore.internal.system.Deadline;
-
-import java.io.File;
-import java.io.FileOutputStream;
-import java.io.IOException;
-import java.util.Locale;
-import java.util.concurrent.ArrayBlockingQueue;
-import java.util.concurrent.TimeUnit;
-
-/**
- * This OpMode illustrates how to open a webcam and retrieve images from it. It requires a configuration
- * containing a webcam with the default name ("Webcam 1"). When the opmode runs, pressing the 'A' button
- * will cause a frame from the camera to be written to a file on the device, which can then be retrieved
- * by various means (e.g.: Device File Explorer in Android Studio; plugging the device into a PC and
- * using Media Transfer; ADB; etc)
- */
-@TeleOp(name="Concept: Webcam", group ="Concept")
-@Disabled
-public class ConceptWebcam extends LinearOpMode {
-
- //----------------------------------------------------------------------------------------------
- // State
- //----------------------------------------------------------------------------------------------
-
- private static final String TAG = "Webcam Sample";
-
- /** How long we are to wait to be granted permission to use the camera before giving up. Here,
- * we wait indefinitely */
- private static final int secondsPermissionTimeout = Integer.MAX_VALUE;
-
- /** State regarding our interaction with the camera */
- private CameraManager cameraManager;
- private WebcamName cameraName;
- private Camera camera;
- private CameraCaptureSession cameraCaptureSession;
-
- /** The queue into which all frames from the camera are placed as they become available.
- * Frames which are not processed by the OpMode are automatically discarded. */
- private EvictingBlockingQueue frameQueue;
-
- /** State regarding where and how to save frames when the 'A' button is pressed. */
- private int captureCounter = 0;
- private File captureDirectory = AppUtil.ROBOT_DATA_DIR;
-
- /** A utility object that indicates where the asynchronous callbacks from the camera
- * infrastructure are to run. In this OpMode, that's all hidden from you (but see {@link #startCamera}
- * if you're curious): no knowledge of multi-threading is needed here. */
- private Handler callbackHandler;
-
- //----------------------------------------------------------------------------------------------
- // Main OpMode entry
- //----------------------------------------------------------------------------------------------
-
- @Override public void runOpMode() {
-
- callbackHandler = CallbackLooper.getDefault().getHandler();
-
- cameraManager = ClassFactory.getInstance().getCameraManager();
- cameraName = hardwareMap.get(WebcamName.class, "Webcam 1");
-
- initializeFrameQueue(2);
- AppUtil.getInstance().ensureDirectoryExists(captureDirectory);
-
- try {
- openCamera();
- if (camera == null) return;
-
- startCamera();
- if (cameraCaptureSession == null) return;
-
- telemetry.addData(">", "Press Play to start");
- telemetry.update();
- waitForStart();
- telemetry.clear();
- telemetry.addData(">", "Started...Press 'A' to capture frame");
-
- boolean buttonPressSeen = false;
- boolean captureWhenAvailable = false;
- while (opModeIsActive()) {
-
- boolean buttonIsPressed = gamepad1.a;
- if (buttonIsPressed && !buttonPressSeen) {
- captureWhenAvailable = true;
- }
- buttonPressSeen = buttonIsPressed;
-
- if (captureWhenAvailable) {
- Bitmap bmp = frameQueue.poll();
- if (bmp != null) {
- captureWhenAvailable = false;
- onNewFrame(bmp);
- }
- }
-
- telemetry.update();
- }
- } finally {
- closeCamera();
- }
- }
-
- /** Do something with the frame */
- private void onNewFrame(Bitmap frame) {
- saveBitmap(frame);
- frame.recycle(); // not strictly necessary, but helpful
- }
-
- //----------------------------------------------------------------------------------------------
- // Camera operations
- //----------------------------------------------------------------------------------------------
-
- private void initializeFrameQueue(int capacity) {
- /** The frame queue will automatically throw away bitmap frames if they are not processed
- * quickly by the OpMode. This avoids a buildup of frames in memory */
- frameQueue = new EvictingBlockingQueue(new ArrayBlockingQueue(capacity));
- frameQueue.setEvictAction(new Consumer() {
- @Override public void accept(Bitmap frame) {
- // RobotLog.ii(TAG, "frame recycled w/o processing");
- frame.recycle(); // not strictly necessary, but helpful
- }
- });
- }
-
- private void openCamera() {
- if (camera != null) return; // be idempotent
-
- Deadline deadline = new Deadline(secondsPermissionTimeout, TimeUnit.SECONDS);
- camera = cameraManager.requestPermissionAndOpenCamera(deadline, cameraName, null);
- if (camera == null) {
- error("camera not found or permission to use not granted: %s", cameraName);
- }
- }
-
- private void startCamera() {
- if (cameraCaptureSession != null) return; // be idempotent
-
- /** YUY2 is supported by all Webcams, per the USB Webcam standard: See "USB Device Class Definition
- * for Video Devices: Uncompressed Payload, Table 2-1". Further, often this is the *only*
- * image format supported by a camera */
- final int imageFormat = ImageFormat.YUY2;
-
- /** Verify that the image is supported, and fetch size and desired frame rate if so */
- CameraCharacteristics cameraCharacteristics = cameraName.getCameraCharacteristics();
- if (!contains(cameraCharacteristics.getAndroidFormats(), imageFormat)) {
- error("image format not supported");
- return;
- }
- final Size size = cameraCharacteristics.getDefaultSize(imageFormat);
- final int fps = cameraCharacteristics.getMaxFramesPerSecond(imageFormat, size);
-
- /** Some of the logic below runs asynchronously on other threads. Use of the synchronizer
- * here allows us to wait in this method until all that asynchrony completes before returning. */
- final ContinuationSynchronizer synchronizer = new ContinuationSynchronizer<>();
- try {
- /** Create a session in which requests to capture frames can be made */
- camera.createCaptureSession(Continuation.create(callbackHandler, new CameraCaptureSession.StateCallbackDefault() {
- @Override public void onConfigured(@NonNull CameraCaptureSession session) {
- try {
- /** The session is ready to go. Start requesting frames */
- final CameraCaptureRequest captureRequest = camera.createCaptureRequest(imageFormat, size, fps);
- session.startCapture(captureRequest,
- new CameraCaptureSession.CaptureCallback() {
- @Override public void onNewFrame(@NonNull CameraCaptureSession session, @NonNull CameraCaptureRequest request, @NonNull CameraFrame cameraFrame) {
- /** A new frame is available. The frame data has not been copied for us, and we can only access it
- * for the duration of the callback. So we copy here manually. */
- Bitmap bmp = captureRequest.createEmptyBitmap();
- cameraFrame.copyToBitmap(bmp);
- frameQueue.offer(bmp);
- }
- },
- Continuation.create(callbackHandler, new CameraCaptureSession.StatusCallback() {
- @Override public void onCaptureSequenceCompleted(@NonNull CameraCaptureSession session, CameraCaptureSequenceId cameraCaptureSequenceId, long lastFrameNumber) {
- RobotLog.ii(TAG, "capture sequence %s reports completed: lastFrame=%d", cameraCaptureSequenceId, lastFrameNumber);
- }
- })
- );
- synchronizer.finish(session);
- } catch (CameraException|RuntimeException e) {
- RobotLog.ee(TAG, e, "exception starting capture");
- error("exception starting capture");
- session.close();
- synchronizer.finish(null);
- }
- }
- }));
- } catch (CameraException|RuntimeException e) {
- RobotLog.ee(TAG, e, "exception starting camera");
- error("exception starting camera");
- synchronizer.finish(null);
- }
-
- /** Wait for all the asynchrony to complete */
- try {
- synchronizer.await();
- } catch (InterruptedException e) {
- Thread.currentThread().interrupt();
- }
-
- /** Retrieve the created session. This will be null on error. */
- cameraCaptureSession = synchronizer.getValue();
- }
-
- private void stopCamera() {
- if (cameraCaptureSession != null) {
- cameraCaptureSession.stopCapture();
- cameraCaptureSession.close();
- cameraCaptureSession = null;
- }
- }
-
- private void closeCamera() {
- stopCamera();
- if (camera != null) {
- camera.close();
- camera = null;
- }
- }
-
- //----------------------------------------------------------------------------------------------
- // Utilities
- //----------------------------------------------------------------------------------------------
-
- private void error(String msg) {
- telemetry.log().add(msg);
- telemetry.update();
- }
- private void error(String format, Object...args) {
- telemetry.log().add(format, args);
- telemetry.update();
- }
-
- private boolean contains(int[] array, int value) {
- for (int i : array) {
- if (i == value) return true;
- }
- return false;
- }
-
- private void saveBitmap(Bitmap bitmap) {
- File file = new File(captureDirectory, String.format(Locale.getDefault(), "webcam-frame-%d.jpg", captureCounter++));
- try {
- try (FileOutputStream outputStream = new FileOutputStream(file)) {
- bitmap.compress(Bitmap.CompressFormat.JPEG, 100, outputStream);
- telemetry.log().add("captured %s", file.getName());
- }
- } catch (IOException e) {
- RobotLog.ee(TAG, e, "exception in saveBitmap()");
- error("exception saving %s", file.getName());
- }
- }
-}
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/FTC_FieldCoordinateSystemDefinition.pdf b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/FTC_FieldCoordinateSystemDefinition.pdf
deleted file mode 100644
index c424d47..0000000
Binary files a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/FTC_FieldCoordinateSystemDefinition.pdf and /dev/null differ
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/HardwarePushbot.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/HardwarePushbot.java
deleted file mode 100644
index 4848137..0000000
--- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/HardwarePushbot.java
+++ /dev/null
@@ -1,105 +0,0 @@
-/* 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.hardware.DcMotor;
-import com.qualcomm.robotcore.hardware.HardwareMap;
-import com.qualcomm.robotcore.hardware.Servo;
-import com.qualcomm.robotcore.util.ElapsedTime;
-
-/**
- * This is NOT an opmode.
- *
- * This class can be used to define all the specific hardware for a single robot.
- * In this case that robot is a Pushbot.
- * See PushbotTeleopTank_Iterative and others classes starting with "Pushbot" for usage examples.
- *
- * This hardware class assumes the following device names have been configured on the robot:
- * Note: All names are lower case and some have single spaces between words.
- *
- * Motor channel: Left drive motor: "left_drive"
- * Motor channel: Right drive motor: "right_drive"
- * Motor channel: Manipulator drive motor: "left_arm"
- * Servo channel: Servo to open left claw: "left_hand"
- * Servo channel: Servo to open right claw: "right_hand"
- */
-public class HardwarePushbot
-{
- /* Public OpMode members. */
- public DcMotor leftDrive = null;
- public DcMotor rightDrive = null;
- public DcMotor leftArm = null;
- public Servo leftClaw = null;
- public Servo rightClaw = null;
-
- public static final double MID_SERVO = 0.5 ;
- public static final double ARM_UP_POWER = 0.45 ;
- public static final double ARM_DOWN_POWER = -0.45 ;
-
- /* local OpMode members. */
- HardwareMap hwMap = null;
- private ElapsedTime period = new ElapsedTime();
-
- /* Constructor */
- public HardwarePushbot(){
-
- }
-
- /* Initialize standard Hardware interfaces */
- public void init(HardwareMap ahwMap) {
- // Save reference to Hardware map
- hwMap = ahwMap;
-
- // Define and Initialize Motors
- leftDrive = hwMap.get(DcMotor.class, "left_drive");
- rightDrive = hwMap.get(DcMotor.class, "right_drive");
- leftArm = hwMap.get(DcMotor.class, "left_arm");
- leftDrive.setDirection(DcMotor.Direction.FORWARD); // Set to REVERSE if using AndyMark motors
- rightDrive.setDirection(DcMotor.Direction.REVERSE);// Set to FORWARD if using AndyMark motors
-
- // Set all motors to zero power
- leftDrive.setPower(0);
- rightDrive.setPower(0);
- leftArm.setPower(0);
-
- // Set all motors to run without encoders.
- // May want to use RUN_USING_ENCODERS if encoders are installed.
- leftDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
- rightDrive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
- leftArm.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
-
- // Define and initialize ALL installed servos.
- leftClaw = hwMap.get(Servo.class, "left_hand");
- rightClaw = hwMap.get(Servo.class, "right_hand");
- leftClaw.setPosition(MID_SERVO);
- rightClaw.setPosition(MID_SERVO);
- }
- }
-
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/PushbotAutoDriveByGyro_Linear.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/PushbotAutoDriveByGyro_Linear.java
deleted file mode 100644
index a366bec..0000000
--- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/PushbotAutoDriveByGyro_Linear.java
+++ /dev/null
@@ -1,363 +0,0 @@
-/* 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.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 com.qualcomm.robotcore.util.Range;
-
-/**
- * This file illustrates the concept of driving a path based on Gyro heading and encoder counts.
- * It uses the common Pushbot hardware class to define the drive on the robot.
- * The code is structured as a LinearOpMode
- *
- * The code REQUIRES that you DO have encoders on the wheels,
- * otherwise you would use: PushbotAutoDriveByTime;
- *
- * This code ALSO requires that you have a Modern Robotics I2C gyro with the name "gyro"
- * otherwise you would use: PushbotAutoDriveByEncoder;
- *
- * This code requires that the drive Motors have been configured such that a positive
- * power command moves them forward, and causes the encoders to count UP.
- *
- * This code uses the RUN_TO_POSITION mode to enable the Motor controllers to generate the run profile
- *
- * In order to calibrate the Gyro correctly, the robot must remain stationary during calibration.
- * This is performed when the INIT button is pressed on the Driver Station.
- * This code assumes that the robot is stationary when the INIT button is pressed.
- * If this is not the case, then the INIT should be performed again.
- *
- * Note: in this example, all angles are referenced to the initial coordinate frame set during the
- * the Gyro Calibration process, or whenever the program issues a resetZAxisIntegrator() call on the Gyro.
- *
- * 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 Clock Wise, looking down on the field.
- * This is consistent with the FTC field coordinate conventions set out in the document:
- * ftc_app\doc\tutorial\FTC_FieldCoordinateSystemDefinition.pdf
- *
- * Use Android Studios 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="Pushbot: Auto Drive By Gyro", group="Pushbot")
-@Disabled
-public class PushbotAutoDriveByGyro_Linear extends LinearOpMode {
-
- /* Declare OpMode members. */
- HardwarePushbot robot = new HardwarePushbot(); // Use a Pushbot's hardware
- ModernRoboticsI2cGyro gyro = null; // Additional Gyro device
-
- static final double COUNTS_PER_MOTOR_REV = 1440 ; // eg: TETRIX Motor Encoder
- static final double DRIVE_GEAR_REDUCTION = 2.0 ; // This is < 1.0 if geared UP
- 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
- // The can/should be tweaked to suite the specific robot drive train.
- static final double DRIVE_SPEED = 0.7; // Nominal speed for better accuracy.
- static final double TURN_SPEED = 0.5; // Nominal half speed for better accuracy.
-
- static final double HEADING_THRESHOLD = 1 ; // As tight as we can make it with an integer gyro
- static final double P_TURN_COEFF = 0.1; // Larger is more responsive, but also less stable
- static final double P_DRIVE_COEFF = 0.15; // Larger is more responsive, but also less stable
-
-
- @Override
- public void runOpMode() {
-
- /*
- * Initialize the standard drive system variables.
- * The init() method of the hardware class does most of the work here
- */
- robot.init(hardwareMap);
- gyro = (ModernRoboticsI2cGyro)hardwareMap.gyroSensor.get("gyro");
-
- // Ensure the robot it stationary, then reset the encoders and calibrate the gyro.
- robot.leftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
- robot.rightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
-
- // Send telemetry message to alert driver that we are calibrating;
- telemetry.addData(">", "Calibrating Gyro"); //
- telemetry.update();
-
- gyro.calibrate();
-
- // make sure the gyro is calibrated before continuing
- while (!isStopRequested() && gyro.isCalibrating()) {
- sleep(50);
- idle();
- }
-
- telemetry.addData(">", "Robot Ready."); //
- telemetry.update();
-
- robot.leftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
- robot.rightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
-
- // Wait for the game to start (Display Gyro value), and reset gyro before we move..
- while (!isStarted()) {
- telemetry.addData(">", "Robot Heading = %d", gyro.getIntegratedZValue());
- telemetry.update();
- }
-
- gyro.resetZAxisIntegrator();
-
- // Step through each leg of the path,
- // Note: Reverse movement is obtained by setting a negative distance (not speed)
- // Put a hold after each turn
- gyroDrive(DRIVE_SPEED, 48.0, 0.0); // Drive FWD 48 inches
- gyroTurn( TURN_SPEED, -45.0); // Turn CCW to -45 Degrees
- gyroHold( TURN_SPEED, -45.0, 0.5); // Hold -45 Deg heading for a 1/2 second
- gyroDrive(DRIVE_SPEED, 12.0, -45.0); // Drive FWD 12 inches at 45 degrees
- gyroTurn( TURN_SPEED, 45.0); // Turn CW to 45 Degrees
- gyroHold( TURN_SPEED, 45.0, 0.5); // Hold 45 Deg heading for a 1/2 second
- gyroTurn( TURN_SPEED, 0.0); // Turn CW to 0 Degrees
- gyroHold( TURN_SPEED, 0.0, 1.0); // Hold 0 Deg heading for a 1 second
- gyroDrive(DRIVE_SPEED,-48.0, 0.0); // Drive REV 48 inches
-
- telemetry.addData("Path", "Complete");
- telemetry.update();
- }
-
-
- /**
- * Method to drive on a fixed compass bearing (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 speed Target speed for forward motion. Should allow for _/- variance for adjusting heading
- * @param distance Distance (in inches) to move from current position. Negative distance means move backwards.
- * @param angle Absolute 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 gyroDrive ( double speed,
- double distance,
- double angle) {
-
- int newLeftTarget;
- int newRightTarget;
- int moveCounts;
- double max;
- double error;
- double steer;
- double leftSpeed;
- double rightSpeed;
-
- // Ensure that the opmode is still active
- if (opModeIsActive()) {
-
- // Determine new target position, and pass to motor controller
- moveCounts = (int)(distance * COUNTS_PER_INCH);
- newLeftTarget = robot.leftDrive.getCurrentPosition() + moveCounts;
- newRightTarget = robot.rightDrive.getCurrentPosition() + moveCounts;
-
- // Set Target and Turn On RUN_TO_POSITION
- robot.leftDrive.setTargetPosition(newLeftTarget);
- robot.rightDrive.setTargetPosition(newRightTarget);
-
- robot.leftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
- robot.rightDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
-
- // start motion.
- speed = Range.clip(Math.abs(speed), 0.0, 1.0);
- robot.leftDrive.setPower(speed);
- robot.rightDrive.setPower(speed);
-
- // keep looping while we are still active, and BOTH motors are running.
- while (opModeIsActive() &&
- (robot.leftDrive.isBusy() && robot.rightDrive.isBusy())) {
-
- // adjust relative speed based on heading error.
- error = getError(angle);
- steer = getSteer(error, P_DRIVE_COEFF);
-
- // if driving in reverse, the motor correction also needs to be reversed
- if (distance < 0)
- steer *= -1.0;
-
- leftSpeed = speed - steer;
- rightSpeed = speed + steer;
-
- // Normalize speeds if either one exceeds +/- 1.0;
- max = Math.max(Math.abs(leftSpeed), Math.abs(rightSpeed));
- if (max > 1.0)
- {
- leftSpeed /= max;
- rightSpeed /= max;
- }
-
- robot.leftDrive.setPower(leftSpeed);
- robot.rightDrive.setPower(rightSpeed);
-
- // Display drive status for the driver.
- telemetry.addData("Err/St", "%5.1f/%5.1f", error, steer);
- telemetry.addData("Target", "%7d:%7d", newLeftTarget, newRightTarget);
- telemetry.addData("Actual", "%7d:%7d", robot.leftDrive.getCurrentPosition(),
- robot.rightDrive.getCurrentPosition());
- telemetry.addData("Speed", "%5.2f:%5.2f", leftSpeed, rightSpeed);
- telemetry.update();
- }
-
- // Stop all motion;
- robot.leftDrive.setPower(0);
- robot.rightDrive.setPower(0);
-
- // Turn off RUN_TO_POSITION
- robot.leftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
- robot.rightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
- }
- }
-
- /**
- * Method to spin on 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 speed Desired speed of turn.
- * @param angle Absolute 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 gyroTurn ( double speed, double angle) {
-
- // keep looping while we are still active, and not on heading.
- while (opModeIsActive() && !onHeading(speed, angle, P_TURN_COEFF)) {
- // Update telemetry & Allow time for other processes to run.
- telemetry.update();
- }
- }
-
- /**
- * Method to obtain & hold a heading for a finite amount of time
- * Move will stop once the requested time has elapsed
- *
- * @param speed Desired speed of turn.
- * @param angle Absolute 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 gyroHold( double speed, double angle, double holdTime) {
-
- ElapsedTime holdTimer = new ElapsedTime();
-
- // keep looping while we have time remaining.
- holdTimer.reset();
- while (opModeIsActive() && (holdTimer.time() < holdTime)) {
- // Update telemetry & Allow time for other processes to run.
- onHeading(speed, angle, P_TURN_COEFF);
- telemetry.update();
- }
-
- // Stop all motion;
- robot.leftDrive.setPower(0);
- robot.rightDrive.setPower(0);
- }
-
- /**
- * Perform one cycle of closed loop heading control.
- *
- * @param speed Desired speed of turn.
- * @param angle Absolute 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 PCoeff Proportional Gain coefficient
- * @return
- */
- boolean onHeading(double speed, double angle, double PCoeff) {
- double error ;
- double steer ;
- boolean onTarget = false ;
- double leftSpeed;
- double rightSpeed;
-
- // determine turn power based on +/- error
- error = getError(angle);
-
- if (Math.abs(error) <= HEADING_THRESHOLD) {
- steer = 0.0;
- leftSpeed = 0.0;
- rightSpeed = 0.0;
- onTarget = true;
- }
- else {
- steer = getSteer(error, PCoeff);
- rightSpeed = speed * steer;
- leftSpeed = -rightSpeed;
- }
-
- // Send desired speeds to motors.
- robot.leftDrive.setPower(leftSpeed);
- robot.rightDrive.setPower(rightSpeed);
-
- // Display it for the driver.
- telemetry.addData("Target", "%5.2f", angle);
- telemetry.addData("Err/St", "%5.2f/%5.2f", error, steer);
- telemetry.addData("Speed.", "%5.2f:%5.2f", leftSpeed, rightSpeed);
-
- return onTarget;
- }
-
- /**
- * getError determines the error between the target angle and the robot's current heading
- * @param targetAngle Desired angle (relative to global reference established at last Gyro Reset).
- * @return error angle: Degrees in the range +/- 180. Centered on the robot's frame of reference
- * +ve error means the robot should turn LEFT (CCW) to reduce error.
- */
- public double getError(double targetAngle) {
-
- double robotError;
-
- // calculate error in -179 to +180 range (
- robotError = targetAngle - gyro.getIntegratedZValue();
- while (robotError > 180) robotError -= 360;
- while (robotError <= -180) robotError += 360;
- return robotError;
- }
-
- /**
- * returns desired steering force. +/- 1 range. +ve = steer left
- * @param error Error angle in robot relative degrees
- * @param PCoeff Proportional Gain Coefficient
- * @return
- */
- public double getSteer(double error, double PCoeff) {
- return Range.clip(error * PCoeff, -1, 1);
- }
-
-}
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/PushbotAutoDriveToLine_Linear.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/PushbotAutoDriveToLine_Linear.java
deleted file mode 100644
index 2e174b1..0000000
--- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/PushbotAutoDriveToLine_Linear.java
+++ /dev/null
@@ -1,118 +0,0 @@
-/* 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.LightSensor;
-
-/**
- * This file illustrates the concept of driving up to a line and then stopping.
- * It uses the common Pushbot hardware class to define the drive on the robot.
- * The code is structured as a LinearOpMode
- *
- * The code shows using two different light sensors:
- * The Primary sensor shown in this code is a legacy NXT Light sensor (called "sensor_light")
- * Alternative "commented out" code uses a MR Optical Distance Sensor (called "sensor_ods")
- * instead of the LEGO sensor. Chose to use one sensor or the other.
- *
- * Setting the correct WHITE_THRESHOLD value is key to stopping correctly.
- * This should be set half way between the light and dark values.
- * These values can be read on the screen once the OpMode has been INIT, but before it is STARTED.
- * Move the senso on asnd off the white line and not the min and max readings.
- * Edit this code to make WHITE_THRESHOLD half way between the min and max.
- *
- * Use Android Studios 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="Pushbot: Auto Drive To Line", group="Pushbot")
-@Disabled
-public class PushbotAutoDriveToLine_Linear extends LinearOpMode {
-
- /* Declare OpMode members. */
- HardwarePushbot robot = new HardwarePushbot(); // Use a Pushbot's hardware
- LightSensor lightSensor; // Primary LEGO Light sensor,
- // OpticalDistanceSensor lightSensor; // Alternative MR ODS sensor
-
- static final double WHITE_THRESHOLD = 0.2; // spans between 0.1 - 0.5 from dark to light
- static final double APPROACH_SPEED = 0.5;
-
- @Override
- public void runOpMode() {
-
- /* Initialize the drive system variables.
- * The init() method of the hardware class does all the work here
- */
- robot.init(hardwareMap);
-
- // If there are encoders connected, switch to RUN_USING_ENCODER mode for greater accuracy
- // robot.leftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
- // robot.rightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
-
- // get a reference to our Light Sensor object.
- lightSensor = hardwareMap.lightSensor.get("sensor_light"); // Primary LEGO Light Sensor
- // lightSensor = hardwareMap.opticalDistanceSensor.get("sensor_ods"); // Alternative MR ODS sensor.
-
- // turn on LED of light sensor.
- lightSensor.enableLed(true);
-
- // Send telemetry message to signify robot waiting;
- telemetry.addData("Status", "Ready to run"); //
- telemetry.update();
-
- // Wait for the game to start (driver presses PLAY)
- // Abort this loop is started or stopped.
- while (!(isStarted() || isStopRequested())) {
-
- // Display the light level while we are waiting to start
- telemetry.addData("Light Level", lightSensor.getLightDetected());
- telemetry.update();
- idle();
- }
-
- // Start the robot moving forward, and then begin looking for a white line.
- robot.leftDrive.setPower(APPROACH_SPEED);
- robot.rightDrive.setPower(APPROACH_SPEED);
-
- // run until the white line is seen OR the driver presses STOP;
- while (opModeIsActive() && (lightSensor.getLightDetected() < WHITE_THRESHOLD)) {
-
- // Display the light level while we are looking for the line
- telemetry.addData("Light Level", lightSensor.getLightDetected());
- telemetry.update();
- }
-
- // Stop all motors
- robot.leftDrive.setPower(0);
- robot.rightDrive.setPower(0);
- }
-}
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/PushbotAutoDriveByEncoder_Linear.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveByEncoder_Linear.java
similarity index 59%
rename from FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/PushbotAutoDriveByEncoder_Linear.java
rename to FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveByEncoder_Linear.java
index facc531..63293d0 100644
--- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/PushbotAutoDriveByEncoder_Linear.java
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveByEncoder_Linear.java
@@ -35,43 +35,50 @@
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.util.ElapsedTime;
-/**
- * This file illustrates the concept of driving a path based on encoder counts.
- * It uses the common Pushbot hardware class to define the drive on the robot.
+/*
+ * 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: PushbotAutoDriveByTime;
+ * otherwise you would use: RobotAutoDriveByTime;
*
* This code ALSO requires that the drive Motors have been configured such that a positive
- * power command moves them forwards, and causes the encoders to count UP.
+ * 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 Backwards for 24 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 methods assumes that each movement is relative to the last stopping place.
+ * 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 Studios 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
+ * 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="Pushbot: Auto Drive By Encoder", group="Pushbot")
+@Autonomous(name="Robot: Auto Drive By Encoder", group="Robot")
@Disabled
-public class PushbotAutoDriveByEncoder_Linear extends LinearOpMode {
+public class RobotAutoDriveByEncoder_Linear extends LinearOpMode {
/* Declare OpMode members. */
- HardwarePushbot robot = new HardwarePushbot(); // Use a Pushbot's hardware
+ 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 = 2.0 ; // This is < 1.0 if geared UP
+ 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);
@@ -81,29 +88,29 @@ public class PushbotAutoDriveByEncoder_Linear extends LinearOpMode {
@Override
public void runOpMode() {
- /*
- * Initialize the drive system variables.
- * The init() method of the hardware class does all the work here
- */
- robot.init(hardwareMap);
+ // Initialize the drive system variables.
+ leftDrive = hardwareMap.get(DcMotor.class, "left_drive");
+ rightDrive = hardwareMap.get(DcMotor.class, "right_drive");
- // Send telemetry message to signify robot waiting;
- telemetry.addData("Status", "Resetting Encoders"); //
- telemetry.update();
+ // 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);
- robot.leftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
- robot.rightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+ leftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+ rightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
- robot.leftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
- robot.rightDrive.setMode(DcMotor.RunMode.RUN_USING_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("Path0", "Starting at %7d :%7d",
- robot.leftDrive.getCurrentPosition(),
- robot.rightDrive.getCurrentPosition());
+ telemetry.addData("Starting at", "%7d :%7d",
+ leftDrive.getCurrentPosition(),
+ rightDrive.getCurrentPosition());
telemetry.update();
- // Wait for the game to start (driver presses PLAY)
+ // Wait for the game to start (driver presses START)
waitForStart();
// Step through each leg of the path,
@@ -112,12 +119,9 @@ public void runOpMode() {
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
- robot.leftClaw.setPosition(1.0); // S4: Stop and close the claw.
- robot.rightClaw.setPosition(0.0);
- sleep(1000); // pause for servos to move
-
telemetry.addData("Path", "Complete");
telemetry.update();
+ sleep(1000); // pause to display final telemetry message.
}
/*
@@ -126,7 +130,7 @@ public void runOpMode() {
* 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.
+ * 3) Driver stops the OpMode running.
*/
public void encoderDrive(double speed,
double leftInches, double rightInches,
@@ -134,23 +138,23 @@ public void encoderDrive(double speed,
int newLeftTarget;
int newRightTarget;
- // Ensure that the opmode is still active
+ // Ensure that the OpMode is still active
if (opModeIsActive()) {
// Determine new target position, and pass to motor controller
- newLeftTarget = robot.leftDrive.getCurrentPosition() + (int)(leftInches * COUNTS_PER_INCH);
- newRightTarget = robot.rightDrive.getCurrentPosition() + (int)(rightInches * COUNTS_PER_INCH);
- robot.leftDrive.setTargetPosition(newLeftTarget);
- robot.rightDrive.setTargetPosition(newRightTarget);
+ 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
- robot.leftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
- robot.rightDrive.setMode(DcMotor.RunMode.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();
- robot.leftDrive.setPower(Math.abs(speed));
- robot.rightDrive.setPower(Math.abs(speed));
+ 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
@@ -160,25 +164,24 @@ public void encoderDrive(double speed,
// onto the next step, use (isBusy() || isBusy()) in the loop test.
while (opModeIsActive() &&
(runtime.seconds() < timeoutS) &&
- (robot.leftDrive.isBusy() && robot.rightDrive.isBusy())) {
+ (leftDrive.isBusy() && rightDrive.isBusy())) {
// Display it for the driver.
- telemetry.addData("Path1", "Running to %7d :%7d", newLeftTarget, newRightTarget);
- telemetry.addData("Path2", "Running at %7d :%7d",
- robot.leftDrive.getCurrentPosition(),
- robot.rightDrive.getCurrentPosition());
+ telemetry.addData("Running to", " %7d :%7d", newLeftTarget, newRightTarget);
+ telemetry.addData("Currently at", " at %7d :%7d",
+ leftDrive.getCurrentPosition(), rightDrive.getCurrentPosition());
telemetry.update();
}
// Stop all motion;
- robot.leftDrive.setPower(0);
- robot.rightDrive.setPower(0);
+ leftDrive.setPower(0);
+ rightDrive.setPower(0);
// Turn off RUN_TO_POSITION
- robot.leftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
- robot.rightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+ leftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+ rightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
- // sleep(250); // optional pause after each move
+ sleep(250); // optional pause after each move.
}
}
}
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveByGyro_Linear.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveByGyro_Linear.java
new file mode 100644
index 0000000..ab70934
--- /dev/null
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveByGyro_Linear.java
@@ -0,0 +1,429 @@
+/* Copyright (c) 2022 FIRST. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted (subject to the limitations in the disclaimer below) provided that
+ * the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this list
+ * of conditions and the following disclaimer.
+ *
+ * Redistributions in binary form must reproduce the above copyright notice, this
+ * list of conditions and the following disclaimer in the documentation and/or
+ * other materials provided with the distribution.
+ *
+ * Neither the name of FIRST nor the names of its contributors may be used to endorse or
+ * promote products derived from this software without specific prior written permission.
+ *
+ * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
+ * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+ * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+ * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+package org.firstinspires.ftc.robotcontroller.external.samples;
+
+import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.IMU;
+import com.qualcomm.robotcore.util.ElapsedTime;
+import com.qualcomm.robotcore.util.Range;
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles;
+
+/*
+ * This OpMode illustrates the concept of driving an autonomous path based on Gyro (IMU) heading and encoder counts.
+ * The code is structured as a LinearOpMode
+ *
+ * The path to be followed by the robot is built from a series of drive, turn or pause steps.
+ * Each step on the path is defined by a single function call, and these can be strung together in any order.
+ *
+ * The code REQUIRES that you have encoders on the drive motors, otherwise you should use: RobotAutoDriveByTime;
+ *
+ * This code uses the Universal IMU interface so it will work with either the BNO055, or BHI260 IMU.
+ * To run as written, the Control/Expansion hub should be mounted horizontally on a flat part of the robot chassis.
+ * The REV Logo should be facing UP, and the USB port should be facing forward.
+ * If this is not the configuration of your REV Control Hub, then the code should be modified to reflect the correct orientation.
+ *
+ * This sample requires that the drive Motors have been configured with names : left_drive and right_drive.
+ * It also requires that a positive power command moves both motors forward, and causes the encoders to count UP.
+ * So please verify that both of your motors move the robot forward on the first move. If not, make the required correction.
+ * See the beginning of runOpMode() to set the FORWARD/REVERSE option for each motor.
+ *
+ * This code uses RUN_TO_POSITION mode for driving straight, and RUN_USING_ENCODER mode for turning and holding.
+ * Note: This code implements the requirement of calling setTargetPosition() at least once before switching to RUN_TO_POSITION mode.
+ *
+ * Notes:
+ *
+ * All angles are referenced to the coordinate-frame that is set whenever resetHeading() is called.
+ * In this sample, the heading is reset when the Start button is touched on the Driver Station.
+ * Note: It would be possible to reset the heading after each move, but this would accumulate steering errors.
+ *
+ * The angle of movement/rotation is assumed to be a standardized rotation around the robot Z axis,
+ * which means that a Positive rotation is Counter Clockwise, looking down on the field.
+ * This is consistent with the FTC field coordinate conventions set out in the document:
+ * https://ftc-docs.firstinspires.org/field-coordinate-system
+ *
+ * Control Approach.
+ *
+ * To reach, or maintain a required heading, this code implements a basic Proportional Controller where:
+ *
+ * Steering power = Heading Error * Proportional Gain.
+ *
+ * "Heading Error" is calculated by taking the difference between the desired heading and the actual heading,
+ * and then "normalizing" it by converting it to a value in the +/- 180 degree range.
+ *
+ * "Proportional Gain" is a constant that YOU choose to set the "strength" of the steering response.
+ *
+ * Use Android Studio to Copy this Class, and Paste it into your "TeamCode" folder with a new name.
+ * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list
+ */
+
+@Autonomous(name="Robot: Auto Drive By Gyro", group="Robot")
+@Disabled
+public class RobotAutoDriveByGyro_Linear extends LinearOpMode {
+
+ /* Declare OpMode members. */
+ private DcMotor leftDrive = null;
+ private DcMotor rightDrive = null;
+ private IMU imu = null; // Control/Expansion Hub IMU
+
+ private double headingError = 0;
+
+ // These variable are declared here (as class members) so they can be updated in various methods,
+ // but still be displayed by sendTelemetry()
+ private double targetHeading = 0;
+ private double driveSpeed = 0;
+ private double turnSpeed = 0;
+ private double leftSpeed = 0;
+ private double rightSpeed = 0;
+ private int leftTarget = 0;
+ private int rightTarget = 0;
+
+ // Calculate the COUNTS_PER_INCH for your specific drive train.
+ // Go to your motor vendor website to determine your motor's COUNTS_PER_MOTOR_REV
+ // For external drive gearing, set DRIVE_GEAR_REDUCTION as needed.
+ // For example, use a value of 2.0 for a 12-tooth spur gear driving a 24-tooth spur gear.
+ // This is gearing DOWN for less speed and more torque.
+ // For gearing UP, use a gear ratio less than 1.0. Note this will affect the direction of wheel rotation.
+ static final double COUNTS_PER_MOTOR_REV = 537.7 ; // eg: GoBILDA 312 RPM Yellow Jacket
+ static final double DRIVE_GEAR_REDUCTION = 1.0 ; // No External Gearing.
+ static final double WHEEL_DIAMETER_INCHES = 4.0 ; // For figuring circumference
+ static final double COUNTS_PER_INCH = (COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION) /
+ (WHEEL_DIAMETER_INCHES * 3.1415);
+
+ // These constants define the desired driving/control characteristics
+ // They can/should be tweaked to suit the specific robot drive train.
+ static final double DRIVE_SPEED = 0.4; // Max driving speed for better distance accuracy.
+ static final double TURN_SPEED = 0.2; // Max turn speed to limit turn rate.
+ static final double HEADING_THRESHOLD = 1.0 ; // How close must the heading get to the target before moving to next step.
+ // Requiring more accuracy (a smaller number) will often make the turn take longer to get into the final position.
+ // Define the Proportional control coefficient (or GAIN) for "heading control".
+ // We define one value when Turning (larger errors), and the other is used when Driving straight (smaller errors).
+ // Increase these numbers if the heading does not correct strongly enough (eg: a heavy robot or using tracks)
+ // Decrease these numbers if the heading does not settle on the correct value (eg: very agile robot with omni wheels)
+ static final double P_TURN_GAIN = 0.02; // Larger is more responsive, but also less stable.
+ static final double P_DRIVE_GAIN = 0.03; // Larger is more responsive, but also less stable.
+
+
+ @Override
+ public void runOpMode() {
+
+ // Initialize the drive system variables.
+ leftDrive = hardwareMap.get(DcMotor.class, "left_drive");
+ rightDrive = hardwareMap.get(DcMotor.class, "right_drive");
+
+ // To drive forward, most robots need the motor on one side to be reversed, because the axles point in opposite directions.
+ // When run, this OpMode should start both motors driving forward. So adjust these two lines based on your first test drive.
+ // Note: The settings here assume direct drive on left and right wheels. Gear Reduction or 90 Deg drives may require direction flips
+ leftDrive.setDirection(DcMotor.Direction.REVERSE);
+ rightDrive.setDirection(DcMotor.Direction.FORWARD);
+
+ /* The next two lines define Hub orientation.
+ * The Default Orientation (shown) is when a hub is mounted horizontally with the printed logo pointing UP and the USB port pointing FORWARD.
+ *
+ * To Do: EDIT these two lines to match YOUR mounting configuration.
+ */
+ RevHubOrientationOnRobot.LogoFacingDirection logoDirection = RevHubOrientationOnRobot.LogoFacingDirection.UP;
+ RevHubOrientationOnRobot.UsbFacingDirection usbDirection = RevHubOrientationOnRobot.UsbFacingDirection.FORWARD;
+ RevHubOrientationOnRobot orientationOnRobot = new RevHubOrientationOnRobot(logoDirection, usbDirection);
+
+ // Now initialize the IMU with this mounting orientation
+ // This sample expects the IMU to be in a REV Hub and named "imu".
+ imu = hardwareMap.get(IMU.class, "imu");
+ imu.initialize(new IMU.Parameters(orientationOnRobot));
+
+ // Ensure the robot is stationary. Reset the encoders and set the motors to BRAKE mode
+ leftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+ rightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
+ leftDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+ rightDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
+
+ // Wait for the game to start (Display Gyro value while waiting)
+ while (opModeInInit()) {
+ telemetry.addData(">", "Robot Heading = %4.0f", getHeading());
+ telemetry.update();
+ }
+
+ // Set the encoders for closed loop speed control, and reset the heading.
+ leftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+ rightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+ imu.resetYaw();
+
+ // Step through each leg of the path,
+ // Notes: Reverse movement is obtained by setting a negative distance (not speed)
+ // holdHeading() is used after turns to let the heading stabilize
+ // Add a sleep(2000) after any step to keep the telemetry data visible for review
+
+ driveStraight(DRIVE_SPEED, 24.0, 0.0); // Drive Forward 24"
+ turnToHeading( TURN_SPEED, -45.0); // Turn CW to -45 Degrees
+ holdHeading( TURN_SPEED, -45.0, 0.5); // Hold -45 Deg heading for a 1/2 second
+
+ driveStraight(DRIVE_SPEED, 17.0, -45.0); // Drive Forward 17" at -45 degrees (12"x and 12"y)
+ turnToHeading( TURN_SPEED, 45.0); // Turn CCW to 45 Degrees
+ holdHeading( TURN_SPEED, 45.0, 0.5); // Hold 45 Deg heading for a 1/2 second
+
+ driveStraight(DRIVE_SPEED, 17.0, 45.0); // Drive Forward 17" at 45 degrees (-12"x and 12"y)
+ turnToHeading( TURN_SPEED, 0.0); // Turn CW to 0 Degrees
+ holdHeading( TURN_SPEED, 0.0, 1.0); // Hold 0 Deg heading for 1 second
+
+ driveStraight(DRIVE_SPEED,-48.0, 0.0); // Drive in Reverse 48" (should return to approx. staring position)
+
+ telemetry.addData("Path", "Complete");
+ telemetry.update();
+ sleep(1000); // Pause to display last telemetry message.
+ }
+
+ /*
+ * ====================================================================================================
+ * Driving "Helper" functions are below this line.
+ * These provide the high and low level methods that handle driving straight and turning.
+ * ====================================================================================================
+ */
+
+ // ********** HIGH Level driving functions. ********************
+
+ /**
+ * Drive in a straight line, on a fixed compass heading (angle), based on encoder counts.
+ * Move will stop if either of these conditions occur:
+ * 1) Move gets to the desired position
+ * 2) Driver stops the OpMode running.
+ *
+ * @param maxDriveSpeed MAX Speed for forward/rev motion (range 0 to +1.0) .
+ * @param distance Distance (in inches) to move from current position. Negative distance means move backward.
+ * @param heading Absolute Heading Angle (in Degrees) relative to last gyro reset.
+ * 0 = fwd. +ve is CCW from fwd. -ve is CW from forward.
+ * If a relative angle is required, add/subtract from the current robotHeading.
+ */
+ public void driveStraight(double maxDriveSpeed,
+ double distance,
+ double heading) {
+
+ // Ensure that the OpMode is still active
+ if (opModeIsActive()) {
+
+ // Determine new target position, and pass to motor controller
+ int moveCounts = (int)(distance * COUNTS_PER_INCH);
+ leftTarget = leftDrive.getCurrentPosition() + moveCounts;
+ rightTarget = rightDrive.getCurrentPosition() + moveCounts;
+
+ // Set Target FIRST, then turn on RUN_TO_POSITION
+ leftDrive.setTargetPosition(leftTarget);
+ rightDrive.setTargetPosition(rightTarget);
+
+ leftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+ rightDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
+
+ // Set the required driving speed (must be positive for RUN_TO_POSITION)
+ // Start driving straight, and then enter the control loop
+ maxDriveSpeed = Math.abs(maxDriveSpeed);
+ moveRobot(maxDriveSpeed, 0);
+
+ // keep looping while we are still active, and BOTH motors are running.
+ while (opModeIsActive() &&
+ (leftDrive.isBusy() && rightDrive.isBusy())) {
+
+ // Determine required steering to keep on heading
+ turnSpeed = getSteeringCorrection(heading, P_DRIVE_GAIN);
+
+ // if driving in reverse, the motor correction also needs to be reversed
+ if (distance < 0)
+ turnSpeed *= -1.0;
+
+ // Apply the turning correction to the current driving speed.
+ moveRobot(driveSpeed, turnSpeed);
+
+ // Display drive status for the driver.
+ sendTelemetry(true);
+ }
+
+ // Stop all motion & Turn off RUN_TO_POSITION
+ moveRobot(0, 0);
+ leftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+ rightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+ }
+ }
+
+ /**
+ * Spin on the central axis to point in a new direction.
+ *
+ * Move will stop if either of these conditions occur:
+ *
+ * 1) Move gets to the heading (angle)
+ *
+ * 2) Driver stops the OpMode running.
+ *
+ * @param maxTurnSpeed Desired MAX speed of turn. (range 0 to +1.0)
+ * @param heading Absolute Heading Angle (in Degrees) relative to last gyro reset.
+ * 0 = fwd. +ve is CCW from fwd. -ve is CW from forward.
+ * If a relative angle is required, add/subtract from current heading.
+ */
+ public void turnToHeading(double maxTurnSpeed, double heading) {
+
+ // Run getSteeringCorrection() once to pre-calculate the current error
+ getSteeringCorrection(heading, P_DRIVE_GAIN);
+
+ // keep looping while we are still active, and not on heading.
+ while (opModeIsActive() && (Math.abs(headingError) > HEADING_THRESHOLD)) {
+
+ // Determine required steering to keep on heading
+ turnSpeed = getSteeringCorrection(heading, P_TURN_GAIN);
+
+ // Clip the speed to the maximum permitted value.
+ turnSpeed = Range.clip(turnSpeed, -maxTurnSpeed, maxTurnSpeed);
+
+ // Pivot in place by applying the turning correction
+ moveRobot(0, turnSpeed);
+
+ // Display drive status for the driver.
+ sendTelemetry(false);
+ }
+
+ // Stop all motion;
+ moveRobot(0, 0);
+ }
+
+ /**
+ * Obtain & hold a heading for a finite amount of time
+ *
+ * Move will stop once the requested time has elapsed
+ *
+ * This function is useful for giving the robot a moment to stabilize its heading between movements.
+ *
+ * @param maxTurnSpeed Maximum differential turn speed (range 0 to +1.0)
+ * @param heading Absolute Heading Angle (in Degrees) relative to last gyro reset.
+ * 0 = fwd. +ve is CCW from fwd. -ve is CW from forward.
+ * If a relative angle is required, add/subtract from current heading.
+ * @param holdTime Length of time (in seconds) to hold the specified heading.
+ */
+ public void holdHeading(double maxTurnSpeed, double heading, double holdTime) {
+
+ ElapsedTime holdTimer = new ElapsedTime();
+ holdTimer.reset();
+
+ // keep looping while we have time remaining.
+ while (opModeIsActive() && (holdTimer.time() < holdTime)) {
+ // Determine required steering to keep on heading
+ turnSpeed = getSteeringCorrection(heading, P_TURN_GAIN);
+
+ // Clip the speed to the maximum permitted value.
+ turnSpeed = Range.clip(turnSpeed, -maxTurnSpeed, maxTurnSpeed);
+
+ // Pivot in place by applying the turning correction
+ moveRobot(0, turnSpeed);
+
+ // Display drive status for the driver.
+ sendTelemetry(false);
+ }
+
+ // Stop all motion;
+ moveRobot(0, 0);
+ }
+
+ // ********** LOW Level driving functions. ********************
+
+ /**
+ * Use a Proportional Controller to determine how much steering correction is required.
+ *
+ * @param desiredHeading The desired absolute heading (relative to last heading reset)
+ * @param proportionalGain Gain factor applied to heading error to obtain turning power.
+ * @return Turning power needed to get to required heading.
+ */
+ public double getSteeringCorrection(double desiredHeading, double proportionalGain) {
+ targetHeading = desiredHeading; // Save for telemetry
+
+ // Determine the heading current error
+ headingError = targetHeading - getHeading();
+
+ // Normalize the error to be within +/- 180 degrees
+ while (headingError > 180) headingError -= 360;
+ while (headingError <= -180) headingError += 360;
+
+ // Multiply the error by the gain to determine the required steering correction/ Limit the result to +/- 1.0
+ return Range.clip(headingError * proportionalGain, -1, 1);
+ }
+
+ /**
+ * Take separate drive (fwd/rev) and turn (right/left) requests,
+ * combines them, and applies the appropriate speed commands to the left and right wheel motors.
+ * @param drive forward motor speed
+ * @param turn clockwise turning motor speed.
+ */
+ public void moveRobot(double drive, double turn) {
+ driveSpeed = drive; // save this value as a class member so it can be used by telemetry.
+ turnSpeed = turn; // save this value as a class member so it can be used by telemetry.
+
+ leftSpeed = drive - turn;
+ rightSpeed = drive + turn;
+
+ // Scale speeds down if either one exceeds +/- 1.0;
+ double max = Math.max(Math.abs(leftSpeed), Math.abs(rightSpeed));
+ if (max > 1.0)
+ {
+ leftSpeed /= max;
+ rightSpeed /= max;
+ }
+
+ leftDrive.setPower(leftSpeed);
+ rightDrive.setPower(rightSpeed);
+ }
+
+ /**
+ * Display the various control parameters while driving
+ *
+ * @param straight Set to true if we are driving straight, and the encoder positions should be included in the telemetry.
+ */
+ private void sendTelemetry(boolean straight) {
+
+ if (straight) {
+ telemetry.addData("Motion", "Drive Straight");
+ telemetry.addData("Target Pos L:R", "%7d:%7d", leftTarget, rightTarget);
+ telemetry.addData("Actual Pos L:R", "%7d:%7d", leftDrive.getCurrentPosition(),
+ rightDrive.getCurrentPosition());
+ } else {
+ telemetry.addData("Motion", "Turning");
+ }
+
+ telemetry.addData("Heading- Target : Current", "%5.2f : %5.0f", targetHeading, getHeading());
+ telemetry.addData("Error : Steer Pwr", "%5.1f : %5.1f", headingError, turnSpeed);
+ telemetry.addData("Wheel Speeds L : R", "%5.2f : %5.2f", leftSpeed, rightSpeed);
+ telemetry.update();
+ }
+
+ /**
+ * read the Robot heading directly from the IMU (in degrees)
+ */
+ public double getHeading() {
+ YawPitchRollAngles orientation = imu.getRobotYawPitchRollAngles();
+ return orientation.getYaw(AngleUnit.DEGREES);
+ }
+}
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/PushbotAutoDriveByTime_Linear.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveByTime_Linear.java
similarity index 63%
rename from FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/PushbotAutoDriveByTime_Linear.java
rename to FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveByTime_Linear.java
index a5bde9d..a714748 100644
--- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/PushbotAutoDriveByTime_Linear.java
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveByTime_Linear.java
@@ -32,35 +32,36 @@
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 file illustrates the concept of driving a path based on time.
- * It uses the common Pushbot hardware class to define the drive on the robot.
+/*
+ * 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: PushbotAutoDriveByEncoder;
+ * otherwise you would use: RobotAutoDriveByEncoder;
*
* The desired path in this example is:
* - Drive forward for 3 seconds
* - Spin right for 1.3 seconds
- * - Drive Backwards for 1 Second
- * - Stop and close the claw.
+ * - 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 Studios 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
+ * 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="Pushbot: Auto Drive By Time", group="Pushbot")
+@Autonomous(name="Robot: Auto Drive By Time", group="Robot")
@Disabled
-public class PushbotAutoDriveByTime_Linear extends LinearOpMode {
+public class RobotAutoDriveByTime_Linear extends LinearOpMode {
/* Declare OpMode members. */
- HardwarePushbot robot = new HardwarePushbot(); // Use a Pushbot's hardware
+ private DcMotor leftDrive = null;
+ private DcMotor rightDrive = null;
+
private ElapsedTime runtime = new ElapsedTime();
@@ -70,53 +71,55 @@ public class PushbotAutoDriveByTime_Linear extends LinearOpMode {
@Override
public void runOpMode() {
- /*
- * Initialize the drive system variables.
- * The init() method of the hardware class does all the work here
- */
- robot.init(hardwareMap);
+ // 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)
+ // Wait for the game to start (driver presses START)
waitForStart();
- // Step through each leg of the path, ensuring that the Auto mode has not been stopped along the way
+ // Step through each leg of the path, ensuring that the OpMode has not been stopped along the way.
// Step 1: Drive forward for 3 seconds
- robot.leftDrive.setPower(FORWARD_SPEED);
- robot.rightDrive.setPower(FORWARD_SPEED);
+ leftDrive.setPower(FORWARD_SPEED);
+ rightDrive.setPower(FORWARD_SPEED);
runtime.reset();
while (opModeIsActive() && (runtime.seconds() < 3.0)) {
- telemetry.addData("Path", "Leg 1: %2.5f S Elapsed", runtime.seconds());
+ telemetry.addData("Path", "Leg 1: %4.1f S Elapsed", runtime.seconds());
telemetry.update();
}
// Step 2: Spin right for 1.3 seconds
- robot.leftDrive.setPower(TURN_SPEED);
- robot.rightDrive.setPower(-TURN_SPEED);
+ leftDrive.setPower(TURN_SPEED);
+ rightDrive.setPower(-TURN_SPEED);
runtime.reset();
while (opModeIsActive() && (runtime.seconds() < 1.3)) {
- telemetry.addData("Path", "Leg 2: %2.5f S Elapsed", runtime.seconds());
+ telemetry.addData("Path", "Leg 2: %4.1f S Elapsed", runtime.seconds());
telemetry.update();
}
- // Step 3: Drive Backwards for 1 Second
- robot.leftDrive.setPower(-FORWARD_SPEED);
- robot.rightDrive.setPower(-FORWARD_SPEED);
+ // 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: %2.5f S Elapsed", runtime.seconds());
+ telemetry.addData("Path", "Leg 3: %4.1f S Elapsed", runtime.seconds());
telemetry.update();
}
- // Step 4: Stop and close the claw.
- robot.leftDrive.setPower(0);
- robot.rightDrive.setPower(0);
- robot.leftClaw.setPosition(1.0);
- robot.rightClaw.setPosition(0.0);
+ // Step 4: Stop
+ leftDrive.setPower(0);
+ rightDrive.setPower(0);
telemetry.addData("Path", "Complete");
telemetry.update();
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..9bac006
--- /dev/null
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveToAprilTagOmni.java
@@ -0,0 +1,321 @@
+/* Copyright (c) 2023 FIRST. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted (subject to the limitations in the disclaimer below) provided that
+ * the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this list
+ * of conditions and the following disclaimer.
+ *
+ * Redistributions in binary form must reproduce the above copyright notice, this
+ * list of conditions and the following disclaimer in the documentation and/or
+ * other materials provided with the distribution.
+ *
+ * Neither the name of FIRST nor the names of its contributors may be used to endorse or
+ * promote products derived from this software without specific prior written permission.
+ *
+ * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
+ * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+ * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+ * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+package org.firstinspires.ftc.robotcontroller.external.samples;
+
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.util.Range;
+import org.firstinspires.ftc.robotcore.external.hardware.camera.BuiltinCameraDirection;
+import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
+import org.firstinspires.ftc.robotcore.external.hardware.camera.controls.ExposureControl;
+import org.firstinspires.ftc.robotcore.external.hardware.camera.controls.GainControl;
+import org.firstinspires.ftc.vision.VisionPortal;
+import org.firstinspires.ftc.vision.apriltag.AprilTagDetection;
+import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor;
+
+import java.util.List;
+import java.util.concurrent.TimeUnit;
+
+/*
+ * This OpMode illustrates using a camera to locate and drive towards a specific AprilTag.
+ * The code assumes a Holonomic (Mecanum or X Drive) Robot.
+ *
+ * For an introduction to AprilTags, see the ftc-docs link below:
+ * https://ftc-docs.firstinspires.org/en/latest/apriltag/vision_portal/apriltag_intro/apriltag-intro.html
+ *
+ * When an AprilTag in the TagLibrary is detected, the SDK provides location and orientation of the tag, relative to the camera.
+ * This information is provided in the "ftcPose" member of the returned "detection", and is explained in the ftc-docs page linked below.
+ * https://ftc-docs.firstinspires.org/apriltag-detection-values
+ *
+ * The drive goal is to rotate to keep the Tag centered in the camera, while strafing to be directly in front of the tag, and
+ * driving towards the tag to achieve the desired distance.
+ * To reduce any motion blur (which will interrupt the detection process) the Camera exposure is reduced to a very low value (5mS)
+ * You can determine the best Exposure and Gain values by using the ConceptAprilTagOptimizeExposure OpMode in this Samples folder.
+ *
+ * The code assumes a Robot Configuration with motors named: 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.
+ *
+ * Under manual control, the left stick will move forward/back & left/right. The right stick will rotate the robot.
+ * Manually drive the robot until it displays Target data on the Driver Station.
+ *
+ * Press and hold the *Left Bumper* to enable the automatic "Drive to target" mode.
+ * Release the Left Bumper to return to manual driving mode.
+ *
+ * Under "Drive To Target" mode, the robot has three goals:
+ * 1) Turn the robot to always keep the Tag centered on the camera frame. (Use the Target Bearing to turn the robot.)
+ * 2) Strafe the robot towards the centerline of the Tag, so it approaches directly in front of the tag. (Use the Target Yaw to strafe the robot)
+ * 3) Drive towards the Tag to get to the desired distance. (Use Tag Range to drive the robot forward/backward)
+ *
+ * Use DESIRED_DISTANCE to set how close you want the robot to get to the target.
+ * Speed and Turn sensitivity can be adjusted using the SPEED_GAIN, STRAFE_GAIN and TURN_GAIN constants.
+ *
+ * Use Android Studio to Copy this Class, and Paste it into the TeamCode/src/main/java/org/firstinspires/ftc/teamcode folder.
+ * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list.
+ *
+ */
+
+@TeleOp(name="Omni Drive To AprilTag", group = "Concept")
+@Disabled
+public class RobotAutoDriveToAprilTagOmni extends LinearOpMode
+{
+ // Adjust these numbers to suit your robot.
+ final double DESIRED_DISTANCE = 12.0; // this is how close the camera should get to the target (inches)
+
+ // Set the GAIN constants to control the relationship between the measured position error, and how much power is
+ // applied to the drive motors to correct the error.
+ // Drive = Error * Gain Make these values smaller for smoother control, or larger for a more aggressive response.
+ final double SPEED_GAIN = 0.02 ; // Forward Speed Control "Gain". e.g. Ramp up to 50% power at a 25 inch error. (0.50 / 25.0)
+ final double STRAFE_GAIN = 0.015 ; // Strafe Speed Control "Gain". e.g. Ramp up to 37% power at a 25 degree Yaw error. (0.375 / 25.0)
+ final double TURN_GAIN = 0.01 ; // Turn Control "Gain". e.g. Ramp up to 25% power at a 25 degree error. (0.25 / 25.0)
+
+ final double MAX_AUTO_SPEED = 0.5; // Clip the approach speed to this max value (adjust for your robot)
+ final double MAX_AUTO_STRAFE= 0.5; // Clip the strafing speed to this max value (adjust for your robot)
+ final double MAX_AUTO_TURN = 0.3; // Clip the turn speed to this max value (adjust for your robot)
+
+ private DcMotor 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 = -1; // Choose the tag you want to approach or set to -1 for ANY tag.
+ private VisionPortal visionPortal; // Used to manage the video source.
+ private AprilTagProcessor aprilTag; // Used for managing the AprilTag detection process.
+ private AprilTagDetection desiredTag = null; // Used to hold the data for a detected AprilTag
+
+ @Override public void runOpMode()
+ {
+ boolean targetFound = false; // Set to true when an AprilTag target is detected
+ double drive = 0; // Desired forward power/speed (-1 to +1)
+ double strafe = 0; // Desired strafe power/speed (-1 to +1)
+ double turn = 0; // Desired turning power/speed (-1 to +1)
+
+ // Initialize the Apriltag Detection process
+ initAprilTag();
+
+ // Initialize the hardware variables. Note that the strings used here as parameters
+ // to 'get' must match the names assigned during the robot configuration.
+ // step (using the FTC Robot Controller app on the phone).
+ 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 START to start OpMode");
+ telemetry.update();
+ waitForStart();
+
+ while (opModeIsActive())
+ {
+ targetFound = false;
+ desiredTag = null;
+
+ // Step through the list of detected tags and look for a matching tag
+ List currentDetections = aprilTag.getDetections();
+ for (AprilTagDetection detection : currentDetections) {
+ // Look to see if we have size info on this tag.
+ if (detection.metadata != null) {
+ // Check to see if we want to track towards this tag.
+ if ((DESIRED_TAG_ID < 0) || (detection.id == DESIRED_TAG_ID)) {
+ // Yes, we want to use this tag.
+ targetFound = true;
+ desiredTag = detection;
+ break; // don't look any further.
+ } else {
+ // This tag is in the library, but we do not want to track it right now.
+ telemetry.addData("Skipping", "Tag ID %d is not desired", detection.id);
+ }
+ } else {
+ // This tag is NOT in the library, so we don't have enough information to track to it.
+ telemetry.addData("Unknown", "Tag ID %d is not in TagLibrary", detection.id);
+ }
+ }
+
+ // Tell the driver what we see, and what to do.
+ if (targetFound) {
+ telemetry.addData("\n>","HOLD Left-Bumper to Drive to Target\n");
+ telemetry.addData("Found", "ID %d (%s)", desiredTag.id, desiredTag.metadata.name);
+ telemetry.addData("Range", "%5.1f inches", desiredTag.ftcPose.range);
+ telemetry.addData("Bearing","%3.0f degrees", desiredTag.ftcPose.bearing);
+ telemetry.addData("Yaw","%3.0f degrees", desiredTag.ftcPose.yaw);
+ } else {
+ telemetry.addData("\n>","Drive using joysticks to find valid target\n");
+ }
+
+ // If Left Bumper is being pressed, AND we have found the desired target, Drive to target Automatically .
+ if (gamepad1.left_bumper && targetFound) {
+
+ // Determine heading, range and Yaw (tag image rotation) error so we can use them to control the robot automatically.
+ double rangeError = (desiredTag.ftcPose.range - DESIRED_DISTANCE);
+ double headingError = desiredTag.ftcPose.bearing;
+ double yawError = desiredTag.ftcPose.yaw;
+
+ // Use the speed and turn "gains" to calculate how we want the robot to move.
+ drive = Range.clip(rangeError * SPEED_GAIN, -MAX_AUTO_SPEED, MAX_AUTO_SPEED);
+ turn = Range.clip(headingError * TURN_GAIN, -MAX_AUTO_TURN, MAX_AUTO_TURN) ;
+ strafe = Range.clip(-yawError * STRAFE_GAIN, -MAX_AUTO_STRAFE, MAX_AUTO_STRAFE);
+
+ telemetry.addData("Auto","Drive %5.2f, Strafe %5.2f, Turn %5.2f ", drive, strafe, turn);
+ } else {
+
+ // drive using manual POV Joystick mode. Slow things down to make the robot more controlable.
+ drive = -gamepad1.left_stick_y / 2.0; // Reduce drive rate to 50%.
+ strafe = -gamepad1.left_stick_x / 2.0; // Reduce strafe rate to 50%.
+ turn = -gamepad1.right_stick_x / 3.0; // Reduce turn rate to 33%.
+ telemetry.addData("Manual","Drive %5.2f, Strafe %5.2f, Turn %5.2f ", drive, strafe, turn);
+ }
+ telemetry.update();
+
+ // Apply desired axes motions to the drivetrain.
+ moveRobot(drive, strafe, turn);
+ sleep(10);
+ }
+ }
+
+ /**
+ * Move robot according to desired axes motions
+ *
+ * Positive X is forward
+ *
+ * Positive Y is strafe left
+ *
+ * Positive Yaw is counter-clockwise
+ */
+ public void moveRobot(double x, double y, double yaw) {
+ // Calculate wheel powers.
+ double 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();
+
+ // Adjust Image Decimation to trade-off detection-range for detection-rate.
+ // e.g. Some typical detection data using a Logitech C920 WebCam
+ // Decimation = 1 .. Detect 2" Tag from 10 feet away at 10 Frames per second
+ // Decimation = 2 .. Detect 2" Tag from 6 feet away at 22 Frames per second
+ // Decimation = 3 .. Detect 2" Tag from 4 feet away at 30 Frames Per Second
+ // Decimation = 3 .. Detect 5" Tag from 10 feet away at 30 Frames Per Second
+ // Note: Decimation can be changed on-the-fly to adapt during a match.
+ aprilTag.setDecimation(2);
+
+ // Create the vision portal by using a builder.
+ if (USE_WEBCAM) {
+ visionPortal = new VisionPortal.Builder()
+ .setCamera(hardwareMap.get(WebcamName.class, "Webcam 1"))
+ .addProcessor(aprilTag)
+ .build();
+ } else {
+ visionPortal = new VisionPortal.Builder()
+ .setCamera(BuiltinCameraDirection.BACK)
+ .addProcessor(aprilTag)
+ .build();
+ }
+ }
+
+ /*
+ Manually set the camera gain and exposure.
+ This can only be called AFTER calling initAprilTag(), and only works for Webcams;
+ */
+ private void setManualExposure(int exposureMS, int gain) {
+ // Wait for the camera to be open, then use the controls
+
+ if (visionPortal == null) {
+ return;
+ }
+
+ // Make sure camera is streaming before we try to set the exposure controls
+ if (visionPortal.getCameraState() != VisionPortal.CameraState.STREAMING) {
+ telemetry.addData("Camera", "Waiting");
+ telemetry.update();
+ while (!isStopRequested() && (visionPortal.getCameraState() != VisionPortal.CameraState.STREAMING)) {
+ sleep(20);
+ }
+ telemetry.addData("Camera", "Ready");
+ telemetry.update();
+ }
+
+ // Set camera controls unless we are stopping.
+ if (!isStopRequested())
+ {
+ ExposureControl exposureControl = visionPortal.getCameraControl(ExposureControl.class);
+ if (exposureControl.getMode() != ExposureControl.Mode.Manual) {
+ exposureControl.setMode(ExposureControl.Mode.Manual);
+ sleep(50);
+ }
+ exposureControl.setExposure((long)exposureMS, TimeUnit.MILLISECONDS);
+ sleep(20);
+ GainControl gainControl = visionPortal.getCameraControl(GainControl.class);
+ gainControl.setGain(gain);
+ sleep(20);
+ }
+ }
+}
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveToAprilTagTank.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveToAprilTagTank.java
new file mode 100644
index 0000000..ba3eb4f
--- /dev/null
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveToAprilTagTank.java
@@ -0,0 +1,298 @@
+/* Copyright (c) 2023 FIRST. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted (subject to the limitations in the disclaimer below) provided that
+ * the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this list
+ * of conditions and the following disclaimer.
+ *
+ * Redistributions in binary form must reproduce the above copyright notice, this
+ * list of conditions and the following disclaimer in the documentation and/or
+ * other materials provided with the distribution.
+ *
+ * Neither the name of FIRST nor the names of its contributors may be used to endorse or
+ * promote products derived from this software without specific prior written permission.
+ *
+ * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
+ * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+ * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+ * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+package org.firstinspires.ftc.robotcontroller.external.samples;
+
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.util.Range;
+import org.firstinspires.ftc.robotcore.external.hardware.camera.BuiltinCameraDirection;
+import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
+import org.firstinspires.ftc.robotcore.external.hardware.camera.controls.ExposureControl;
+import org.firstinspires.ftc.robotcore.external.hardware.camera.controls.GainControl;
+import org.firstinspires.ftc.vision.VisionPortal;
+import org.firstinspires.ftc.vision.apriltag.AprilTagDetection;
+import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor;
+
+import java.util.List;
+import java.util.concurrent.TimeUnit;
+
+/*
+ * This OpMode illustrates using a camera to locate and drive towards a specific AprilTag.
+ * The code assumes a basic two-wheel (Tank) Robot Drivetrain
+ *
+ * For an introduction to AprilTags, see the ftc-docs link below:
+ * https://ftc-docs.firstinspires.org/en/latest/apriltag/vision_portal/apriltag_intro/apriltag-intro.html
+ *
+ * When an AprilTag in the TagLibrary is detected, the SDK provides location and orientation of the tag, relative to the camera.
+ * This information is provided in the "ftcPose" member of the returned "detection", and is explained in the ftc-docs page linked below.
+ * https://ftc-docs.firstinspires.org/apriltag-detection-values
+ *
+ * The driving goal is to rotate to keep the tag centered in the camera, while driving towards the tag to achieve the desired distance.
+ * To reduce any motion blur (which will interrupt the detection process) the Camera exposure is reduced to a very low value (5mS)
+ * You can determine the best exposure and gain values by using the ConceptAprilTagOptimizeExposure OpMode in this Samples folder.
+ *
+ * The code assumes a Robot Configuration with motors named left_drive and right_drive.
+ * The motor directions must be set so a positive power goes forward on both wheels;
+ * This sample assumes that the default AprilTag Library (usually for the current season) is being loaded by default
+ * so you should choose to approach a valid tag ID.
+ *
+ * Under manual control, the left stick will move forward/back, and the right stick will rotate the robot.
+ * This is called POV Joystick mode, different than Tank Drive (where each joystick controls a wheel).
+ *
+ * Manually drive the robot until it displays Target data on the Driver Station.
+ * Press and hold the *Left Bumper* to enable the automatic "Drive to target" mode.
+ * Release the Left Bumper to return to manual driving mode.
+ *
+ * Under "Drive To Target" mode, the robot has two goals:
+ * 1) Turn the robot to always keep the Tag centered on the camera frame. (Use the Target Bearing to turn the robot.)
+ * 2) Drive towards the Tag to get to the desired distance. (Use Tag Range to drive the robot forward/backward)
+ *
+ * Use DESIRED_DISTANCE to set how close you want the robot to get to the target.
+ * Speed and Turn sensitivity can be adjusted using the SPEED_GAIN and TURN_GAIN constants.
+ *
+ * Use Android Studio to Copy this Class, and Paste it into the TeamCode/src/main/java/org/firstinspires/ftc/teamcode folder.
+ * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list.
+ *
+ */
+
+@TeleOp(name="Tank Drive To AprilTag", group = "Concept")
+@Disabled
+public class RobotAutoDriveToAprilTagTank extends LinearOpMode
+{
+ // Adjust these numbers to suit your robot.
+ final double DESIRED_DISTANCE = 12.0; // this is how close the camera should get to the target (inches)
+
+ // Set the GAIN constants to control the relationship between the measured position error, and how much power is
+ // applied to the drive motors to correct the error.
+ // Drive = Error * Gain Make these values smaller for smoother control, or larger for a more aggressive response.
+ final double SPEED_GAIN = 0.02 ; // Speed Control "Gain". e.g. Ramp up to 50% power at a 25 inch error. (0.50 / 25.0)
+ final double TURN_GAIN = 0.01 ; // Turn Control "Gain". e.g. Ramp up to 25% power at a 25 degree error. (0.25 / 25.0)
+
+ final double MAX_AUTO_SPEED = 0.5; // Clip the approach speed to this max value (adjust for your robot)
+ final double MAX_AUTO_TURN = 0.25; // Clip the turn speed to this max value (adjust for your robot)
+
+ private DcMotor leftDrive = null; // Used to control the left drive wheel
+ private DcMotor rightDrive = null; // Used to control the right drive wheel
+
+ private static final boolean USE_WEBCAM = true; // Set true to use a webcam, or false for a phone camera
+ private static final int DESIRED_TAG_ID = -1; // Choose the tag you want to approach or set to -1 for ANY tag.
+ private VisionPortal visionPortal; // Used to manage the video source.
+ private AprilTagProcessor aprilTag; // Used for managing the AprilTag detection process.
+ private AprilTagDetection desiredTag = null; // Used to hold the data for a detected AprilTag
+
+ @Override public void runOpMode()
+ {
+ boolean targetFound = false; // Set to true when an AprilTag target is detected
+ double drive = 0; // Desired forward power/speed (-1 to +1) +ve is forward
+ double turn = 0; // Desired turning power/speed (-1 to +1) +ve is CounterClockwise
+
+ // Initialize the Apriltag Detection process
+ initAprilTag();
+
+ // Initialize the hardware variables. Note that the strings used here as parameters
+ // to 'get' must match the names assigned during the robot configuration.
+ // step (using the FTC Robot Controller app on the phone).
+ leftDrive = hardwareMap.get(DcMotor.class, "left_drive");
+ rightDrive = hardwareMap.get(DcMotor.class, "right_drive");
+
+ // To drive forward, most robots need the motor on one side to be reversed because the axles point in opposite directions.
+ // When run, this OpMode should start both motors driving forward. So adjust these two lines based on your first test drive.
+ // Note: The settings here assume direct drive on left and right wheels. Single Gear Reduction or 90 Deg drives may require direction flips
+ leftDrive.setDirection(DcMotor.Direction.REVERSE);
+ rightDrive.setDirection(DcMotor.Direction.FORWARD);
+
+ if (USE_WEBCAM)
+ setManualExposure(6, 250); // Use low exposure time to reduce motion blur
+
+ // Wait for the driver to press Start
+ telemetry.addData("Camera preview on/off", "3 dots, Camera Stream");
+ telemetry.addData(">", "Touch START to start OpMode");
+ telemetry.update();
+ waitForStart();
+
+ while (opModeIsActive())
+ {
+ targetFound = false;
+ desiredTag = null;
+
+ // Step through the list of detected tags and look for a matching tag
+ List currentDetections = aprilTag.getDetections();
+ for (AprilTagDetection detection : currentDetections) {
+ // Look to see if we have size info on this tag.
+ if (detection.metadata != null) {
+ // Check to see if we want to track towards this tag.
+ if ((DESIRED_TAG_ID < 0) || (detection.id == DESIRED_TAG_ID)) {
+ // Yes, we want to use this tag.
+ targetFound = true;
+ desiredTag = detection;
+ break; // don't look any further.
+ } else {
+ // This tag is in the library, but we do not want to track it right now.
+ telemetry.addData("Skipping", "Tag ID %d is not desired", detection.id);
+ }
+ } else {
+ // This tag is NOT in the library, so we don't have enough information to track to it.
+ telemetry.addData("Unknown", "Tag ID %d is not in TagLibrary", detection.id);
+ }
+ }
+
+ // Tell the driver what we see, and what to do.
+ if (targetFound) {
+ telemetry.addData("\n>","HOLD Left-Bumper to Drive to Target\n");
+ telemetry.addData("Found", "ID %d (%s)", desiredTag.id, desiredTag.metadata.name);
+ telemetry.addData("Range", "%5.1f inches", desiredTag.ftcPose.range);
+ telemetry.addData("Bearing","%3.0f degrees", desiredTag.ftcPose.bearing);
+ } else {
+ telemetry.addData("\n>","Drive using joysticks to find valid target\n");
+ }
+
+ // If Left Bumper is being pressed, AND we have found the desired target, Drive to target Automatically .
+ if (gamepad1.left_bumper && targetFound) {
+
+ // Determine heading and range error so we can use them to control the robot automatically.
+ double rangeError = (desiredTag.ftcPose.range - DESIRED_DISTANCE);
+ double headingError = desiredTag.ftcPose.bearing;
+
+ // Use the speed and turn "gains" to calculate how we want the robot to move. Clip it to the maximum
+ drive = Range.clip(rangeError * SPEED_GAIN, -MAX_AUTO_SPEED, MAX_AUTO_SPEED);
+ turn = Range.clip(headingError * TURN_GAIN, -MAX_AUTO_TURN, MAX_AUTO_TURN) ;
+
+ telemetry.addData("Auto","Drive %5.2f, Turn %5.2f", drive, turn);
+ } else {
+
+ // drive using manual POV Joystick mode.
+ drive = -gamepad1.left_stick_y / 2.0; // Reduce drive rate to 50%.
+ turn = -gamepad1.right_stick_x / 4.0; // Reduce turn rate to 25%.
+ telemetry.addData("Manual","Drive %5.2f, Turn %5.2f", drive, turn);
+ }
+ telemetry.update();
+
+ // Apply desired axes motions to the drivetrain.
+ moveRobot(drive, turn);
+ sleep(10);
+ }
+ }
+
+ /**
+ * Move robot according to desired axes motions
+ *
+ * Positive X is forward
+ *
+ * Positive Yaw is counter-clockwise
+ */
+ public void moveRobot(double x, double yaw) {
+ // Calculate left and right wheel powers.
+ double leftPower = x - yaw;
+ double rightPower = x + yaw;
+
+ // Normalize wheel powers to be less than 1.0
+ double max = Math.max(Math.abs(leftPower), Math.abs(rightPower));
+ if (max >1.0) {
+ leftPower /= max;
+ rightPower /= max;
+ }
+
+ // Send powers to the wheels.
+ leftDrive.setPower(leftPower);
+ rightDrive.setPower(rightPower);
+ }
+
+ /**
+ * Initialize the AprilTag processor.
+ */
+ private void initAprilTag() {
+ // Create the AprilTag processor by using a builder.
+ aprilTag = new AprilTagProcessor.Builder().build();
+
+ // Adjust Image Decimation to trade-off detection-range for detection-rate.
+ // e.g. Some typical detection data using a Logitech C920 WebCam
+ // Decimation = 1 .. Detect 2" Tag from 10 feet away at 10 Frames per second
+ // Decimation = 2 .. Detect 2" Tag from 6 feet away at 22 Frames per second
+ // Decimation = 3 .. Detect 2" Tag from 4 feet away at 30 Frames Per Second
+ // Decimation = 3 .. Detect 5" Tag from 10 feet away at 30 Frames Per Second
+ // Note: Decimation can be changed on-the-fly to adapt during a match.
+ aprilTag.setDecimation(2);
+
+ // Create the vision portal by using a builder.
+ if (USE_WEBCAM) {
+ visionPortal = new VisionPortal.Builder()
+ .setCamera(hardwareMap.get(WebcamName.class, "Webcam 1"))
+ .addProcessor(aprilTag)
+ .build();
+ } else {
+ visionPortal = new VisionPortal.Builder()
+ .setCamera(BuiltinCameraDirection.BACK)
+ .addProcessor(aprilTag)
+ .build();
+ }
+ }
+
+ /*
+ Manually set the camera gain and exposure.
+ This can only be called AFTER calling initAprilTag(), and only works for Webcams;
+ */
+ private void setManualExposure(int exposureMS, int gain) {
+ // Wait for the camera to be open, then use the controls
+
+ if (visionPortal == null) {
+ return;
+ }
+
+ // Make sure camera is streaming before we try to set the exposure controls
+ if (visionPortal.getCameraState() != VisionPortal.CameraState.STREAMING) {
+ telemetry.addData("Camera", "Waiting");
+ telemetry.update();
+ while (!isStopRequested() && (visionPortal.getCameraState() != VisionPortal.CameraState.STREAMING)) {
+ sleep(20);
+ }
+ telemetry.addData("Camera", "Ready");
+ telemetry.update();
+ }
+
+ // Set camera controls unless we are stopping.
+ if (!isStopRequested())
+ {
+ ExposureControl exposureControl = visionPortal.getCameraControl(ExposureControl.class);
+ if (exposureControl.getMode() != ExposureControl.Mode.Manual) {
+ exposureControl.setMode(ExposureControl.Mode.Manual);
+ sleep(50);
+ }
+ exposureControl.setExposure((long)exposureMS, TimeUnit.MILLISECONDS);
+ sleep(20);
+ GainControl gainControl = visionPortal.getCameraControl(GainControl.class);
+ gainControl.setGain(gain);
+ sleep(20);
+ telemetry.addData("Camera", "Ready");
+ telemetry.update();
+ }
+ }
+}
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveToLine_Linear.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveToLine_Linear.java
new file mode 100644
index 0000000..780f260
--- /dev/null
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveToLine_Linear.java
@@ -0,0 +1,142 @@
+/* Copyright (c) 2017 FIRST. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted (subject to the limitations in the disclaimer below) provided that
+ * the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this list
+ * of conditions and the following disclaimer.
+ *
+ * Redistributions in binary form must reproduce the above copyright notice, this
+ * list of conditions and the following disclaimer in the documentation and/or
+ * other materials provided with the distribution.
+ *
+ * Neither the name of FIRST nor the names of its contributors may be used to endorse or
+ * promote products derived from this software without specific prior written permission.
+ *
+ * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
+ * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+ * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+ * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+package org.firstinspires.ftc.robotcontroller.external.samples;
+
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.hardware.DcMotor;
+import com.qualcomm.robotcore.hardware.NormalizedColorSensor;
+import com.qualcomm.robotcore.hardware.NormalizedRGBA;
+import com.qualcomm.robotcore.hardware.SwitchableLight;
+
+/*
+ * This OpMode illustrates the concept of driving up to a line and then stopping.
+ * The code is structured as a LinearOpMode
+ *
+ * The Sensor used here can be a REV Color Sensor V2 or V3. Make sure the white LED is turned on.
+ * The sensor can be plugged into any I2C port, and must be named "sensor_color" in the active configuration.
+ *
+ * Depending on the height of your color sensor, you may want to set the sensor "gain".
+ * The higher the gain, the greater the reflected light reading will be.
+ * Use the SensorColor sample in this folder to determine the minimum gain value that provides an
+ * "Alpha" reading of 1.0 when you are on top of the white line. In this sample, we use a gain of 15
+ * which works well with a Rev V2 color sensor
+ *
+ * Setting the correct WHITE_THRESHOLD value is key to stopping correctly.
+ * This should be set halfway between the bare-tile, and white-line "Alpha" values.
+ * The reflected light value can be read on the screen once the OpMode has been INIT, but before it is STARTED.
+ * Move the sensor on and off the white line and note the min and max readings.
+ * Edit this code to make WHITE_THRESHOLD halfway between the min and max.
+ *
+ * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name.
+ * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list
+ */
+
+@Autonomous(name="Robot: Auto Drive To Line", group="Robot")
+@Disabled
+public class RobotAutoDriveToLine_Linear extends LinearOpMode {
+
+ /* Declare OpMode members. */
+ private DcMotor leftDrive = null;
+ private DcMotor rightDrive = null;
+
+ /** The variable to store a reference to our color sensor hardware object */
+ NormalizedColorSensor colorSensor;
+
+ static final double WHITE_THRESHOLD = 0.5; // spans between 0.0 - 1.0 from dark to light
+ static final double APPROACH_SPEED = 0.25;
+
+ @Override
+ public void runOpMode() {
+
+ // Initialize the drive system variables.
+ leftDrive = hardwareMap.get(DcMotor.class, "left_drive");
+ rightDrive = hardwareMap.get(DcMotor.class, "right_drive");
+
+ // To drive forward, most robots need the motor on one side to be reversed, because the axles point in opposite directions.
+ // When run, this OpMode should start both motors driving forward. So adjust these two lines based on your first test drive.
+ // Note: The settings here assume direct drive on left and right wheels. Gear Reduction or 90 Deg drives may require direction flips
+ leftDrive.setDirection(DcMotor.Direction.REVERSE);
+ rightDrive.setDirection(DcMotor.Direction.FORWARD);
+
+ // If there are encoders connected, switch to RUN_USING_ENCODER mode for greater accuracy
+ // leftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+ // rightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
+
+ // Get a reference to our sensor object. It's recommended to use NormalizedColorSensor over
+ // ColorSensor, because NormalizedColorSensor consistently gives values between 0 and 1, while
+ // the values you get from ColorSensor are dependent on the specific sensor you're using.
+ colorSensor = hardwareMap.get(NormalizedColorSensor.class, "sensor_color");
+
+ // If necessary, turn ON the white LED (if there is no LED switch on the sensor)
+ if (colorSensor instanceof SwitchableLight) {
+ ((SwitchableLight)colorSensor).enableLight(true);
+ }
+
+ // Some sensors allow you to set your light sensor gain for optimal sensitivity...
+ // See the SensorColor sample in this folder for how to determine the optimal gain.
+ // A gain of 15 causes a Rev Color Sensor V2 to produce an Alpha value of 1.0 at about 1.5" above the floor.
+ colorSensor.setGain(15);
+
+ // Wait for driver to press START)
+ // Abort this loop is started or stopped.
+ while (opModeInInit()) {
+
+ // Send telemetry message to signify robot waiting;
+ telemetry.addData("Status", "Ready to drive to white line."); //
+
+ // Display the light level while we are waiting to start
+ getBrightness();
+ }
+
+ // Start the robot moving forward, and then begin looking for a white line.
+ leftDrive.setPower(APPROACH_SPEED);
+ rightDrive.setPower(APPROACH_SPEED);
+
+ // run until the white line is seen OR the driver presses STOP;
+ while (opModeIsActive() && (getBrightness() < WHITE_THRESHOLD)) {
+ sleep(5);
+ }
+
+ // Stop all motors
+ leftDrive.setPower(0);
+ rightDrive.setPower(0);
+ }
+
+ // to obtain reflected light, read the normalized values from the color sensor. Return the Alpha channel.
+ double getBrightness() {
+ NormalizedRGBA colors = colorSensor.getNormalizedColors();
+ telemetry.addData("Light Level (0 to 1)", "%4.2f", colors.alpha);
+ telemetry.update();
+
+ return colors.alpha;
+ }
+}
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/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/PushbotTeleopPOV_Linear.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotTeleopPOV_Linear.java
similarity index 61%
rename from FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/PushbotTeleopPOV_Linear.java
rename to FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotTeleopPOV_Linear.java
index cc168d8..af3840d 100644
--- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/PushbotTeleopPOV_Linear.java
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotTeleopPOV_Linear.java
@@ -32,30 +32,39 @@
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 uses the common Pushbot hardware class to define the devices on the robot.
- * All device access is managed through the HardwarePushbot class.
+/*
+ * This OpMode executes a POV Game style Teleop for a direct drive robot
* The code is structured as a LinearOpMode
*
- * This particular OpMode executes a POV Game style Teleop for a PushBot
* In this mode the left stick moves the robot FWD and back, the Right stick turns left and right.
- * It raises and lowers the claw using the Gampad Y and A buttons respectively.
+ * 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 Studios 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
+ * 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="Pushbot: Teleop POV", group="Pushbot")
+@TeleOp(name="Robot: Teleop POV", group="Robot")
@Disabled
-public class PushbotTeleopPOV_Linear extends LinearOpMode {
+public class RobotTeleopPOV_Linear extends LinearOpMode {
/* Declare OpMode members. */
- HardwarePushbot robot = new HardwarePushbot(); // Use a Pushbot's hardware
- double clawOffset = 0; // Servo mid position
- final double CLAW_SPEED = 0.02 ; // sets rate to move servo
+ 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() {
@@ -65,22 +74,38 @@ public void runOpMode() {
double turn;
double max;
- /* Initialize the hardware variables.
- * The init() method of the hardware class does all the work here
- */
- robot.init(hardwareMap);
+ // 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("Say", "Hello Driver"); //
+ telemetry.addData(">", "Robot Ready. Press START."); //
telemetry.update();
- // Wait for the game to start (driver presses PLAY)
+ // Wait for the game to start (driver presses START)
waitForStart();
// run until the end of the match (driver presses STOP)
while (opModeIsActive()) {
- // Run wheels in POV mode (note: The joystick goes negative when pushed forwards, so negate it)
+ // 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;
@@ -99,8 +124,8 @@ public void runOpMode() {
}
// Output the safe vales to the motor drives.
- robot.leftDrive.setPower(left);
- robot.rightDrive.setPower(right);
+ leftDrive.setPower(left);
+ rightDrive.setPower(right);
// Use gamepad left & right Bumpers to open and close the claw
if (gamepad1.right_bumper)
@@ -110,16 +135,16 @@ else if (gamepad1.left_bumper)
// Move both servos to new position. Assume servos are mirror image of each other.
clawOffset = Range.clip(clawOffset, -0.5, 0.5);
- robot.leftClaw.setPosition(robot.MID_SERVO + clawOffset);
- robot.rightClaw.setPosition(robot.MID_SERVO - clawOffset);
+ leftClaw.setPosition(MID_SERVO + clawOffset);
+ rightClaw.setPosition(MID_SERVO - clawOffset);
// Use gamepad buttons to move arm up (Y) and down (A)
if (gamepad1.y)
- robot.leftArm.setPower(robot.ARM_UP_POWER);
+ leftArm.setPower(ARM_UP_POWER);
else if (gamepad1.a)
- robot.leftArm.setPower(robot.ARM_DOWN_POWER);
+ leftArm.setPower(ARM_DOWN_POWER);
else
- robot.leftArm.setPower(0.0);
+ leftArm.setPower(0.0);
// Send telemetry message to signify robot running;
telemetry.addData("claw", "Offset = %.2f", clawOffset);
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/PushbotTeleopTank_Iterative.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotTeleopTank_Iterative.java
similarity index 54%
rename from FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/PushbotTeleopTank_Iterative.java
rename to FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotTeleopTank_Iterative.java
index ec336a8..a622f27 100644
--- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/PushbotTeleopTank_Iterative.java
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotTeleopTank_Iterative.java
@@ -32,74 +32,99 @@
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 file provides basic Telop driving for a Pushbot robot.
+/*
+ * This OpMode executes a Tank Drive control TeleOp a direct drive robot
* The code is structured as an Iterative OpMode
*
- * This OpMode uses the common Pushbot hardware class to define the devices on the robot.
- * All device access is managed through the HardwarePushbot class.
- *
- * This particular OpMode executes a basic Tank Drive Teleop for a PushBot
- * It raises and lowers the claw using the Gampad Y and A buttons respectively.
+ * 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 Studios 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
+ * 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="Pushbot: Teleop Tank", group="Pushbot")
+@TeleOp(name="Robot: Teleop Tank", group="Robot")
@Disabled
-public class PushbotTeleopTank_Iterative extends OpMode{
+public class RobotTeleopTank_Iterative extends OpMode{
/* Declare OpMode members. */
- HardwarePushbot robot = new HardwarePushbot(); // use the class created to define a Pushbot's hardware
- double clawOffset = 0.0 ; // Servo mid position
- final double CLAW_SPEED = 0.02 ; // sets rate to move servo
+ 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() {
- /* Initialize the hardware variables.
- * The init() method of the hardware class does all the work here
- */
- robot.init(hardwareMap);
+ // 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("Say", "Hello Driver"); //
+ telemetry.addData(">", "Robot Ready. Press START."); //
}
/*
- * Code to run REPEATEDLY after the driver hits INIT, but before they hit PLAY
+ * Code to run REPEATEDLY after the driver hits INIT, but before they hit START
*/
@Override
public void init_loop() {
}
/*
- * Code to run ONCE when the driver hits PLAY
+ * Code to run ONCE when the driver hits START
*/
@Override
public void start() {
}
/*
- * Code to run REPEATEDLY after the driver hits PLAY but before they hit STOP
+ * Code to run REPEATEDLY after the driver hits START but before they hit STOP
*/
@Override
public void loop() {
double left;
double right;
- // Run wheels in tank mode (note: The joystick goes negative when pushed forwards, so negate it)
+ // 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;
- robot.leftDrive.setPower(left);
- robot.rightDrive.setPower(right);
+ leftDrive.setPower(left);
+ rightDrive.setPower(right);
// Use gamepad left & right Bumpers to open and close the claw
if (gamepad1.right_bumper)
@@ -109,16 +134,16 @@ else if (gamepad1.left_bumper)
// Move both servos to new position. Assume servos are mirror image of each other.
clawOffset = Range.clip(clawOffset, -0.5, 0.5);
- robot.leftClaw.setPosition(robot.MID_SERVO + clawOffset);
- robot.rightClaw.setPosition(robot.MID_SERVO - clawOffset);
+ 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)
- robot.leftArm.setPower(robot.ARM_UP_POWER);
+ leftArm.setPower(ARM_UP_POWER);
else if (gamepad1.a)
- robot.leftArm.setPower(robot.ARM_DOWN_POWER);
+ leftArm.setPower(ARM_DOWN_POWER);
else
- robot.leftArm.setPower(0.0);
+ leftArm.setPower(0.0);
// Send telemetry message to signify robot running;
telemetry.addData("claw", "Offset = %.2f", clawOffset);
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
index 337b703..bcf5b80 100644
--- 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
@@ -33,14 +33,13 @@
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;
/*
- * Display patterns of a REV Robotics Blinkin LED Driver.
+ * 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.
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorAdafruitRGB.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorAdafruitRGB.java
deleted file mode 100644
index 6a94e25..0000000
--- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorAdafruitRGB.java
+++ /dev/null
@@ -1,167 +0,0 @@
-/* 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;
-import com.qualcomm.robotcore.hardware.DeviceInterfaceModule;
-import com.qualcomm.robotcore.hardware.DigitalChannel;
-
-/*
- *
- * This is an example LinearOpMode that shows how to use
- * the Adafruit RGB Sensor. It assumes that the I2C
- * cable for the sensor is connected to an I2C port on the
- * Core Device Interface Module.
- *
- * It also assuems that the LED pin of the sensor is connected
- * to the digital signal pin of a digital port on the
- * Core Device Interface Module.
- *
- * You can use the digital port to turn the sensor's onboard
- * LED on or off.
- *
- * The op mode assumes that the Core Device Interface Module
- * is configured with a name of "dim" and that the Adafruit color sensor
- * is configured as an I2C device with a name of "sensor_color".
- *
- * It also assumes that the LED pin of the RGB sensor
- * is connected to the signal pin of digital port #5 (zero indexed)
- * of the Core Device Interface Module.
- *
- * 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: AdafruitRGB", group = "Sensor")
-@Disabled // Comment this out to add to the opmode list
-public class SensorAdafruitRGB extends LinearOpMode {
-
- ColorSensor sensorRGB;
- DeviceInterfaceModule cdim;
-
- // we assume that the LED pin of the RGB sensor is connected to
- // digital port 5 (zero indexed).
- static final int LED_CHANNEL = 5;
-
- @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 DeviceInterfaceModule object.
- cdim = hardwareMap.deviceInterfaceModule.get("dim");
-
- // set the digital channel to output mode.
- // remember, the Adafruit sensor is actually two devices.
- // It's an I2C sensor and it's also an LED that can be turned on or off.
- cdim.setDigitalChannelMode(LED_CHANNEL, DigitalChannel.Mode.OUTPUT);
-
- // get a reference to our ColorSensor object.
- sensorRGB = hardwareMap.colorSensor.get("sensor_color");
-
- // turn the LED on in the beginning, just so user will know that the sensor is active.
- cdim.setDigitalChannelState(LED_CHANNEL, bLedOn);
-
- // wait for the start button to be pressed.
- waitForStart();
-
- // 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 gamepad.
- bCurrState = gamepad1.x;
-
- // check for button-press state transitions.
- if ((bCurrState == true) && (bCurrState != bPrevState)) {
-
- // button is transitioning to a pressed state. Toggle the LED.
- bLedOn = !bLedOn;
- cdim.setDigitalChannelState(LED_CHANNEL, bLedOn);
- }
-
- // update previous state variable.
- bPrevState = bCurrState;
-
- // convert the RGB values to HSV values.
- Color.RGBToHSV((sensorRGB.red() * 255) / 800, (sensorRGB.green() * 255) / 800, (sensorRGB.blue() * 255) / 800, hsvValues);
-
- // send the info back to driver station using telemetry function.
- telemetry.addData("LED", bLedOn ? "On" : "Off");
- telemetry.addData("Clear", sensorRGB.alpha());
- telemetry.addData("Red ", sensorRGB.red());
- telemetry.addData("Green", sensorRGB.green());
- telemetry.addData("Blue ", sensorRGB.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/SensorBNO055IMU.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorBNO055IMU.java
index 9102a10..405da1e 100644
--- 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
@@ -34,7 +34,6 @@
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;
@@ -46,16 +45,19 @@
import java.util.Locale;
-/**
- * {@link SensorBNO055IMU} gives a short demo on how to use the BNO055 Inertial Motion Unit (IMU) from AdaFruit.
+/*
+ * 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
+ * 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
+@Disabled // Comment this out to add to the OpMode list
public class SensorBNO055IMU extends LinearOpMode
{
//----------------------------------------------------------------------------------------------
@@ -81,7 +83,7 @@ public class SensorBNO055IMU extends LinearOpMode
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.calibrationDataFile = "BNO055IMUCalibration.json"; // see the calibration sample OpMode
parameters.loggingEnabled = true;
parameters.loggingTag = "IMU";
parameters.accelerationIntegrationAlgorithm = new JustLoggingAccelerationIntegrator();
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
index 064e8a4..93f1789 100644
--- 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
@@ -29,13 +29,11 @@
package org.firstinspires.ftc.robotcontroller.external.samples;
-import com.qualcomm.hardware.adafruit.AdafruitBNO055IMU;
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;
@@ -46,60 +44,60 @@
import java.io.File;
import java.util.Locale;
-/**
- * {@link SensorBNO055IMUCalibration} calibrates the IMU accelerometer per
+/*
+ * This OpMode calibrates a BNO055 IMU per
* "Section 3.11 Calibration" of the BNO055 specification.
*
- *
Manual calibration of the IMU is definitely NOT necessary: except for the magnetometer
- * (which is not used by the default {@link BNO055IMU.SensorMode#IMU
- * SensorMode#IMU}), 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.
+ * 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:
+ * 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
+ * "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:
+ * but in essence:
*
- * There is a calibration status register available [...] that returns the calibration status
+ * 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):
+ * datasheet for more information):
*
- *
- * GYR: Simply let the sensor sit flat for a few seconds.
- * 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.
- * MAG: Move slowly in a figure 8 pattern in the air, until the calibration values reaches 3.
- * 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."
- *
+ * 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.
+ * 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.
+ * 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 exmaple,
+ * 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.
+ * magnetometer cannot actually be calibrated.
*
- * @see AdafruitBNO055IMU
- * @see BNO055IMU.Parameters#calibrationDataFile
- * @see BNO055 product page
- * @see BNO055 specification
+ * 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
+@Disabled // Uncomment this to add to the OpMode list
public class SensorBNO055IMUCalibration extends LinearOpMode
{
//----------------------------------------------------------------------------------------------
@@ -122,7 +120,7 @@ public class SensorBNO055IMUCalibration extends LinearOpMode
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("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");
@@ -156,7 +154,7 @@ public class SensorBNO055IMUCalibration extends LinearOpMode
// 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
+ // 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";
@@ -229,4 +227,4 @@ String formatAngle(AngleUnit angleUnit, double angle) {
String formatDegrees(double degrees){
return String.format(Locale.getDefault(), "%.1f", AngleUnit.DEGREES.normalize(degrees));
}
-}
\ No newline at end of file
+}
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
index 8209b83..7546c9d 100644
--- 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
@@ -32,7 +32,6 @@
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;
@@ -40,12 +39,11 @@
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 is an example LinearOpMode that shows how to use a color sensor in a generic
- * way, regardless of which particular make or model of color sensor is used. The Op Mode
+/*
+ * 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.
@@ -64,7 +62,7 @@
* 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 Op Mode to the Driver Station OpMode list
+ * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list
*/
@TeleOp(name = "Sensor: Color", group = "Sensor")
@Disabled
@@ -78,13 +76,13 @@ public class SensorColor extends LinearOpMode {
* 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 Op Mode, as it is in all LinearOpModes.
+ /*
+ * 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
- * Op Mode runs, and we want to be able to *guarantee* that we restore it to something reasonable
- * and palatable when the Op Mode ends. The simplest way to do that is to use a try...finally
+ * 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.
*/
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorDIO.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorDIO.java
deleted file mode 100644
index 12d5d93..0000000
--- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorDIO.java
+++ /dev/null
@@ -1,107 +0,0 @@
-/* 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.DeviceInterfaceModule;
-import com.qualcomm.robotcore.hardware.DigitalChannel;
-
-/*
- * This is an example LinearOpMode that shows how to use the digital inputs and outputs on the
- * the Modern Robotics Device Interface Module. In addition, it shows how to use the Red and Blue LED
- *
- * This op mode assumes that there is a Device Interface Module attached, named 'dim'.
- * On this DIM there is a digital input named 'digin' and an output named 'digout'
- *
- * To fully exercise this sample, connect pin 3 of the digin connector to pin 3 of the digout.
- * Note: Pin 1 is indicated by the black stripe, so pin 3 is at the opposite end.
- *
- * The X button on the gamepad will be used to activate the digital output pin.
- * The Red/Blue LED will be used to indicate the state of the digital input pin.
- * Blue = false (0V), Red = true (5V)
- * If the two pins are linked, the gamepad will change the LED color.
- *
- * 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: DIM DIO", group = "Sensor")
-@Disabled
-public class SensorDIO extends LinearOpMode {
-
-final int BLUE_LED_CHANNEL = 0;
-final int RED_LED_CHANNEL = 1;
-
- @Override
- public void runOpMode() {
-
- boolean inputPin; // Input State
- boolean outputPin; // Output State
- DeviceInterfaceModule dim; // Device Object
- DigitalChannel digIn; // Device Object
- DigitalChannel digOut; // Device Object
-
- // get a reference to a Modern Robotics DIM, and IO channels.
- dim = hardwareMap.get(DeviceInterfaceModule.class, "dim"); // Use generic form of device mapping
- digIn = hardwareMap.get(DigitalChannel.class, "digin"); // Use generic form of device mapping
- digOut = hardwareMap.get(DigitalChannel.class, "digout"); // Use generic form of device mapping
-
- digIn.setMode(DigitalChannel.Mode.INPUT); // Set the direction of each channel
- digOut.setMode(DigitalChannel.Mode.OUTPUT);
-
- // wait for the start button to be pressed.
- telemetry.addData(">", "Press play, and then user X button to set DigOut");
- telemetry.update();
- waitForStart();
-
- while (opModeIsActive()) {
-
- outputPin = gamepad1.x ; // Set the output pin based on x button
- digOut.setState(outputPin);
- inputPin = digIn.getState(); // Read the input pin
-
- // Display input pin state on LEDs
- if (inputPin) {
- dim.setLED(RED_LED_CHANNEL, true);
- dim.setLED(BLUE_LED_CHANNEL, false);
- }
- else {
- dim.setLED(RED_LED_CHANNEL, false);
- dim.setLED(BLUE_LED_CHANNEL, true);
- }
-
- telemetry.addData("Output", outputPin );
- telemetry.addData("Input", inputPin );
- telemetry.addData("LED", inputPin ? "Red" : "Blue" );
- telemetry.update();
- }
- }
-}
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorDigitalTouch.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorDigitalTouch.java
index b367924..44c3ca9 100644
--- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorDigitalTouch.java
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorDigitalTouch.java
@@ -1,4 +1,4 @@
-/* Copyright (c) 2017 FIRST. All rights reserved.
+/* Copyright (c) 2024 FIRST. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted (subject to the limitations in the disclaimer below) provided that
@@ -35,51 +35,41 @@
import com.qualcomm.robotcore.hardware.DigitalChannel;
/*
- * This is an example LinearOpMode that shows how to use
- * a REV Robotics Touch Sensor.
+ * This OpMode demonstrates how to use a digital channel.
*
- * It assumes that the touch sensor is configured with a name of "sensor_digital".
+ * The OpMode assumes that the digital channel is configured with a name of "digitalTouch".
*
* Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name.
- * Remove or comment out the @Disabled line to add this opmode to the Driver Station OpMode list.
+ * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list.
*/
-@TeleOp(name = "Sensor: Digital touch", group = "Sensor")
+@TeleOp(name = "Sensor: digital channel", group = "Sensor")
@Disabled
public class SensorDigitalTouch extends LinearOpMode {
- /**
- * The REV Robotics Touch Sensor
- * is treated as a digital channel. It is HIGH if the button is unpressed.
- * It pulls LOW if the button is pressed.
- *
- * Also, when you connect a REV Robotics Touch Sensor to the digital I/O port on the
- * Expansion Hub using a 4-wire JST cable, the second pin gets connected to the Touch Sensor.
- * The lower (first) pin stays unconnected.*
- */
-
- DigitalChannel digitalTouch; // Hardware Device Object
+ DigitalChannel digitalTouch; // Digital channel Object
@Override
public void runOpMode() {
- // get a reference to our digitalTouch object.
- digitalTouch = hardwareMap.get(DigitalChannel.class, "sensor_digital");
+ // get a reference to our touchSensor object.
+ digitalTouch = hardwareMap.get(DigitalChannel.class, "digitalTouch");
- // set the digital channel to input.
digitalTouch.setMode(DigitalChannel.Mode.INPUT);
+ telemetry.addData("DigitalTouchSensorExample", "Press start to continue...");
+ telemetry.update();
// wait for the start button to be pressed.
waitForStart();
- // while the op mode is active, loop and read the light levels.
+ // while the OpMode is active, loop and read the digital channel.
// Note we use opModeIsActive() as our loop condition because it is an interruptible method.
while (opModeIsActive()) {
+ // button is pressed if value returned is LOW or false.
// send the info back to driver station using telemetry function.
- // if the digital channel returns true it's HIGH and the button is unpressed.
- if (digitalTouch.getState() == true) {
- telemetry.addData("Digital Touch", "Is Not Pressed");
+ if (digitalTouch.getState() == false) {
+ telemetry.addData("Button", "PRESSED");
} else {
- telemetry.addData("Digital Touch", "Is Pressed");
+ telemetry.addData("Button", "NOT PRESSED");
}
telemetry.update();
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorHuskyLens.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorHuskyLens.java
new file mode 100644
index 0000000..af7ca55
--- /dev/null
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorHuskyLens.java
@@ -0,0 +1,160 @@
+/*
+Copyright (c) 2023 FIRST
+
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without modification,
+are permitted (subject to the limitations in the disclaimer below) provided that
+the following conditions are met:
+
+Redistributions of source code must retain the above copyright notice, this list
+of conditions and the following disclaimer.
+
+Redistributions in binary form must reproduce the above copyright notice, this
+list of conditions and the following disclaimer in the documentation and/or
+other materials provided with the distribution.
+
+Neither the name of FIRST nor the names of its contributors may be used to
+endorse or promote products derived from this software without specific prior
+written permission.
+
+NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
+LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR
+TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
+THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+package org.firstinspires.ftc.robotcontroller.external.samples;
+
+import com.qualcomm.hardware.dfrobot.HuskyLens;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import org.firstinspires.ftc.robotcore.internal.system.Deadline;
+
+import java.util.concurrent.TimeUnit;
+
+/*
+ * This OpMode illustrates how to use the DFRobot HuskyLens.
+ *
+ * The HuskyLens is a Vision Sensor with a built-in object detection model. It can
+ * detect a number of predefined objects and AprilTags in the 36h11 family, can
+ * recognize colors, and can be trained to detect custom objects. See this website for
+ * documentation: https://wiki.dfrobot.com/HUSKYLENS_V1.0_SKU_SEN0305_SEN0336
+ *
+ * For detailed instructions on how a HuskyLens is used in FTC, please see this tutorial:
+ * https://ftc-docs.firstinspires.org/en/latest/devices/huskylens/huskylens.html
+ *
+ * This sample illustrates how to detect AprilTags, but can be used to detect other types
+ * of objects by changing the algorithm. It assumes that the HuskyLens is configured with
+ * a name of "huskylens".
+ *
+ * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name.
+ * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list
+ */
+@TeleOp(name = "Sensor: HuskyLens", group = "Sensor")
+@Disabled
+public class SensorHuskyLens extends LinearOpMode {
+
+ private final int READ_PERIOD = 1;
+
+ private HuskyLens huskyLens;
+
+ @Override
+ public void runOpMode()
+ {
+ huskyLens = hardwareMap.get(HuskyLens.class, "huskylens");
+
+ /*
+ * This sample rate limits the reads solely to allow a user time to observe
+ * what is happening on the Driver Station telemetry. Typical applications
+ * would not likely rate limit.
+ */
+ Deadline rateLimit = new Deadline(READ_PERIOD, TimeUnit.SECONDS);
+
+ /*
+ * Immediately expire so that the first time through we'll do the read.
+ */
+ rateLimit.expire();
+
+ /*
+ * Basic check to see if the device is alive and communicating. This is not
+ * technically necessary here as the HuskyLens class does this in its
+ * doInitialization() method which is called when the device is pulled out of
+ * the hardware map. However, sometimes it's unclear why a device reports as
+ * failing on initialization. In the case of this device, it's because the
+ * call to knock() failed.
+ */
+ if (!huskyLens.knock()) {
+ telemetry.addData(">>", "Problem communicating with " + huskyLens.getDeviceName());
+ } else {
+ telemetry.addData(">>", "Press start to continue");
+ }
+
+ /*
+ * The device uses the concept of an algorithm to determine what types of
+ * objects it will look for and/or what mode it is in. The algorithm may be
+ * selected using the scroll wheel on the device, or via software as shown in
+ * the call to selectAlgorithm().
+ *
+ * The SDK itself does not assume that the user wants a particular algorithm on
+ * startup, and hence does not set an algorithm.
+ *
+ * Users, should, in general, explicitly choose the algorithm they want to use
+ * within the OpMode by calling selectAlgorithm() and passing it one of the values
+ * found in the enumeration HuskyLens.Algorithm.
+ *
+ * Other algorithm choices for FTC might be: OBJECT_RECOGNITION, COLOR_RECOGNITION or OBJECT_CLASSIFICATION.
+ */
+ huskyLens.selectAlgorithm(HuskyLens.Algorithm.TAG_RECOGNITION);
+
+ telemetry.update();
+ waitForStart();
+
+ /*
+ * Looking for AprilTags per the call to selectAlgorithm() above. A handy grid
+ * for testing may be found at https://wiki.dfrobot.com/HUSKYLENS_V1.0_SKU_SEN0305_SEN0336#target_20.
+ *
+ * Note again that the device only recognizes the 36h11 family of tags out of the box.
+ */
+ while(opModeIsActive()) {
+ if (!rateLimit.hasExpired()) {
+ continue;
+ }
+ rateLimit.reset();
+
+ /*
+ * All algorithms, except for LINE_TRACKING, return a list of Blocks where a
+ * Block represents the outline of a recognized object along with its ID number.
+ * ID numbers allow you to identify what the device saw. See the HuskyLens documentation
+ * referenced in the header comment above for more information on IDs and how to
+ * assign them to objects.
+ *
+ * Returns an empty array if no objects are seen.
+ */
+ HuskyLens.Block[] blocks = huskyLens.blocks();
+ telemetry.addData("Block count", blocks.length);
+ for (int i = 0; i < blocks.length; i++) {
+ telemetry.addData("Block", blocks[i].toString());
+ /*
+ * Here inside the FOR loop, you could save or evaluate specific info for the currently recognized Bounding Box:
+ * - blocks[i].width and blocks[i].height (size of box, in pixels)
+ * - blocks[i].left and blocks[i].top (edges of box)
+ * - blocks[i].x and blocks[i].y (center location)
+ * - blocks[i].id (Color ID)
+ *
+ * These values have Java type int (integer).
+ */
+ }
+
+ telemetry.update();
+ }
+ }
+}
\ No newline at end of file
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorIMUNonOrthogonal.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorIMUNonOrthogonal.java
new file mode 100644
index 0000000..70bc8d4
--- /dev/null
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorIMUNonOrthogonal.java
@@ -0,0 +1,184 @@
+/* Copyright (c) 2022 FIRST. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted (subject to the limitations in the disclaimer below) provided that
+ * the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this list
+ * of conditions and the following disclaimer.
+ *
+ * Redistributions in binary form must reproduce the above copyright notice, this
+ * list of conditions and the following disclaimer in the documentation and/or
+ * other materials provided with the distribution.
+ *
+ * Neither the name of FIRST nor the names of its contributors may be used to endorse or
+ * promote products derived from this software without specific prior written permission.
+ *
+ * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
+ * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+ * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+ * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+package org.firstinspires.ftc.robotcontroller.external.samples;
+
+import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.hardware.IMU;
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+import org.firstinspires.ftc.robotcore.external.navigation.AngularVelocity;
+import org.firstinspires.ftc.robotcore.external.navigation.Orientation;
+import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles;
+
+import static com.qualcomm.hardware.rev.RevHubOrientationOnRobot.xyzOrientation;
+
+/*
+ * This OpMode shows how to use the new universal IMU interface. This
+ * interface may be used with the BNO055 IMU or the BHI260 IMU. It assumes that an IMU is configured
+ * on the robot with the name "imu".
+ *
+ * The sample will display the current Yaw, Pitch and Roll of the robot.
+ * With the correct orientation parameters selected, pitch/roll/yaw should act as follows:
+ * Pitch value should INCREASE as the robot is tipped UP at the front. (Rotation about X)
+ * Roll value should INCREASE as the robot is tipped UP at the left side. (Rotation about Y)
+ * Yaw value should INCREASE as the robot is rotated Counter Clockwise. (Rotation about Z)
+ *
+ * The yaw can be reset (to zero) by pressing the Y button on the gamepad (Triangle on a PS4 controller)
+ *
+ * This specific sample DOES NOT assume that the Hub is mounted on one of the three orthogonal
+ * planes (X/Y, X/Z or Y/Z) OR that the Hub has only been rotated in a range of 90 degree increments.
+ *
+ * Note: if your Hub is mounted Orthogonally (on a orthogonal surface, angled at some multiple of
+ * 90 Degrees) then you should use the simpler SensorIMUOrthogonal sample in this folder.
+ *
+ * But... If your Hub is mounted Non-Orthogonally, you must specify one or more rotational angles
+ * that transform a "Default" Hub orientation into your desired orientation. That is what is
+ * illustrated here.
+ *
+ * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name.
+ * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list.
+ *
+ * Finally, edit this OpMode to use at least one angle around an axis to orient your Hub.
+ */
+@TeleOp(name = "Sensor: IMU Non-Orthogonal", group = "Sensor")
+@Disabled // Comment this out to add to the OpMode list
+public class SensorIMUNonOrthogonal extends LinearOpMode
+{
+ // The IMU sensor object
+ IMU imu;
+
+ //----------------------------------------------------------------------------------------------
+ // Main logic
+ //----------------------------------------------------------------------------------------------
+
+ @Override public void runOpMode() throws InterruptedException {
+
+ // Retrieve and initialize the IMU.
+ // This sample expects the IMU to be in a REV Hub and named "imu".
+ imu = hardwareMap.get(IMU.class, "imu");
+
+ /* Define how the hub is mounted to the robot to get the correct Yaw, Pitch and Roll values.
+ *
+ * You can apply up to three axis rotations to orient your Hub according to how it's mounted on the robot.
+ *
+ * The starting point for these rotations is the "Default" Hub orientation, which is:
+ * 1) Hub laying flat on a horizontal surface, with the Printed Logo facing UP
+ * 2) Rotated such that the USB ports are facing forward on the robot.
+ *
+ * If you are using a REV External IMU, the "Default" orientation is the same as for a REV Hub, but instead of
+ * the USB ports facing forward, the I2C port faces forward.
+ *
+ * The order that the rotations are performed matters, so this sample shows doing them in the order X, Y, then Z.
+ * For specifying non-orthogonal hub mounting orientations, we must temporarily use axes
+ * defined relative to the Hub itself, instead of the usual Robot Coordinate System axes
+ * used for the results the IMU gives us. In the starting orientation, the Hub axes are
+ * aligned with the Robot Coordinate System:
+ *
+ * X Axis: Starting at Center of Hub, pointing out towards I2C connectors
+ * Y Axis: Starting at Center of Hub, pointing out towards USB connectors
+ * Z Axis: Starting at Center of Hub, pointing Up through LOGO
+ *
+ * Positive rotation is defined by right-hand rule with thumb pointing in +ve direction on axis.
+ *
+ * Some examples.
+ *
+ * ----------------------------------------------------------------------------------------------------------------------------------
+ * Example A) Assume that the hub is mounted on a sloped plate at the back of the robot, with the USB ports coming out the top of the hub.
+ * The plate is tilted UP 60 degrees from horizontal.
+ *
+ * To get the "Default" hub into this configuration you would just need a single rotation.
+ * 1) Rotate the Hub +60 degrees around the X axis to tilt up the front edge.
+ * 2) No rotation around the Y or Z axes.
+ *
+ * So the X,Y,Z rotations would be 60,0,0
+ *
+ * ----------------------------------------------------------------------------------------------------------------------------------
+ * Example B) Assume that the hub is laying flat on the chassis, but it has been twisted 30 degrees towards the right front wheel to make
+ * the USB cable accessible.
+ *
+ * To get the "Default" hub into this configuration you would just need a single rotation, but around a different axis.
+ * 1) No rotation around the X or Y axes.
+ * 2) Rotate the Hub -30 degrees (Clockwise) around the Z axis, since a positive angle would be Counter Clockwise.
+ *
+ * So the X,Y,Z rotations would be 0,0,-30
+ *
+ * ----------------------------------------------------------------------------------------------------------------------------------
+ * Example C) Assume that the hub is mounted on a vertical plate on the right side of the robot, with the Logo facing out, and the
+ * Hub rotated so that the USB ports are facing down 30 degrees towards the back wheels of the robot.
+ *
+ * To get the "Default" hub into this configuration will require several rotations.
+ * 1) Rotate the hub +90 degrees around the X axis to get it standing upright with the logo pointing backwards on the robot
+ * 2) Next, rotate the hub +90 around the Y axis to get it facing to the right.
+ * 3) Finally rotate the hub +120 degrees around the Z axis to take the USB ports from vertical to sloping down 30 degrees and
+ * facing towards the back of the robot.
+ *
+ * So the X,Y,Z rotations would be 90,90,120
+ */
+
+ // The next three lines define the desired axis rotations.
+ // To Do: EDIT these values to match YOUR mounting configuration.
+ double xRotation = 0; // enter the desired X rotation angle here.
+ double yRotation = 0; // enter the desired Y rotation angle here.
+ double zRotation = 0; // enter the desired Z rotation angle here.
+
+ Orientation hubRotation = xyzOrientation(xRotation, yRotation, zRotation);
+
+ // Now initialize the IMU with this mounting orientation
+ RevHubOrientationOnRobot orientationOnRobot = new RevHubOrientationOnRobot(hubRotation);
+ imu.initialize(new IMU.Parameters(orientationOnRobot));
+
+ // Loop and update the dashboard
+ while (!isStopRequested()) {
+ telemetry.addData("Hub orientation", "X=%.1f, Y=%.1f, Z=%.1f \n", xRotation, yRotation, zRotation);
+
+ // Check to see if heading reset is requested
+ if (gamepad1.y) {
+ telemetry.addData("Yaw", "Resetting\n");
+ imu.resetYaw();
+ } else {
+ telemetry.addData("Yaw", "Press Y (triangle) on Gamepad to reset\n");
+ }
+
+ // Retrieve Rotational Angles and Velocities
+ YawPitchRollAngles orientation = imu.getRobotYawPitchRollAngles();
+ AngularVelocity angularVelocity = imu.getRobotAngularVelocity(AngleUnit.DEGREES);
+
+ telemetry.addData("Yaw (Z)", "%.2f Deg. (Heading)", orientation.getYaw(AngleUnit.DEGREES));
+ telemetry.addData("Pitch (X)", "%.2f Deg.", orientation.getPitch(AngleUnit.DEGREES));
+ telemetry.addData("Roll (Y)", "%.2f Deg.\n", orientation.getRoll(AngleUnit.DEGREES));
+ telemetry.addData("Yaw (Z) velocity", "%.2f Deg/Sec", angularVelocity.zRotationRate);
+ telemetry.addData("Pitch (X) velocity", "%.2f Deg/Sec", angularVelocity.xRotationRate);
+ telemetry.addData("Roll (Y) velocity", "%.2f Deg/Sec", angularVelocity.yRotationRate);
+ telemetry.update();
+ }
+ }
+}
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorIMUOrthogonal.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorIMUOrthogonal.java
new file mode 100644
index 0000000..af4c202
--- /dev/null
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorIMUOrthogonal.java
@@ -0,0 +1,146 @@
+/* Copyright (c) 2022 FIRST. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted (subject to the limitations in the disclaimer below) provided that
+ * the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this list
+ * of conditions and the following disclaimer.
+ *
+ * Redistributions in binary form must reproduce the above copyright notice, this
+ * list of conditions and the following disclaimer in the documentation and/or
+ * other materials provided with the distribution.
+ *
+ * Neither the name of FIRST nor the names of its contributors may be used to endorse or
+ * promote products derived from this software without specific prior written permission.
+ *
+ * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
+ * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+ * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+ * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+package org.firstinspires.ftc.robotcontroller.external.samples;
+
+import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.hardware.IMU;
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+import org.firstinspires.ftc.robotcore.external.navigation.AngularVelocity;
+import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles;
+
+/*
+ * This OpMode shows how to use the new universal IMU interface. This
+ * interface may be used with the BNO055 IMU or the BHI260 IMU. It assumes that an IMU is configured
+ * on the robot with the name "imu".
+ *
+ * The sample will display the current Yaw, Pitch and Roll of the robot.
+ * With the correct orientation parameters selected, pitch/roll/yaw should act as follows:
+ * Pitch value should INCREASE as the robot is tipped UP at the front. (Rotation about X)
+ * Roll value should INCREASE as the robot is tipped UP at the left side. (Rotation about Y)
+ * Yaw value should INCREASE as the robot is rotated Counter Clockwise. (Rotation about Z)
+ *
+ * The yaw can be reset (to zero) by pressing the Y button on the gamepad (Triangle on a PS4 controller)
+ *
+ * This specific sample assumes that the Hub is mounted on one of the three orthogonal planes
+ * (X/Y, X/Z or Y/Z) and that the Hub has only been rotated in a range of 90 degree increments.
+ *
+ * Note: if your Hub is mounted on a surface angled at some non-90 Degree multiple (like 30) look at
+ * the alternative SensorIMUNonOrthogonal sample in this folder.
+ *
+ * This "Orthogonal" requirement means that:
+ *
+ * 1) The Logo printed on the top of the Hub can ONLY be pointing in one of six directions:
+ * FORWARD, BACKWARD, UP, DOWN, LEFT and RIGHT.
+ *
+ * 2) The USB ports can only be pointing in one of the same six directions:
+ * FORWARD, BACKWARD, UP, DOWN, LEFT and RIGHT.
+ *
+ * So, To fully define how your Hub is mounted to the robot, you must simply specify:
+ * logoFacingDirection
+ * usbFacingDirection
+ *
+ * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name.
+ * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list.
+ *
+ * Finally, choose the two correct parameters to define how your Hub is mounted and edit this OpMode
+ * to use those parameters.
+ */
+@TeleOp(name = "Sensor: IMU Orthogonal", group = "Sensor")
+@Disabled // Comment this out to add to the OpMode list
+public class SensorIMUOrthogonal extends LinearOpMode
+{
+ // The IMU sensor object
+ IMU imu;
+
+ //----------------------------------------------------------------------------------------------
+ // Main logic
+ //----------------------------------------------------------------------------------------------
+
+ @Override public void runOpMode() throws InterruptedException {
+
+ // Retrieve and initialize the IMU.
+ // This sample expects the IMU to be in a REV Hub and named "imu".
+ imu = hardwareMap.get(IMU.class, "imu");
+
+ /* Define how the hub is mounted on the robot to get the correct Yaw, Pitch and Roll values.
+ *
+ * Two input parameters are required to fully specify the Orientation.
+ * The first parameter specifies the direction the printed logo on the Hub is pointing.
+ * The second parameter specifies the direction the USB connector on the Hub is pointing.
+ * All directions are relative to the robot, and left/right is as-viewed from behind the robot.
+ *
+ * If you are using a REV 9-Axis IMU, you can use the Rev9AxisImuOrientationOnRobot class instead of the
+ * RevHubOrientationOnRobot class, which has an I2cPortFacingDirection instead of a UsbFacingDirection.
+ */
+
+ /* The next two lines define Hub orientation.
+ * The Default Orientation (shown) is when a hub is mounted horizontally with the printed logo pointing UP and the USB port pointing FORWARD.
+ *
+ * To Do: EDIT these two lines to match YOUR mounting configuration.
+ */
+ RevHubOrientationOnRobot.LogoFacingDirection logoDirection = RevHubOrientationOnRobot.LogoFacingDirection.UP;
+ RevHubOrientationOnRobot.UsbFacingDirection usbDirection = RevHubOrientationOnRobot.UsbFacingDirection.FORWARD;
+
+ RevHubOrientationOnRobot orientationOnRobot = new RevHubOrientationOnRobot(logoDirection, usbDirection);
+
+ // Now initialize the IMU with this mounting orientation
+ // Note: if you choose two conflicting directions, this initialization will cause a code exception.
+ imu.initialize(new IMU.Parameters(orientationOnRobot));
+
+ // Loop and update the dashboard
+ while (!isStopRequested()) {
+
+ telemetry.addData("Hub orientation", "Logo=%s USB=%s\n ", logoDirection, usbDirection);
+
+ // Check to see if heading reset is requested
+ if (gamepad1.y) {
+ telemetry.addData("Yaw", "Resetting\n");
+ imu.resetYaw();
+ } else {
+ telemetry.addData("Yaw", "Press Y (triangle) on Gamepad to reset\n");
+ }
+
+ // Retrieve Rotational Angles and Velocities
+ YawPitchRollAngles orientation = imu.getRobotYawPitchRollAngles();
+ AngularVelocity angularVelocity = imu.getRobotAngularVelocity(AngleUnit.DEGREES);
+
+ telemetry.addData("Yaw (Z)", "%.2f Deg. (Heading)", orientation.getYaw(AngleUnit.DEGREES));
+ telemetry.addData("Pitch (X)", "%.2f Deg.", orientation.getPitch(AngleUnit.DEGREES));
+ telemetry.addData("Roll (Y)", "%.2f Deg.\n", orientation.getRoll(AngleUnit.DEGREES));
+ telemetry.addData("Yaw (Z) velocity", "%.2f Deg/Sec", angularVelocity.zRotationRate);
+ telemetry.addData("Pitch (X) velocity", "%.2f Deg/Sec", angularVelocity.xRotationRate);
+ telemetry.addData("Roll (Y) velocity", "%.2f Deg/Sec", angularVelocity.yRotationRate);
+ telemetry.update();
+ }
+ }
+}
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorKLNavxMicro.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorKLNavxMicro.java
index 583f525..ccc945f 100644
--- 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
@@ -36,7 +36,6 @@
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;
@@ -44,11 +43,11 @@
import org.firstinspires.ftc.robotcore.external.navigation.Orientation;
/*
- * This is an example LinearOpMode that shows how to use Kauai Labs navX Micro Robotics Navigation
+ * 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
+ * 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
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorLimelight3A.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorLimelight3A.java
new file mode 100644
index 0000000..6c1f702
--- /dev/null
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorLimelight3A.java
@@ -0,0 +1,159 @@
+/*
+Copyright (c) 2024 Limelight Vision
+
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without modification,
+are permitted (subject to the limitations in the disclaimer below) provided that
+the following conditions are met:
+
+Redistributions of source code must retain the above copyright notice, this list
+of conditions and the following disclaimer.
+
+Redistributions in binary form must reproduce the above copyright notice, this
+list of conditions and the following disclaimer in the documentation and/or
+other materials provided with the distribution.
+
+Neither the name of FIRST nor the names of its contributors may be used to
+endorse or promote products derived from this software without specific prior
+written permission.
+
+NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
+LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR
+TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
+THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+package org.firstinspires.ftc.robotcontroller.external.samples;
+
+import com.qualcomm.hardware.limelightvision.LLResult;
+import com.qualcomm.hardware.limelightvision.LLResultTypes;
+import com.qualcomm.hardware.limelightvision.LLStatus;
+import com.qualcomm.hardware.limelightvision.Limelight3A;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import org.firstinspires.ftc.robotcore.external.navigation.Pose3D;
+
+import java.util.List;
+
+/*
+ * This OpMode illustrates how to use the Limelight3A Vision Sensor.
+ *
+ * @see Limelight
+ *
+ * Notes on configuration:
+ *
+ * The device presents itself, when plugged into a USB port on a Control Hub as an ethernet
+ * interface. A DHCP server running on the Limelight automatically assigns the Control Hub an
+ * ip address for the new ethernet interface.
+ *
+ * Since the Limelight is plugged into a USB port, it will be listed on the top level configuration
+ * activity along with the Control Hub Portal and other USB devices such as webcams. Typically
+ * serial numbers are displayed below the device's names. In the case of the Limelight device, the
+ * Control Hub's assigned ip address for that ethernet interface is used as the "serial number".
+ *
+ * Tapping the Limelight's name, transitions to a new screen where the user can rename the Limelight
+ * and specify the Limelight's ip address. Users should take care not to confuse the ip address of
+ * the Limelight itself, which can be configured through the Limelight settings page via a web browser,
+ * and the ip address the Limelight device assigned the Control Hub and which is displayed in small text
+ * below the name of the Limelight on the top level configuration screen.
+ */
+@TeleOp(name = "Sensor: Limelight3A", group = "Sensor")
+@Disabled
+public class SensorLimelight3A extends LinearOpMode {
+
+ private Limelight3A limelight;
+
+ @Override
+ public void runOpMode() throws InterruptedException
+ {
+ limelight = hardwareMap.get(Limelight3A.class, "limelight");
+
+ telemetry.setMsTransmissionInterval(11);
+
+ limelight.pipelineSwitch(0);
+
+ /*
+ * Starts polling for data. If you neglect to call start(), getLatestResult() will return null.
+ */
+ limelight.start();
+
+ telemetry.addData(">", "Robot Ready. Press Play.");
+ telemetry.update();
+ waitForStart();
+
+ while (opModeIsActive()) {
+ LLStatus status = limelight.getStatus();
+ telemetry.addData("Name", "%s",
+ status.getName());
+ telemetry.addData("LL", "Temp: %.1fC, CPU: %.1f%%, FPS: %d",
+ status.getTemp(), status.getCpu(),(int)status.getFps());
+ telemetry.addData("Pipeline", "Index: %d, Type: %s",
+ status.getPipelineIndex(), status.getPipelineType());
+
+ LLResult result = limelight.getLatestResult();
+ if (result != null) {
+ // Access general information
+ Pose3D botpose = result.getBotpose();
+ double captureLatency = result.getCaptureLatency();
+ double targetingLatency = result.getTargetingLatency();
+ double parseLatency = result.getParseLatency();
+ telemetry.addData("LL Latency", captureLatency + targetingLatency);
+ telemetry.addData("Parse Latency", parseLatency);
+ telemetry.addData("PythonOutput", java.util.Arrays.toString(result.getPythonOutput()));
+
+ if (result.isValid()) {
+ telemetry.addData("tx", result.getTx());
+ telemetry.addData("txnc", result.getTxNC());
+ telemetry.addData("ty", result.getTy());
+ telemetry.addData("tync", result.getTyNC());
+
+ telemetry.addData("Botpose", botpose.toString());
+
+ // Access barcode results
+ List barcodeResults = result.getBarcodeResults();
+ for (LLResultTypes.BarcodeResult br : barcodeResults) {
+ telemetry.addData("Barcode", "Data: %s", br.getData());
+ }
+
+ // Access classifier results
+ List classifierResults = result.getClassifierResults();
+ for (LLResultTypes.ClassifierResult cr : classifierResults) {
+ telemetry.addData("Classifier", "Class: %s, Confidence: %.2f", cr.getClassName(), cr.getConfidence());
+ }
+
+ // Access detector results
+ List detectorResults = result.getDetectorResults();
+ for (LLResultTypes.DetectorResult dr : detectorResults) {
+ telemetry.addData("Detector", "Class: %s, Area: %.2f", dr.getClassName(), dr.getTargetArea());
+ }
+
+ // Access fiducial results
+ List fiducialResults = result.getFiducialResults();
+ for (LLResultTypes.FiducialResult fr : fiducialResults) {
+ telemetry.addData("Fiducial", "ID: %d, Family: %s, X: %.2f, Y: %.2f", fr.getFiducialId(), fr.getFamily(),fr.getTargetXDegrees(), fr.getTargetYDegrees());
+ }
+
+ // Access color results
+ List colorResults = result.getColorResults();
+ for (LLResultTypes.ColorResult cr : colorResults) {
+ telemetry.addData("Color", "X: %.2f, Y: %.2f", cr.getTargetXDegrees(), cr.getTargetYDegrees());
+ }
+ }
+ } else {
+ telemetry.addData("Limelight", "No data available");
+ }
+
+ telemetry.update();
+ }
+ limelight.stop();
+ }
+}
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorMRColor.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorMRColor.java
index 513ace8..32b37e0 100644
--- 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
@@ -32,7 +32,6 @@
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;
@@ -40,16 +39,16 @@
/*
*
- * This is an example LinearOpMode that shows how to use
+ * This OpMode that shows how to use
* a Modern Robotics Color Sensor.
*
- * The op mode assumes that the 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
+ * 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
@@ -88,7 +87,7 @@ public void runOpMode() {
// wait for the start button to be pressed.
waitForStart();
- // while the op mode is active, loop and read the RGB data.
+ // 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()) {
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorMRCompass.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorMRCompass.java
deleted file mode 100644
index 583ac1a..0000000
--- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorMRCompass.java
+++ /dev/null
@@ -1,148 +0,0 @@
-/* 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.ModernRoboticsI2cCompassSensor;
-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.CompassSensor;
-import com.qualcomm.robotcore.util.ElapsedTime;
-
-import org.firstinspires.ftc.robotcore.external.navigation.Acceleration;
-
-/**
- * The {@link SensorMRCompass} op mode provides a demonstration of the
- * functionality provided by the Modern Robotics compass sensor.
- *
- * The op mode assumes that the MR compass is configured with a name of "compass".
- *
- * 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 Compass Sensor
- */
-@TeleOp(name = "Sensor: MR compass", group = "Sensor")
-@Disabled // comment out or remove this line to enable this opmode
-public class SensorMRCompass extends LinearOpMode {
-
- ModernRoboticsI2cCompassSensor compass;
- ElapsedTime timer = new ElapsedTime();
-
- @Override public void runOpMode() {
-
- // get a reference to our compass
- compass = hardwareMap.get(ModernRoboticsI2cCompassSensor.class, "compass");
-
- telemetry.log().setCapacity(20);
- telemetry.log().add("The compass sensor operates quite well out-of-the");
- telemetry.log().add("box, as shipped by the manufacturer. Precision can");
- telemetry.log().add("however be somewhat improved with calibration.");
- telemetry.log().add("");
- telemetry.log().add("To calibrate the compass once the opmode is");
- telemetry.log().add("started, make sure the compass is level, then");
- telemetry.log().add("press 'A' on the gamepad. Next, slowly rotate the ");
- telemetry.log().add("compass in a full 360 degree circle while keeping");
- telemetry.log().add("it level. When complete, press 'B'.");
-
- // wait for the start button to be pressed
- waitForStart();
- telemetry.log().clear();
-
- while (opModeIsActive()) {
-
- // If the A button is pressed, start calibration and wait for the A button to rise
- if (gamepad1.a && !compass.isCalibrating()) {
-
- telemetry.log().clear();
- telemetry.log().add("Calibration started");
- telemetry.log().add("Slowly rotate compass 360deg");
- telemetry.log().add("Press 'B' when complete");
- compass.setMode(CompassSensor.CompassMode.CALIBRATION_MODE);
- timer.reset();
-
- while (gamepad1.a && opModeIsActive()) {
- doTelemetry();
- idle();
- }
- }
-
- // If the B button is pressed, stop calibration and wait for the B button to rise
- if (gamepad1.b && compass.isCalibrating()) {
-
- telemetry.log().clear();
- telemetry.log().add("Calibration complete");
- compass.setMode(CompassSensor.CompassMode.MEASUREMENT_MODE);
-
- if (compass.calibrationFailed()) {
- telemetry.log().add("Calibration failed");
- compass.writeCommand(ModernRoboticsI2cCompassSensor.Command.NORMAL);
- }
-
- while (gamepad1.a && opModeIsActive()) {
- doTelemetry();
- idle();
- }
- }
-
- doTelemetry();
- }
- }
-
- protected void doTelemetry() {
-
- if (compass.isCalibrating()) {
-
- telemetry.addData("compass", "calibrating %s", Math.round(timer.seconds())%2==0 ? "|.." : "..|");
-
- } else {
-
- // getDirection() returns a traditional compass heading in the range [0,360),
- // with values increasing in a CW direction
- telemetry.addData("heading", "%.1f", compass.getDirection());
-
- // getAcceleration() returns the current 3D acceleration experienced by
- // the sensor. This is used internally to the sensor to compute its tilt and thence
- // to correct the magnetometer reading to produce tilt-corrected values in getDirection()
- Acceleration accel = compass.getAcceleration();
- double accelMagnitude = Math.sqrt(accel.xAccel*accel.xAccel + accel.yAccel*accel.yAccel + accel.zAccel*accel.zAccel);
- telemetry.addData("accel", accel);
- telemetry.addData("accel magnitude", "%.3f", accelMagnitude);
-
- // getMagneticFlux returns the 3D magnetic field flux experienced by the sensor
- telemetry.addData("mag flux", compass.getMagneticFlux());
- }
-
- // the command register provides status data
- telemetry.addData("command", "%s", compass.readCommand());
-
- telemetry.update();
- }
-}
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
index e8df38f..91c0062 100644
--- 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
@@ -33,29 +33,27 @@
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;
/*
- * This is an example LinearOpMode that shows how to use the Modern Robotics Gyro.
+ * This OpMode shows how to use the Modern Robotics Gyro.
*
- * The op mode assumes that the gyro sensor is attached to a Device Interface Module
+ * 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
+ * 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.
+ /* 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
@@ -83,7 +81,7 @@ public void runOpMode() {
// 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.
+ // during the initialization phase at the start of each OpMode.
telemetry.log().add("Gyro Calibrating. Do Not Move!");
modernRoboticsI2cGyro.calibrate();
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
index 8424ef7..6d2dfa3 100644
--- 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
@@ -35,12 +35,11 @@
import com.qualcomm.robotcore.hardware.OpticalDistanceSensor;
/*
- * This is an example LinearOpMode that shows how to use
- * a Modern Robotics Optical Distance Sensor
+ * 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
+ * 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
@@ -57,7 +56,7 @@ public void runOpMode() {
// wait for the start button to be pressed.
waitForStart();
- // while the op mode is active, loop and read the light levels.
+ // 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()) {
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
index d7de1ff..2039ef9 100644
--- 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
@@ -33,22 +33,20 @@
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;
-/**
- * {@link SensorMRRangeSensor} illustrates how to use the Modern Robotics
- * Range Sensor.
+/*
+ * This OpMode illustrates how to use the Modern Robotics Range Sensor.
*
- * The op mode assumes that the range sensor is configured with a name of "sensor_range".
+ * 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
+ * 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
+@Disabled // comment out or remove this line to enable this OpMode
public class SensorMRRangeSensor extends LinearOpMode {
ModernRoboticsI2cRangeSensor rangeSensor;
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorOctoQuad.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorOctoQuad.java
new file mode 100644
index 0000000..f797c6b
--- /dev/null
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorOctoQuad.java
@@ -0,0 +1,141 @@
+/*
+ * Copyright (c) 2024 DigitalChickenLabs
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+package org.firstinspires.ftc.robotcontroller.external.samples;
+
+import com.qualcomm.hardware.digitalchickenlabs.OctoQuad;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+
+/*
+ * This OpMode illustrates how to use the DigitalChickenLabs OctoQuad Quadrature Encoder & Pulse Width Interface Module
+ *
+ * The OctoQuad has 8 input channels that can used to read either Relative Quadrature Encoders or Pulse-Width Absolute Encoder inputs.
+ * Relative Quadrature encoders are found on most FTC motors, and some stand-alone position sensors like the REV Thru-Bore encoder.
+ * Pulse-Width encoders are less common. The REV Thru-Bore encoder can provide its absolute position via a variable pulse width,
+ * as can several sonar rangefinders such as the MaxBotix MB1000 series.
+ *
+ * This basic sample shows how an OctoQuad can be used to read the position three Odometry pods fitted
+ * with REV Thru-Bore encoders. For a more advanced example showing additional OctoQuad capabilities, see the SensorOctoQuadAdv sample.
+ *
+ * This OpMode assumes that the OctoQuad is attached to an I2C interface named "octoquad" in the robot configuration.
+ *
+ * The code assumes the first three OctoQuad inputs are connected as follows
+ * - Chan 0: for measuring forward motion on the left side of the robot.
+ * - Chan 1: for measuring forward motion on the right side of the robot.
+ * - Chan 2: for measuring Lateral (strafing) motion.
+ *
+ * The encoder values may be reset to zero by pressing the X (left most) button on Gamepad 1.
+ *
+ * This sample does not show how to interpret these readings, just how to obtain and display them.
+ *
+ * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name.
+ * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list
+ *
+ * See the sensor's product page: https://www.tindie.com/products/35114/
+ */
+@TeleOp(name = "OctoQuad Basic", group="OctoQuad")
+@Disabled
+public class SensorOctoQuad extends LinearOpMode {
+
+ // Identify which encoder OctoQuad inputs are connected to each odometry pod.
+ private final int ODO_LEFT = 0; // Facing forward direction on left side of robot (Axial motion)
+ private final int ODO_RIGHT = 1; // Facing forward direction on right side or robot (Axial motion)
+ private final int ODO_PERP = 2; // Facing perpendicular direction at the center of the robot (Lateral motion)
+
+ // Declare the OctoQuad object and members to store encoder positions and velocities
+ private OctoQuad octoquad;
+
+ private int posLeft;
+ private int posRight;
+ private int posPerp;
+
+ /**
+ * This function is executed when this OpMode is selected from the Driver Station.
+ */
+ @Override
+ public void runOpMode() {
+
+ // Connect to OctoQuad by referring to its name in the Robot Configuration.
+ octoquad = hardwareMap.get(OctoQuad.class, "octoquad");
+
+ // Read the Firmware Revision number from the OctoQuad and display it as telemetry.
+ telemetry.addData("OctoQuad Firmware Version ", octoquad.getFirmwareVersion());
+
+ // Reverse the count-direction of any encoder that is not what you require.
+ // e.g. if you push the robot forward and the left encoder counts down, then reverse it so it counts up.
+ octoquad.setSingleEncoderDirection(ODO_LEFT, OctoQuad.EncoderDirection.REVERSE);
+ octoquad.setSingleEncoderDirection(ODO_RIGHT, OctoQuad.EncoderDirection.FORWARD);
+ octoquad.setSingleEncoderDirection(ODO_PERP, OctoQuad.EncoderDirection.FORWARD);
+
+ // Any changes that are made should be saved in FLASH just in case there is a sensor power glitch.
+ octoquad.saveParametersToFlash();
+
+ telemetry.addLine("\nPress START to read encoder values");
+ telemetry.update();
+
+ waitForStart();
+
+ // Configure the telemetry for optimal display of data.
+ telemetry.setDisplayFormat(Telemetry.DisplayFormat.MONOSPACE);
+ telemetry.setMsTransmissionInterval(50);
+
+ // Set all the encoder inputs to zero.
+ octoquad.resetAllPositions();
+
+ // Loop while displaying the odometry pod positions.
+ while (opModeIsActive()) {
+ telemetry.addData(">", "Press X to Reset Encoders\n");
+
+ // Check for X button to reset encoders.
+ if (gamepad1.x) {
+ // Reset the position of all encoders to zero.
+ octoquad.resetAllPositions();
+ }
+
+ // Read all the encoder data. Load into local members.
+ readOdometryPods();
+
+ // Display the values.
+ telemetry.addData("Left ", "%8d counts", posLeft);
+ telemetry.addData("Right", "%8d counts", posRight);
+ telemetry.addData("Perp ", "%8d counts", posPerp);
+ telemetry.update();
+ }
+ }
+
+ private void readOdometryPods() {
+ // For best performance, we should only perform ONE transaction with the OctoQuad each cycle.
+ // Since this example only needs to read positions from a few channels, we could use either
+ // readPositionRange(idxFirst, idxLast) to get a select number of sequential channels
+ // or
+ // readAllPositions() to get all 8 encoder readings
+ //
+ // Since both calls take almost the same amount of time, and the actual channels may not end up
+ // being sequential, we will read all of the encoder positions, and then pick out the ones we need.
+ int[] positions = octoquad.readAllPositions();
+ posLeft = positions[ODO_LEFT];
+ posRight = positions[ODO_RIGHT];
+ posPerp = positions[ODO_PERP];
+ }
+}
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorOctoQuadAdv.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorOctoQuadAdv.java
new file mode 100644
index 0000000..e763b9a
--- /dev/null
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorOctoQuadAdv.java
@@ -0,0 +1,278 @@
+/*
+ * Copyright (c) 2024 DigitalChickenLabs
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+package org.firstinspires.ftc.robotcontroller.external.samples;
+
+import com.qualcomm.hardware.digitalchickenlabs.OctoQuad;
+import com.qualcomm.hardware.digitalchickenlabs.OctoQuadBase;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.util.ElapsedTime;
+import com.qualcomm.robotcore.util.MovingStatistics;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+
+import java.util.ArrayList;
+import java.util.List;
+
+/*
+ * This OpMode illustrates how to use advanced features of the DigitalChickenLabs OctoQuad Quadrature Encoder & Pulse Width Interface Module
+ *
+ * The OctoQuad has 8 input channels that can used to read either Quadrature Encoder signals (like with most FTC motors)
+ * or Pulse-Width style Absolute Encoder inputs (eg: REV Through Bore encoder pulse width output).
+ *
+ * This OpMode illustrates several of the more advanced features of an OctoQuad, including Pulse Width measurement and velocity measurement.
+ * For a more basic OpMode just showing how to read encoder inputs, see the SensorOctoQuad sample.
+ *
+ * This OpMode assumes that the OctoQuad is attached to an I2C interface named "octoquad" in the robot configuration.
+ *
+ * One system that requires a lot of encoder inputs is a Swerve Drive system.
+ * Such a drive requires two encoders per drive module: one for position & velocity of the Drive motor/wheel, and one for position/angle of the Steering motor.
+ * The Drive motor usually requires a quadrature encoder, and the Steering motor requires an Absolute encoder.
+ * This quantity and combination of encoder inputs is a challenge, but a single OctoQuad could be used to read them all.
+ *
+ * This sample will configure an OctoQuad for a swerve drive, as follows
+ * - Configure 4 Relative Quadrature Encoder inputs and 4 Absolute Pulse-Width Encoder inputs
+ * - Configure a velocity sample interval of 25 mSec
+ * - Configure a pulse width input range of 1-1024 uSec for a REV Through Bore Encoder's Absolute Pulse output.
+ *
+ * An OctoSwerveDrive class will be created to initialize the OctoQuad, and manage the 4 swerve modules.
+ * An OctoSwerveModule class will be created to configure and read a single swerve module.
+ *
+ * Wiring:
+ * The OctoQuad will be configured to accept Quadrature encoders on the first four channels and Absolute (pulse width) encoders on the last four channels.
+ *
+ * The standard 4-pin to 4-pin cable can be used to connect each Driver Motor encoder to the OctoQuad. (channels 0-3)
+ *
+ * A modified version of the REV 6-4 pin cable (shipped with the encoder) connects the steering encoder to the OctoQuad. (channels 4-7)
+ * To connect the Absolute position signal from a REV Thru-Bore encoder to the OctoQuad, the Provided 6-pin to 4-pin cable will need to be modified.
+ * At the 6-pin connector end, move the yellow wire from its initial pin-3 position to the ABS pin-5 position. This can be done easily
+ * by using a small flathead screwdriver to lift the small white tab holding the metal wire crimp in place and gently pulling the wire out.
+ * See the OctoSwerveDrive() constructor below for the correct wheel/channel assignment.
+ *
+ * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name.
+ * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list
+ *
+ * Note: If you prefer, you can move the two support classes from this file, and place them in their own files.
+ * But leaving them in place is simpler for this example.
+ *
+ * See the sensor's product page: https://www.tindie.com/products/35114/
+ */
+@TeleOp(name="OctoQuad Advanced", group="OctoQuad")
+@Disabled
+public class SensorOctoQuadAdv extends LinearOpMode {
+
+ @Override
+ public void runOpMode() throws InterruptedException {
+ // Connect to the OctoQuad by looking up its name in the hardwareMap.
+ OctoQuad octoquad = hardwareMap.get(OctoQuad.class, "octoquad");
+
+ // Create the interface for the Swerve Drive Encoders
+ OctoSwerveDrive octoSwerveDrive = new OctoSwerveDrive(octoquad);
+
+ // Display the OctoQuad firmware revision
+ telemetry.addLine("OctoQuad Firmware v" + octoquad.getFirmwareVersion());
+ telemetry.addLine("\nPress START to read encoder values");
+ telemetry.update();
+
+ waitForStart();
+
+ // Configure the telemetry for optimal display of data.
+ telemetry.setDisplayFormat(Telemetry.DisplayFormat.MONOSPACE);
+ telemetry.setMsTransmissionInterval(50);
+
+ // Run stats to determine cycle times.
+ MovingStatistics avgTime = new MovingStatistics(100);
+ ElapsedTime elapsedTime = new ElapsedTime();
+
+ while (opModeIsActive()) {
+ telemetry.addData(">", "Press X to Reset Encoders\n");
+
+ if(gamepad1.x) {
+ octoquad.resetAllPositions();
+ }
+
+ // read all the swerve drive encoders and update positions and velocities.
+ octoSwerveDrive.updateModules();
+ octoSwerveDrive.show(telemetry);
+
+ // Update cycle time stats
+ avgTime.add(elapsedTime.nanoseconds());
+ elapsedTime.reset();
+
+ telemetry.addData("Loop time", "%.1f mS", avgTime.getMean()/1000000);
+ telemetry.update();
+ }
+ }
+}
+
+// ============================ Internal (Inner) Classes =============================
+
+/***
+ * OctoSwerveDrive class manages 4 Swerve Modules
+ * - Performs general OctoQuad initialization
+ * - Creates 4 module classes, one for each swerve module
+ * - Updates swerve drive status by reading all data from OctoQuad and Updating each module
+ * - Displays all swerve drive data as telemetry
+ */
+class OctoSwerveDrive {
+
+ private final OctoQuad octoquad;
+ private final List allModules = new ArrayList<>();
+
+ // members to hold encoder data for each module.
+ public final OctoSwerveModule LeftFront;
+ public final OctoSwerveModule RightFront;
+ public final OctoSwerveModule LeftBack;
+ public final OctoSwerveModule RightBack;
+
+ // Prepare an object to hold an entire OctoQuad encoder readable register bank (pos and vel)
+ private final OctoQuad.EncoderDataBlock encoderDataBlock = new OctoQuad.EncoderDataBlock();
+
+ public OctoSwerveDrive(OctoQuad octoquad) {
+ this.octoquad = octoquad;
+
+ // Clear out all prior settings and encoder data before setting up desired configuration
+ octoquad.resetEverything();
+
+ // Assume first 4 channels are relative encoders and the next 4 are absolute encoders
+ octoquad.setChannelBankConfig(OctoQuad.ChannelBankConfig.BANK1_QUADRATURE_BANK2_PULSE_WIDTH);
+
+ // Create the four OctoSwerveModules, and add them to a 'list' for easier reference.
+
+ // Note: The wheel/channel order is set here. Ensure your encoder cables match.
+ //
+ // Note: The angleOffset must be set for each module so a forward facing wheel shows a steer angle of 0 degrees.
+ // The process for determining the correct angleOffsets is as follows:
+ // 1) Set all the angleValues (below) to zero then build and deploy the code.
+ // 2) Physically rotate all the swerve drives so the wheels are facing forward in the desired 0 degree position
+ // 3) Run the OpMode, view the telemetry and record the "Degrees" value for each module.
+ // 4) Update the code by entering the recorded Degrees value for each module as the angleOffset (last) parameter in the lines below.
+ //
+ // Rebuild and deploy the new code. Verify that the telemetry now indicates 0 degrees when the wheels are facing forward.
+ // Also verify that the correct module values change appropriately when you manually spin (drive) and rotate (steer) a wheel.
+
+ allModules.add(LeftFront = new OctoSwerveModule(octoquad, "LF ",0,0));// Drive = 0, Steer = 4
+ allModules.add(RightFront = new OctoSwerveModule(octoquad, "RF ",1,0));// Drive = 1, Steer = 5
+ allModules.add(LeftBack = new OctoSwerveModule(octoquad, "LB ",2,0));// Drive = 2, Steer = 6
+ allModules.add(RightBack = new OctoSwerveModule(octoquad, "RB ",3,0));// Drive = 3, Steer = 7
+
+ // now make sure the settings persist through any power glitches.
+ octoquad.saveParametersToFlash();
+ }
+
+ /**
+ * Updates all 4 swerve modules
+ */
+ public void updateModules() {
+ // Read full OctoQuad data block and then pass DataBlock to each swerve module to update themselves.
+ octoquad.readAllEncoderData(encoderDataBlock);
+ for (OctoSwerveModule module : allModules) {
+ module.updateModule(encoderDataBlock);
+ }
+ }
+
+ /**
+ * Generate telemetry data for all modules.
+ * @param telemetry OpMode Telemetry object
+ */
+ public void show(Telemetry telemetry) {
+ // create general header block and then have each module add its own telemetry
+ telemetry.addData("pos", " Count CPS Degree DPS");
+ for (OctoSwerveModule module : allModules) {
+ module.show(telemetry);
+ }
+ }
+}
+
+/***
+ * The OctoSwerveModule class manages a single swerve module
+ */
+class OctoSwerveModule {
+
+ public double driveCounts;
+ public double driveCountsPerSec;
+ public double steerDegrees;
+ public double steerDegreesPerSec;
+
+ private final String name;
+ private final int channel;
+ private final double angleOffset;
+ private final double steerDirMult;
+
+ private static final int VELOCITY_SAMPLE_INTERVAL_MS = 25; // To provide 40 updates per second.
+ private static final double DEGREES_PER_US = (360.0 / 1024.0); // based on REV Through Bore Encoder
+ private static final double VELOCITY_SAMPLES_PER_S = (1000.0 / VELOCITY_SAMPLE_INTERVAL_MS);
+
+ // The correct drive and turn directions must be set for the Swerve Module based on the specific hardware geometry.
+ // Forward motion must generate an increasing drive count.
+ // Counter Clockwise steer rotation must generate an increasing Steer Angle (degrees)
+ private static final boolean INVERT_DRIVE_ENCODER = false; // Set true if forward motion decreases drive "Count"
+ private static final boolean INVERT_STEER_ENCODER = false; // Set true if counter clockwise steer action decreases steer "Degree"
+
+ /***
+ * @param octoquad provide access to configure OctoQuad
+ * @param name name used for telemetry display
+ * @param quadChannel Quadrature encoder channel. Pulse Width channel is this + 4
+ * @param angleOffset Angle to subtract from absolute encoder to calibrate zero position. (see comments above)
+ */
+ public OctoSwerveModule (OctoQuad octoquad, String name, int quadChannel, double angleOffset) {
+ this.name = name;
+ this.channel = quadChannel;
+ this.angleOffset = angleOffset;
+ this.steerDirMult = INVERT_STEER_ENCODER ? -1 : 1 ; // create a multiplier to flip the steer angle.
+
+ // Set the drive encoder direction. Note the absolute encoder does not have built-in direction inversion.
+ octoquad.setSingleEncoderDirection(channel, INVERT_DRIVE_ENCODER ? OctoQuad.EncoderDirection.REVERSE : OctoQuad.EncoderDirection.FORWARD);
+
+ // Set the velocity sample interval on both encoders
+ octoquad.setSingleVelocitySampleInterval(channel, VELOCITY_SAMPLE_INTERVAL_MS);
+ octoquad.setSingleVelocitySampleInterval(channel + 4, VELOCITY_SAMPLE_INTERVAL_MS);
+
+ // Setup Absolute encoder pulse range to match REV Through Bore encoder.
+ octoquad.setSingleChannelPulseWidthParams (channel + 4, new OctoQuad.ChannelPulseWidthParams(1,1024));
+ }
+
+ /***
+ * Calculate the Swerve module's position and velocity values
+ * @param encoderDataBlock most recent full data block read from OctoQuad.
+ */
+ public void updateModule(OctoQuad.EncoderDataBlock encoderDataBlock) {
+ driveCounts = encoderDataBlock.positions[channel]; // get Counts.
+ driveCountsPerSec = encoderDataBlock.velocities[channel] * VELOCITY_SAMPLES_PER_S; // convert counts/interval to counts/sec
+
+ // convert uS to degrees. Add in any possible direction flip.
+ steerDegrees = AngleUnit.normalizeDegrees((encoderDataBlock.positions[channel+ 4] * DEGREES_PER_US * steerDirMult) - angleOffset);
+ // convert uS/interval to deg per sec. Add in any possible direction flip.
+ steerDegreesPerSec = encoderDataBlock.velocities[channel + 4] * DEGREES_PER_US * steerDirMult * VELOCITY_SAMPLES_PER_S;
+ }
+
+ /**
+ * Display the Swerve module's state as telemetry
+ * @param telemetry OpMode Telemetry object
+ */
+ public void show(Telemetry telemetry) {
+ telemetry.addData(name, "%8.0f %7.0f %7.0f %6.0f", driveCounts, driveCountsPerSec, steerDegrees, steerDegreesPerSec);
+ }
+}
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorREV2mDistance.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorREV2mDistance.java
index 30bb377..13883c3 100644
--- 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
@@ -33,38 +33,36 @@ are permitted (subject to the limitations in the disclaimer below) provided that
package org.firstinspires.ftc.robotcontroller.external.samples;
import com.qualcomm.hardware.rev.Rev2mDistanceSensor;
-import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
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;
-/**
- * {@link SensorREV2mDistance} illustrates how to use the REV Robotics
- * Time-of-Flight Range Sensor.
+/*
+ * This OpMode illustrates how to use the REV Robotics 2M Distance Sensor.
*
- * The op mode assumes that the range sensor is configured with a name of "sensor_range".
+ * 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
+ * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list
*
- * @see REV Robotics Web Page
+ * 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 sensorRange;
+ private DistanceSensor sensorDistance;
@Override
public void runOpMode() {
// you can use this as a regular DistanceSensor.
- sensorRange = hardwareMap.get(DistanceSensor.class, "sensor_range");
+ 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)sensorRange;
+ Rev2mDistanceSensor sensorTimeOfFlight = (Rev2mDistanceSensor) sensorDistance;
telemetry.addData(">>", "Press start to continue");
telemetry.update();
@@ -72,11 +70,11 @@ public void runOpMode() {
waitForStart();
while(opModeIsActive()) {
// generic DistanceSensor methods.
- telemetry.addData("deviceName",sensorRange.getDeviceName() );
- telemetry.addData("range", String.format("%.01f mm", sensorRange.getDistance(DistanceUnit.MM)));
- telemetry.addData("range", String.format("%.01f cm", sensorRange.getDistance(DistanceUnit.CM)));
- telemetry.addData("range", String.format("%.01f m", sensorRange.getDistance(DistanceUnit.METER)));
- telemetry.addData("range", String.format("%.01f in", sensorRange.getDistance(DistanceUnit.INCH)));
+ 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()));
@@ -86,4 +84,4 @@ public void runOpMode() {
}
}
-}
\ No newline at end of file
+}
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorSparkFunOTOS.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorSparkFunOTOS.java
new file mode 100644
index 0000000..3a25230
--- /dev/null
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorSparkFunOTOS.java
@@ -0,0 +1,156 @@
+/*
+ SPDX-License-Identifier: MIT
+
+ Copyright (c) 2024 SparkFun Electronics
+*/
+package org.firstinspires.ftc.robotcontroller.external.samples;
+
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import com.qualcomm.hardware.sparkfun.SparkFunOTOS;
+
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
+
+/*
+ * This OpMode illustrates how to use the SparkFun Qwiic Optical Tracking Odometry Sensor (OTOS)
+ *
+ * The OpMode assumes that the sensor is configured with a name of "sensor_otos".
+ *
+ * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name.
+ * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list
+ *
+ * See the sensor's product page: https://www.sparkfun.com/products/24904
+ */
+@TeleOp(name = "Sensor: SparkFun OTOS", group = "Sensor")
+@Disabled
+public class SensorSparkFunOTOS extends LinearOpMode {
+ // Create an instance of the sensor
+ SparkFunOTOS myOtos;
+
+ @Override
+ public void runOpMode() throws InterruptedException {
+ // Get a reference to the sensor
+ myOtos = hardwareMap.get(SparkFunOTOS.class, "sensor_otos");
+
+ // All the configuration for the OTOS is done in this helper method, check it out!
+ configureOtos();
+
+ // Wait for the start button to be pressed
+ waitForStart();
+
+ // Loop until the OpMode ends
+ while (opModeIsActive()) {
+ // Get the latest position, which includes the x and y coordinates, plus the
+ // heading angle
+ SparkFunOTOS.Pose2D pos = myOtos.getPosition();
+
+ // Reset the tracking if the user requests it
+ if (gamepad1.y) {
+ myOtos.resetTracking();
+ }
+
+ // Re-calibrate the IMU if the user requests it
+ if (gamepad1.x) {
+ myOtos.calibrateImu();
+ }
+
+ // Inform user of available controls
+ telemetry.addLine("Press Y (triangle) on Gamepad to reset tracking");
+ telemetry.addLine("Press X (square) on Gamepad to calibrate the IMU");
+ telemetry.addLine();
+
+ // Log the position to the telemetry
+ telemetry.addData("X coordinate", pos.x);
+ telemetry.addData("Y coordinate", pos.y);
+ telemetry.addData("Heading angle", pos.h);
+
+ // Update the telemetry on the driver station
+ telemetry.update();
+ }
+ }
+
+ private void configureOtos() {
+ telemetry.addLine("Configuring OTOS...");
+ telemetry.update();
+
+ // Set the desired units for linear and angular measurements. Can be either
+ // meters or inches for linear, and radians or degrees for angular. If not
+ // set, the default is inches and degrees. Note that this setting is not
+ // persisted in the sensor, so you need to set at the start of all your
+ // OpModes if using the non-default value.
+ // myOtos.setLinearUnit(DistanceUnit.METER);
+ myOtos.setLinearUnit(DistanceUnit.INCH);
+ // myOtos.setAngularUnit(AnguleUnit.RADIANS);
+ myOtos.setAngularUnit(AngleUnit.DEGREES);
+
+ // Assuming you've mounted your sensor to a robot and it's not centered,
+ // you can specify the offset for the sensor relative to the center of the
+ // robot. The units default to inches and degrees, but if you want to use
+ // different units, specify them before setting the offset! Note that as of
+ // firmware version 1.0, these values will be lost after a power cycle, so
+ // you will need to set them each time you power up the sensor. For example, if
+ // the sensor is mounted 5 inches to the left (negative X) and 10 inches
+ // forward (positive Y) of the center of the robot, and mounted 90 degrees
+ // clockwise (negative rotation) from the robot's orientation, the offset
+ // would be {-5, 10, -90}. These can be any value, even the angle can be
+ // tweaked slightly to compensate for imperfect mounting (eg. 1.3 degrees).
+ SparkFunOTOS.Pose2D offset = new SparkFunOTOS.Pose2D(0, 0, 0);
+ myOtos.setOffset(offset);
+
+ // Here we can set the linear and angular scalars, which can compensate for
+ // scaling issues with the sensor measurements. Note that as of firmware
+ // version 1.0, these values will be lost after a power cycle, so you will
+ // need to set them each time you power up the sensor. They can be any value
+ // from 0.872 to 1.127 in increments of 0.001 (0.1%). It is recommended to
+ // first set both scalars to 1.0, then calibrate the angular scalar, then
+ // the linear scalar. To calibrate the angular scalar, spin the robot by
+ // multiple rotations (eg. 10) to get a precise error, then set the scalar
+ // to the inverse of the error. Remember that the angle wraps from -180 to
+ // 180 degrees, so for example, if after 10 rotations counterclockwise
+ // (positive rotation), the sensor reports -15 degrees, the required scalar
+ // would be 3600/3585 = 1.004. To calibrate the linear scalar, move the
+ // robot a known distance and measure the error; do this multiple times at
+ // multiple speeds to get an average, then set the linear scalar to the
+ // inverse of the error. For example, if you move the robot 100 inches and
+ // the sensor reports 103 inches, set the linear scalar to 100/103 = 0.971
+ myOtos.setLinearScalar(1.0);
+ myOtos.setAngularScalar(1.0);
+
+ // The IMU on the OTOS includes a gyroscope and accelerometer, which could
+ // have an offset. Note that as of firmware version 1.0, the calibration
+ // will be lost after a power cycle; the OTOS performs a quick calibration
+ // when it powers up, but it is recommended to perform a more thorough
+ // calibration at the start of all your OpModes. Note that the sensor must
+ // be completely stationary and flat during calibration! When calling
+ // calibrateImu(), you can specify the number of samples to take and whether
+ // to wait until the calibration is complete. If no parameters are provided,
+ // it will take 255 samples and wait until done; each sample takes about
+ // 2.4ms, so about 612ms total
+ myOtos.calibrateImu();
+
+ // Reset the tracking algorithm - this resets the position to the origin,
+ // but can also be used to recover from some rare tracking errors
+ myOtos.resetTracking();
+
+ // After resetting the tracking, the OTOS will report that the robot is at
+ // the origin. If your robot does not start at the origin, or you have
+ // another source of location information (eg. vision odometry), you can set
+ // the OTOS location to match and it will continue to track from there.
+ SparkFunOTOS.Pose2D currentPosition = new SparkFunOTOS.Pose2D(0, 0, 0);
+ myOtos.setPosition(currentPosition);
+
+ // Get the hardware and firmware version
+ SparkFunOTOS.Version hwVersion = new SparkFunOTOS.Version();
+ SparkFunOTOS.Version fwVersion = new SparkFunOTOS.Version();
+ myOtos.getVersionInfo(hwVersion, fwVersion);
+
+ telemetry.addLine("OTOS configured! Press start to get position data!");
+ telemetry.addLine();
+ telemetry.addLine(String.format("OTOS Hardware Version: v%d.%d", hwVersion.major, hwVersion.minor));
+ telemetry.addLine(String.format("OTOS Firmware Version: v%d.%d", fwVersion.major, fwVersion.minor));
+ telemetry.update();
+ }
+}
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorMRIrSeeker.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorTouch.java
similarity index 54%
rename from FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorMRIrSeeker.java
rename to FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorTouch.java
index 9bd6452..3d79447 100644
--- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorMRIrSeeker.java
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorTouch.java
@@ -32,53 +32,47 @@
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.IrSeekerSensor;
+import com.qualcomm.robotcore.hardware.TouchSensor;
/*
- * This is an example LinearOpMode that shows how to use
- * the Modern Robotics ITR Seeker
+ * 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 op mode assumes that the IR Seeker
- * is configured with a name of "sensor_ir".
+ * The OpMode assumes that the touch sensor is configured with a name of "sensor_touch".
*
- * Set the switch on the Modern Robotics IR beacon to 1200 at 180.
- * Turn on the IR beacon.
- * Make sure the side of the beacon with the LED on is facing the robot.
+ * 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
+ * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list.
*/
-@TeleOp(name = "Sensor: MR IR Seeker", group = "Sensor")
+@TeleOp(name = "Sensor: REV touch sensor", group = "Sensor")
@Disabled
-public class SensorMRIrSeeker extends LinearOpMode {
+public class SensorTouch extends LinearOpMode {
+ TouchSensor touchSensor; // Touch sensor Object
- @Override
- public void runOpMode() {
+ @Override
+ public void runOpMode() {
- IrSeekerSensor irSeeker; // Hardware Device Object
+ // get a reference to our touchSensor object.
+ touchSensor = hardwareMap.get(TouchSensor.class, "sensor_touch");
- // get a reference to our GyroSensor object.
- irSeeker = hardwareMap.get(IrSeekerSensor.class, "sensor_ir");
+ // wait for the start button to be pressed.
+ waitForStart();
- // 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()) {
- 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");
+ }
- // Ensure we have a IR signal
- if (irSeeker.signalDetected())
- {
- // Display angle and strength
- telemetry.addData("Angle", irSeeker.getAngle());
- telemetry.addData("Strength", irSeeker.getStrength());
- }
- else
- {
- // Display loss of signal
- telemetry.addData("Seeker", "Signal Lost");
- }
-
- telemetry.update();
+ telemetry.update();
+ }
}
- }
}
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/UtilityCameraFrameCapture.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/UtilityCameraFrameCapture.java
new file mode 100644
index 0000000..69420cc
--- /dev/null
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/UtilityCameraFrameCapture.java
@@ -0,0 +1,127 @@
+/*
+ * Copyright (c) 2023 FIRST
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted (subject to the limitations in the disclaimer below) provided that
+ * the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this list
+ * of conditions and the following disclaimer.
+ *
+ * Redistributions in binary form must reproduce the above copyright notice, this
+ * list of conditions and the following disclaimer in the documentation and/or
+ * other materials provided with the distribution.
+ *
+ * Neither the name of FIRST nor the names of its contributors may be used to
+ * endorse or promote products derived from this software without specific prior
+ * written permission.
+ *
+ * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
+ * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+ * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR
+ * TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
+ * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+package org.firstinspires.ftc.robotcontroller.external.samples;
+
+import android.util.Size;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import org.firstinspires.ftc.robotcore.external.hardware.camera.BuiltinCameraDirection;
+import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
+import org.firstinspires.ftc.vision.VisionPortal;
+
+import java.util.Locale;
+
+/*
+ * This OpMode helps calibrate a webcam or RC phone camera, useful for AprilTag pose estimation
+ * with the FTC VisionPortal. It captures a camera frame (image) and stores it on the Robot Controller
+ * (Control Hub or RC phone), with each press of the gamepad button X (or Square).
+ * Full calibration instructions are here:
+ *
+ * https://ftc-docs.firstinspires.org/camera-calibration
+ *
+ * In Android Studio, copy this class into your "teamcode" folder with a new name.
+ * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list.
+ *
+ * In OnBot Java, use "Add File" to add this OpMode from the list of Samples.
+ */
+
+@TeleOp(name = "Utility: Camera Frame Capture", group = "Utility")
+@Disabled
+public class UtilityCameraFrameCapture extends LinearOpMode
+{
+ /*
+ * EDIT THESE PARAMETERS AS NEEDED
+ */
+ final boolean USING_WEBCAM = false;
+ final BuiltinCameraDirection INTERNAL_CAM_DIR = BuiltinCameraDirection.BACK;
+ final int RESOLUTION_WIDTH = 640;
+ final int RESOLUTION_HEIGHT = 480;
+
+ // Internal state
+ boolean lastX;
+ int frameCount;
+ long capReqTime;
+
+ @Override
+ public void runOpMode()
+ {
+ VisionPortal portal;
+
+ if (USING_WEBCAM)
+ {
+ portal = new VisionPortal.Builder()
+ .setCamera(hardwareMap.get(WebcamName.class, "Webcam 1"))
+ .setCameraResolution(new Size(RESOLUTION_WIDTH, RESOLUTION_HEIGHT))
+ .build();
+ }
+ else
+ {
+ portal = new VisionPortal.Builder()
+ .setCamera(INTERNAL_CAM_DIR)
+ .setCameraResolution(new Size(RESOLUTION_WIDTH, RESOLUTION_HEIGHT))
+ .build();
+ }
+
+ while (!isStopRequested())
+ {
+ boolean x = gamepad1.x;
+
+ if (x && !lastX)
+ {
+ portal.saveNextFrameRaw(String.format(Locale.US, "CameraFrameCapture-%06d", frameCount++));
+ capReqTime = System.currentTimeMillis();
+ }
+
+ lastX = x;
+
+ telemetry.addLine("######## Camera Capture Utility ########");
+ telemetry.addLine(String.format(Locale.US, " > Resolution: %dx%d", RESOLUTION_WIDTH, RESOLUTION_HEIGHT));
+ telemetry.addLine(" > Press X (or Square) to capture a frame");
+ telemetry.addData(" > Camera Status", portal.getCameraState());
+
+ if (capReqTime != 0)
+ {
+ telemetry.addLine("\nCaptured Frame!");
+ }
+
+ if (capReqTime != 0 && System.currentTimeMillis() - capReqTime > 1000)
+ {
+ capReqTime = 0;
+ }
+
+ telemetry.update();
+ }
+ }
+}
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/UtilityOctoQuadConfigMenu.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/UtilityOctoQuadConfigMenu.java
new file mode 100644
index 0000000..a962919
--- /dev/null
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/UtilityOctoQuadConfigMenu.java
@@ -0,0 +1,812 @@
+/*
+ * Copyright (c) 2024 DigitalChickenLabs
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+package org.firstinspires.ftc.robotcontroller.external.samples;
+
+import com.qualcomm.hardware.digitalchickenlabs.OctoQuad;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.hardware.Gamepad;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+
+import java.util.ArrayList;
+import java.util.Stack;
+
+/*
+ * This OpMode illustrates how to use the DigitalChickenLabs OctoQuad Quadrature Encoder & Pulse Width Interface Module.
+ *
+ * The OctoQuad has 8 input channels that can used to read either Relative Quadrature Encoders or Pulse-Width Absolute Encoder inputs.
+ * Relative Quadrature encoders are found on most FTC motors, and some stand-alone position sensors like the REV Thru-Bore encoder.
+ * Pulse-Width encoders are less common. The REV Thru-Bore encoder can provide its absolute position via a variable pulse width,
+ * as can several sonar rangefinders such as the MaxBotix MB1000 series.
+ *
+ * This OpMode assumes that the OctoQuad is attached to an I2C interface named "octoquad" in the robot configuration.
+ *
+ * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list
+ *
+ * Select, Init and run the "OctoQuad Configuration Tool" OpMode
+ * Read the blue User-Interface tips at the top of the telemetry screen.
+ * Use the UI buttons to navigate the menu and make any desired changes to the OctoQuad configuration.
+ * Use the Program Settings To FLASH option to make any changes permanent.
+ *
+ * See the sensor's product page: https://www.tindie.com/products/digitalchickenlabs/octoquad-8ch-quadrature-pulse-width-decoder/
+ */
+@TeleOp(name = "OctoQuad Configuration Tool", group="OctoQuad")
+@Disabled
+public class UtilityOctoQuadConfigMenu extends LinearOpMode
+{
+ TelemetryMenu.MenuElement rootMenu = new TelemetryMenu.MenuElement("OctoQuad Config Menu", true);
+ TelemetryMenu.MenuElement menuHwInfo = new TelemetryMenu.MenuElement("Hardware Information", false);
+ TelemetryMenu.EnumOption optionI2cResetMode;
+ TelemetryMenu.EnumOption optionChannelBankConfig;
+
+ TelemetryMenu.MenuElement menuEncoderDirections = new TelemetryMenu.MenuElement("Set Encoder Directions", false);
+ TelemetryMenu.BooleanOption[] optionsEncoderDirections = new TelemetryMenu.BooleanOption[OctoQuad.NUM_ENCODERS];
+
+ TelemetryMenu.MenuElement menuVelocityIntervals = new TelemetryMenu.MenuElement("Velocity Measurement Intervals", false);
+ TelemetryMenu.IntegerOption[] optionsVelocityIntervals = new TelemetryMenu.IntegerOption[OctoQuad.NUM_ENCODERS];
+
+ TelemetryMenu.MenuElement menuAbsParams = new TelemetryMenu.MenuElement("Abs. Encoder Pulse Width Params", false);
+ TelemetryMenu.IntegerOption[] optionsAbsParamsMax = new TelemetryMenu.IntegerOption[OctoQuad.NUM_ENCODERS];
+ TelemetryMenu.IntegerOption[] optionsAbsParamsMin = new TelemetryMenu.IntegerOption[OctoQuad.NUM_ENCODERS];
+
+ TelemetryMenu.OptionElement optionProgramToFlash;
+ TelemetryMenu.OptionElement optionSendToRAM;
+
+ TelemetryMenu.StaticClickableOption optionExit;
+
+ OctoQuad octoquad;
+
+ boolean error = false;
+
+ @Override
+ public void runOpMode()
+ {
+ octoquad = hardwareMap.getAll(OctoQuad.class).get(0);
+
+ if(octoquad.getChipId() != OctoQuad.OCTOQUAD_CHIP_ID)
+ {
+ telemetry.addLine("Error: cannot communicate with OctoQuad. Check your wiring and configuration and try again");
+ telemetry.update();
+
+ error = true;
+ }
+ else
+ {
+ if(octoquad.getFirmwareVersion().maj != OctoQuad.SUPPORTED_FW_VERSION_MAJ)
+ {
+ telemetry.addLine("Error: The OctoQuad is running a different major firmware version than this driver was built for. Cannot run configuration tool");
+ telemetry.update();
+
+ error = true;
+ }
+ }
+
+ if(error)
+ {
+ waitForStart();
+ return;
+ }
+
+ telemetry.addLine("Retrieving current configuration from OctoQuad");
+ telemetry.update();
+
+ optionExit = new TelemetryMenu.StaticClickableOption("Exit configuration menu")
+ {
+ @Override
+ void onClick() // called on OpMode thread
+ {
+ requestOpModeStop();
+ }
+ };
+
+ optionI2cResetMode = new TelemetryMenu.EnumOption("I2C Reset Mode", OctoQuad.I2cRecoveryMode.values(), octoquad.getI2cRecoveryMode());
+ optionChannelBankConfig = new TelemetryMenu.EnumOption("Channel Bank Modes", OctoQuad.ChannelBankConfig.values(), octoquad.getChannelBankConfig());
+
+ menuHwInfo.addChild(new TelemetryMenu.StaticItem("Board Firmware: v" + octoquad.getFirmwareVersion()));
+ //menuHwInfo.addChild(new TelemetryMenu.StaticItem("Board unique ID: FIXME"));
+
+ for(int i = 0; i < OctoQuad.NUM_ENCODERS; i++)
+ {
+ optionsEncoderDirections[i] = new TelemetryMenu.BooleanOption(
+ String.format("Encoder %d direction", i),
+ octoquad.getSingleEncoderDirection(i) == OctoQuad.EncoderDirection.REVERSE,
+ "-",
+ "+");
+ }
+ menuEncoderDirections.addChildren(optionsEncoderDirections);
+
+ for(int i = 0; i < OctoQuad.NUM_ENCODERS; i++)
+ {
+ optionsVelocityIntervals[i] = new TelemetryMenu.IntegerOption(
+ String.format("Chan %d velocity intvl", i),
+ OctoQuad.MIN_VELOCITY_MEASUREMENT_INTERVAL_MS,
+ OctoQuad.MAX_VELOCITY_MEASUREMENT_INTERVAL_MS,
+ octoquad.getSingleVelocitySampleInterval(i));
+ }
+ menuVelocityIntervals.addChildren(optionsVelocityIntervals);
+
+ for(int i = 0; i < OctoQuad.NUM_ENCODERS; i++)
+ {
+ OctoQuad.ChannelPulseWidthParams params = octoquad.getSingleChannelPulseWidthParams(i);
+
+ optionsAbsParamsMax[i] = new TelemetryMenu.IntegerOption(
+ String.format("Chan %d max pulse length", i),
+ OctoQuad.MIN_PULSE_WIDTH_US,
+ OctoQuad.MAX_PULSE_WIDTH_US,
+ params.max_length_us);
+
+ optionsAbsParamsMin[i] = new TelemetryMenu.IntegerOption(
+ String.format("Chan %d min pulse length", i),
+ OctoQuad.MIN_PULSE_WIDTH_US,
+ OctoQuad.MAX_PULSE_WIDTH_US,
+ params.min_length_us);
+ }
+ menuAbsParams.addChildren(optionsAbsParamsMin);
+ menuAbsParams.addChildren(optionsAbsParamsMax);
+
+ optionProgramToFlash = new TelemetryMenu.OptionElement()
+ {
+ String name = "Program Settings to FLASH";
+ long lastClickTime = 0;
+
+ @Override
+ protected String getDisplayText()
+ {
+ if(lastClickTime == 0)
+ {
+ return name;
+ }
+ else
+ {
+ if(System.currentTimeMillis() - lastClickTime < 1000)
+ {
+ return name + " **OK**";
+ }
+ else
+ {
+ lastClickTime = 0;
+ return name;
+ }
+ }
+ }
+
+ @Override
+ void onClick()
+ {
+ sendSettingsToRam();
+ octoquad.saveParametersToFlash();
+ lastClickTime = System.currentTimeMillis();
+ }
+ };
+
+ optionSendToRAM = new TelemetryMenu.OptionElement()
+ {
+ String name = "Send Settings to RAM";
+ long lastClickTime = 0;
+
+ @Override
+ protected String getDisplayText()
+ {
+ if(lastClickTime == 0)
+ {
+ return name;
+ }
+ else
+ {
+ if(System.currentTimeMillis() - lastClickTime < 1000)
+ {
+ return name + " **OK**";
+ }
+ else
+ {
+ lastClickTime = 0;
+ return name;
+ }
+ }
+ }
+
+ @Override
+ void onClick()
+ {
+ sendSettingsToRam();
+ lastClickTime = System.currentTimeMillis();
+ }
+ };
+
+ rootMenu.addChild(menuHwInfo);
+ rootMenu.addChild(optionI2cResetMode);
+ rootMenu.addChild(optionChannelBankConfig);
+ rootMenu.addChild(menuEncoderDirections);
+ rootMenu.addChild(menuVelocityIntervals);
+ rootMenu.addChild(menuAbsParams);
+ rootMenu.addChild(optionProgramToFlash);
+ rootMenu.addChild(optionSendToRAM);
+ rootMenu.addChild(optionExit);
+
+ TelemetryMenu menu = new TelemetryMenu(telemetry, rootMenu);
+
+ while (!isStopRequested())
+ {
+ menu.loop(gamepad1);
+ telemetry.update();
+ sleep(20);
+ }
+ }
+
+ void sendSettingsToRam()
+ {
+ for(int i = 0; i < OctoQuad.NUM_ENCODERS; i++)
+ {
+ octoquad.setSingleEncoderDirection(i, optionsEncoderDirections[i].getValue() ? OctoQuad.EncoderDirection.REVERSE : OctoQuad.EncoderDirection.FORWARD);
+ octoquad.setSingleVelocitySampleInterval(i, optionsVelocityIntervals[i].getValue());
+
+ OctoQuad.ChannelPulseWidthParams params = new OctoQuad.ChannelPulseWidthParams();
+ params.max_length_us = optionsAbsParamsMax[i].getValue();
+ params.min_length_us = optionsAbsParamsMin[i].getValue();
+
+ octoquad.setSingleChannelPulseWidthParams(i, params);
+ }
+
+ octoquad.setI2cRecoveryMode((OctoQuad.I2cRecoveryMode) optionI2cResetMode.getValue());
+ octoquad.setChannelBankConfig((OctoQuad.ChannelBankConfig) optionChannelBankConfig.getValue());
+ }
+
+ /*
+ * Copyright (c) 2023 OpenFTC Team
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+ public static class TelemetryMenu
+ {
+ private final MenuElement root;
+ private MenuElement currentLevel;
+
+ private boolean dpadUpPrev;
+ private boolean dpadDnPrev;
+ private boolean dpadRightPrev;
+ private boolean dpadLeftPrev;
+ private boolean xPrev;
+ private boolean lbPrev;
+
+ private int selectedIdx = 0;
+ private Stack selectedIdxStack = new Stack<>();
+
+ private final Telemetry telemetry;
+
+ /**
+ * TelemetryMenu constructor
+ * @param telemetry pass in 'telemetry' from your OpMode
+ * @param root the root menu element
+ */
+ public TelemetryMenu(Telemetry telemetry, MenuElement root)
+ {
+ this.root = root;
+ this.currentLevel = root;
+ this.telemetry = telemetry;
+
+ telemetry.setDisplayFormat(Telemetry.DisplayFormat.HTML);
+ telemetry.setMsTransmissionInterval(50);
+ }
+
+ /**
+ * Call this from inside your loop to put the current menu state into
+ * telemetry, and process gamepad inputs for navigating the menu
+ * @param gamepad the gamepad you want to use to navigate the menu
+ */
+ public void loop(Gamepad gamepad)
+ {
+ // Capture current state of the gamepad buttons we care about;
+ // We can only look once or we risk a race condition
+ boolean dpadUp = gamepad.dpad_up;
+ boolean dpadDn = gamepad.dpad_down;
+ boolean dpadRight = gamepad.dpad_right;
+ boolean dpadLeft = gamepad.dpad_left;
+ boolean x = gamepad.x;
+ boolean lb = gamepad.left_bumper;
+
+ // Figure out who our children our at this level
+ // and figure out which item is currently highlighted
+ // with the selection pointer
+ ArrayList children = currentLevel.children();
+ Element currentSelection = children.get(selectedIdx);
+
+ // Left and right are inputs to the selected item (if it's an Option)
+ if (currentSelection instanceof OptionElement)
+ {
+ if (dpadRight && !dpadRightPrev) // rising edge
+ {
+ ((OptionElement) currentSelection).onRightInput();
+ }
+ else if (dpadLeft && !dpadLeftPrev) // rising edge
+ {
+ ((OptionElement) currentSelection).onLeftInput();
+ }
+ }
+
+ // Up and down navigate the current selection pointer
+ if (dpadUp && !dpadUpPrev) // rising edge
+ {
+ selectedIdx--; // Move selection pointer up
+ }
+ else if (dpadDn && !dpadDnPrev) // rising edge
+ {
+ selectedIdx++; // Move selection pointer down
+ }
+
+ // Make selected index sane (don't let it go out of bounds) :eyes:
+ if (selectedIdx >= children.size())
+ {
+ selectedIdx = children.size()-1;
+ }
+ else if (selectedIdx < 0)
+ {
+ selectedIdx = 0;
+ }
+
+ // Select: either enter submenu or input to option
+ else if (x && !xPrev) // rising edge
+ {
+ // Select up element
+ if (currentSelection instanceof SpecialUpElement)
+ {
+ // We can only go up if we're not at the root level
+ if (currentLevel != root)
+ {
+ // Restore selection pointer to where it was before
+ selectedIdx = selectedIdxStack.pop();
+
+ // Change to the parent level
+ currentLevel = currentLevel.parent();
+ }
+ }
+ // Input to option
+ else if (currentSelection instanceof OptionElement)
+ {
+ ((OptionElement) currentSelection).onClick();
+ }
+ // Enter submenu
+ else if (currentSelection instanceof MenuElement)
+ {
+ // Save our current selection pointer so we can restore it
+ // later if the user navigates back up a level
+ selectedIdxStack.push(selectedIdx);
+
+ // We have no idea what's in the submenu :monkey: so best to
+ // just set the selection pointer to the first element
+ selectedIdx = 0;
+
+ // Now the current level becomes the submenu that the selection
+ // pointer was on
+ currentLevel = (MenuElement) currentSelection;
+ }
+ }
+
+ // Go up a level
+ else if (lb && !lbPrev)
+ {
+ // We can only go up if we're not at the root level
+ if (currentLevel != root)
+ {
+ // Restore selection pointer to where it was before
+ selectedIdx = selectedIdxStack.pop();
+
+ // Change to the parent level
+ currentLevel = currentLevel.parent();
+ }
+ }
+
+ // Save the current button states so that we can look for
+ // the rising edge the next time around the loop :)
+ dpadUpPrev = dpadUp;
+ dpadDnPrev = dpadDn;
+ dpadRightPrev = dpadRight;
+ dpadLeftPrev = dpadLeft;
+ xPrev = x;
+ lbPrev = lb;
+
+ // Start building the text display.
+ // First, we add the static directions for gamepad operation
+ StringBuilder builder = new StringBuilder();
+ builder.append("");
+ builder.append("Navigate items.....dpad up/down\n")
+ .append("Select.............X or Square\n")
+ .append("Edit option........dpad left/right\n")
+ .append("Up one level.......left bumper\n");
+ builder.append("");
+ builder.append("\n");
+
+ // Now actually add the menu options. We start by adding the name of the current menu level.
+ builder.append("");
+ builder.append("Current Menu: ").append(currentLevel.name).append("\n");
+
+ // Now we loop through all the child elements of this level and add them
+ for (int i = 0; i < children.size(); i++)
+ {
+ // If the selection pointer is at this index, put a green dot in the box :)
+ if (selectedIdx == i)
+ {
+ builder.append("[•] ");
+ }
+ // Otherwise, just put an empty box
+ else
+ {
+ builder.append("[ ] ");
+ }
+
+ // Figure out who the selection pointer is pointing at :eyes:
+ Element e = children.get(i);
+
+ // If it's pointing at a submenu, indicate that it's a submenu to the user
+ // by prefixing "> " to the name.
+ if (e instanceof MenuElement)
+ {
+ builder.append("> ");
+ }
+
+ // Finally, add the element's name
+ builder.append(e.getDisplayText());
+
+ // We musn't forget the newline
+ builder.append("\n");
+ }
+
+ // Don't forget to close the font tag either
+ builder.append("");
+
+ // Build the string!!!! :nerd:
+ String menu = builder.toString();
+
+ // Add it to telemetry
+ telemetry.addLine(menu);
+ }
+
+ public static class MenuElement extends Element
+ {
+ private String name;
+ private ArrayList children = new ArrayList<>();
+
+ /**
+ * Create a new MenuElement; may either be the root menu, or a submenu (set isRoot accordingly)
+ * @param name the name for this menu
+ * @param isRoot whether this is a root menu, or a submenu
+ */
+ public MenuElement(String name, boolean isRoot)
+ {
+ this.name = name;
+
+ // If it's not the root menu, we add the up one level option as the first element
+ if (!isRoot)
+ {
+ children.add(new SpecialUpElement());
+ }
+ }
+
+ /**
+ * Add a child element to this menu (may either be an Option or another menu)
+ * @param child the child element to add
+ */
+ public void addChild(Element child)
+ {
+ child.setParent(this);
+ children.add(child);
+ }
+
+ /**
+ * Add multiple child elements to this menu (may either be option, or another menu)
+ * @param children the children to add
+ */
+ public void addChildren(Element[] children)
+ {
+ for (Element e : children)
+ {
+ e.setParent(this);
+ this.children.add(e);
+ }
+ }
+
+ @Override
+ protected String getDisplayText()
+ {
+ return name;
+ }
+
+ private ArrayList children()
+ {
+ return children;
+ }
+ }
+
+ public static abstract class OptionElement extends Element
+ {
+ /**
+ * Override this to get notified when the element is clicked
+ */
+ void onClick() {}
+
+ /**
+ * Override this to get notified when the element gets a "left edit" input
+ */
+ protected void onLeftInput() {}
+
+ /**
+ * Override this to get notified when the element gets a "right edit" input
+ */
+ protected void onRightInput() {}
+ }
+
+ public static class EnumOption extends OptionElement
+ {
+ protected int idx = 0;
+ protected Enum[] e;
+ protected String name;
+
+ public EnumOption(String name, Enum[] e)
+ {
+ this.e = e;
+ this.name = name;
+ }
+
+ public EnumOption(String name, Enum[] e, Enum def)
+ {
+ this(name, e);
+ idx = def.ordinal();
+ }
+
+ @Override
+ public void onLeftInput()
+ {
+ idx++;
+
+ if(idx > e.length-1)
+ {
+ idx = 0;
+ }
+ }
+
+ @Override
+ public void onRightInput()
+ {
+ idx--;
+
+ if(idx < 0)
+ {
+ idx = e.length-1;
+ }
+ }
+
+ @Override
+ public void onClick()
+ {
+ //onRightInput();
+ }
+
+ @Override
+ protected String getDisplayText()
+ {
+ return String.format("%s: %s", name, e[idx].name());
+ }
+
+ public Enum getValue()
+ {
+ return e[idx];
+ }
+ }
+
+ public static class IntegerOption extends OptionElement
+ {
+ protected int i;
+ protected int min;
+ protected int max;
+ protected String name;
+
+ public IntegerOption(String name, int min, int max, int def)
+ {
+ this.name = name;
+ this.min = min;
+ this.max = max;
+ this.i = def;
+ }
+
+ @Override
+ public void onLeftInput()
+ {
+ i--;
+
+ if(i < min)
+ {
+ i = max;
+ }
+ }
+
+ @Override
+ public void onRightInput()
+ {
+ i++;
+
+ if(i > max)
+ {
+ i = min;
+ }
+ }
+
+ @Override
+ public void onClick()
+ {
+ //onRightInput();
+ }
+
+ @Override
+ protected String getDisplayText()
+ {
+ return String.format("%s: %d", name, i);
+ }
+
+ public int getValue()
+ {
+ return i;
+ }
+ }
+
+ static class BooleanOption extends OptionElement
+ {
+ private String name;
+ private boolean val = true;
+
+ private String customTrue;
+ private String customFalse;
+
+ BooleanOption(String name, boolean def)
+ {
+ this.name = name;
+ this.val = def;
+ }
+
+ BooleanOption(String name, boolean def, String customTrue, String customFalse)
+ {
+ this(name, def);
+ this.customTrue = customTrue;
+ this.customFalse = customFalse;
+ }
+
+ @Override
+ public void onLeftInput()
+ {
+ val = !val;
+ }
+
+ @Override
+ public void onRightInput()
+ {
+ val = !val;
+ }
+
+ @Override
+ public void onClick()
+ {
+ val = !val;
+ }
+
+ @Override
+ protected String getDisplayText()
+ {
+ String valStr;
+
+ if(customTrue != null && customFalse != null)
+ {
+ valStr = val ? customTrue : customFalse;
+ }
+ else
+ {
+ valStr = val ? "true" : "false";
+ }
+
+ return String.format("%s: %s", name, valStr);
+ }
+
+ public boolean getValue()
+ {
+ return val;
+ }
+ }
+
+ /**
+ *
+ */
+ public static class StaticItem extends OptionElement
+ {
+ private String name;
+
+ public StaticItem(String name)
+ {
+ this.name = name;
+ }
+
+ @Override
+ protected String getDisplayText()
+ {
+ return name;
+ }
+ }
+
+ public static abstract class StaticClickableOption extends OptionElement
+ {
+ private String name;
+
+ public StaticClickableOption(String name)
+ {
+ this.name = name;
+ }
+
+ abstract void onClick();
+
+ @Override
+ protected String getDisplayText()
+ {
+ return name;
+ }
+ }
+
+ private static abstract class Element
+ {
+ private MenuElement parent;
+
+ protected void setParent(MenuElement parent)
+ {
+ this.parent = parent;
+ }
+
+ protected MenuElement parent()
+ {
+ return parent;
+ }
+
+ protected abstract String getDisplayText();
+ }
+
+ private static class SpecialUpElement extends Element
+ {
+ @Override
+ protected String getDisplayText()
+ {
+ return ".. ↰ Up One Level";
+ }
+ }
+ }
+}
diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/readme.md b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/readme.md
index 1808cd3..326978d 100644
--- 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
@@ -27,15 +27,9 @@ 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.
-Hardware: This is NOT an OpMode, but a helper class that is used to describe
- one particular robot's hardware configuration: eg: For the K9 or Pushbot.
- Look at any Pushbot sample to see how this can be used in an OpMode.
- Teams can copy one of these to their team folder to create their own robot definition.
-
-Pushbot: This is a Sample OpMode that uses the Pushbot robot hardware as a base.
- It may be used to provide some standard baseline Pushbot OpModes, or
- to demonstrate how a particular sensor or concept can be used directly on the
- Pushbot chassis.
+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,
@@ -43,15 +37,9 @@ Concept: This is a sample OpMode that illustrates performing a specific function
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.
-Library: This is a class, or set of classes used to implement some strategy.
- These will typically NOT implement a full OpMode. Instead they will be included
- by an OpMode to provide some stand-alone capability.
-
After the prefix, other conventions will apply:
* Sensor class names are constructed as: Sensor - Company - Type
-* Hardware class names are constructed as: Hardware - Robot type
-* Pushbot class names are constructed as: Pushbot - Mode - Action - OpModetype
+* Robot class names are constructed as: Robot - Mode - Action - OpModetype
* Concept class names are constructed as: Concept - Topic - OpModetype
-* Library class names are constructed as: Library - 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
index 0f1aa13..e85e625 100644
--- 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
@@ -5,45 +5,40 @@ This document defines the FTC Sample OpMode and Class conventions.
### OpMode Name
-A range of different samples classes will reside in the java/external/samples folder.
+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.
-For ease of understanding, 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:
+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 Tank Drive examples.
+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.
-Hardware: This is not an actual OpMode, but a helper class that is used to describe
- one particular robot's hardware configuration: eg: For the K9 or Pushbot.
- Look at any Pushbot sample to see how this can be used in an OpMode.
- Teams can copy one of these to create their own robot definition.
-
-Pushbot: This is a Sample OpMode that uses the Pushbot robot hardware as a base.
- It may be used to provide some standard baseline Pushbot opmodes, or
- to demonstrate how a particular sensor or concept can be used directly on the
- Pushbot chassis.
+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.
+ locate based on their name. These OpModes may not produce a drivable robot.
-Library: This is a class, or set of classes used to implement some strategy.
- These will typically NOT implement a full opmode. Instead they will be included
- by an OpMode to provide some stand-alone capability.
+Utility: This sample type is provided as a useful tool, or aide, to perform some specific development task.
+ It is not expected to be something you would include in your own robot code.
+ To use the tool, comment out the @Disabled annotation and build the App.
+ Read the comments found in the sample for an explanation of its intended use.
After the prefix, other conventions will apply:
* Sensor class names should constructed as: Sensor - Company - Type
-* Hardware class names should be constructed as: Hardware - Robot type
-* Pushbot class names should be constructed as: Pushbot - Mode - Action - OpModetype
+* Robot class names should be constructed as: Robot - Mode - Action - OpModetype
* Concept class names should be constructed as: Concept - Topic - OpModetype
-* Library class names should be constructed as: Library - Topic - OpModetype
### Sample OpMode Content/Style
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
index 9a94f54..ceab67d 100644
--- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/internal/FtcOpModeRegister.java
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/internal/FtcOpModeRegister.java
@@ -34,10 +34,8 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
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.
+ * {@link FtcOpModeRegister} is responsible for registering OpModes for use in an FTC game.
* @see #register(OpModeManager)
*/
public class FtcOpModeRegister implements OpModeRegister {
@@ -49,7 +47,7 @@ public class FtcOpModeRegister implements OpModeRegister {
* 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}.
+ * See, for example the class annotations in {@link org.firstinspires.ftc.robotcontroller.external.samples.ConceptNullOp}.
*
* 2) The other, retired, method is to modify this {@link #register(OpModeManager)}
* method to include explicit calls to OpModeManager.register().
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
index e7ea299..3f1f77c 100644
--- a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/internal/FtcRobotControllerActivity.java
+++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/internal/FtcRobotControllerActivity.java
@@ -306,9 +306,9 @@ protected void onCreate(Bundle savedInstanceState) {
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 that what was installed previously
+ // 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(YearMonth.now()).getValue();
+ 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.
@@ -395,10 +395,9 @@ public boolean onMenuItemClick(MenuItem item) {
readNetworkType();
ServiceController.startService(FtcRobotControllerWatchdogService.class);
bindToService();
- logPackageVersions();
- logDeviceSerialNumber();
- AndroidBoard.getInstance().logAndroidBoardInfo();
+ RobotLog.logAppInfo();
RobotLog.logDeviceInfo();
+ AndroidBoard.getInstance().logAndroidBoardInfo();
if (preferencesHelper.readBoolean(getString(R.string.pref_wifi_automute), false)) {
initWifiMute(true);
@@ -499,19 +498,6 @@ protected void unbindFromService() {
}
}
- protected void logPackageVersions() {
- RobotLog.logBuildConfig(com.qualcomm.ftcrobotcontroller.BuildConfig.class);
- RobotLog.logBuildConfig(com.qualcomm.robotcore.BuildConfig.class);
- RobotLog.logBuildConfig(com.qualcomm.hardware.BuildConfig.class);
- RobotLog.logBuildConfig(com.qualcomm.ftccommon.BuildConfig.class);
- RobotLog.logBuildConfig(com.google.blocks.BuildConfig.class);
- RobotLog.logBuildConfig(org.firstinspires.inspection.BuildConfig.class);
- }
-
- protected void logDeviceSerialNumber() {
- RobotLog.ii(TAG, "Android device serial number: " + Device.getSerialNumberOrUnknown());
- }
-
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.
@@ -639,8 +625,8 @@ public void onConfigurationChanged(Configuration newConfig) {
}
/**
- * Updates the orientation of monitorContainer (which contains cameraMonitorView and
- * tfodMonitorView) based on the given configuration. Makes the children split the space.
+ * 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);
diff --git a/FtcRobotController/src/main/res/layout/activity_ftc_controller.xml b/FtcRobotController/src/main/res/layout/activity_ftc_controller.xml
index 935bb2b..6524f94 100644
--- a/FtcRobotController/src/main/res/layout/activity_ftc_controller.xml
+++ b/FtcRobotController/src/main/res/layout/activity_ftc_controller.xml
@@ -140,13 +140,6 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
android:layout_weight="1"
android:orientation="vertical"
/>
-
diff --git a/FtcRobotController/src/main/res/menu/ftc_robot_controller.xml b/FtcRobotController/src/main/res/menu/ftc_robot_controller.xml
index d14a89f..657c1aa 100644
--- a/FtcRobotController/src/main/res/menu/ftc_robot_controller.xml
+++ b/FtcRobotController/src/main/res/menu/ftc_robot_controller.xml
@@ -31,36 +31,37 @@ 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.
-->
-