commit c210315e16a3d9f88c989b96241dd46b79fc8e3c Author: Arkm20 Date: Sun Mar 22 19:53:38 2026 -0500 Initial commit diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..b85aa2f --- /dev/null +++ b/.gitignore @@ -0,0 +1,81 @@ +# Built application files +*.apk +*.aar +*.ap_ +*.aab + +!libs/*.aar + +# Files for the ART/Dalvik VM +*.dex + +# Java/JDK files +*.class +*.hprof + +# Generated files +bin/ +gen/ +out/ +# Uncomment the following line in case you need and you don't have the release build type files in your app +# release/ + +# Gradle files +.gradle/ +build/ + +# Local configuration file (sdk path, etc) +local.properties + +# Proguard folder generated by Eclipse +proguard/ + +# Log Files +*.log + +# Android Studio Navigation editor temp files +.navigation/ + +# Android Studio captures folder +captures/ + +# IntelliJ +*.iml +.idea/ + +# For Mac users +.DS_Store + +# Keystore files +# Uncomment the following lines if you do not want to check your keystore files in. +#*.jks +#*.keystore + +# External native build folder generated in Android Studio 2.2 and later +.externalNativeBuild +.cxx/ + +# Google Services (e.g. APIs or Firebase) +# google-services.json + +# Freeline +freeline.py +freeline/ +freeline_project_description.json + +# fastlane +fastlane/report.xml +fastlane/Preview.html +fastlane/screenshots +fastlane/test_output +fastlane/readme.md + +# Version control +vcs.xml + +# lint +lint/intermediates/ +lint/generated/ +lint/outputs/ +lint/tmp/ +# lint/reports/ \ No newline at end of file diff --git a/FtcRobotController/build.gradle b/FtcRobotController/build.gradle new file mode 100644 index 0000000..8c796da --- /dev/null +++ b/FtcRobotController/build.gradle @@ -0,0 +1,30 @@ +import java.text.SimpleDateFormat + +// +// build.gradle in FtcRobotController +// +apply plugin: 'com.android.library' + +android { + + defaultConfig { + minSdkVersion 24 + //noinspection ExpiredTargetSdkVersion + targetSdkVersion 28 + buildConfigField "String", "APP_BUILD_TIME", '"' + (new SimpleDateFormat("yyyy-MM-dd'T'HH:mm:ss.SSSZ", Locale.ROOT).format(new Date())) + '"' + } + + buildFeatures { + buildConfig = true + } + compileSdk 34 + + + compileOptions { + sourceCompatibility JavaVersion.VERSION_1_8 + targetCompatibility JavaVersion.VERSION_1_8 + } + namespace = 'com.qualcomm.ftcrobotcontroller' +} + +apply from: '../build.dependencies.gradle' diff --git a/FtcRobotController/src/main/AndroidManifest.xml b/FtcRobotController/src/main/AndroidManifest.xml new file mode 100644 index 0000000..4c58576 --- /dev/null +++ b/FtcRobotController/src/main/AndroidManifest.xml @@ -0,0 +1,79 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/BasicOmniOpMode_Linear.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/BasicOmniOpMode_Linear.java new file mode 100644 index 0000000..2644143 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/BasicOmniOpMode_Linear.java @@ -0,0 +1,167 @@ +/* Copyright (c) 2021 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.ElapsedTime; + +/* + * This file contains an example of a Linear "OpMode". + * An OpMode is a 'program' that runs in either the autonomous or the teleop period of an FTC match. + * The names of OpModes appear on the menu of the FTC Driver Station. + * When a selection is made from the menu, the corresponding OpMode is executed. + * + * This particular OpMode illustrates driving a 4-motor Omni-Directional (or Holonomic) robot. + * This code will work with either a Mecanum-Drive or an X-Drive train. + * Both of these drives are illustrated at https://gm0.org/en/latest/docs/robot-design/drivetrains/holonomic.html + * Note that a Mecanum drive must display an X roller-pattern when viewed from above. + * + * Also note that it is critical to set the correct rotation direction for each motor. See details below. + * + * Holonomic drives provide the ability for the robot to move in three axes (directions) simultaneously. + * Each motion axis is controlled by one Joystick axis. + * + * 1) Axial: Driving forward and backward Left-joystick Forward/Backward + * 2) Lateral: Strafing right and left Left-joystick Right and Left + * 3) Yaw: Rotating Clockwise and counter clockwise Right-joystick Right and Left + * + * This code is written assuming that the right-side motors need to be reversed for the robot to drive forward. + * When you first test your robot, if it moves backward when you push the left stick forward, then you must flip + * the direction of all 4 motors (see code below). + * + * 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="Basic: Omni Linear OpMode", group="Linear OpMode") +@Disabled +public class BasicOmniOpMode_Linear extends LinearOpMode { + + // Declare OpMode members for each of the 4 motors. + private ElapsedTime runtime = new ElapsedTime(); + private DcMotor frontLeftDrive = null; + private DcMotor backLeftDrive = null; + private DcMotor frontRightDrive = null; + private DcMotor backRightDrive = null; + + @Override + public void runOpMode() { + + // Initialize the hardware variables. Note that the strings used here must correspond + // to the names assigned during the robot configuration step on the DS or RC devices. + frontLeftDrive = hardwareMap.get(DcMotor.class, "front_left_drive"); + backLeftDrive = hardwareMap.get(DcMotor.class, "back_left_drive"); + frontRightDrive = hardwareMap.get(DcMotor.class, "front_right_drive"); + backRightDrive = hardwareMap.get(DcMotor.class, "back_right_drive"); + + // ######################################################################################## + // !!! IMPORTANT Drive Information. Test your motor directions. !!!!! + // ######################################################################################## + // Most robots need the motors on one side to be reversed to drive forward. + // The motor reversals shown here are for a "direct drive" robot (the wheels turn the same direction as the motor shaft) + // If your robot has additional gear reductions or uses a right-angled drive, it's important to ensure + // that your motors are turning in the correct direction. So, start out with the reversals here, BUT + // when you first test your robot, push the left joystick forward and observe the direction the wheels turn. + // Reverse the direction (flip FORWARD <-> REVERSE ) of any wheel that runs backward + // Keep testing until ALL the wheels move the robot forward when you push the left joystick forward. + frontLeftDrive.setDirection(DcMotor.Direction.REVERSE); + backLeftDrive.setDirection(DcMotor.Direction.REVERSE); + frontRightDrive.setDirection(DcMotor.Direction.FORWARD); + backRightDrive.setDirection(DcMotor.Direction.FORWARD); + + // Wait for the game to start (driver presses START) + telemetry.addData("Status", "Initialized"); + telemetry.update(); + + waitForStart(); + runtime.reset(); + + // run until the end of the match (driver presses STOP) + while (opModeIsActive()) { + double max; + + // POV Mode uses left joystick to go forward & strafe, and right joystick to rotate. + double axial = -gamepad1.left_stick_y; // Note: pushing stick forward gives negative value + double lateral = gamepad1.left_stick_x; + double yaw = gamepad1.right_stick_x; + + // Combine the joystick requests for each axis-motion to determine each wheel's power. + // Set up a variable for each drive wheel to save the power level for telemetry. + double frontLeftPower = axial + lateral + yaw; + double frontRightPower = axial - lateral - yaw; + double backLeftPower = axial - lateral + yaw; + double backRightPower = axial + lateral - yaw; + + // Normalize the values so no wheel power exceeds 100% + // This ensures that the robot maintains the desired motion. + max = Math.max(Math.abs(frontLeftPower), Math.abs(frontRightPower)); + max = Math.max(max, Math.abs(backLeftPower)); + max = Math.max(max, Math.abs(backRightPower)); + + if (max > 1.0) { + frontLeftPower /= max; + frontRightPower /= max; + backLeftPower /= max; + backRightPower /= max; + } + + // This is test code: + // + // Uncomment the following code to test your motor directions. + // Each button should make the corresponding motor run FORWARD. + // 1) First get all the motors to take to correct positions on the robot + // by adjusting your Robot Configuration if necessary. + // 2) Then make sure they run in the correct direction by modifying the + // the setDirection() calls above. + // Once the correct motors move in the correct direction re-comment this code. + + /* + frontLeftPower = gamepad1.x ? 1.0 : 0.0; // X gamepad + backLeftPower = gamepad1.a ? 1.0 : 0.0; // A gamepad + frontRightPower = gamepad1.y ? 1.0 : 0.0; // Y gamepad + backRightPower = gamepad1.b ? 1.0 : 0.0; // B gamepad + */ + + // Send calculated power to wheels + frontLeftDrive.setPower(frontLeftPower); + frontRightDrive.setPower(frontRightPower); + backLeftDrive.setPower(backLeftPower); + backRightDrive.setPower(backRightPower); + + // Show the elapsed game time and wheel power. + telemetry.addData("Status", "Run Time: " + runtime.toString()); + telemetry.addData("Front left/Right", "%4.2f, %4.2f", frontLeftPower, frontRightPower); + telemetry.addData("Back left/Right", "%4.2f, %4.2f", backLeftPower, backRightPower); + telemetry.update(); + } + }} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/BasicOpMode_Iterative.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/BasicOpMode_Iterative.java new file mode 100644 index 0000000..6504e58 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/BasicOpMode_Iterative.java @@ -0,0 +1,140 @@ +/* Copyright (c) 2017 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.util.ElapsedTime; +import com.qualcomm.robotcore.util.Range; + +/* + * This file contains an example of an iterative (Non-Linear) "OpMode". + * An OpMode is a 'program' that runs in either the autonomous or the teleop period of an FTC match. + * The names of OpModes appear on the menu of the FTC Driver Station. + * When a selection is made from the menu, the corresponding OpMode + * class is instantiated on the Robot Controller and executed. + * + * This particular OpMode just executes a basic Tank Drive Teleop for a two wheeled robot + * It includes all the skeletal structure that all iterative OpModes contain. + * + * 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="Basic: Iterative OpMode", group="Iterative OpMode") +@Disabled +public class BasicOpMode_Iterative extends OpMode +{ + // Declare OpMode members. + private ElapsedTime runtime = new ElapsedTime(); + private DcMotor leftDrive = null; + private DcMotor rightDrive = null; + + /* + * Code to run ONCE when the driver hits INIT + */ + @Override + public void init() { + telemetry.addData("Status", "Initialized"); + + // 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. + // 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); + + // Tell the driver that initialization is complete. + telemetry.addData("Status", "Initialized"); + } + + /* + * Code to run REPEATEDLY after the driver hits INIT, but before they hit START + */ + @Override + public void init_loop() { + } + + /* + * Code to run ONCE when the driver hits START + */ + @Override + public void start() { + runtime.reset(); + } + + /* + * Code to run REPEATEDLY after the driver hits START but before they hit STOP + */ + @Override + public void loop() { + // Setup a variable for each drive wheel to save power level for telemetry + double leftPower; + double rightPower; + + // Choose to drive using either Tank Mode, or POV Mode + // Comment out the method that's not used. The default below is POV. + + // POV Mode uses left stick to go forward, and right stick to turn. + // - This uses basic math to combine motions and is easier to drive straight. + double drive = -gamepad1.left_stick_y; + double turn = gamepad1.right_stick_x; + leftPower = Range.clip(drive + turn, -1.0, 1.0) ; + rightPower = Range.clip(drive - turn, -1.0, 1.0) ; + + // Tank Mode uses one stick to control each wheel. + // - This requires no math, but it is hard to drive forward slowly and keep straight. + // leftPower = -gamepad1.left_stick_y ; + // rightPower = -gamepad1.right_stick_y ; + + // Send calculated power to wheels + leftDrive.setPower(leftPower); + rightDrive.setPower(rightPower); + + // Show the elapsed game time and wheel power. + telemetry.addData("Status", "Run Time: " + runtime.toString()); + telemetry.addData("Motors", "left (%.2f), right (%.2f)", leftPower, rightPower); + } + + /* + * Code to run ONCE after the driver hits STOP + */ + @Override + public void stop() { + } + +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/BasicOpMode_Linear.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/BasicOpMode_Linear.java new file mode 100644 index 0000000..ab0bb25 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/BasicOpMode_Linear.java @@ -0,0 +1,115 @@ +/* Copyright (c) 2017 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.util.ElapsedTime; +import com.qualcomm.robotcore.util.Range; + + +/* + * This file contains an minimal example of a Linear "OpMode". An OpMode is a 'program' that runs in either + * the autonomous or the teleop period of an FTC match. The names of OpModes appear on the menu + * of the FTC Driver Station. When a selection is made from the menu, the corresponding OpMode + * class is instantiated on the Robot Controller and executed. + * + * This particular OpMode just executes a basic Tank Drive Teleop for a two wheeled robot + * It includes all the skeletal structure that all linear OpModes contain. + * + * 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="Basic: Linear OpMode", group="Linear OpMode") +@Disabled +public class BasicOpMode_Linear extends LinearOpMode { + + // Declare OpMode members. + private ElapsedTime runtime = new ElapsedTime(); + private DcMotor leftDrive = null; + private DcMotor rightDrive = null; + + @Override + public void runOpMode() { + telemetry.addData("Status", "Initialized"); + telemetry.update(); + + // Initialize the hardware variables. Note that the strings used here as parameters + // to 'get' must correspond to the names assigned during the robot configuration + // step (using the FTC Robot Controller app on the phone). + leftDrive = hardwareMap.get(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. + // 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); + + // Wait for the game to start (driver presses START) + waitForStart(); + runtime.reset(); + + // run until the end of the match (driver presses STOP) + while (opModeIsActive()) { + + // Setup a variable for each drive wheel to save power level for telemetry + double leftPower; + double rightPower; + + // Choose to drive using either Tank Mode, or POV Mode + // Comment out the method that's not used. The default below is POV. + + // POV Mode uses left stick to go forward, and right stick to turn. + // - This uses basic math to combine motions and is easier to drive straight. + double drive = -gamepad1.left_stick_y; + double turn = gamepad1.right_stick_x; + leftPower = Range.clip(drive + turn, -1.0, 1.0) ; + rightPower = Range.clip(drive - turn, -1.0, 1.0) ; + + // Tank Mode uses one stick to control each wheel. + // - This requires no math, but it is hard to drive forward slowly and keep straight. + // leftPower = -gamepad1.left_stick_y ; + // rightPower = -gamepad1.right_stick_y ; + + // Send calculated power to wheels + leftDrive.setPower(leftPower); + rightDrive.setPower(rightPower); + + // Show the elapsed game time and wheel power. + telemetry.addData("Status", "Run Time: " + runtime.toString()); + telemetry.addData("Motors", "left (%.2f), right (%.2f)", leftPower, rightPower); + telemetry.update(); + } + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTag.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTag.java new file mode 100644 index 0000000..4ee7ffe --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTag.java @@ -0,0 +1,215 @@ +/* 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 org.firstinspires.ftc.vision.apriltag.AprilTagDetection; +import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor; + +import java.util.List; + +/* + * This OpMode illustrates the basics of AprilTag recognition and pose estimation, + * including Java Builder structures for specifying Vision parameters. + * + * For an introduction to AprilTags, see the FTC-DOCS link below: + * https://ftc-docs.firstinspires.org/en/latest/apriltag/vision_portal/apriltag_intro/apriltag-intro.html + * + * In this sample, any visible tag ID will be detected and displayed, but only tags that are included in the default + * "TagLibrary" will have their position and orientation information displayed. This default TagLibrary contains + * the current Season's AprilTags and a small set of "test Tags" in the high number range. + * + * When an AprilTag in the TagLibrary is detected, the SDK provides location and orientation of the tag, relative to the camera. + * This information is provided in the "ftcPose" member of the returned "detection", and is explained in the ftc-docs page linked below. + * https://ftc-docs.firstinspires.org/apriltag-detection-values + * + * To experiment with using AprilTags to navigate, try out these two driving samples: + * RobotAutoDriveToAprilTagOmni and RobotAutoDriveToAprilTagTank + * + * There are many "default" VisionPortal and AprilTag configuration parameters that may be overridden if desired. + * These default parameters are shown as comments in the code below. + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list. + */ +@TeleOp(name = "Concept: AprilTag", group = "Concept") +@Disabled +public class ConceptAprilTag extends LinearOpMode { + + private static final boolean USE_WEBCAM = true; // true for webcam, false for phone camera + + /** + * The variable to store our instance of the AprilTag processor. + */ + private AprilTagProcessor aprilTag; + + /** + * The variable to store our instance of the vision portal. + */ + private VisionPortal visionPortal; + + @Override + public void runOpMode() { + + initAprilTag(); + + // Wait for the DS start button to be touched. + telemetry.addData("DS preview on/off", "3 dots, Camera Stream"); + telemetry.addData(">", "Touch START to start OpMode"); + telemetry.update(); + waitForStart(); + + while (opModeIsActive()) { + + telemetryAprilTag(); + + // Push telemetry to the Driver Station. + telemetry.update(); + + // Save CPU resources; can resume streaming when needed. + if (gamepad1.dpad_down) { + visionPortal.stopStreaming(); + } else if (gamepad1.dpad_up) { + visionPortal.resumeStreaming(); + } + + // Share the CPU. + sleep(20); + } + + // Save more CPU resources when camera is no longer needed. + visionPortal.close(); + + } // end method runOpMode() + + /** + * Initialize the AprilTag processor. + */ + private void initAprilTag() { + + // Create the AprilTag processor. + aprilTag = new AprilTagProcessor.Builder() + + // The following default settings are available to un-comment and edit as needed. + //.setDrawAxes(false) + //.setDrawCubeProjection(false) + //.setDrawTagOutline(true) + //.setTagFamily(AprilTagProcessor.TagFamily.TAG_36h11) + //.setTagLibrary(AprilTagGameDatabase.getCenterStageTagLibrary()) + //.setOutputUnits(DistanceUnit.INCH, AngleUnit.DEGREES) + + // == CAMERA CALIBRATION == + // If you do not manually specify calibration parameters, the SDK will attempt + // to load a predefined calibration for your camera. + //.setLensIntrinsics(578.272, 578.272, 402.145, 221.506) + // ... these parameters are fx, fy, cx, cy. + + .build(); + + // Adjust Image Decimation to trade-off detection-range for detection-rate. + // eg: 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 (default) + // Decimation = 3 .. Detect 5" Tag from 10 feet away at 30 Frames Per Second (default) + // Note: Decimation can be changed on-the-fly to adapt during a match. + //aprilTag.setDecimation(3); + + // Create the vision portal by using a builder. + VisionPortal.Builder builder = new VisionPortal.Builder(); + + // Set the camera (webcam vs. built-in RC phone camera). + if (USE_WEBCAM) { + builder.setCamera(hardwareMap.get(WebcamName.class, "Webcam 1")); + } else { + builder.setCamera(BuiltinCameraDirection.BACK); + } + + // Choose a camera resolution. Not all cameras support all resolutions. + //builder.setCameraResolution(new Size(640, 480)); + + // Enable the RC preview (LiveView). Set "false" to omit camera monitoring. + //builder.enableLiveView(true); + + // Set the stream format; MJPEG uses less bandwidth than default YUY2. + //builder.setStreamFormat(VisionPortal.StreamFormat.YUY2); + + // Choose whether or not LiveView stops if no processors are enabled. + // If set "true", monitor shows solid orange screen if no processors enabled. + // If set "false", monitor shows camera view without annotations. + //builder.setAutoStopLiveView(false); + + // Set and enable the processor. + builder.addProcessor(aprilTag); + + // Build the Vision Portal, using the above settings. + visionPortal = builder.build(); + + // Disable or re-enable the aprilTag processor at any time. + //visionPortal.setProcessorEnabled(aprilTag, true); + + } // end method initAprilTag() + + + /** + * Add telemetry about AprilTag detections. + */ + private void telemetryAprilTag() { + + List currentDetections = aprilTag.getDetections(); + telemetry.addData("# AprilTags Detected", currentDetections.size()); + + // Step through the list of detections and display info for each one. + for (AprilTagDetection detection : currentDetections) { + if (detection.metadata != null) { + telemetry.addLine(String.format("\n==== (ID %d) %s", detection.id, detection.metadata.name)); + telemetry.addLine(String.format("XYZ %6.1f %6.1f %6.1f (inch)", detection.ftcPose.x, detection.ftcPose.y, detection.ftcPose.z)); + telemetry.addLine(String.format("PRY %6.1f %6.1f %6.1f (deg)", detection.ftcPose.pitch, detection.ftcPose.roll, detection.ftcPose.yaw)); + telemetry.addLine(String.format("RBE %6.1f %6.1f %6.1f (inch, deg, deg)", detection.ftcPose.range, detection.ftcPose.bearing, detection.ftcPose.elevation)); + } else { + telemetry.addLine(String.format("\n==== (ID %d) Unknown", detection.id)); + telemetry.addLine(String.format("Center %6.0f %6.0f (pixels)", detection.center.x, detection.center.y)); + } + } // end for() loop + + // Add "key" information to telemetry + telemetry.addLine("\nkey:\nXYZ = X (Right), Y (Forward), Z (Up) dist."); + telemetry.addLine("PRY = Pitch, Roll & Yaw (XYZ Rotation)"); + telemetry.addLine("RBE = Range, Bearing & Elevation"); + + } // end method telemetryAprilTag() + +} // end class diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagEasy.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagEasy.java new file mode 100644 index 0000000..7bda71b --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagEasy.java @@ -0,0 +1,161 @@ +/* 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 org.firstinspires.ftc.robotcore.external.hardware.camera.BuiltinCameraDirection; +import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; +import org.firstinspires.ftc.vision.VisionPortal; +import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; +import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor; + +import java.util.List; + +/* + * This OpMode illustrates the basics of AprilTag recognition and pose estimation, using + * the easy way. + * + * For an introduction to AprilTags, see the FTC-DOCS link below: + * https://ftc-docs.firstinspires.org/en/latest/apriltag/vision_portal/apriltag_intro/apriltag-intro.html + * + * In this sample, any visible tag ID will be detected and displayed, but only tags that are included in the default + * "TagLibrary" will have their position and orientation information displayed. This default TagLibrary contains + * the current Season's AprilTags and a small set of "test Tags" in the high number range. + * + * When an AprilTag in the TagLibrary is detected, the SDK provides location and orientation of the tag, relative to the camera. + * This information is provided in the "ftcPose" member of the returned "detection", and is explained in the ftc-docs page linked below. + * https://ftc-docs.firstinspires.org/apriltag-detection-values + * + * To experiment with using AprilTags to navigate, try out these two driving samples: + * RobotAutoDriveToAprilTagOmni and RobotAutoDriveToAprilTagTank + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list. + */ +@TeleOp(name = "Concept: AprilTag Easy", group = "Concept") +@Disabled +public class ConceptAprilTagEasy extends LinearOpMode { + + private static final boolean USE_WEBCAM = true; // true for webcam, false for phone camera + + /** + * The variable to store our instance of the AprilTag processor. + */ + private AprilTagProcessor aprilTag; + + /** + * The variable to store our instance of the vision portal. + */ + private VisionPortal visionPortal; + + @Override + public void runOpMode() { + + initAprilTag(); + + // Wait for the DS start button to be touched. + telemetry.addData("DS preview on/off", "3 dots, Camera Stream"); + telemetry.addData(">", "Touch START to start OpMode"); + telemetry.update(); + waitForStart(); + + while (opModeIsActive()) { + + telemetryAprilTag(); + + // Push telemetry to the Driver Station. + telemetry.update(); + + // Save CPU resources; can resume streaming when needed. + if (gamepad1.dpad_down) { + visionPortal.stopStreaming(); + } else if (gamepad1.dpad_up) { + visionPortal.resumeStreaming(); + } + + // Share the CPU. + sleep(20); + } + + // Save more CPU resources when camera is no longer needed. + visionPortal.close(); + + } // end method runOpMode() + + /** + * Initialize the AprilTag processor. + */ + private void initAprilTag() { + + // Create the AprilTag processor the easy way. + aprilTag = AprilTagProcessor.easyCreateWithDefaults(); + + // Create the vision portal the easy way. + if (USE_WEBCAM) { + visionPortal = VisionPortal.easyCreateWithDefaults( + hardwareMap.get(WebcamName.class, "Webcam 1"), aprilTag); + } else { + visionPortal = VisionPortal.easyCreateWithDefaults( + BuiltinCameraDirection.BACK, aprilTag); + } + + } // end method initAprilTag() + + /** + * Add telemetry about AprilTag detections. + */ + private void telemetryAprilTag() { + + List currentDetections = aprilTag.getDetections(); + telemetry.addData("# AprilTags Detected", currentDetections.size()); + + // Step through the list of detections and display info for each one. + for (AprilTagDetection detection : currentDetections) { + if (detection.metadata != null) { + telemetry.addLine(String.format("\n==== (ID %d) %s", detection.id, detection.metadata.name)); + telemetry.addLine(String.format("XYZ %6.1f %6.1f %6.1f (inch)", detection.ftcPose.x, detection.ftcPose.y, detection.ftcPose.z)); + telemetry.addLine(String.format("PRY %6.1f %6.1f %6.1f (deg)", detection.ftcPose.pitch, detection.ftcPose.roll, detection.ftcPose.yaw)); + telemetry.addLine(String.format("RBE %6.1f %6.1f %6.1f (inch, deg, deg)", detection.ftcPose.range, detection.ftcPose.bearing, detection.ftcPose.elevation)); + } else { + telemetry.addLine(String.format("\n==== (ID %d) Unknown", detection.id)); + telemetry.addLine(String.format("Center %6.0f %6.0f (pixels)", detection.center.x, detection.center.y)); + } + } // end for() loop + + // Add "key" information to telemetry + telemetry.addLine("\nkey:\nXYZ = X (Right), Y (Forward), Z (Up) dist."); + telemetry.addLine("PRY = Pitch, Roll & Yaw (XYZ Rotation)"); + telemetry.addLine("RBE = Range, Bearing & Elevation"); + + } // end method telemetryAprilTag() + +} // end class diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagLocalization.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagLocalization.java new file mode 100644 index 0000000..3cb4d9f --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagLocalization.java @@ -0,0 +1,250 @@ +/* Copyright (c) 2024 Dryw Wade. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +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.hardware.camera.BuiltinCameraDirection; +import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.robotcore.external.navigation.Position; +import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles; +import org.firstinspires.ftc.vision.VisionPortal; +import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; +import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor; + +import java.util.List; + +/* + * This OpMode illustrates the basics of AprilTag based localization. + * + * For an introduction to AprilTags, see the FTC-DOCS link below: + * https://ftc-docs.firstinspires.org/en/latest/apriltag/vision_portal/apriltag_intro/apriltag-intro.html + * + * In this sample, any visible tag ID will be detected and displayed, but only tags that are included in the default + * "TagLibrary" will be used to compute the robot's location and orientation. This default TagLibrary contains + * the current Season's AprilTags and a small set of "test Tags" in the high number range. + * + * When an AprilTag in the TagLibrary is detected, the SDK provides location and orientation of the robot, relative to the field origin. + * This information is provided in the "robotPose" member of the returned "detection". + * + * To learn about the Field Coordinate System that is defined for FTC (and used by this OpMode), see the FTC-DOCS link below: + * https://ftc-docs.firstinspires.org/en/latest/game_specific_resources/field_coordinate_system/field-coordinate-system.html + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list. + */ +@TeleOp(name = "Concept: AprilTag Localization", group = "Concept") +@Disabled +public class ConceptAprilTagLocalization extends LinearOpMode { + + private static final boolean USE_WEBCAM = true; // true for webcam, false for phone camera + + /** + * Variables to store the position and orientation of the camera on the robot. Setting these + * values requires a definition of the axes of the camera and robot: + * + * Camera axes: + * Origin location: Center of the lens + * Axes orientation: +x right, +y down, +z forward (from camera's perspective) + * + * Robot axes (this is typical, but you can define this however you want): + * Origin location: Center of the robot at field height + * Axes orientation: +x right, +y forward, +z upward + * + * Position: + * If all values are zero (no translation), that implies the camera is at the center of the + * robot. Suppose your camera is positioned 5 inches to the left, 7 inches forward, and 12 + * inches above the ground - you would need to set the position to (-5, 7, 12). + * + * Orientation: + * If all values are zero (no rotation), that implies the camera is pointing straight up. In + * most cases, you'll need to set the pitch to -90 degrees (rotation about the x-axis), meaning + * the camera is horizontal. Use a yaw of 0 if the camera is pointing forwards, +90 degrees if + * it's pointing straight left, -90 degrees for straight right, etc. You can also set the roll + * to +/-90 degrees if it's vertical, or 180 degrees if it's upside-down. + */ + private Position cameraPosition = new Position(DistanceUnit.INCH, + 0, 0, 0, 0); + private YawPitchRollAngles cameraOrientation = new YawPitchRollAngles(AngleUnit.DEGREES, + 0, -90, 0, 0); + + /** + * The variable to store our instance of the AprilTag processor. + */ + private AprilTagProcessor aprilTag; + + /** + * The variable to store our instance of the vision portal. + */ + private VisionPortal visionPortal; + + @Override + public void runOpMode() { + + initAprilTag(); + + // Wait for the DS start button to be touched. + telemetry.addData("DS preview on/off", "3 dots, Camera Stream"); + telemetry.addData(">", "Touch START to start OpMode"); + telemetry.update(); + waitForStart(); + + while (opModeIsActive()) { + + telemetryAprilTag(); + + // Push telemetry to the Driver Station. + telemetry.update(); + + // Save CPU resources; can resume streaming when needed. + if (gamepad1.dpad_down) { + visionPortal.stopStreaming(); + } else if (gamepad1.dpad_up) { + visionPortal.resumeStreaming(); + } + + // Share the CPU. + sleep(20); + } + + // Save more CPU resources when camera is no longer needed. + visionPortal.close(); + + } // end method runOpMode() + + /** + * Initialize the AprilTag processor. + */ + private void initAprilTag() { + + // Create the AprilTag processor. + aprilTag = new AprilTagProcessor.Builder() + + // The following default settings are available to un-comment and edit as needed. + //.setDrawAxes(false) + //.setDrawCubeProjection(false) + //.setDrawTagOutline(true) + //.setTagFamily(AprilTagProcessor.TagFamily.TAG_36h11) + //.setTagLibrary(AprilTagGameDatabase.getCenterStageTagLibrary()) + //.setOutputUnits(DistanceUnit.INCH, AngleUnit.DEGREES) + .setCameraPose(cameraPosition, cameraOrientation) + + // == CAMERA CALIBRATION == + // If you do not manually specify calibration parameters, the SDK will attempt + // to load a predefined calibration for your camera. + //.setLensIntrinsics(578.272, 578.272, 402.145, 221.506) + // ... these parameters are fx, fy, cx, cy. + + .build(); + + // Adjust Image Decimation to trade-off detection-range for detection-rate. + // eg: 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 (default) + // Decimation = 3 .. Detect 5" Tag from 10 feet away at 30 Frames Per Second (default) + // Note: Decimation can be changed on-the-fly to adapt during a match. + //aprilTag.setDecimation(3); + + // Create the vision portal by using a builder. + VisionPortal.Builder builder = new VisionPortal.Builder(); + + // Set the camera (webcam vs. built-in RC phone camera). + if (USE_WEBCAM) { + builder.setCamera(hardwareMap.get(WebcamName.class, "Webcam 1")); + } else { + builder.setCamera(BuiltinCameraDirection.BACK); + } + + // Choose a camera resolution. Not all cameras support all resolutions. + //builder.setCameraResolution(new Size(640, 480)); + + // Enable the RC preview (LiveView). Set "false" to omit camera monitoring. + //builder.enableLiveView(true); + + // Set the stream format; MJPEG uses less bandwidth than default YUY2. + //builder.setStreamFormat(VisionPortal.StreamFormat.YUY2); + + // Choose whether or not LiveView stops if no processors are enabled. + // If set "true", monitor shows solid orange screen if no processors enabled. + // If set "false", monitor shows camera view without annotations. + //builder.setAutoStopLiveView(false); + + // Set and enable the processor. + builder.addProcessor(aprilTag); + + // Build the Vision Portal, using the above settings. + visionPortal = builder.build(); + + // Disable or re-enable the aprilTag processor at any time. + //visionPortal.setProcessorEnabled(aprilTag, true); + + } // end method initAprilTag() + + /** + * Add telemetry about AprilTag detections. + */ + private void telemetryAprilTag() { + + List currentDetections = aprilTag.getDetections(); + telemetry.addData("# AprilTags Detected", currentDetections.size()); + + // Step through the list of detections and display info for each one. + for (AprilTagDetection detection : currentDetections) { + if (detection.metadata != null) { + telemetry.addLine(String.format("\n==== (ID %d) %s", detection.id, detection.metadata.name)); + // Only use tags that don't have Obelisk in them + if (!detection.metadata.name.contains("Obelisk")) { + telemetry.addLine(String.format("XYZ %6.1f %6.1f %6.1f (inch)", + detection.robotPose.getPosition().x, + detection.robotPose.getPosition().y, + detection.robotPose.getPosition().z)); + telemetry.addLine(String.format("PRY %6.1f %6.1f %6.1f (deg)", + detection.robotPose.getOrientation().getPitch(AngleUnit.DEGREES), + detection.robotPose.getOrientation().getRoll(AngleUnit.DEGREES), + detection.robotPose.getOrientation().getYaw(AngleUnit.DEGREES))); + } + } 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)"); + + } // end method telemetryAprilTag() + +} // end class diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagMultiPortal.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagMultiPortal.java new file mode 100644 index 0000000..da5cc3e --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagMultiPortal.java @@ -0,0 +1,104 @@ +/* Copyright (c) 2024 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import org.firstinspires.ftc.robotcore.external.hardware.camera.BuiltinCameraDirection; +import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; +import org.firstinspires.ftc.vision.VisionPortal; +import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor; + +/** + * This OpMode demonstrates the basics of using multiple vision portals simultaneously + */ +@TeleOp(name = "Concept: AprilTagMultiPortal", group = "Concept") +@Disabled +public class ConceptAprilTagMultiPortal extends LinearOpMode +{ + VisionPortal portal1; + VisionPortal portal2; + + AprilTagProcessor aprilTagProcessor1; + AprilTagProcessor aprilTagProcessor2; + + @Override + public void runOpMode() throws InterruptedException + { + // Because we want to show two camera feeds simultaneously, we need to inform + // the SDK that we want it to split the camera monitor area into two smaller + // areas for us. It will then give us View IDs which we can pass to the individual + // vision portals to allow them to properly hook into the UI in tandem. + int[] viewIds = VisionPortal.makeMultiPortalView(2, VisionPortal.MultiPortalLayout.VERTICAL); + + // We extract the two view IDs from the array to make our lives a little easier later. + // NB: the array is 2 long because we asked for 2 portals up above. + int portal1ViewId = viewIds[0]; + int portal2ViewId = viewIds[1]; + + // If we want to run AprilTag detection on two portals simultaneously, + // we need to create two distinct instances of the AprilTag processor, + // one for each portal. If you want to see more detail about different + // options that you have when creating these processors, go check out + // the ConceptAprilTag OpMode. + aprilTagProcessor1 = AprilTagProcessor.easyCreateWithDefaults(); + aprilTagProcessor2 = AprilTagProcessor.easyCreateWithDefaults(); + + // Now we build both portals. The CRITICAL thing to notice here is the call to + // setLiveViewContainerId(), where we pass in the IDs we received earlier from + // makeMultiPortalView(). + portal1 = new VisionPortal.Builder() + .setCamera(hardwareMap.get(WebcamName.class, "Webcam 1")) + .setLiveViewContainerId(portal1ViewId) + .addProcessor(aprilTagProcessor1) + .build(); + portal2 = new VisionPortal.Builder() + .setCamera(hardwareMap.get(WebcamName.class, "Webcam 2")) + .setLiveViewContainerId(portal2ViewId) + .addProcessor(aprilTagProcessor2) + .build(); + + waitForStart(); + + // Main Loop + while (opModeIsActive()) + { + // Just show some basic telemetry to demonstrate both processors are working in parallel + // on their respective cameras. If you want to see more detail about the information you + // can get back from the processor, you should look at ConceptAprilTag. + telemetry.addData("Number of tags in Camera 1", aprilTagProcessor1.getDetections().size()); + telemetry.addData("Number of tags in Camera 2", aprilTagProcessor2.getDetections().size()); + telemetry.update(); + sleep(20); + } + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagOptimizeExposure.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagOptimizeExposure.java new file mode 100644 index 0000000..adee952 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagOptimizeExposure.java @@ -0,0 +1,246 @@ +/* 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.util.Range; +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 determines the best Exposure for minimizing image motion-blur on a Webcam + * Note that it is not possible to control the exposure for a Phone Camera, so if you are using a Phone for the Robot Controller + * this OpMode/Feature only applies to an externally connected Webcam + * + * The goal is to determine the smallest (shortest) Exposure value that still provides reliable Tag Detection. + * Starting with the minimum Exposure and maximum Gain, the exposure is slowly increased until the Tag is + * detected reliably from the likely operational distance. + * + * + * The best way to run this optimization is to view the camera preview screen while changing the exposure and gains. + * + * To do this, you need to view the RobotController screen directly (not from Driver Station) + * This can be done directly from a RC phone screen (if you are using an external Webcam), but for a Control Hub you must either plug an + * HDMI monitor into the Control Hub HDMI port, or use an external viewer program like ScrCpy (https://scrcpy.org/) + * + * 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="Optimize AprilTag Exposure", group = "Concept") +@Disabled +public class ConceptAprilTagOptimizeExposure extends LinearOpMode +{ + private VisionPortal visionPortal = null; // Used to manage the video source. + private AprilTagProcessor aprilTag; // Used for managing the AprilTag detection process. + private int myExposure ; + private int minExposure ; + private int maxExposure ; + private int myGain ; + private int minGain ; + private int maxGain ; + + boolean thisExpUp = false; + boolean thisExpDn = false; + boolean thisGainUp = false; + boolean thisGainDn = false; + + boolean lastExpUp = false; + boolean lastExpDn = false; + boolean lastGainUp = false; + boolean lastGainDn = false; + @Override public void runOpMode() + { + // Initialize the Apriltag Detection process + initAprilTag(); + + // Establish Min and Max Gains and Exposure. Then set a low exposure with high gain + getCameraSetting(); + myExposure = Math.min(5, minExposure); + myGain = maxGain; + setManualExposure(myExposure, myGain); + + // Wait for the match to begin. + telemetry.addData("Camera preview on/off", "3 dots, Camera Stream"); + telemetry.addData(">", "Touch START to start OpMode"); + telemetry.update(); + waitForStart(); + + while (opModeIsActive()) + { + telemetry.addLine("Find lowest Exposure that gives reliable detection."); + telemetry.addLine("Use Left bump/trig to adjust Exposure."); + telemetry.addLine("Use Right bump/trig to adjust Gain.\n"); + + // Display how many Tags Detected + List currentDetections = aprilTag.getDetections(); + int numTags = currentDetections.size(); + if (numTags > 0 ) + telemetry.addData("Tag", "####### %d Detected ######", currentDetections.size()); + else + telemetry.addData("Tag", "----------- none - ----------"); + + telemetry.addData("Exposure","%d (%d - %d)", myExposure, minExposure, maxExposure); + telemetry.addData("Gain","%d (%d - %d)", myGain, minGain, maxGain); + telemetry.update(); + + // check to see if we need to change exposure or gain. + thisExpUp = gamepad1.left_bumper; + thisExpDn = gamepad1.left_trigger > 0.25; + thisGainUp = gamepad1.right_bumper; + thisGainDn = gamepad1.right_trigger > 0.25; + + // look for clicks to change exposure + if (thisExpUp && !lastExpUp) { + myExposure = Range.clip(myExposure + 1, minExposure, maxExposure); + setManualExposure(myExposure, myGain); + } else if (thisExpDn && !lastExpDn) { + myExposure = Range.clip(myExposure - 1, minExposure, maxExposure); + setManualExposure(myExposure, myGain); + } + + // look for clicks to change the gain + if (thisGainUp && !lastGainUp) { + myGain = Range.clip(myGain + 1, minGain, maxGain ); + setManualExposure(myExposure, myGain); + } else if (thisGainDn && !lastGainDn) { + myGain = Range.clip(myGain - 1, minGain, maxGain ); + setManualExposure(myExposure, myGain); + } + + lastExpUp = thisExpUp; + lastExpDn = thisExpDn; + lastGainUp = thisGainUp; + lastGainDn = thisGainDn; + + sleep(20); + } + } + + /** + * Initialize the AprilTag processor. + */ + private void initAprilTag() { + // Create the AprilTag processor by using a builder. + aprilTag = new AprilTagProcessor.Builder().build(); + + // Create the WEBCAM vision portal by using a builder. + visionPortal = new VisionPortal.Builder() + .setCamera(hardwareMap.get(WebcamName.class, "Webcam 1")) + .addProcessor(aprilTag) + .build(); + } + + /* + Manually set the camera gain and exposure. + Can only be called AFTER calling initAprilTag(); + Returns true if controls are set. + */ + private boolean setManualExposure(int exposureMS, int gain) { + // Ensure Vision Portal has been setup. + if (visionPortal == null) { + return false; + } + + // Wait for the camera to be open + 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()) + { + // Set exposure. Make sure we are in Manual Mode for these values to take effect. + 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); + + // Set Gain. + GainControl gainControl = visionPortal.getCameraControl(GainControl.class); + gainControl.setGain(gain); + sleep(20); + return (true); + } else { + return (false); + } + } + + /* + Read this camera's minimum and maximum Exposure and Gain settings. + Can only be called AFTER calling initAprilTag(); + */ + private void getCameraSetting() { + // Ensure Vision Portal has been setup. + if (visionPortal == null) { + return; + } + + // Wait for the camera to be open + 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(); + } + + // Get camera control values unless we are stopping. + if (!isStopRequested()) { + ExposureControl exposureControl = visionPortal.getCameraControl(ExposureControl.class); + minExposure = (int)exposureControl.getMinExposure(TimeUnit.MILLISECONDS) + 1; + maxExposure = (int)exposureControl.getMaxExposure(TimeUnit.MILLISECONDS); + + GainControl gainControl = visionPortal.getCameraControl(GainControl.class); + minGain = gainControl.getMinGain(); + maxGain = gainControl.getMaxGain(); + } + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagSwitchableCameras.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagSwitchableCameras.java new file mode 100644 index 0000000..02e83d3 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagSwitchableCameras.java @@ -0,0 +1,194 @@ +/* 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 org.firstinspires.ftc.robotcore.external.ClassFactory; +import org.firstinspires.ftc.robotcore.external.hardware.camera.CameraName; +import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; +import org.firstinspires.ftc.vision.VisionPortal; +import org.firstinspires.ftc.vision.VisionPortal.CameraState; +import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; +import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor; + +import java.util.List; + +/* + * This OpMode illustrates the basics of AprilTag recognition and pose estimation, using + * two webcams. + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list. + */ +@TeleOp(name = "Concept: AprilTag Switchable Cameras", group = "Concept") +@Disabled +public class ConceptAprilTagSwitchableCameras extends LinearOpMode { + + /* + * Variables used for switching cameras. + */ + private WebcamName webcam1, webcam2; + private boolean oldLeftBumper; + private boolean oldRightBumper; + + /** + * The variable to store our instance of the AprilTag processor. + */ + private AprilTagProcessor aprilTag; + + /** + * The variable to store our instance of the vision portal. + */ + private VisionPortal visionPortal; + + @Override + public void runOpMode() { + + initAprilTag(); + + // Wait for the DS start button to be touched. + telemetry.addData("DS preview on/off", "3 dots, Camera Stream"); + telemetry.addData(">", "Touch START to start OpMode"); + telemetry.update(); + waitForStart(); + + while (opModeIsActive()) { + + telemetryCameraSwitching(); + telemetryAprilTag(); + + // Push telemetry to the Driver Station. + telemetry.update(); + + // Save CPU resources; can resume streaming when needed. + if (gamepad1.dpad_down) { + visionPortal.stopStreaming(); + } else if (gamepad1.dpad_up) { + visionPortal.resumeStreaming(); + } + + doCameraSwitching(); + + // Share the CPU. + sleep(20); + } + + // Save more CPU resources when camera is no longer needed. + visionPortal.close(); + + } // end runOpMode() + + /** + * Initialize the AprilTag processor. + */ + private void initAprilTag() { + + // Create the AprilTag processor by using a builder. + aprilTag = new AprilTagProcessor.Builder().build(); + + webcam1 = hardwareMap.get(WebcamName.class, "Webcam 1"); + webcam2 = hardwareMap.get(WebcamName.class, "Webcam 2"); + CameraName switchableCamera = ClassFactory.getInstance() + .getCameraManager().nameForSwitchableCamera(webcam1, webcam2); + + // Create the vision portal by using a builder. + visionPortal = new VisionPortal.Builder() + .setCamera(switchableCamera) + .addProcessor(aprilTag) + .build(); + + } // end method initAprilTag() + + /** + * Add telemetry about camera switching. + */ + private void telemetryCameraSwitching() { + + if (visionPortal.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"); + } + + } // end method telemetryCameraSwitching() + + /** + * Add telemetry about AprilTag detections. + */ + private void telemetryAprilTag() { + + List currentDetections = aprilTag.getDetections(); + telemetry.addData("# AprilTags Detected", currentDetections.size()); + + // Step through the list of detections and display info for each one. + for (AprilTagDetection detection : currentDetections) { + if (detection.metadata != null) { + telemetry.addLine(String.format("\n==== (ID %d) %s", detection.id, detection.metadata.name)); + telemetry.addLine(String.format("XYZ %6.1f %6.1f %6.1f (inch)", detection.ftcPose.x, detection.ftcPose.y, detection.ftcPose.z)); + telemetry.addLine(String.format("PRY %6.1f %6.1f %6.1f (deg)", detection.ftcPose.pitch, detection.ftcPose.roll, detection.ftcPose.yaw)); + telemetry.addLine(String.format("RBE %6.1f %6.1f %6.1f (inch, deg, deg)", detection.ftcPose.range, detection.ftcPose.bearing, detection.ftcPose.elevation)); + } else { + telemetry.addLine(String.format("\n==== (ID %d) Unknown", detection.id)); + telemetry.addLine(String.format("Center %6.0f %6.0f (pixels)", detection.center.x, detection.center.y)); + } + } // end for() loop + + // Add "key" information to telemetry + telemetry.addLine("\nkey:\nXYZ = X (Right), Y (Forward), Z (Up) dist."); + telemetry.addLine("PRY = Pitch, Roll & Yaw (XYZ Rotation)"); + telemetry.addLine("RBE = Range, Bearing & Elevation"); + + } // end method telemetryAprilTag() + + /** + * Set the active camera according to input from the gamepad. + */ + private void doCameraSwitching() { + if (visionPortal.getCameraState() == CameraState.STREAMING) { + // If the left bumper is pressed, use Webcam 1. + // If the right bumper is pressed, use Webcam 2. + boolean newLeftBumper = gamepad1.left_bumper; + boolean newRightBumper = gamepad1.right_bumper; + if (newLeftBumper && !oldLeftBumper) { + visionPortal.setActiveCamera(webcam1); + } else if (newRightBumper && !oldRightBumper) { + visionPortal.setActiveCamera(webcam2); + } + oldLeftBumper = newLeftBumper; + oldRightBumper = newRightBumper; + } + + } // end method doCameraSwitching() + +} // end class diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptBlackboard.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptBlackboard.java new file mode 100644 index 0000000..2c93abb --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptBlackboard.java @@ -0,0 +1,87 @@ +/* Copyright (c) 2025 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +/* + * Demonstrates how to use an OpMode to store data in the blackboard and retrieve it. + * This is useful for storing information in Auto and then retrieving it in your TeleOp. + * + * Be aware that this is NOT saved across power reboots or downloads or robot resets. + * + * You also need to make sure that both the storing and retrieving are done using the same + * type. For example, storing a Double in the blackboard and retrieving it as an Integer + * will fail when you typecast it. + * + * The blackboard is a standard hashmap so you can use methods like: + * put, get, containsKey, remove, etc. + */ +@TeleOp(name = "Concept: Blackboard", group = "Concept") +@Disabled +public class ConceptBlackboard extends OpMode { + // We use constants here so we won't have the problem of typos in different places when we + // meant to refer to the same thing. + public static final String TIMES_STARTED_KEY = "Times started"; + public static final String ALLIANCE_KEY = "Alliance"; + + /** + * This method will be called once, when the INIT button is pressed. + */ + @Override + public void init() { + // This gets us what is in the blackboard or the default if it isn't in there. + Object timesStarted = blackboard.getOrDefault(TIMES_STARTED_KEY, 0); + blackboard.put(TIMES_STARTED_KEY, (int) timesStarted + 1); + + telemetry.addData("OpMode started times", blackboard.get(TIMES_STARTED_KEY)); + } + + /** + * This method will be called repeatedly during the period between when + * the START button is pressed and when the OpMode is stopped. + *

+ * If the left bumper is pressed it will store the value "RED" in the blackboard for alliance. + * If the right bumper is pressed it will store the value "BLUE" in the blackboard for alliance. + *

+ * You'll notice that if you quit the OpMode and come back in, the value will persist. + */ + @Override + public void loop() { + if (gamepad1.left_bumper) { + blackboard.put(ALLIANCE_KEY, "RED"); + } else if (gamepad1.right_bumper) { + blackboard.put(ALLIANCE_KEY, "BLUE"); + } + telemetry.addData("Alliance", blackboard.get(ALLIANCE_KEY)); + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptExploringIMUOrientation.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptExploringIMUOrientation.java new file mode 100644 index 0000000..751d54f --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptExploringIMUOrientation.java @@ -0,0 +1,184 @@ +/* +Copyright (c) 2022 REV Robotics, FIRST + +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, +are permitted (subject to the limitations in the disclaimer below) provided that +the following conditions are met: + +Redistributions of source code must retain the above copyright notice, this list +of conditions and the following disclaimer. + +Redistributions in binary form must reproduce the above copyright notice, this +list of conditions and the following disclaimer in the documentation and/or +other materials provided with the distribution. + +Neither the name of REV Robotics nor the names of its contributors may be used to +endorse or promote products derived from this software without specific prior +written permission. + +NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS +LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF +THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.IMU; +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.AngularVelocity; +import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles; + +/* + * This OpMode demonstrates the impact of setting the IMU orientation correctly or incorrectly. This + * code assumes there is an IMU configured with the name "imu". + * + * Note: This OpMode is more of a tool than a code sample. The User Interface portion of this code + * goes beyond simply showing how to interface to the IMU.
+ * For a minimal example of interfacing to an IMU, please see the SensorIMUOrthogonal or SensorIMUNonOrthogonal sample OpModes. + * + * This OpMode enables you to re-specify the Hub Mounting orientation dynamically by using gamepad controls. + * While doing so, the sample will display how Pitch, Roll and Yaw angles change as the hub is moved. + * + * The gamepad controls let you change the two parameters that specify how the Control/Expansion Hub is mounted.
+ * The first parameter specifies which direction the printed logo on the Hub is pointing.
+ * The second parameter specifies which direction the USB connector on the Hub is pointing.
+ * All directions are relative to the robot, and left/right is as viewed from behind the robot. + * + * How will you know if you have chosen the correct Orientation? With the correct orientation + * parameters selected, pitch/roll/yaw should act as follows: + * + * Pitch value should INCREASE as the robot is tipped UP at the front. (Rotation about X)
+ * Roll value should INCREASE as the robot is tipped UP at the left side. (Rotation about Y)
+ * Yaw value should INCREASE as the robot is rotated Counter Clockwise. (Rotation about Z)
+ * + * The Yaw can be reset (to zero) by pressing the Y button on the gamepad (Triangle on a PS4 controller) + * + * The rotational velocities should follow the change in corresponding axes. + */ + +@TeleOp(name="Concept: IMU Orientation", group="Concept") +@Disabled +public class ConceptExploringIMUOrientation extends LinearOpMode { + static RevHubOrientationOnRobot.LogoFacingDirection[] logoFacingDirections + = RevHubOrientationOnRobot.LogoFacingDirection.values(); + static RevHubOrientationOnRobot.UsbFacingDirection[] usbFacingDirections + = RevHubOrientationOnRobot.UsbFacingDirection.values(); + static int LAST_DIRECTION = logoFacingDirections.length - 1; + static float TRIGGER_THRESHOLD = 0.2f; + + IMU imu; + int logoFacingDirectionPosition; + int usbFacingDirectionPosition; + boolean orientationIsValid = true; + + @Override public void runOpMode() throws InterruptedException { + imu = hardwareMap.get(IMU.class, "imu"); + logoFacingDirectionPosition = 0; // Up + usbFacingDirectionPosition = 2; // Forward + + updateOrientation(); + + boolean justChangedLogoDirection = false; + boolean justChangedUsbDirection = false; + + // Loop until stop requested + while (!isStopRequested()) { + + // Check to see if Yaw reset is requested (Y button) + if (gamepad1.y) { + telemetry.addData("Yaw", "Resetting\n"); + imu.resetYaw(); + } else { + telemetry.addData("Yaw", "Press Y (triangle) on Gamepad to reset.\n"); + } + + // Check to see if new Logo Direction is requested + if (gamepad1.left_bumper || gamepad1.right_bumper) { + if (!justChangedLogoDirection) { + justChangedLogoDirection = true; + if (gamepad1.left_bumper) { + logoFacingDirectionPosition--; + if (logoFacingDirectionPosition < 0) { + logoFacingDirectionPosition = LAST_DIRECTION; + } + } else { + logoFacingDirectionPosition++; + if (logoFacingDirectionPosition > LAST_DIRECTION) { + logoFacingDirectionPosition = 0; + } + } + updateOrientation(); + } + } else { + justChangedLogoDirection = false; + } + + // Check to see if new USB Direction is requested + if (gamepad1.left_trigger > TRIGGER_THRESHOLD || gamepad1.right_trigger > TRIGGER_THRESHOLD) { + if (!justChangedUsbDirection) { + justChangedUsbDirection = true; + if (gamepad1.left_trigger > TRIGGER_THRESHOLD) { + usbFacingDirectionPosition--; + if (usbFacingDirectionPosition < 0) { + usbFacingDirectionPosition = LAST_DIRECTION; + } + } else { + usbFacingDirectionPosition++; + if (usbFacingDirectionPosition > LAST_DIRECTION) { + usbFacingDirectionPosition = 0; + } + } + updateOrientation(); + } + } else { + justChangedUsbDirection = false; + } + + // Display User instructions and IMU data + telemetry.addData("logo Direction (set with bumpers)", logoFacingDirections[logoFacingDirectionPosition]); + telemetry.addData("usb Direction (set with triggers)", usbFacingDirections[usbFacingDirectionPosition] + "\n"); + + if (orientationIsValid) { + YawPitchRollAngles orientation = imu.getRobotYawPitchRollAngles(); + AngularVelocity angularVelocity = imu.getRobotAngularVelocity(AngleUnit.DEGREES); + + telemetry.addData("Yaw (Z)", "%.2f Deg. (Heading)", orientation.getYaw(AngleUnit.DEGREES)); + telemetry.addData("Pitch (X)", "%.2f Deg.", orientation.getPitch(AngleUnit.DEGREES)); + telemetry.addData("Roll (Y)", "%.2f Deg.\n", orientation.getRoll(AngleUnit.DEGREES)); + telemetry.addData("Yaw (Z) velocity", "%.2f Deg/Sec", angularVelocity.zRotationRate); + telemetry.addData("Pitch (X) velocity", "%.2f Deg/Sec", angularVelocity.xRotationRate); + telemetry.addData("Roll (Y) velocity", "%.2f Deg/Sec", angularVelocity.yRotationRate); + } else { + telemetry.addData("Error", "Selected orientation on robot is invalid"); + } + + telemetry.update(); + } + } + + // apply any requested orientation changes. + void updateOrientation() { + RevHubOrientationOnRobot.LogoFacingDirection logo = logoFacingDirections[logoFacingDirectionPosition]; + RevHubOrientationOnRobot.UsbFacingDirection usb = usbFacingDirections[usbFacingDirectionPosition]; + try { + RevHubOrientationOnRobot orientationOnRobot = new RevHubOrientationOnRobot(logo, usb); + imu.initialize(new IMU.Parameters(orientationOnRobot)); + orientationIsValid = true; + } catch (IllegalArgumentException e) { + orientationIsValid = false; + } + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptGamepadEdgeDetection.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptGamepadEdgeDetection.java new file mode 100644 index 0000000..d3c4417 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptGamepadEdgeDetection.java @@ -0,0 +1,108 @@ +/* +Copyright (c) 2024 Miriam Sinton-Remes + +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, +are permitted (subject to the limitations in the disclaimer below) provided that +the following conditions are met: + +Redistributions of source code must retain the above copyright notice, this list +of conditions and the following disclaimer. + +Redistributions in binary form must reproduce the above copyright notice, this +list of conditions and the following disclaimer in the documentation and/or +other materials provided with the distribution. + +Neither the name of FIRST nor the names of its contributors may be used to +endorse or promote products derived from this software without specific prior +written permission. + +NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS +LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESSFOR A PARTICULAR PURPOSE +ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF +THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +/* + * This OpMode illustrates using edge detection on a gamepad. + * + * Simply checking the state of a gamepad button each time could result in triggering an effect + * multiple times. Edge detection ensures that you only detect one button press, regardless of how + * long the button is held. + * + * There are two main types of edge detection. Rising edge detection will trigger when a button is + * first pressed. Falling edge detection will trigger when the button is released. + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list. + */ + +@Disabled +@TeleOp(name="Concept: Gamepad Edge Detection", group ="Concept") +public class ConceptGamepadEdgeDetection extends LinearOpMode { + + @Override + public void runOpMode() { + // Wait for the DS start button to be pressed + waitForStart(); + + while (opModeIsActive()) { + // Update the telemetry + telemetryButtonData(); + + // Wait 2 seconds before doing another check + sleep(2000); + } + } + + public void telemetryButtonData() { + // Add the status of the Gamepad 1 Left Bumper + telemetry.addData("Gamepad 1 Left Bumper Pressed", gamepad1.leftBumperWasPressed()); + telemetry.addData("Gamepad 1 Left Bumper Released", gamepad1.leftBumperWasReleased()); + telemetry.addData("Gamepad 1 Left Bumper Status", gamepad1.left_bumper); + + // Add an empty line to separate the buttons in telemetry + telemetry.addLine(); + + // Add the status of the Gamepad 1 Right Bumper + telemetry.addData("Gamepad 1 Right Bumper Pressed", gamepad1.rightBumperWasPressed()); + telemetry.addData("Gamepad 1 Right Bumper Released", gamepad1.rightBumperWasReleased()); + telemetry.addData("Gamepad 1 Right Bumper Status", gamepad1.right_bumper); + + // Add an empty line to separate the buttons in telemetry + telemetry.addLine(); + + // Add the status of the Gamepad 1 Left trigger + telemetry.addData("Gamepad 1 Left Trigger Pressed", gamepad1.leftTriggerWasPressed()); + telemetry.addData("Gamepad 1 Left Trigger Released", gamepad1.leftTriggerWasReleased()); + telemetry.addData("Gamepad 1 Left Trigger Status", gamepad1.left_trigger_pressed); + + // Add an empty line to separate the buttons in telemetry + telemetry.addLine(); + + // Add the status of the Gamepad 1 Right trigger + telemetry.addData("Gamepad 1 Right Trigger Pressed", gamepad1.rightTriggerWasPressed()); + telemetry.addData("Gamepad 1 Right Trigger Released", gamepad1.rightTriggerWasReleased()); + telemetry.addData("Gamepad 1 Right Trigger Status", gamepad1.right_trigger_pressed); + + // Add a note that the telemetry is only updated every 2 seconds + telemetry.addLine("\nTelemetry is updated every 2 seconds."); + + // Update the telemetry on the DS screen + telemetry.update(); + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptGamepadRumble.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptGamepadRumble.java new file mode 100644 index 0000000..cf846e1 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptGamepadRumble.java @@ -0,0 +1,200 @@ +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.Gamepad; +import com.qualcomm.robotcore.util.ElapsedTime; + +/* + * This OpMode illustrates using the rumble feature of many gamepads. + * + * Note: Some gamepads "rumble" better than others. + * The Xbox & PS4 have a left (rumble1) and right (rumble2) rumble motor. + * These two gamepads have two distinct rumble modes: Large on the left, and small on the right + * The Etpark gamepad may only respond to rumble1, and may only run at full power. + * The Logitech F310 gamepad does not have *any* rumble ability. + * + * Moral: You should use this sample to experiment with your specific gamepads to explore their rumble features. + * + * The rumble motors are accessed through the standard gamepad1 and gamepad2 objects. + * Several new methods were added to the Gamepad class in FTC SDK Rev 7 + * The key methods are as follows: + * + * .rumble(double rumble1, double rumble2, int durationMs) + * This method sets the rumble power of both motors for a specific time duration. + * Both rumble arguments are motor-power levels in the 0.0 to 1.0 range. + * durationMs is the desired length of the rumble action in milliseconds. + * This method returns immediately. + * Note: + * Use a durationMs of Gamepad.RUMBLE_DURATION_CONTINUOUS to provide a continuous rumble + * Use a power of 0, or duration of 0 to stop a rumble. + * + * .rumbleBlips(int count) allows an easy way to signal the driver with a series of rumble blips. + * Just specify how many blips you want. + * This method returns immediately. + * + * .runRumbleEffect(customRumbleEffect) allows you to run a custom rumble sequence that you have + * built using the Gamepad.RumbleEffect.Builder() + * A "custom effect" is a sequence of steps, where each step can rumble any of the + * rumble motors for a specific period at a specific power level. + * The Custom Effect will play as the (un-blocked) OpMode continues to run + * + * .isRumbling() returns true if there is a rumble effect in progress. + * Use this call to prevent stepping on a current rumble. + * + * .stopRumble() Stop any ongoing rumble or custom rumble effect. + * + * .rumble(int durationMs) Full power rumble for fixed duration. + * + * Note: Whenever a new Rumble command is issued, any currently executing rumble action will + * be truncated, and the new action started immediately. Take these precautions: + * 1) Do Not SPAM the rumble motors by issuing rapid fire commands + * 2) Multiple sources for rumble commands must coordinate to avoid tromping on each other. + * + * This can be achieved several possible ways: + * 1) Only having one source for rumble actions + * 2) Issuing rumble commands on transitions, rather than states. + * e.g. The moment a touch sensor is pressed, rather than the entire time it is being pressed. + * 3) Scheduling rumble commands based on timed events. e.g. 10 seconds prior to endgame + * 4) Rumble on non-overlapping mechanical actions. e.g. arm fully-extended or fully-retracted. + * 5) Use isRumbling() to hold off on a new rumble if one is already in progress. + * + * The examples shown here are representstive of how to invoke a gamepad rumble. + * It is assumed that these will be modified to suit the specific robot and team strategy needs. + * + * ######## Read the telemetry display on the Driver Station Screen for instructions. ###### + * + * Ex 1) This example shows a) how to create a custom rumble effect, and then b) how to trigger it based + * on game time. One use for this might be to alert the driver that half-time or End-game is approaching. + * + * Ex 2) This example shows tying the rumble power to a changing sensor value. + * In this case it is the Gamepad trigger, but it could be any sensor output scaled to the 0-1 range. + * Since it takes over the rumble motors, it is only performed when the Left Bumper is pressed. + * Note that this approach MUST include a way to turn OFF the rumble when the button is released. + * + * Ex 3) This example shows a simple way to trigger a 3-blip sequence. In this case it is + * triggered by the gamepad A (Cross) button, but it could be any sensor, like a touch or light sensor. + * Note that this code ensures that it only rumbles once when the input goes true. + * + * Ex 4) This example shows how to trigger a single rumble when an input value gets over a certain value. + * In this case it is reading the Right Trigger, but it could be any variable sensor, like a + * range sensor, or position sensor. The code needs to ensure that it is only triggered once, so + * it waits till the sensor drops back below the threshold before it can trigger again. + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list. + */ + +@Disabled +@TeleOp(name="Concept: Gamepad Rumble", group ="Concept") +public class ConceptGamepadRumble extends LinearOpMode +{ + boolean lastA = false; // Use to track the prior button state. + boolean lastLB = false; // Use to track the prior button state. + boolean highLevel = false; // used to prevent multiple level-based rumbles. + boolean secondHalf = false; // Use to prevent multiple half-time warning rumbles. + + Gamepad.RumbleEffect customRumbleEffect; // Use to build a custom rumble sequence. + ElapsedTime runtime = new ElapsedTime(); // Use to determine when end game is starting. + + final double HALF_TIME = 60.0; // Wait this many seconds before rumble-alert for half-time. + final double TRIGGER_THRESHOLD = 0.75; // Squeeze more than 3/4 to get rumble. + + @Override + public void runOpMode() + { + // Example 1. a) start by creating a three-pulse rumble sequence: right, LEFT, LEFT + customRumbleEffect = new Gamepad.RumbleEffect.Builder() + .addStep(0.0, 1.0, 500) // Rumble right motor 100% for 500 mSec + .addStep(0.0, 0.0, 300) // Pause for 300 mSec + .addStep(1.0, 0.0, 250) // Rumble left motor 100% for 250 mSec + .addStep(0.0, 0.0, 250) // Pause for 250 mSec + .addStep(1.0, 0.0, 250) // Rumble left motor 100% for 250 mSec + .build(); + + telemetry.addData(">", "Press Start"); + telemetry.update(); + + waitForStart(); + runtime.reset(); // Start game timer. + + // Loop while monitoring buttons for rumble triggers + while (opModeIsActive()) + { + // Read and save the current gamepad button states. + boolean currentA = gamepad1.a ; + boolean currentLB = gamepad1.left_bumper ; + + // Display the current Rumble status. Just for interest. + telemetry.addData(">", "Are we RUMBLING? %s\n", gamepad1.isRumbling() ? "YES" : "no" ); + + // ---------------------------------------------------------------------------------------- + // Example 1. b) Watch the runtime timer, and run the custom rumble when we hit half-time. + // Make sure we only signal once by setting "secondHalf" flag to prevent further rumbles. + // ---------------------------------------------------------------------------------------- + if ((runtime.seconds() > HALF_TIME) && !secondHalf) { + gamepad1.runRumbleEffect(customRumbleEffect); + secondHalf =true; + } + + // Display the time remaining while we are still counting down. + if (!secondHalf) { + telemetry.addData(">", "Halftime Alert Countdown: %3.0f Sec \n", (HALF_TIME - runtime.seconds()) ); + } + + + // ---------------------------------------------------------------------------------------- + // Example 2. If Left Bumper is being pressed, power the rumble motors based on the two trigger depressions. + // This is useful to see how the rumble feels at various power levels. + // ---------------------------------------------------------------------------------------- + if (currentLB) { + // Left Bumper is being pressed, so send left and right "trigger" values to left and right rumble motors. + gamepad1.rumble(gamepad1.left_trigger, gamepad1.right_trigger, Gamepad.RUMBLE_DURATION_CONTINUOUS); + + // Show what is being sent to rumbles + telemetry.addData(">", "Squeeze triggers to control rumbles"); + telemetry.addData("> : Rumble", "Left: %.0f%% Right: %.0f%%", gamepad1.left_trigger * 100, gamepad1.right_trigger * 100); + } else { + // Make sure rumble is turned off when Left Bumper is released (only one time each press) + if (lastLB) { + gamepad1.stopRumble(); + } + + // Prompt for manual rumble action + telemetry.addData(">", "Hold Left-Bumper to test Manual Rumble"); + telemetry.addData(">", "Press A (Cross) for three blips"); + telemetry.addData(">", "Squeeze right trigger slowly for 1 blip"); + } + lastLB = currentLB; // remember the current button state for next time around the loop + + + // ---------------------------------------------------------------------------------------- + // Example 3. Blip 3 times at the moment that A (Cross) is pressed. (look for pressed transition) + // BUT !!! Skip it altogether if the Gamepad is already rumbling. + // ---------------------------------------------------------------------------------------- + if (currentA && !lastA) { + if (!gamepad1.isRumbling()) // Check for possible overlap of rumbles. + gamepad1.rumbleBlips(3); + } + lastA = currentA; // remember the current button state for next time around the loop + + + // ---------------------------------------------------------------------------------------- + // Example 4. Rumble once when gamepad right trigger goes above the THRESHOLD. + // ---------------------------------------------------------------------------------------- + if (gamepad1.right_trigger > TRIGGER_THRESHOLD) { + if (!highLevel) { + gamepad1.rumble(0.9, 0, 200); // 200 mSec burst on left motor. + highLevel = true; // Hold off any more triggers + } + } else { + highLevel = false; // We can trigger again now. + } + + // Send the telemetry data to the Driver Station, and then pause to pace the program. + telemetry.update(); + sleep(10); + } + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptGamepadTouchpad.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptGamepadTouchpad.java new file mode 100644 index 0000000..84d8cec --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptGamepadTouchpad.java @@ -0,0 +1,77 @@ +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import org.firstinspires.ftc.robotcore.external.Telemetry; + +/* + * This OpMode illustrates using the touchpad feature found on some gamepads. + * + * The Sony PS4 gamepad can detect two distinct touches on the central touchpad. + * Other gamepads with different touchpads may provide mixed results. + * + * The touchpads are accessed through the standard gamepad1 and gamepad2 objects. + * Several new members were added to the Gamepad class in FTC SDK Rev 7 + * + * .touchpad_finger_1 returns true if at least one finger is detected. + * .touchpad_finger_1_x finger 1 X coordinate. Valid if touchpad_finger_1 is true + * .touchpad_finger_1_y finger 1 Y coordinate. Valid if touchpad_finger_1 is true + * + * .touchpad_finger_2 returns true if a second finger is detected + * .touchpad_finger_2_x finger 2 X coordinate. Valid if touchpad_finger_2 is true + * .touchpad_finger_2_y finger 2 Y coordinate. Valid if touchpad_finger_2 is true + * + * Finger touches are reported with an X and Y coordinate in following coordinate system. + * + * 1) X is the Horizontal axis, and Y is the vertical axis + * 2) The 0,0 origin is at the center of the touchpad. + * 3) 1.0, 1.0 is at the top right corner of the touchpad. + * 4) -1.0,-1.0 is at the bottom left corner of the touchpad. + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list. + */ + +@Disabled +@TeleOp(name="Concept: Gamepad Touchpad", group ="Concept") +public class ConceptGamepadTouchpad extends LinearOpMode +{ + @Override + public void runOpMode() + { + telemetry.setDisplayFormat(Telemetry.DisplayFormat.MONOSPACE); + + telemetry.addData(">", "Press Start"); + telemetry.update(); + + waitForStart(); + + while (opModeIsActive()) + { + boolean finger = false; + + // Display finger 1 x & y position if finger detected + if(gamepad1.touchpad_finger_1) + { + finger = true; + telemetry.addLine(String.format("Finger 1: x=%5.2f y=%5.2f\n", gamepad1.touchpad_finger_1_x, gamepad1.touchpad_finger_1_y)); + } + + // Display finger 2 x & y position if finger detected + if(gamepad1.touchpad_finger_2) + { + finger = true; + telemetry.addLine(String.format("Finger 2: x=%5.2f y=%5.2f\n", gamepad1.touchpad_finger_2_x, gamepad1.touchpad_finger_2_y)); + } + + if(!finger) + { + telemetry.addLine("No fingers"); + } + + telemetry.update(); + sleep(10); + } + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptLEDStick.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptLEDStick.java new file mode 100644 index 0000000..01729bb --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptLEDStick.java @@ -0,0 +1,123 @@ +package org.firstinspires.ftc.robotcontroller.external.samples; +/* + Copyright (c) 2021-24 Alan Smith + + All rights reserved. + + Redistribution and use in source and binary forms, with or without modification, + are permitted (subject to the limitations in the disclaimer below) provided that + the following conditions are met: + + Redistributions of source code must retain the above copyright notice, this list + of conditions and the following disclaimer. + + Redistributions in binary form must reproduce the above copyright notice, this + list of conditions and the following disclaimer in the documentation and/or + other materials provided with the distribution. + + Neither the name of Alan Smith nor the names of its contributors may be used to + endorse or promote products derived from this software without specific prior + written permission. + + NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESSFOR A PARTICULAR PURPOSE + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR + TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF + THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ + +import android.graphics.Color; + +import com.qualcomm.hardware.sparkfun.SparkFunLEDStick; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.util.Range; + +/* + * This OpMode illustrates how to use the SparkFun QWIIC LED Strip + * + * This is a simple way to add a strip of 10 LEDs to your robot where you can set the color of each + * LED or the whole strip. This allows for driver feedback or even just fun ways to show your team + * colors. + * + * Why? + * Because more LEDs == more fun!! + * + * This OpMode assumes that the QWIIC LED Stick is attached to an I2C interface named "back_leds" in the robot configuration. + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list + * + * You can buy this product here: https://www.sparkfun.com/products/18354 + * Don't forget to also buy this to make it easy to connect to your Control or Expansion Hub: + * https://www.sparkfun.com/products/25596 + */ +@TeleOp(name = "Concept: LED Stick", group = "Concept") +@Disabled +public class ConceptLEDStick extends OpMode { + private boolean wasUp; + private boolean wasDown; + private int brightness = 5; // this needs to be between 0 and 31 + private final static double END_GAME_TIME = 120 - 30; + + private SparkFunLEDStick ledStick; + + @Override + public void init() { + ledStick = hardwareMap.get(SparkFunLEDStick.class, "back_leds"); + ledStick.setBrightness(brightness); + ledStick.setColor(Color.GREEN); + } + + @Override + public void start() { + resetRuntime(); + } + + @Override + public void loop() { + telemetry.addLine("Hold the A button to turn blue"); + telemetry.addLine("Hold the B button to turn red"); + telemetry.addLine("Hold the left bumper to turn off"); + telemetry.addLine("Use DPAD Up/Down to change brightness"); + + if (getRuntime() > END_GAME_TIME) { + int[] ledColors = {Color.RED, Color.YELLOW, Color.RED, Color.YELLOW, Color.RED, + Color.YELLOW, Color.RED, Color.YELLOW, Color.RED, Color.YELLOW}; + ledStick.setColors(ledColors); + } else if (gamepad1.a) { + ledStick.setColor(Color.BLUE); + } else if (gamepad1.b) { + ledStick.setColor(Color.RED); + } else if (gamepad1.left_bumper) { + ledStick.turnAllOff(); + } else { + ledStick.setColor(Color.GREEN); + } + + /* + * Use DPAD up and down to change brightness + */ + int newBrightness = brightness; + if (gamepad1.dpad_up && !wasUp) { + newBrightness = brightness + 1; + } else if (gamepad1.dpad_down && !wasDown) { + newBrightness = brightness - 1; + } + if (newBrightness != brightness) { + brightness = Range.clip(newBrightness, 0, 31); + ledStick.setBrightness(brightness); + } + telemetry.addData("Brightness", brightness); + + wasDown = gamepad1.dpad_down; + wasUp = gamepad1.dpad_up; + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptMotorBulkRead.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptMotorBulkRead.java new file mode 100644 index 0000000..5439f78 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptMotorBulkRead.java @@ -0,0 +1,227 @@ +/* Copyright (c) 2019 Phil Malone. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.hardware.lynx.LynxModule; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotorEx; +import com.qualcomm.robotcore.util.ElapsedTime; + +import java.util.List; + +/* + * This OpMode illustrates how to use the Expansion Hub's Bulk-Read feature to speed up control cycle times. + * In this example there are 4 motors that need their encoder positions, and velocities read. + * The sample is written to work with one or two expansion hubs, with no assumption as to where the motors are located. + * + * Three scenarios are tested: + * Cache Mode = OFF This is the normal default, where no cache is used, and every read produces a discrete transaction with + * an expansion hub, which is the slowest approach, but guarentees that the value is as fresh (recent) as possible.. + * + * Cache Mode = AUTO This mode will attempt to minimize the number of discrete read commands, by performing bulk-reads + * and then returning values that have been cached. The cache is updated automatically whenever any specific encoder is re-read. + * This mode will always return new data, but it may perform more bulk-reads than absolutely required. + * Extra reads will be performed if multiple encoder/velocity reads are performed on the same encoder in one control cycle. + * This mode is a good compromise between the OFF and MANUAL modes. + * Note: If there are significant user-program delays between encoder reads, the cached value may not be fresh (recent). + * You can issue a clearBulkCache() call at any time force a fresh bulk-read on the next encoder read. + * + * Cache Mode = MANUAL This mode requires the user's code to determine the best time to clear the cached bulk-read data. + * Well organized code will reset the cache once at the beginning of the control cycle, and then immediately read and store all the encoder values. + * This approach will produce the shortest cycle times, but it does require the user to manually clear the cache. + * Since NO automatic Bulk-Reads are performed, neglecting to clear the bulk cache will result in the same values being returned + * each time an encoder read is performed. + * + * ------------------------------------- + * + * General tip to speed up your control cycles: + * + * No matter what method you use to read encoders and other inputs, you should try to + * avoid reading the same encoder input multiple times around a control loop. + * Under normal conditions, this will slow down the control loop. + * The preferred method is to read all the required inputs ONCE at the beginning of the loop, + * and save the values in variable that can be used by other parts of the control code. + * + * eg: if you are sending encoder positions to your telemetry display, putting a getCurrentPosition() + * call in the telemetry statement will force the code to go and get another copy which will take time. + * It's much better read the position into a variable once, and use that variable for control AND display. + * Reading saved variables takes no time at all. + * + * Once you put all your sensor reads at the beginning of the control cycle, it's very easy to use + * the bulk-read AUTO mode to streamline your cycle timing. + */ +@TeleOp (name = "Motor Bulk Reads", group = "Tests") +@Disabled +public class ConceptMotorBulkRead extends LinearOpMode { + + final int TEST_CYCLES = 500; // Number of control cycles to run to determine cycle times. + + private DcMotorEx m1, m2, m3, m4; // Motor Objects + private long e1, e2, e3, e4; // Encoder Values + private double v1, v2, v3, v4; // Velocities + + // Cycle Times + double t1 = 0; + double t2 = 0; + double t3 = 0; + + @Override + public void runOpMode() { + + int cycles; + + // Important Step 1: Make sure you use DcMotorEx when you instantiate your motors. + m1 = hardwareMap.get(DcMotorEx.class, "m1"); // Configure the robot to use these 4 motor names, + m2 = hardwareMap.get(DcMotorEx.class, "m2"); // or change these strings to match your existing Robot Configuration. + m3 = hardwareMap.get(DcMotorEx.class, "m3"); + m4 = hardwareMap.get(DcMotorEx.class, "m4"); + + // Important Step 2: Get access to a list of Expansion Hub Modules to enable changing caching methods. + List allHubs = hardwareMap.getAll(LynxModule.class); + + ElapsedTime timer = new ElapsedTime(); + + telemetry.addData(">", "Press START to start tests"); + telemetry.addData(">", "Test results will update for each access method."); + telemetry.update(); + waitForStart(); + + // -------------------------------------------------------------------------------------- + // Run control loop using legacy encoder reads + // In this mode, a single read is done for each encoder position, and a bulk read is done for each velocity read. + // This is the worst case scenario. + // This is the same as using LynxModule.BulkCachingMode.OFF + // -------------------------------------------------------------------------------------- + + displayCycleTimes("Test 1 of 3 (Wait for completion)"); + + timer.reset(); + cycles = 0; + while (opModeIsActive() && (cycles++ < TEST_CYCLES)) { + e1 = m1.getCurrentPosition(); + e2 = m2.getCurrentPosition(); + e3 = m3.getCurrentPosition(); + e4 = m4.getCurrentPosition(); + + v1 = m1.getVelocity(); + v2 = m2.getVelocity(); + v3 = m3.getVelocity(); + v4 = m4.getVelocity(); + + // Put Control loop action code here. + + } + // calculate the average cycle time. + t1 = timer.milliseconds() / cycles; + displayCycleTimes("Test 2 of 3 (Wait for completion)"); + + // -------------------------------------------------------------------------------------- + // Run test cycles using AUTO cache mode + // In this mode, only one bulk read is done per cycle, UNLESS you read a specific encoder/velocity item AGAIN in that cycle. + // -------------------------------------------------------------------------------------- + + // Important Step 3: Option A. Set all Expansion hubs to use the AUTO Bulk Caching mode + for (LynxModule module : allHubs) { + module.setBulkCachingMode(LynxModule.BulkCachingMode.AUTO); + } + + timer.reset(); + cycles = 0; + while (opModeIsActive() && (cycles++ < TEST_CYCLES)) { + e1 = m1.getCurrentPosition(); // Uses 1 bulk-read for all 4 encoder/velocity reads, + e2 = m2.getCurrentPosition(); // but don't do any `get` operations more than once per cycle. + e3 = m3.getCurrentPosition(); + e4 = m4.getCurrentPosition(); + + v1 = m1.getVelocity(); + v2 = m2.getVelocity(); + v3 = m3.getVelocity(); + v4 = m4.getVelocity(); + + // Put Control loop action code here. + + } + // calculate the average cycle time. + t2 = timer.milliseconds() / cycles; + displayCycleTimes("Test 3 of 3 (Wait for completion)"); + + // -------------------------------------------------------------------------------------- + // Run test cycles using MANUAL cache mode + // In this mode, only one block read is done each control cycle. + // This is the MOST efficient method, but it does require that the cache is cleared manually each control cycle. + // -------------------------------------------------------------------------------------- + + // Important Step 3: Option B. Set all Expansion hubs to use the MANUAL Bulk Caching mode + for (LynxModule module : allHubs) { + module.setBulkCachingMode(LynxModule.BulkCachingMode.MANUAL); + } + + timer.reset(); + cycles = 0; + while (opModeIsActive() && (cycles++ < TEST_CYCLES)) { + + // Important Step 4: If you are using MANUAL mode, you must clear the BulkCache once per control cycle + for (LynxModule module : allHubs) { + module.clearBulkCache(); + } + + e1 = m1.getCurrentPosition(); // Uses 1 bulk-read to obtain ALL the motor data + e2 = m2.getCurrentPosition(); // There is no penalty for doing more `get` operations in this cycle, + e3 = m3.getCurrentPosition(); // but they will return the same data. + e4 = m4.getCurrentPosition(); + + v1 = m1.getVelocity(); + v2 = m2.getVelocity(); + v3 = m3.getVelocity(); + v4 = m4.getVelocity(); + + // Put Control loop action code here. + + } + // calculate the average cycle time. + t3 = timer.milliseconds() / cycles; + displayCycleTimes("Complete"); + + // wait until op-mode is stopped by user, before clearing display. + while (opModeIsActive()) ; + } + + // Display three comparison times. + void displayCycleTimes(String status) { + telemetry.addData("Testing", status); + telemetry.addData("Cache = OFF", "%5.1f mS/cycle", t1); + telemetry.addData("Cache = AUTO", "%5.1f mS/cycle", t2); + telemetry.addData("Cache = MANUAL", "%5.1f mS/cycle", t3); + telemetry.update(); + } +} + diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptNullOp.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptNullOp.java new file mode 100644 index 0000000..4a887bd --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptNullOp.java @@ -0,0 +1,89 @@ +/* Copyright (c) 2017 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.util.ElapsedTime; + +/* + * Demonstrates an empty iterative OpMode + */ +@TeleOp(name = "Concept: NullOp", group = "Concept") +@Disabled +public class ConceptNullOp extends OpMode { + + private ElapsedTime runtime = new ElapsedTime(); + + /** + * This method will be called once, when the INIT button is pressed. + */ + @Override + public void init() { + telemetry.addData("Status", "Initialized"); + } + + /** + * This method will be called repeatedly during the period between when + * the INIT button is pressed and when the START button is pressed (or the + * OpMode is stopped). + */ + @Override + public void init_loop() { + } + + /** + * This method will be called once, when the START button is pressed. + */ + @Override + public void start() { + runtime.reset(); + } + + /** + * This method will be called repeatedly during the period between when + * the START button is pressed and when the OpMode is stopped. + */ + @Override + public void loop() { + telemetry.addData("Status", "Run Time: " + runtime.toString()); + } + + /** + * This method will be called once, when this OpMode is stopped. + *

+ * Your ability to control hardware from this method will be limited. + */ + @Override + public void stop() { + + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptRampMotorSpeed.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptRampMotorSpeed.java new file mode 100644 index 0000000..6e0be37 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptRampMotorSpeed.java @@ -0,0 +1,114 @@ +/* Copyright (c) 2017 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; + +/* + * This OpMode ramps a single motor speed up and down repeatedly until Stop is pressed. + * The code is structured as a LinearOpMode + * + * This code assumes a DC motor configured with the name "left_drive" as is found on a Robot. + * + * INCREMENT sets how much to increase/decrease the power each cycle + * CYCLE_MS sets the update period. + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list + */ +@TeleOp(name = "Concept: Ramp Motor Speed", group = "Concept") +@Disabled +public class ConceptRampMotorSpeed extends LinearOpMode { + + static final double INCREMENT = 0.01; // amount to ramp motor each CYCLE_MS cycle + static final int CYCLE_MS = 50; // period of each cycle + static final double MAX_FWD = 1.0; // Maximum FWD power applied to motor + static final double MAX_REV = -1.0; // Maximum REV power applied to motor + + // Define class members + DcMotor motor; + double power = 0; + boolean rampUp = true; + + + @Override + public void runOpMode() { + + // Connect to motor (Assume standard left wheel) + // Change the text in quotes to match any motor name on your robot. + motor = hardwareMap.get(DcMotor.class, "left_drive"); + + // Wait for the start button + telemetry.addData(">", "Press Start to run Motors." ); + telemetry.update(); + waitForStart(); + + // Ramp motor speeds till stop pressed. + while(opModeIsActive()) { + + // Ramp the motors, according to the rampUp variable. + if (rampUp) { + // Keep stepping up until we hit the max value. + power += INCREMENT ; + if (power >= MAX_FWD ) { + power = MAX_FWD; + rampUp = !rampUp; // Switch ramp direction + } + } + else { + // Keep stepping down until we hit the min value. + power -= INCREMENT ; + if (power <= MAX_REV ) { + power = MAX_REV; + rampUp = !rampUp; // Switch ramp direction + } + } + + // Display the current value + telemetry.addData("Motor Power", "%5.2f", power); + telemetry.addData(">", "Press Stop to end test." ); + telemetry.update(); + + // Set the motor to the new power and pause; + motor.setPower(power); + sleep(CYCLE_MS); + idle(); + } + + // Turn off motor and signal done; + motor.setPower(0); + telemetry.addData(">", "Done"); + telemetry.update(); + + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptRevLED.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptRevLED.java new file mode 100644 index 0000000..9c168d5 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptRevLED.java @@ -0,0 +1,78 @@ +/* + Copyright (c) 2024 Alan Smith + All rights reserved. + Redistribution and use in source and binary forms, with or without modification, + are permitted (subject to the limitations in the disclaimer below) provided that + the following conditions are met: + Redistributions of source code must retain the above copyright notice, this list + of conditions and the following disclaimer. + Redistributions in binary form must reproduce the above copyright notice, this + list of conditions and the following disclaimer in the documentation and/or + other materials provided with the distribution. + Neither the name of Alan Smith nor the names of its contributors may be used to + endorse or promote products derived from this software without specific prior + written permission. + NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESSFOR A PARTICULAR PURPOSE + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR + TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF + THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ +package org.firstinspires.ftc.robotcontroller.external.samples; + +/* + * This OpMode illustrates how to use the REV Digital Indicator + * + * This is a simple way to add the REV Digital Indicator which has a red and green LED. + * (and as you may remember, red + green = yellow so when they are both on you can get yellow) + * + * Why? + * You can use this to show information to the driver. For example, green might be that you can + * pick up more game elements and red would be that you already have the possession limit. + * + * This OpMode assumes that the REV Digital Indicator is setup as 2 Digital Channels named + * front_led_green and front_led_red. (the green should be the lower of the 2 channels it is plugged + * into and the red should be the higher) + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list + * + * You can buy this product here: https://www.revrobotics.com/rev-31-2010/ + */ +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.LED; + +@TeleOp(name = "Concept: RevLED", group = "Concept") +@Disabled +public class ConceptRevLED extends OpMode { + LED frontLED_red; + LED frontLED_green; + + @Override + public void init() { + frontLED_green = hardwareMap.get(LED.class, "front_led_green"); + frontLED_red = hardwareMap.get(LED.class, "front_led_red"); + } + + @Override + public void loop() { + if (gamepad1.a) { + frontLED_red.on(); + } else { + frontLED_red.off(); + } + if (gamepad1.b) { + frontLED_green.on(); + } else { + frontLED_green.off(); + } + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptRevSPARKMini.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptRevSPARKMini.java new file mode 100644 index 0000000..798d689 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptRevSPARKMini.java @@ -0,0 +1,111 @@ +/* Copyright (c) 2018 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotorSimple; +import com.qualcomm.robotcore.util.ElapsedTime; +import com.qualcomm.robotcore.util.Range; + + +/* + * This OpMode demonstrates a POV Drive system, with commented-out code for a Tank Drive system, + * for a two wheeled robot using two REV SPARKminis. + * To use this example, connect two REV SPARKminis into servo ports on the Control Hub. On the + * robot configuration, use the drop down list under 'Servos' to select 'REV SPARKmini Controller' + * and name them 'left_drive' and 'right_drive'. + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list + */ + +@TeleOp(name="REV SPARKmini Simple Drive Example", group="Concept") +@Disabled +public class ConceptRevSPARKMini extends LinearOpMode { + + // Declare OpMode members. + private ElapsedTime runtime = new ElapsedTime(); + private DcMotorSimple leftDrive = null; + private DcMotorSimple rightDrive = null; + + @Override + public void runOpMode() { + telemetry.addData("Status", "Initialized"); + telemetry.update(); + + // Initialize the hardware variables. Note that the strings used here as parameters + // to 'get' must correspond to the names assigned during robot configuration. + leftDrive = hardwareMap.get(DcMotorSimple.class, "left_drive"); + rightDrive = hardwareMap.get(DcMotorSimple.class, "right_drive"); + + // Most robots need the motor on one side to be reversed to drive forward + // Reverse the motor that runs backward when connected directly to the battery + leftDrive.setDirection(DcMotorSimple.Direction.FORWARD); + rightDrive.setDirection(DcMotorSimple.Direction.REVERSE); + + // Wait for the game to start (driver presses START) + waitForStart(); + runtime.reset(); + + // run until the end of the match (driver presses STOP) + while (opModeIsActive()) { + + // Setup a variable for each drive wheel to save power level for telemetry + double leftPower; + double rightPower; + + // Choose to drive using either Tank Mode, or POV Mode + // Comment out the method that's not used. The default below is POV. + + // POV Mode uses left stick to go forward, and right stick to turn. + // - This uses basic math to combine motions and is easier to drive straight. + double drive = -gamepad1.left_stick_y; + double turn = gamepad1.right_stick_x; + leftPower = Range.clip(drive + turn, -1.0, 1.0) ; + rightPower = Range.clip(drive - turn, -1.0, 1.0) ; + + // Tank Mode uses one stick to control each wheel. + // - This requires no math, but it is hard to drive forward slowly and keep straight. + // leftPower = -gamepad1.left_stick_y ; + // rightPower = -gamepad1.right_stick_y ; + + // Send calculated power to wheels + leftDrive.setPower(leftPower); + rightDrive.setPower(rightPower); + + // Show the elapsed game time and wheel power. + telemetry.addData("Status", "Run Time: " + runtime.toString()); + telemetry.addData("Motors", "left (%.2f), right (%.2f)", leftPower, rightPower); + telemetry.update(); + } + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptScanServo.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptScanServo.java new file mode 100644 index 0000000..2b8ad33 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptScanServo.java @@ -0,0 +1,115 @@ +/* Copyright (c) 2017 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.Servo; + +/* + * This OpMode scans a single servo back and forward until Stop is pressed. + * The code is structured as a LinearOpMode + * INCREMENT sets how much to increase/decrease the servo position each cycle + * CYCLE_MS sets the update period. + * + * This code assumes a Servo configured with the name "left_hand" as is found on a Robot. + * + * NOTE: When any servo position is set, ALL attached servos are activated, so ensure that any other + * connected servos are able to move freely before running this test. + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list + */ +@TeleOp(name = "Concept: Scan Servo", group = "Concept") +@Disabled +public class ConceptScanServo extends LinearOpMode { + + static final double INCREMENT = 0.01; // amount to slew servo each CYCLE_MS cycle + static final int CYCLE_MS = 50; // period of each cycle + static final double MAX_POS = 1.0; // Maximum rotational position + static final double MIN_POS = 0.0; // Minimum rotational position + + // Define class members + Servo servo; + double position = (MAX_POS - MIN_POS) / 2; // Start at halfway position + boolean rampUp = true; + + + @Override + public void runOpMode() { + + // Connect to servo (Assume Robot Left Hand) + // Change the text in quotes to match any servo name on your robot. + servo = hardwareMap.get(Servo.class, "left_hand"); + + // Wait for the start button + telemetry.addData(">", "Press Start to scan Servo." ); + telemetry.update(); + waitForStart(); + + + // Scan servo till stop pressed. + while(opModeIsActive()){ + + // slew the servo, according to the rampUp (direction) variable. + if (rampUp) { + // Keep stepping up until we hit the max value. + position += INCREMENT ; + if (position >= MAX_POS ) { + position = MAX_POS; + rampUp = !rampUp; // Switch ramp direction + } + } + else { + // Keep stepping down until we hit the min value. + position -= INCREMENT ; + if (position <= MIN_POS ) { + position = MIN_POS; + rampUp = !rampUp; // Switch ramp direction + } + } + + // Display the current value + telemetry.addData("Servo Position", "%5.2f", position); + telemetry.addData(">", "Press Stop to end test." ); + telemetry.update(); + + // Set the servo to the new position and pause; + servo.setPosition(position); + sleep(CYCLE_MS); + idle(); + } + + // Signal done; + telemetry.addData(">", "Done"); + telemetry.update(); + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptSoundsASJava.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptSoundsASJava.java new file mode 100644 index 0000000..1ceaa17 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptSoundsASJava.java @@ -0,0 +1,133 @@ +/* Copyright (c) 2018 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.ftccommon.SoundPlayer; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +/* + * This OpMode demonstrates how to play simple sounds on both the RC and DS phones. + * It illustrates how to build sounds into your application as a resource. + * This technique is best suited for use with Android Studio since it assumes you will be creating a new application + * + * If you are using OnBotJava, please see the ConceptSoundsOnBotJava sample + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list + * + * Operation: + * + * Gamepad X & B buttons are used to trigger sounds in this example, but any event can be used. + * Note: Time should be allowed for sounds to complete before playing other sounds. + * + * For sound files to be used as a compiled-in resource, they need to be located in a folder called "raw" under your "res" (resources) folder. + * You can create your own "raw" folder from scratch, or you can copy the one from the FtcRobotController module. + * + * Android Studio coders will ultimately need a folder in your path as follows: + * /TeamCode/src/main/res/raw + * + * Copy any .wav files you want to play into this folder. + * Make sure that your files ONLY use lower-case characters, and have no spaces or special characters other than underscore. + * + * The name you give your .wav files will become the resource ID for these sounds. + * eg: gold.wav becomes R.raw.gold + * + * If you wish to use the sounds provided for this sample, they are located in: + * /FtcRobotController/src/main/res/raw + * You can copy and paste the entire 'raw' folder using Android Studio. + * + */ + +@TeleOp(name="Concept: Sound Resources", group="Concept") +@Disabled +public class ConceptSoundsASJava extends LinearOpMode { + + // Declare OpMode members. + private boolean goldFound; // Sound file present flags + private boolean silverFound; + + private boolean isX = false; // Gamepad button state variables + private boolean isB = false; + + private boolean wasX = false; // Gamepad button history variables + private boolean WasB = false; + + @Override + public void runOpMode() { + + // Determine Resource IDs for sounds built into the RC application. + int silverSoundID = hardwareMap.appContext.getResources().getIdentifier("silver", "raw", hardwareMap.appContext.getPackageName()); + int goldSoundID = hardwareMap.appContext.getResources().getIdentifier("gold", "raw", hardwareMap.appContext.getPackageName()); + + // Determine if sound resources are found. + // Note: Preloading is NOT required, but it's a good way to verify all your sounds are available before you run. + if (goldSoundID != 0) + goldFound = SoundPlayer.getInstance().preload(hardwareMap.appContext, goldSoundID); + + if (silverSoundID != 0) + silverFound = SoundPlayer.getInstance().preload(hardwareMap.appContext, silverSoundID); + + // Display sound status + telemetry.addData("gold resource", goldFound ? "Found" : "NOT found\n Add gold.wav to /src/main/res/raw" ); + telemetry.addData("silver resource", silverFound ? "Found" : "Not found\n Add silver.wav to /src/main/res/raw" ); + + // Wait for the game to start (driver presses START) + telemetry.addData(">", "Press Start to continue"); + telemetry.update(); + waitForStart(); + + telemetry.addData(">", "Press X, B to play sounds."); + telemetry.update(); + + // run until the end of the match (driver presses STOP) + while (opModeIsActive()) { + + // say Silver each time gamepad X is pressed (This sound is a resource) + if (silverFound && (isX = gamepad1.x) && !wasX) { + SoundPlayer.getInstance().startPlaying(hardwareMap.appContext, silverSoundID); + telemetry.addData("Playing", "Resource Silver"); + telemetry.update(); + } + + // say Gold each time gamepad B is pressed (This sound is a resource) + if (goldFound && (isB = gamepad1.b) && !WasB) { + SoundPlayer.getInstance().startPlaying(hardwareMap.appContext, goldSoundID); + telemetry.addData("Playing", "Resource Gold"); + telemetry.update(); + } + + // Save last button states + wasX = isX; + WasB = isB; + } + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptSoundsOnBotJava.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptSoundsOnBotJava.java new file mode 100644 index 0000000..fbb7416 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptSoundsOnBotJava.java @@ -0,0 +1,120 @@ +/* Copyright (c) 2018 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.ftccommon.SoundPlayer; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import java.io.File; + +/* + * This OpMode demonstrates how to play simple sounds on both the RC and DS phones. + * It illustrates how to play sound files that have been copied to the RC Phone + * This technique is best suited for use with OnBotJava since it does not require the app to be modified. + * + * Operation: + * + * Gamepad X & B buttons are used to trigger sounds in this example, but any event can be used. + * Note: Time should be allowed for sounds to complete before playing other sounds. + * + * To play a new sound, you will need to copy the .wav files to the phone, and then provide the full path to them as part of your OpMode. + * This is done in this sample for the two sound files. silver.wav and gold.wav + * + * You can put the files in a variety of soundPaths, but we recommend you put them in the /FIRST/blocks/sounds folder. + * Your OpModes will have guaranteed access to this folder, and you can transfer files into this folder using the BLOCKS web page. + * -- There is a link called "sounds" on the right hand side of the color bar on the BLOCKS page that can be used to send sound files to this folder by default. + * Or you can use Windows File Manager, or ADB to transfer the sound files + * + * To get full use of THIS sample, you will need to copy two sound file called silver.wav and gold.wav to /FIRST/blocks/sounds on the RC phone. + * They can be located here: + * https://github.com/ftctechnh/ftc_app/tree/master/FtcRobotController/src/main/res/raw/gold.wav + * https://github.com/ftctechnh/ftc_app/tree/master/FtcRobotController/src/main/res/raw/silver.wav + */ + +@TeleOp(name="Concept: Sound Files", group="Concept") +@Disabled +public class ConceptSoundsOnBotJava extends LinearOpMode { + + // Point to sound files on the phone's drive + private String soundPath = "/FIRST/blocks/sounds"; + private File goldFile = new File("/sdcard" + soundPath + "/gold.wav"); + private File silverFile = new File("/sdcard" + soundPath + "/silver.wav"); + + // Declare OpMode members. + private boolean isX = false; // Gamepad button state variables + private boolean isB = false; + + private boolean wasX = false; // Gamepad button history variables + private boolean WasB = false; + + @Override + public void runOpMode() { + + // Make sure that the sound files exist on the phone + boolean goldFound = goldFile.exists(); + boolean silverFound = silverFile.exists(); + + // Display sound status + telemetry.addData("gold sound", goldFound ? "Found" : "NOT Found \nCopy gold.wav to " + soundPath ); + telemetry.addData("silver sound", silverFound ? "Found" : "NOT Found \nCopy silver.wav to " + soundPath ); + + // Wait for the game to start (driver presses PLAY) + telemetry.addData(">", "Press Start to continue"); + telemetry.update(); + waitForStart(); + + telemetry.addData(">", "Press X or B to play sounds."); + telemetry.update(); + + // run until the end of the match (driver presses STOP) + while (opModeIsActive()) { + + // say Silver each time gamepad X is pressed (This sound is a resource) + if (silverFound && (isX = gamepad1.x) && !wasX) { + SoundPlayer.getInstance().startPlaying(hardwareMap.appContext, silverFile); + telemetry.addData("Playing", "Silver File"); + telemetry.update(); + } + + // say Gold each time gamepad B is pressed (This sound is a resource) + if (goldFound && (isB = gamepad1.b) && !WasB) { + SoundPlayer.getInstance().startPlaying(hardwareMap.appContext, goldFile); + telemetry.addData("Playing", "Gold File"); + telemetry.update(); + } + + // Save last button states + wasX = isX; + WasB = isB; + } + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptSoundsSKYSTONE.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptSoundsSKYSTONE.java new file mode 100644 index 0000000..983e434 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptSoundsSKYSTONE.java @@ -0,0 +1,122 @@ +/* Copyright (c) 2018 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import android.content.Context; +import com.qualcomm.ftccommon.SoundPlayer; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +/* + * This OpMode demonstrates how to play one of the several SKYSTONE/Star Wars sounds loaded into the SDK. + * It does this by creating a simple "chooser" controlled by the gamepad Up Down buttons. + * This code also prevents sounds from stacking up by setting a "playing" flag, which is cleared when the sound finishes playing. + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list + * + * Operation: + * Use the DPAD to change the selected sound, and the Right Bumper to play it. + */ + +@TeleOp(name="SKYSTONE Sounds", group="Concept") +@Disabled +public class ConceptSoundsSKYSTONE extends LinearOpMode { + + // List of available sound resources + String sounds[] = {"ss_alarm", "ss_bb8_down", "ss_bb8_up", "ss_darth_vader", "ss_fly_by", + "ss_mf_fail", "ss_laser", "ss_laser_burst", "ss_light_saber", "ss_light_saber_long", "ss_light_saber_short", + "ss_light_speed", "ss_mine", "ss_power_up", "ss_r2d2_up", "ss_roger_roger", "ss_siren", "ss_wookie" }; + boolean soundPlaying = false; + + @Override + public void runOpMode() { + + // Variables for choosing from the available sounds + int soundIndex = 0; + int soundID = -1; + boolean was_dpad_up = false; + boolean was_dpad_down = false; + + Context myApp = hardwareMap.appContext; + + // create a sound parameter that holds the desired player parameters. + SoundPlayer.PlaySoundParams params = new SoundPlayer.PlaySoundParams(); + params.loopControl = 0; + params.waitForNonLoopingSoundsToFinish = true; + + // In this sample, we will skip waiting for the user to press play, and start displaying sound choices right away + while (!isStopRequested()) { + + // Look for DPAD presses to change the selection + if (gamepad1.dpad_down && !was_dpad_down) { + // Go to next sound (with list wrap) and display it + soundIndex = (soundIndex + 1) % sounds.length; + } + + if (gamepad1.dpad_up && !was_dpad_up) { + // Go to previous sound (with list wrap) and display it + soundIndex = (soundIndex + sounds.length - 1) % sounds.length; + } + + // Look for trigger to see if we should play sound + // Only start a new sound if we are currently not playing one. + if (gamepad1.right_bumper && !soundPlaying) { + + // Determine Resource IDs for the sounds you want to play, and make sure it's valid. + if ((soundID = myApp.getResources().getIdentifier(sounds[soundIndex], "raw", myApp.getPackageName())) != 0){ + + // Signal that the sound is now playing. + soundPlaying = true; + + // Start playing, and also Create a callback that will clear the playing flag when the sound is complete. + SoundPlayer.getInstance().startPlaying(myApp, soundID, params, null, + new Runnable() { + public void run() { + soundPlaying = false; + }} ); + } + } + + // Remember the last state of the dpad to detect changes. + was_dpad_up = gamepad1.dpad_up; + was_dpad_down = gamepad1.dpad_down; + + // Display the current sound choice, and the playing status. + telemetry.addData("", "Use DPAD up/down to choose sound."); + telemetry.addData("", "Press Right Bumper to play sound."); + telemetry.addData("", ""); + telemetry.addData("Sound >", sounds[soundIndex]); + telemetry.addData("Status >", soundPlaying ? "Playing" : "Stopped"); + telemetry.update(); + } + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptTelemetry.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptTelemetry.java new file mode 100644 index 0000000..f2c6097 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptTelemetry.java @@ -0,0 +1,177 @@ +/* Copyright (c) 2017 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.VoltageSensor; +import com.qualcomm.robotcore.util.ElapsedTime; +import org.firstinspires.ftc.robotcore.external.Func; +import org.firstinspires.ftc.robotcore.external.Telemetry; + +/* + * This OpMode illustrates various ways in which telemetry can be + * transmitted from the robot controller to the driver station. The sample illustrates + * numeric and text data, formatted output, and optimized evaluation of expensive-to-acquire + * information. The telemetry log is illustrated by scrolling a poem + * to the driver station. + * + * Also see the Telemetry javadocs. + */ +@TeleOp(name = "Concept: Telemetry", group = "Concept") +@Disabled +public class ConceptTelemetry extends LinearOpMode { + /** Keeps track of the line of the poem which is to be emitted next */ + int poemLine = 0; + + /** Keeps track of how long it's been since we last emitted a line of poetry */ + ElapsedTime poemElapsed = new ElapsedTime(); + + static final String[] poem = new String[] { + + "Mary had a little lamb,", + "His fleece was white as snow,", + "And everywhere that Mary went,", + "The lamb was sure to go.", + "", + "He followed her to school one day,", + "Which was against the rule,", + "It made the children laugh and play", + "To see a lamb at school.", + "", + "And so the teacher turned it out,", + "But still it lingered near,", + "And waited patiently about,", + "Till Mary did appear.", + "", + "\"Why does the lamb love Mary so?\"", + "The eager children cry.", + "\"Why, Mary loves the lamb, you know,\"", + "The teacher did reply.", + "", + "" + }; + + @Override public void runOpMode() { + + /* we keep track of how long it's been since the OpMode was started, just + * to have some interesting data to show */ + ElapsedTime opmodeRunTime = new ElapsedTime(); + + // We show the log in oldest-to-newest order, as that's better for poetry + telemetry.log().setDisplayOrder(Telemetry.Log.DisplayOrder.OLDEST_FIRST); + // We can control the number of lines shown in the log + telemetry.log().setCapacity(6); + // The interval between lines of poetry, in seconds + double sPoemInterval = 0.6; + + /* + * Wait until we've been given the ok to go. For something to do, we emit the + * elapsed time as we sit here and wait. If we didn't want to do anything while + * we waited, we would just call waitForStart(). + */ + while (!isStarted()) { + telemetry.addData("time", "%.1f seconds", opmodeRunTime.seconds()); + telemetry.update(); + idle(); + } + + // Ok, we've been given the ok to go + + /* + * As an illustration, the first line on our telemetry display will display the battery voltage. + * The idea here is that it's expensive to compute the voltage (at least for purposes of illustration) + * so you don't want to do it unless the data is _actually_ going to make it to the + * driver station (recall that telemetry transmission is throttled to reduce bandwidth use. + * Note that getBatteryVoltage() below returns 'Infinity' if there's no voltage sensor attached. + * + * @see Telemetry#getMsTransmissionInterval() + */ + telemetry.addData("voltage", "%.1f volts", new Func() { + @Override public Double value() { + return getBatteryVoltage(); + } + }); + + // Reset to keep some timing stats for the post-'start' part of the OpMode + opmodeRunTime.reset(); + int loopCount = 1; + + // Go go gadget robot! + while (opModeIsActive()) { + + // Emit poetry if it's been a while + if (poemElapsed.seconds() > sPoemInterval) { + emitPoemLine(); + } + + // As an illustration, show some loop timing information + telemetry.addData("loop count", loopCount); + telemetry.addData("ms/loop", "%.3f ms", opmodeRunTime.milliseconds() / loopCount); + + // Show joystick information as some other illustrative data + telemetry.addLine("left joystick | ") + .addData("x", gamepad1.left_stick_x) + .addData("y", gamepad1.left_stick_y); + telemetry.addLine("right joystick | ") + .addData("x", gamepad1.right_stick_x) + .addData("y", gamepad1.right_stick_y); + + /* + * Transmit the telemetry to the driver station, subject to throttling. + * See the documentation for Telemetry.getMsTransmissionInterval() for more information. + */ + telemetry.update(); + + // Update loop info + loopCount++; + } + } + + // emits a line of poetry to the telemetry log + void emitPoemLine() { + telemetry.log().add(poem[poemLine]); + poemLine = (poemLine+1) % poem.length; + poemElapsed.reset(); + } + + // Computes the current battery voltage + double getBatteryVoltage() { + double result = Double.POSITIVE_INFINITY; + for (VoltageSensor sensor : hardwareMap.voltageSensor) { + double voltage = sensor.getVoltage(); + if (voltage > 0) { + result = Math.min(result, voltage); + } + } + return result; + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVisionColorLocator_Circle.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVisionColorLocator_Circle.java new file mode 100644 index 0000000..4ec17aa --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVisionColorLocator_Circle.java @@ -0,0 +1,245 @@ +/* + * Copyright (c) 2024 Phil Malone + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import android.graphics.Color; +import android.util.Size; + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; +import org.firstinspires.ftc.vision.VisionPortal; +import org.firstinspires.ftc.vision.opencv.Circle; +import org.firstinspires.ftc.vision.opencv.ColorBlobLocatorProcessor; +import org.firstinspires.ftc.vision.opencv.ColorRange; +import org.firstinspires.ftc.vision.opencv.ImageRegion; + +import java.util.List; + +/* + * This OpMode illustrates how to use a video source (camera) to locate specifically colored regions. + * This sample is targeted towards circular blobs. To see rectangles, look at ConceptVisionColorLocator_Rectangle + * + * Unlike a "color sensor" which determines the color of nearby object, this "color locator" + * will search the Region Of Interest (ROI) in a camera image, and find any "blobs" of color that + * match the requested color range. These blobs can be further filtered and sorted to find the one + * most likely to be the item the user is looking for. + * + * To perform this function, a VisionPortal runs a ColorBlobLocatorProcessor process. + * The ColorBlobLocatorProcessor (CBLP) process is created first, and then the VisionPortal is built. + * The (CBLP) analyses the ROI and locates pixels that match the ColorRange to form a "mask". + * The matching pixels are then collected into contiguous "blobs" of pixels. + * The outer boundaries of these blobs are called its "contour". For each blob, the process then + * creates the smallest possible circle that will fully encase the contour. The user can then call + * getBlobs() to retrieve the list of Blobs, where each contains the contour and the circle. + * Note: The default sort order for Blobs is ContourArea, in descending order, so the biggest + * contours are listed first. + * + * A colored enclosing circle is drawn on the camera preview to show the location of each Blob + * The original Blob contour can also be added to the preview. + * This is helpful when configuring the ColorBlobLocatorProcessor parameters. + * + * Tip: Connect an HDMI monitor to the Control Hub to view the Color Location process in real-time. + * Or use a screen copy utility like ScrCpy.exe to view the video remotely. + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list + */ + +@Disabled +@TeleOp(name = "Concept: Vision Color-Locator (Circle)", group = "Concept") +public class ConceptVisionColorLocator_Circle extends LinearOpMode { + @Override + public void runOpMode() { + /* Build a "Color Locator" vision processor based on the ColorBlobLocatorProcessor class. + * - Specify the color range you are looking for. Use a predefined color, or create your own + * + * .setTargetColorRange(ColorRange.BLUE) // use a predefined color match + * Available colors are: RED, BLUE, YELLOW, GREEN, ARTIFACT_GREEN, ARTIFACT_PURPLE + * .setTargetColorRange(new ColorRange(ColorSpace.YCrCb, // or define your own color match + * new Scalar( 32, 176, 0), + * new Scalar(255, 255, 132))) + * + * - Focus the color locator by defining a RegionOfInterest (ROI) which you want to search. + * This can be the entire frame, or a sub-region defined using: + * 1) standard image coordinates or 2) a normalized +/- 1.0 coordinate system. + * Use one form of the ImageRegion class to define the ROI. + * ImageRegion.entireFrame() + * ImageRegion.asImageCoordinates(50, 50, 150, 150) 100x100 pixels at upper left corner + * ImageRegion.asUnityCenterCoordinates(-0.5, 0.5, 0.5, -0.5) 50% width/height in center + * + * - Define which contours are included. + * You can get ALL the contours, ignore contours that are completely inside another contour. + * .setContourMode(ColorBlobLocatorProcessor.ContourMode.ALL_FLATTENED_HIERARCHY) + * .setContourMode(ColorBlobLocatorProcessor.ContourMode.EXTERNAL_ONLY) + * EXTERNAL_ONLY helps to avoid bright reflection spots from breaking up solid colors. + * + * - Turn the displays of contours ON or OFF. + * Turning these on helps debugging but takes up valuable CPU time. + * .setDrawContours(true) Draws an outline of each contour. + * .setEnclosingCircleColor(int color) Draws a circle around each contour. 0 to disable. + * .setBoxFitColor(int color) Draws a rectangle around each contour. 0 to disable. ON by default. + * + * + * - include any pre-processing of the image or mask before looking for Blobs. + * There are some extra processing you can include to improve the formation of blobs. + * Using these features requires an understanding of how they may effect the final + * blobs. The "pixels" argument sets the NxN kernel size. + * .setBlurSize(int pixels) + * Blurring an image helps to provide a smooth color transition between objects, + * and smoother contours. The higher the number, the more blurred the image becomes. + * Note: Even "pixels" values will be incremented to satisfy the "odd number" requirement. + * Blurring too much may hide smaller features. A size of 5 is good for a 320x240 image. + * + * .setErodeSize(int pixels) + * Erosion removes floating pixels and thin lines so that only substantive objects remain. + * Erosion can grow holes inside regions, and also shrink objects. + * "pixels" in the range of 2-4 are suitable for low res images. + * + * .setDilateSize(int pixels) + * Dilation makes objects and lines more visible by filling in small holes, and making + * filled shapes appear larger. Dilation is useful for joining broken parts of an + * object, such as when removing noise from an image. + * "pixels" in the range of 2-4 are suitable for low res images. + * + * .setMorphOperationType(MorphOperationType morphOperationType) + * This defines the order in which the Erode/Dilate actions are performed. + * OPENING: Will Erode and then Dilate which will make small noise blobs go away + * CLOSING: Will Dilate and then Erode which will tend to fill in any small holes in blob edges. + */ + ColorBlobLocatorProcessor colorLocator = new ColorBlobLocatorProcessor.Builder() + .setTargetColorRange(ColorRange.ARTIFACT_PURPLE) // Use a predefined color match + .setContourMode(ColorBlobLocatorProcessor.ContourMode.EXTERNAL_ONLY) + .setRoi(ImageRegion.asUnityCenterCoordinates(-0.75, 0.75, 0.75, -0.75)) + .setDrawContours(true) // Show contours on the Stream Preview + .setBoxFitColor(0) // Disable the drawing of rectangles + .setCircleFitColor(Color.rgb(255, 255, 0)) // Draw a circle + .setBlurSize(5) // Smooth the transitions between different colors in image + + // the following options have been added to fill in perimeter holes. + .setDilateSize(15) // Expand blobs to fill any divots on the edges + .setErodeSize(15) // Shrink blobs back to original size + .setMorphOperationType(ColorBlobLocatorProcessor.MorphOperationType.CLOSING) + + .build(); + /* + * Build a vision portal to run the Color Locator process. + * + * - Add the colorLocator process created above. + * - Set the desired video resolution. + * Since a high resolution will not improve this process, choose a lower resolution + * that is supported by your camera. This will improve overall performance and reduce + * latency. + * - Choose your video source. This may be + * .setCamera(hardwareMap.get(WebcamName.class, "Webcam 1")) ..... for a webcam + * or + * .setCamera(BuiltinCameraDirection.BACK) ... for a Phone Camera + */ + VisionPortal portal = new VisionPortal.Builder() + .addProcessor(colorLocator) + .setCameraResolution(new Size(320, 240)) + .setCamera(hardwareMap.get(WebcamName.class, "Webcam 1")) + .build(); + + telemetry.setMsTransmissionInterval(100); // Speed up telemetry updates for debugging. + telemetry.setDisplayFormat(Telemetry.DisplayFormat.MONOSPACE); + + // WARNING: To view the stream preview on the Driver Station, this code runs in INIT mode. + while (opModeIsActive() || opModeInInit()) { + telemetry.addData("preview on/off", "... Camera Stream\n"); + + // Read the current list + List blobs = colorLocator.getBlobs(); + + /* + * The list of Blobs can be filtered to remove unwanted Blobs. + * Note: All contours will be still displayed on the Stream Preview, but only those + * that satisfy the filter conditions will remain in the current list of + * "blobs". Multiple filters may be used. + * + * To perform a filter + * ColorBlobLocatorProcessor.Util.filterByCriteria(criteria, minValue, maxValue, blobs); + * + * The following criteria are currently supported. + * + * ColorBlobLocatorProcessor.BlobCriteria.BY_CONTOUR_AREA + * A Blob's area is the number of pixels contained within the Contour. Filter out any + * that are too big or small. Start with a large range and then refine the range based + * on the likely size of the desired object in the viewfinder. + * + * ColorBlobLocatorProcessor.BlobCriteria.BY_DENSITY + * A blob's density is an indication of how "full" the contour is. + * If you put a rubber band around the contour you would get the "Convex Hull" of the + * contour. The density is the ratio of Contour-area to Convex Hull-area. + * + * ColorBlobLocatorProcessor.BlobCriteria.BY_ASPECT_RATIO + * A blob's Aspect ratio is the ratio of boxFit long side to short side. + * A perfect Square has an aspect ratio of 1. All others are > 1 + * + * ColorBlobLocatorProcessor.BlobCriteria.BY_ARC_LENGTH + * A blob's arc length is the perimeter of the blob. + * This can be used in conjunction with an area filter to detect oddly shaped blobs. + * + * ColorBlobLocatorProcessor.BlobCriteria.BY_CIRCULARITY + * A blob's circularity is how circular it is based on the known area and arc length. + * A perfect circle has a circularity of 1. All others are < 1 + */ + ColorBlobLocatorProcessor.Util.filterByCriteria( + ColorBlobLocatorProcessor.BlobCriteria.BY_CONTOUR_AREA, + 50, 20000, blobs); // filter out very small blobs. + + ColorBlobLocatorProcessor.Util.filterByCriteria( + ColorBlobLocatorProcessor.BlobCriteria.BY_CIRCULARITY, + 0.6, 1, blobs); /* filter out non-circular blobs. + * NOTE: You may want to adjust the minimum value depending on your use case. + * Circularity values will be affected by shadows, and will therefore vary based + * on the location of the camera on your robot and venue lighting. It is strongly + * encouraged to test your vision on the competition field if your event allows + * sensor calibration time. + */ + + /* + * The list of Blobs can be sorted using the same Blob attributes as listed above. + * No more than one sort call should be made. Sorting can use ascending or descending order. + * Here is an example.: + * ColorBlobLocatorProcessor.Util.sortByCriteria( + * ColorBlobLocatorProcessor.BlobCriteria.BY_CONTOUR_AREA, SortOrder.DESCENDING, blobs); + */ + + telemetry.addLine("Circularity Radius Center"); + + // Display the Blob's circularity, and the size (radius) and center location of its circleFit. + for (ColorBlobLocatorProcessor.Blob b : blobs) { + + Circle circleFit = b.getCircle(); + telemetry.addLine(String.format("%5.3f %3d (%3d,%3d)", + b.getCircularity(), (int) circleFit.getRadius(), (int) circleFit.getX(), (int) circleFit.getY())); + } + + telemetry.update(); + sleep(100); // Match the telemetry update interval. + } + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVisionColorLocator_Rectangle.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVisionColorLocator_Rectangle.java new file mode 100644 index 0000000..5681f71 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVisionColorLocator_Rectangle.java @@ -0,0 +1,218 @@ +/* + * Copyright (c) 2024 Phil Malone + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import android.util.Size; + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.util.SortOrder; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; +import org.firstinspires.ftc.vision.VisionPortal; +import org.firstinspires.ftc.vision.opencv.ColorBlobLocatorProcessor; +import org.firstinspires.ftc.vision.opencv.ColorRange; +import org.firstinspires.ftc.vision.opencv.ImageRegion; +import org.opencv.core.RotatedRect; + +import java.util.List; + +/* + * This OpMode illustrates how to use a video source (camera) to locate specifically colored regions + * + * Unlike a "color sensor" which determines the color of nearby object, this "color locator" + * will search the Region Of Interest (ROI) in a camera image, and find any "blobs" of color that + * match the requested color range. These blobs can be further filtered and sorted to find the one + * most likely to be the item the user is looking for. + * + * To perform this function, a VisionPortal runs a ColorBlobLocatorProcessor process. + * The ColorBlobLocatorProcessor (CBLP) process is created first, and then the VisionPortal is built. + * The (CBLP) analyses the ROI and locates pixels that match the ColorRange to form a "mask". + * The matching pixels are then collected into contiguous "blobs" of pixels. + * The outer boundaries of these blobs are called its "contour". For each blob, the process then + * creates the smallest possible rectangle "boxFit" that will fully encase the contour. The user can + * then call getBlobs() to retrieve the list of Blobs, where each contains the contour and the boxFit. + * Note: The default sort order for Blobs is ContourArea, in descending order, so the biggest + * contours are listed first. + * + * A colored boxFit rectangle is drawn on the camera preview to show the location of each Blob + * The original Blob contour can also be added to the preview. + * This is helpful when configuring the ColorBlobLocatorProcessor parameters. + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list + */ + +@Disabled +@TeleOp(name = "Concept: Color-Locator (Rectangle)", group = "Concept") +public class ConceptVisionColorLocator_Rectangle extends LinearOpMode +{ + @Override + public void runOpMode() + { + /* Build a "Color Locator" vision processor based on the ColorBlobLocatorProcessor class. + * - Specify the color range you are looking for. Use a predefined color, or create your own + * + * .setTargetColorRange(ColorRange.BLUE) // use a predefined color match + * Available colors are: RED, BLUE YELLOW GREEN ARTIFACT_GREEN and ARTIFACT_PURPLE + * .setTargetColorRange(new ColorRange(ColorSpace.YCrCb, // or define your own color match + * new Scalar( 32, 176, 0), + * new Scalar(255, 255, 132))) + * + * - Focus the color locator by defining a RegionOfInterest (ROI) which you want to search. + * This can be the entire frame, or a sub-region defined using: + * 1) standard image coordinates or 2) a normalized +/- 1.0 coordinate system. + * Use one form of the ImageRegion class to define the ROI. + * ImageRegion.entireFrame() + * ImageRegion.asImageCoordinates(50, 50, 150, 150) 100x100 pixels at upper left corner + * ImageRegion.asUnityCenterCoordinates(-0.5, 0.5, 0.5, -0.5) 50% width/height in center + * + * - Define which contours are included. + * You can get ALL the contours, ignore contours that are completely inside another contour. + * .setContourMode(ColorBlobLocatorProcessor.ContourMode.ALL_FLATTENED_HIERARCHY) + * .setContourMode(ColorBlobLocatorProcessor.ContourMode.EXTERNAL_ONLY) + * EXTERNAL_ONLY helps to avoid bright reflection spots from breaking up solid colors. + * + * - turn the display of contours ON or OFF. Helps debugging but takes up valuable CPU time. + * .setDrawContours(true) + * + * - include any pre-processing of the image or mask before looking for Blobs. + * There are some extra processing you can include to improve the formation of blobs. + * Using these features requires an understanding of how they may effect the final blobs. + * The "pixels" argument sets the NxN kernel size. + * .setBlurSize(int pixels) + * Blurring an image helps to provide a smooth color transition between objects, + * and smoother contours. The higher the number, the more blurred the image becomes. + * Note: Even "pixels" values will be incremented to satisfy the "odd number" requirement. + * Blurring too much may hide smaller features. A size of 5 is good for a 320x240 image. + * + * .setErodeSize(int pixels) + * Erosion removes floating pixels and thin lines so that only substantive objects remain. + * Erosion can grow holes inside regions, and also shrink objects. + * "pixels" in the range of 2-4 are suitable for low res images. + * + * .setDilateSize(int pixels) + * Dilation makes objects and lines more visible by filling in small holes, and making + * filled shapes appear larger. Dilation is useful for joining broken parts of an + * object, such as when removing noise from an image. + * "pixels" in the range of 2-4 are suitable for low res images. + */ + ColorBlobLocatorProcessor colorLocator = new ColorBlobLocatorProcessor.Builder() + .setTargetColorRange(ColorRange.ARTIFACT_PURPLE) // use a predefined color match + .setContourMode(ColorBlobLocatorProcessor.ContourMode.EXTERNAL_ONLY) + .setRoi(ImageRegion.asUnityCenterCoordinates(-0.75, 0.75, 0.75, -0.75)) + .setDrawContours(true) // Show contours on the Stream Preview + .setBlurSize(5) // Smooth the transitions between different colors in image + .build(); + + /* + * Build a vision portal to run the Color Locator process. + * + * - Add the colorLocator process created above. + * - Set the desired video resolution. + * A high resolution will not improve this process, so choose a lower resolution that is + * supported by your camera. This will improve overall performance and reduce latency. + * - Choose your video source. This may be + * .setCamera(hardwareMap.get(WebcamName.class, "Webcam 1")) ..... for a webcam + * or + * .setCamera(BuiltinCameraDirection.BACK) ... for a Phone Camera + */ + VisionPortal portal = new VisionPortal.Builder() + .addProcessor(colorLocator) + .setCameraResolution(new Size(320, 240)) + .setCamera(hardwareMap.get(WebcamName.class, "Webcam 1")) + .build(); + + telemetry.setMsTransmissionInterval(100); // Speed up telemetry updates for debugging. + telemetry.setDisplayFormat(Telemetry.DisplayFormat.MONOSPACE); + + // WARNING: To view the stream preview on the Driver Station, this code runs in INIT mode. + while (opModeIsActive() || opModeInInit()) + { + telemetry.addData("preview on/off", "... Camera Stream\n"); + + // Read the current list + List blobs = colorLocator.getBlobs(); + + /* + * The list of Blobs can be filtered to remove unwanted Blobs. + * Note: All contours will be still displayed on the Stream Preview, but only those + * that satisfy the filter conditions will remain in the current list of "blobs". + * Multiple filters may be used. + * + * To perform a filter + * ColorBlobLocatorProcessor.Util.filterByCriteria(criteria, minValue, maxValue, blobs); + * + * The following criteria are currently supported. + * + * ColorBlobLocatorProcessor.BlobCriteria.BY_CONTOUR_AREA + * A Blob's area is the number of pixels contained within the Contour. Filter out any + * that are too big or small. Start with a large range and then refine the range + * based on the likely size of the desired object in the viewfinder. + * + * ColorBlobLocatorProcessor.BlobCriteria.BY_DENSITY + * A blob's density is an indication of how "full" the contour is. + * If you put a rubber band around the contour you get the "Convex Hull" of the contour. + * The density is the ratio of Contour-area to Convex Hull-area. + * + * ColorBlobLocatorProcessor.BlobCriteria.BY_ASPECT_RATIO + * A blob's Aspect ratio is the ratio of boxFit long side to short side. + * A perfect Square has an aspect ratio of 1. All others are > 1 + * + * ColorBlobLocatorProcessor.BlobCriteria.BY_ARC_LENGTH + * A blob's arc length is the perimeter of the blob. + * This can be used in conjunction with an area filter to detect oddly shaped blobs. + * + * ColorBlobLocatorProcessor.BlobCriteria.BY_CIRCULARITY + * A blob's circularity is how circular it is based on the known area and arc length. + * A perfect circle has a circularity of 1. All others are < 1 + */ + ColorBlobLocatorProcessor.Util.filterByCriteria( + ColorBlobLocatorProcessor.BlobCriteria.BY_CONTOUR_AREA, + 50, 20000, blobs); // filter out very small blobs. + + /* + * The list of Blobs can be sorted using the same Blob attributes as listed above. + * No more than one sort call should be made. Sorting can be ASCENDING or DESCENDING. + * Here is an example.: + * ColorBlobLocatorProcessor.Util.sortByCriteria( + * ColorBlobLocatorProcessor.BlobCriteria.BY_CONTOUR_AREA, SortOrder.DESCENDING, blobs); + */ + + telemetry.addLine("Ctr:(X,Y) Area Dens Aspect Arc Circ"); + + // Display the size (area) and center location for each Blob. + for(ColorBlobLocatorProcessor.Blob b : blobs) + { + RotatedRect boxFit = b.getBoxFit(); + telemetry.addLine(String.format("(%3d,%3d) %5d %4.2f %5.2f %3d %5.3f ", + (int) boxFit.center.x, (int) boxFit.center.y, b.getContourArea(), b.getDensity(), + b.getAspectRatio(), (int) b.getArcLength(), b.getCircularity())); + } + + telemetry.update(); + sleep(100); // Match the telemetry update interval. + } + } +} \ No newline at end of file diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVisionColorSensor.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVisionColorSensor.java new file mode 100644 index 0000000..5cce468 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptVisionColorSensor.java @@ -0,0 +1,158 @@ +/* + * Copyright (c) 2024 Phil Malone + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import android.util.Size; + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; +import org.firstinspires.ftc.vision.VisionPortal; +import org.firstinspires.ftc.vision.opencv.ImageRegion; +import org.firstinspires.ftc.vision.opencv.PredominantColorProcessor; + +/* + * This OpMode illustrates how to use a video source (camera) as a color sensor + * + * A "color sensor" will typically determine the color of the object that it is pointed at. + * + * This sample performs the same function, except it uses a video camera to inspect an object or scene. + * The user may choose to inspect all, or just a Region of Interest (ROI), of the active camera view. + * The user must also provide a list of "acceptable colors" (Swatches) from which the closest matching + * color will be selected. + * + * To perform this function, a VisionPortal runs a PredominantColorProcessor process. + * The PredominantColorProcessor (PCP) process is created first, and then the VisionPortal is built. + * The (PCP) analyses the ROI and splits the colored pixels into several color-clusters. + * The largest of these clusters is then considered to be the "Predominant Color" + * The process then matches the Predominant Color with the closest Swatch and returns that match. + * + * To aid the user, a colored rectangle is drawn on the camera preview to show the RegionOfInterest, + * The Predominant Color is used to paint the rectangle border, so the user can visualize the color. + * + * Tip: Connect an HDMI monitor to the Control Hub to view the Color Sensor process in real-time. + * Or use a screen copy utility like ScrCpy.exe to view the video remotely. + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list + */ + +@Disabled +@TeleOp(name = "Concept: Vision Color-Sensor", group = "Concept") +public class ConceptVisionColorSensor extends LinearOpMode +{ + @Override + public void runOpMode() + { + /* Build a "Color Sensor" vision processor based on the PredominantColorProcessor class. + * + * - Focus the color sensor by defining a RegionOfInterest (ROI) which you want to inspect. + * This can be the entire frame, or a sub-region defined using: + * 1) standard image coordinates or 2) a normalized +/- 1.0 coordinate system. + * Use one form of the ImageRegion class to define the ROI. + * ImageRegion.entireFrame() + * ImageRegion.asImageCoordinates(50, 50, 150, 150) 100x100 square at the top left corner + * ImageRegion.asUnityCenterCoordinates(-0.1, 0.1, 0.1, -0.1) 10% W * H centered square + * + * - Set the list of "acceptable" color swatches (matches). + * Only colors that you assign here will be returned. + * If you know the sensor will be pointing to one of a few specific colors, enter them here. + * Or, if the sensor may be pointed randomly, provide some additional colors that may match. + * Possible choices are: + * RED, ORANGE, YELLOW, GREEN, CYAN, BLUE, PURPLE, MAGENTA, BLACK, WHITE + * Note: For the 2026 season ARTIFACT_PURPLE and ARTIFACT_GREEN have been added. + * + * Note that in the example shown below, only some of the available colors are included. + * This will force any other colored region into one of these colors. + * eg: Green may be reported as YELLOW, as this may be the "closest" match. + */ + PredominantColorProcessor colorSensor = new PredominantColorProcessor.Builder() + .setRoi(ImageRegion.asUnityCenterCoordinates(-0.1, 0.1, 0.1, -0.1)) + .setSwatches( + PredominantColorProcessor.Swatch.ARTIFACT_GREEN, + PredominantColorProcessor.Swatch.ARTIFACT_PURPLE, + PredominantColorProcessor.Swatch.RED, + PredominantColorProcessor.Swatch.BLUE, + PredominantColorProcessor.Swatch.YELLOW, + PredominantColorProcessor.Swatch.BLACK, + PredominantColorProcessor.Swatch.WHITE) + .build(); + + /* + * Build a vision portal to run the Color Sensor process. + * + * - Add the colorSensor process created above. + * - Set the desired video resolution. + * Since a high resolution will not improve this process, choose a lower resolution + * supported by your camera. This will improve overall performance and reduce latency. + * - Choose your video source. This may be + * .setCamera(hardwareMap.get(WebcamName.class, "Webcam 1")) ..... for a webcam + * or + * .setCamera(BuiltinCameraDirection.BACK) ... for a Phone Camera + */ + VisionPortal portal = new VisionPortal.Builder() + .addProcessor(colorSensor) + .setCameraResolution(new Size(320, 240)) + .setCamera(hardwareMap.get(WebcamName.class, "Webcam 1")) + .build(); + + telemetry.setMsTransmissionInterval(100); // Speed up telemetry updates, for debugging. + telemetry.setDisplayFormat(Telemetry.DisplayFormat.MONOSPACE); + + // WARNING: To view the stream preview on the Driver Station, this code runs in INIT mode. + while (opModeIsActive() || opModeInInit()) + { + telemetry.addLine("Preview on/off: 3 dots, Camera Stream\n"); + + // Request the most recent color analysis. This will return the closest matching + // colorSwatch and the predominant color in the RGB, HSV and YCrCb color spaces. + // The color space values are returned as three-element int[] arrays as follows: + // RGB Red 0-255, Green 0-255, Blue 0-255 + // HSV Hue 0-180, Saturation 0-255, Value 0-255 + // YCrCb Luminance(Y) 0-255, Cr 0-255 (center 128), Cb 0-255 (center 128) + // + // Note: to take actions based on the detected color, simply use the colorSwatch or + // color space value in a comparison or switch. eg: + + // if (result.closestSwatch == PredominantColorProcessor.Swatch.RED) {.. some code ..} + // or: + // if (result.RGB[0] > 128) {... some code ...} + + PredominantColorProcessor.Result result = colorSensor.getAnalysis(); + + // Display the Color Sensor result. + telemetry.addData("Best Match", result.closestSwatch); + telemetry.addLine(String.format("RGB (%3d, %3d, %3d)", + result.RGB[0], result.RGB[1], result.RGB[2])); + telemetry.addLine(String.format("HSV (%3d, %3d, %3d)", + result.HSV[0], result.HSV[1], result.HSV[2])); + telemetry.addLine(String.format("YCrCb (%3d, %3d, %3d)", + result.YCrCb[0], result.YCrCb[1], result.YCrCb[2])); + telemetry.update(); + + sleep(20); + } + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveByEncoder_Linear.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveByEncoder_Linear.java new file mode 100644 index 0000000..63293d0 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveByEncoder_Linear.java @@ -0,0 +1,187 @@ +/* Copyright (c) 2017 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.util.ElapsedTime; + +/* + * This OpMode illustrates the concept of driving a path based on encoder counts. + * The code is structured as a LinearOpMode + * + * The code REQUIRES that you DO have encoders on the wheels, + * otherwise you would use: RobotAutoDriveByTime; + * + * This code ALSO requires that the drive Motors have been configured such that a positive + * power command moves them forward, and causes the encoders to count UP. + * + * The desired path in this example is: + * - Drive forward for 48 inches + * - Spin right for 12 Inches + * - Drive Backward for 24 inches + * - Stop and close the claw. + * + * The code is written using a method called: encoderDrive(speed, leftInches, rightInches, timeoutS) + * that performs the actual movement. + * This method assumes that each movement is relative to the last stopping place. + * There are other ways to perform encoder based moves, but this method is probably the simplest. + * This code uses the RUN_TO_POSITION mode to enable the Motor controllers to generate the run profile + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list + */ + +@Autonomous(name="Robot: Auto Drive By Encoder", group="Robot") +@Disabled +public class RobotAutoDriveByEncoder_Linear extends LinearOpMode { + + /* Declare OpMode members. */ + private DcMotor leftDrive = null; + private DcMotor rightDrive = null; + + private ElapsedTime runtime = new ElapsedTime(); + + // Calculate the COUNTS_PER_INCH for your specific drive train. + // Go to your motor vendor website to determine your motor's COUNTS_PER_MOTOR_REV + // For external drive gearing, set DRIVE_GEAR_REDUCTION as needed. + // For example, use a value of 2.0 for a 12-tooth spur gear driving a 24-tooth spur gear. + // This is gearing DOWN for less speed and more torque. + // For gearing UP, use a gear ratio less than 1.0. Note this will affect the direction of wheel rotation. + static final double COUNTS_PER_MOTOR_REV = 1440 ; // eg: TETRIX Motor Encoder + static final double DRIVE_GEAR_REDUCTION = 1.0 ; // No External Gearing. + static final double WHEEL_DIAMETER_INCHES = 4.0 ; // For figuring circumference + static final double COUNTS_PER_INCH = (COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION) / + (WHEEL_DIAMETER_INCHES * 3.1415); + static final double DRIVE_SPEED = 0.6; + static final double TURN_SPEED = 0.5; + + @Override + public void runOpMode() { + + // Initialize the drive system variables. + leftDrive = hardwareMap.get(DcMotor.class, "left_drive"); + rightDrive = hardwareMap.get(DcMotor.class, "right_drive"); + + // To drive forward, most robots need the motor on one side to be reversed, because the axles point in opposite directions. + // When run, this OpMode should start both motors driving forward. So adjust these two lines based on your first test drive. + // Note: The settings here assume direct drive on left and right wheels. Gear Reduction or 90 Deg drives may require direction flips + leftDrive.setDirection(DcMotor.Direction.REVERSE); + rightDrive.setDirection(DcMotor.Direction.FORWARD); + + leftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + rightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + + leftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + rightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + + // Send telemetry message to indicate successful Encoder reset + telemetry.addData("Starting at", "%7d :%7d", + leftDrive.getCurrentPosition(), + rightDrive.getCurrentPosition()); + telemetry.update(); + + // Wait for the game to start (driver presses START) + waitForStart(); + + // Step through each leg of the path, + // Note: Reverse movement is obtained by setting a negative distance (not speed) + encoderDrive(DRIVE_SPEED, 48, 48, 5.0); // S1: Forward 47 Inches with 5 Sec timeout + encoderDrive(TURN_SPEED, 12, -12, 4.0); // S2: Turn Right 12 Inches with 4 Sec timeout + encoderDrive(DRIVE_SPEED, -24, -24, 4.0); // S3: Reverse 24 Inches with 4 Sec timeout + + telemetry.addData("Path", "Complete"); + telemetry.update(); + sleep(1000); // pause to display final telemetry message. + } + + /* + * Method to perform a relative move, based on encoder counts. + * Encoders are not reset as the move is based on the current position. + * Move will stop if any of three conditions occur: + * 1) Move gets to the desired position + * 2) Move runs out of time + * 3) Driver stops the OpMode running. + */ + public void encoderDrive(double speed, + double leftInches, double rightInches, + double timeoutS) { + int newLeftTarget; + int newRightTarget; + + // Ensure that the OpMode is still active + if (opModeIsActive()) { + + // Determine new target position, and pass to motor controller + newLeftTarget = leftDrive.getCurrentPosition() + (int)(leftInches * COUNTS_PER_INCH); + newRightTarget = rightDrive.getCurrentPosition() + (int)(rightInches * COUNTS_PER_INCH); + leftDrive.setTargetPosition(newLeftTarget); + rightDrive.setTargetPosition(newRightTarget); + + // Turn On RUN_TO_POSITION + leftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION); + rightDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION); + + // reset the timeout time and start motion. + runtime.reset(); + leftDrive.setPower(Math.abs(speed)); + rightDrive.setPower(Math.abs(speed)); + + // keep looping while we are still active, and there is time left, and both motors are running. + // Note: We use (isBusy() && isBusy()) in the loop test, which means that when EITHER motor hits + // its target position, the motion will stop. This is "safer" in the event that the robot will + // always end the motion as soon as possible. + // However, if you require that BOTH motors have finished their moves before the robot continues + // onto the next step, use (isBusy() || isBusy()) in the loop test. + while (opModeIsActive() && + (runtime.seconds() < timeoutS) && + (leftDrive.isBusy() && rightDrive.isBusy())) { + + // Display it for the driver. + telemetry.addData("Running to", " %7d :%7d", newLeftTarget, newRightTarget); + telemetry.addData("Currently at", " at %7d :%7d", + leftDrive.getCurrentPosition(), rightDrive.getCurrentPosition()); + telemetry.update(); + } + + // Stop all motion; + leftDrive.setPower(0); + rightDrive.setPower(0); + + // Turn off RUN_TO_POSITION + leftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + rightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + + sleep(250); // optional pause after each move. + } + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveByGyro_Linear.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveByGyro_Linear.java new file mode 100644 index 0000000..ab70934 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveByGyro_Linear.java @@ -0,0 +1,429 @@ +/* Copyright (c) 2022 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.IMU; +import com.qualcomm.robotcore.util.ElapsedTime; +import com.qualcomm.robotcore.util.Range; +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles; + +/* + * This OpMode illustrates the concept of driving an autonomous path based on Gyro (IMU) heading and encoder counts. + * The code is structured as a LinearOpMode + * + * The path to be followed by the robot is built from a series of drive, turn or pause steps. + * Each step on the path is defined by a single function call, and these can be strung together in any order. + * + * The code REQUIRES that you have encoders on the drive motors, otherwise you should use: RobotAutoDriveByTime; + * + * This code uses the Universal IMU interface so it will work with either the BNO055, or BHI260 IMU. + * To run as written, the Control/Expansion hub should be mounted horizontally on a flat part of the robot chassis. + * The REV Logo should be facing UP, and the USB port should be facing forward. + * If this is not the configuration of your REV Control Hub, then the code should be modified to reflect the correct orientation. + * + * This sample requires that the drive Motors have been configured with names : left_drive and right_drive. + * It also requires that a positive power command moves both motors forward, and causes the encoders to count UP. + * So please verify that both of your motors move the robot forward on the first move. If not, make the required correction. + * See the beginning of runOpMode() to set the FORWARD/REVERSE option for each motor. + * + * This code uses RUN_TO_POSITION mode for driving straight, and RUN_USING_ENCODER mode for turning and holding. + * Note: This code implements the requirement of calling setTargetPosition() at least once before switching to RUN_TO_POSITION mode. + * + * Notes: + * + * All angles are referenced to the coordinate-frame that is set whenever resetHeading() is called. + * In this sample, the heading is reset when the Start button is touched on the Driver Station. + * Note: It would be possible to reset the heading after each move, but this would accumulate steering errors. + * + * The angle of movement/rotation is assumed to be a standardized rotation around the robot Z axis, + * which means that a Positive rotation is Counter Clockwise, looking down on the field. + * This is consistent with the FTC field coordinate conventions set out in the document: + * https://ftc-docs.firstinspires.org/field-coordinate-system + * + * Control Approach. + * + * To reach, or maintain a required heading, this code implements a basic Proportional Controller where: + * + * Steering power = Heading Error * Proportional Gain. + * + * "Heading Error" is calculated by taking the difference between the desired heading and the actual heading, + * and then "normalizing" it by converting it to a value in the +/- 180 degree range. + * + * "Proportional Gain" is a constant that YOU choose to set the "strength" of the steering response. + * + * Use Android Studio to Copy this Class, and Paste it into your "TeamCode" folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list + */ + +@Autonomous(name="Robot: Auto Drive By Gyro", group="Robot") +@Disabled +public class RobotAutoDriveByGyro_Linear extends LinearOpMode { + + /* Declare OpMode members. */ + private DcMotor leftDrive = null; + private DcMotor rightDrive = null; + private IMU imu = null; // Control/Expansion Hub IMU + + private double headingError = 0; + + // These variable are declared here (as class members) so they can be updated in various methods, + // but still be displayed by sendTelemetry() + private double targetHeading = 0; + private double driveSpeed = 0; + private double turnSpeed = 0; + private double leftSpeed = 0; + private double rightSpeed = 0; + private int leftTarget = 0; + private int rightTarget = 0; + + // Calculate the COUNTS_PER_INCH for your specific drive train. + // Go to your motor vendor website to determine your motor's COUNTS_PER_MOTOR_REV + // For external drive gearing, set DRIVE_GEAR_REDUCTION as needed. + // For example, use a value of 2.0 for a 12-tooth spur gear driving a 24-tooth spur gear. + // This is gearing DOWN for less speed and more torque. + // For gearing UP, use a gear ratio less than 1.0. Note this will affect the direction of wheel rotation. + static final double COUNTS_PER_MOTOR_REV = 537.7 ; // eg: GoBILDA 312 RPM Yellow Jacket + static final double DRIVE_GEAR_REDUCTION = 1.0 ; // No External Gearing. + static final double WHEEL_DIAMETER_INCHES = 4.0 ; // For figuring circumference + static final double COUNTS_PER_INCH = (COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION) / + (WHEEL_DIAMETER_INCHES * 3.1415); + + // These constants define the desired driving/control characteristics + // They can/should be tweaked to suit the specific robot drive train. + static final double DRIVE_SPEED = 0.4; // Max driving speed for better distance accuracy. + static final double TURN_SPEED = 0.2; // Max turn speed to limit turn rate. + static final double HEADING_THRESHOLD = 1.0 ; // How close must the heading get to the target before moving to next step. + // Requiring more accuracy (a smaller number) will often make the turn take longer to get into the final position. + // Define the Proportional control coefficient (or GAIN) for "heading control". + // We define one value when Turning (larger errors), and the other is used when Driving straight (smaller errors). + // Increase these numbers if the heading does not correct strongly enough (eg: a heavy robot or using tracks) + // Decrease these numbers if the heading does not settle on the correct value (eg: very agile robot with omni wheels) + static final double P_TURN_GAIN = 0.02; // Larger is more responsive, but also less stable. + static final double P_DRIVE_GAIN = 0.03; // Larger is more responsive, but also less stable. + + + @Override + public void runOpMode() { + + // Initialize the drive system variables. + leftDrive = hardwareMap.get(DcMotor.class, "left_drive"); + rightDrive = hardwareMap.get(DcMotor.class, "right_drive"); + + // To drive forward, most robots need the motor on one side to be reversed, because the axles point in opposite directions. + // When run, this OpMode should start both motors driving forward. So adjust these two lines based on your first test drive. + // Note: The settings here assume direct drive on left and right wheels. Gear Reduction or 90 Deg drives may require direction flips + leftDrive.setDirection(DcMotor.Direction.REVERSE); + rightDrive.setDirection(DcMotor.Direction.FORWARD); + + /* The next two lines define Hub orientation. + * The Default Orientation (shown) is when a hub is mounted horizontally with the printed logo pointing UP and the USB port pointing FORWARD. + * + * To Do: EDIT these two lines to match YOUR mounting configuration. + */ + RevHubOrientationOnRobot.LogoFacingDirection logoDirection = RevHubOrientationOnRobot.LogoFacingDirection.UP; + RevHubOrientationOnRobot.UsbFacingDirection usbDirection = RevHubOrientationOnRobot.UsbFacingDirection.FORWARD; + RevHubOrientationOnRobot orientationOnRobot = new RevHubOrientationOnRobot(logoDirection, usbDirection); + + // Now initialize the IMU with this mounting orientation + // This sample expects the IMU to be in a REV Hub and named "imu". + imu = hardwareMap.get(IMU.class, "imu"); + imu.initialize(new IMU.Parameters(orientationOnRobot)); + + // Ensure the robot is stationary. Reset the encoders and set the motors to BRAKE mode + leftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + rightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER); + leftDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + rightDrive.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + + // Wait for the game to start (Display Gyro value while waiting) + while (opModeInInit()) { + telemetry.addData(">", "Robot Heading = %4.0f", getHeading()); + telemetry.update(); + } + + // Set the encoders for closed loop speed control, and reset the heading. + leftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + rightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + imu.resetYaw(); + + // Step through each leg of the path, + // Notes: Reverse movement is obtained by setting a negative distance (not speed) + // holdHeading() is used after turns to let the heading stabilize + // Add a sleep(2000) after any step to keep the telemetry data visible for review + + driveStraight(DRIVE_SPEED, 24.0, 0.0); // Drive Forward 24" + turnToHeading( TURN_SPEED, -45.0); // Turn CW to -45 Degrees + holdHeading( TURN_SPEED, -45.0, 0.5); // Hold -45 Deg heading for a 1/2 second + + driveStraight(DRIVE_SPEED, 17.0, -45.0); // Drive Forward 17" at -45 degrees (12"x and 12"y) + turnToHeading( TURN_SPEED, 45.0); // Turn CCW to 45 Degrees + holdHeading( TURN_SPEED, 45.0, 0.5); // Hold 45 Deg heading for a 1/2 second + + driveStraight(DRIVE_SPEED, 17.0, 45.0); // Drive Forward 17" at 45 degrees (-12"x and 12"y) + turnToHeading( TURN_SPEED, 0.0); // Turn CW to 0 Degrees + holdHeading( TURN_SPEED, 0.0, 1.0); // Hold 0 Deg heading for 1 second + + driveStraight(DRIVE_SPEED,-48.0, 0.0); // Drive in Reverse 48" (should return to approx. staring position) + + telemetry.addData("Path", "Complete"); + telemetry.update(); + sleep(1000); // Pause to display last telemetry message. + } + + /* + * ==================================================================================================== + * Driving "Helper" functions are below this line. + * These provide the high and low level methods that handle driving straight and turning. + * ==================================================================================================== + */ + + // ********** HIGH Level driving functions. ******************** + + /** + * Drive in a straight line, on a fixed compass heading (angle), based on encoder counts. + * Move will stop if either of these conditions occur: + * 1) Move gets to the desired position + * 2) Driver stops the OpMode running. + * + * @param maxDriveSpeed MAX Speed for forward/rev motion (range 0 to +1.0) . + * @param distance Distance (in inches) to move from current position. Negative distance means move backward. + * @param heading Absolute Heading Angle (in Degrees) relative to last gyro reset. + * 0 = fwd. +ve is CCW from fwd. -ve is CW from forward. + * If a relative angle is required, add/subtract from the current robotHeading. + */ + public void driveStraight(double maxDriveSpeed, + double distance, + double heading) { + + // Ensure that the OpMode is still active + if (opModeIsActive()) { + + // Determine new target position, and pass to motor controller + int moveCounts = (int)(distance * COUNTS_PER_INCH); + leftTarget = leftDrive.getCurrentPosition() + moveCounts; + rightTarget = rightDrive.getCurrentPosition() + moveCounts; + + // Set Target FIRST, then turn on RUN_TO_POSITION + leftDrive.setTargetPosition(leftTarget); + rightDrive.setTargetPosition(rightTarget); + + leftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION); + rightDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION); + + // Set the required driving speed (must be positive for RUN_TO_POSITION) + // Start driving straight, and then enter the control loop + maxDriveSpeed = Math.abs(maxDriveSpeed); + moveRobot(maxDriveSpeed, 0); + + // keep looping while we are still active, and BOTH motors are running. + while (opModeIsActive() && + (leftDrive.isBusy() && rightDrive.isBusy())) { + + // Determine required steering to keep on heading + turnSpeed = getSteeringCorrection(heading, P_DRIVE_GAIN); + + // if driving in reverse, the motor correction also needs to be reversed + if (distance < 0) + turnSpeed *= -1.0; + + // Apply the turning correction to the current driving speed. + moveRobot(driveSpeed, turnSpeed); + + // Display drive status for the driver. + sendTelemetry(true); + } + + // Stop all motion & Turn off RUN_TO_POSITION + moveRobot(0, 0); + leftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + rightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + } + } + + /** + * Spin on the central axis to point in a new direction. + *

+ * Move will stop if either of these conditions occur: + *

+ * 1) Move gets to the heading (angle) + *

+ * 2) Driver stops the OpMode running. + * + * @param maxTurnSpeed Desired MAX speed of turn. (range 0 to +1.0) + * @param heading Absolute Heading Angle (in Degrees) relative to last gyro reset. + * 0 = fwd. +ve is CCW from fwd. -ve is CW from forward. + * If a relative angle is required, add/subtract from current heading. + */ + public void turnToHeading(double maxTurnSpeed, double heading) { + + // Run getSteeringCorrection() once to pre-calculate the current error + getSteeringCorrection(heading, P_DRIVE_GAIN); + + // keep looping while we are still active, and not on heading. + while (opModeIsActive() && (Math.abs(headingError) > HEADING_THRESHOLD)) { + + // Determine required steering to keep on heading + turnSpeed = getSteeringCorrection(heading, P_TURN_GAIN); + + // Clip the speed to the maximum permitted value. + turnSpeed = Range.clip(turnSpeed, -maxTurnSpeed, maxTurnSpeed); + + // Pivot in place by applying the turning correction + moveRobot(0, turnSpeed); + + // Display drive status for the driver. + sendTelemetry(false); + } + + // Stop all motion; + moveRobot(0, 0); + } + + /** + * Obtain & hold a heading for a finite amount of time + *

+ * Move will stop once the requested time has elapsed + *

+ * This function is useful for giving the robot a moment to stabilize its heading between movements. + * + * @param maxTurnSpeed Maximum differential turn speed (range 0 to +1.0) + * @param heading Absolute Heading Angle (in Degrees) relative to last gyro reset. + * 0 = fwd. +ve is CCW from fwd. -ve is CW from forward. + * If a relative angle is required, add/subtract from current heading. + * @param holdTime Length of time (in seconds) to hold the specified heading. + */ + public void holdHeading(double maxTurnSpeed, double heading, double holdTime) { + + ElapsedTime holdTimer = new ElapsedTime(); + holdTimer.reset(); + + // keep looping while we have time remaining. + while (opModeIsActive() && (holdTimer.time() < holdTime)) { + // Determine required steering to keep on heading + turnSpeed = getSteeringCorrection(heading, P_TURN_GAIN); + + // Clip the speed to the maximum permitted value. + turnSpeed = Range.clip(turnSpeed, -maxTurnSpeed, maxTurnSpeed); + + // Pivot in place by applying the turning correction + moveRobot(0, turnSpeed); + + // Display drive status for the driver. + sendTelemetry(false); + } + + // Stop all motion; + moveRobot(0, 0); + } + + // ********** LOW Level driving functions. ******************** + + /** + * Use a Proportional Controller to determine how much steering correction is required. + * + * @param desiredHeading The desired absolute heading (relative to last heading reset) + * @param proportionalGain Gain factor applied to heading error to obtain turning power. + * @return Turning power needed to get to required heading. + */ + public double getSteeringCorrection(double desiredHeading, double proportionalGain) { + targetHeading = desiredHeading; // Save for telemetry + + // Determine the heading current error + headingError = targetHeading - getHeading(); + + // Normalize the error to be within +/- 180 degrees + while (headingError > 180) headingError -= 360; + while (headingError <= -180) headingError += 360; + + // Multiply the error by the gain to determine the required steering correction/ Limit the result to +/- 1.0 + return Range.clip(headingError * proportionalGain, -1, 1); + } + + /** + * Take separate drive (fwd/rev) and turn (right/left) requests, + * combines them, and applies the appropriate speed commands to the left and right wheel motors. + * @param drive forward motor speed + * @param turn clockwise turning motor speed. + */ + public void moveRobot(double drive, double turn) { + driveSpeed = drive; // save this value as a class member so it can be used by telemetry. + turnSpeed = turn; // save this value as a class member so it can be used by telemetry. + + leftSpeed = drive - turn; + rightSpeed = drive + turn; + + // Scale speeds down if either one exceeds +/- 1.0; + double max = Math.max(Math.abs(leftSpeed), Math.abs(rightSpeed)); + if (max > 1.0) + { + leftSpeed /= max; + rightSpeed /= max; + } + + leftDrive.setPower(leftSpeed); + rightDrive.setPower(rightSpeed); + } + + /** + * Display the various control parameters while driving + * + * @param straight Set to true if we are driving straight, and the encoder positions should be included in the telemetry. + */ + private void sendTelemetry(boolean straight) { + + if (straight) { + telemetry.addData("Motion", "Drive Straight"); + telemetry.addData("Target Pos L:R", "%7d:%7d", leftTarget, rightTarget); + telemetry.addData("Actual Pos L:R", "%7d:%7d", leftDrive.getCurrentPosition(), + rightDrive.getCurrentPosition()); + } else { + telemetry.addData("Motion", "Turning"); + } + + telemetry.addData("Heading- Target : Current", "%5.2f : %5.0f", targetHeading, getHeading()); + telemetry.addData("Error : Steer Pwr", "%5.1f : %5.1f", headingError, turnSpeed); + telemetry.addData("Wheel Speeds L : R", "%5.2f : %5.2f", leftSpeed, rightSpeed); + telemetry.update(); + } + + /** + * read the Robot heading directly from the IMU (in degrees) + */ + public double getHeading() { + YawPitchRollAngles orientation = imu.getRobotYawPitchRollAngles(); + return orientation.getYaw(AngleUnit.DEGREES); + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveByTime_Linear.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveByTime_Linear.java new file mode 100644 index 0000000..a714748 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveByTime_Linear.java @@ -0,0 +1,128 @@ +/* Copyright (c) 2017 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.util.ElapsedTime; + +/* + * This OpMode illustrates the concept of driving a path based on time. + * The code is structured as a LinearOpMode + * + * The code assumes that you do NOT have encoders on the wheels, + * otherwise you would use: RobotAutoDriveByEncoder; + * + * The desired path in this example is: + * - Drive forward for 3 seconds + * - Spin right for 1.3 seconds + * - Drive Backward for 1 Second + * + * The code is written in a simple form with no optimizations. + * However, there are several ways that this type of sequence could be streamlined, + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list + */ + +@Autonomous(name="Robot: Auto Drive By Time", group="Robot") +@Disabled +public class RobotAutoDriveByTime_Linear extends LinearOpMode { + + /* Declare OpMode members. */ + private DcMotor leftDrive = null; + private DcMotor rightDrive = null; + + private ElapsedTime runtime = new ElapsedTime(); + + + static final double FORWARD_SPEED = 0.6; + static final double TURN_SPEED = 0.5; + + @Override + public void runOpMode() { + + // Initialize the drive system variables. + leftDrive = hardwareMap.get(DcMotor.class, "left_drive"); + rightDrive = hardwareMap.get(DcMotor.class, "right_drive"); + + // To drive forward, most robots need the motor on one side to be reversed, because the axles point in opposite directions. + // When run, this OpMode should start both motors driving forward. So adjust these two lines based on your first test drive. + // Note: The settings here assume direct drive on left and right wheels. Gear Reduction or 90 Deg drives may require direction flips + leftDrive.setDirection(DcMotor.Direction.REVERSE); + rightDrive.setDirection(DcMotor.Direction.FORWARD); + + // Send telemetry message to signify robot waiting; + telemetry.addData("Status", "Ready to run"); // + telemetry.update(); + + // Wait for the game to start (driver presses START) + waitForStart(); + + // Step through each leg of the path, ensuring that the OpMode has not been stopped along the way. + + // Step 1: Drive forward for 3 seconds + leftDrive.setPower(FORWARD_SPEED); + rightDrive.setPower(FORWARD_SPEED); + runtime.reset(); + while (opModeIsActive() && (runtime.seconds() < 3.0)) { + telemetry.addData("Path", "Leg 1: %4.1f S Elapsed", runtime.seconds()); + telemetry.update(); + } + + // Step 2: Spin right for 1.3 seconds + leftDrive.setPower(TURN_SPEED); + rightDrive.setPower(-TURN_SPEED); + runtime.reset(); + while (opModeIsActive() && (runtime.seconds() < 1.3)) { + telemetry.addData("Path", "Leg 2: %4.1f S Elapsed", runtime.seconds()); + telemetry.update(); + } + + // Step 3: Drive Backward for 1 Second + leftDrive.setPower(-FORWARD_SPEED); + rightDrive.setPower(-FORWARD_SPEED); + runtime.reset(); + while (opModeIsActive() && (runtime.seconds() < 1.0)) { + telemetry.addData("Path", "Leg 3: %4.1f S Elapsed", runtime.seconds()); + telemetry.update(); + } + + // Step 4: Stop + leftDrive.setPower(0); + rightDrive.setPower(0); + + telemetry.addData("Path", "Complete"); + telemetry.update(); + sleep(1000); + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveToAprilTagOmni.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveToAprilTagOmni.java new file mode 100644 index 0000000..4b777e2 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveToAprilTagOmni.java @@ -0,0 +1,321 @@ +/* Copyright (c) 2023 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.util.Range; +import org.firstinspires.ftc.robotcore.external.hardware.camera.BuiltinCameraDirection; +import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; +import org.firstinspires.ftc.robotcore.external.hardware.camera.controls.ExposureControl; +import org.firstinspires.ftc.robotcore.external.hardware.camera.controls.GainControl; +import org.firstinspires.ftc.vision.VisionPortal; +import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; +import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor; + +import java.util.List; +import java.util.concurrent.TimeUnit; + +/* + * This OpMode illustrates using a camera to locate and drive towards a specific AprilTag. + * The code assumes a Holonomic (Mecanum or X Drive) Robot. + * + * For an introduction to AprilTags, see the ftc-docs link below: + * https://ftc-docs.firstinspires.org/en/latest/apriltag/vision_portal/apriltag_intro/apriltag-intro.html + * + * When an AprilTag in the TagLibrary is detected, the SDK provides location and orientation of the tag, relative to the camera. + * This information is provided in the "ftcPose" member of the returned "detection", and is explained in the ftc-docs page linked below. + * https://ftc-docs.firstinspires.org/apriltag-detection-values + * + * The drive goal is to rotate to keep the Tag centered in the camera, while strafing to be directly in front of the tag, and + * driving towards the tag to achieve the desired distance. + * To reduce any motion blur (which will interrupt the detection process) the Camera exposure is reduced to a very low value (5mS) + * You can determine the best Exposure and Gain values by using the ConceptAprilTagOptimizeExposure OpMode in this Samples folder. + * + * The code assumes a Robot Configuration with motors named: front_left_drive and front_right_drive, back_left_drive and back_right_drive. + * The motor directions must be set so a positive power goes forward on all wheels. + * This sample assumes that the current game AprilTag Library (usually for the current season) is being loaded by default, + * so you should choose to approach a valid tag ID. + * + * Under manual control, the left stick will move forward/back & left/right. The right stick will rotate the robot. + * Manually drive the robot until it displays Target data on the Driver Station. + * + * Press and hold the *Left Bumper* to enable the automatic "Drive to target" mode. + * Release the Left Bumper to return to manual driving mode. + * + * Under "Drive To Target" mode, the robot has three goals: + * 1) Turn the robot to always keep the Tag centered on the camera frame. (Use the Target Bearing to turn the robot.) + * 2) Strafe the robot towards the centerline of the Tag, so it approaches directly in front of the tag. (Use the Target Yaw to strafe the robot) + * 3) Drive towards the Tag to get to the desired distance. (Use Tag Range to drive the robot forward/backward) + * + * Use DESIRED_DISTANCE to set how close you want the robot to get to the target. + * Speed and Turn sensitivity can be adjusted using the SPEED_GAIN, STRAFE_GAIN and TURN_GAIN constants. + * + * Use Android Studio to Copy this Class, and Paste it into the TeamCode/src/main/java/org/firstinspires/ftc/teamcode folder. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list. + * + */ + +@TeleOp(name="Omni Drive To AprilTag", group = "Concept") +@Disabled +public class RobotAutoDriveToAprilTagOmni extends LinearOpMode +{ + // Adjust these numbers to suit your robot. + final double DESIRED_DISTANCE = 12.0; // this is how close the camera should get to the target (inches) + + // Set the GAIN constants to control the relationship between the measured position error, and how much power is + // applied to the drive motors to correct the error. + // Drive = Error * Gain Make these values smaller for smoother control, or larger for a more aggressive response. + final double SPEED_GAIN = 0.02 ; // Forward Speed Control "Gain". e.g. Ramp up to 50% power at a 25 inch error. (0.50 / 25.0) + final double STRAFE_GAIN = 0.015 ; // Strafe Speed Control "Gain". e.g. Ramp up to 37% power at a 25 degree Yaw error. (0.375 / 25.0) + final double TURN_GAIN = 0.01 ; // Turn Control "Gain". e.g. Ramp up to 25% power at a 25 degree error. (0.25 / 25.0) + + final double MAX_AUTO_SPEED = 0.5; // Clip the approach speed to this max value (adjust for your robot) + final double MAX_AUTO_STRAFE= 0.5; // Clip the strafing speed to this max value (adjust for your robot) + final double MAX_AUTO_TURN = 0.3; // Clip the turn speed to this max value (adjust for your robot) + + private DcMotor frontLeftDrive = null; // Used to control the left front drive wheel + private DcMotor frontRightDrive = null; // Used to control the right front drive wheel + private DcMotor backLeftDrive = null; // Used to control the left back drive wheel + private DcMotor backRightDrive = null; // Used to control the right back drive wheel + + private static final boolean USE_WEBCAM = true; // Set true to use a webcam, or false for a phone camera + private static final int DESIRED_TAG_ID = -1; // Choose the tag you want to approach or set to -1 for ANY tag. + private VisionPortal visionPortal; // Used to manage the video source. + private AprilTagProcessor aprilTag; // Used for managing the AprilTag detection process. + private AprilTagDetection desiredTag = null; // Used to hold the data for a detected AprilTag + + @Override public void runOpMode() + { + boolean targetFound = false; // Set to true when an AprilTag target is detected + double drive = 0; // Desired forward power/speed (-1 to +1) + double strafe = 0; // Desired strafe power/speed (-1 to +1) + double turn = 0; // Desired turning power/speed (-1 to +1) + + // Initialize the Apriltag Detection process + initAprilTag(); + + // Initialize the hardware variables. Note that the strings used here as parameters + // to 'get' must match the names assigned during the robot configuration. + // step (using the FTC Robot Controller app on the phone). + frontLeftDrive = hardwareMap.get(DcMotor.class, "front_left_drive"); + frontRightDrive = hardwareMap.get(DcMotor.class, "front_right_drive"); + backLeftDrive = hardwareMap.get(DcMotor.class, "back_left_drive"); + backRightDrive = hardwareMap.get(DcMotor.class, "back_right_drive"); + + // To drive forward, most robots need the motor on one side to be reversed, because the axles point in opposite directions. + // When run, this OpMode should start both motors driving forward. So adjust these two lines based on your first test drive. + // Note: The settings here assume direct drive on left and right wheels. Gear Reduction or 90 Deg drives may require direction flips + frontLeftDrive.setDirection(DcMotor.Direction.REVERSE); + backLeftDrive.setDirection(DcMotor.Direction.REVERSE); + frontRightDrive.setDirection(DcMotor.Direction.FORWARD); + backRightDrive.setDirection(DcMotor.Direction.FORWARD); + + if (USE_WEBCAM) + setManualExposure(6, 250); // Use low exposure time to reduce motion blur + + // Wait for driver to press start + telemetry.addData("Camera preview on/off", "3 dots, Camera Stream"); + telemetry.addData(">", "Touch START to start OpMode"); + telemetry.update(); + waitForStart(); + + while (opModeIsActive()) + { + targetFound = false; + desiredTag = null; + + // Step through the list of detected tags and look for a matching tag + List currentDetections = aprilTag.getDetections(); + for (AprilTagDetection detection : currentDetections) { + // Look to see if we have size info on this tag. + if (detection.metadata != null) { + // Check to see if we want to track towards this tag. + if ((DESIRED_TAG_ID < 0) || (detection.id == DESIRED_TAG_ID)) { + // Yes, we want to use this tag. + targetFound = true; + desiredTag = detection; + break; // don't look any further. + } else { + // This tag is in the library, but we do not want to track it right now. + telemetry.addData("Skipping", "Tag ID %d is not desired", detection.id); + } + } else { + // This tag is NOT in the library, so we don't have enough information to track to it. + telemetry.addData("Unknown", "Tag ID %d is not in TagLibrary", detection.id); + } + } + + // Tell the driver what we see, and what to do. + if (targetFound) { + telemetry.addData("\n>","HOLD Left-Bumper to Drive to Target\n"); + telemetry.addData("Found", "ID %d (%s)", desiredTag.id, desiredTag.metadata.name); + telemetry.addData("Range", "%5.1f inches", desiredTag.ftcPose.range); + telemetry.addData("Bearing","%3.0f degrees", desiredTag.ftcPose.bearing); + telemetry.addData("Yaw","%3.0f degrees", desiredTag.ftcPose.yaw); + } else { + telemetry.addData("\n>","Drive using joysticks to find valid target\n"); + } + + // If Left Bumper is being pressed, AND we have found the desired target, Drive to target Automatically . + if (gamepad1.left_bumper && targetFound) { + + // Determine heading, range and Yaw (tag image rotation) error so we can use them to control the robot automatically. + double rangeError = (desiredTag.ftcPose.range - DESIRED_DISTANCE); + double headingError = desiredTag.ftcPose.bearing; + double yawError = desiredTag.ftcPose.yaw; + + // Use the speed and turn "gains" to calculate how we want the robot to move. + drive = Range.clip(rangeError * SPEED_GAIN, -MAX_AUTO_SPEED, MAX_AUTO_SPEED); + turn = Range.clip(headingError * TURN_GAIN, -MAX_AUTO_TURN, MAX_AUTO_TURN) ; + strafe = Range.clip(-yawError * STRAFE_GAIN, -MAX_AUTO_STRAFE, MAX_AUTO_STRAFE); + + telemetry.addData("Auto","Drive %5.2f, Strafe %5.2f, Turn %5.2f ", drive, strafe, turn); + } else { + + // drive using manual POV Joystick mode. Slow things down to make the robot more controlable. + drive = -gamepad1.left_stick_y / 2.0; // Reduce drive rate to 50%. + strafe = -gamepad1.left_stick_x / 2.0; // Reduce strafe rate to 50%. + turn = -gamepad1.right_stick_x / 3.0; // Reduce turn rate to 33%. + telemetry.addData("Manual","Drive %5.2f, Strafe %5.2f, Turn %5.2f ", drive, strafe, turn); + } + telemetry.update(); + + // Apply desired axes motions to the drivetrain. + moveRobot(drive, strafe, turn); + sleep(10); + } + } + + /** + * Move robot according to desired axes motions + *

+ * Positive X is forward + *

+ * Positive Y is strafe left + *

+ * Positive Yaw is counter-clockwise + */ + public void moveRobot(double x, double y, double yaw) { + // Calculate wheel powers. + double frontLeftPower = x - y - yaw; + double frontRightPower = x + y + yaw; + double backLeftPower = x + y - yaw; + double backRightPower = x - y + yaw; + + // Normalize wheel powers to be less than 1.0 + double max = Math.max(Math.abs(frontLeftPower), Math.abs(frontRightPower)); + max = Math.max(max, Math.abs(backLeftPower)); + max = Math.max(max, Math.abs(backRightPower)); + + if (max > 1.0) { + frontLeftPower /= max; + frontRightPower /= max; + backLeftPower /= max; + backRightPower /= max; + } + + // Send powers to the wheels. + frontLeftDrive.setPower(frontLeftPower); + frontRightDrive.setPower(frontRightPower); + backLeftDrive.setPower(backLeftPower); + backRightDrive.setPower(backRightPower); + } + + /** + * Initialize the AprilTag processor. + */ + private void initAprilTag() { + // Create the AprilTag processor by using a builder. + aprilTag = new AprilTagProcessor.Builder().build(); + + // Adjust Image Decimation to trade-off detection-range for detection-rate. + // e.g. Some typical detection data using a Logitech C920 WebCam + // Decimation = 1 .. Detect 2" Tag from 10 feet away at 10 Frames per second + // Decimation = 2 .. Detect 2" Tag from 6 feet away at 22 Frames per second + // Decimation = 3 .. Detect 2" Tag from 4 feet away at 30 Frames Per Second + // Decimation = 3 .. Detect 5" Tag from 10 feet away at 30 Frames Per Second + // Note: Decimation can be changed on-the-fly to adapt during a match. + aprilTag.setDecimation(2); + + // Create the vision portal by using a builder. + if (USE_WEBCAM) { + visionPortal = new VisionPortal.Builder() + .setCamera(hardwareMap.get(WebcamName.class, "Webcam 1")) + .addProcessor(aprilTag) + .build(); + } else { + visionPortal = new VisionPortal.Builder() + .setCamera(BuiltinCameraDirection.BACK) + .addProcessor(aprilTag) + .build(); + } + } + + /* + Manually set the camera gain and exposure. + This can only be called AFTER calling initAprilTag(), and only works for Webcams; + */ + private void setManualExposure(int exposureMS, int gain) { + // Wait for the camera to be open, then use the controls + + if (visionPortal == null) { + return; + } + + // Make sure camera is streaming before we try to set the exposure controls + if (visionPortal.getCameraState() != VisionPortal.CameraState.STREAMING) { + telemetry.addData("Camera", "Waiting"); + telemetry.update(); + while (!isStopRequested() && (visionPortal.getCameraState() != VisionPortal.CameraState.STREAMING)) { + sleep(20); + } + telemetry.addData("Camera", "Ready"); + telemetry.update(); + } + + // Set camera controls unless we are stopping. + if (!isStopRequested()) + { + ExposureControl exposureControl = visionPortal.getCameraControl(ExposureControl.class); + if (exposureControl.getMode() != ExposureControl.Mode.Manual) { + exposureControl.setMode(ExposureControl.Mode.Manual); + sleep(50); + } + exposureControl.setExposure((long)exposureMS, TimeUnit.MILLISECONDS); + sleep(20); + GainControl gainControl = visionPortal.getCameraControl(GainControl.class); + gainControl.setGain(gain); + sleep(20); + } + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveToAprilTagTank.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveToAprilTagTank.java new file mode 100644 index 0000000..ba3eb4f --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveToAprilTagTank.java @@ -0,0 +1,298 @@ +/* Copyright (c) 2023 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.util.Range; +import org.firstinspires.ftc.robotcore.external.hardware.camera.BuiltinCameraDirection; +import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; +import org.firstinspires.ftc.robotcore.external.hardware.camera.controls.ExposureControl; +import org.firstinspires.ftc.robotcore.external.hardware.camera.controls.GainControl; +import org.firstinspires.ftc.vision.VisionPortal; +import org.firstinspires.ftc.vision.apriltag.AprilTagDetection; +import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor; + +import java.util.List; +import java.util.concurrent.TimeUnit; + +/* + * This OpMode illustrates using a camera to locate and drive towards a specific AprilTag. + * The code assumes a basic two-wheel (Tank) Robot Drivetrain + * + * For an introduction to AprilTags, see the ftc-docs link below: + * https://ftc-docs.firstinspires.org/en/latest/apriltag/vision_portal/apriltag_intro/apriltag-intro.html + * + * When an AprilTag in the TagLibrary is detected, the SDK provides location and orientation of the tag, relative to the camera. + * This information is provided in the "ftcPose" member of the returned "detection", and is explained in the ftc-docs page linked below. + * https://ftc-docs.firstinspires.org/apriltag-detection-values + * + * The driving goal is to rotate to keep the tag centered in the camera, while driving towards the tag to achieve the desired distance. + * To reduce any motion blur (which will interrupt the detection process) the Camera exposure is reduced to a very low value (5mS) + * You can determine the best exposure and gain values by using the ConceptAprilTagOptimizeExposure OpMode in this Samples folder. + * + * The code assumes a Robot Configuration with motors named left_drive and right_drive. + * The motor directions must be set so a positive power goes forward on both wheels; + * This sample assumes that the default AprilTag Library (usually for the current season) is being loaded by default + * so you should choose to approach a valid tag ID. + * + * Under manual control, the left stick will move forward/back, and the right stick will rotate the robot. + * This is called POV Joystick mode, different than Tank Drive (where each joystick controls a wheel). + * + * Manually drive the robot until it displays Target data on the Driver Station. + * Press and hold the *Left Bumper* to enable the automatic "Drive to target" mode. + * Release the Left Bumper to return to manual driving mode. + * + * Under "Drive To Target" mode, the robot has two goals: + * 1) Turn the robot to always keep the Tag centered on the camera frame. (Use the Target Bearing to turn the robot.) + * 2) Drive towards the Tag to get to the desired distance. (Use Tag Range to drive the robot forward/backward) + * + * Use DESIRED_DISTANCE to set how close you want the robot to get to the target. + * Speed and Turn sensitivity can be adjusted using the SPEED_GAIN and TURN_GAIN constants. + * + * Use Android Studio to Copy this Class, and Paste it into the TeamCode/src/main/java/org/firstinspires/ftc/teamcode folder. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list. + * + */ + +@TeleOp(name="Tank Drive To AprilTag", group = "Concept") +@Disabled +public class RobotAutoDriveToAprilTagTank extends LinearOpMode +{ + // Adjust these numbers to suit your robot. + final double DESIRED_DISTANCE = 12.0; // this is how close the camera should get to the target (inches) + + // Set the GAIN constants to control the relationship between the measured position error, and how much power is + // applied to the drive motors to correct the error. + // Drive = Error * Gain Make these values smaller for smoother control, or larger for a more aggressive response. + final double SPEED_GAIN = 0.02 ; // Speed Control "Gain". e.g. Ramp up to 50% power at a 25 inch error. (0.50 / 25.0) + final double TURN_GAIN = 0.01 ; // Turn Control "Gain". e.g. Ramp up to 25% power at a 25 degree error. (0.25 / 25.0) + + final double MAX_AUTO_SPEED = 0.5; // Clip the approach speed to this max value (adjust for your robot) + final double MAX_AUTO_TURN = 0.25; // Clip the turn speed to this max value (adjust for your robot) + + private DcMotor leftDrive = null; // Used to control the left drive wheel + private DcMotor rightDrive = null; // Used to control the right drive wheel + + private static final boolean USE_WEBCAM = true; // Set true to use a webcam, or false for a phone camera + private static final int DESIRED_TAG_ID = -1; // Choose the tag you want to approach or set to -1 for ANY tag. + private VisionPortal visionPortal; // Used to manage the video source. + private AprilTagProcessor aprilTag; // Used for managing the AprilTag detection process. + private AprilTagDetection desiredTag = null; // Used to hold the data for a detected AprilTag + + @Override public void runOpMode() + { + boolean targetFound = false; // Set to true when an AprilTag target is detected + double drive = 0; // Desired forward power/speed (-1 to +1) +ve is forward + double turn = 0; // Desired turning power/speed (-1 to +1) +ve is CounterClockwise + + // Initialize the Apriltag Detection process + initAprilTag(); + + // Initialize the hardware variables. Note that the strings used here as parameters + // to 'get' must match the names assigned during the robot configuration. + // step (using the FTC Robot Controller app on the phone). + leftDrive = hardwareMap.get(DcMotor.class, "left_drive"); + rightDrive = hardwareMap.get(DcMotor.class, "right_drive"); + + // To drive forward, most robots need the motor on one side to be reversed because the axles point in opposite directions. + // When run, this OpMode should start both motors driving forward. So adjust these two lines based on your first test drive. + // Note: The settings here assume direct drive on left and right wheels. Single Gear Reduction or 90 Deg drives may require direction flips + leftDrive.setDirection(DcMotor.Direction.REVERSE); + rightDrive.setDirection(DcMotor.Direction.FORWARD); + + if (USE_WEBCAM) + setManualExposure(6, 250); // Use low exposure time to reduce motion blur + + // Wait for the driver to press Start + telemetry.addData("Camera preview on/off", "3 dots, Camera Stream"); + telemetry.addData(">", "Touch START to start OpMode"); + telemetry.update(); + waitForStart(); + + while (opModeIsActive()) + { + targetFound = false; + desiredTag = null; + + // Step through the list of detected tags and look for a matching tag + List currentDetections = aprilTag.getDetections(); + for (AprilTagDetection detection : currentDetections) { + // Look to see if we have size info on this tag. + if (detection.metadata != null) { + // Check to see if we want to track towards this tag. + if ((DESIRED_TAG_ID < 0) || (detection.id == DESIRED_TAG_ID)) { + // Yes, we want to use this tag. + targetFound = true; + desiredTag = detection; + break; // don't look any further. + } else { + // This tag is in the library, but we do not want to track it right now. + telemetry.addData("Skipping", "Tag ID %d is not desired", detection.id); + } + } else { + // This tag is NOT in the library, so we don't have enough information to track to it. + telemetry.addData("Unknown", "Tag ID %d is not in TagLibrary", detection.id); + } + } + + // Tell the driver what we see, and what to do. + if (targetFound) { + telemetry.addData("\n>","HOLD Left-Bumper to Drive to Target\n"); + telemetry.addData("Found", "ID %d (%s)", desiredTag.id, desiredTag.metadata.name); + telemetry.addData("Range", "%5.1f inches", desiredTag.ftcPose.range); + telemetry.addData("Bearing","%3.0f degrees", desiredTag.ftcPose.bearing); + } else { + telemetry.addData("\n>","Drive using joysticks to find valid target\n"); + } + + // If Left Bumper is being pressed, AND we have found the desired target, Drive to target Automatically . + if (gamepad1.left_bumper && targetFound) { + + // Determine heading and range error so we can use them to control the robot automatically. + double rangeError = (desiredTag.ftcPose.range - DESIRED_DISTANCE); + double headingError = desiredTag.ftcPose.bearing; + + // Use the speed and turn "gains" to calculate how we want the robot to move. Clip it to the maximum + drive = Range.clip(rangeError * SPEED_GAIN, -MAX_AUTO_SPEED, MAX_AUTO_SPEED); + turn = Range.clip(headingError * TURN_GAIN, -MAX_AUTO_TURN, MAX_AUTO_TURN) ; + + telemetry.addData("Auto","Drive %5.2f, Turn %5.2f", drive, turn); + } else { + + // drive using manual POV Joystick mode. + drive = -gamepad1.left_stick_y / 2.0; // Reduce drive rate to 50%. + turn = -gamepad1.right_stick_x / 4.0; // Reduce turn rate to 25%. + telemetry.addData("Manual","Drive %5.2f, Turn %5.2f", drive, turn); + } + telemetry.update(); + + // Apply desired axes motions to the drivetrain. + moveRobot(drive, turn); + sleep(10); + } + } + + /** + * Move robot according to desired axes motions + *

+ * Positive X is forward + *

+ * Positive Yaw is counter-clockwise + */ + public void moveRobot(double x, double yaw) { + // Calculate left and right wheel powers. + double leftPower = x - yaw; + double rightPower = x + yaw; + + // Normalize wheel powers to be less than 1.0 + double max = Math.max(Math.abs(leftPower), Math.abs(rightPower)); + if (max >1.0) { + leftPower /= max; + rightPower /= max; + } + + // Send powers to the wheels. + leftDrive.setPower(leftPower); + rightDrive.setPower(rightPower); + } + + /** + * Initialize the AprilTag processor. + */ + private void initAprilTag() { + // Create the AprilTag processor by using a builder. + aprilTag = new AprilTagProcessor.Builder().build(); + + // Adjust Image Decimation to trade-off detection-range for detection-rate. + // e.g. Some typical detection data using a Logitech C920 WebCam + // Decimation = 1 .. Detect 2" Tag from 10 feet away at 10 Frames per second + // Decimation = 2 .. Detect 2" Tag from 6 feet away at 22 Frames per second + // Decimation = 3 .. Detect 2" Tag from 4 feet away at 30 Frames Per Second + // Decimation = 3 .. Detect 5" Tag from 10 feet away at 30 Frames Per Second + // Note: Decimation can be changed on-the-fly to adapt during a match. + aprilTag.setDecimation(2); + + // Create the vision portal by using a builder. + if (USE_WEBCAM) { + visionPortal = new VisionPortal.Builder() + .setCamera(hardwareMap.get(WebcamName.class, "Webcam 1")) + .addProcessor(aprilTag) + .build(); + } else { + visionPortal = new VisionPortal.Builder() + .setCamera(BuiltinCameraDirection.BACK) + .addProcessor(aprilTag) + .build(); + } + } + + /* + Manually set the camera gain and exposure. + This can only be called AFTER calling initAprilTag(), and only works for Webcams; + */ + private void setManualExposure(int exposureMS, int gain) { + // Wait for the camera to be open, then use the controls + + if (visionPortal == null) { + return; + } + + // Make sure camera is streaming before we try to set the exposure controls + if (visionPortal.getCameraState() != VisionPortal.CameraState.STREAMING) { + telemetry.addData("Camera", "Waiting"); + telemetry.update(); + while (!isStopRequested() && (visionPortal.getCameraState() != VisionPortal.CameraState.STREAMING)) { + sleep(20); + } + telemetry.addData("Camera", "Ready"); + telemetry.update(); + } + + // Set camera controls unless we are stopping. + if (!isStopRequested()) + { + ExposureControl exposureControl = visionPortal.getCameraControl(ExposureControl.class); + if (exposureControl.getMode() != ExposureControl.Mode.Manual) { + exposureControl.setMode(ExposureControl.Mode.Manual); + sleep(50); + } + exposureControl.setExposure((long)exposureMS, TimeUnit.MILLISECONDS); + sleep(20); + GainControl gainControl = visionPortal.getCameraControl(GainControl.class); + gainControl.setGain(gain); + sleep(20); + telemetry.addData("Camera", "Ready"); + telemetry.update(); + } + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveToLine_Linear.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveToLine_Linear.java new file mode 100644 index 0000000..780f260 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveToLine_Linear.java @@ -0,0 +1,142 @@ +/* Copyright (c) 2017 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.robotcore.eventloop.opmode.Autonomous; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.NormalizedColorSensor; +import com.qualcomm.robotcore.hardware.NormalizedRGBA; +import com.qualcomm.robotcore.hardware.SwitchableLight; + +/* + * This OpMode illustrates the concept of driving up to a line and then stopping. + * The code is structured as a LinearOpMode + * + * The Sensor used here can be a REV Color Sensor V2 or V3. Make sure the white LED is turned on. + * The sensor can be plugged into any I2C port, and must be named "sensor_color" in the active configuration. + * + * Depending on the height of your color sensor, you may want to set the sensor "gain". + * The higher the gain, the greater the reflected light reading will be. + * Use the SensorColor sample in this folder to determine the minimum gain value that provides an + * "Alpha" reading of 1.0 when you are on top of the white line. In this sample, we use a gain of 15 + * which works well with a Rev V2 color sensor + * + * Setting the correct WHITE_THRESHOLD value is key to stopping correctly. + * This should be set halfway between the bare-tile, and white-line "Alpha" values. + * The reflected light value can be read on the screen once the OpMode has been INIT, but before it is STARTED. + * Move the sensor on and off the white line and note the min and max readings. + * Edit this code to make WHITE_THRESHOLD halfway between the min and max. + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list + */ + +@Autonomous(name="Robot: Auto Drive To Line", group="Robot") +@Disabled +public class RobotAutoDriveToLine_Linear extends LinearOpMode { + + /* Declare OpMode members. */ + private DcMotor leftDrive = null; + private DcMotor rightDrive = null; + + /** The variable to store a reference to our color sensor hardware object */ + NormalizedColorSensor colorSensor; + + static final double WHITE_THRESHOLD = 0.5; // spans between 0.0 - 1.0 from dark to light + static final double APPROACH_SPEED = 0.25; + + @Override + public void runOpMode() { + + // Initialize the drive system variables. + leftDrive = hardwareMap.get(DcMotor.class, "left_drive"); + rightDrive = hardwareMap.get(DcMotor.class, "right_drive"); + + // To drive forward, most robots need the motor on one side to be reversed, because the axles point in opposite directions. + // When run, this OpMode should start both motors driving forward. So adjust these two lines based on your first test drive. + // Note: The settings here assume direct drive on left and right wheels. Gear Reduction or 90 Deg drives may require direction flips + leftDrive.setDirection(DcMotor.Direction.REVERSE); + rightDrive.setDirection(DcMotor.Direction.FORWARD); + + // If there are encoders connected, switch to RUN_USING_ENCODER mode for greater accuracy + // leftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + // rightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + + // Get a reference to our sensor object. It's recommended to use NormalizedColorSensor over + // ColorSensor, because NormalizedColorSensor consistently gives values between 0 and 1, while + // the values you get from ColorSensor are dependent on the specific sensor you're using. + colorSensor = hardwareMap.get(NormalizedColorSensor.class, "sensor_color"); + + // If necessary, turn ON the white LED (if there is no LED switch on the sensor) + if (colorSensor instanceof SwitchableLight) { + ((SwitchableLight)colorSensor).enableLight(true); + } + + // Some sensors allow you to set your light sensor gain for optimal sensitivity... + // See the SensorColor sample in this folder for how to determine the optimal gain. + // A gain of 15 causes a Rev Color Sensor V2 to produce an Alpha value of 1.0 at about 1.5" above the floor. + colorSensor.setGain(15); + + // Wait for driver to press START) + // Abort this loop is started or stopped. + while (opModeInInit()) { + + // Send telemetry message to signify robot waiting; + telemetry.addData("Status", "Ready to drive to white line."); // + + // Display the light level while we are waiting to start + getBrightness(); + } + + // Start the robot moving forward, and then begin looking for a white line. + leftDrive.setPower(APPROACH_SPEED); + rightDrive.setPower(APPROACH_SPEED); + + // run until the white line is seen OR the driver presses STOP; + while (opModeIsActive() && (getBrightness() < WHITE_THRESHOLD)) { + sleep(5); + } + + // Stop all motors + leftDrive.setPower(0); + rightDrive.setPower(0); + } + + // to obtain reflected light, read the normalized values from the color sensor. Return the Alpha channel. + double getBrightness() { + NormalizedRGBA colors = colorSensor.getNormalizedColors(); + telemetry.addData("Light Level (0 to 1)", "%4.2f", colors.alpha); + telemetry.update(); + + return colors.alpha; + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotTeleopMecanumFieldRelativeDrive.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotTeleopMecanumFieldRelativeDrive.java new file mode 100644 index 0000000..c5c64d0 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotTeleopMecanumFieldRelativeDrive.java @@ -0,0 +1,163 @@ +/* Copyright (c) 2025 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.IMU; + +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +/* + * This OpMode illustrates how to program your robot to drive field relative. This means + * that the robot drives the direction you push the joystick regardless of the current orientation + * of the robot. + * + * This OpMode assumes that you have four mecanum wheels each on its own motor named: + * front_left_motor, front_right_motor, back_left_motor, back_right_motor + * + * and that the left motors are flipped such that when they turn clockwise the wheel moves backwards + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list + * + */ +@TeleOp(name = "Robot: Field Relative Mecanum Drive", group = "Robot") +@Disabled +public class RobotTeleopMecanumFieldRelativeDrive extends OpMode { + // This declares the four motors needed + DcMotor frontLeftDrive; + DcMotor frontRightDrive; + DcMotor backLeftDrive; + DcMotor backRightDrive; + + // This declares the IMU needed to get the current direction the robot is facing + IMU imu; + + @Override + public void init() { + frontLeftDrive = hardwareMap.get(DcMotor.class, "front_left_drive"); + frontRightDrive = hardwareMap.get(DcMotor.class, "front_right_drive"); + backLeftDrive = hardwareMap.get(DcMotor.class, "back_left_drive"); + backRightDrive = hardwareMap.get(DcMotor.class, "back_right_drive"); + + // We set the left motors in reverse which is needed for drive trains where the left + // motors are opposite to the right ones. + backLeftDrive.setDirection(DcMotor.Direction.REVERSE); + frontLeftDrive.setDirection(DcMotor.Direction.REVERSE); + + // This uses RUN_USING_ENCODER to be more accurate. If you don't have the encoder + // wires, you should remove these + frontLeftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + frontRightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + backLeftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + backRightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + + imu = hardwareMap.get(IMU.class, "imu"); + // This needs to be changed to match the orientation on your robot + RevHubOrientationOnRobot.LogoFacingDirection logoDirection = + RevHubOrientationOnRobot.LogoFacingDirection.UP; + RevHubOrientationOnRobot.UsbFacingDirection usbDirection = + RevHubOrientationOnRobot.UsbFacingDirection.FORWARD; + + RevHubOrientationOnRobot orientationOnRobot = new + RevHubOrientationOnRobot(logoDirection, usbDirection); + imu.initialize(new IMU.Parameters(orientationOnRobot)); + } + + @Override + public void loop() { + telemetry.addLine("Press A to reset Yaw"); + telemetry.addLine("Hold left bumper to drive in robot relative"); + telemetry.addLine("The left joystick sets the robot direction"); + telemetry.addLine("Moving the right joystick left and right turns the robot"); + + // If you press the A button, then you reset the Yaw to be zero from the way + // the robot is currently pointing + if (gamepad1.a) { + imu.resetYaw(); + } + // If you press the left bumper, you get a drive from the point of view of the robot + // (much like driving an RC vehicle) + if (gamepad1.left_bumper) { + drive(-gamepad1.left_stick_y, gamepad1.left_stick_x, gamepad1.right_stick_x); + } else { + driveFieldRelative(-gamepad1.left_stick_y, gamepad1.left_stick_x, gamepad1.right_stick_x); + } + } + + // This routine drives the robot field relative + private void driveFieldRelative(double forward, double right, double rotate) { + // First, convert direction being asked to drive to polar coordinates + double theta = Math.atan2(forward, right); + double r = Math.hypot(right, forward); + + // Second, rotate angle by the angle the robot is pointing + theta = AngleUnit.normalizeRadians(theta - + imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS)); + + // Third, convert back to cartesian + double newForward = r * Math.sin(theta); + double newRight = r * Math.cos(theta); + + // Finally, call the drive method with robot relative forward and right amounts + drive(newForward, newRight, rotate); + } + + // Thanks to FTC16072 for sharing this code!! + public void drive(double forward, double right, double rotate) { + // This calculates the power needed for each wheel based on the amount of forward, + // strafe right, and rotate + double frontLeftPower = forward + right + rotate; + double frontRightPower = forward - right - rotate; + double backRightPower = forward + right - rotate; + double backLeftPower = forward - right + rotate; + + double maxPower = 1.0; + double maxSpeed = 1.0; // make this slower for outreaches + + // This is needed to make sure we don't pass > 1.0 to any wheel + // It allows us to keep all of the motors in proportion to what they should + // be and not get clipped + maxPower = Math.max(maxPower, Math.abs(frontLeftPower)); + maxPower = Math.max(maxPower, Math.abs(frontRightPower)); + maxPower = Math.max(maxPower, Math.abs(backRightPower)); + maxPower = Math.max(maxPower, Math.abs(backLeftPower)); + + // We multiply by maxSpeed so that it can be set lower for outreaches + // When a young child is driving the robot, we may not want to allow full + // speed. + frontLeftDrive.setPower(maxSpeed * (frontLeftPower / maxPower)); + frontRightDrive.setPower(maxSpeed * (frontRightPower / maxPower)); + backLeftDrive.setPower(maxSpeed * (backLeftPower / maxPower)); + backRightDrive.setPower(maxSpeed * (backRightPower / maxPower)); + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotTeleopPOV_Linear.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotTeleopPOV_Linear.java new file mode 100644 index 0000000..af3840d --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotTeleopPOV_Linear.java @@ -0,0 +1,159 @@ +/* Copyright (c) 2017 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.Servo; +import com.qualcomm.robotcore.util.Range; + +/* + * This OpMode executes a POV Game style Teleop for a direct drive robot + * The code is structured as a LinearOpMode + * + * In this mode the left stick moves the robot FWD and back, the Right stick turns left and right. + * It raises and lowers the arm using the Gamepad Y and A buttons respectively. + * It also opens and closes the claws slowly using the left and right Bumper buttons. + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list + */ + +@TeleOp(name="Robot: Teleop POV", group="Robot") +@Disabled +public class RobotTeleopPOV_Linear extends LinearOpMode { + + /* Declare OpMode members. */ + public DcMotor leftDrive = null; + public DcMotor rightDrive = null; + public DcMotor leftArm = null; + public Servo leftClaw = null; + public Servo rightClaw = null; + + double clawOffset = 0; + + public static final double MID_SERVO = 0.5 ; + public static final double CLAW_SPEED = 0.02 ; // sets rate to move servo + public static final double ARM_UP_POWER = 0.45 ; + public static final double ARM_DOWN_POWER = -0.45 ; + + @Override + public void runOpMode() { + double left; + double right; + double drive; + double turn; + double max; + + // Define and Initialize Motors + leftDrive = hardwareMap.get(DcMotor.class, "left_drive"); + rightDrive = hardwareMap.get(DcMotor.class, "right_drive"); + leftArm = hardwareMap.get(DcMotor.class, "left_arm"); + + // To drive forward, most robots need the motor on one side to be reversed, because the axles point in opposite directions. + // Pushing the left stick forward MUST make robot go forward. So adjust these two lines based on your first test drive. + // Note: The settings here assume direct drive on left and right wheels. Gear Reduction or 90 Deg drives may require direction flips + leftDrive.setDirection(DcMotor.Direction.REVERSE); + rightDrive.setDirection(DcMotor.Direction.FORWARD); + + // If there are encoders connected, switch to RUN_USING_ENCODER mode for greater accuracy + // leftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + // rightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + + // Define and initialize ALL installed servos. + leftClaw = hardwareMap.get(Servo.class, "left_hand"); + rightClaw = hardwareMap.get(Servo.class, "right_hand"); + leftClaw.setPosition(MID_SERVO); + rightClaw.setPosition(MID_SERVO); + + // Send telemetry message to signify robot waiting; + telemetry.addData(">", "Robot Ready. Press START."); // + telemetry.update(); + + // Wait for the game to start (driver presses START) + waitForStart(); + + // run until the end of the match (driver presses STOP) + while (opModeIsActive()) { + + // Run wheels in POV mode (note: The joystick goes negative when pushed forward, so negate it) + // In this mode the Left stick moves the robot fwd and back, the Right stick turns left and right. + // This way it's also easy to just drive straight, or just turn. + drive = -gamepad1.left_stick_y; + turn = gamepad1.right_stick_x; + + // Combine drive and turn for blended motion. + left = drive + turn; + right = drive - turn; + + // Normalize the values so neither exceed +/- 1.0 + max = Math.max(Math.abs(left), Math.abs(right)); + if (max > 1.0) + { + left /= max; + right /= max; + } + + // Output the safe vales to the motor drives. + leftDrive.setPower(left); + rightDrive.setPower(right); + + // Use gamepad left & right Bumpers to open and close the claw + if (gamepad1.right_bumper) + clawOffset += CLAW_SPEED; + else if (gamepad1.left_bumper) + clawOffset -= CLAW_SPEED; + + // Move both servos to new position. Assume servos are mirror image of each other. + clawOffset = Range.clip(clawOffset, -0.5, 0.5); + leftClaw.setPosition(MID_SERVO + clawOffset); + rightClaw.setPosition(MID_SERVO - clawOffset); + + // Use gamepad buttons to move arm up (Y) and down (A) + if (gamepad1.y) + leftArm.setPower(ARM_UP_POWER); + else if (gamepad1.a) + leftArm.setPower(ARM_DOWN_POWER); + else + leftArm.setPower(0.0); + + // Send telemetry message to signify robot running; + telemetry.addData("claw", "Offset = %.2f", clawOffset); + telemetry.addData("left", "%.2f", left); + telemetry.addData("right", "%.2f", right); + telemetry.update(); + + // Pace this loop so jaw action is reasonable speed. + sleep(50); + } + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotTeleopTank_Iterative.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotTeleopTank_Iterative.java new file mode 100644 index 0000000..a622f27 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotTeleopTank_Iterative.java @@ -0,0 +1,160 @@ +/* Copyright (c) 2017 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.Servo; +import com.qualcomm.robotcore.util.Range; + +/* + * This OpMode executes a Tank Drive control TeleOp a direct drive robot + * The code is structured as an Iterative OpMode + * + * In this mode, the left and right joysticks control the left and right motors respectively. + * Pushing a joystick forward will make the attached motor drive forward. + * It raises and lowers the claw using the Gamepad Y and A buttons respectively. + * It also opens and closes the claws slowly using the left and right Bumper buttons. + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list + */ + +@TeleOp(name="Robot: Teleop Tank", group="Robot") +@Disabled +public class RobotTeleopTank_Iterative extends OpMode{ + + /* Declare OpMode members. */ + public DcMotor leftDrive = null; + public DcMotor rightDrive = null; + public DcMotor leftArm = null; + public Servo leftClaw = null; + public Servo rightClaw = null; + + double clawOffset = 0; + + public static final double MID_SERVO = 0.5 ; + public static final double CLAW_SPEED = 0.02 ; // sets rate to move servo + public static final double ARM_UP_POWER = 0.50 ; // Run arm motor up at 50% power + public static final double ARM_DOWN_POWER = -0.25 ; // Run arm motor down at -25% power + + /* + * Code to run ONCE when the driver hits INIT + */ + @Override + public void init() { + // Define and Initialize Motors + leftDrive = hardwareMap.get(DcMotor.class, "left_drive"); + rightDrive = hardwareMap.get(DcMotor.class, "right_drive"); + leftArm = hardwareMap.get(DcMotor.class, "left_arm"); + + // To drive forward, most robots need the motor on one side to be reversed, because the axles point in opposite directions. + // Pushing the left and right sticks forward MUST make robot go forward. So adjust these two lines based on your first test drive. + // Note: The settings here assume direct drive on left and right wheels. Gear Reduction or 90 Deg drives may require direction flips + leftDrive.setDirection(DcMotor.Direction.REVERSE); + rightDrive.setDirection(DcMotor.Direction.FORWARD); + + // If there are encoders connected, switch to RUN_USING_ENCODER mode for greater accuracy + // leftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + // rightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + + // Define and initialize ALL installed servos. + leftClaw = hardwareMap.get(Servo.class, "left_hand"); + rightClaw = hardwareMap.get(Servo.class, "right_hand"); + leftClaw.setPosition(MID_SERVO); + rightClaw.setPosition(MID_SERVO); + + // Send telemetry message to signify robot waiting; + telemetry.addData(">", "Robot Ready. Press START."); // + } + + /* + * Code to run REPEATEDLY after the driver hits INIT, but before they hit START + */ + @Override + public void init_loop() { + } + + /* + * Code to run ONCE when the driver hits START + */ + @Override + public void start() { + } + + /* + * Code to run REPEATEDLY after the driver hits START but before they hit STOP + */ + @Override + public void loop() { + double left; + double right; + + // Run wheels in tank mode (note: The joystick goes negative when pushed forward, so negate it) + left = -gamepad1.left_stick_y; + right = -gamepad1.right_stick_y; + + leftDrive.setPower(left); + rightDrive.setPower(right); + + // Use gamepad left & right Bumpers to open and close the claw + if (gamepad1.right_bumper) + clawOffset += CLAW_SPEED; + else if (gamepad1.left_bumper) + clawOffset -= CLAW_SPEED; + + // Move both servos to new position. Assume servos are mirror image of each other. + clawOffset = Range.clip(clawOffset, -0.5, 0.5); + leftClaw.setPosition(MID_SERVO + clawOffset); + rightClaw.setPosition(MID_SERVO - clawOffset); + + // Use gamepad buttons to move the arm up (Y) and down (A) + if (gamepad1.y) + leftArm.setPower(ARM_UP_POWER); + else if (gamepad1.a) + leftArm.setPower(ARM_DOWN_POWER); + else + leftArm.setPower(0.0); + + // Send telemetry message to signify robot running; + telemetry.addData("claw", "Offset = %.2f", clawOffset); + telemetry.addData("left", "%.2f", left); + telemetry.addData("right", "%.2f", right); + } + + /* + * Code to run ONCE after the driver hits STOP + */ + @Override + public void stop() { + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SampleRevBlinkinLedDriver.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SampleRevBlinkinLedDriver.java new file mode 100644 index 0000000..bcf5b80 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SampleRevBlinkinLedDriver.java @@ -0,0 +1,163 @@ +/* + * Copyright (c) 2018 Craig MacFarlane + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, are permitted + * (subject to the limitations in the disclaimer below) provided that the following conditions are + * met: + * + * Redistributions of source code must retain the above copyright notice, this list of conditions + * and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this list of conditions + * and the following disclaimer in the documentation and/or other materials provided with the + * distribution. + * + * Neither the name of Craig MacFarlane nor the names of its contributors may be used to + * endorse or promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS LICENSE. THIS + * SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED + * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, + * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF + * THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.hardware.rev.RevBlinkinLedDriver; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.robotcore.internal.system.Deadline; + +import java.util.concurrent.TimeUnit; + +/* + * This OpMode demonstrates use of the REV Robotics Blinkin LED Driver. + * AUTO mode cycles through all of the patterns. + * MANUAL mode allows the user to manually change patterns using the + * left and right bumpers of a gamepad. + * + * Configure the driver on a servo port, and name it "blinkin". + * + * Displays the first pattern upon init. + */ +@TeleOp(name="BlinkinExample") +@Disabled +public class SampleRevBlinkinLedDriver extends OpMode { + + /* + * Change the pattern every 10 seconds in AUTO mode. + */ + private final static int LED_PERIOD = 10; + + /* + * Rate limit gamepad button presses to every 500ms. + */ + private final static int GAMEPAD_LOCKOUT = 500; + + RevBlinkinLedDriver blinkinLedDriver; + RevBlinkinLedDriver.BlinkinPattern pattern; + + Telemetry.Item patternName; + Telemetry.Item display; + DisplayKind displayKind; + Deadline ledCycleDeadline; + Deadline gamepadRateLimit; + + protected enum DisplayKind { + MANUAL, + AUTO + } + + @Override + public void init() + { + displayKind = DisplayKind.AUTO; + + blinkinLedDriver = hardwareMap.get(RevBlinkinLedDriver.class, "blinkin"); + pattern = RevBlinkinLedDriver.BlinkinPattern.RAINBOW_RAINBOW_PALETTE; + blinkinLedDriver.setPattern(pattern); + + display = telemetry.addData("Display Kind: ", displayKind.toString()); + patternName = telemetry.addData("Pattern: ", pattern.toString()); + + ledCycleDeadline = new Deadline(LED_PERIOD, TimeUnit.SECONDS); + gamepadRateLimit = new Deadline(GAMEPAD_LOCKOUT, TimeUnit.MILLISECONDS); + } + + @Override + public void loop() + { + handleGamepad(); + + if (displayKind == DisplayKind.AUTO) { + doAutoDisplay(); + } else { + /* + * MANUAL mode: Nothing to do, setting the pattern as a result of a gamepad event. + */ + } + } + + /* + * handleGamepad + * + * Responds to a gamepad button press. Demonstrates rate limiting for + * button presses. If loop() is called every 10ms and and you don't rate + * limit, then any given button press may register as multiple button presses, + * which in this application is problematic. + * + * A: Manual mode, Right bumper displays the next pattern, left bumper displays the previous pattern. + * B: Auto mode, pattern cycles, changing every LED_PERIOD seconds. + */ + protected void handleGamepad() + { + if (!gamepadRateLimit.hasExpired()) { + return; + } + + if (gamepad1.a) { + setDisplayKind(DisplayKind.MANUAL); + gamepadRateLimit.reset(); + } else if (gamepad1.b) { + setDisplayKind(DisplayKind.AUTO); + gamepadRateLimit.reset(); + } else if ((displayKind == DisplayKind.MANUAL) && (gamepad1.left_bumper)) { + pattern = pattern.previous(); + displayPattern(); + gamepadRateLimit.reset(); + } else if ((displayKind == DisplayKind.MANUAL) && (gamepad1.right_bumper)) { + pattern = pattern.next(); + displayPattern(); + gamepadRateLimit.reset(); + } + } + + protected void setDisplayKind(DisplayKind displayKind) + { + this.displayKind = displayKind; + display.setValue(displayKind.toString()); + } + + protected void doAutoDisplay() + { + if (ledCycleDeadline.hasExpired()) { + pattern = pattern.next(); + displayPattern(); + ledCycleDeadline.reset(); + } + } + + protected void displayPattern() + { + blinkinLedDriver.setPattern(pattern); + patternName.setValue(pattern.toString()); + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorAndyMarkIMUNonOrthogonal.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorAndyMarkIMUNonOrthogonal.java new file mode 100644 index 0000000..cd678ef --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorAndyMarkIMUNonOrthogonal.java @@ -0,0 +1,193 @@ +/* Copyright (c) 2025 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.hardware.andymark.AndyMarkIMUOrientationOnRobot; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.IMU; +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.AngularVelocity; +import org.firstinspires.ftc.robotcore.external.navigation.Orientation; +import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles; + +import static com.qualcomm.hardware.andymark.AndyMarkIMUOrientationOnRobot.xyzOrientation; + +/* + * This OpMode shows how to use the AndyMark IMU sensor. It assumes that the AndyMark IMU is + * configured with the name "imu". + * + * The sample will display the current Yaw, Pitch and Roll of the robot. + * + * With the correct orientation parameters selected, pitch/roll/yaw should act as follows: + * Pitch value should INCREASE as the robot is tipped UP at the front. (Rotation about X) + * Roll value should INCREASE as the robot is tipped UP at the left side. (Rotation about Y) + * Yaw value should INCREASE as the robot is rotated Counter Clockwise. (Rotation about Z) + * + * The yaw can be reset (to zero) by pressing the Y button on the gamepad (Triangle on a PS4 controller). + * + * This specific sample DOES NOT assume that the AndyMark IMU is mounted on one of the three + * orthogonal planes (X/Y, X/Z or Y/Z) OR that the AndyMark IMU has only been rotated in a range of + * 90 degree increments. + * + * Note: if your AndyMark IMU is mounted Orthogonally (on a orthogonal surface, angled at some + * multiple of 90 Degrees), then you should use the simpler SensorAndyMarkIMUOrthogonal sample in + * this folder. + * + * But... If your AndyMark IMU is mounted Non-Orthogonally, you must specify one or more rotational + * angles that transform a "Default" IMU orientation into your desired orientation. That is what is + * illustrated here. + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list. + * + * Finally, edit this OpMode to use at least one angle around an axis to orient your AndyMark IMU. + */ +@TeleOp(name = "Sensor: AndyMark IMU Non-Orthogonal", group = "Sensor") +@Disabled // Comment this out to add to the OpMode list +public class SensorAndyMarkIMUNonOrthogonal extends LinearOpMode +{ + // The AndyMark IMU sensor object + private IMU imu; + + //---------------------------------------------------------------------------------------------- + // Main logic + //---------------------------------------------------------------------------------------------- + + @Override public void runOpMode() throws InterruptedException { + + // Retrieve and initialize the AndyMark IMU. + // This sample expects the AndyMark IMU to be named "imu". + imu = hardwareMap.get(IMU.class, "imu"); + + /* Define how the AndyMark IMU is mounted to the robot to get the correct Yaw, Pitch, and + * Roll values. + * + * You can apply up to three axis rotations to orient your AndyMark IMU according to how + * it's mounted on the robot. + * + * The starting point for these rotations is the "Default" IMU orientation, which is: + * 1) IMU laying flat on a horizontal surface, with the AndyMark logo facing up. + * 2) Rotated such that the I2C port is facing forward on the robot. + * + * The order that the rotations are performed matters, so this sample shows doing them in + * the order X, Y, then Z. + * For specifying non-orthogonal IMU mounting orientations, we must temporarily use axes + * defined relative to the AndyMark IMU itself, instead of the usual Robot Coordinate System + * axes used for the results the AndyMark IMU gives us. In the starting orientation, the + * AndyMark IMU axes are aligned with the Robot Coordinate System: + * + * X Axis: Starting at center of IMU, pointing out towards the side. + * Y Axis: Starting at center of IMU, pointing out towards I2C port. + * Z Axis: Starting at center of IMU, pointing up through the AndyMark logo. + * + * Positive rotation is defined by right-hand rule with thumb pointing in +ve direction on + * axis. + * + * Some examples. + * + * ---------------------------------------------------------------------------------------------------------------------------------- + * Example A) Assume that the AndyMark IMU is mounted on a sloped plate at the back of the + * robot, with the I2C port coming out the top of the AndyMark IMU. The plate is tilted UP + * 60 degrees from horizontal. + * + * To get the "Default" IMU into this configuration you would just need a single rotation. + * 1) Rotate the AndyMark IMU +60 degrees around the X axis to tilt up the front edge. + * 2) No rotation around the Y or Z axes. + * + * So the X,Y,Z rotations would be 60,0,0 + * + * ---------------------------------------------------------------------------------------------------------------------------------- + * Example B) Assume that the AndyMark IMU is laying flat on the chassis, but it has been + * twisted 30 degrees towards the right front wheel. + * + * To get the "Default" IMU into this configuration you would just need a single rotation, + * but around a different axis. + * 1) No rotation around the X or Y axes. + * 2) Rotate the AndyMark IMU -30 degrees (Clockwise) around the Z axis, since a positive + * angle would be Counter Clockwise. + * + * So the X,Y,Z rotations would be 0,0,-30 + * + * ---------------------------------------------------------------------------------------------------------------------------------- + * Example C) Assume that the AndyMark IMU is mounted on a vertical plate on the right side + * of the robot, with the AndyMark logo facing out, and the AndyMark IMU rotated so that the + * I2C port is facing down 30 degrees towards the back wheels of the robot. + * + * To get the "Default" IMU into this configuration will require several rotations. + * 1) Rotate the AndyMark IMU +90 degrees around the X axis to get it standing upright with + * the logo pointing backwards on the robot + * 2) Next, rotate the AndyMark IMU +90 around the Y axis to get the logo facing to the + * right. + * 3) Finally rotate the AndyMark IMU +120 degrees around the Z axis to take the I2C port + * from vertical to sloping down 30 degrees and facing towards the back of the robot. + * + * So the X,Y,Z rotations would be 90,90,120 + */ + + // The next three lines define the desired axis rotations. + // To Do: EDIT these values to match YOUR mounting configuration. + double xRotation = 0; // enter the desired X rotation angle here. + double yRotation = 0; // enter the desired Y rotation angle here. + double zRotation = 0; // enter the desired Z rotation angle here. + + Orientation imuRotation = xyzOrientation(xRotation, yRotation, zRotation); + + // Now initialize the AndyMark IMU with this mounting orientation. + AndyMarkIMUOrientationOnRobot orientationOnRobot = new AndyMarkIMUOrientationOnRobot(imuRotation); + imu.initialize(new IMU.Parameters(orientationOnRobot)); + + // Loop and update the dashboard. + while (!isStopRequested()) { + telemetry.addData("IMU orientation", "X=%.1f, Y=%.1f, Z=%.1f \n", xRotation, yRotation, zRotation); + + // Check to see if heading reset is requested. + if (gamepad1.y) { + telemetry.addData("Yaw", "Resetting\n"); + imu.resetYaw(); + } else { + telemetry.addData("Yaw", "Press Y (triangle) on Gamepad to reset\n"); + } + + // Retrieve rotational angles and velocities. + YawPitchRollAngles orientation = imu.getRobotYawPitchRollAngles(); + AngularVelocity angularVelocity = imu.getRobotAngularVelocity(AngleUnit.DEGREES); + + telemetry.addData("Yaw (Z)", "%.2f Deg. (Heading)", orientation.getYaw(AngleUnit.DEGREES)); + telemetry.addData("Pitch (X)", "%.2f Deg.", orientation.getPitch(AngleUnit.DEGREES)); + telemetry.addData("Roll (Y)", "%.2f Deg.\n", orientation.getRoll(AngleUnit.DEGREES)); + telemetry.addData("Yaw (Z) velocity", "%.2f Deg/Sec", angularVelocity.zRotationRate); + telemetry.addData("Pitch (X) velocity", "%.2f Deg/Sec", angularVelocity.xRotationRate); + telemetry.addData("Roll (Y) velocity", "%.2f Deg/Sec", angularVelocity.yRotationRate); + telemetry.update(); + } + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorAndyMarkIMUOrthogonal.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorAndyMarkIMUOrthogonal.java new file mode 100644 index 0000000..58cee6d --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorAndyMarkIMUOrthogonal.java @@ -0,0 +1,144 @@ +/* Copyright (c) 2025 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.hardware.andymark.AndyMarkIMUOrientationOnRobot; +import com.qualcomm.hardware.andymark.AndyMarkIMUOrientationOnRobot.I2cPortFacingDirection; +import com.qualcomm.hardware.andymark.AndyMarkIMUOrientationOnRobot.LogoFacingDirection; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.IMU; +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.AngularVelocity; +import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles; + +/* + * This OpMode shows how to use the AndyMark IMU sensor. It assumes that the AndyMark IMU is + * configured with the name "imu". + * + * The sample will display the current Yaw, Pitch and Roll of the robot. + * + * With the correct orientation parameters selected, pitch/roll/yaw should act as follows: + * Pitch value should INCREASE as the robot is tipped UP at the front. (Rotation about X) + * Roll value should INCREASE as the robot is tipped UP at the left side. (Rotation about Y) + * Yaw value should INCREASE as the robot is rotated Counter Clockwise. (Rotation about Z) + * + * The yaw can be reset (to zero) by pressing the Y button on the gamepad (Triangle on a PS4 controller). + * + * This specific sample assumes that the AndyMark IMU is mounted on one of the three orthogonal + * planes (X/Y, X/Z or Y/Z) and that the AndyMark IMU has only been rotated in a range of 90 degree + * increments. + * + * Note: if your AndyMark IMU is mounted on a surface angled at some non-90 Degree multiple (like + * 30), then you should use the SensorAndyMarkIMUNonOrthogonal sample in this folder. + * + * This "Orthogonal" requirement means that: + * + * 1) The AndyMark logo printed on the top of the AndyMark IMU can ONLY be pointing in one of six directions: + * FORWARD, BACKWARD, UP, DOWN, LEFT and RIGHT. + * + * 2) The I2C port can only be pointing in one of the same six directions: + * FORWARD, BACKWARD, UP, DOWN, LEFT and RIGHT. + * + * So, to fully define how your AndyMark IMU is mounted to the robot, you must simply specify: + * LogoFacingDirection + * I2cPortFacingDirection + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list. + * + * Finally, choose the two correct parameters to define how your AndyMark IMU is mounted and edit + * this OpMode to use those parameters. + */ +@TeleOp(name = "Sensor: AndyMark IMU Orthogonal", group = "Sensor") +@Disabled // Comment this out to add to the OpMode list +public class SensorAndyMarkIMUOrthogonal extends LinearOpMode +{ + // The AndyMark IMU sensor object + private IMU imu; + + //---------------------------------------------------------------------------------------------- + // Main logic + //---------------------------------------------------------------------------------------------- + + @Override public void runOpMode() throws InterruptedException { + + // Retrieve and initialize the AndyMark IMU. + // This sample expects the AndyMark IMU to be named "imu". + imu = hardwareMap.get(IMU.class, "imu"); + + /* Define how the AndyMark IMU is mounted to the robot to get the correct Yaw, Pitch, and + * Roll values. + * + * Two input parameters are required to fully specify the Orientation. + * The first parameter specifies the direction the AndyMark logo on the IMU is pointing. + * The second parameter specifies the direction the I2C port on the IMU is pointing. + * All directions are relative to the robot, and left/right is as-viewed from behind the robot. + */ + + /* The next two lines define the IMU orientation. + * To Do: EDIT these two lines to match YOUR mounting configuration. + */ + LogoFacingDirection logoDirection = LogoFacingDirection.UP; + I2cPortFacingDirection i2cDirection = I2cPortFacingDirection.FORWARD; + + AndyMarkIMUOrientationOnRobot orientationOnRobot = new AndyMarkIMUOrientationOnRobot(logoDirection, i2cDirection); + + // Now initialize the AndyMark IMU with this mounting orientation. + // Note: if you choose two conflicting directions, this initialization will cause a code exception. + imu.initialize(new IMU.Parameters(orientationOnRobot)); + + // Loop and update the dashboard. + while (!isStopRequested()) { + telemetry.addData("IMU orientation", "Logo=%s I2C=%s\n ", logoDirection, i2cDirection); + + // Check to see if heading reset is requested. + if (gamepad1.y) { + telemetry.addData("Yaw", "Resetting\n"); + imu.resetYaw(); + } else { + telemetry.addData("Yaw", "Press Y (triangle) on Gamepad to reset\n"); + } + + // Retrieve rotational angles and velocities. + YawPitchRollAngles orientation = imu.getRobotYawPitchRollAngles(); + AngularVelocity angularVelocity = imu.getRobotAngularVelocity(AngleUnit.DEGREES); + + telemetry.addData("Yaw (Z)", "%.2f Deg. (Heading)", orientation.getYaw(AngleUnit.DEGREES)); + telemetry.addData("Pitch (X)", "%.2f Deg.", orientation.getPitch(AngleUnit.DEGREES)); + telemetry.addData("Roll (Y)", "%.2f Deg.\n", orientation.getRoll(AngleUnit.DEGREES)); + telemetry.addData("Yaw (Z) velocity", "%.2f Deg/Sec", angularVelocity.zRotationRate); + telemetry.addData("Pitch (X) velocity", "%.2f Deg/Sec", angularVelocity.xRotationRate); + telemetry.addData("Roll (Y) velocity", "%.2f Deg/Sec", angularVelocity.yRotationRate); + telemetry.update(); + } + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorAndyMarkTOF.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorAndyMarkTOF.java new file mode 100644 index 0000000..ce6e943 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorAndyMarkTOF.java @@ -0,0 +1,85 @@ +/* +Copyright (c) 2025 FIRST + +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, +are permitted (subject to the limitations in the disclaimer below) provided that +the following conditions are met: + +Redistributions of source code must retain the above copyright notice, this list +of conditions and the following disclaimer. + +Redistributions in binary form must reproduce the above copyright notice, this +list of conditions and the following disclaimer in the documentation and/or +other materials provided with the distribution. + +Neither the name of FIRST nor the names of its contributors may be used to +endorse or promote products derived from this software without specific prior +written permission. + +NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS +LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF +THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.hardware.andymark.AndyMarkTOF; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DistanceSensor; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; + +/* + * This OpMode illustrates how to use the AndyMark 2m TOF Lidar Sensor. + * + * The OpMode assumes that the sensor is configured with a name of "sensor_distance". + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list + */ +@TeleOp(name = "Sensor: AndyMarkTOF", group = "Sensor") +@Disabled +public class SensorAndyMarkTOF extends LinearOpMode { + + private DistanceSensor sensorDistance; + + @Override + public void runOpMode() { + // you can use this as a regular DistanceSensor. + sensorDistance = hardwareMap.get(DistanceSensor.class, "sensor_distance"); + + // you can also cast this to a AndyMarkTOF if you want to use added + // methods associated with the AndyMarkTOF class. + AndyMarkTOF sensorTimeOfFlight = (AndyMarkTOF) sensorDistance; + + telemetry.addData(">>", "Press start to continue"); + telemetry.update(); + + waitForStart(); + while(opModeIsActive()) { + // generic DistanceSensor methods. + telemetry.addData("deviceName", sensorDistance.getDeviceName() ); + telemetry.addData("range", String.format("%.01f mm", sensorDistance.getDistance(DistanceUnit.MM))); + telemetry.addData("range", String.format("%.01f cm", sensorDistance.getDistance(DistanceUnit.CM))); + telemetry.addData("range", String.format("%.01f m", sensorDistance.getDistance(DistanceUnit.METER))); + telemetry.addData("range", String.format("%.01f in", sensorDistance.getDistance(DistanceUnit.INCH))); + + // AndyMarkTOF specific methods. + telemetry.addData("ID", String.format("%x", sensorTimeOfFlight.getModelID())); + telemetry.addData("did time out", Boolean.toString(sensorTimeOfFlight.didTimeoutOccur())); + + telemetry.update(); + } + } + +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorBNO055IMU.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorBNO055IMU.java new file mode 100644 index 0000000..405da1e --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorBNO055IMU.java @@ -0,0 +1,186 @@ +/* Copyright (c) 2017 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.hardware.bosch.BNO055IMU; +import com.qualcomm.hardware.bosch.JustLoggingAccelerationIntegrator; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import org.firstinspires.ftc.robotcore.external.Func; +import org.firstinspires.ftc.robotcore.external.navigation.Acceleration; +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder; +import org.firstinspires.ftc.robotcore.external.navigation.AxesReference; +import org.firstinspires.ftc.robotcore.external.navigation.Orientation; +import org.firstinspires.ftc.robotcore.external.navigation.Position; +import org.firstinspires.ftc.robotcore.external.navigation.Velocity; + +import java.util.Locale; + +/* + * This OpMode gives a short demo on how to use the BNO055 Inertial Motion Unit (IMU) from AdaFruit. + * + * Note: this is a Legacy example that will not work with newer Control/Expansion Hubs that use a different IMU + * Please use the new SensorIMUOrthogonal or SensorIMUNonOrthogonal samples for a more universal IMU interface. + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list + * + * @see Adafruit IMU + */ +@TeleOp(name = "Sensor: BNO055 IMU", group = "Sensor") +@Disabled // Comment this out to add to the OpMode list +public class SensorBNO055IMU extends LinearOpMode + { + //---------------------------------------------------------------------------------------------- + // State + //---------------------------------------------------------------------------------------------- + + // The IMU sensor object + BNO055IMU imu; + + // State used for updating telemetry + Orientation angles; + Acceleration gravity; + + //---------------------------------------------------------------------------------------------- + // Main logic + //---------------------------------------------------------------------------------------------- + + @Override public void runOpMode() { + + // Set up the parameters with which we will use our IMU. Note that integration + // algorithm here just reports accelerations to the logcat log; it doesn't actually + // provide positional information. + BNO055IMU.Parameters parameters = new BNO055IMU.Parameters(); + parameters.angleUnit = BNO055IMU.AngleUnit.DEGREES; + parameters.accelUnit = BNO055IMU.AccelUnit.METERS_PERSEC_PERSEC; + parameters.calibrationDataFile = "BNO055IMUCalibration.json"; // see the calibration sample OpMode + parameters.loggingEnabled = true; + parameters.loggingTag = "IMU"; + parameters.accelerationIntegrationAlgorithm = new JustLoggingAccelerationIntegrator(); + + // Retrieve and initialize the IMU. We expect the IMU to be attached to an I2C port + // on a Core Device Interface Module, configured to be a sensor of type "AdaFruit IMU", + // and named "imu". + imu = hardwareMap.get(BNO055IMU.class, "imu"); + imu.initialize(parameters); + + // Set up our telemetry dashboard + composeTelemetry(); + + // Wait until we're told to go + waitForStart(); + + // Start the logging of measured acceleration + imu.startAccelerationIntegration(new Position(), new Velocity(), 1000); + + // Loop and update the dashboard + while (opModeIsActive()) { + telemetry.update(); + } + } + + //---------------------------------------------------------------------------------------------- + // Telemetry Configuration + //---------------------------------------------------------------------------------------------- + + void composeTelemetry() { + + // At the beginning of each telemetry update, grab a bunch of data + // from the IMU that we will then display in separate lines. + telemetry.addAction(new Runnable() { @Override public void run() + { + // Acquiring the angles is relatively expensive; we don't want + // to do that in each of the three items that need that info, as that's + // three times the necessary expense. + angles = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES); + gravity = imu.getGravity(); + } + }); + + telemetry.addLine() + .addData("status", new Func() { + @Override public String value() { + return imu.getSystemStatus().toShortString(); + } + }) + .addData("calib", new Func() { + @Override public String value() { + return imu.getCalibrationStatus().toString(); + } + }); + + telemetry.addLine() + .addData("heading", new Func() { + @Override public String value() { + return formatAngle(angles.angleUnit, angles.firstAngle); + } + }) + .addData("roll", new Func() { + @Override public String value() { + return formatAngle(angles.angleUnit, angles.secondAngle); + } + }) + .addData("pitch", new Func() { + @Override public String value() { + return formatAngle(angles.angleUnit, angles.thirdAngle); + } + }); + + telemetry.addLine() + .addData("grvty", new Func() { + @Override public String value() { + return gravity.toString(); + } + }) + .addData("mag", new Func() { + @Override public String value() { + return String.format(Locale.getDefault(), "%.3f", + Math.sqrt(gravity.xAccel*gravity.xAccel + + gravity.yAccel*gravity.yAccel + + gravity.zAccel*gravity.zAccel)); + } + }); + } + + //---------------------------------------------------------------------------------------------- + // Formatting + //---------------------------------------------------------------------------------------------- + + String formatAngle(AngleUnit angleUnit, double angle) { + return formatDegrees(AngleUnit.DEGREES.fromUnit(angleUnit, angle)); + } + + String formatDegrees(double degrees){ + return String.format(Locale.getDefault(), "%.1f", AngleUnit.DEGREES.normalize(degrees)); + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorBNO055IMUCalibration.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorBNO055IMUCalibration.java new file mode 100644 index 0000000..93f1789 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorBNO055IMUCalibration.java @@ -0,0 +1,230 @@ +/* Copyright (c) 2017 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.hardware.bosch.BNO055IMU; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.util.ReadWriteFile; +import org.firstinspires.ftc.robotcore.external.Func; +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder; +import org.firstinspires.ftc.robotcore.external.navigation.AxesReference; +import org.firstinspires.ftc.robotcore.external.navigation.Orientation; +import org.firstinspires.ftc.robotcore.internal.system.AppUtil; + +import java.io.File; +import java.util.Locale; + +/* + * This OpMode calibrates a BNO055 IMU per + * "Section 3.11 Calibration" of the BNO055 specification. + * + * Note: this is a Legacy example that will not work with newer Control/Expansion Hubs that use a different IMU + * Please use the new SensorIMUOrthogonal or SensorIMUNonOrthogonal samples for a more universal IMU interface. + * + * Manual calibration of the IMU is definitely NOT necessary: except for the magnetometer (which is not used by the + * default "IMU" SensorMode), the BNO055 is internally self-calibrating and thus can be very successfully used without + * manual intervention. That said, performing a one-time calibration, saving the results persistently, then loading them + * again at each run can help reduce the time that automatic calibration requires. + * + * This summary of the calibration process from Intel is informative: + * http://iotdk.intel.com/docs/master/upm/classupm_1_1_b_n_o055.html + * + * "This device requires calibration in order to operate accurately. [...] Calibration data is + * lost on a power cycle. See one of the examples for a description of how to calibrate the device, + * but in essence: + * + * There is a calibration status register available [...] that returns the calibration status + * of the accelerometer (ACC), magnetometer (MAG), gyroscope (GYR), and overall system (SYS). + * Each of these values range from 0 (uncalibrated) to 3 (fully calibrated). Calibration [ideally] + * involves certain motions to get all 4 values at 3. The motions are as follows (though see the + * datasheet for more information): + * + * 1. GYR: Simply let the sensor sit flat for a few seconds. + * 2. ACC: Move the sensor in various positions. Start flat, then rotate slowly by 45 + * degrees, hold for a few seconds, then continue rotating another 45 degrees and + * hold, etc. 6 or more movements of this type may be required. You can move through + * any axis you desire, but make sure that the device is lying at least once + * perpendicular to the x, y, and z axis. + * 3. MAG: Move slowly in a figure 8 pattern in the air, until the calibration values reaches 3. + * 4. SYS: This will usually reach 3 when the other items have also reached 3. If not, continue + * slowly moving the device though various axes until it does." + * + * To calibrate the IMU, run this sample OpMode with a gamepad attached to the driver station. + * Once the IMU has reached sufficient calibration as reported on telemetry, press the 'A' + * button on the gamepad to write the calibration to a file. That file can then be indicated + * later when running an OpMode which uses the IMU. + * + * Note: if your intended uses of the IMU do not include use of all its sensors (for example, + * you might not use the magnetometer), then it makes little sense for you to wait for full + * calibration of the sensors you are not using before saving the calibration data. Indeed, + * it appears that in a SensorMode that doesn't use the magnetometer (for example), the + * magnetometer cannot actually be calibrated. + * + * References: + * The AdafruitBNO055IMU Javadoc + * The BNO055IMU.Parameters.calibrationDataFile Javadoc + * The BNO055 product page: https://www.bosch-sensortec.com/bst/products/all_products/bno055 + * The BNO055 datasheet: https://www.bosch-sensortec.com/media/boschsensortec/downloads/datasheets/bst-bno055-ds000.pdf + */ +@TeleOp(name = "Sensor: BNO055 IMU Calibration", group = "Sensor") +@Disabled // Uncomment this to add to the OpMode list +public class SensorBNO055IMUCalibration extends LinearOpMode + { + //---------------------------------------------------------------------------------------------- + // State + //---------------------------------------------------------------------------------------------- + + // Our sensors, motors, and other devices go here, along with other long term state + BNO055IMU imu; + + // State used for updating telemetry + Orientation angles; + + //---------------------------------------------------------------------------------------------- + // Main logic + //---------------------------------------------------------------------------------------------- + + @Override public void runOpMode() { + + telemetry.log().setCapacity(12); + telemetry.log().add(""); + telemetry.log().add("Please refer to the calibration instructions"); + telemetry.log().add("contained in the Adafruit IMU calibration"); + telemetry.log().add("sample OpMode."); + telemetry.log().add(""); + telemetry.log().add("When sufficient calibration has been reached,"); + telemetry.log().add("press the 'A' button to write the current"); + telemetry.log().add("calibration data to a file."); + telemetry.log().add(""); + + // We are expecting the IMU to be attached to an I2C port on a Core Device Interface Module and named "imu". + BNO055IMU.Parameters parameters = new BNO055IMU.Parameters(); + parameters.loggingEnabled = true; + parameters.loggingTag = "IMU"; + imu = hardwareMap.get(BNO055IMU.class, "imu"); + imu.initialize(parameters); + + composeTelemetry(); + telemetry.log().add("Waiting for start..."); + + // Wait until we're told to go + while (!isStarted()) { + telemetry.update(); + idle(); + } + + telemetry.log().add("...started..."); + + while (opModeIsActive()) { + + if (gamepad1.a) { + + // Get the calibration data + BNO055IMU.CalibrationData calibrationData = imu.readCalibrationData(); + + // Save the calibration data to a file. You can choose whatever file + // name you wish here, but you'll want to indicate the same file name + // when you initialize the IMU in an OpMode in which it is used. If you + // have more than one IMU on your robot, you'll of course want to use + // different configuration file names for each. + String filename = "AdafruitIMUCalibration.json"; + File file = AppUtil.getInstance().getSettingsFile(filename); + ReadWriteFile.writeFile(file, calibrationData.serialize()); + telemetry.log().add("saved to '%s'", filename); + + // Wait for the button to be released + while (gamepad1.a) { + telemetry.update(); + idle(); + } + } + + telemetry.update(); + } + } + + void composeTelemetry() { + + // At the beginning of each telemetry update, grab a bunch of data + // from the IMU that we will then display in separate lines. + telemetry.addAction(new Runnable() { @Override public void run() + { + // Acquiring the angles is relatively expensive; we don't want + // to do that in each of the three items that need that info, as that's + // three times the necessary expense. + angles = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES); + } + }); + + telemetry.addLine() + .addData("status", new Func() { + @Override public String value() { + return imu.getSystemStatus().toShortString(); + } + }) + .addData("calib", new Func() { + @Override public String value() { + return imu.getCalibrationStatus().toString(); + } + }); + + telemetry.addLine() + .addData("heading", new Func() { + @Override public String value() { + return formatAngle(angles.angleUnit, angles.firstAngle); + } + }) + .addData("roll", new Func() { + @Override public String value() { + return formatAngle(angles.angleUnit, angles.secondAngle); + } + }) + .addData("pitch", new Func() { + @Override public String value() { + return formatAngle(angles.angleUnit, angles.thirdAngle); + } + }); + } + + //---------------------------------------------------------------------------------------------- + // Formatting + //---------------------------------------------------------------------------------------------- + + String formatAngle(AngleUnit angleUnit, double angle) { + return formatDegrees(AngleUnit.DEGREES.fromUnit(angleUnit, angle)); + } + + String formatDegrees(double degrees){ + return String.format(Locale.getDefault(), "%.1f", AngleUnit.DEGREES.normalize(degrees)); + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorColor.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorColor.java new file mode 100644 index 0000000..d0411af --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorColor.java @@ -0,0 +1,225 @@ +/* Copyright (c) 2017-2020 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import android.app.Activity; +import android.graphics.Color; +import android.view.View; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DistanceSensor; +import com.qualcomm.robotcore.hardware.NormalizedColorSensor; +import com.qualcomm.robotcore.hardware.NormalizedRGBA; +import com.qualcomm.robotcore.hardware.SwitchableLight; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; + +/* + * This OpMode shows how to use a color sensor in a generic + * way, regardless of which particular make or model of color sensor is used. The OpMode + * assumes that the color sensor is configured with a name of "sensor_color". + * + * There will be some variation in the values measured depending on the specific sensor you are using. + * + * If the color sensor supports adjusting the gain, you can increase the gain (a multiplier to make + * the sensor report higher values) by holding down the A button on the gamepad, and decrease the + * gain by holding down the B button on the gamepad. The AndyMark Proximity & Color Sensor does not + * support this. + * + * If the color sensor has a light which is controllable from software, you can use the X button on + * the gamepad to toggle the light on and off. The REV sensors don't support this, but instead have + * a physical switch on them to turn the light on and off, beginning with REV Color Sensor V2. The + * AndyMark Proximity & Color Sensor does not support this. + * + * If the color sensor also supports short-range distance measurements (usually via an infrared + * proximity sensor), the reported distance will be written to telemetry. As of September 2025, + * the only color sensors that support this are the ones from REV Robotics and the AndyMark + * Proximity & Color Sensor. These infrared proximity sensor measurements are only useful at very + * small distances, and are sensitive to ambient light and surface reflectivity. You should use a + * different sensor if you need precise distance measurements. + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list + */ +@TeleOp(name = "Sensor: Color", group = "Sensor") +@Disabled +public class SensorColor extends LinearOpMode { + + /** The colorSensor field will contain a reference to our color sensor hardware object */ + NormalizedColorSensor colorSensor; + + /** The relativeLayout field is used to aid in providing interesting visual feedback + * in this sample application; you probably *don't* need this when you use a color sensor on your + * robot. Note that you won't see anything change on the Driver Station, only on the Robot Controller. */ + View relativeLayout; + + /* + * The runOpMode() method is the root of this OpMode, as it is in all LinearOpModes. + * Our implementation here, though is a bit unusual: we've decided to put all the actual work + * in the runSample() method rather than directly in runOpMode() itself. The reason we do that is + * that in this sample we're changing the background color of the robot controller screen as the + * OpMode runs, and we want to be able to *guarantee* that we restore it to something reasonable + * and palatable when the OpMode ends. The simplest way to do that is to use a try...finally + * block around the main, core logic, and an easy way to make that all clear was to separate + * the former from the latter in separate methods. + */ + @Override public void runOpMode() { + + // Get a reference to the RelativeLayout so we can later change the background + // color of the Robot Controller app to match the hue detected by the RGB sensor. + int relativeLayoutId = hardwareMap.appContext.getResources().getIdentifier("RelativeLayout", "id", hardwareMap.appContext.getPackageName()); + relativeLayout = ((Activity) hardwareMap.appContext).findViewById(relativeLayoutId); + + try { + runSample(); // actually execute the sample + } finally { + // On the way out, *guarantee* that the background is reasonable. It doesn't actually start off + // as pure white, but it's too much work to dig out what actually was used, and this is good + // enough to at least make the screen reasonable again. + // Set the panel back to the default color + relativeLayout.post(new Runnable() { + public void run() { + relativeLayout.setBackgroundColor(Color.WHITE); + } + }); + } + } + + protected void runSample() { + // You can give the sensor a gain value, will be multiplied by the sensor's raw value before the + // normalized color values are calculated. Color sensors (especially the REV Color Sensor V3) + // can give very low values (depending on the lighting conditions), which only use a small part + // of the 0-1 range that is available for the red, green, and blue values. In brighter conditions, + // you should use a smaller gain than in dark conditions. If your gain is too high, all of the + // colors will report at or near 1, and you won't be able to determine what color you are + // actually looking at. For this reason, it's better to err on the side of a lower gain + // (but always greater than or equal to 1). + float gain = 2; + + // Once per loop, we will update this hsvValues array. The first element (0) will contain the + // hue, the second element (1) will contain the saturation, and the third element (2) will + // contain the value. See http://web.archive.org/web/20190311170843/https://infohost.nmt.edu/tcc/help/pubs/colortheory/web/hsv.html + // for an explanation of HSV color. + final float[] hsvValues = new float[3]; + + // xButtonPreviouslyPressed and xButtonCurrentlyPressed keep track of the previous and current + // state of the X button on the gamepad + boolean xButtonPreviouslyPressed = false; + boolean xButtonCurrentlyPressed = false; + + // Get a reference to our sensor object. It's recommended to use NormalizedColorSensor over + // ColorSensor, because NormalizedColorSensor consistently gives values between 0 and 1, while + // the values you get from ColorSensor are dependent on the specific sensor you're using. + colorSensor = hardwareMap.get(NormalizedColorSensor.class, "sensor_color"); + + // If possible, turn the light on in the beginning (it might already be on anyway, + // we just make sure it is if we can). + if (colorSensor instanceof SwitchableLight) { + ((SwitchableLight)colorSensor).enableLight(true); + } + + // Wait for the start button to be pressed. + waitForStart(); + + // Loop until we are asked to stop + while (opModeIsActive()) { + // Explain basic gain information via telemetry + telemetry.addLine("Hold the A button on gamepad 1 to increase gain, or B to decrease it.\n"); + telemetry.addLine("Higher gain values mean that the sensor will report larger numbers for Red, Green, and Blue, and Value\n"); + + // Update the gain value if either of the A or B gamepad buttons is being held + if (gamepad1.a) { + // Only increase the gain by a small amount, since this loop will occur multiple times per second. + gain += 0.005; + } else if (gamepad1.b && gain > 1) { // A gain of less than 1 will make the values smaller, which is not helpful. + gain -= 0.005; + } + + // Show the gain value via telemetry + telemetry.addData("Gain", gain); + + // Tell the sensor our desired gain value (normally you would do this during initialization, + // not during the loop) + colorSensor.setGain(gain); + + // Check the status of the X button on the gamepad + xButtonCurrentlyPressed = gamepad1.x; + + // If the button state is different than what it was, then act + if (xButtonCurrentlyPressed != xButtonPreviouslyPressed) { + // If the button is (now) down, then toggle the light + if (xButtonCurrentlyPressed) { + if (colorSensor instanceof SwitchableLight) { + SwitchableLight light = (SwitchableLight)colorSensor; + light.enableLight(!light.isLightOn()); + } + } + } + xButtonPreviouslyPressed = xButtonCurrentlyPressed; + + // Get the normalized colors from the sensor + NormalizedRGBA colors = colorSensor.getNormalizedColors(); + + /* Use telemetry to display feedback on the driver station. We show the red, green, and blue + * normalized values from the sensor (in the range of 0 to 1), as well as the equivalent + * HSV (hue, saturation and value) values. See http://web.archive.org/web/20190311170843/https://infohost.nmt.edu/tcc/help/pubs/colortheory/web/hsv.html + * for an explanation of HSV color. */ + + // Update the hsvValues array by passing it to Color.colorToHSV() + Color.colorToHSV(colors.toColor(), hsvValues); + + telemetry.addLine() + .addData("Red", "%.3f", colors.red) + .addData("Green", "%.3f", colors.green) + .addData("Blue", "%.3f", colors.blue); + telemetry.addLine() + .addData("Hue", "%.3f", hsvValues[0]) + .addData("Saturation", "%.3f", hsvValues[1]) + .addData("Value", "%.3f", hsvValues[2]); + telemetry.addData("Alpha", "%.3f", colors.alpha); + + /* If this color sensor also has a distance sensor, display the measured distance. + * Note that the reported distance is only useful at very close range, and is impacted by + * ambient light and surface reflectivity. */ + if (colorSensor instanceof DistanceSensor) { + telemetry.addData("Distance (cm)", "%.3f", ((DistanceSensor) colorSensor).getDistance(DistanceUnit.CM)); + } + + telemetry.update(); + + // Change the Robot Controller's background color to match the color detected by the color sensor. + relativeLayout.post(new Runnable() { + public void run() { + relativeLayout.setBackgroundColor(Color.HSVToColor(hsvValues)); + } + }); + } + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorDigitalTouch.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorDigitalTouch.java new file mode 100644 index 0000000..44c3ca9 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorDigitalTouch.java @@ -0,0 +1,78 @@ +/* Copyright (c) 2024 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DigitalChannel; + +/* + * This OpMode demonstrates how to use a digital channel. + * + * The OpMode assumes that the digital channel is configured with a name of "digitalTouch". + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list. + */ +@TeleOp(name = "Sensor: digital channel", group = "Sensor") +@Disabled +public class SensorDigitalTouch extends LinearOpMode { + DigitalChannel digitalTouch; // Digital channel Object + + @Override + public void runOpMode() { + + // get a reference to our touchSensor object. + digitalTouch = hardwareMap.get(DigitalChannel.class, "digitalTouch"); + + digitalTouch.setMode(DigitalChannel.Mode.INPUT); + telemetry.addData("DigitalTouchSensorExample", "Press start to continue..."); + telemetry.update(); + + // wait for the start button to be pressed. + waitForStart(); + + // while the OpMode is active, loop and read the digital channel. + // Note we use opModeIsActive() as our loop condition because it is an interruptible method. + while (opModeIsActive()) { + + // button is pressed if value returned is LOW or false. + // send the info back to driver station using telemetry function. + if (digitalTouch.getState() == false) { + telemetry.addData("Button", "PRESSED"); + } else { + telemetry.addData("Button", "NOT PRESSED"); + } + + telemetry.update(); + } + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorGoBildaPinpoint.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorGoBildaPinpoint.java new file mode 100644 index 0000000..5b0f6e9 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorGoBildaPinpoint.java @@ -0,0 +1,116 @@ +/* +* Copyright (c) 2025 Alan Smith +* +* Permission is hereby granted, free of charge, to any person obtaining a copy +* of this software and associated documentation files (the "Software"), to deal +* in the Software without restriction, including without limitation the rights +* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +* copies of the Software, and to permit persons to whom the Software is +* furnished to do so, subject to the following conditions: + +* The above copyright notice and this permission notice shall be included in all +* copies or substantial portions of the Software. + +* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +* SOFTWARE. +*/ +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.hardware.gobilda.GoBildaPinpointDriver; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.robotcore.external.navigation.Pose2D; + +/* + * This OpMode illustrates how to use the GoBildaPinpoint + * + * The OpMode assumes that the sensor is configured with a name of "pinpoint". + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list + * + * See the sensor's product page: https://www.gobilda.com/pinpoint-odometry-computer-imu-sensor-fusion-for-2-wheel-odometry/ + */ +@TeleOp(name = "Sensor: GoBilda Pinpoint", group = "Sensor") +@Disabled +public class SensorGoBildaPinpoint extends OpMode { + // Create an instance of the sensor + GoBildaPinpointDriver pinpoint; + + @Override + public void init() { + // Get a reference to the sensor + pinpoint = hardwareMap.get(GoBildaPinpointDriver.class, "pinpoint"); + + // Configure the sensor + configurePinpoint(); + + // Set the location of the robot - this should be the place you are starting the robot from + pinpoint.setPosition(new Pose2D(DistanceUnit.INCH, 0, 0, AngleUnit.DEGREES, 0)); + } + + @Override + public void loop() { + telemetry.addLine("Push your robot around to see it track"); + telemetry.addLine("Press A to reset the position"); + if(gamepad1.a){ + // You could use readings from April Tags here to give a new known position to the pinpoint + pinpoint.setPosition(new Pose2D(DistanceUnit.INCH, 0, 0, AngleUnit.DEGREES, 0)); + } + pinpoint.update(); + Pose2D pose2D = pinpoint.getPosition(); + + telemetry.addData("X coordinate (IN)", pose2D.getX(DistanceUnit.INCH)); + telemetry.addData("Y coordinate (IN)", pose2D.getY(DistanceUnit.INCH)); + telemetry.addData("Heading angle (DEGREES)", pose2D.getHeading(AngleUnit.DEGREES)); + } + + public void configurePinpoint(){ + /* + * Set the odometry pod positions relative to the point that you want the position to be measured from. + * + * The X pod offset refers to how far sideways from the tracking point the X (forward) odometry pod is. + * Left of the center is a positive number, right of center is a negative number. + * + * The Y pod offset refers to how far forwards from the tracking point the Y (strafe) odometry pod is. + * Forward of center is a positive number, backwards is a negative number. + */ + pinpoint.setOffsets(-84.0, -168.0, DistanceUnit.MM); //these are tuned for 3110-0002-0001 Product Insight #1 + + /* + * Set the kind of pods used by your robot. If you're using goBILDA odometry pods, select either + * the goBILDA_SWINGARM_POD, or the goBILDA_4_BAR_POD. + * If you're using another kind of odometry pod, uncomment setEncoderResolution and input the + * number of ticks per unit of your odometry pod. For example: + * pinpoint.setEncoderResolution(13.26291192, DistanceUnit.MM); + */ + pinpoint.setEncoderResolution(GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_4_BAR_POD); + + /* + * Set the direction that each of the two odometry pods count. The X (forward) pod should + * increase when you move the robot forward. And the Y (strafe) pod should increase when + * you move the robot to the left. + */ + pinpoint.setEncoderDirections(GoBildaPinpointDriver.EncoderDirection.FORWARD, + GoBildaPinpointDriver.EncoderDirection.FORWARD); + + /* + * Before running the robot, recalibrate the IMU. This needs to happen when the robot is stationary + * The IMU will automatically calibrate when first powered on, but recalibrating before running + * the robot is a good idea to ensure that the calibration is "good". + * resetPosAndIMU will reset the position to 0,0,0 and also recalibrate the IMU. + * This is recommended before you run your autonomous, as a bad initial calibration can cause + * an incorrect starting value for x, y, and heading. + */ + pinpoint.resetPosAndIMU(); + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorHuskyLens.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorHuskyLens.java new file mode 100644 index 0000000..af7ca55 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorHuskyLens.java @@ -0,0 +1,160 @@ +/* +Copyright (c) 2023 FIRST + +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, +are permitted (subject to the limitations in the disclaimer below) provided that +the following conditions are met: + +Redistributions of source code must retain the above copyright notice, this list +of conditions and the following disclaimer. + +Redistributions in binary form must reproduce the above copyright notice, this +list of conditions and the following disclaimer in the documentation and/or +other materials provided with the distribution. + +Neither the name of FIRST nor the names of its contributors may be used to +endorse or promote products derived from this software without specific prior +written permission. + +NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS +LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF +THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.hardware.dfrobot.HuskyLens; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import org.firstinspires.ftc.robotcore.internal.system.Deadline; + +import java.util.concurrent.TimeUnit; + +/* + * This OpMode illustrates how to use the DFRobot HuskyLens. + * + * The HuskyLens is a Vision Sensor with a built-in object detection model. It can + * detect a number of predefined objects and AprilTags in the 36h11 family, can + * recognize colors, and can be trained to detect custom objects. See this website for + * documentation: https://wiki.dfrobot.com/HUSKYLENS_V1.0_SKU_SEN0305_SEN0336 + * + * For detailed instructions on how a HuskyLens is used in FTC, please see this tutorial: + * https://ftc-docs.firstinspires.org/en/latest/devices/huskylens/huskylens.html + * + * This sample illustrates how to detect AprilTags, but can be used to detect other types + * of objects by changing the algorithm. It assumes that the HuskyLens is configured with + * a name of "huskylens". + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list + */ +@TeleOp(name = "Sensor: HuskyLens", group = "Sensor") +@Disabled +public class SensorHuskyLens extends LinearOpMode { + + private final int READ_PERIOD = 1; + + private HuskyLens huskyLens; + + @Override + public void runOpMode() + { + huskyLens = hardwareMap.get(HuskyLens.class, "huskylens"); + + /* + * This sample rate limits the reads solely to allow a user time to observe + * what is happening on the Driver Station telemetry. Typical applications + * would not likely rate limit. + */ + Deadline rateLimit = new Deadline(READ_PERIOD, TimeUnit.SECONDS); + + /* + * Immediately expire so that the first time through we'll do the read. + */ + rateLimit.expire(); + + /* + * Basic check to see if the device is alive and communicating. This is not + * technically necessary here as the HuskyLens class does this in its + * doInitialization() method which is called when the device is pulled out of + * the hardware map. However, sometimes it's unclear why a device reports as + * failing on initialization. In the case of this device, it's because the + * call to knock() failed. + */ + if (!huskyLens.knock()) { + telemetry.addData(">>", "Problem communicating with " + huskyLens.getDeviceName()); + } else { + telemetry.addData(">>", "Press start to continue"); + } + + /* + * The device uses the concept of an algorithm to determine what types of + * objects it will look for and/or what mode it is in. The algorithm may be + * selected using the scroll wheel on the device, or via software as shown in + * the call to selectAlgorithm(). + * + * The SDK itself does not assume that the user wants a particular algorithm on + * startup, and hence does not set an algorithm. + * + * Users, should, in general, explicitly choose the algorithm they want to use + * within the OpMode by calling selectAlgorithm() and passing it one of the values + * found in the enumeration HuskyLens.Algorithm. + * + * Other algorithm choices for FTC might be: OBJECT_RECOGNITION, COLOR_RECOGNITION or OBJECT_CLASSIFICATION. + */ + huskyLens.selectAlgorithm(HuskyLens.Algorithm.TAG_RECOGNITION); + + telemetry.update(); + waitForStart(); + + /* + * Looking for AprilTags per the call to selectAlgorithm() above. A handy grid + * for testing may be found at https://wiki.dfrobot.com/HUSKYLENS_V1.0_SKU_SEN0305_SEN0336#target_20. + * + * Note again that the device only recognizes the 36h11 family of tags out of the box. + */ + while(opModeIsActive()) { + if (!rateLimit.hasExpired()) { + continue; + } + rateLimit.reset(); + + /* + * All algorithms, except for LINE_TRACKING, return a list of Blocks where a + * Block represents the outline of a recognized object along with its ID number. + * ID numbers allow you to identify what the device saw. See the HuskyLens documentation + * referenced in the header comment above for more information on IDs and how to + * assign them to objects. + * + * Returns an empty array if no objects are seen. + */ + HuskyLens.Block[] blocks = huskyLens.blocks(); + telemetry.addData("Block count", blocks.length); + for (int i = 0; i < blocks.length; i++) { + telemetry.addData("Block", blocks[i].toString()); + /* + * Here inside the FOR loop, you could save or evaluate specific info for the currently recognized Bounding Box: + * - blocks[i].width and blocks[i].height (size of box, in pixels) + * - blocks[i].left and blocks[i].top (edges of box) + * - blocks[i].x and blocks[i].y (center location) + * - blocks[i].id (Color ID) + * + * These values have Java type int (integer). + */ + } + + telemetry.update(); + } + } +} \ No newline at end of file diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorIMUNonOrthogonal.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorIMUNonOrthogonal.java new file mode 100644 index 0000000..70bc8d4 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorIMUNonOrthogonal.java @@ -0,0 +1,184 @@ +/* Copyright (c) 2022 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.IMU; +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.AngularVelocity; +import org.firstinspires.ftc.robotcore.external.navigation.Orientation; +import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles; + +import static com.qualcomm.hardware.rev.RevHubOrientationOnRobot.xyzOrientation; + +/* + * This OpMode shows how to use the new universal IMU interface. This + * interface may be used with the BNO055 IMU or the BHI260 IMU. It assumes that an IMU is configured + * on the robot with the name "imu". + * + * The sample will display the current Yaw, Pitch and Roll of the robot.
+ * With the correct orientation parameters selected, pitch/roll/yaw should act as follows: + * Pitch value should INCREASE as the robot is tipped UP at the front. (Rotation about X) + * Roll value should INCREASE as the robot is tipped UP at the left side. (Rotation about Y) + * Yaw value should INCREASE as the robot is rotated Counter Clockwise. (Rotation about Z) + * + * The yaw can be reset (to zero) by pressing the Y button on the gamepad (Triangle on a PS4 controller) + * + * This specific sample DOES NOT assume that the Hub is mounted on one of the three orthogonal + * planes (X/Y, X/Z or Y/Z) OR that the Hub has only been rotated in a range of 90 degree increments. + * + * Note: if your Hub is mounted Orthogonally (on a orthogonal surface, angled at some multiple of + * 90 Degrees) then you should use the simpler SensorIMUOrthogonal sample in this folder. + * + * But... If your Hub is mounted Non-Orthogonally, you must specify one or more rotational angles + * that transform a "Default" Hub orientation into your desired orientation. That is what is + * illustrated here. + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list. + * + * Finally, edit this OpMode to use at least one angle around an axis to orient your Hub. + */ +@TeleOp(name = "Sensor: IMU Non-Orthogonal", group = "Sensor") +@Disabled // Comment this out to add to the OpMode list +public class SensorIMUNonOrthogonal extends LinearOpMode +{ + // The IMU sensor object + IMU imu; + + //---------------------------------------------------------------------------------------------- + // Main logic + //---------------------------------------------------------------------------------------------- + + @Override public void runOpMode() throws InterruptedException { + + // Retrieve and initialize the IMU. + // This sample expects the IMU to be in a REV Hub and named "imu". + imu = hardwareMap.get(IMU.class, "imu"); + + /* Define how the hub is mounted to the robot to get the correct Yaw, Pitch and Roll values. + * + * You can apply up to three axis rotations to orient your Hub according to how it's mounted on the robot. + * + * The starting point for these rotations is the "Default" Hub orientation, which is: + * 1) Hub laying flat on a horizontal surface, with the Printed Logo facing UP + * 2) Rotated such that the USB ports are facing forward on the robot. + * + * If you are using a REV External IMU, the "Default" orientation is the same as for a REV Hub, but instead of + * the USB ports facing forward, the I2C port faces forward. + * + * The order that the rotations are performed matters, so this sample shows doing them in the order X, Y, then Z. + * For specifying non-orthogonal hub mounting orientations, we must temporarily use axes + * defined relative to the Hub itself, instead of the usual Robot Coordinate System axes + * used for the results the IMU gives us. In the starting orientation, the Hub axes are + * aligned with the Robot Coordinate System: + * + * X Axis: Starting at Center of Hub, pointing out towards I2C connectors + * Y Axis: Starting at Center of Hub, pointing out towards USB connectors + * Z Axis: Starting at Center of Hub, pointing Up through LOGO + * + * Positive rotation is defined by right-hand rule with thumb pointing in +ve direction on axis. + * + * Some examples. + * + * ---------------------------------------------------------------------------------------------------------------------------------- + * Example A) Assume that the hub is mounted on a sloped plate at the back of the robot, with the USB ports coming out the top of the hub. + * The plate is tilted UP 60 degrees from horizontal. + * + * To get the "Default" hub into this configuration you would just need a single rotation. + * 1) Rotate the Hub +60 degrees around the X axis to tilt up the front edge. + * 2) No rotation around the Y or Z axes. + * + * So the X,Y,Z rotations would be 60,0,0 + * + * ---------------------------------------------------------------------------------------------------------------------------------- + * Example B) Assume that the hub is laying flat on the chassis, but it has been twisted 30 degrees towards the right front wheel to make + * the USB cable accessible. + * + * To get the "Default" hub into this configuration you would just need a single rotation, but around a different axis. + * 1) No rotation around the X or Y axes. + * 2) Rotate the Hub -30 degrees (Clockwise) around the Z axis, since a positive angle would be Counter Clockwise. + * + * So the X,Y,Z rotations would be 0,0,-30 + * + * ---------------------------------------------------------------------------------------------------------------------------------- + * Example C) Assume that the hub is mounted on a vertical plate on the right side of the robot, with the Logo facing out, and the + * Hub rotated so that the USB ports are facing down 30 degrees towards the back wheels of the robot. + * + * To get the "Default" hub into this configuration will require several rotations. + * 1) Rotate the hub +90 degrees around the X axis to get it standing upright with the logo pointing backwards on the robot + * 2) Next, rotate the hub +90 around the Y axis to get it facing to the right. + * 3) Finally rotate the hub +120 degrees around the Z axis to take the USB ports from vertical to sloping down 30 degrees and + * facing towards the back of the robot. + * + * So the X,Y,Z rotations would be 90,90,120 + */ + + // The next three lines define the desired axis rotations. + // To Do: EDIT these values to match YOUR mounting configuration. + double xRotation = 0; // enter the desired X rotation angle here. + double yRotation = 0; // enter the desired Y rotation angle here. + double zRotation = 0; // enter the desired Z rotation angle here. + + Orientation hubRotation = xyzOrientation(xRotation, yRotation, zRotation); + + // Now initialize the IMU with this mounting orientation + RevHubOrientationOnRobot orientationOnRobot = new RevHubOrientationOnRobot(hubRotation); + imu.initialize(new IMU.Parameters(orientationOnRobot)); + + // Loop and update the dashboard + while (!isStopRequested()) { + telemetry.addData("Hub orientation", "X=%.1f, Y=%.1f, Z=%.1f \n", xRotation, yRotation, zRotation); + + // Check to see if heading reset is requested + if (gamepad1.y) { + telemetry.addData("Yaw", "Resetting\n"); + imu.resetYaw(); + } else { + telemetry.addData("Yaw", "Press Y (triangle) on Gamepad to reset\n"); + } + + // Retrieve Rotational Angles and Velocities + YawPitchRollAngles orientation = imu.getRobotYawPitchRollAngles(); + AngularVelocity angularVelocity = imu.getRobotAngularVelocity(AngleUnit.DEGREES); + + telemetry.addData("Yaw (Z)", "%.2f Deg. (Heading)", orientation.getYaw(AngleUnit.DEGREES)); + telemetry.addData("Pitch (X)", "%.2f Deg.", orientation.getPitch(AngleUnit.DEGREES)); + telemetry.addData("Roll (Y)", "%.2f Deg.\n", orientation.getRoll(AngleUnit.DEGREES)); + telemetry.addData("Yaw (Z) velocity", "%.2f Deg/Sec", angularVelocity.zRotationRate); + telemetry.addData("Pitch (X) velocity", "%.2f Deg/Sec", angularVelocity.xRotationRate); + telemetry.addData("Roll (Y) velocity", "%.2f Deg/Sec", angularVelocity.yRotationRate); + telemetry.update(); + } + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorIMUOrthogonal.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorIMUOrthogonal.java new file mode 100644 index 0000000..af4c202 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorIMUOrthogonal.java @@ -0,0 +1,146 @@ +/* Copyright (c) 2022 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.hardware.rev.RevHubOrientationOnRobot; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.IMU; +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.AngularVelocity; +import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles; + +/* + * This OpMode shows how to use the new universal IMU interface. This + * interface may be used with the BNO055 IMU or the BHI260 IMU. It assumes that an IMU is configured + * on the robot with the name "imu". + * + * The sample will display the current Yaw, Pitch and Roll of the robot.
+ * With the correct orientation parameters selected, pitch/roll/yaw should act as follows: + * Pitch value should INCREASE as the robot is tipped UP at the front. (Rotation about X)
+ * Roll value should INCREASE as the robot is tipped UP at the left side. (Rotation about Y)
+ * Yaw value should INCREASE as the robot is rotated Counter Clockwise. (Rotation about Z)
+ * + * The yaw can be reset (to zero) by pressing the Y button on the gamepad (Triangle on a PS4 controller) + * + * This specific sample assumes that the Hub is mounted on one of the three orthogonal planes + * (X/Y, X/Z or Y/Z) and that the Hub has only been rotated in a range of 90 degree increments. + * + * Note: if your Hub is mounted on a surface angled at some non-90 Degree multiple (like 30) look at + * the alternative SensorIMUNonOrthogonal sample in this folder. + * + * This "Orthogonal" requirement means that: + * + * 1) The Logo printed on the top of the Hub can ONLY be pointing in one of six directions: + * FORWARD, BACKWARD, UP, DOWN, LEFT and RIGHT. + * + * 2) The USB ports can only be pointing in one of the same six directions:
+ * FORWARD, BACKWARD, UP, DOWN, LEFT and RIGHT. + * + * So, To fully define how your Hub is mounted to the robot, you must simply specify:
+ * logoFacingDirection
+ * usbFacingDirection + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list. + * + * Finally, choose the two correct parameters to define how your Hub is mounted and edit this OpMode + * to use those parameters. + */ +@TeleOp(name = "Sensor: IMU Orthogonal", group = "Sensor") +@Disabled // Comment this out to add to the OpMode list +public class SensorIMUOrthogonal extends LinearOpMode +{ + // The IMU sensor object + IMU imu; + + //---------------------------------------------------------------------------------------------- + // Main logic + //---------------------------------------------------------------------------------------------- + + @Override public void runOpMode() throws InterruptedException { + + // Retrieve and initialize the IMU. + // This sample expects the IMU to be in a REV Hub and named "imu". + imu = hardwareMap.get(IMU.class, "imu"); + + /* Define how the hub is mounted on the robot to get the correct Yaw, Pitch and Roll values. + * + * Two input parameters are required to fully specify the Orientation. + * The first parameter specifies the direction the printed logo on the Hub is pointing. + * The second parameter specifies the direction the USB connector on the Hub is pointing. + * All directions are relative to the robot, and left/right is as-viewed from behind the robot. + * + * If you are using a REV 9-Axis IMU, you can use the Rev9AxisImuOrientationOnRobot class instead of the + * RevHubOrientationOnRobot class, which has an I2cPortFacingDirection instead of a UsbFacingDirection. + */ + + /* The next two lines define Hub orientation. + * The Default Orientation (shown) is when a hub is mounted horizontally with the printed logo pointing UP and the USB port pointing FORWARD. + * + * To Do: EDIT these two lines to match YOUR mounting configuration. + */ + RevHubOrientationOnRobot.LogoFacingDirection logoDirection = RevHubOrientationOnRobot.LogoFacingDirection.UP; + RevHubOrientationOnRobot.UsbFacingDirection usbDirection = RevHubOrientationOnRobot.UsbFacingDirection.FORWARD; + + RevHubOrientationOnRobot orientationOnRobot = new RevHubOrientationOnRobot(logoDirection, usbDirection); + + // Now initialize the IMU with this mounting orientation + // Note: if you choose two conflicting directions, this initialization will cause a code exception. + imu.initialize(new IMU.Parameters(orientationOnRobot)); + + // Loop and update the dashboard + while (!isStopRequested()) { + + telemetry.addData("Hub orientation", "Logo=%s USB=%s\n ", logoDirection, usbDirection); + + // Check to see if heading reset is requested + if (gamepad1.y) { + telemetry.addData("Yaw", "Resetting\n"); + imu.resetYaw(); + } else { + telemetry.addData("Yaw", "Press Y (triangle) on Gamepad to reset\n"); + } + + // Retrieve Rotational Angles and Velocities + YawPitchRollAngles orientation = imu.getRobotYawPitchRollAngles(); + AngularVelocity angularVelocity = imu.getRobotAngularVelocity(AngleUnit.DEGREES); + + telemetry.addData("Yaw (Z)", "%.2f Deg. (Heading)", orientation.getYaw(AngleUnit.DEGREES)); + telemetry.addData("Pitch (X)", "%.2f Deg.", orientation.getPitch(AngleUnit.DEGREES)); + telemetry.addData("Roll (Y)", "%.2f Deg.\n", orientation.getRoll(AngleUnit.DEGREES)); + telemetry.addData("Yaw (Z) velocity", "%.2f Deg/Sec", angularVelocity.zRotationRate); + telemetry.addData("Pitch (X) velocity", "%.2f Deg/Sec", angularVelocity.xRotationRate); + telemetry.addData("Roll (Y) velocity", "%.2f Deg/Sec", angularVelocity.yRotationRate); + telemetry.update(); + } + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorKLNavxMicro.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorKLNavxMicro.java new file mode 100644 index 0000000..ccc945f --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorKLNavxMicro.java @@ -0,0 +1,128 @@ +/* Copyright (c) 2017 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.hardware.kauailabs.NavxMicroNavigationSensor; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.Gyroscope; +import com.qualcomm.robotcore.hardware.IntegratingGyroscope; +import com.qualcomm.robotcore.util.ElapsedTime; +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.AngularVelocity; +import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder; +import org.firstinspires.ftc.robotcore.external.navigation.AxesReference; +import org.firstinspires.ftc.robotcore.external.navigation.Orientation; + +/* + * This OpMode shows how to use Kauai Labs navX Micro Robotics Navigation + * Sensor. It assumes that the sensor is configured with a name of "navx". + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list + */ +@TeleOp(name = "Sensor: KL navX Micro", group = "Sensor") +@Disabled +public class SensorKLNavxMicro extends LinearOpMode { + + /** In this sample, for illustration purposes we use two interfaces on the one gyro object. + * That's likely atypical: you'll probably use one or the other in any given situation, + * depending on what you're trying to do. {@link IntegratingGyroscope} (and it's base interface, + * {@link Gyroscope}) are common interfaces supported by possibly several different gyro + * implementations. {@link NavxMicroNavigationSensor}, by contrast, provides functionality that + * is unique to the navX Micro sensor. + */ + IntegratingGyroscope gyro; + NavxMicroNavigationSensor navxMicro; + + // A timer helps provide feedback while calibration is taking place + ElapsedTime timer = new ElapsedTime(); + + @Override public void runOpMode() throws InterruptedException { + // Get a reference to a Modern Robotics GyroSensor object. We use several interfaces + // on this object to illustrate which interfaces support which functionality. + navxMicro = hardwareMap.get(NavxMicroNavigationSensor.class, "navx"); + gyro = (IntegratingGyroscope)navxMicro; + // If you're only interested int the IntegratingGyroscope interface, the following will suffice. + // gyro = hardwareMap.get(IntegratingGyroscope.class, "navx"); + + // The gyro automatically starts calibrating. This takes a few seconds. + telemetry.log().add("Gyro Calibrating. Do Not Move!"); + + // Wait until the gyro calibration is complete + timer.reset(); + while (navxMicro.isCalibrating()) { + telemetry.addData("calibrating", "%s", Math.round(timer.seconds())%2==0 ? "|.." : "..|"); + telemetry.update(); + Thread.sleep(50); + } + telemetry.log().clear(); telemetry.log().add("Gyro Calibrated. Press Start."); + telemetry.clear(); telemetry.update(); + + // Wait for the start button to be pressed + waitForStart(); + telemetry.log().clear(); + + while (opModeIsActive()) { + + // Read dimensionalized data from the gyro. This gyro can report angular velocities + // about all three axes. Additionally, it internally integrates the Z axis to + // be able to report an absolute angular Z orientation. + AngularVelocity rates = gyro.getAngularVelocity(AngleUnit.DEGREES); + Orientation angles = gyro.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES); + + telemetry.addLine() + .addData("dx", formatRate(rates.xRotationRate)) + .addData("dy", formatRate(rates.yRotationRate)) + .addData("dz", "%s deg/s", formatRate(rates.zRotationRate)); + + telemetry.addLine() + .addData("heading", formatAngle(angles.angleUnit, angles.firstAngle)) + .addData("roll", formatAngle(angles.angleUnit, angles.secondAngle)) + .addData("pitch", "%s deg", formatAngle(angles.angleUnit, angles.thirdAngle)); + telemetry.update(); + + idle(); // Always call idle() at the bottom of your while(opModeIsActive()) loop + } + } + + String formatRate(float rate) { + return String.format("%.3f", rate); + } + + String formatAngle(AngleUnit angleUnit, double angle) { + return formatDegrees(AngleUnit.DEGREES.fromUnit(angleUnit, angle)); + } + + String formatDegrees(double degrees){ + return String.format("%.1f", AngleUnit.DEGREES.normalize(degrees)); + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorLimelight3A.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorLimelight3A.java new file mode 100644 index 0000000..2579cd0 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorLimelight3A.java @@ -0,0 +1,157 @@ +/* +Copyright (c) 2024 Limelight Vision + +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, +are permitted (subject to the limitations in the disclaimer below) provided that +the following conditions are met: + +Redistributions of source code must retain the above copyright notice, this list +of conditions and the following disclaimer. + +Redistributions in binary form must reproduce the above copyright notice, this +list of conditions and the following disclaimer in the documentation and/or +other materials provided with the distribution. + +Neither the name of FIRST nor the names of its contributors may be used to +endorse or promote products derived from this software without specific prior +written permission. + +NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS +LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF +THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.hardware.limelightvision.LLResult; +import com.qualcomm.hardware.limelightvision.LLResultTypes; +import com.qualcomm.hardware.limelightvision.LLStatus; +import com.qualcomm.hardware.limelightvision.Limelight3A; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import org.firstinspires.ftc.robotcore.external.navigation.Pose3D; + +import java.util.List; + +/* + * This OpMode illustrates how to use the Limelight3A Vision Sensor. + * + * @see Limelight + * + * Notes on configuration: + * + * The device presents itself, when plugged into a USB port on a Control Hub as an ethernet + * interface. A DHCP server running on the Limelight automatically assigns the Control Hub an + * ip address for the new ethernet interface. + * + * Since the Limelight is plugged into a USB port, it will be listed on the top level configuration + * activity along with the Control Hub Portal and other USB devices such as webcams. Typically + * serial numbers are displayed below the device's names. In the case of the Limelight device, the + * Control Hub's assigned ip address for that ethernet interface is used as the "serial number". + * + * Tapping the Limelight's name, transitions to a new screen where the user can rename the Limelight + * and specify the Limelight's ip address. Users should take care not to confuse the ip address of + * the Limelight itself, which can be configured through the Limelight settings page via a web browser, + * and the ip address the Limelight device assigned the Control Hub and which is displayed in small text + * below the name of the Limelight on the top level configuration screen. + */ +@TeleOp(name = "Sensor: Limelight3A", group = "Sensor") +@Disabled +public class SensorLimelight3A extends LinearOpMode { + + private Limelight3A limelight; + + @Override + public void runOpMode() throws InterruptedException + { + limelight = hardwareMap.get(Limelight3A.class, "limelight"); + + telemetry.setMsTransmissionInterval(11); + + limelight.pipelineSwitch(0); + + /* + * Starts polling for data. If you neglect to call start(), getLatestResult() will return null. + */ + limelight.start(); + + telemetry.addData(">", "Robot Ready. Press Play."); + telemetry.update(); + waitForStart(); + + while (opModeIsActive()) { + LLStatus status = limelight.getStatus(); + telemetry.addData("Name", "%s", + status.getName()); + telemetry.addData("LL", "Temp: %.1fC, CPU: %.1f%%, FPS: %d", + status.getTemp(), status.getCpu(),(int)status.getFps()); + telemetry.addData("Pipeline", "Index: %d, Type: %s", + status.getPipelineIndex(), status.getPipelineType()); + + LLResult result = limelight.getLatestResult(); + if (result.isValid()) { + // Access general information + Pose3D botpose = result.getBotpose(); + double captureLatency = result.getCaptureLatency(); + double targetingLatency = result.getTargetingLatency(); + double parseLatency = result.getParseLatency(); + telemetry.addData("LL Latency", captureLatency + targetingLatency); + telemetry.addData("Parse Latency", parseLatency); + telemetry.addData("PythonOutput", java.util.Arrays.toString(result.getPythonOutput())); + + telemetry.addData("tx", result.getTx()); + telemetry.addData("txnc", result.getTxNC()); + telemetry.addData("ty", result.getTy()); + telemetry.addData("tync", result.getTyNC()); + + telemetry.addData("Botpose", botpose.toString()); + + // Access barcode results + List barcodeResults = result.getBarcodeResults(); + for (LLResultTypes.BarcodeResult br : barcodeResults) { + telemetry.addData("Barcode", "Data: %s", br.getData()); + } + + // Access classifier results + List classifierResults = result.getClassifierResults(); + for (LLResultTypes.ClassifierResult cr : classifierResults) { + telemetry.addData("Classifier", "Class: %s, Confidence: %.2f", cr.getClassName(), cr.getConfidence()); + } + + // Access detector results + List detectorResults = result.getDetectorResults(); + for (LLResultTypes.DetectorResult dr : detectorResults) { + telemetry.addData("Detector", "Class: %s, Area: %.2f", dr.getClassName(), dr.getTargetArea()); + } + + // Access fiducial results + List fiducialResults = result.getFiducialResults(); + for (LLResultTypes.FiducialResult fr : fiducialResults) { + telemetry.addData("Fiducial", "ID: %d, Family: %s, X: %.2f, Y: %.2f", fr.getFiducialId(), fr.getFamily(), fr.getTargetXDegrees(), fr.getTargetYDegrees()); + } + + // Access color results + List colorResults = result.getColorResults(); + for (LLResultTypes.ColorResult cr : colorResults) { + telemetry.addData("Color", "X: %.2f, Y: %.2f", cr.getTargetXDegrees(), cr.getTargetYDegrees()); + } + } else { + telemetry.addData("Limelight", "No data available"); + } + + telemetry.update(); + } + limelight.stop(); + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorMRColor.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorMRColor.java new file mode 100644 index 0000000..32b37e0 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorMRColor.java @@ -0,0 +1,138 @@ +/* Copyright (c) 2017 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import android.app.Activity; +import android.graphics.Color; +import android.view.View; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.ColorSensor; + +/* + * + * This OpMode that shows how to use + * a Modern Robotics Color Sensor. + * + * The OpMode assumes that the color sensor + * is configured with a name of "sensor_color". + * + * You can use the X button on gamepad1 to toggle the LED on and off. + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list + */ +@TeleOp(name = "Sensor: MR Color", group = "Sensor") +@Disabled +public class SensorMRColor extends LinearOpMode { + + ColorSensor colorSensor; // Hardware Device Object + + + @Override + public void runOpMode() { + + // hsvValues is an array that will hold the hue, saturation, and value information. + float hsvValues[] = {0F,0F,0F}; + + // values is a reference to the hsvValues array. + final float values[] = hsvValues; + + // get a reference to the RelativeLayout so we can change the background + // color of the Robot Controller app to match the hue detected by the RGB sensor. + int relativeLayoutId = hardwareMap.appContext.getResources().getIdentifier("RelativeLayout", "id", hardwareMap.appContext.getPackageName()); + final View relativeLayout = ((Activity) hardwareMap.appContext).findViewById(relativeLayoutId); + + // bPrevState and bCurrState represent the previous and current state of the button. + boolean bPrevState = false; + boolean bCurrState = false; + + // bLedOn represents the state of the LED. + boolean bLedOn = true; + + // get a reference to our ColorSensor object. + colorSensor = hardwareMap.get(ColorSensor.class, "sensor_color"); + + // Set the LED in the beginning + colorSensor.enableLed(bLedOn); + + // wait for the start button to be pressed. + waitForStart(); + + // while the OpMode is active, loop and read the RGB data. + // Note we use opModeIsActive() as our loop condition because it is an interruptible method. + while (opModeIsActive()) { + + // check the status of the x button on either gamepad. + bCurrState = gamepad1.x; + + // check for button state transitions. + if (bCurrState && (bCurrState != bPrevState)) { + + // button is transitioning to a pressed state. So Toggle LED + bLedOn = !bLedOn; + colorSensor.enableLed(bLedOn); + } + + // update previous state variable. + bPrevState = bCurrState; + + // convert the RGB values to HSV values. + Color.RGBToHSV(colorSensor.red() * 8, colorSensor.green() * 8, colorSensor.blue() * 8, hsvValues); + + // send the info back to driver station using telemetry function. + telemetry.addData("LED", bLedOn ? "On" : "Off"); + telemetry.addData("Clear", colorSensor.alpha()); + telemetry.addData("Red ", colorSensor.red()); + telemetry.addData("Green", colorSensor.green()); + telemetry.addData("Blue ", colorSensor.blue()); + telemetry.addData("Hue", hsvValues[0]); + + // change the background color to match the color detected by the RGB sensor. + // pass a reference to the hue, saturation, and value array as an argument + // to the HSVToColor method. + relativeLayout.post(new Runnable() { + public void run() { + relativeLayout.setBackgroundColor(Color.HSVToColor(0xff, values)); + } + }); + + telemetry.update(); + } + + // Set the panel back to the default color + relativeLayout.post(new Runnable() { + public void run() { + relativeLayout.setBackgroundColor(Color.WHITE); + } + }); + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorMRGyro.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorMRGyro.java new file mode 100644 index 0000000..91c0062 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorMRGyro.java @@ -0,0 +1,160 @@ +/* Copyright (c) 2017 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.hardware.modernrobotics.ModernRoboticsI2cGyro; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.IntegratingGyroscope; +import com.qualcomm.robotcore.util.ElapsedTime; +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.AngularVelocity; +import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder; +import org.firstinspires.ftc.robotcore.external.navigation.AxesReference; + +/* + * This OpMode shows how to use the Modern Robotics Gyro. + * + * The OpMode assumes that the gyro sensor is attached to a Device Interface Module + * I2C channel and is configured with a name of "gyro". + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list +*/ +@TeleOp(name = "Sensor: MR Gyro", group = "Sensor") +@Disabled +public class SensorMRGyro extends LinearOpMode { + + /* In this sample, for illustration purposes we use two interfaces on the one gyro object. + * That's likely atypical: you'll probably use one or the other in any given situation, + * depending on what you're trying to do. {@link IntegratingGyroscope} (and it's base interface, + * {@link Gyroscope}) are common interfaces supported by possibly several different gyro + * implementations. {@link ModernRoboticsI2cGyro}, by contrast, provides functionality that + * is unique to the Modern Robotics gyro sensor. + */ + IntegratingGyroscope gyro; + ModernRoboticsI2cGyro modernRoboticsI2cGyro; + + // A timer helps provide feedback while calibration is taking place + ElapsedTime timer = new ElapsedTime(); + + @Override + public void runOpMode() { + + boolean lastResetState = false; + boolean curResetState = false; + + // Get a reference to a Modern Robotics gyro object. We use several interfaces + // on this object to illustrate which interfaces support which functionality. + modernRoboticsI2cGyro = hardwareMap.get(ModernRoboticsI2cGyro.class, "gyro"); + gyro = (IntegratingGyroscope)modernRoboticsI2cGyro; + // If you're only interested int the IntegratingGyroscope interface, the following will suffice. + // gyro = hardwareMap.get(IntegratingGyroscope.class, "gyro"); + // A similar approach will work for the Gyroscope interface, if that's all you need. + + // Start calibrating the gyro. This takes a few seconds and is worth performing + // during the initialization phase at the start of each OpMode. + telemetry.log().add("Gyro Calibrating. Do Not Move!"); + modernRoboticsI2cGyro.calibrate(); + + // Wait until the gyro calibration is complete + timer.reset(); + while (!isStopRequested() && modernRoboticsI2cGyro.isCalibrating()) { + telemetry.addData("calibrating", "%s", Math.round(timer.seconds())%2==0 ? "|.." : "..|"); + telemetry.update(); + sleep(50); + } + + telemetry.log().clear(); telemetry.log().add("Gyro Calibrated. Press Start."); + telemetry.clear(); telemetry.update(); + + // Wait for the start button to be pressed + waitForStart(); + telemetry.log().clear(); + telemetry.log().add("Press A & B to reset heading"); + + // Loop until we're asked to stop + while (opModeIsActive()) { + + // If the A and B buttons are pressed just now, reset Z heading. + curResetState = (gamepad1.a && gamepad1.b); + if (curResetState && !lastResetState) { + modernRoboticsI2cGyro.resetZAxisIntegrator(); + } + lastResetState = curResetState; + + // The raw() methods report the angular rate of change about each of the + // three axes directly as reported by the underlying sensor IC. + int rawX = modernRoboticsI2cGyro.rawX(); + int rawY = modernRoboticsI2cGyro.rawY(); + int rawZ = modernRoboticsI2cGyro.rawZ(); + int heading = modernRoboticsI2cGyro.getHeading(); + int integratedZ = modernRoboticsI2cGyro.getIntegratedZValue(); + + // Read dimensionalized data from the gyro. This gyro can report angular velocities + // about all three axes. Additionally, it internally integrates the Z axis to + // be able to report an absolute angular Z orientation. + AngularVelocity rates = gyro.getAngularVelocity(AngleUnit.DEGREES); + float zAngle = gyro.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES).firstAngle; + + // Read administrative information from the gyro + int zAxisOffset = modernRoboticsI2cGyro.getZAxisOffset(); + int zAxisScalingCoefficient = modernRoboticsI2cGyro.getZAxisScalingCoefficient(); + + telemetry.addLine() + .addData("dx", formatRate(rates.xRotationRate)) + .addData("dy", formatRate(rates.yRotationRate)) + .addData("dz", "%s deg/s", formatRate(rates.zRotationRate)); + telemetry.addData("angle", "%s deg", formatFloat(zAngle)); + telemetry.addData("heading", "%3d deg", heading); + telemetry.addData("integrated Z", "%3d", integratedZ); + telemetry.addLine() + .addData("rawX", formatRaw(rawX)) + .addData("rawY", formatRaw(rawY)) + .addData("rawZ", formatRaw(rawZ)); + telemetry.addLine().addData("z offset", zAxisOffset).addData("z coeff", zAxisScalingCoefficient); + telemetry.update(); + } + } + + String formatRaw(int rawValue) { + return String.format("%d", rawValue); + } + + String formatRate(float rate) { + return String.format("%.3f", rate); + } + + String formatFloat(float rate) { + return String.format("%.3f", rate); + } + +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorMROpticalDistance.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorMROpticalDistance.java new file mode 100644 index 0000000..6d2dfa3 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorMROpticalDistance.java @@ -0,0 +1,70 @@ +/* Copyright (c) 2017 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.OpticalDistanceSensor; + +/* + * This OpMode shows how to use a Modern Robotics Optical Distance Sensor + * It assumes that the ODS sensor is configured with a name of "sensor_ods". + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list + */ +@TeleOp(name = "Sensor: MR ODS", group = "Sensor") +@Disabled +public class SensorMROpticalDistance extends LinearOpMode { + + OpticalDistanceSensor odsSensor; // Hardware Device Object + + @Override + public void runOpMode() { + + // get a reference to our Light Sensor object. + odsSensor = hardwareMap.get(OpticalDistanceSensor.class, "sensor_ods"); + + // wait for the start button to be pressed. + waitForStart(); + + // while the OpMode is active, loop and read the light levels. + // Note we use opModeIsActive() as our loop condition because it is an interruptible method. + while (opModeIsActive()) { + + // send the info back to driver station using telemetry function. + telemetry.addData("Raw", odsSensor.getRawLightDetected()); + telemetry.addData("Normal", odsSensor.getLightDetected()); + + telemetry.update(); + } + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorMRRangeSensor.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorMRRangeSensor.java new file mode 100644 index 0000000..2039ef9 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorMRRangeSensor.java @@ -0,0 +1,70 @@ +/* Copyright (c) 2017 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.hardware.modernrobotics.ModernRoboticsI2cRangeSensor; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; + +/* + * This OpMode illustrates how to use the Modern Robotics Range Sensor. + * + * The OpMode assumes that the range sensor is configured with a name of "sensor_range". + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list + * + * @see MR Range Sensor + */ +@TeleOp(name = "Sensor: MR range sensor", group = "Sensor") +@Disabled // comment out or remove this line to enable this OpMode +public class SensorMRRangeSensor extends LinearOpMode { + + ModernRoboticsI2cRangeSensor rangeSensor; + + @Override public void runOpMode() { + + // get a reference to our compass + rangeSensor = hardwareMap.get(ModernRoboticsI2cRangeSensor.class, "sensor_range"); + + // wait for the start button to be pressed + waitForStart(); + + while (opModeIsActive()) { + telemetry.addData("raw ultrasonic", rangeSensor.rawUltrasonic()); + telemetry.addData("raw optical", rangeSensor.rawOptical()); + telemetry.addData("cm optical", "%.2f cm", rangeSensor.cmOptical()); + telemetry.addData("cm", "%.2f cm", rangeSensor.getDistance(DistanceUnit.CM)); + telemetry.update(); + } + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorOctoQuad.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorOctoQuad.java new file mode 100644 index 0000000..312d7f5 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorOctoQuad.java @@ -0,0 +1,156 @@ +/* + * Copyright (c) 2025 DigitalChickenLabs + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.hardware.digitalchickenlabs.OctoQuad; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.util.ElapsedTime; + +import org.firstinspires.ftc.robotcore.external.Telemetry; + +/* + * This OpMode illustrates how to use DigitalChickenLabs OctoQuad Quad Encoder & Pulse Width I/F Module + * + * The OctoQuad has 8 input channels that can used to read either Relative Quadrature Encoders or + * Pulse-Width Absolute Encoder inputs. Relative Quadrature encoders are found on most FTC motors, + * and some stand-alone position sensors like the REV Thru-Bore encoder. Pulse-Width encoders are + * less common. The REV Thru-Bore encoder can provide its absolute position via a variable pulse width, + * as can several sonar rangefinders such as the MaxBotix MB1000 series. + * + * Note: SDK 11.0+ requires that the OctoQuad is running firmware V3.0 or greater. + * Visit https://github.com/DigitalChickenLabs/OctoQuad/tree/master/firmware for instruction + * on how to upgrade your OctoQuad's firmware. + * + * This basic sample shows how an OctoQuad can be used to read the position of three Odometry pods + * fitted with REV Thru-Bore encoders. For a more advanced example with additional OctoQuad + * capabilities, see the SensorOctoQuadAdv sample. + * + * This OpMode assumes the OctoQuad is attached to an I2C interface named "octoquad" in the robot config. + * + * The code assumes the first three OctoQuad inputs are connected as follows + * - Chan 0: for measuring forward motion on the left side of the robot. + * - Chan 1: for measuring forward motion on the right side of the robot. + * - Chan 2: for measuring Lateral (strafing) motion. + * + * The encoder values may be reset to zero by pressing the X (left most) button on Gamepad 1. + * + * This sample does not show how to interpret these readings, just how to obtain and display them. + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list + * + * See the sensor's product page: https://www.tindie.com/products/35114/ + */ +@Disabled +@TeleOp(name = "OctoQuad Basic", group="OctoQuad") +public class SensorOctoQuad extends LinearOpMode { + + // Identify which encoder OctoQuad inputs are connected to each odometry pod. + private final int ODO_LEFT = 0; // Facing forward direction on left side of robot + private final int ODO_RIGHT = 1; // Facing forward direction on right side or robot + private final int ODO_PERP = 2; // Facing perpendicular direction at the center of the robot + + // Declare the OctoQuad object; + private OctoQuad octoquad; + + private int posLeft; + private int posRight; + private int posPerp; + private int velLeft; + private int velRight; + private int velPerp; + + /** + * This function is executed when this OpMode is selected from the Driver Station. + */ + @Override + public void runOpMode() { + + // Connect to OctoQuad by referring to its name in the Robot Configuration. + octoquad = hardwareMap.get(OctoQuad.class, "octoquad"); + + // Read the Firmware Revision number from the OctoQuad and display it as telemetry. + telemetry.addData("OctoQuad Firmware Version ", octoquad.getFirmwareVersion()); + + // Reverse the count-direction of any encoder that is not what you require. + // e.g. if you push the robot forward and the left encoder counts down, then reverse it. + octoquad.setSingleEncoderDirection(ODO_LEFT, OctoQuad.EncoderDirection.REVERSE); + octoquad.setSingleEncoderDirection(ODO_RIGHT, OctoQuad.EncoderDirection.FORWARD); + octoquad.setSingleEncoderDirection(ODO_PERP, OctoQuad.EncoderDirection.FORWARD); + + // set the interval over which pulses are counted to determine velocity. + octoquad.setAllVelocitySampleIntervals(50); // 50 mSec means 20 velocity updates per second. + + // Save any changes that are made, just in case there is a sensor power glitch. + octoquad.saveParametersToFlash(); + + telemetry.addLine("\nPress START to read encoder values"); + telemetry.update(); + + waitForStart(); + + // Configure the telemetry for optimal display of data. + telemetry.setDisplayFormat(Telemetry.DisplayFormat.MONOSPACE); + telemetry.setMsTransmissionInterval(100); + + // Set all the encoder inputs to zero. + octoquad.resetAllPositions(); + + // Loop while displaying the odometry pod positions. + while (opModeIsActive()) { + telemetry.addData(">", "Press X to Reset Encoders\n"); + + // Check for X button to reset encoders. + if (gamepad1.x) { + // Reset the position of all encoders to zero. + octoquad.resetAllPositions(); + } + + // Read all the encoder data. Load into local members. + readOdometryPods(); + + // Display the values. + telemetry.addData("Left P", "%7d V :%6d CPS ", posLeft, velLeft); + telemetry.addData("Right P", "%7d V :%6d CPS ", posRight, velRight); + telemetry.addData("Perp P", "%7d V :%6d CPS ", posPerp, velPerp); + telemetry.update(); + } + } + + private void readOdometryPods() { + // For best performance, we should only perform ONE transaction with the OctoQuad each cycle. + // This can be achieved in one of two ways: + // 1) by doing a block data read once per control cycle + // or + // 2) by doing individual caching reads, but only reading each encoder value ONCE per cycle. + // + // Since method 2 is simplest, we will use it here. + posLeft = octoquad.readSinglePosition_Caching(ODO_LEFT); + posRight = octoquad.readSinglePosition_Caching(ODO_RIGHT); + posPerp = octoquad.readSinglePosition_Caching(ODO_PERP); + velLeft = octoquad.readSingleVelocity_Caching(ODO_LEFT) * 20; // scale up to cps + velRight = octoquad.readSingleVelocity_Caching(ODO_RIGHT) * 20; // scale up to cps + velPerp = octoquad.readSingleVelocity_Caching(ODO_PERP) * 20; // scale up to cps + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorOctoQuadAdv.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorOctoQuadAdv.java new file mode 100644 index 0000000..0e412ef --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorOctoQuadAdv.java @@ -0,0 +1,288 @@ +/* + * Copyright (c) 2024 DigitalChickenLabs + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.hardware.digitalchickenlabs.OctoQuad; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.util.ElapsedTime; +import com.qualcomm.robotcore.util.MovingStatistics; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; + + +import org.firstinspires.ftc.robotcore.external.Telemetry; +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; + +import java.util.ArrayList; +import java.util.List; + +/* + * This OpMode illustrates how to use advanced features of the DigitalChickenLabs OctoQuad Quadrature + * Encoder & Pulse Width Interface Module. The OctoQuad has 8 input channels that can used to read + * either Quadrature Encoder signals (like with most FTC motors) or Pulse-Width style Absolute Encoder + * inputs (eg: REV Through Bore encoder pulse width output). + * + * This OpMode illustrates several of the more advanced features of an OctoQuad, including Pulse Width + * measurement and velocity measurement. For a more basic OpMode just showing how to read encoder + * inputs, see the SensorOctoQuad sample. + * + * This OpMode assumes the OctoQuad is attached to an I2C interface named "octoquad" in the robot config. + * + * One system that requires a lot of encoder inputs is a Swerve Drive system. + * Such a drive requires two encoders per drive module: + * One encoder for the Drive motor/wheel position.velocity, and one for Steer motor angle. + * The Drive motor requires a quadrature encoder, and the Steering motor requires an Absolute encoder. + * + * This sample will configure an OctoQuad for a swerve drive, as follows + * - Configure 4 Relative Quadrature Encoder inputs and 4 Absolute Pulse-Width Encoder inputs + * - Configure a velocity sample interval of 25 mSec + * - Configure a pulse width input range of 1-1024 uSec for a REV Encoder's Absolute Pulse output. + * + * An OctoSwerveDrive class will be created to initialize the OctoQuad, and manage 4 swerve modules. + * An OctoSwerveModule class will be created to configure and read a single swerve module. + * + * Wiring: + * The OctoQuad will be configured to accept Quadrature encoders on the first four channels and + * Absolute (pulse width) encoders on the last four channels. + * + * The standard encoder cable will connect each Driver Motor encoder to the OctoQuad. (channels 0-3) + * + * A modified version of the REV 6-4 pin cable (shipped with the encoder) connects the steering encoder + * to the OctoQuad. (channels 4-7) + * + * To connect the Absolute position signal from a REV Thru-Bore encoder to the OctoQuad, the + * Provided 6-pin to 4-pin cable will need to be modified. + * At the 6-pin connector end, move the yellow wire from its initial pin-3 position to the + * ABS pin-5 position. This can be done easily by using a small flathead screwdriver to lift the + * small white tab holding the metal wire crimp in place and gently pulling the wire out. + * See the OctoSwerveDrive() constructor below for the correct wheel/channel assignment. + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list + * + * See the sensor's product page: https://www.tindie.com/products/35114/ + */ +@Disabled +@TeleOp(name="OctoQuad Advanced", group="OctoQuad") +public class SensorOctoQuadAdv extends LinearOpMode { + + @Override + public void runOpMode() throws InterruptedException { + // Connect to the OctoQuad by looking up its name in the hardwareMap. + OctoQuad octoquad = hardwareMap.get(OctoQuad.class, "octoquad"); + + // Create the interface for the Swerve Drive Encoders + OctoSwerveDrive octoSwerveDrive = new OctoSwerveDrive(octoquad); + + // Display the OctoQuad firmware revision + telemetry.addLine("OctoQuad Firmware v" + octoquad.getFirmwareVersion()); + telemetry.addLine("\nPress START to read encoder values"); + telemetry.update(); + + waitForStart(); + + // Configure the telemetry for optimal display of data. + telemetry.setDisplayFormat(Telemetry.DisplayFormat.MONOSPACE); + telemetry.setMsTransmissionInterval(50); + + // Run stats to determine cycle times. + MovingStatistics avgTime = new MovingStatistics(100); + ElapsedTime elapsedTime = new ElapsedTime(); + + while (opModeIsActive()) { + telemetry.addData(">", "Press X to Reset Encoders\n"); + + if(gamepad1.x) { + octoquad.resetAllPositions(); + } + + // read all the swerve drive encoders and update positions and velocities. + octoSwerveDrive.updateModules(); + octoSwerveDrive.show(telemetry); + + // Update cycle time stats + avgTime.add(elapsedTime.nanoseconds()); + elapsedTime.reset(); + + telemetry.addData("Loop time", "%.1f mS", avgTime.getMean()/1000000); + telemetry.update(); + } + } +} + +// ============================ Internal (Inner) Classes ============================= + +/*** + * OctoSwerveDrive class manages 4 Swerve Modules + * - Performs general OctoQuad initialization + * - Creates 4 module classes, one for each swerve module + * - Updates swerve drive status by reading all data from OctoQuad and Updating each module + * - Displays all swerve drive data as telemetry + */ +class OctoSwerveDrive { + + private final OctoQuad octoquad; + private final List allModules = new ArrayList<>(); + + // members to hold encoder data for each module. + public final OctoSwerveModule LeftFront; + public final OctoSwerveModule RightFront; + public final OctoSwerveModule LeftBack; + public final OctoSwerveModule RightBack; + + // Prepare an object to hold an entire OctoQuad encoder readable register bank (pos and vel) + private final OctoQuad.EncoderDataBlock encoderDataBlock = new OctoQuad.EncoderDataBlock(); + + public OctoSwerveDrive(OctoQuad octoquad) { + this.octoquad = octoquad; + + // Clear out all prior settings and encoder data before setting up desired configuration + octoquad.resetEverything(); + + // Assume first 4 channels are relative encoders and the next 4 are absolute encoders + octoquad.setChannelBankConfig(OctoQuad.ChannelBankConfig.BANK1_QUADRATURE_BANK2_PULSE_WIDTH); + + // Create the four OctoSwerveModules, and add them to a 'list' for easier reference. + + // Notes: + // The wheel/channel order is set here. Ensure your encoder cables match. + // + // The angleOffset must be set for each module so a forward facing wheel shows a steer + // angle of 0 degrees. The process for determining the correct angleOffsets is as follows: + // 1) Set all the angleValues (below) to zero then build and deploy the code. + // 2) Rotate all the swerve drives so the wheels are forward in the desired 0 degree position + // 3) Run the OpMode, view the telemetry and record the "Degrees" value for each module. + // 4) Update the code by entering the recorded Degrees value for each module as the + // angleOffset (last) parameter in the lines below. + // + // Rebuild and deploy the new code. Verify that the telemetry now indicates 0 degrees when + // the wheels are facing forward. Also verify that the correct module values change + // appropriately when you manually spin (drive) and rotate (steer) a wheel. + + allModules.add(LeftFront = new OctoSwerveModule(octoquad, "LF ",0,0));// Drive=0, Steer=4 + allModules.add(RightFront = new OctoSwerveModule(octoquad, "RF ",1,0));// Drive=1, Steer=5 + allModules.add(LeftBack = new OctoSwerveModule(octoquad, "LB ",2,0));// Drive=2, Steer=6 + allModules.add(RightBack = new OctoSwerveModule(octoquad, "RB ",3,0));// Drive=3, Steer=7 + + // now make sure the settings persist through any power glitches. + octoquad.saveParametersToFlash(); + } + + /** + * Updates all 4 swerve modules + */ + public void updateModules() { + // Read full OctoQuad data block and then pass it to each swerve module to update themselves. + octoquad.readAllEncoderData(encoderDataBlock); + for (OctoSwerveModule module : allModules) { + module.updateModule(encoderDataBlock); + } + } + + /** + * Generate telemetry data for all modules. + * @param telemetry OpMode Telemetry object + */ + public void show(Telemetry telemetry) { + // create general header block and then have each module add its own telemetry + telemetry.addData("pos", " Count CPS Degree DPS"); + for (OctoSwerveModule module : allModules) { + module.show(telemetry); + } + } +} + +/*** + * The OctoSwerveModule class manages a single swerve module + */ +class OctoSwerveModule { + + public double driveCounts; + public double driveCountsPerSec; + public double steerDegrees; + public double steerDegreesPerSec; + + private final String name; + private final int channel; + private final double angleOffset; + + private static final int VELOCITY_SAMPLE_INTERVAL_MS = 25; // To provide 40 updates/Sec. + private static final double DEGREES_PER_US = (360.0 / 1024.0); // REV Through Bore Encoder + private static final double VELOCITY_SAMPLES_PER_S = (1000.0 / VELOCITY_SAMPLE_INTERVAL_MS); + + // The correct drive and turn directions must be set for the Swerve Module. + // Forward motion must generate an increasing drive count. + // Counter Clockwise steer rotation must generate an increasing Steer Angle (degrees) + private static final boolean INVERT_DRIVE_ENCODER = false; + private static final boolean INVERT_STEER_ENCODER = false; + + /*** + * @param octoquad provide access to configure OctoQuad + * @param name name used for telemetry display + * @param quadChannel Quadrature encoder channel. Pulse Width channel is this + 4 + * @param angleOffset Angle to subtract from absolute encoder to calibrate zero position. + */ + public OctoSwerveModule (OctoQuad octoquad, String name, int quadChannel, double angleOffset) { + this.name = name; + this.channel = quadChannel; + this.angleOffset = angleOffset; + + // Set both encoder directions. + octoquad.setSingleEncoderDirection(channel, + INVERT_DRIVE_ENCODER ? OctoQuad.EncoderDirection.REVERSE : OctoQuad.EncoderDirection.FORWARD); + octoquad.setSingleEncoderDirection(channel + 4, + INVERT_STEER_ENCODER ? OctoQuad.EncoderDirection.REVERSE : OctoQuad.EncoderDirection.FORWARD); + + // Set the velocity sample interval on both encoders + octoquad.setSingleVelocitySampleInterval(channel, VELOCITY_SAMPLE_INTERVAL_MS); + octoquad.setSingleVelocitySampleInterval(channel + 4, VELOCITY_SAMPLE_INTERVAL_MS); + + // Setup Absolute encoder pulse range to match REV Through Bore encoder. + octoquad.setSingleChannelPulseWidthParams (channel + 4, + new OctoQuad.ChannelPulseWidthParams(1,1024)); + } + + /*** + * Calculate the Swerve module's position and velocity values + * @param encoderDataBlock most recent full data block read from OctoQuad. + */ + public void updateModule(OctoQuad.EncoderDataBlock encoderDataBlock) { + driveCounts = encoderDataBlock.positions[channel]; + driveCountsPerSec = encoderDataBlock.velocities[channel] * VELOCITY_SAMPLES_PER_S; + + // convert uS to degrees. Add in any possible direction flip. + steerDegrees = AngleUnit.normalizeDegrees( + (encoderDataBlock.positions[channel+ 4] * DEGREES_PER_US) - angleOffset); + // convert uS/interval to deg per sec. Add in any possible direction flip. + steerDegreesPerSec = encoderDataBlock.velocities[channel + 4] * + DEGREES_PER_US * VELOCITY_SAMPLES_PER_S; + } + + /** + * Display the Swerve module's state as telemetry + * @param telemetry OpMode Telemetry object + */ + public void show(Telemetry telemetry) { + telemetry.addData(name, "%8.0f %7.0f %7.0f %6.0f", + driveCounts, driveCountsPerSec, steerDegrees, steerDegreesPerSec); + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorOctoQuadLocalization.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorOctoQuadLocalization.java new file mode 100644 index 0000000..486c468 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorOctoQuadLocalization.java @@ -0,0 +1,255 @@ +/* + * Copyright (c) 2025 DigitalChickenLabs + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.hardware.digitalchickenlabs.OctoQuad; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import org.firstinspires.ftc.robotcore.external.Telemetry; + +/* + * This OpMode demonstrates how to use the "absolute localizer" feature of the + * Digital Chicken OctoQuad FTC Edition MK2. + * + * Note: The MK2 OctoQuad has an Inertial Measurement Unit (IMU) that the MK1 does not, so this + * code will ONLY work with the MK2 version. + * + * It is STRONGLY recommended to read the Quick Start guide for the localizer feature, located here: + * https://github.com/DigitalChickenLabs/OctoQuad/blob/master/documentation/ + * + * This OpMode assumes that the OctoQuad is attached to an I2C interface named "octoquad" in the + * robot configuration. + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list + * + * See the sensor's product page: https://www.tindie.com/products/37799/ + */ + +@Disabled +@TeleOp(name="OctoQuad Localizer", group="OctoQuad") +public class SensorOctoQuadLocalization extends LinearOpMode +{ + // ##################################################################################### + // + // YOU MUST ADJUST THESE CONSTANTS FOR YOUR ROBOT! + // SEE THE QUICKSTART GUIDE: + // https://github.com/DigitalChickenLabs/OctoQuad/blob/master/documentation/ + // + // AND THE TUNING OPMODES: + // https://github.com/DigitalChickenLabs/OctoQuad/blob/master/code_examples/FTC + // + // ##################################################################################### + static final int DEADWHEEL_PORT_X = 0; // encoder port on OctoQuad + static final int DEADWHEEL_PORT_Y = 1; // encoder port on OctoQuad + static final OctoQuad.EncoderDirection DEADWHEEL_X_DIR = OctoQuad.EncoderDirection.FORWARD; + static final OctoQuad.EncoderDirection DEADWHEEL_Y_DIR = OctoQuad.EncoderDirection.REVERSE; + static final float X_TICKS_PER_MM = 12.34f; // eg. 19.89f for a goBILDA 4-Bar Odometry Pod + static final float Y_TICKS_PER_MM = 12.34f; // eg. 19.89f for a goBILDA 4-Bar Odometry Pod + static final float TCP_OFFSET_X_MM = 123.4f; // eg. 147.0f from QuickStart Guide diagram + static final float TCP_OFFSET_Y_MM = 123.4f; // eg.-158.0f from QuickStart Guide diagram + static final float IMU_SCALAR = 1.0f; // Rotational scale factor. + // ##################################################################################### + + // Conversion factor for radians --> degrees + static final double RAD2DEG = 180/Math.PI; + + // For tracking the number of CRC mismatches + int badPackets = 0; + int totalPackets = 0; + boolean warn = false; + + // Data structure which will store the localizer data read from the OctoQuad + OctoQuad.LocalizerDataBlock localizer = new OctoQuad.LocalizerDataBlock(); + + /* + * Main OpMode function + */ + public void runOpMode() + { + // Begin by retrieving a handle to the device from the hardware map. + OctoQuad oq = hardwareMap.get(OctoQuad.class, "octoquad"); + + // Increase the telemetry update frequency to make the data display a bit less laggy + telemetry.setMsTransmissionInterval(50); + telemetry.setDisplayFormat(Telemetry.DisplayFormat.HTML); + + // Configure a range of parameters for the absolute localizer + // --> Read the quick start guide for an explanation of these!! + // IMPORTANT: these parameter changes will not take effect until the localizer is reset! + oq.setSingleEncoderDirection(DEADWHEEL_PORT_X, DEADWHEEL_X_DIR); + oq.setSingleEncoderDirection(DEADWHEEL_PORT_Y, DEADWHEEL_Y_DIR); + oq.setLocalizerPortX(DEADWHEEL_PORT_X); + oq.setLocalizerPortY(DEADWHEEL_PORT_Y); + oq.setLocalizerCountsPerMM_X(X_TICKS_PER_MM); + oq.setLocalizerCountsPerMM_Y(Y_TICKS_PER_MM); + oq.setLocalizerTcpOffsetMM_X(TCP_OFFSET_X_MM); + oq.setLocalizerTcpOffsetMM_Y(TCP_OFFSET_Y_MM); + oq.setLocalizerImuHeadingScalar(IMU_SCALAR); + oq.setLocalizerVelocityIntervalMS(25); + oq.setI2cRecoveryMode(OctoQuad.I2cRecoveryMode.MODE_1_PERIPH_RST_ON_FRAME_ERR); + + // Resetting the localizer will apply the parameters configured above. + // This function will NOT block until calibration of the IMU is complete - + // for that you need to look at the status returned by getLocalizerStatus() + oq.resetLocalizerAndCalibrateIMU(); + + /* + * Init Loop + */ + while (opModeInInit()) + { + telemetry.addData("OQ Firmware Version", oq.getFirmwareVersionString()); + telemetry.addData("Localizer Status", oq.getLocalizerStatus()); + telemetry.addData("Heading Axis Detection", oq.getLocalizerHeadingAxisChoice()); + telemetry.addLine(" "); + + warnIfNotTuned(); + + telemetry.addLine("Press Play to start navigating"); + telemetry.update(); + + sleep(100); + } + + /* + * Don't proceed to the main loop until calibration is complete + */ + while (!isStopRequested() && (oq.getLocalizerStatus() != OctoQuad.LocalizerStatus.RUNNING)) + { + telemetry.addLine("Waiting for IMU Calibration to complete\n"); + telemetry.addData("Localizer Status", oq.getLocalizerStatus()); + telemetry.update(); + sleep(100); + } + + // Establish a starting position for the robot. This will be 0,0,0 by default so this line + // is rather redundant, but you could change it to be anything you want + oq.setLocalizerPose(0, 0, 0); + + // Not required for localizer, but left in here since raw counts are displayed + // on telemetry for debugging + oq.resetAllPositions(); + + /* + * MAIN LOOP + */ + while (opModeIsActive()) + { + // Use the Gamepad A/Cross button to teleport to a new location and heading + if (gamepad1.crossWasPressed()) + { + oq.setLocalizerPose(200, 200, (float)(Math.PI/2.0f)); + } + + // Read updated data from the OctoQuad into the 'localizer' data structure + oq.readLocalizerData(localizer); + + // ################################################################################# + // IMPORTANT! Check whether the received CRC for the data is correct. An incorrect + // CRC indicates that the data was corrupted in transit and cannot be + // trusted. This can be caused by internal or external ESD. + // + // If the CRC is NOT reported as OK, you should throw away the data and + // try to read again. + // + // NOTE: Raw odometry pod counts are displayed to verify correct direction of rotation + // When the robot is pushed FWD, the X pod counts must increase in value + // When the robot is pushed LEFT, the Y pod counts must increase in value + // Use the setSingleEncoderDirection() method to make any reversals. + // ################################################################################# + if (localizer.crcOk) + { + warnIfNotTuned(); + + // Display Robot position data + telemetry.addData("Localizer Status", localizer.localizerStatus); + telemetry.addData("Heading deg", "%.2f", localizer.heading_rad * RAD2DEG); + telemetry.addData("Rotation dps", "%.2f", localizer.velHeading_radS * RAD2DEG); + telemetry.addData("X Pos mm", localizer.posX_mm); + telemetry.addData("Y Pos mm", localizer.posY_mm); + telemetry.addData("X Vel mm/s", localizer.velX_mmS); + telemetry.addData("Y Vel mm/s", localizer.velY_mmS); + telemetry.addLine("\nPress Gamepad A (Cross) to teleport"); + + // ############################################################################# + // IMPORTANT!! + // + // These two encoder status lines will slow the loop down, + // so they can be removed once the encoder direction has been verified. + // ############################################################################# + telemetry.addData("\nRaw X Pod Counts", oq.readSinglePosition_Caching(DEADWHEEL_PORT_X)); + telemetry.addData("Raw Y Pod Counts", oq.readSinglePosition_Caching(DEADWHEEL_PORT_Y)); + } + else + { + badPackets++; + telemetry.addLine("Data CRC not valid"); + } + totalPackets++; + + // Print some statistics about CRC validation + telemetry.addLine(String.format("%d CRC error(s) in %d packets", badPackets, totalPackets)); + + // Send updated telemetry to the Driver Station + telemetry.update(); + } + } + + long lastWarnFlashTimeMs = 0; + boolean warnFlash = false; + + void warnIfNotTuned() + { + String warnString = ""; + if (IMU_SCALAR == 1.0f) + { + warnString += "WARNING: IMU_SCALAR not tuned.
"; + warn = true; + } + if (X_TICKS_PER_MM == 12.34f || Y_TICKS_PER_MM == 12.34f) + { + warnString += "WARNING: TICKS_PER_MM not tuned.
"; + warn = true; + } + if (TCP_OFFSET_X_MM == 123.4f || TCP_OFFSET_Y_MM == 123.4f) + { + warnString += "WARNING: TCP_OFFSET not tuned.
"; + warn = true; + } + if (warn) + { + warnString +="
 - Read the code COMMENTS for tuning help.
"; + + if (System.currentTimeMillis() - lastWarnFlashTimeMs > 1000) + { + lastWarnFlashTimeMs = System.currentTimeMillis(); + warnFlash = !warnFlash; + } + + telemetry.addLine(String.format("%s", + warnFlash ? "red" : "white", warnString)); + } + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorREV2mDistance.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorREV2mDistance.java new file mode 100644 index 0000000..13883c3 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorREV2mDistance.java @@ -0,0 +1,87 @@ +/* +Copyright (c) 2018 FIRST + +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, +are permitted (subject to the limitations in the disclaimer below) provided that +the following conditions are met: + +Redistributions of source code must retain the above copyright notice, this list +of conditions and the following disclaimer. + +Redistributions in binary form must reproduce the above copyright notice, this +list of conditions and the following disclaimer in the documentation and/or +other materials provided with the distribution. + +Neither the name of FIRST nor the names of its contributors may be used to +endorse or promote products derived from this software without specific prior +written permission. + +NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS +LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR +TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF +THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. +*/ +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.hardware.rev.Rev2mDistanceSensor; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DistanceSensor; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; + +/* + * This OpMode illustrates how to use the REV Robotics 2M Distance Sensor. + * + * The OpMode assumes that the sensor is configured with a name of "sensor_distance". + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list + * + * See the sensor's product page: https://www.revrobotics.com/rev-31-1505/ + */ +@TeleOp(name = "Sensor: REV2mDistance", group = "Sensor") +@Disabled +public class SensorREV2mDistance extends LinearOpMode { + + private DistanceSensor sensorDistance; + + @Override + public void runOpMode() { + // you can use this as a regular DistanceSensor. + sensorDistance = hardwareMap.get(DistanceSensor.class, "sensor_distance"); + + // you can also cast this to a Rev2mDistanceSensor if you want to use added + // methods associated with the Rev2mDistanceSensor class. + Rev2mDistanceSensor sensorTimeOfFlight = (Rev2mDistanceSensor) sensorDistance; + + telemetry.addData(">>", "Press start to continue"); + telemetry.update(); + + waitForStart(); + while(opModeIsActive()) { + // generic DistanceSensor methods. + telemetry.addData("deviceName", sensorDistance.getDeviceName() ); + telemetry.addData("range", String.format("%.01f mm", sensorDistance.getDistance(DistanceUnit.MM))); + telemetry.addData("range", String.format("%.01f cm", sensorDistance.getDistance(DistanceUnit.CM))); + telemetry.addData("range", String.format("%.01f m", sensorDistance.getDistance(DistanceUnit.METER))); + telemetry.addData("range", String.format("%.01f in", sensorDistance.getDistance(DistanceUnit.INCH))); + + // Rev2mDistanceSensor specific methods. + telemetry.addData("ID", String.format("%x", sensorTimeOfFlight.getModelID())); + telemetry.addData("did time out", Boolean.toString(sensorTimeOfFlight.didTimeoutOccur())); + + telemetry.update(); + } + } + +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorSparkFunOTOS.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorSparkFunOTOS.java new file mode 100644 index 0000000..3a25230 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorSparkFunOTOS.java @@ -0,0 +1,156 @@ +/* + SPDX-License-Identifier: MIT + + Copyright (c) 2024 SparkFun Electronics +*/ +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import com.qualcomm.hardware.sparkfun.SparkFunOTOS; + +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; + +/* + * This OpMode illustrates how to use the SparkFun Qwiic Optical Tracking Odometry Sensor (OTOS) + * + * The OpMode assumes that the sensor is configured with a name of "sensor_otos". + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list + * + * See the sensor's product page: https://www.sparkfun.com/products/24904 + */ +@TeleOp(name = "Sensor: SparkFun OTOS", group = "Sensor") +@Disabled +public class SensorSparkFunOTOS extends LinearOpMode { + // Create an instance of the sensor + SparkFunOTOS myOtos; + + @Override + public void runOpMode() throws InterruptedException { + // Get a reference to the sensor + myOtos = hardwareMap.get(SparkFunOTOS.class, "sensor_otos"); + + // All the configuration for the OTOS is done in this helper method, check it out! + configureOtos(); + + // Wait for the start button to be pressed + waitForStart(); + + // Loop until the OpMode ends + while (opModeIsActive()) { + // Get the latest position, which includes the x and y coordinates, plus the + // heading angle + SparkFunOTOS.Pose2D pos = myOtos.getPosition(); + + // Reset the tracking if the user requests it + if (gamepad1.y) { + myOtos.resetTracking(); + } + + // Re-calibrate the IMU if the user requests it + if (gamepad1.x) { + myOtos.calibrateImu(); + } + + // Inform user of available controls + telemetry.addLine("Press Y (triangle) on Gamepad to reset tracking"); + telemetry.addLine("Press X (square) on Gamepad to calibrate the IMU"); + telemetry.addLine(); + + // Log the position to the telemetry + telemetry.addData("X coordinate", pos.x); + telemetry.addData("Y coordinate", pos.y); + telemetry.addData("Heading angle", pos.h); + + // Update the telemetry on the driver station + telemetry.update(); + } + } + + private void configureOtos() { + telemetry.addLine("Configuring OTOS..."); + telemetry.update(); + + // Set the desired units for linear and angular measurements. Can be either + // meters or inches for linear, and radians or degrees for angular. If not + // set, the default is inches and degrees. Note that this setting is not + // persisted in the sensor, so you need to set at the start of all your + // OpModes if using the non-default value. + // myOtos.setLinearUnit(DistanceUnit.METER); + myOtos.setLinearUnit(DistanceUnit.INCH); + // myOtos.setAngularUnit(AnguleUnit.RADIANS); + myOtos.setAngularUnit(AngleUnit.DEGREES); + + // Assuming you've mounted your sensor to a robot and it's not centered, + // you can specify the offset for the sensor relative to the center of the + // robot. The units default to inches and degrees, but if you want to use + // different units, specify them before setting the offset! Note that as of + // firmware version 1.0, these values will be lost after a power cycle, so + // you will need to set them each time you power up the sensor. For example, if + // the sensor is mounted 5 inches to the left (negative X) and 10 inches + // forward (positive Y) of the center of the robot, and mounted 90 degrees + // clockwise (negative rotation) from the robot's orientation, the offset + // would be {-5, 10, -90}. These can be any value, even the angle can be + // tweaked slightly to compensate for imperfect mounting (eg. 1.3 degrees). + SparkFunOTOS.Pose2D offset = new SparkFunOTOS.Pose2D(0, 0, 0); + myOtos.setOffset(offset); + + // Here we can set the linear and angular scalars, which can compensate for + // scaling issues with the sensor measurements. Note that as of firmware + // version 1.0, these values will be lost after a power cycle, so you will + // need to set them each time you power up the sensor. They can be any value + // from 0.872 to 1.127 in increments of 0.001 (0.1%). It is recommended to + // first set both scalars to 1.0, then calibrate the angular scalar, then + // the linear scalar. To calibrate the angular scalar, spin the robot by + // multiple rotations (eg. 10) to get a precise error, then set the scalar + // to the inverse of the error. Remember that the angle wraps from -180 to + // 180 degrees, so for example, if after 10 rotations counterclockwise + // (positive rotation), the sensor reports -15 degrees, the required scalar + // would be 3600/3585 = 1.004. To calibrate the linear scalar, move the + // robot a known distance and measure the error; do this multiple times at + // multiple speeds to get an average, then set the linear scalar to the + // inverse of the error. For example, if you move the robot 100 inches and + // the sensor reports 103 inches, set the linear scalar to 100/103 = 0.971 + myOtos.setLinearScalar(1.0); + myOtos.setAngularScalar(1.0); + + // The IMU on the OTOS includes a gyroscope and accelerometer, which could + // have an offset. Note that as of firmware version 1.0, the calibration + // will be lost after a power cycle; the OTOS performs a quick calibration + // when it powers up, but it is recommended to perform a more thorough + // calibration at the start of all your OpModes. Note that the sensor must + // be completely stationary and flat during calibration! When calling + // calibrateImu(), you can specify the number of samples to take and whether + // to wait until the calibration is complete. If no parameters are provided, + // it will take 255 samples and wait until done; each sample takes about + // 2.4ms, so about 612ms total + myOtos.calibrateImu(); + + // Reset the tracking algorithm - this resets the position to the origin, + // but can also be used to recover from some rare tracking errors + myOtos.resetTracking(); + + // After resetting the tracking, the OTOS will report that the robot is at + // the origin. If your robot does not start at the origin, or you have + // another source of location information (eg. vision odometry), you can set + // the OTOS location to match and it will continue to track from there. + SparkFunOTOS.Pose2D currentPosition = new SparkFunOTOS.Pose2D(0, 0, 0); + myOtos.setPosition(currentPosition); + + // Get the hardware and firmware version + SparkFunOTOS.Version hwVersion = new SparkFunOTOS.Version(); + SparkFunOTOS.Version fwVersion = new SparkFunOTOS.Version(); + myOtos.getVersionInfo(hwVersion, fwVersion); + + telemetry.addLine("OTOS configured! Press start to get position data!"); + telemetry.addLine(); + telemetry.addLine(String.format("OTOS Hardware Version: v%d.%d", hwVersion.major, hwVersion.minor)); + telemetry.addLine(String.format("OTOS Firmware Version: v%d.%d", fwVersion.major, fwVersion.minor)); + telemetry.update(); + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorTouch.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorTouch.java new file mode 100644 index 0000000..3d79447 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorTouch.java @@ -0,0 +1,78 @@ +/* Copyright (c) 2017 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.TouchSensor; + +/* + * This OpMode demonstrates how to use a REV Robotics Touch Sensor, REV Robotics Magnetic Limit Switch, or other device + * that implements the TouchSensor interface. Any touch sensor that connects its output to ground when pressed + * (known as "active low") can be configured as a "REV Touch Sensor". This includes REV's Magnetic Limit Switch. + * + * The OpMode assumes that the touch sensor is configured with a name of "sensor_touch". + * + * A REV Robotics Touch Sensor must be configured on digital port number 1, 3, 5, or 7. + * A Magnetic Limit Switch can be configured on any digital port. + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list. + */ +@TeleOp(name = "Sensor: REV touch sensor", group = "Sensor") +@Disabled +public class SensorTouch extends LinearOpMode { + TouchSensor touchSensor; // Touch sensor Object + + @Override + public void runOpMode() { + + // get a reference to our touchSensor object. + touchSensor = hardwareMap.get(TouchSensor.class, "sensor_touch"); + + // wait for the start button to be pressed. + waitForStart(); + + // while the OpMode is active, loop and read whether the sensor is being pressed. + // Note we use opModeIsActive() as our loop condition because it is an interruptible method. + while (opModeIsActive()) { + + // send the info back to driver station using telemetry function. + if (touchSensor.isPressed()) { + telemetry.addData("Touch Sensor", "Is Pressed"); + } else { + telemetry.addData("Touch Sensor", "Is Not Pressed"); + } + + telemetry.update(); + } + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/UtilityCameraFrameCapture.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/UtilityCameraFrameCapture.java new file mode 100644 index 0000000..69420cc --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/UtilityCameraFrameCapture.java @@ -0,0 +1,127 @@ +/* + * Copyright (c) 2023 FIRST + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to + * endorse or promote products derived from this software without specific prior + * written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR + * TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF + * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import android.util.Size; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import org.firstinspires.ftc.robotcore.external.hardware.camera.BuiltinCameraDirection; +import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName; +import org.firstinspires.ftc.vision.VisionPortal; + +import java.util.Locale; + +/* + * This OpMode helps calibrate a webcam or RC phone camera, useful for AprilTag pose estimation + * with the FTC VisionPortal. It captures a camera frame (image) and stores it on the Robot Controller + * (Control Hub or RC phone), with each press of the gamepad button X (or Square). + * Full calibration instructions are here: + * + * https://ftc-docs.firstinspires.org/camera-calibration + * + * In Android Studio, copy this class into your "teamcode" folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list. + * + * In OnBot Java, use "Add File" to add this OpMode from the list of Samples. + */ + +@TeleOp(name = "Utility: Camera Frame Capture", group = "Utility") +@Disabled +public class UtilityCameraFrameCapture extends LinearOpMode +{ + /* + * EDIT THESE PARAMETERS AS NEEDED + */ + final boolean USING_WEBCAM = false; + final BuiltinCameraDirection INTERNAL_CAM_DIR = BuiltinCameraDirection.BACK; + final int RESOLUTION_WIDTH = 640; + final int RESOLUTION_HEIGHT = 480; + + // Internal state + boolean lastX; + int frameCount; + long capReqTime; + + @Override + public void runOpMode() + { + VisionPortal portal; + + if (USING_WEBCAM) + { + portal = new VisionPortal.Builder() + .setCamera(hardwareMap.get(WebcamName.class, "Webcam 1")) + .setCameraResolution(new Size(RESOLUTION_WIDTH, RESOLUTION_HEIGHT)) + .build(); + } + else + { + portal = new VisionPortal.Builder() + .setCamera(INTERNAL_CAM_DIR) + .setCameraResolution(new Size(RESOLUTION_WIDTH, RESOLUTION_HEIGHT)) + .build(); + } + + while (!isStopRequested()) + { + boolean x = gamepad1.x; + + if (x && !lastX) + { + portal.saveNextFrameRaw(String.format(Locale.US, "CameraFrameCapture-%06d", frameCount++)); + capReqTime = System.currentTimeMillis(); + } + + lastX = x; + + telemetry.addLine("######## Camera Capture Utility ########"); + telemetry.addLine(String.format(Locale.US, " > Resolution: %dx%d", RESOLUTION_WIDTH, RESOLUTION_HEIGHT)); + telemetry.addLine(" > Press X (or Square) to capture a frame"); + telemetry.addData(" > Camera Status", portal.getCameraState()); + + if (capReqTime != 0) + { + telemetry.addLine("\nCaptured Frame!"); + } + + if (capReqTime != 0 && System.currentTimeMillis() - capReqTime > 1000) + { + capReqTime = 0; + } + + telemetry.update(); + } + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/UtilityOctoQuadConfigMenu.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/UtilityOctoQuadConfigMenu.java new file mode 100644 index 0000000..60be6c9 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/UtilityOctoQuadConfigMenu.java @@ -0,0 +1,821 @@ +/* + * Copyright (c) 2024 DigitalChickenLabs + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples; + +import com.qualcomm.hardware.digitalchickenlabs.OctoQuad; +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.Gamepad; + +import org.firstinspires.ftc.robotcore.external.Telemetry; + +import java.util.ArrayList; +import java.util.Stack; + +/* + * This OpMode illustrates how to use the DigitalChickenLabs OctoQuad Quadrature Encoder & Pulse Width Interface Module. + * + * The OctoQuad has 8 input channels that can used to read either Relative Quadrature Encoders or Pulse-Width Absolute Encoder inputs. + * Relative Quadrature encoders are found on most FTC motors, and some stand-alone position sensors like the REV Thru-Bore encoder. + * Pulse-Width encoders are less common. The REV Thru-Bore encoder can provide its absolute position via a variable pulse width, + * as can several sonar rangefinders such as the MaxBotix MB1000 series. + * + * This OpMode assumes that the OctoQuad is attached to an I2C interface named "octoquad" in the robot configuration. + * + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list + * + * Select, Init and run the "OctoQuad Configuration Tool" OpMode + * Read the blue User-Interface tips at the top of the telemetry screen. + * Use the UI buttons to navigate the menu and make any desired changes to the OctoQuad configuration. + * Use the Program Settings To FLASH option to make any changes permanent. + * + * See the sensor's product page: https://www.tindie.com/products/digitalchickenlabs/octoquad-8ch-quadrature-pulse-width-decoder/ + */ +@TeleOp(name = "OctoQuad Configuration Tool", group="OctoQuad") +@Disabled +public class UtilityOctoQuadConfigMenu extends LinearOpMode +{ + TelemetryMenu.MenuElement rootMenu = new TelemetryMenu.MenuElement("OctoQuad Config Menu", true); + TelemetryMenu.MenuElement menuHwInfo = new TelemetryMenu.MenuElement("Hardware Information", false); + TelemetryMenu.EnumOption optionI2cResetMode; + TelemetryMenu.EnumOption optionChannelBankConfig; + + TelemetryMenu.MenuElement menuEncoderDirections = new TelemetryMenu.MenuElement("Set Encoder Directions", false); + TelemetryMenu.BooleanOption[] optionsEncoderDirections = new TelemetryMenu.BooleanOption[OctoQuad.NUM_ENCODERS]; + + TelemetryMenu.MenuElement menuVelocityIntervals = new TelemetryMenu.MenuElement("Velocity Measurement Intervals", false); + TelemetryMenu.IntegerOption[] optionsVelocityIntervals = new TelemetryMenu.IntegerOption[OctoQuad.NUM_ENCODERS]; + + TelemetryMenu.MenuElement menuAbsParams = new TelemetryMenu.MenuElement("Abs. Encoder Pulse Width Params", false); + TelemetryMenu.IntegerOption[] optionsAbsParamsMax = new TelemetryMenu.IntegerOption[OctoQuad.NUM_ENCODERS]; + TelemetryMenu.IntegerOption[] optionsAbsParamsMin = new TelemetryMenu.IntegerOption[OctoQuad.NUM_ENCODERS]; + TelemetryMenu.BooleanOption[] optionsAbsParamsWrapTracking = new TelemetryMenu.BooleanOption[OctoQuad.NUM_ENCODERS]; + + TelemetryMenu.OptionElement optionProgramToFlash; + TelemetryMenu.OptionElement optionSendToRAM; + + TelemetryMenu.StaticClickableOption optionExit; + + OctoQuad octoquad; + + boolean error = false; + + @Override + public void runOpMode() + { + octoquad = hardwareMap.getAll(OctoQuad.class).get(0); + + if(octoquad.getChipId() != OctoQuad.OCTOQUAD_CHIP_ID) + { + telemetry.addLine("Error: cannot communicate with OctoQuad. Check your wiring and configuration and try again"); + telemetry.update(); + + error = true; + } + else + { + if(octoquad.getFirmwareVersion().maj != OctoQuad.SUPPORTED_FW_VERSION_MAJ) + { + telemetry.addLine("Error: The OctoQuad is running a different major firmware version than this driver was built for. Cannot run configuration tool"); + telemetry.update(); + + error = true; + } + } + + if(error) + { + waitForStart(); + return; + } + + telemetry.addLine("Retrieving current configuration from OctoQuad"); + telemetry.update(); + + optionExit = new TelemetryMenu.StaticClickableOption("Exit configuration menu") + { + @Override + void onClick() // called on OpMode thread + { + requestOpModeStop(); + } + }; + + optionI2cResetMode = new TelemetryMenu.EnumOption("I2C Reset Mode", OctoQuad.I2cRecoveryMode.values(), octoquad.getI2cRecoveryMode()); + optionChannelBankConfig = new TelemetryMenu.EnumOption("Channel Bank Modes", OctoQuad.ChannelBankConfig.values(), octoquad.getChannelBankConfig()); + + menuHwInfo.addChild(new TelemetryMenu.StaticItem("Board Firmware: v" + octoquad.getFirmwareVersion())); + //menuHwInfo.addChild(new TelemetryMenu.StaticItem("Board unique ID: FIXME")); + + for(int i = 0; i < OctoQuad.NUM_ENCODERS; i++) + { + optionsEncoderDirections[i] = new TelemetryMenu.BooleanOption( + String.format("Encoder %d direction", i), + octoquad.getSingleEncoderDirection(i) == OctoQuad.EncoderDirection.REVERSE, + "-", + "+"); + } + menuEncoderDirections.addChildren(optionsEncoderDirections); + + for(int i = 0; i < OctoQuad.NUM_ENCODERS; i++) + { + optionsVelocityIntervals[i] = new TelemetryMenu.IntegerOption( + String.format("Chan %d velocity intvl", i), + OctoQuad.MIN_VELOCITY_MEASUREMENT_INTERVAL_MS, + OctoQuad.MAX_VELOCITY_MEASUREMENT_INTERVAL_MS, + octoquad.getSingleVelocitySampleInterval(i)); + } + menuVelocityIntervals.addChildren(optionsVelocityIntervals); + + for(int i = 0; i < OctoQuad.NUM_ENCODERS; i++) + { + OctoQuad.ChannelPulseWidthParams params = octoquad.getSingleChannelPulseWidthParams(i); + + optionsAbsParamsMax[i] = new TelemetryMenu.IntegerOption( + String.format("Chan %d max pulse length", i), + OctoQuad.MIN_PULSE_WIDTH_US, + OctoQuad.MAX_PULSE_WIDTH_US, + params.max_length_us); + + optionsAbsParamsMin[i] = new TelemetryMenu.IntegerOption( + String.format("Chan %d min pulse length", i), + OctoQuad.MIN_PULSE_WIDTH_US, + OctoQuad.MAX_PULSE_WIDTH_US, + params.min_length_us); + + optionsAbsParamsWrapTracking[i] = new TelemetryMenu.BooleanOption( + String.format("Chan %d wrap tracking enabled", i), + octoquad.getSingleChannelPulseWidthTracksWrap(i), + "yes", + "no"); + } + menuAbsParams.addChildren(optionsAbsParamsMin); + menuAbsParams.addChildren(optionsAbsParamsMax); + menuAbsParams.addChildren(optionsAbsParamsWrapTracking); + + optionProgramToFlash = new TelemetryMenu.OptionElement() + { + String name = "Program Settings to FLASH"; + long lastClickTime = 0; + + @Override + protected String getDisplayText() + { + if(lastClickTime == 0) + { + return name; + } + else + { + if(System.currentTimeMillis() - lastClickTime < 1000) + { + return name + " **OK**"; + } + else + { + lastClickTime = 0; + return name; + } + } + } + + @Override + void onClick() + { + sendSettingsToRam(); + octoquad.saveParametersToFlash(); + lastClickTime = System.currentTimeMillis(); + } + }; + + optionSendToRAM = new TelemetryMenu.OptionElement() + { + String name = "Send Settings to RAM"; + long lastClickTime = 0; + + @Override + protected String getDisplayText() + { + if(lastClickTime == 0) + { + return name; + } + else + { + if(System.currentTimeMillis() - lastClickTime < 1000) + { + return name + " **OK**"; + } + else + { + lastClickTime = 0; + return name; + } + } + } + + @Override + void onClick() + { + sendSettingsToRam(); + lastClickTime = System.currentTimeMillis(); + } + }; + + rootMenu.addChild(menuHwInfo); + rootMenu.addChild(optionI2cResetMode); + rootMenu.addChild(optionChannelBankConfig); + rootMenu.addChild(menuEncoderDirections); + rootMenu.addChild(menuVelocityIntervals); + rootMenu.addChild(menuAbsParams); + rootMenu.addChild(optionProgramToFlash); + rootMenu.addChild(optionSendToRAM); + rootMenu.addChild(optionExit); + + TelemetryMenu menu = new TelemetryMenu(telemetry, rootMenu); + + while (!isStopRequested()) + { + menu.loop(gamepad1); + telemetry.update(); + sleep(20); + } + } + + void sendSettingsToRam() + { + for(int i = 0; i < OctoQuad.NUM_ENCODERS; i++) + { + octoquad.setSingleEncoderDirection(i, optionsEncoderDirections[i].getValue() ? OctoQuad.EncoderDirection.REVERSE : OctoQuad.EncoderDirection.FORWARD); + octoquad.setSingleVelocitySampleInterval(i, optionsVelocityIntervals[i].getValue()); + + OctoQuad.ChannelPulseWidthParams params = new OctoQuad.ChannelPulseWidthParams(); + params.max_length_us = optionsAbsParamsMax[i].getValue(); + params.min_length_us = optionsAbsParamsMin[i].getValue(); + + octoquad.setSingleChannelPulseWidthParams(i, params); + octoquad.setSingleChannelPulseWidthTracksWrap(i, optionsAbsParamsWrapTracking[i].val); + } + + octoquad.setI2cRecoveryMode((OctoQuad.I2cRecoveryMode) optionI2cResetMode.getValue()); + octoquad.setChannelBankConfig((OctoQuad.ChannelBankConfig) optionChannelBankConfig.getValue()); + } + + /* + * Copyright (c) 2023 OpenFTC Team + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in all + * copies or substantial portions of the Software. + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + + public static class TelemetryMenu + { + private final MenuElement root; + private MenuElement currentLevel; + + private boolean dpadUpPrev; + private boolean dpadDnPrev; + private boolean dpadRightPrev; + private boolean dpadLeftPrev; + private boolean xPrev; + private boolean lbPrev; + + private int selectedIdx = 0; + private Stack selectedIdxStack = new Stack<>(); + + private final Telemetry telemetry; + + /** + * TelemetryMenu constructor + * @param telemetry pass in 'telemetry' from your OpMode + * @param root the root menu element + */ + public TelemetryMenu(Telemetry telemetry, MenuElement root) + { + this.root = root; + this.currentLevel = root; + this.telemetry = telemetry; + + telemetry.setDisplayFormat(Telemetry.DisplayFormat.HTML); + telemetry.setMsTransmissionInterval(50); + } + + /** + * Call this from inside your loop to put the current menu state into + * telemetry, and process gamepad inputs for navigating the menu + * @param gamepad the gamepad you want to use to navigate the menu + */ + public void loop(Gamepad gamepad) + { + // Capture current state of the gamepad buttons we care about; + // We can only look once or we risk a race condition + boolean dpadUp = gamepad.dpad_up; + boolean dpadDn = gamepad.dpad_down; + boolean dpadRight = gamepad.dpad_right; + boolean dpadLeft = gamepad.dpad_left; + boolean x = gamepad.x; + boolean lb = gamepad.left_bumper; + + // Figure out who our children our at this level + // and figure out which item is currently highlighted + // with the selection pointer + ArrayList children = currentLevel.children(); + Element currentSelection = children.get(selectedIdx); + + // Left and right are inputs to the selected item (if it's an Option) + if (currentSelection instanceof OptionElement) + { + if (dpadRight && !dpadRightPrev) // rising edge + { + ((OptionElement) currentSelection).onRightInput(); + } + else if (dpadLeft && !dpadLeftPrev) // rising edge + { + ((OptionElement) currentSelection).onLeftInput(); + } + } + + // Up and down navigate the current selection pointer + if (dpadUp && !dpadUpPrev) // rising edge + { + selectedIdx--; // Move selection pointer up + } + else if (dpadDn && !dpadDnPrev) // rising edge + { + selectedIdx++; // Move selection pointer down + } + + // Make selected index sane (don't let it go out of bounds) :eyes: + if (selectedIdx >= children.size()) + { + selectedIdx = children.size()-1; + } + else if (selectedIdx < 0) + { + selectedIdx = 0; + } + + // Select: either enter submenu or input to option + else if (x && !xPrev) // rising edge + { + // Select up element + if (currentSelection instanceof SpecialUpElement) + { + // We can only go up if we're not at the root level + if (currentLevel != root) + { + // Restore selection pointer to where it was before + selectedIdx = selectedIdxStack.pop(); + + // Change to the parent level + currentLevel = currentLevel.parent(); + } + } + // Input to option + else if (currentSelection instanceof OptionElement) + { + ((OptionElement) currentSelection).onClick(); + } + // Enter submenu + else if (currentSelection instanceof MenuElement) + { + // Save our current selection pointer so we can restore it + // later if the user navigates back up a level + selectedIdxStack.push(selectedIdx); + + // We have no idea what's in the submenu :monkey: so best to + // just set the selection pointer to the first element + selectedIdx = 0; + + // Now the current level becomes the submenu that the selection + // pointer was on + currentLevel = (MenuElement) currentSelection; + } + } + + // Go up a level + else if (lb && !lbPrev) + { + // We can only go up if we're not at the root level + if (currentLevel != root) + { + // Restore selection pointer to where it was before + selectedIdx = selectedIdxStack.pop(); + + // Change to the parent level + currentLevel = currentLevel.parent(); + } + } + + // Save the current button states so that we can look for + // the rising edge the next time around the loop :) + dpadUpPrev = dpadUp; + dpadDnPrev = dpadDn; + dpadRightPrev = dpadRight; + dpadLeftPrev = dpadLeft; + xPrev = x; + lbPrev = lb; + + // Start building the text display. + // First, we add the static directions for gamepad operation + StringBuilder builder = new StringBuilder(); + builder.append(""); + builder.append("Navigate items.....dpad up/down\n") + .append("Select.............X or Square\n") + .append("Edit option........dpad left/right\n") + .append("Up one level.......left bumper\n"); + builder.append(""); + builder.append("\n"); + + // Now actually add the menu options. We start by adding the name of the current menu level. + builder.append(""); + builder.append("Current Menu: ").append(currentLevel.name).append("\n"); + + // Now we loop through all the child elements of this level and add them + for (int i = 0; i < children.size(); i++) + { + // If the selection pointer is at this index, put a green dot in the box :) + if (selectedIdx == i) + { + builder.append("[•] "); + } + // Otherwise, just put an empty box + else + { + builder.append("[ ] "); + } + + // Figure out who the selection pointer is pointing at :eyes: + Element e = children.get(i); + + // If it's pointing at a submenu, indicate that it's a submenu to the user + // by prefixing "> " to the name. + if (e instanceof MenuElement) + { + builder.append("> "); + } + + // Finally, add the element's name + builder.append(e.getDisplayText()); + + // We musn't forget the newline + builder.append("\n"); + } + + // Don't forget to close the font tag either + builder.append(""); + + // Build the string!!!! :nerd: + String menu = builder.toString(); + + // Add it to telemetry + telemetry.addLine(menu); + } + + public static class MenuElement extends Element + { + private String name; + private ArrayList children = new ArrayList<>(); + + /** + * Create a new MenuElement; may either be the root menu, or a submenu (set isRoot accordingly) + * @param name the name for this menu + * @param isRoot whether this is a root menu, or a submenu + */ + public MenuElement(String name, boolean isRoot) + { + this.name = name; + + // If it's not the root menu, we add the up one level option as the first element + if (!isRoot) + { + children.add(new SpecialUpElement()); + } + } + + /** + * Add a child element to this menu (may either be an Option or another menu) + * @param child the child element to add + */ + public void addChild(Element child) + { + child.setParent(this); + children.add(child); + } + + /** + * Add multiple child elements to this menu (may either be option, or another menu) + * @param children the children to add + */ + public void addChildren(Element[] children) + { + for (Element e : children) + { + e.setParent(this); + this.children.add(e); + } + } + + @Override + protected String getDisplayText() + { + return name; + } + + private ArrayList children() + { + return children; + } + } + + public static abstract class OptionElement extends Element + { + /** + * Override this to get notified when the element is clicked + */ + void onClick() {} + + /** + * Override this to get notified when the element gets a "left edit" input + */ + protected void onLeftInput() {} + + /** + * Override this to get notified when the element gets a "right edit" input + */ + protected void onRightInput() {} + } + + public static class EnumOption extends OptionElement + { + protected int idx = 0; + protected Enum[] e; + protected String name; + + public EnumOption(String name, Enum[] e) + { + this.e = e; + this.name = name; + } + + public EnumOption(String name, Enum[] e, Enum def) + { + this(name, e); + idx = def.ordinal(); + } + + @Override + public void onLeftInput() + { + idx++; + + if(idx > e.length-1) + { + idx = 0; + } + } + + @Override + public void onRightInput() + { + idx--; + + if(idx < 0) + { + idx = e.length-1; + } + } + + @Override + public void onClick() + { + //onRightInput(); + } + + @Override + protected String getDisplayText() + { + return String.format("%s: %s", name, e[idx].name()); + } + + public Enum getValue() + { + return e[idx]; + } + } + + public static class IntegerOption extends OptionElement + { + protected int i; + protected int min; + protected int max; + protected String name; + + public IntegerOption(String name, int min, int max, int def) + { + this.name = name; + this.min = min; + this.max = max; + this.i = def; + } + + @Override + public void onLeftInput() + { + i--; + + if(i < min) + { + i = max; + } + } + + @Override + public void onRightInput() + { + i++; + + if(i > max) + { + i = min; + } + } + + @Override + public void onClick() + { + //onRightInput(); + } + + @Override + protected String getDisplayText() + { + return String.format("%s: %d", name, i); + } + + public int getValue() + { + return i; + } + } + + static class BooleanOption extends OptionElement + { + private String name; + private boolean val = true; + + private String customTrue; + private String customFalse; + + BooleanOption(String name, boolean def) + { + this.name = name; + this.val = def; + } + + BooleanOption(String name, boolean def, String customTrue, String customFalse) + { + this(name, def); + this.customTrue = customTrue; + this.customFalse = customFalse; + } + + @Override + public void onLeftInput() + { + val = !val; + } + + @Override + public void onRightInput() + { + val = !val; + } + + @Override + public void onClick() + { + val = !val; + } + + @Override + protected String getDisplayText() + { + String valStr; + + if(customTrue != null && customFalse != null) + { + valStr = val ? customTrue : customFalse; + } + else + { + valStr = val ? "true" : "false"; + } + + return String.format("%s: %s", name, valStr); + } + + public boolean getValue() + { + return val; + } + } + + /** + * + */ + public static class StaticItem extends OptionElement + { + private String name; + + public StaticItem(String name) + { + this.name = name; + } + + @Override + protected String getDisplayText() + { + return name; + } + } + + public static abstract class StaticClickableOption extends OptionElement + { + private String name; + + public StaticClickableOption(String name) + { + this.name = name; + } + + abstract void onClick(); + + @Override + protected String getDisplayText() + { + return name; + } + } + + private static abstract class Element + { + private MenuElement parent; + + protected void setParent(MenuElement parent) + { + this.parent = parent; + } + + protected MenuElement parent() + { + return parent; + } + + protected abstract String getDisplayText(); + } + + private static class SpecialUpElement extends Element + { + @Override + protected String getDisplayText() + { + return ".. ↰ Up One Level"; + } + } + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/externalhardware/ConceptExternalHardwareClass.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/externalhardware/ConceptExternalHardwareClass.java new file mode 100644 index 0000000..cee35f6 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/externalhardware/ConceptExternalHardwareClass.java @@ -0,0 +1,142 @@ +/* Copyright (c) 2022 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples.externalhardware; + +import com.qualcomm.robotcore.eventloop.opmode.Disabled; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.util.Range; + +/* + * This OpMode illustrates how to use an external "hardware" class to modularize all the robot's sensors and actuators. + * This approach is very efficient because the same hardware class can be used by all of your teleop and autonomous OpModes + * without requiring many copy & paste operations. Once you have defined and tested the hardware class with one OpMode, + * it is instantly available to other OpModes. + * + * The real benefit of this approach is that as you tweak your robot hardware, you only need to make changes in ONE place (the Hardware Class). + * So, to be effective you should put as much or your hardware setup and access code as possible in the hardware class. + * Essentially anything you do with hardware in BOTH Teleop and Auto should likely go in the hardware class. + * + * The Hardware Class is created in a separate file, and then an "instance" of this class is created in each OpMode. + * In order for the class to do typical OpMode things (like send telemetry data) it must be passed a reference to the + * OpMode object when it's created, so it can access all core OpMode functions. This is illustrated below. + * + * In this concept sample, the hardware class file is called RobotHardware.java and it must accompany this sample OpMode. + * So, if you copy ConceptExternalHardwareClass.java into TeamCode (using Android Studio or OnBotJava) then RobotHardware.java + * must also be copied to the same location (maintaining its name). + * + * For comparison purposes, this sample and its accompanying hardware class duplicates the functionality of the + * RobotTelopPOV_Linear OpMode. It assumes three motors (left_drive, right_drive and arm) and two servos (left_hand and right_hand) + * + * View the RobotHardware.java class file for more details + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name. + * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list + * + * In OnBot Java, add a new OpMode, select this sample, and select TeleOp. + * Also add another new file named RobotHardware.java, select the sample with that name, and select Not an OpMode. + */ + +@TeleOp(name="Concept: Robot Hardware Class", group="Robot") +@Disabled +public class ConceptExternalHardwareClass extends LinearOpMode { + + // Create a RobotHardware object to be used to access robot hardware. + // Prefix any hardware functions with "robot." to access this class. + RobotHardware robot = new RobotHardware(this); + + @Override + public void runOpMode() { + double drive = 0; + double turn = 0; + double arm = 0; + double handOffset = 0; + + // initialize all the hardware, using the hardware class. See how clean and simple this is? + robot.init(); + + // Send telemetry message to signify robot waiting; + // Wait for the game to start (driver presses START) + waitForStart(); + + // run until the end of the match (driver presses STOP) + while (opModeIsActive()) { + + // Run wheels in POV mode (note: The joystick goes negative when pushed forward, so negate it) + // In this mode the Left stick moves the robot fwd and back, the Right stick turns left and right. + // This way it's also easy to just drive straight, or just turn. + drive = -gamepad1.left_stick_y; + turn = gamepad1.right_stick_x; + + // Combine drive and turn for blended motion. Use RobotHardware class + robot.driveRobot(drive, turn); + + // Use gamepad left & right Bumpers to open and close the claw + // Use the SERVO constants defined in RobotHardware class. + // Each time around the loop, the servos will move by a small amount. + // Limit the total offset to half of the full travel range + if (gamepad1.right_bumper) + handOffset += robot.HAND_SPEED; + else if (gamepad1.left_bumper) + handOffset -= robot.HAND_SPEED; + handOffset = Range.clip(handOffset, -0.5, 0.5); + + // Move both servos to new position. Use RobotHardware class + robot.setHandPositions(handOffset); + + // Use gamepad buttons to move arm up (Y) and down (A) + // Use the MOTOR constants defined in RobotHardware class. + if (gamepad1.y) + arm = robot.ARM_UP_POWER; + else if (gamepad1.a) + arm = robot.ARM_DOWN_POWER; + else + arm = 0; + + robot.setArmPower(arm); + + // Send telemetry messages to explain controls and show robot status + telemetry.addData("Drive", "Left Stick"); + telemetry.addData("Turn", "Right Stick"); + telemetry.addData("Arm Up/Down", "Y & A Buttons"); + telemetry.addData("Hand Open/Closed", "Left and Right Bumpers"); + telemetry.addData("-", "-------"); + + telemetry.addData("Drive Power", "%.2f", drive); + telemetry.addData("Turn Power", "%.2f", turn); + telemetry.addData("Arm Power", "%.2f", arm); + telemetry.addData("Hand Position", "Offset = %.2f", handOffset); + telemetry.update(); + + // Pace this loop so hands move at a reasonable speed. + sleep(50); + } + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/externalhardware/RobotHardware.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/externalhardware/RobotHardware.java new file mode 100644 index 0000000..64f2206 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/externalhardware/RobotHardware.java @@ -0,0 +1,167 @@ +/* Copyright (c) 2022 FIRST. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, + * are permitted (subject to the limitations in the disclaimer below) provided that + * the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this list + * of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this + * list of conditions and the following disclaimer in the documentation and/or + * other materials provided with the distribution. + * + * Neither the name of FIRST nor the names of its contributors may be used to endorse or + * promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS + * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, + * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + +package org.firstinspires.ftc.robotcontroller.external.samples.externalhardware; + +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.Servo; +import com.qualcomm.robotcore.util.Range; + +/* + * This file works in conjunction with the External Hardware Class sample called: ConceptExternalHardwareClass.java + * Please read the explanations in that Sample about how to use this class definition. + * + * This file defines a Java Class that performs all the setup and configuration for a sample robot's hardware (motors and sensors). + * It assumes three motors (left_drive, right_drive and arm) and two servos (left_hand and right_hand) + * + * This one file/class can be used by ALL of your OpModes without having to cut & paste the code each time. + * + * Where possible, the actual hardware objects are "abstracted" (or hidden) so the OpMode code just makes calls into the class, + * rather than accessing the internal hardware directly. This is why the objects are declared "private". + * + * Use Android Studio to Copy this Class, and Paste it into your team's code folder with *exactly the same name*. + * + * Or... In OnBot Java, add a new file named RobotHardware.java, select this sample, and select Not an OpMode. + * Also add a new OpMode, select the sample ConceptExternalHardwareClass.java, and select TeleOp. + * + */ + +public class RobotHardware { + + /* Declare OpMode members. */ + private LinearOpMode myOpMode = null; // gain access to methods in the calling OpMode. + + // Define Motor and Servo objects (Make them private so they can't be accessed externally) + private DcMotor leftDrive = null; + private DcMotor rightDrive = null; + private DcMotor armMotor = null; + private Servo leftHand = null; + private Servo rightHand = null; + + // Define Drive constants. Make them public so they CAN be used by the calling OpMode + public static final double MID_SERVO = 0.5 ; + public static final double HAND_SPEED = 0.02 ; // sets rate to move servo + public static final double ARM_UP_POWER = 0.45 ; + public static final double ARM_DOWN_POWER = -0.45 ; + + // Define a constructor that allows the OpMode to pass a reference to itself. + public RobotHardware (LinearOpMode opmode) { + myOpMode = opmode; + } + + /** + * Initialize all the robot's hardware. + * This method must be called ONCE when the OpMode is initialized. + *

+ * All of the hardware devices are accessed via the hardware map, and initialized. + */ + public void init() { + // Define and Initialize Motors (note: need to use reference to actual OpMode). + leftDrive = myOpMode.hardwareMap.get(DcMotor.class, "left_drive"); + rightDrive = myOpMode.hardwareMap.get(DcMotor.class, "right_drive"); + armMotor = myOpMode.hardwareMap.get(DcMotor.class, "arm"); + + // To drive forward, most robots need the motor on one side to be reversed, because the axles point in opposite directions. + // Pushing the left stick forward MUST make robot go forward. So adjust these two lines based on your first test drive. + // Note: The settings here assume direct drive on left and right wheels. Gear Reduction or 90 Deg drives may require direction flips + leftDrive.setDirection(DcMotor.Direction.REVERSE); + rightDrive.setDirection(DcMotor.Direction.FORWARD); + + // If there are encoders connected, switch to RUN_USING_ENCODER mode for greater accuracy + // leftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + // rightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER); + + // Define and initialize ALL installed servos. + leftHand = myOpMode.hardwareMap.get(Servo.class, "left_hand"); + rightHand = myOpMode.hardwareMap.get(Servo.class, "right_hand"); + leftHand.setPosition(MID_SERVO); + rightHand.setPosition(MID_SERVO); + + myOpMode.telemetry.addData(">", "Hardware Initialized"); + myOpMode.telemetry.update(); + } + + /** + * Calculates the left/right motor powers required to achieve the requested + * robot motions: Drive (Axial motion) and Turn (Yaw motion). + * Then sends these power levels to the motors. + * + * @param Drive Fwd/Rev driving power (-1.0 to 1.0) +ve is forward + * @param Turn Right/Left turning power (-1.0 to 1.0) +ve is CW + */ + public void driveRobot(double Drive, double Turn) { + // Combine drive and turn for blended motion. + double left = Drive + Turn; + double right = Drive - Turn; + + // Scale the values so neither exceed +/- 1.0 + double max = Math.max(Math.abs(left), Math.abs(right)); + if (max > 1.0) + { + left /= max; + right /= max; + } + + // Use existing function to drive both wheels. + setDrivePower(left, right); + } + + /** + * Pass the requested wheel motor powers to the appropriate hardware drive motors. + * + * @param leftWheel Fwd/Rev driving power (-1.0 to 1.0) +ve is forward + * @param rightWheel Fwd/Rev driving power (-1.0 to 1.0) +ve is forward + */ + public void setDrivePower(double leftWheel, double rightWheel) { + // Output the values to the motor drives. + leftDrive.setPower(leftWheel); + rightDrive.setPower(rightWheel); + } + + /** + * Pass the requested arm power to the appropriate hardware drive motor + * + * @param power driving power (-1.0 to 1.0) + */ + public void setArmPower(double power) { + armMotor.setPower(power); + } + + /** + * Send the two hand-servos to opposing (mirrored) positions, based on the passed offset. + * + * @param offset + */ + public void setHandPositions(double offset) { + offset = Range.clip(offset, -0.5, 0.5); + leftHand.setPosition(MID_SERVO + offset); + rightHand.setPosition(MID_SERVO - offset); + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/readme.md b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/readme.md new file mode 100644 index 0000000..326978d --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/readme.md @@ -0,0 +1,45 @@ + +## Caution +No Team-specific code should be placed or modified in this ``.../samples`` folder. + +Samples should be Copied from here, and then Pasted into the team's +[/TeamCode/src/main/java/org/firstinspires/ftc/teamcode](../../../../../../../../../../TeamCode/src/main/java/org/firstinspires/ftc/teamcode) + folder, using the Android Studio cut and paste commands. This automatically changes all file and +class names to be consistent. From there, the sample can be modified to suit the team's needs. + +For more detailed instructions see the /teamcode readme. + +### Naming of Samples + +To gain a better understanding of how the samples are organized, and how to interpret the +naming system, it will help to understand the conventions that were used during their creation. + +These conventions are described (in detail) in the sample_conventions.md file in this folder. + +To summarize: A range of different samples classes will reside in the java/external/samples. +The class names will follow a naming convention which indicates the purpose of each class. +The prefix of the name will be one of the following: + +Basic: This is a minimally functional OpMode used to illustrate the skeleton/structure + of a particular style of OpMode. These are bare bones examples. + +Sensor: This is a Sample OpMode that shows how to use a specific sensor. + It is not intended to drive a functioning robot, it is simply showing the minimal code + required to read and display the sensor values. + +Robot: This is a Sample OpMode that assumes a simple two-motor (differential) drive base. + It may be used to provide a common baseline driving OpMode, or + to demonstrate how a particular sensor or concept can be used to navigate. + +Concept: This is a sample OpMode that illustrates performing a specific function or concept. + These may be complex, but their operation should be explained clearly in the comments, + or the comments should reference an external doc, guide or tutorial. + Each OpMode should try to only demonstrate a single concept so they are easy to + locate based on their name. These OpModes may not produce a drivable robot. + +After the prefix, other conventions will apply: + +* Sensor class names are constructed as: Sensor - Company - Type +* Robot class names are constructed as: Robot - Mode - Action - OpModetype +* Concept class names are constructed as: Concept - Topic - OpModetype + diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/sample_conventions.md b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/sample_conventions.md new file mode 100644 index 0000000..e85e625 --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/sample_conventions.md @@ -0,0 +1,113 @@ +## Sample Class/Opmode conventions +#### V 1.1.0 8/9/2017 + +This document defines the FTC Sample OpMode and Class conventions. + +### OpMode Name + +To gain a better understanding of how the samples are organized, and how to interpret the +naming system, it will help to understand the conventions that were used during their creation. + +To summarize: A range of different samples classes will reside in the java/external/samples. +The class names will follow a naming convention which indicates the purpose of each class. +The prefix of the name will be one of the following: + +Basic: This is a minimally functional OpMode used to illustrate the skeleton/structure + of a particular style of OpMode. These are bare bones examples. + +Sensor: This is a Sample OpMode that shows how to use a specific sensor. + It is not intended to drive a functioning robot, it is simply showing the minimal code + required to read and display the sensor values. + +Robot: This is a Sample OpMode that assumes a simple two-motor (differential) drive base. + It may be used to provide a common baseline driving OpMode, or + to demonstrate how a particular sensor or concept can be used to navigate. + +Concept: This is a sample OpMode that illustrates performing a specific function or concept. + These may be complex, but their operation should be explained clearly in the comments, + or the comments should reference an external doc, guide or tutorial. + Each OpMode should try to only demonstrate a single concept so they are easy to + locate based on their name. These OpModes may not produce a drivable robot. + +Utility: This sample type is provided as a useful tool, or aide, to perform some specific development task. + It is not expected to be something you would include in your own robot code. + To use the tool, comment out the @Disabled annotation and build the App. + Read the comments found in the sample for an explanation of its intended use. + +After the prefix, other conventions will apply: + +* Sensor class names should constructed as: Sensor - Company - Type +* Robot class names should be constructed as: Robot - Mode - Action - OpModetype +* Concept class names should be constructed as: Concept - Topic - OpModetype + +### Sample OpMode Content/Style + +Code is formatted as per the Google Style Guide: + +https://google.github.io/styleguide/javaguide.html + +With “Sensor” and “Hardware” samples, the code should demonstrate the essential function, +and not be embellished with too much additional “clever” code. If a sensor has special +addressing needs, or has a variety of modes or outputs, these should be demonstrated as +simply as possible. + +Special programming methods, or robot control techniques should be reserved for “Concept” Samples, +and where possible, Samples should strive to only demonstrate a single concept, +eg: State machine coding, or a User Menu system, and not combine them into a single “all inclusive” +sample. This will prevent an “all inclusive” Sample being deleted just because one part of it +becomes obsolete. + +### Device Configuration Names + +The following device names are used in the external samples + +** Motors: +left_drive +right_drive +left_arm + +** Servos: +left_hand +right_hand +arm +claw + +** Sensors: +sensor_color +sensor_ir +sensor_light +sensor_ods +sensor_range +sensor_touch +sensor_color_distance +sensor_digital +digin +digout + +** Localization: +compass +gyro +imu +navx + +### Device Object Names + +Device Object names should use the same words as the device’s configuration name, but they +should be re-structured to be a suitable Java variable name. This should keep the same word order, +but adopt the style of beginning with a lower case letter, and then each subsequent word +starting with an upper case letter. + +Eg: from the examples above: tool, leftMotor, rightClawServo, rearLightSensor. + +Note: Sometimes it’s helpful to put the device type first, followed by the variant. +eg: motorLeft and motorRight, but this should only be done if the same word order +is used on the device configuration name. + +### OpMode code Comments + +Sample comments should read like normal code comments, that is, as an explanation of what the +sample code is doing. They should NOT be directives to the user, +like: “insert your joystick code here” as these comments typically aren’t +detailed enough to be useful. They also often get left in the code and become garbage. + +Instead, an example of the joystick code should be shown with a comment describing what it is doing. diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/internal/FtcOpModeRegister.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/internal/FtcOpModeRegister.java new file mode 100644 index 0000000..ceab67d --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/internal/FtcOpModeRegister.java @@ -0,0 +1,68 @@ +/* Copyright (c) 2014, 2015 Qualcomm Technologies Inc + +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, +are permitted (subject to the limitations in the disclaimer below) provided that +the following conditions are met: + +Redistributions of source code must retain the above copyright notice, this list +of conditions and the following disclaimer. + +Redistributions in binary form must reproduce the above copyright notice, this +list of conditions and the following disclaimer in the documentation and/or +other materials provided with the distribution. + +Neither the name of Qualcomm Technologies Inc nor the names of its contributors +may be used to endorse or promote products derived from this software without +specific prior written permission. + +NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS +LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ + +package org.firstinspires.ftc.robotcontroller.internal; + +import com.qualcomm.robotcore.eventloop.opmode.OpModeManager; +import com.qualcomm.robotcore.eventloop.opmode.OpModeRegister; + +/** + * {@link FtcOpModeRegister} is responsible for registering OpModes for use in an FTC game. + * @see #register(OpModeManager) + */ +public class FtcOpModeRegister implements OpModeRegister { + + /** + * {@link #register(OpModeManager)} is called by the SDK game in order to register + * OpMode classes or instances that will participate in an FTC game. + * + * There are two mechanisms by which an OpMode may be registered. + * + * 1) The preferred method is by means of class annotations in the OpMode itself. + * See, for example the class annotations in {@link org.firstinspires.ftc.robotcontroller.external.samples.ConceptNullOp}. + * + * 2) The other, retired, method is to modify this {@link #register(OpModeManager)} + * method to include explicit calls to OpModeManager.register(). + * This method of modifying this file directly is discouraged, as it + * makes updates to the SDK harder to integrate into your code. + * + * @param manager the object which contains methods for carrying out OpMode registrations + * + * @see com.qualcomm.robotcore.eventloop.opmode.TeleOp + * @see com.qualcomm.robotcore.eventloop.opmode.Autonomous + */ + public void register(OpModeManager manager) { + + /** + * Any manual OpMode class registrations should go here. + */ + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/internal/FtcRobotControllerActivity.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/internal/FtcRobotControllerActivity.java new file mode 100644 index 0000000..3f1f77c --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/internal/FtcRobotControllerActivity.java @@ -0,0 +1,845 @@ +/* Copyright (c) 2014, 2015 Qualcomm Technologies Inc + +All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, +are permitted (subject to the limitations in the disclaimer below) provided that +the following conditions are met: + +Redistributions of source code must retain the above copyright notice, this list +of conditions and the following disclaimer. + +Redistributions in binary form must reproduce the above copyright notice, this +list of conditions and the following disclaimer in the documentation and/or +other materials provided with the distribution. + +Neither the name of Qualcomm Technologies Inc nor the names of its contributors +may be used to endorse or promote products derived from this software without +specific prior written permission. + +NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS +LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, +THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ + +package org.firstinspires.ftc.robotcontroller.internal; + +import android.app.ActionBar; +import android.app.Activity; +import android.app.ActivityManager; +import android.content.ComponentName; +import android.content.Context; +import android.content.Intent; +import android.content.ServiceConnection; +import android.content.SharedPreferences; +import android.content.res.Configuration; +import android.hardware.usb.UsbDevice; +import android.hardware.usb.UsbManager; +import android.net.wifi.WifiManager; +import android.os.Bundle; +import android.os.IBinder; +import android.preference.PreferenceManager; +import androidx.annotation.NonNull; +import androidx.annotation.Nullable; +import androidx.annotation.StringRes; +import android.view.Menu; +import android.view.MenuItem; +import android.view.MotionEvent; +import android.view.View; +import android.view.WindowManager; +import android.webkit.WebView; +import android.widget.ImageButton; +import android.widget.LinearLayout; +import android.widget.LinearLayout.LayoutParams; +import android.widget.PopupMenu; +import android.widget.TextView; + +import com.google.blocks.ftcrobotcontroller.ProgrammingWebHandlers; +import com.google.blocks.ftcrobotcontroller.runtime.BlocksOpMode; +import com.qualcomm.ftccommon.ClassManagerFactory; +import com.qualcomm.ftccommon.FtcAboutActivity; +import com.qualcomm.ftccommon.FtcEventLoop; +import com.qualcomm.ftccommon.FtcEventLoopIdle; +import com.qualcomm.ftccommon.FtcRobotControllerService; +import com.qualcomm.ftccommon.FtcRobotControllerService.FtcRobotControllerBinder; +import com.qualcomm.ftccommon.FtcRobotControllerSettingsActivity; +import com.qualcomm.ftccommon.LaunchActivityConstantsList; +import com.qualcomm.ftccommon.LaunchActivityConstantsList.RequestCode; +import com.qualcomm.ftccommon.Restarter; +import com.qualcomm.ftccommon.UpdateUI; +import com.qualcomm.ftccommon.configuration.EditParameters; +import com.qualcomm.ftccommon.configuration.FtcLoadFileActivity; +import com.qualcomm.ftccommon.configuration.RobotConfigFile; +import com.qualcomm.ftccommon.configuration.RobotConfigFileManager; +import com.qualcomm.ftcrobotcontroller.BuildConfig; +import com.qualcomm.ftcrobotcontroller.R; +import com.qualcomm.hardware.HardwareFactory; +import com.qualcomm.robotcore.eventloop.EventLoopManager; +import com.qualcomm.robotcore.eventloop.opmode.FtcRobotControllerServiceState; +import com.qualcomm.robotcore.eventloop.opmode.OpModeRegister; +import com.qualcomm.robotcore.hardware.configuration.LynxConstants; +import com.qualcomm.robotcore.hardware.configuration.Utility; +import com.qualcomm.robotcore.robot.Robot; +import com.qualcomm.robotcore.robot.RobotState; +import com.qualcomm.robotcore.util.ClockWarningSource; +import com.qualcomm.robotcore.util.Device; +import com.qualcomm.robotcore.util.Dimmer; +import com.qualcomm.robotcore.util.ImmersiveMode; +import com.qualcomm.robotcore.util.RobotLog; +import com.qualcomm.robotcore.util.WebServer; +import com.qualcomm.robotcore.wifi.NetworkConnection; +import com.qualcomm.robotcore.wifi.NetworkConnectionFactory; +import com.qualcomm.robotcore.wifi.NetworkType; + +import org.firstinspires.ftc.ftccommon.external.SoundPlayingRobotMonitor; +import org.firstinspires.ftc.ftccommon.internal.AnnotatedHooksClassFilter; +import org.firstinspires.ftc.ftccommon.internal.FtcRobotControllerWatchdogService; +import org.firstinspires.ftc.ftccommon.internal.ProgramAndManageActivity; +import org.firstinspires.ftc.onbotjava.ExternalLibraries; +import org.firstinspires.ftc.onbotjava.OnBotJavaHelperImpl; +import org.firstinspires.ftc.onbotjava.OnBotJavaProgrammingMode; +import org.firstinspires.ftc.robotcore.external.navigation.MotionDetection; +import org.firstinspires.ftc.robotcore.internal.hardware.android.AndroidBoard; +import org.firstinspires.ftc.robotcore.internal.network.DeviceNameManagerFactory; +import org.firstinspires.ftc.robotcore.internal.network.PreferenceRemoterRC; +import org.firstinspires.ftc.robotcore.internal.network.StartResult; +import org.firstinspires.ftc.robotcore.internal.network.WifiDirectChannelChanger; +import org.firstinspires.ftc.robotcore.internal.network.WifiMuteEvent; +import org.firstinspires.ftc.robotcore.internal.network.WifiMuteStateMachine; +import org.firstinspires.ftc.robotcore.internal.opmode.ClassManager; +import org.firstinspires.ftc.robotcore.internal.opmode.OnBotJavaHelper; +import org.firstinspires.ftc.robotcore.internal.system.AppAliveNotifier; +import org.firstinspires.ftc.robotcore.internal.system.AppUtil; +import org.firstinspires.ftc.robotcore.internal.system.Assert; +import org.firstinspires.ftc.robotcore.internal.system.PreferencesHelper; +import org.firstinspires.ftc.robotcore.internal.system.ServiceController; +import org.firstinspires.ftc.robotcore.internal.ui.ThemedActivity; +import org.firstinspires.ftc.robotcore.internal.ui.UILocation; +import org.firstinspires.ftc.robotcore.internal.webserver.RobotControllerWebInfo; +import org.firstinspires.ftc.robotserver.internal.programmingmode.ProgrammingModeManager; +import org.firstinspires.inspection.RcInspectionActivity; +import org.threeten.bp.YearMonth; +import org.xmlpull.v1.XmlPullParserException; + +import java.io.FileNotFoundException; +import java.util.List; +import java.util.Queue; +import java.util.concurrent.ConcurrentLinkedQueue; + +@SuppressWarnings("WeakerAccess") +public class FtcRobotControllerActivity extends Activity + { + public static final String TAG = "RCActivity"; + public String getTag() { return TAG; } + + private static final int REQUEST_CONFIG_WIFI_CHANNEL = 1; + private static final int NUM_GAMEPADS = 2; + + protected WifiManager.WifiLock wifiLock; + protected RobotConfigFileManager cfgFileMgr; + + private OnBotJavaHelper onBotJavaHelper; + + protected ProgrammingModeManager programmingModeManager; + + protected UpdateUI.Callback callback; + protected Context context; + protected Utility utility; + protected StartResult prefRemoterStartResult = new StartResult(); + protected StartResult deviceNameStartResult = new StartResult(); + protected PreferencesHelper preferencesHelper; + protected final SharedPreferencesListener sharedPreferencesListener = new SharedPreferencesListener(); + + protected ImageButton buttonMenu; + protected TextView textDeviceName; + protected TextView textNetworkConnectionStatus; + protected TextView textRobotStatus; + protected TextView[] textGamepad = new TextView[NUM_GAMEPADS]; + protected TextView textOpMode; + protected TextView textErrorMessage; + protected ImmersiveMode immersion; + + protected UpdateUI updateUI; + protected Dimmer dimmer; + protected LinearLayout entireScreenLayout; + + protected FtcRobotControllerService controllerService; + protected NetworkType networkType; + + protected FtcEventLoop eventLoop; + protected Queue receivedUsbAttachmentNotifications; + + protected WifiMuteStateMachine wifiMuteStateMachine; + protected MotionDetection motionDetection; + + private static boolean permissionsValidated = false; + + private WifiDirectChannelChanger wifiDirectChannelChanger; + + protected class RobotRestarter implements Restarter { + + public void requestRestart() { + requestRobotRestart(); + } + + } + + protected boolean serviceShouldUnbind = false; + protected ServiceConnection connection = new ServiceConnection() { + @Override + public void onServiceConnected(ComponentName name, IBinder service) { + FtcRobotControllerBinder binder = (FtcRobotControllerBinder) service; + onServiceBind(binder.getService()); + } + + @Override + public void onServiceDisconnected(ComponentName name) { + RobotLog.vv(FtcRobotControllerService.TAG, "%s.controllerService=null", TAG); + controllerService = null; + } + }; + + @Override + protected void onNewIntent(Intent intent) { + super.onNewIntent(intent); + + if (UsbManager.ACTION_USB_DEVICE_ATTACHED.equals(intent.getAction())) { + UsbDevice usbDevice = intent.getParcelableExtra(UsbManager.EXTRA_DEVICE); + RobotLog.vv(TAG, "ACTION_USB_DEVICE_ATTACHED: %s", usbDevice.getDeviceName()); + + if (usbDevice != null) { // paranoia + // We might get attachment notifications before the event loop is set up, so + // we hold on to them and pass them along only when we're good and ready. + if (receivedUsbAttachmentNotifications != null) { // *total* paranoia + receivedUsbAttachmentNotifications.add(usbDevice); + passReceivedUsbAttachmentsToEventLoop(); + } + } + } + } + + protected void passReceivedUsbAttachmentsToEventLoop() { + if (this.eventLoop != null) { + for (;;) { + UsbDevice usbDevice = receivedUsbAttachmentNotifications.poll(); + if (usbDevice == null) + break; + this.eventLoop.onUsbDeviceAttached(usbDevice); + } + } + else { + // Paranoia: we don't want the pending list to grow without bound when we don't + // (yet) have an event loop + while (receivedUsbAttachmentNotifications.size() > 100) { + receivedUsbAttachmentNotifications.poll(); + } + } + } + + /** + * There are cases where a permission may be revoked and the system restart will restart the + * FtcRobotControllerActivity, instead of the launch activity. Detect when that happens, and throw + * the device back to the permission validator activity. + */ + protected boolean enforcePermissionValidator() { + if (!permissionsValidated) { + RobotLog.vv(TAG, "Redirecting to permission validator"); + Intent permissionValidatorIntent = new Intent(AppUtil.getDefContext(), PermissionValidatorWrapper.class); + startActivity(permissionValidatorIntent); + finish(); + return true; + } else { + RobotLog.vv(TAG, "Permissions validated already"); + return false; + } + } + + public static void setPermissionsValidated() { + permissionsValidated = true; + } + + @Override + protected void onCreate(Bundle savedInstanceState) { + super.onCreate(savedInstanceState); + + if (enforcePermissionValidator()) { + return; + } + + RobotLog.onApplicationStart(); // robustify against onCreate() following onDestroy() but using the same app instance, which apparently does happen + RobotLog.vv(TAG, "onCreate()"); + ThemedActivity.appAppThemeToActivity(getTag(), this); // do this way instead of inherit to help AppInventor + + // Oddly, sometimes after a crash & restart the root activity will be something unexpected, like from the before crash? We don't yet understand + RobotLog.vv(TAG, "rootActivity is of class %s", AppUtil.getInstance().getRootActivity().getClass().getSimpleName()); + RobotLog.vv(TAG, "launchActivity is of class %s", FtcRobotControllerWatchdogService.launchActivity()); + Assert.assertTrue(FtcRobotControllerWatchdogService.isLaunchActivity(AppUtil.getInstance().getRootActivity())); + Assert.assertTrue(AppUtil.getInstance().isRobotController()); + + // Quick check: should we pretend we're not here, and so allow the Lynx to operate as + // a stand-alone USB-connected module? + if (LynxConstants.isRevControlHub()) { + // Double-sure check that we can talk to the DB over the serial TTY + AndroidBoard.getInstance().getAndroidBoardIsPresentPin().setState(true); + } + + context = this; + utility = new Utility(this); + + DeviceNameManagerFactory.getInstance().start(deviceNameStartResult); + + PreferenceRemoterRC.getInstance().start(prefRemoterStartResult); + + receivedUsbAttachmentNotifications = new ConcurrentLinkedQueue(); + eventLoop = null; + + setContentView(R.layout.activity_ftc_controller); + + preferencesHelper = new PreferencesHelper(TAG, context); + preferencesHelper.writeBooleanPrefIfDifferent(context.getString(R.string.pref_rc_connected), true); + preferencesHelper.getSharedPreferences().registerOnSharedPreferenceChangeListener(sharedPreferencesListener); + + // Check if this RC app is from a later FTC season than what was installed previously + int ftcSeasonYearOfPreviouslyInstalledRc = preferencesHelper.readInt(getString(R.string.pref_ftc_season_year_of_current_rc), 0); + int ftcSeasonYearOfCurrentlyInstalledRc = AppUtil.getInstance().getFtcSeasonYear(AppUtil.getInstance().getLocalSdkBuildMonth()).getValue(); + if (ftcSeasonYearOfCurrentlyInstalledRc > ftcSeasonYearOfPreviouslyInstalledRc) { + preferencesHelper.writeIntPrefIfDifferent(getString(R.string.pref_ftc_season_year_of_current_rc), ftcSeasonYearOfCurrentlyInstalledRc); + // Since it's a new FTC season, we should reset certain settings back to their default values. + preferencesHelper.writeBooleanPrefIfDifferent(getString(R.string.pref_warn_about_2_4_ghz_band), true); + preferencesHelper.writeBooleanPrefIfDifferent(getString(R.string.pref_warn_about_obsolete_software), true); + preferencesHelper.writeBooleanPrefIfDifferent(getString(R.string.pref_warn_about_mismatched_app_versions), true); + preferencesHelper.writeBooleanPrefIfDifferent(getString(R.string.pref_warn_about_incorrect_clocks), true); + } + + entireScreenLayout = (LinearLayout) findViewById(R.id.entire_screen); + buttonMenu = (ImageButton) findViewById(R.id.menu_buttons); + buttonMenu.setOnClickListener(new View.OnClickListener() { + @Override + public void onClick(View v) { + PopupMenu popupMenu = new PopupMenu(FtcRobotControllerActivity.this, v); + popupMenu.setOnMenuItemClickListener(new PopupMenu.OnMenuItemClickListener() { + @Override + public boolean onMenuItemClick(MenuItem item) { + return onOptionsItemSelected(item); // Delegate to the handler for the hardware menu button + } + }); + popupMenu.inflate(R.menu.ftc_robot_controller); + AnnotatedHooksClassFilter.getInstance().callOnCreateMenuMethods( + FtcRobotControllerActivity.this, popupMenu.getMenu()); + popupMenu.show(); + } + }); + + updateMonitorLayout(getResources().getConfiguration()); + + BlocksOpMode.setActivityAndWebView(this, (WebView) findViewById(R.id.webViewBlocksRuntime)); + + ExternalLibraries.getInstance().onCreate(); + onBotJavaHelper = new OnBotJavaHelperImpl(); + + /* + * Paranoia as the ClassManagerFactory requires EXTERNAL_STORAGE permissions + * and we've seen on the DS where the finish() call above does not short-circuit + * the onCreate() call for the activity and then we crash here because we don't + * have permissions. So... + */ + if (permissionsValidated) { + ClassManager.getInstance().setOnBotJavaClassHelper(onBotJavaHelper); + ClassManagerFactory.registerFilters(); + ClassManagerFactory.processAllClasses(); + } + + cfgFileMgr = new RobotConfigFileManager(this); + + // Clean up 'dirty' status after a possible crash + RobotConfigFile configFile = cfgFileMgr.getActiveConfig(); + if (configFile.isDirty()) { + configFile.markClean(); + cfgFileMgr.setActiveConfig(false, configFile); + } + + textDeviceName = (TextView) findViewById(R.id.textDeviceName); + textNetworkConnectionStatus = (TextView) findViewById(R.id.textNetworkConnectionStatus); + textRobotStatus = (TextView) findViewById(R.id.textRobotStatus); + textOpMode = (TextView) findViewById(R.id.textOpMode); + textErrorMessage = (TextView) findViewById(R.id.textErrorMessage); + textGamepad[0] = (TextView) findViewById(R.id.textGamepad1); + textGamepad[1] = (TextView) findViewById(R.id.textGamepad2); + immersion = new ImmersiveMode(getWindow().getDecorView()); + dimmer = new Dimmer(this); + dimmer.longBright(); + + programmingModeManager = new ProgrammingModeManager(); + programmingModeManager.register(new ProgrammingWebHandlers()); + programmingModeManager.register(new OnBotJavaProgrammingMode()); + + updateUI = createUpdateUI(); + callback = createUICallback(updateUI); + + PreferenceManager.setDefaultValues(this, R.xml.app_settings, false); + + WifiManager wifiManager = (WifiManager) getApplicationContext().getSystemService(Context.WIFI_SERVICE); + wifiLock = wifiManager.createWifiLock(WifiManager.WIFI_MODE_FULL_HIGH_PERF, ""); + + hittingMenuButtonBrightensScreen(); + + wifiLock.acquire(); + callback.networkConnectionUpdate(NetworkConnection.NetworkEvent.DISCONNECTED); + readNetworkType(); + ServiceController.startService(FtcRobotControllerWatchdogService.class); + bindToService(); + RobotLog.logAppInfo(); + RobotLog.logDeviceInfo(); + AndroidBoard.getInstance().logAndroidBoardInfo(); + + if (preferencesHelper.readBoolean(getString(R.string.pref_wifi_automute), false)) { + initWifiMute(true); + } + + FtcAboutActivity.setBuildTimeFromBuildConfig(BuildConfig.APP_BUILD_TIME); + + // check to see if there is a preferred Wi-Fi to use. + checkPreferredChannel(); + + AnnotatedHooksClassFilter.getInstance().callOnCreateMethods(this); + } + + protected UpdateUI createUpdateUI() { + Restarter restarter = new RobotRestarter(); + UpdateUI result = new UpdateUI(this, dimmer); + result.setRestarter(restarter); + result.setTextViews(textNetworkConnectionStatus, textRobotStatus, textGamepad, textOpMode, textErrorMessage, textDeviceName); + return result; + } + + protected UpdateUI.Callback createUICallback(UpdateUI updateUI) { + UpdateUI.Callback result = updateUI.new Callback(); + result.setStateMonitor(new SoundPlayingRobotMonitor()); + return result; + } + + @Override + protected void onStart() { + super.onStart(); + RobotLog.vv(TAG, "onStart()"); + + entireScreenLayout.setOnTouchListener(new View.OnTouchListener() { + @Override + public boolean onTouch(View v, MotionEvent event) { + dimmer.handleDimTimer(); + return false; + } + }); + } + + @Override + protected void onResume() { + super.onResume(); + RobotLog.vv(TAG, "onResume()"); + + // In case the user just got back from fixing their clock, refresh ClockWarningSource + ClockWarningSource.getInstance().onPossibleRcClockUpdate(); + } + + @Override + protected void onPause() { + super.onPause(); + RobotLog.vv(TAG, "onPause()"); + } + + @Override + protected void onStop() { + // Note: this gets called even when the configuration editor is launched. That is, it gets + // called surprisingly often. So, we don't actually do much here. + super.onStop(); + RobotLog.vv(TAG, "onStop()"); + } + + @Override + protected void onDestroy() { + super.onDestroy(); + RobotLog.vv(TAG, "onDestroy()"); + + shutdownRobot(); // Ensure the robot is put away to bed + if (callback != null) callback.close(); + + PreferenceRemoterRC.getInstance().stop(prefRemoterStartResult); + DeviceNameManagerFactory.getInstance().stop(deviceNameStartResult); + + unbindFromService(); + // If the app manually (?) is stopped, then we don't need the auto-starting function (?) + ServiceController.stopService(FtcRobotControllerWatchdogService.class); + if (wifiLock != null) wifiLock.release(); + if (preferencesHelper != null) preferencesHelper.getSharedPreferences().unregisterOnSharedPreferenceChangeListener(sharedPreferencesListener); + + RobotLog.cancelWriteLogcatToDisk(); + + AnnotatedHooksClassFilter.getInstance().callOnDestroyMethods(this); + } + + protected void bindToService() { + readNetworkType(); + Intent intent = new Intent(this, FtcRobotControllerService.class); + intent.putExtra(NetworkConnectionFactory.NETWORK_CONNECTION_TYPE, networkType); + serviceShouldUnbind = bindService(intent, connection, Context.BIND_AUTO_CREATE); + } + + protected void unbindFromService() { + if (serviceShouldUnbind) { + unbindService(connection); + serviceShouldUnbind = false; + } + } + + protected void readNetworkType() { + // Control hubs are always running the access point model. Everything else, for the time + // being always runs the Wi-Fi Direct model. + if (Device.isRevControlHub() == true) { + networkType = NetworkType.RCWIRELESSAP; + } else { + networkType = NetworkType.fromString(preferencesHelper.readString(context.getString(R.string.pref_pairing_kind), NetworkType.globalDefaultAsString())); + } + + // update the app_settings + preferencesHelper.writeStringPrefIfDifferent(context.getString(R.string.pref_pairing_kind), networkType.toString()); + } + + @Override + public void onWindowFocusChanged(boolean hasFocus) { + super.onWindowFocusChanged(hasFocus); + + if (hasFocus) { + immersion.hideSystemUI(); + getWindow().setFlags(WindowManager.LayoutParams.FLAG_TRANSLUCENT_NAVIGATION, WindowManager.LayoutParams.FLAG_TRANSLUCENT_NAVIGATION); + } + } + + @Override + public boolean onCreateOptionsMenu(Menu menu) { + getMenuInflater().inflate(R.menu.ftc_robot_controller, menu); + AnnotatedHooksClassFilter.getInstance().callOnCreateMenuMethods(this, menu); + return true; + } + + private boolean isRobotRunning() { + if (controllerService == null) { + return false; + } + + Robot robot = controllerService.getRobot(); + + if ((robot == null) || (robot.eventLoopManager == null)) { + return false; + } + + RobotState robotState = robot.eventLoopManager.state; + + if (robotState != RobotState.RUNNING) { + return false; + } else { + return true; + } + } + + @Override + public boolean onOptionsItemSelected(MenuItem item) { + int id = item.getItemId(); + + if (id == R.id.action_program_and_manage) { + if (isRobotRunning()) { + Intent programmingModeIntent = new Intent(AppUtil.getDefContext(), ProgramAndManageActivity.class); + RobotControllerWebInfo webInfo = programmingModeManager.getWebServer().getConnectionInformation(); + programmingModeIntent.putExtra(LaunchActivityConstantsList.RC_WEB_INFO, webInfo.toJson()); + startActivity(programmingModeIntent); + } else { + AppUtil.getInstance().showToast(UILocation.ONLY_LOCAL, context.getString(R.string.toastWifiUpBeforeProgrammingMode)); + } + } else if (id == R.id.action_inspection_mode) { + Intent inspectionModeIntent = new Intent(AppUtil.getDefContext(), RcInspectionActivity.class); + startActivity(inspectionModeIntent); + return true; + } else if (id == R.id.action_restart_robot) { + dimmer.handleDimTimer(); + AppUtil.getInstance().showToast(UILocation.BOTH, context.getString(R.string.toastRestartingRobot)); + requestRobotRestart(); + return true; + } + else if (id == R.id.action_configure_robot) { + EditParameters parameters = new EditParameters(); + Intent intentConfigure = new Intent(AppUtil.getDefContext(), FtcLoadFileActivity.class); + parameters.putIntent(intentConfigure); + startActivityForResult(intentConfigure, RequestCode.CONFIGURE_ROBOT_CONTROLLER.ordinal()); + } + else if (id == R.id.action_settings) { + // historical: this once erroneously used FTC_CONFIGURE_REQUEST_CODE_ROBOT_CONTROLLER + Intent settingsIntent = new Intent(AppUtil.getDefContext(), FtcRobotControllerSettingsActivity.class); + startActivityForResult(settingsIntent, RequestCode.SETTINGS_ROBOT_CONTROLLER.ordinal()); + return true; + } + else if (id == R.id.action_about) { + Intent intent = new Intent(AppUtil.getDefContext(), FtcAboutActivity.class); + startActivity(intent); + return true; + } + else if (id == R.id.action_exit_app) { + + //Clear backstack and everything to prevent edge case where VM might be + //restarted (after it was exited) if more than one activity was on the + //backstack for some reason. + finishAffinity(); + + //For lollipop and up, we can clear ourselves from the recents list too + if (android.os.Build.VERSION.SDK_INT >= android.os.Build.VERSION_CODES.LOLLIPOP) { + ActivityManager manager = (ActivityManager) getSystemService(ACTIVITY_SERVICE); + List tasks = manager.getAppTasks(); + + for (ActivityManager.AppTask task : tasks) { + task.finishAndRemoveTask(); + } + } + + // Allow the user to use the Control Hub operating system's UI, instead of relaunching the app + AppAliveNotifier.getInstance().disableAppWatchdogUntilNextAppStart(); + + //Finally, nuke the VM from orbit + AppUtil.getInstance().exitApplication(); + + return true; + } + + return super.onOptionsItemSelected(item); + } + + @Override + public void onConfigurationChanged(Configuration newConfig) { + super.onConfigurationChanged(newConfig); + // don't destroy assets on screen rotation + updateMonitorLayout(newConfig); + } + + /** + * Updates the orientation of monitorContainer (which contains cameraMonitorView) + * based on the given configuration. Makes the children split the space. + */ + private void updateMonitorLayout(Configuration configuration) { + LinearLayout monitorContainer = (LinearLayout) findViewById(R.id.monitorContainer); + if (configuration.orientation == Configuration.ORIENTATION_LANDSCAPE) { + // When the phone is landscape, lay out the monitor views horizontally. + monitorContainer.setOrientation(LinearLayout.HORIZONTAL); + for (int i = 0; i < monitorContainer.getChildCount(); i++) { + View view = monitorContainer.getChildAt(i); + view.setLayoutParams(new LayoutParams(0, LayoutParams.MATCH_PARENT, 1 /* weight */)); + } + } else { + // When the phone is portrait, lay out the monitor views vertically. + monitorContainer.setOrientation(LinearLayout.VERTICAL); + for (int i = 0; i < monitorContainer.getChildCount(); i++) { + View view = monitorContainer.getChildAt(i); + view.setLayoutParams(new LayoutParams(LayoutParams.MATCH_PARENT, 0, 1 /* weight */)); + } + } + monitorContainer.requestLayout(); + } + + @Override + protected void onActivityResult(int request, int result, Intent intent) { + if (request == REQUEST_CONFIG_WIFI_CHANNEL) { + if (result == RESULT_OK) { + AppUtil.getInstance().showToast(UILocation.BOTH, context.getString(R.string.toastWifiConfigurationComplete)); + } + } + // was some historical confusion about launch codes here, so we err safely + if (request == RequestCode.CONFIGURE_ROBOT_CONTROLLER.ordinal() || request == RequestCode.SETTINGS_ROBOT_CONTROLLER.ordinal()) { + // We always do a refresh, whether it was a cancel or an OK, for robustness + shutdownRobot(); + cfgFileMgr.getActiveConfigAndUpdateUI(); + updateUIAndRequestRobotSetup(); + } + } + + public void onServiceBind(final FtcRobotControllerService service) { + RobotLog.vv(FtcRobotControllerService.TAG, "%s.controllerService=bound", TAG); + controllerService = service; + updateUI.setControllerService(controllerService); + + controllerService.setOnBotJavaHelper(onBotJavaHelper); + + updateUIAndRequestRobotSetup(); + programmingModeManager.setState(new FtcRobotControllerServiceState() { + @NonNull + @Override + public WebServer getWebServer() { + return service.getWebServer(); + } + + @Nullable + @Override + public OnBotJavaHelper getOnBotJavaHelper() { + return service.getOnBotJavaHelper(); + } + + @Override + public EventLoopManager getEventLoopManager() { + return service.getRobot().eventLoopManager; + } + }); + + AnnotatedHooksClassFilter.getInstance().callWebHandlerRegistrarMethods(this, + service.getWebServer().getWebHandlerManager()); + } + + private void updateUIAndRequestRobotSetup() { + if (controllerService != null) { + callback.networkConnectionUpdate(controllerService.getNetworkConnectionStatus()); + callback.updateRobotStatus(controllerService.getRobotStatus()); + // Only show this first-time toast on headless systems: what we have now on non-headless suffices + requestRobotSetup(LynxConstants.isRevControlHub() + ? new Runnable() { + @Override public void run() { + showRestartRobotCompleteToast(R.string.toastRobotSetupComplete); + } + } + : null); + } + } + + private void requestRobotSetup(@Nullable Runnable runOnComplete) { + if (controllerService == null) return; + + RobotConfigFile file = cfgFileMgr.getActiveConfigAndUpdateUI(); + HardwareFactory hardwareFactory = new HardwareFactory(context); + try { + hardwareFactory.setXmlPullParser(file.getXml()); + } catch (FileNotFoundException | XmlPullParserException e) { + RobotLog.ww(TAG, e, "Unable to set configuration file %s. Falling back on noConfig.", file.getName()); + file = RobotConfigFile.noConfig(cfgFileMgr); + try { + hardwareFactory.setXmlPullParser(file.getXml()); + cfgFileMgr.setActiveConfigAndUpdateUI(false, file); + } catch (FileNotFoundException | XmlPullParserException e1) { + RobotLog.ee(TAG, e1, "Failed to fall back on noConfig"); + } + } + + OpModeRegister userOpModeRegister = createOpModeRegister(); + eventLoop = new FtcEventLoop(hardwareFactory, userOpModeRegister, callback, this); + FtcEventLoopIdle idleLoop = new FtcEventLoopIdle(hardwareFactory, userOpModeRegister, callback, this); + + controllerService.setCallback(callback); + controllerService.setupRobot(eventLoop, idleLoop, runOnComplete); + + passReceivedUsbAttachmentsToEventLoop(); + AndroidBoard.showErrorIfUnknownControlHub(); + + AnnotatedHooksClassFilter.getInstance().callOnCreateEventLoopMethods(this, eventLoop); + } + + protected OpModeRegister createOpModeRegister() { + return new FtcOpModeRegister(); + } + + private void shutdownRobot() { + if (controllerService != null) controllerService.shutdownRobot(); + } + + private void requestRobotRestart() { + AppUtil.getInstance().showToast(UILocation.BOTH, AppUtil.getDefContext().getString(R.string.toastRestartingRobot)); + // + RobotLog.clearGlobalErrorMsg(); + RobotLog.clearGlobalWarningMsg(); + shutdownRobot(); + requestRobotSetup(new Runnable() { + @Override public void run() { + showRestartRobotCompleteToast(R.string.toastRestartRobotComplete); + } + }); + } + + private void showRestartRobotCompleteToast(@StringRes int resid) { + AppUtil.getInstance().showToast(UILocation.BOTH, AppUtil.getDefContext().getString(resid)); + } + + private void checkPreferredChannel() { + // For P2P network, check to see what preferred channel is. + if (networkType == NetworkType.WIFIDIRECT) { + int prefChannel = preferencesHelper.readInt(getString(com.qualcomm.ftccommon.R.string.pref_wifip2p_channel), -1); + if (prefChannel == -1) { + prefChannel = 0; + RobotLog.vv(TAG, "pref_wifip2p_channel: No preferred channel defined. Will use a default value of %d", prefChannel); + } else { + RobotLog.vv(TAG, "pref_wifip2p_channel: Found existing preferred channel (%d).", prefChannel); + } + + // attempt to set the preferred channel. + RobotLog.vv(TAG, "pref_wifip2p_channel: attempting to set preferred channel..."); + wifiDirectChannelChanger = new WifiDirectChannelChanger(); + wifiDirectChannelChanger.changeToChannel(prefChannel); + } + } + + protected void hittingMenuButtonBrightensScreen() { + ActionBar actionBar = getActionBar(); + if (actionBar != null) { + actionBar.addOnMenuVisibilityListener(new ActionBar.OnMenuVisibilityListener() { + @Override + public void onMenuVisibilityChanged(boolean isVisible) { + if (isVisible) { + dimmer.handleDimTimer(); + } + } + }); + } + } + + protected class SharedPreferencesListener implements SharedPreferences.OnSharedPreferenceChangeListener { + @Override public void onSharedPreferenceChanged(SharedPreferences sharedPreferences, String key) { + if (key.equals(context.getString(R.string.pref_app_theme))) { + ThemedActivity.restartForAppThemeChange(getTag(), getString(R.string.appThemeChangeRestartNotifyRC)); + } else if (key.equals(context.getString(R.string.pref_wifi_automute))) { + if (preferencesHelper.readBoolean(context.getString(R.string.pref_wifi_automute), false)) { + initWifiMute(true); + } else { + initWifiMute(false); + } + } + } + } + + protected void initWifiMute(boolean enable) { + if (enable) { + wifiMuteStateMachine = new WifiMuteStateMachine(); + wifiMuteStateMachine.initialize(); + wifiMuteStateMachine.start(); + + motionDetection = new MotionDetection(2.0, 10); + motionDetection.startListening(); + motionDetection.registerListener(new MotionDetection.MotionDetectionListener() { + @Override + public void onMotionDetected(double vector) + { + wifiMuteStateMachine.consumeEvent(WifiMuteEvent.USER_ACTIVITY); + } + }); + } else { + wifiMuteStateMachine.stop(); + wifiMuteStateMachine = null; + motionDetection.stopListening(); + motionDetection.purgeListeners(); + motionDetection = null; + } + } + + @Override + public void onUserInteraction() { + if (wifiMuteStateMachine != null) { + wifiMuteStateMachine.consumeEvent(WifiMuteEvent.USER_ACTIVITY); + } + } +} diff --git a/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/internal/PermissionValidatorWrapper.java b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/internal/PermissionValidatorWrapper.java new file mode 100644 index 0000000..a0094bc --- /dev/null +++ b/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/internal/PermissionValidatorWrapper.java @@ -0,0 +1,91 @@ +/* + * Copyright (c) 2018 Craig MacFarlane + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without modification, are permitted + * (subject to the limitations in the disclaimer below) provided that the following conditions are + * met: + * + * Redistributions of source code must retain the above copyright notice, this list of conditions + * and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, this list of conditions + * and the following disclaimer in the documentation and/or other materials provided with the + * distribution. + * + * Neither the name of Craig MacFarlane nor the names of its contributors may be used to + * endorse or promote products derived from this software without specific prior written permission. + * + * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS LICENSE. THIS + * SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED + * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES + * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, + * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF + * THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ +package org.firstinspires.ftc.robotcontroller.internal; + +import android.Manifest; +import android.os.Bundle; + +import com.qualcomm.ftcrobotcontroller.R; + +import org.firstinspires.ftc.robotcore.internal.system.Misc; +import org.firstinspires.ftc.robotcore.internal.system.PermissionValidatorActivity; + +import java.util.ArrayList; +import java.util.List; + +public class PermissionValidatorWrapper extends PermissionValidatorActivity { + + private final String TAG = "PermissionValidatorWrapper"; + + /* + * The list of dangerous permissions the robot controller needs. + */ + protected List robotControllerPermissions = new ArrayList() {{ + add(Manifest.permission.WRITE_EXTERNAL_STORAGE); + add(Manifest.permission.READ_EXTERNAL_STORAGE); + add(Manifest.permission.CAMERA); + add(Manifest.permission.ACCESS_COARSE_LOCATION); + add(Manifest.permission.ACCESS_FINE_LOCATION); + add(Manifest.permission.READ_PHONE_STATE); + }}; + + private final static Class startApplication = FtcRobotControllerActivity.class; + + public String mapPermissionToExplanation(final String permission) { + if (permission.equals(Manifest.permission.WRITE_EXTERNAL_STORAGE)) { + return Misc.formatForUser(R.string.permRcWriteExternalStorageExplain); + } else if (permission.equals(Manifest.permission.READ_EXTERNAL_STORAGE)) { + return Misc.formatForUser(R.string.permRcReadExternalStorageExplain); + } else if (permission.equals(Manifest.permission.CAMERA)) { + return Misc.formatForUser(R.string.permRcCameraExplain); + } else if (permission.equals(Manifest.permission.ACCESS_COARSE_LOCATION)) { + return Misc.formatForUser(R.string.permAccessLocationExplain); + } else if (permission.equals(Manifest.permission.ACCESS_FINE_LOCATION)) { + return Misc.formatForUser(R.string.permAccessLocationExplain); + } else if (permission.equals(Manifest.permission.READ_PHONE_STATE)) { + return Misc.formatForUser(R.string.permReadPhoneState); + } + return Misc.formatForUser(R.string.permGenericExplain); + } + + @Override + protected void onCreate(Bundle savedInstanceState) + { + super.onCreate(savedInstanceState); + + permissions = robotControllerPermissions; + } + + protected Class onStartApplication() + { + FtcRobotControllerActivity.setPermissionsValidated(); + return startApplication; + } +} diff --git a/FtcRobotController/src/main/res/drawable-xhdpi/icon_menu.png b/FtcRobotController/src/main/res/drawable-xhdpi/icon_menu.png new file mode 100644 index 0000000..6b9e997 Binary files /dev/null and b/FtcRobotController/src/main/res/drawable-xhdpi/icon_menu.png differ diff --git a/FtcRobotController/src/main/res/drawable-xhdpi/icon_robotcontroller.png b/FtcRobotController/src/main/res/drawable-xhdpi/icon_robotcontroller.png new file mode 100644 index 0000000..022552f Binary files /dev/null and b/FtcRobotController/src/main/res/drawable-xhdpi/icon_robotcontroller.png differ diff --git a/FtcRobotController/src/main/res/layout/activity_ftc_controller.xml b/FtcRobotController/src/main/res/layout/activity_ftc_controller.xml new file mode 100644 index 0000000..6524f94 --- /dev/null +++ b/FtcRobotController/src/main/res/layout/activity_ftc_controller.xml @@ -0,0 +1,184 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/FtcRobotController/src/main/res/menu/ftc_robot_controller.xml b/FtcRobotController/src/main/res/menu/ftc_robot_controller.xml new file mode 100644 index 0000000..657c1aa --- /dev/null +++ b/FtcRobotController/src/main/res/menu/ftc_robot_controller.xml @@ -0,0 +1,78 @@ + + +

+ + + + + + + + + + + + + + + diff --git a/FtcRobotController/src/main/res/raw/gold.wav b/FtcRobotController/src/main/res/raw/gold.wav new file mode 100644 index 0000000..3a7baf8 Binary files /dev/null and b/FtcRobotController/src/main/res/raw/gold.wav differ diff --git a/FtcRobotController/src/main/res/raw/silver.wav b/FtcRobotController/src/main/res/raw/silver.wav new file mode 100644 index 0000000..25918a7 Binary files /dev/null and b/FtcRobotController/src/main/res/raw/silver.wav differ diff --git a/FtcRobotController/src/main/res/values/dimens.xml b/FtcRobotController/src/main/res/values/dimens.xml new file mode 100644 index 0000000..63f1bab --- /dev/null +++ b/FtcRobotController/src/main/res/values/dimens.xml @@ -0,0 +1,40 @@ + + + + + + 16dp + 5dp + + \ No newline at end of file diff --git a/FtcRobotController/src/main/res/values/strings.xml b/FtcRobotController/src/main/res/values/strings.xml new file mode 100644 index 0000000..6ea191e --- /dev/null +++ b/FtcRobotController/src/main/res/values/strings.xml @@ -0,0 +1,72 @@ + + + + + + + FTC Robot Controller + + + Self Inspect + Program & Manage + Blocks + Settings + Restart Robot + Configure Robot + About + Exit + + + Configuration Complete + Restarting Robot + The Robot Controller must be fully up and running before entering Program and Manage Mode. + + + + @style/AppThemeRedRC + @style/AppThemeGreenRC + @style/AppThemeBlueRC + @style/AppThemePurpleRC + @style/AppThemeOrangeRC + @style/AppThemeTealRC + + + pref_ftc_season_year_of_current_rc_new + + @string/packageNameRobotController + + diff --git a/FtcRobotController/src/main/res/values/styles.xml b/FtcRobotController/src/main/res/values/styles.xml new file mode 100644 index 0000000..07689c0 --- /dev/null +++ b/FtcRobotController/src/main/res/values/styles.xml @@ -0,0 +1,23 @@ + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/FtcRobotController/src/main/res/xml/app_settings.xml b/FtcRobotController/src/main/res/xml/app_settings.xml new file mode 100644 index 0000000..58d3aa9 --- /dev/null +++ b/FtcRobotController/src/main/res/xml/app_settings.xml @@ -0,0 +1,93 @@ + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/FtcRobotController/src/main/res/xml/device_filter.xml b/FtcRobotController/src/main/res/xml/device_filter.xml new file mode 100644 index 0000000..7b75350 --- /dev/null +++ b/FtcRobotController/src/main/res/xml/device_filter.xml @@ -0,0 +1,49 @@ + + + + + + + + + + + + + + diff --git a/LICENSE b/LICENSE new file mode 100644 index 0000000..88b776b --- /dev/null +++ b/LICENSE @@ -0,0 +1,29 @@ +Copyright (c) 2014-2022 FIRST. All rights reserved. + +Redistribution and use in source and binary forms, with or without modification, +are permitted (subject to the limitations in the disclaimer below) provided that +the following conditions are met: + +Redistributions of source code must retain the above copyright notice, this list +of conditions and the following disclaimer. + +Redistributions in binary form must reproduce the above copyright notice, this +list of conditions and the following disclaimer in the documentation and/or +other materials provided with the distribution. + +Neither the name of FIRST nor the names of its contributors +may be used to endorse or promote products derived from this software without +specific prior written permission. + +NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS +LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS +IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR +ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES +(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON +ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT +(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS +SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + diff --git a/README.md b/README.md new file mode 100644 index 0000000..cf1ead0 --- /dev/null +++ b/README.md @@ -0,0 +1,117 @@ +# ftc-lib + +## Yes, ts was generated... No I do not believe in an AI takeover + +**ftc-lib** is a high-performance, state-machine-driven framework designed for FTC teams who prioritize clean code, modularity, and hardware optimization. It provides a robust abstraction layer over standard FTC OpModes, integrating **PedroPathing** for precision movement and **Panels (Sloth)** for live configuration and telemetry. + +--- + +## 🚀 Key Features + +* **Template Method Pattern:** A locked-down backend that handles the "boring stuff" (Bulk reads, FPS capping, Telemetry timing) so you only write robot logic. +* **Subsystem Architecture:** Fully isolated mechanisms with their own internal "Micro-States." +* **Hierarchical State Machines:** Orchestrate complex robot actions by mapping a "Global State" to specific "Subsystem States." +* **Automatic Hardware Optimization:** + * **Bulk Reads:** Automatically sets all expansion hubs to Manual Caching mode for the fastest possible loop times. + * **FPS Capping:** Prevents CPU/Battery waste by locking loop speeds to a target (e.g., 50 FPS). +* **Stateful PID Control:** A built-in PID utility that handles time-deltas ($dt$) and integral sums internally. +* **Unified Telemetry:** A joined engine that pipes data to both the Driver Station and the **Panels** dashboard simultaneously. +* **Live Tuning:** Centralized `Constants.java` utilizing `@Configurable` for real-time value editing without recompiling. + +--- + +## 📂 Project Structure + +```text +teamcode/ +├── lib/ # The Core Framework (Don't touch) +│ ├── BaseStateOpMode.java # The engine that runs the robot +│ ├── Subsystem.java # The blueprint for all mechanisms +│ ├── SubsystemManager.java # Automates the lifecycle of subsystems +│ └── PIDController.java # Stateful math utility +├── subsystems/ # Your robot parts (Drivetrain, Lift, Intake) +├── util/ # Utilities (AutoTransfer, FPSCounter) +├── Constants.java # The "Control Panel" for the entire robot +└── opmodes/ # Your actual TeleOp and Autonomous files +``` + +--- + +## 🛠 Usage Guide + +### 1. Define your Constants +Use the `Constants.java` file to store every hardware name, PID value, and speed multiplier. + +```java +@Configurable +public static class LIFT { + public static double kP = 0.015; + public static int SCORING_POS = 2500; +} +``` + +### 2. Create a Subsystem +Inherit from `Subsystem`. Define "Micro-States" for this specific mechanism. + +```java +public class LiftSubsystem extends Subsystem { + public enum LiftState { IDLE, EXTENDING } + private LiftState state = LiftState.IDLE; + + @Override + public void update() { + // Run PID logic here + } +} +``` + +### 3. Build your TeleOp +Inherit from `BaseStateOpMode`. This gives you the `stateMachineUpdate()` hook where you map gamepad inputs to states. + +```java +@TeleOp +public class MainTeleOp extends BaseStateOpMode { + + @Override + protected void setupSubsystems() { + manager.register(new DriveSubsystem(), new LiftSubsystem()); + } + + @Override + protected void stateMachineUpdate() { + if (gamepad1.aWasPressed()) { + robotState = GlobalState.SCORING; + } + } +} +``` + +--- + +## ⚡ Performance Details + +### The "Tick" Lifecycle +Every loop, `ftc-lib` executes in this strict order: +1. **Hardware Sync:** Clears Bulk Cache on all Hubs. +2. **Logic Tick:** Runs your `stateMachineUpdate()`. +3. **Subsystem Tick:** All subsystems calculate PIDs and update motor powers. +4. **Telemetry Gate:** If the `TELEMETRY_DELAY_MS` has passed, it pushes data to Panels and the Driver Station. +5. **Sync Sleep:** Adjusts thread sleep time to maintain a consistent `TARGET_FPS`. + +### Auton-to-TeleOp Persistence +Using the `AutoTransfer` utility, this library can automatically carry over the robot's end-of-auton position (from **PedroPathing**) and the Alliance color into TeleOp, ensuring your field-centric drive and automation remain seamless. + +--- + +## 📦 Dependencies +* [PedroPathing](https://github.com/pedropathing/pedro-pathing) +* [Panels/Configurables (Sloth)](https://panels.bylazar.com/) +* [Panels/Telemetry (Sloth)](https://panels.bylazar.com/) + +--- + +## 🤝 Contribution +When adding new subsystems: +1. Ensure all hardware names are in `Constants`. +2. Ensure `publishTelemetry` only sends data when `GLOBAL.DEBUG_MODE` is true. +3. Never use `sleep()` inside a subsystem; use state timers instead. \ No newline at end of file diff --git a/TeamCode/build.gradle b/TeamCode/build.gradle new file mode 100644 index 0000000..878287a --- /dev/null +++ b/TeamCode/build.gradle @@ -0,0 +1,28 @@ +// +// build.gradle in TeamCode +// +// Most of the definitions for building your module reside in a common, shared +// file 'build.common.gradle'. Being factored in this way makes it easier to +// integrate updates to the FTC into your code. If you really need to customize +// the build definitions, you can place those customizations in this file, but +// please think carefully as to whether such customizations are really necessary +// before doing so. + + +// Custom definitions may go here + +// Include common definitions from above. +apply from: '../build.common.gradle' +apply from: '../build.dependencies.gradle' + +android { + namespace = 'org.firstinspires.ftc.teamcode' + + packagingOptions { + jniLibs.useLegacyPackaging true + } +} + +dependencies { + implementation project(':FtcRobotController') +} diff --git a/TeamCode/lib/OpModeAnnotationProcessor.jar b/TeamCode/lib/OpModeAnnotationProcessor.jar new file mode 100644 index 0000000..4825cc3 Binary files /dev/null and b/TeamCode/lib/OpModeAnnotationProcessor.jar differ diff --git a/TeamCode/src/main/AndroidManifest.xml b/TeamCode/src/main/AndroidManifest.xml new file mode 100644 index 0000000..3705b31 --- /dev/null +++ b/TeamCode/src/main/AndroidManifest.xml @@ -0,0 +1,11 @@ + + + + + + + + diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/BaseOpMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/BaseOpMode.java new file mode 100644 index 0000000..18104b7 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/BaseOpMode.java @@ -0,0 +1,83 @@ +package org.firstinspires.ftc.teamcode.lib; + +import android.os.SystemClock; + +import com.qualcomm.hardware.lynx.LynxModule; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.util.ElapsedTime; +import org.firstinspires.ftc.robotcore.external.Telemetry; + +// PANELS IMPORTS +import com.bylazar.telemetry.JoinedTelemetry; + +import org.firstinspires.ftc.teamcode.teleOp.Constants; +import org.firstinspires.ftc.teamcode.util.FPSCounter; + +import java.util.List; + +public abstract class BaseOpMode extends OpMode { + protected final SubsysManager manager = new SubsysManager(); + protected final FPSCounter fps = new FPSCounter(); + private final ElapsedTime telemetryTimer = new ElapsedTime(); + private List allHubs; + + protected Telemetry unifiedTelemetry; + + protected abstract void setupSubsystems(); + protected abstract void stateMachineUpdate(); + protected void onInit() {} + + @Override + public final void init() { + unifiedTelemetry = new JoinedTelemetry(telemetry); + + allHubs = hardwareMap.getAll(LynxModule.class); + for (LynxModule hub : allHubs) { + hub.setBulkCachingMode(LynxModule.BulkCachingMode.MANUAL); + } + + setupSubsystems(); + + manager.initAll(hardwareMap); + + onInit(); + } + + @Override + public final void start() { + fps.reset(); + fps.startLoop(); + telemetryTimer.reset(); + } + + @Override + public final void loop() { + for (LynxModule hub : allHubs) { + hub.clearBulkCache(); + } + + fps.startLoop(); + stateMachineUpdate(); + + manager.updateAll(); + + if (telemetryTimer.milliseconds() >= Constants.GLOBAL.TELEMETRY_DELAY_MS) { + + manager.publishTelemetryAll(unifiedTelemetry); + + if (Constants.GLOBAL.DEBUG_MODE) { + unifiedTelemetry.addData("Loop Time (ms)", fps.getCurrentLoopTime()); + unifiedTelemetry.addData("Target FPS", Constants.GLOBAL.TARGET_FPS); + unifiedTelemetry.addData("Current FPS", fps.getAverageFps()); + } + + unifiedTelemetry.update(); + telemetryTimer.reset(); + } + + long sleepTime = fps.getSyncTime(Constants.GLOBAL.TARGET_FPS); + if (sleepTime > 0) { + SystemClock.sleep(sleepTime); + } + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/PIDController.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/PIDController.java new file mode 100644 index 0000000..fe6bd7f --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/PIDController.java @@ -0,0 +1,51 @@ +package org.firstinspires.ftc.teamcode.lib; + +import com.qualcomm.robotcore.util.ElapsedTime; + +public class PIDController { + private double kP, kI, kD; + + private double lastError = 0; + private double integralSum = 0; + private final ElapsedTime timer = new ElapsedTime(); + private boolean hasRunOnce = false; + + public PIDController(double kP, double kI, double kD) { + setCoefficients(kP, kI, kD); + } + + public void setCoefficients(double kP, double kI, double kD) { + this.kP = kP; + this.kI = kI; + this.kD = kD; + } + + public double calculate(double target, double current) { + double error = target - current; + + if (!hasRunOnce) { + timer.reset(); + lastError = error; + hasRunOnce = true; + return kP * error; + } + + double dt = timer.seconds(); + timer.reset(); + + if (dt == 0.0) return 0.0; + + double derivative = (error - lastError) / dt; + integralSum += (error * dt); + lastError = error; + + return (kP * error) + (kI * integralSum) + (kD * derivative); + } + + public void reset() { + integralSum = 0; + lastError = 0; + hasRunOnce = false; + timer.reset(); + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/SubsysManager.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/SubsysManager.java new file mode 100644 index 0000000..65a7811 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/SubsysManager.java @@ -0,0 +1,34 @@ +package org.firstinspires.ftc.teamcode.lib; + +import com.qualcomm.robotcore.hardware.HardwareMap; +import org.firstinspires.ftc.robotcore.external.Telemetry; + +import java.util.ArrayList; +import java.util.Arrays; +import java.util.List; + +public class SubsysManager { + private final List subsystems = new ArrayList<>(); + + public void register(Subsystem... subs) { + subsystems.addAll(Arrays.asList(subs)); + } + + public void initAll(HardwareMap hwMap) { + for (Subsystem sub : subsystems) { + sub.init(hwMap); + } + } + + public void updateAll() { + for (Subsystem sub : subsystems) { + sub.update(); + } + } + + public void publishTelemetryAll(Telemetry telemetry) { + for (Subsystem sub : subsystems) { + sub.publishTelemetry(telemetry); + } + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/Subsystem.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/Subsystem.java new file mode 100644 index 0000000..901200f --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/Subsystem.java @@ -0,0 +1,12 @@ +package org.firstinspires.ftc.teamcode.lib; + +import com.qualcomm.robotcore.hardware.HardwareMap; +import org.firstinspires.ftc.robotcore.external.Telemetry; + +public abstract class Subsystem { + public abstract void init(HardwareMap hwMap); + + public abstract void update(); + + public void publishTelemetry(Telemetry telemetry) {} +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java new file mode 100644 index 0000000..fa30420 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Constants.java @@ -0,0 +1,19 @@ +package org.firstinspires.ftc.teamcode.pedroPathing; + +import com.pedropathing.follower.Follower; +import com.pedropathing.follower.FollowerConstants; +import com.pedropathing.ftc.FollowerBuilder; +import com.pedropathing.paths.PathConstraints; +import com.qualcomm.robotcore.hardware.HardwareMap; + +public class Constants { + public static FollowerConstants followerConstants = new FollowerConstants(); + + public static PathConstraints pathConstraints = new PathConstraints(0.99, 100, 1, 1); + + public static Follower createFollower(HardwareMap hardwareMap) { + return new FollowerBuilder(followerConstants, hardwareMap) + .pathConstraints(pathConstraints) + .build(); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java new file mode 100644 index 0000000..d0942f4 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/Tuning.java @@ -0,0 +1,1585 @@ +package org.firstinspires.ftc.teamcode.pedroPathing; + +import static com.pedropathing.math.MathFunctions.quadraticFit; +import static org.firstinspires.ftc.teamcode.pedroPathing.Tuning.*; + +import android.annotation.SuppressLint; +import com.bylazar.configurables.PanelsConfigurables; +import com.bylazar.configurables.annotations.Configurable; +import com.bylazar.configurables.annotations.IgnoreConfigurable; +import com.bylazar.field.FieldManager; +import com.bylazar.field.PanelsField; +import com.bylazar.field.Style; +import com.bylazar.telemetry.PanelsTelemetry; +import com.bylazar.telemetry.TelemetryManager; +import com.pedropathing.follower.Follower; +import com.pedropathing.geometry.*; +import com.pedropathing.math.*; +import com.pedropathing.paths.*; +import com.pedropathing.telemetry.SelectableOpMode; +import com.pedropathing.util.*; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.util.ElapsedTime; + +import java.util.ArrayList; +import java.util.List; + +/** + * This is the Tuning class. It contains a selection menu for various tuning OpModes. + * + * @author Baron Henderson - 20077 The Indubitables + * @version 1.0, 6/26/2025 + */ +@Configurable +@TeleOp(name = "Tuning", group = "Pedro Pathing") +public class Tuning extends SelectableOpMode { + public static Follower follower; + + @IgnoreConfigurable + static PoseHistory poseHistory; + + @IgnoreConfigurable + static TelemetryManager telemetryM; + + @IgnoreConfigurable + static ArrayList changes = new ArrayList<>(); + + public Tuning() { + super("Select a Tuning OpMode", s -> { + s.folder("Localization", l -> { + l.add("Localization Test", LocalizationTest::new); + l.add("Offsets Tuner", OffsetsTuner::new); + l.add("Forward Tuner", ForwardTuner::new); + l.add("Lateral Tuner", LateralTuner::new); + l.add("Turn Tuner", TurnTuner::new); + }); + s.folder("Automatic", a -> { + a.add("Forward Velocity Tuner", ForwardVelocityTuner::new); + a.add("Lateral Velocity Tuner", LateralVelocityTuner::new); + a.add("Forward Zero Power Acceleration Tuner", ForwardZeroPowerAccelerationTuner::new); + a.add("Lateral Zero Power Acceleration Tuner", LateralZeroPowerAccelerationTuner::new); + a.add("Predictive Braking", PredictiveBrakingTuner::new); + }); + s.folder("Manual", p -> { + p.add("Translational Tuner", TranslationalTuner::new); + p.add("Heading Tuner", HeadingTuner::new); + p.add("Drive Tuner", DriveTuner::new); + p.add("Line Tuner", Line::new); + p.add("Centripetal Tuner", CentripetalTuner::new); + }); + s.folder("Tests", p -> { + p.add("Line", Line::new); + p.add("Triangle", Triangle::new); + p.add("Circle", Circle::new); + }); + }); + } + + @Override + public void onSelect() { + if (follower == null) { + follower = Constants.createFollower(hardwareMap); + PanelsConfigurables.INSTANCE.refreshClass(this); + } else { + follower = Constants.createFollower(hardwareMap); + } + + follower.setStartingPose(new Pose()); + + poseHistory = follower.getPoseHistory(); + + telemetryM = PanelsTelemetry.INSTANCE.getTelemetry(); + + Drawing.init(); + } + + @Override + public void onLog(List lines) {} + + public static void drawOnlyCurrent() { + try { + Drawing.drawRobot(follower.getPose()); + Drawing.sendPacket(); + } catch (Exception e) { + throw new RuntimeException("Drawing failed " + e); + } + } + + public static void draw() { + Drawing.drawDebug(follower); + } + + /** This creates a full stop of the robot by setting the drive motors to run at 0 power. */ + public static void stopRobot() { + follower.startTeleopDrive(true); + follower.setTeleOpDrive(0,0,0,true); + } +} + +/** + * This is the LocalizationTest OpMode. This is basically just a simple mecanum drive attached to a + * PoseUpdater. The OpMode will print out the robot's pose to telemetry as well as draw the robot. + * You should use this to check the robot's localization. + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Baron Henderson - 20077 The Indubitables + * @version 1.0, 5/6/2024 + */ +class LocalizationTest extends OpMode { + @Override + public void init() { + follower.setStartingPose(new Pose(72,72)); + } + + /** This initializes the PoseUpdater, the mecanum drive motors, and the Panels telemetry. */ + @Override + public void init_loop() { + telemetryM.debug("This will print your robot's position to telemetry while " + + "allowing robot control through a basic mecanum drive on gamepad 1."); + telemetryM.update(telemetry); + follower.update(); + drawOnlyCurrent(); + } + + @Override + public void start() { + follower.startTeleopDrive(); + follower.update(); + } + + /** + * This updates the robot's pose estimate, the simple mecanum drive, and updates the + * Panels telemetry with the robot's position as well as draws the robot's position. + */ + @Override + public void loop() { + follower.setTeleOpDrive(-gamepad1.left_stick_y, -gamepad1.left_stick_x, -gamepad1.right_stick_x, true); + follower.update(); + + telemetryM.debug("x:" + follower.getPose().getX()); + telemetryM.debug("y:" + follower.getPose().getY()); + telemetryM.debug("heading:" + follower.getPose().getHeading()); + telemetryM.debug("total heading:" + follower.getTotalHeading()); + telemetryM.update(telemetry); + + draw(); + } +} + +/** + * This is the ForwardTuner OpMode. This tracks the forward movement of the robot and displays the + * necessary ticks to inches multiplier. This displayed multiplier is what's necessary to scale the + * robot's current distance in ticks to the specified distance in inches. So, to use this, run the + * tuner, then pull/push the robot to the specified distance using a ruler on the ground. When you're + * at the end of the distance, record the ticks to inches multiplier. Feel free to run multiple trials + * and average the results. Then, input the multiplier into the forward ticks to inches in your + * localizer of choice. + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Baron Henderson - 20077 The Indubitables + * @version 1.0, 5/6/2024 + */ +class ForwardTuner extends OpMode { + public static double DISTANCE = 48; + + @Override + public void init() { + follower.setStartingPose(new Pose(72,72)); + follower.update(); + drawOnlyCurrent(); + } + + /** This initializes the PoseUpdater as well as the Panels telemetry. */ + @Override + public void init_loop() { + telemetryM.debug("Pull your robot forward " + DISTANCE + " inches. Your forward ticks to inches will be shown on the telemetry."); + telemetryM.update(telemetry); + drawOnlyCurrent(); + } + + /** + * This updates the robot's pose estimate, and updates the Panels telemetry with the + * calculated multiplier and draws the robot. + */ + @Override + public void loop() { + follower.update(); + + telemetryM.debug("Distance Moved: " + (follower.getPose().getX() - 72)); + telemetryM.debug("The multiplier will display what your forward ticks to inches should be to scale your current distance to " + DISTANCE + " inches."); + telemetryM.debug("Multiplier: " + (DISTANCE / ((follower.getPose().getX() - 72) / follower.getPoseTracker().getLocalizer().getForwardMultiplier()))); + telemetryM.update(telemetry); + + draw(); + } +} + +/** + * This is the LateralTuner OpMode. This tracks the strafe movement of the robot and displays the + * necessary ticks to inches multiplier. This displayed multiplier is what's necessary to scale the + * robot's current distance in ticks to the specified distance in inches. So, to use this, run the + * tuner, then pull/push the robot to the specified distance using a ruler on the ground. When you're + * at the end of the distance, record the ticks to inches multiplier. Feel free to run multiple trials + * and average the results. Then, input the multiplier into the strafe ticks to inches in your + * localizer of choice. + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Baron Henderson - 20077 The Indubitables + * @version 2.0, 6/26/2025 + */ +class LateralTuner extends OpMode { + public static double DISTANCE = 48; + + @Override + public void init() { + follower.setStartingPose(new Pose(72,72)); + follower.update(); + drawOnlyCurrent(); + } + + /** This initializes the PoseUpdater as well as the Panels telemetry. */ + @Override + public void init_loop() { + telemetryM.debug("Pull your robot to the right " + DISTANCE + " inches. Your strafe ticks to inches will be shown on the telemetry."); + telemetryM.update(telemetry); + drawOnlyCurrent(); + } + + /** + * This updates the robot's pose estimate, and updates the Panels telemetry with the + * calculated multiplier and draws the robot. + */ + @Override + public void loop() { + follower.update(); + + telemetryM.debug("Distance Moved: " + (follower.getPose().getY() - 72)); + telemetryM.debug("The multiplier will display what your strafe ticks to inches should be to scale your current distance to " + DISTANCE + " inches."); + telemetryM.debug("Multiplier: " + (DISTANCE / ((follower.getPose().getY() - 72) / follower.getPoseTracker().getLocalizer().getLateralMultiplier()))); + telemetryM.update(telemetry); + + draw(); + } +} + +/** + * This is the TurnTuner OpMode. This tracks the turning movement of the robot and displays the + * necessary ticks to inches multiplier. This displayed multiplier is what's necessary to scale the + * robot's current angle in ticks to the specified angle in radians. So, to use this, run the + * tuner, then pull/push the robot to the specified angle using a protractor or lines on the ground. + * When you're at the end of the angle, record the ticks to inches multiplier. Feel free to run + * multiple trials and average the results. Then, input the multiplier into the turning ticks to + * radians in your localizer of choice. + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Baron Henderson - 20077 The Indubitables + * @version 1.0, 5/6/2024 + */ +class TurnTuner extends OpMode { + public static double ANGLE = 2 * Math.PI; + + @Override + public void init() { + follower.setStartingPose(new Pose(72,72)); + follower.update(); + drawOnlyCurrent(); + } + + /** This initializes the PoseUpdater as well as the Panels telemetry. */ + @Override + public void init_loop() { + telemetryM.debug("Turn your robot " + ANGLE + " radians. Your turn ticks to inches will be shown on the telemetry."); + telemetryM.update(telemetry); + + drawOnlyCurrent(); + } + + /** + * This updates the robot's pose estimate, and updates the Panels telemetry with the + * calculated multiplier and draws the robot. + */ + @Override + public void loop() { + follower.update(); + + telemetryM.debug("Total Angle: " + follower.getTotalHeading()); + telemetryM.debug("The multiplier will display what your turn ticks to inches should be to scale your current angle to " + ANGLE + " radians."); + telemetryM.debug("Multiplier: " + (ANGLE / (follower.getTotalHeading() / follower.getPoseTracker().getLocalizer().getTurningMultiplier()))); + telemetryM.update(telemetry); + + draw(); + } +} + +/** + * This is the ForwardVelocityTuner autonomous follower OpMode. This runs the robot forwards at max + * power until it reaches some specified distance. It records the most recent velocities, and on + * reaching the end of the distance, it averages them and prints out the velocity obtained. It is + * recommended to run this multiple times on a full battery to get the best results. What this does + * is, when paired with StrafeVelocityTuner, allows FollowerConstants to create a Vector that + * empirically represents the direction your mecanum wheels actually prefer to go in, allowing for + * more accurate following. + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @author Baron Henderson - 20077 The Indubitables + * @version 1.0, 3/13/2024 + */ +class ForwardVelocityTuner extends OpMode { + private final ArrayList velocities = new ArrayList<>(); + public static double DISTANCE = 48; + public static double RECORD_NUMBER = 10; + + private boolean end; + + @Override + public void init() { + follower.setStartingPose(new Pose(72, 72)); + } + + /** This initializes the drive motors as well as the cache of velocities and the Panels telemetry. */ + @Override + public void init_loop() { + telemetryM.debug("The robot will run at 1 power until it reaches " + DISTANCE + " inches forward."); + telemetryM.debug("Make sure you have enough room, since the robot has inertia after cutting power."); + telemetryM.debug("After running the distance, the robot will cut power from the drivetrain and display the forward velocity."); + telemetryM.debug("Press B on game pad 1 to stop."); + telemetryM.debug("pose", follower.getPose()); + telemetryM.update(telemetry); + follower.update(); + drawOnlyCurrent(); + } + + /** This starts the OpMode by setting the drive motors to run forward at full power. */ + @Override + public void start() { + for (int i = 0; i < RECORD_NUMBER; i++) { + velocities.add(0.0); + } + follower.startTeleopDrive(true); + follower.update(); + end = false; + } + + /** + * This runs the OpMode. At any point during the running of the OpMode, pressing B on + * game pad 1 will stop the OpMode. This continuously records the RECORD_NUMBER most recent + * velocities, and when the robot has run forward enough, these last velocities recorded are + * averaged and printed. + */ + @Override + public void loop() { + if (gamepad1.bWasPressed()) { + stopRobot(); + requestOpModeStop(); + } + + follower.update(); + draw(); + + + if (!end) { + if (Math.abs(follower.getPose().getX()) > (DISTANCE + 72)) { + end = true; + stopRobot(); + } else { + follower.setTeleOpDrive(1,0,0,true); + //double currentVelocity = Math.abs(follower.getVelocity().getXComponent()); + double currentVelocity = Math.abs(follower.poseTracker.getLocalizer().getVelocity().getX()); + velocities.add(currentVelocity); + velocities.remove(0); + } + } else { + stopRobot(); + double average = 0; + for (double velocity : velocities) { + average += velocity; + } + average /= velocities.size(); + telemetryM.debug("Forward Velocity: " + average); + telemetryM.debug("\n"); + telemetryM.debug("Press A to set the Forward Velocity temporarily (while robot remains on)."); + + for (int i = 0; i < velocities.size(); i++) { + telemetry.addData(String.valueOf(i), velocities.get(i)); + } + + telemetryM.update(telemetry); + telemetry.update(); + + if (gamepad1.aWasPressed()) { + follower.setXVelocity(average); + String message = "XMovement: " + average; + changes.add(message); + } + } + } +} + +/** + * This is the StrafeVelocityTuner autonomous follower OpMode. This runs the robot left at max + * power until it reaches some specified distance. It records the most recent velocities, and on + * reaching the end of the distance, it averages them and prints out the velocity obtained. It is + * recommended to run this multiple times on a full battery to get the best results. What this does + * is, when paired with ForwardVelocityTuner, allows FollowerConstants to create a Vector that + * empirically represents the direction your mecanum wheels actually prefer to go in, allowing for + * more accurate following. + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @author Baron Henderson - 20077 The Indubitables + * @version 1.0, 3/13/2024 + */ +class LateralVelocityTuner extends OpMode { + private final ArrayList velocities = new ArrayList<>(); + + public static double DISTANCE = 48; + public static double RECORD_NUMBER = 10; + + private boolean end; + + @Override + public void init() { + follower.setStartingPose(new Pose(72, 72)); + } + + /** + * This initializes the drive motors as well as the cache of velocities and the Panels + * telemetryM. + */ + @Override + public void init_loop() { + telemetryM.debug("The robot will run at 1 power until it reaches " + DISTANCE + " inches to the left."); + telemetryM.debug("Make sure you have enough room, since the robot has inertia after cutting power."); + telemetryM.debug("After running the distance, the robot will cut power from the drivetrain and display the strafe velocity."); + telemetryM.debug("Press B on Gamepad 1 to stop."); + telemetryM.update(telemetry); + follower.update(); + drawOnlyCurrent(); + } + + /** This starts the OpMode by setting the drive motors to run left at full power. */ + @Override + public void start() { + for (int i = 0; i < RECORD_NUMBER; i++) { + velocities.add(0.0); + } + follower.startTeleopDrive(true); + follower.update(); + } + + /** + * This runs the OpMode. At any point during the running of the OpMode, pressing B on + * game pad1 will stop the OpMode. This continuously records the RECORD_NUMBER most recent + * velocities, and when the robot has run sideways enough, these last velocities recorded are + * averaged and printed. + */ + @Override + public void loop() { + if (gamepad1.bWasPressed()) { + stopRobot(); + requestOpModeStop(); + } + + follower.update(); + draw(); + + if (!end) { + if (Math.abs(follower.getPose().getY()) > (DISTANCE + 72)) { + end = true; + stopRobot(); + } else { + follower.setTeleOpDrive(0,1,0,true); + double currentVelocity = Math.abs(follower.getVelocity().dot(new Vector(1, Math.PI / 2))); + velocities.add(currentVelocity); + velocities.remove(0); + } + } else { + stopRobot(); + double average = 0; + for (double velocity : velocities) { + average += velocity; + } + average /= velocities.size(); + + telemetryM.debug("Strafe Velocity: " + average); + telemetryM.debug("\n"); + telemetryM.debug("Press A to set the Lateral Velocity temporarily (while robot remains on)."); + telemetryM.update(telemetry); + + if (gamepad1.aWasPressed()) { + follower.setYVelocity(average); + String message = "YMovement: " + average; + changes.add(message); + } + } + } +} + +/** + * This is the ForwardZeroPowerAccelerationTuner autonomous follower OpMode. This runs the robot + * forward until a specified velocity is achieved. Then, the robot cuts power to the motors, setting + * them to zero power. The deceleration, or negative acceleration, is then measured until the robot + * stops. The accelerations across the entire time the robot is slowing down is then averaged and + * that number is then printed. This is used to determine how the robot will decelerate in the + * forward direction when power is cut, making the estimations used in the calculations for the + * drive Vector more accurate and giving better braking at the end of Paths. + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Baron Henderson - 20077 The Indubitables + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @version 1.0, 3/13/2024 + */ +class ForwardZeroPowerAccelerationTuner extends OpMode { + private final ArrayList accelerations = new ArrayList<>(); + public static double VELOCITY = 30; + + private double previousVelocity; + private long previousTimeNano; + + private boolean stopping; + private boolean end; + + @Override + public void init() { + follower.setStartingPose(new Pose(72, 72)); + } + + /** This initializes the drive motors as well as the Panels telemetryM. */ + @Override + public void init_loop() { + telemetryM.debug("The robot will run forward until it reaches " + VELOCITY + " inches per second."); + telemetryM.debug("Then, it will cut power from the drivetrain and roll to a stop."); + telemetryM.debug("Make sure you have enough room."); + telemetryM.debug("After stopping, the forward zero power acceleration (natural deceleration) will be displayed."); + telemetryM.debug("Press B on Gamepad 1 to stop."); + telemetryM.update(telemetry); + follower.update(); + drawOnlyCurrent(); + } + + /** This starts the OpMode by setting the drive motors to run forward at full power. */ + @Override + public void start() { + follower.startTeleopDrive(false); + follower.update(); + follower.setTeleOpDrive(1,0,0,true); + } + + /** + * This runs the OpMode. At any point during the running of the OpMode, pressing B on + * game pad 1 will stop the OpMode. When the robot hits the specified velocity, the robot will + * record its deceleration / negative acceleration until it stops. Then, it will average all the + * recorded deceleration / negative acceleration and print that value. + */ + @Override + public void loop() { + if (gamepad1.bWasPressed()) { + stopRobot(); + requestOpModeStop(); + } + + follower.update(); + draw(); + + Vector heading = new Vector(1.0, follower.getPose().getHeading()); + if (!end) { + if (!stopping) { + if (follower.getVelocity().dot(heading) > VELOCITY) { + previousVelocity = follower.getVelocity().dot(heading); + previousTimeNano = System.nanoTime(); + stopping = true; + follower.setTeleOpDrive(0,0,0,true); + } + } else { + double currentVelocity = follower.getVelocity().dot(heading); + accelerations.add((currentVelocity - previousVelocity) / ((System.nanoTime() - previousTimeNano) / Math.pow(10.0, 9))); + previousVelocity = currentVelocity; + previousTimeNano = System.nanoTime(); + if (currentVelocity < follower.getConstraints().getVelocityConstraint()) { + end = true; + } + } + } else { + double average = 0; + for (double acceleration : accelerations) { + average += acceleration; + } + average /= accelerations.size(); + + telemetryM.debug("Forward Zero Power Acceleration (Deceleration): " + average); + telemetryM.debug("\n"); + telemetryM.debug("Press A to set the Forward Zero Power Acceleration temporarily (while robot remains on)."); + telemetryM.update(telemetry); + + if (gamepad1.aWasPressed()) { + follower.getConstants().setForwardZeroPowerAcceleration(average); + String message = "Forward Zero Power Acceleration: " + average; + changes.add(message); + } + } + } +} + +/** + * This is the LateralZeroPowerAccelerationTuner autonomous follower OpMode. This runs the robot + * to the left until a specified velocity is achieved. Then, the robot cuts power to the motors, setting + * them to zero power. The deceleration, or negative acceleration, is then measured until the robot + * stops. The accelerations across the entire time the robot is slowing down is then averaged and + * that number is then printed. This is used to determine how the robot will decelerate in the + * forward direction when power is cut, making the estimations used in the calculations for the + * drive Vector more accurate and giving better braking at the end of Paths. + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @author Baron Henderson - 20077 The Indubitables + * @version 1.0, 3/13/2024 + */ +class LateralZeroPowerAccelerationTuner extends OpMode { + private final ArrayList accelerations = new ArrayList<>(); + public static double VELOCITY = 30; + private double previousVelocity; + private long previousTimeNano; + private boolean stopping; + private boolean end; + + @Override + public void init() { + follower.setStartingPose(new Pose(72, 72)); + } + + /** This initializes the drive motors as well as the Panels telemetry. */ + @Override + public void init_loop() { + telemetryM.debug("The robot will run to the left until it reaches " + VELOCITY + " inches per second."); + telemetryM.debug("Then, it will cut power from the drivetrain and roll to a stop."); + telemetryM.debug("Make sure you have enough room."); + telemetryM.debug("After stopping, the lateral zero power acceleration (natural deceleration) will be displayed."); + telemetryM.debug("Press B on game pad 1 to stop."); + telemetryM.update(telemetry); + follower.update(); + drawOnlyCurrent(); + } + + /** This starts the OpMode by setting the drive motors to run forward at full power. */ + @Override + public void start() { + follower.startTeleopDrive(false); + follower.update(); + follower.setTeleOpDrive(0,1,0,true); + } + + /** + * This runs the OpMode. At any point during the running of the OpMode, pressing B on + * game pad 1 will stop the OpMode. When the robot hits the specified velocity, the robot will + * record its deceleration / negative acceleration until it stops. Then, it will average all the + * recorded deceleration / negative acceleration and print that value. + */ + @Override + public void loop() { + if (gamepad1.bWasPressed()) { + stopRobot(); + requestOpModeStop(); + } + + follower.update(); + draw(); + + Vector heading = new Vector(1.0, follower.getPose().getHeading() - Math.PI / 2); + if (!end) { + if (!stopping) { + if (Math.abs(follower.getVelocity().dot(heading)) > VELOCITY) { + previousVelocity = Math.abs(follower.getVelocity().dot(heading)); + previousTimeNano = System.nanoTime(); + stopping = true; + follower.setTeleOpDrive(0,0,0,true); + } + } else { + double currentVelocity = Math.abs(follower.getVelocity().dot(heading)); + accelerations.add((currentVelocity - previousVelocity) / ((System.nanoTime() - previousTimeNano) / Math.pow(10.0, 9))); + previousVelocity = currentVelocity; + previousTimeNano = System.nanoTime(); + if (currentVelocity < follower.getConstraints().getVelocityConstraint()) { + end = true; + } + } + } else { + double average = 0; + for (double acceleration : accelerations) { + average += acceleration; + } + average /= accelerations.size(); + + telemetryM.debug("Lateral Zero Power Acceleration (Deceleration): " + average); + telemetryM.debug("\n"); + telemetryM.debug("Press A to set the Lateral Zero Power Acceleration temporarily (while robot remains on)."); + telemetryM.update(telemetry); + + if (gamepad1.aWasPressed()) { + follower.getConstants().setLateralZeroPowerAcceleration(average); + String message = "Lateral Zero Power Acceleration: " + average; + changes.add(message); + } + } + } +} + +/** + * This is the Translational PIDF Tuner OpMode. It will keep the robot in place. + * The user should push the robot laterally to test the PIDF and adjust the PIDF values accordingly. + * + * @author Baron Henderson - 20077 The Indubitables + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @version 1.0, 3/12/2024 + */ +class TranslationalTuner extends OpMode { + public static double DISTANCE = 40; + private boolean forward = true; + + private Path forwards; + private Path backwards; + + @Override + public void init() { + follower.setStartingPose(new Pose(72, 72)); + } + + /** This initializes the Follower and creates the forward and backward Paths. */ + @Override + public void init_loop() { + telemetryM.debug("This will activate the translational PIDF(s)"); + telemetryM.debug("The robot will try to stay in place while you push it laterally."); + telemetryM.debug("You can adjust the PIDF values to tune the robot's translational PIDF(s)."); + telemetryM.update(telemetry); + follower.update(); + drawOnlyCurrent(); + } + + @Override + public void start() { + follower.deactivateAllPIDFs(); + follower.activateTranslational(); + forwards = new Path(new BezierLine(new Pose(72,72), new Pose(DISTANCE + 72,72))); + forwards.setConstantHeadingInterpolation(0); + backwards = new Path(new BezierLine(new Pose(DISTANCE + 72,72), new Pose(72,72))); + backwards.setConstantHeadingInterpolation(0); + follower.followPath(forwards); + } + + /** This runs the OpMode, updating the Follower as well as printing out the debug statements to the Telemetry */ + @Override + public void loop() { + follower.update(); + draw(); + + if (!follower.isBusy()) { + if (forward) { + forward = false; + follower.followPath(backwards); + } else { + forward = true; + follower.followPath(forwards); + } + } + + telemetryM.debug("Push the robot laterally to test the Translational PIDF(s)."); + telemetryM.addData("Zero Line", 0); + telemetryM.addData("Error X", follower.errorCalculator.getTranslationalError().getXComponent()); + telemetryM.addData("Error Y", follower.errorCalculator.getTranslationalError().getYComponent()); + telemetryM.update(telemetry); + } +} + +/** + * This is the Heading PIDF Tuner OpMode. It will keep the robot in place. + * The user should try to turn the robot to test the PIDF and adjust the PIDF values accordingly. + * It will try to keep the robot at a constant heading while the user tries to turn it. + * + * @author Baron Henderson - 20077 The Indubitables + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @version 1.0, 3/12/2024 + */ +class HeadingTuner extends OpMode { + public static double DISTANCE = 40; + private boolean forward = true; + + private Path forwards; + private Path backwards; + + @Override + public void init() { + follower.setStartingPose(new Pose(72, 72)); + } + + /** + * This initializes the Follower and creates the forward and backward Paths. Additionally, this + * initializes the Panels telemetry. + */ + @Override + public void init_loop() { + telemetryM.debug("This will activate the heading PIDF(s)."); + telemetryM.debug("The robot will try to stay at a constant heading while you try to turn it."); + telemetryM.debug("You can adjust the PIDF values to tune the robot's heading PIDF(s)."); + telemetryM.update(telemetry); + follower.update(); + drawOnlyCurrent(); + } + + @Override + public void start() { + follower.deactivateAllPIDFs(); + follower.activateHeading(); + forwards = new Path(new BezierLine(new Pose(72,72), new Pose(DISTANCE + 72,72))); + forwards.setConstantHeadingInterpolation(0); + backwards = new Path(new BezierLine(new Pose(DISTANCE + 72,72), new Pose(72,72))); + backwards.setConstantHeadingInterpolation(0); + follower.followPath(forwards); + } + + /** + * This runs the OpMode, updating the Follower as well as printing out the debug statements to + * the Telemetry, as well as the Panels. + */ + @Override + public void loop() { + follower.update(); + draw(); + + if (!follower.isBusy()) { + if (forward) { + forward = false; + follower.followPath(backwards); + } else { + forward = true; + follower.followPath(forwards); + } + } + + telemetryM.debug("Turn the robot manually to test the Heading PIDF(s)."); + telemetryM.addData("Zero Line", 0); + telemetryM.addData("Error", follower.errorCalculator.getHeadingError()); + telemetryM.update(telemetry); + } +} + +/** + * This is the Drive PIDF Tuner OpMode. It will run the robot in a straight line going forward and back. + * + * @author Baron Henderson - 20077 The Indubitables + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @version 1.0, 3/12/2024 + */ +class DriveTuner extends OpMode { + public static double DISTANCE = 40; + private boolean forward = true; + + private PathChain forwards; + private PathChain backwards; + + @Override + public void init() { + follower.setStartingPose(new Pose(72, 72)); + } + + /** + * This initializes the Follower and creates the forward and backward Paths. Additionally, this + * initializes the Panels telemetry. + */ + @Override + public void init_loop() { + telemetryM.debug("This will run the robot in a straight line going " + DISTANCE + "inches forward."); + telemetryM.debug("The robot will go forward and backward continuously along the path."); + telemetryM.debug("Make sure you have enough room."); + telemetryM.update(telemetry); + follower.update(); + drawOnlyCurrent(); + } + + @Override + public void start() { + follower.deactivateAllPIDFs(); + follower.activateDrive(); + + forwards = follower.pathBuilder() + .setGlobalDeceleration() + .addPath(new BezierLine(new Pose(72,72), new Pose(DISTANCE + 72,72))) + .setConstantHeadingInterpolation(0) + .build(); + + backwards = follower.pathBuilder() + .setGlobalDeceleration() + .addPath(new BezierLine(new Pose(DISTANCE + 72,72), new Pose(72,72))) + .setConstantHeadingInterpolation(0) + .build(); + + follower.followPath(forwards); + } + + /** + * This runs the OpMode, updating the Follower as well as printing out the debug statements to + * the Telemetry, as well as the Panels. + */ + @Override + public void loop() { + follower.update(); + draw(); + + if (!follower.isBusy()) { + if (forward) { + forward = false; + follower.followPath(backwards); + } else { + forward = true; + follower.followPath(forwards); + } + } + + telemetryM.debug("Driving forward?: " + forward); + telemetryM.addData("Zero Line", 0); + telemetryM.addData("Error", follower.errorCalculator.getDriveErrors()[1]); + telemetryM.update(telemetry); + } +} + +/** + * This is the Line Test Tuner OpMode. It will drive the robot forward and back + * The user should push the robot laterally and angular to test out the drive, heading, and translational PIDFs. + * + * @author Baron Henderson - 20077 The Indubitables + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @version 1.0, 3/12/2024 + */ +class Line extends OpMode { + public static double DISTANCE = 40; + private boolean forward = true; + + private Path forwards; + private Path backwards; + + @Override + public void init() { + follower.setStartingPose(new Pose(72, 72)); + } + + /** This initializes the Follower and creates the forward and backward Paths. */ + @Override + public void init_loop() { + telemetryM.debug("This will activate all the PIDF(s)"); + telemetryM.debug("The robot will go forward and backward continuously along the path while correcting."); + telemetryM.debug("You can adjust the PIDF values to tune the robot's drive PIDF(s)."); + telemetryM.update(telemetry); + follower.update(); + drawOnlyCurrent(); + } + + @Override + public void start() { + follower.activateAllPIDFs(); + forwards = new Path(new BezierLine(new Pose(72,72), new Pose(DISTANCE + 72,72))); + forwards.setConstantHeadingInterpolation(0); + backwards = new Path(new BezierLine(new Pose(DISTANCE + 72,72), new Pose(72,72))); + backwards.setConstantHeadingInterpolation(0); + follower.followPath(forwards); + } + + /** This runs the OpMode, updating the Follower as well as printing out the debug statements to the Telemetry */ + @Override + public void loop() { + follower.update(); + draw(); + + if (!follower.isBusy()) { + if (forward) { + forward = false; + follower.followPath(backwards); + } else { + forward = true; + follower.followPath(forwards); + } + } + + telemetryM.debug("Driving Forward?: " + forward); + telemetryM.update(telemetry); + } +} + +/** + * This is the Centripetal Tuner OpMode. It runs the robot in a specified distance + * forward and to the left. On reaching the end of the forward Path, the robot runs the backward + * Path the same distance back to the start. Rinse and repeat! This is good for testing a variety + * of Vectors, like the drive Vector, the translational Vector, the heading Vector, and the + * centripetal Vector. + * + * @author Baron Henderson - 20077 The Indubitables + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @version 1.0, 3/13/2024 + */ +class CentripetalTuner extends OpMode { + public static double DISTANCE = 20; + private boolean forward = true; + + private Path forwards; + private Path backwards; + + @Override + public void init() { + follower.setStartingPose(new Pose(72, 72)); + } + + /** + * This initializes the Follower and creates the forward and backward Paths. + * Additionally, this initializes the Panels telemetry. + */ + @Override + public void init_loop() { + telemetryM.debug("This will run the robot in a curve going " + DISTANCE + " inches to the left and the same number of inches forward."); + telemetryM.debug("The robot will go continuously along the path."); + telemetryM.debug("Make sure you have enough room."); + telemetryM.update(telemetry); + follower.update(); + drawOnlyCurrent(); + } + + @Override + public void start() { + follower.activateAllPIDFs(); + forwards = new Path(new BezierCurve(new Pose(72,72), new Pose(Math.abs(DISTANCE) + 72,72), new Pose(Math.abs(DISTANCE) + 72,DISTANCE + 72))); + backwards = new Path(new BezierCurve(new Pose(Math.abs(DISTANCE) + 72,DISTANCE + 72), new Pose(Math.abs(DISTANCE) + 72,72), new Pose(72,72))); + + backwards.setTangentHeadingInterpolation(); + backwards.reverseHeadingInterpolation(); + + follower.followPath(forwards); + } + + /** + * This runs the OpMode, updating the Follower as well as printing out the debug statements to + * the Telemetry, as well as the Panels. + */ + @Override + public void loop() { + follower.update(); + draw(); + if (!follower.isBusy()) { + if (forward) { + forward = false; + follower.followPath(backwards); + } else { + forward = true; + follower.followPath(forwards); + } + } + + telemetryM.debug("Driving away from the origin along the curve?: " + forward); + telemetryM.update(telemetry); + } +} + +/** + * This is the Triangle autonomous OpMode. + * It runs the robot in a triangle, with the starting point being the bottom-middle point. + * + * @author Baron Henderson - 20077 The Indubitables + * @author Samarth Mahapatra - 1002 CircuitRunners Robotics Surge + * @version 1.0, 12/30/2024 + */ +class Triangle extends OpMode { + + private final Pose startPose = new Pose(72, 72, Math.toRadians(0)); + private final Pose interPose = new Pose(24 + 72, -24 + 72, Math.toRadians(90)); + private final Pose endPose = new Pose(24 + 72, 24 + 72, Math.toRadians(45)); + + private PathChain triangle; + + /** + * This runs the OpMode, updating the Follower as well as printing out the debug statements to + * the Telemetry, as well as the Panels. + */ + @Override + public void loop() { + follower.update(); + draw(); + + if (follower.atParametricEnd()) { + follower.followPath(triangle, true); + } + } + + @Override + public void init() { + follower.setStartingPose(new Pose(72, 72)); + } + + @Override + public void init_loop() { + telemetryM.debug("This will run in a roughly triangular shape, starting on the bottom-middle point."); + telemetryM.debug("So, make sure you have enough space to the left, front, and right to run the OpMode."); + telemetryM.update(telemetry); + follower.update(); + drawOnlyCurrent(); + } + + /** Creates the PathChain for the "triangle".*/ + @Override + public void start() { + follower.setStartingPose(startPose); + + triangle = follower.pathBuilder() + .addPath(new BezierLine(startPose, interPose)) + .setLinearHeadingInterpolation(startPose.getHeading(), interPose.getHeading()) + .addPath(new BezierLine(interPose, endPose)) + .setLinearHeadingInterpolation(interPose.getHeading(), endPose.getHeading()) + .addPath(new BezierLine(endPose, startPose)) + .setLinearHeadingInterpolation(endPose.getHeading(), startPose.getHeading()) + .build(); + + follower.followPath(triangle); + } +} + +/** + * This is the Circle autonomous OpMode. It runs the robot in a PathChain that's actually not quite + * a circle, but some Bezier curves that have control points set essentially in a square. However, + * it turns enough to tune your centripetal force correction and some of your heading. Some lag in + * heading is to be expected. + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @version 1.0, 3/12/2024 + */ +class Circle extends OpMode { + public static double RADIUS = 10; + private PathChain circle; + + public void start() { + circle = follower.pathBuilder() + .addPath(new BezierCurve(new Pose(72, 72), new Pose(RADIUS + 72, 72), new Pose(RADIUS + 72, RADIUS + 72))) + .setHeadingInterpolation(HeadingInterpolator.facingPoint(72, RADIUS + 72)) + .addPath(new BezierCurve(new Pose(RADIUS + 72, RADIUS + 72), new Pose(RADIUS + 72, (2 * RADIUS) + 72), new Pose(72, (2 * RADIUS) + 72))) + .setHeadingInterpolation(HeadingInterpolator.facingPoint(72, RADIUS + 72)) + .addPath(new BezierCurve(new Pose(72, (2 * RADIUS) + 72), new Pose(-RADIUS + 72, (2 * RADIUS) + 72), new Pose(-RADIUS + 72, RADIUS + 72))) + .setHeadingInterpolation(HeadingInterpolator.facingPoint(72, RADIUS + 72)) + .addPath(new BezierCurve(new Pose(-RADIUS + 72, RADIUS + 72), new Pose(-RADIUS + 72, 72), new Pose(72, 72))) + .setHeadingInterpolation(HeadingInterpolator.facingPoint(72, RADIUS + 72)) + .build(); + follower.followPath(circle); + } + + @Override + public void init_loop() { + telemetryM.debug("This will run in a roughly circular shape of radius " + RADIUS + ", starting on the right-most edge. "); + telemetryM.debug("So, make sure you have enough space to the left, front, and back to run the OpMode."); + telemetryM.debug("It will also continuously face the center of the circle to test your heading and centripetal correction."); + telemetryM.update(telemetry); + follower.update(); + drawOnlyCurrent(); + } + + @Override + public void init() { + follower.setStartingPose(new Pose(72, 72)); + } + + /** + * This runs the OpMode, updating the Follower as well as printing out the debug statements to + * the Telemetry, as well as the FTC Dashboard. + */ + @Override + public void loop() { + follower.update(); + draw(); + + if (follower.atParametricEnd()) { + follower.followPath(circle); + } + } +} + +/** + * This is the OffsetsTuner OpMode. This tracks the movement of the robot as it turns 180 degrees, + * and calculates what the robot's strafeX and forwardY offsets should be. Ensure that your strafeX and forwardY offsets + * are set to 0 before running this OpMode. After running, input the displayed offsets into your localizer constants. + * + * @author Havish Sripada - 12808 RevAmped Robotics + * @author Baron Henderson + */ +class OffsetsTuner extends OpMode { + @Override + public void init() { + follower.setStartingPose(new Pose(72,72)); + follower.update(); + drawOnlyCurrent(); + } + + /** This initializes the PoseUpdater as well as the Panels telemetry. */ + @Override + public void init_loop() { + telemetryM.debug("Prerequisite: Make sure both your offsets are set to 0 in your localizer constants."); + telemetryM.debug("Turn your robot " + Math.PI + " radians. Your offsets in inches will be shown on the telemetry."); + telemetryM.update(telemetry); + + drawOnlyCurrent(); + } + + /** + * This updates the robot's pose estimate, and updates the Panels telemetry with the + * calculated offsets and draws the robot. + */ + @Override + public void loop() { + follower.update(); + + telemetryM.debug("Total Angle: " + follower.getTotalHeading()); + + telemetryM.debug("The following values are the offsets in inches that should be applied to your localizer."); + telemetryM.debug("strafeX: " + ((72.0-follower.getPose().getX()) / 2.0)); + telemetryM.debug("forwardY: " + ((72.0-follower.getPose().getY()) / 2.0)); + telemetryM.update(telemetry); + + draw(); + } +} + +/** + * This is the Predictive Braking Tuner. It runs the robot forward and backward at various power + * levels, recording the robot’s velocity and position immediately before braking. The motors are + * then set to zero-power brake mode, which represents the fastest theoretical braking the robot + * can achieve. Once the robot comes to a complete stop, the tuner measures the stopping distance. + * Using the collected data, it generates a velocity-vs-stopping-distance graph and fits a + * quadratic curve to model the braking behavior. + * + * @author Ashay Sarda - 19745 Turtle Walkers + * @author Jacob Ophoven - 18535 Frozen Code + * @version 1.0, 12/26/2025 + */ +class PredictiveBrakingTuner extends OpMode { + private static final double[] TEST_POWERS = + {1, 1, 1, 0.9, 0.9, 0.8, 0.7, 0.6, 0.5, 0.4, 0.3, 0.2}; + + private static final int DRIVE_TIME_MS = 1000; + private static final int BRAKE_WAIT_MS = 500; + + private enum State { + START_MOVE, + WAIT_DRIVE_TIME, + APPLY_BRAKE, + WAIT_BRAKE_TIME, + RECORD, + DONE + } + + private static class BrakeRecord { + double timeMs; + Pose pose; + double velocity; + + BrakeRecord(double timeMs, Pose pose, double velocity) { + this.timeMs = timeMs; + this.pose = pose; + this.velocity = velocity; + } + } + + private State state = State.START_MOVE; + + private final ElapsedTime timer = new ElapsedTime(); + + private int iteration = 0; + + private Vector startPosition; + private double measuredVelocity; + + private final List velocityToBrakingDistance = new ArrayList<>(); + private final List brakeData = new ArrayList<>(); + + @Override + public void init() {} + + @Override + public void init_loop() { + telemetryM.debug("The robot will move forwards and backwards starting at max speed and slowing down."); + telemetryM.debug("Make sure you have enough room. Leave at least 4-5 feet."); + telemetryM.debug("After stopping, kFriction and kBraking will be displayed."); + telemetryM.debug("Make sure to turn the timer off."); + telemetryM.debug("Press B on game pad 1 to stop."); + telemetryM.update(telemetry); + follower.update(); + drawOnlyCurrent(); + } + + @Override + public void start() { + timer.reset(); + follower.update(); + follower.startTeleOpDrive(true); + } + + @SuppressLint("DefaultLocale") + @Override + public void loop() { + follower.update(); + + if (gamepad1.b) { + stopRobot(); + requestOpModeStop(); + return; + } + + switch (state) { + case START_MOVE: { + if (iteration >= TEST_POWERS.length) { + state = State.DONE; + break; + } + + double currentPower = TEST_POWERS[iteration]; + follower.setMaxPower(currentPower); + if (iteration % 2 != 0) { + follower.setTeleOpDrive(-1, 0, 0, true); + } else { + follower.setTeleOpDrive(1, 0, 0, true); + } + + timer.reset(); + state = State.WAIT_DRIVE_TIME; + break; + } + + case WAIT_DRIVE_TIME: { + if (timer.milliseconds() >= DRIVE_TIME_MS) { + measuredVelocity = follower.getVelocity().getMagnitude(); + startPosition = follower.getPose().getAsVector(); + state = State.APPLY_BRAKE; + } + break; + } + + case APPLY_BRAKE: { + stopRobot(); + + timer.reset(); + state = State.WAIT_BRAKE_TIME; + break; + } + + case WAIT_BRAKE_TIME: { + double t = timer.milliseconds(); + Pose currentPose = follower.getPose(); + double currentVelocity = follower.getVelocity().getMagnitude(); + + brakeData.add(new BrakeRecord(t, currentPose, currentVelocity)); + + if (timer.milliseconds() >= BRAKE_WAIT_MS || follower.getVelocity().getMagnitude() <= .05) { + state = State.RECORD; + } + break; + } + + case RECORD: { + Vector endPosition = follower.getPose().getAsVector(); + double brakingDistance = endPosition.minus(startPosition).getMagnitude(); + + velocityToBrakingDistance.add(new double[]{measuredVelocity, brakingDistance}); + + telemetryM.debug("Test " + iteration, + String.format("v=%.3f d=%.3f", measuredVelocity, + brakingDistance)); + telemetryM.update(telemetry); + + iteration++; + state = State.START_MOVE; + + break; + } + + case DONE: { + stopRobot(); + + double[] coefficients = quadraticFit(velocityToBrakingDistance); + + telemetryM.debug("Tuning Complete"); + telemetryM.debug("Braking Profile:"); + telemetryM.debug("kQuadratic", coefficients[1]); + telemetryM.debug("kLinear", coefficients[0]); + telemetryM.update(telemetry); + telemetryM.debug("Tuning Complete"); + telemetryM.debug("Braking Profile:"); + telemetryM.debug("kQuadraticFriction", coefficients[1]); + telemetryM.debug("kLinearBraking", coefficients[0]); + for (BrakeRecord record : brakeData) { + Pose p = record.pose; + telemetryM.debug(String.format("t=%.0f ms, x=%.2f, y=%.2f, θ=%.2f, v=%.2f", + record.timeMs, p.getX(), p.getY(), + p.getHeading(), + record.velocity)); + } + telemetryM.update(); + break; + } + } + + telemetry.update(); + } +} + +/** + * This is the Drawing class. It handles the drawing of stuff on Panels Dashboard, like the robot. + * + * @author Lazar - 19234 + * @version 1.1, 5/19/2025 + */ +class Drawing { + public static final double ROBOT_RADIUS = 9; // woah + private static final FieldManager panelsField = PanelsField.INSTANCE.getField(); + + private static final Style robotLook = new Style( + "", "#3F51B5", 0.75 + ); + private static final Style historyLook = new Style( + "", "#4CAF50", 0.75 + ); + + /** + * This prepares Panels Field for using Pedro Offsets + */ + public static void init() { + panelsField.setOffsets(PanelsField.INSTANCE.getPresets().getPEDRO_PATHING()); + } + + /** + * This draws everything that will be used in the Follower's telemetryDebug() method. This takes + * a Follower as an input, so an instance of the DashbaordDrawingHandler class is not needed. + * + * @param follower Pedro Follower instance. + */ + public static void drawDebug(Follower follower) { + if (follower.getCurrentPath() != null) { + drawPath(follower.getCurrentPath(), robotLook); + Pose closestPoint = follower.getPointFromPath(follower.getCurrentPath().getClosestPointTValue()); + drawRobot(new Pose(closestPoint.getX(), closestPoint.getY(), follower.getCurrentPath().getHeadingGoal(follower.getCurrentPath().getClosestPointTValue())), robotLook); + } + drawPoseHistory(follower.getPoseHistory(), historyLook); + drawRobot(follower.getPose(), historyLook); + + sendPacket(); + } + + /** + * This draws a robot at a specified Pose with a specified + * look. The heading is represented as a line. + * + * @param pose the Pose to draw the robot at + * @param style the parameters used to draw the robot with + */ + public static void drawRobot(Pose pose, Style style) { + if (pose == null || Double.isNaN(pose.getX()) || Double.isNaN(pose.getY()) || Double.isNaN(pose.getHeading())) { + return; + } + + panelsField.setStyle(style); + panelsField.moveCursor(pose.getX(), pose.getY()); + panelsField.circle(ROBOT_RADIUS); + + Vector v = pose.getHeadingAsUnitVector(); + v.setMagnitude(v.getMagnitude() * ROBOT_RADIUS); + double x1 = pose.getX() + v.getXComponent() / 2, y1 = pose.getY() + v.getYComponent() / 2; + double x2 = pose.getX() + v.getXComponent(), y2 = pose.getY() + v.getYComponent(); + + panelsField.setStyle(style); + panelsField.moveCursor(x1, y1); + panelsField.line(x2, y2); + } + + /** + * This draws a robot at a specified Pose. The heading is represented as a line. + * + * @param pose the Pose to draw the robot at + */ + public static void drawRobot(Pose pose) { + drawRobot(pose, robotLook); + } + + /** + * This draws a Path with a specified look. + * + * @param path the Path to draw + * @param style the parameters used to draw the Path with + */ + public static void drawPath(Path path, Style style) { + double[][] points = path.getPanelsDrawingPoints(); + + for (int i = 0; i < points[0].length; i++) { + for (int j = 0; j < points.length; j++) { + if (Double.isNaN(points[j][i])) { + points[j][i] = 0; + } + } + } + + panelsField.setStyle(style); + panelsField.moveCursor(points[0][0], points[0][1]); + panelsField.line(points[1][0], points[1][1]); + } + + /** + * This draws all the Paths in a PathChain with a + * specified look. + * + * @param pathChain the PathChain to draw + * @param style the parameters used to draw the PathChain with + */ + public static void drawPath(PathChain pathChain, Style style) { + for (int i = 0; i < pathChain.size(); i++) { + drawPath(pathChain.getPath(i), style); + } + } + + /** + * This draws the pose history of the robot. + * + * @param poseTracker the PoseHistory to get the pose history from + * @param style the parameters used to draw the pose history with + */ + public static void drawPoseHistory(PoseHistory poseTracker, Style style) { + panelsField.setStyle(style); + + int size = poseTracker.getXPositionsArray().length; + for (int i = 0; i < size - 1; i++) { + + panelsField.moveCursor(poseTracker.getXPositionsArray()[i], poseTracker.getYPositionsArray()[i]); + panelsField.line(poseTracker.getXPositionsArray()[i + 1], poseTracker.getYPositionsArray()[i + 1]); + } + } + + /** + * This draws the pose history of the robot. + * + * @param poseTracker the PoseHistory to get the pose history from + */ + public static void drawPoseHistory(PoseHistory poseTracker) { + drawPoseHistory(poseTracker, historyLook); + } + + /** + * This tries to send the current packet to FTControl Panels. + */ + public static void sendPacket() { + panelsField.update(); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/readme.md b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/readme.md new file mode 100644 index 0000000..4d1da42 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/readme.md @@ -0,0 +1,131 @@ +## TeamCode Module + +Welcome! + +This module, TeamCode, is the place where you will write/paste the code for your team's +robot controller App. This module is currently empty (a clean slate) but the +process for adding OpModes is straightforward. + +## Creating your own OpModes + +The easiest way to create your own OpMode is to copy a Sample OpMode and make it your own. + +Sample opmodes exist in the FtcRobotController module. +To locate these samples, find the FtcRobotController module in the "Project/Android" tab. + +Expand the following tree elements: + FtcRobotController/java/org.firstinspires.ftc.robotcontroller/external/samples + +### Naming of Samples + +To gain a better understanding of how the samples are organized, and how to interpret the +naming system, it will help to understand the conventions that were used during their creation. + +These conventions are described (in detail) in the sample_conventions.md file in this folder. + +To summarize: A range of different samples classes will reside in the java/external/samples. +The class names will follow a naming convention which indicates the purpose of each class. +The prefix of the name will be one of the following: + +Basic: This is a minimally functional OpMode used to illustrate the skeleton/structure + of a particular style of OpMode. These are bare bones examples. + +Sensor: This is a Sample OpMode that shows how to use a specific sensor. + It is not intended to drive a functioning robot, it is simply showing the minimal code + required to read and display the sensor values. + +Robot: This is a Sample OpMode that assumes a simple two-motor (differential) drive base. + It may be used to provide a common baseline driving OpMode, or + to demonstrate how a particular sensor or concept can be used to navigate. + +Concept: This is a sample OpMode that illustrates performing a specific function or concept. + These may be complex, but their operation should be explained clearly in the comments, + or the comments should reference an external doc, guide or tutorial. + Each OpMode should try to only demonstrate a single concept so they are easy to + locate based on their name. These OpModes may not produce a drivable robot. + +After the prefix, other conventions will apply: + +* Sensor class names are constructed as: Sensor - Company - Type +* Robot class names are constructed as: Robot - Mode - Action - OpModetype +* Concept class names are constructed as: Concept - Topic - OpModetype + +Once you are familiar with the range of samples available, you can choose one to be the +basis for your own robot. In all cases, the desired sample(s) needs to be copied into +your TeamCode module to be used. + +This is done inside Android Studio directly, using the following steps: + + 1) Locate the desired sample class in the Project/Android tree. + + 2) Right click on the sample class and select "Copy" + + 3) Expand the TeamCode/java folder + + 4) Right click on the org.firstinspires.ftc.teamcode folder and select "Paste" + + 5) You will be prompted for a class name for the copy. + Choose something meaningful based on the purpose of this class. + Start with a capital letter, and remember that there may be more similar classes later. + +Once your copy has been created, you should prepare it for use on your robot. +This is done by adjusting the OpMode's name, and enabling it to be displayed on the +Driver Station's OpMode list. + +Each OpMode sample class begins with several lines of code like the ones shown below: + +``` + @TeleOp(name="Template: Linear OpMode", group="Linear Opmode") + @Disabled +``` + +The name that will appear on the driver station's "opmode list" is defined by the code: + ``name="Template: Linear OpMode"`` +You can change what appears between the quotes to better describe your opmode. +The "group=" portion of the code can be used to help organize your list of OpModes. + +As shown, the current OpMode will NOT appear on the driver station's OpMode list because of the + ``@Disabled`` annotation which has been included. +This line can simply be deleted , or commented out, to make the OpMode visible. + + + +## ADVANCED Multi-Team App management: Cloning the TeamCode Module + +In some situations, you have multiple teams in your club and you want them to all share +a common code organization, with each being able to *see* the others code but each having +their own team module with their own code that they maintain themselves. + +In this situation, you might wish to clone the TeamCode module, once for each of these teams. +Each of the clones would then appear along side each other in the Android Studio module list, +together with the FtcRobotController module (and the original TeamCode module). + +Selective Team phones can then be programmed by selecting the desired Module from the pulldown list +prior to clicking to the green Run arrow. + +Warning: This is not for the inexperienced Software developer. +You will need to be comfortable with File manipulations and managing Android Studio Modules. +These changes are performed OUTSIDE of Android Studios, so close Android Studios before you do this. + +Also.. Make a full project backup before you start this :) + +To clone TeamCode, do the following: + +Note: Some names start with "Team" and others start with "team". This is intentional. + +1) Using your operating system file management tools, copy the whole "TeamCode" + folder to a sibling folder with a corresponding new name, eg: "Team0417". + +2) In the new Team0417 folder, delete the TeamCode.iml file. + +3) the new Team0417 folder, rename the "src/main/java/org/firstinspires/ftc/teamcode" folder + to a matching name with a lowercase 'team' eg: "team0417". + +4) In the new Team0417/src/main folder, edit the "AndroidManifest.xml" file, change the line that contains + package="org.firstinspires.ftc.teamcode" + to be + package="org.firstinspires.ftc.team0417" + +5) Add: include ':Team0417' to the "/settings.gradle" file. + +6) Open up Android Studios and clean out any old files by using the menu to "Build/Clean Project"" \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsys/Drivetrain.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsys/Drivetrain.java new file mode 100644 index 0000000..6cc342c --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/subsys/Drivetrain.java @@ -0,0 +1,94 @@ +package org.firstinspires.ftc.teamcode.subsys; + +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.HardwareMap; +import org.firstinspires.ftc.robotcore.external.Telemetry; + +import org.firstinspires.ftc.teamcode.teleOp.Constants; +import org.firstinspires.ftc.teamcode.lib.Subsystem; + +public class Drivetrain extends Subsystem { + + // Subsystem Micro-States + public enum DriveState { + NORMAL, + PRECISION, + LOCKED // Safety state to halt motors instantly + } + + private DriveState currentState = DriveState.NORMAL; + + private DcMotor frontLeft, frontRight, backLeft, backRight; + + private double driveY = 0.0; + private double driveX = 0.0; + private double driveTurn = 0.0; + + @Override + public void init(HardwareMap hwMap) { + frontLeft = hwMap.get(DcMotor.class, Constants.DRIVE.FL_NAME); + frontRight = hwMap.get(DcMotor.class, Constants.DRIVE.FR_NAME); + backLeft = hwMap.get(DcMotor.class, Constants.DRIVE.BL_NAME); + backRight = hwMap.get(DcMotor.class, Constants.DRIVE.BR_NAME); + + frontRight.setDirection(DcMotor.Direction.REVERSE); + backRight.setDirection(DcMotor.Direction.REVERSE); + + frontLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + frontRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + backLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + backRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE); + } + + // Command Hooks for the OpMode to control this subsystem + public void setTargetState(DriveState newState) { + this.currentState = newState; + } + + public void setDriveVectors(double y, double x, double turn) { + this.driveY = y; + this.driveX = x; + this.driveTurn = turn; + } + + @Override + public void update() { + if (currentState == DriveState.LOCKED) { + frontLeft.setPower(0); frontRight.setPower(0); + backLeft.setPower(0); backRight.setPower(0); + return; + } + + double speedMultiplier = (currentState == DriveState.PRECISION) + ? Constants.DRIVE.PRECISION_SPEED + : Constants.DRIVE.NORMAL_SPEED; + + double flPower = (driveY + driveX + driveTurn) * speedMultiplier; + double frPower = (driveY - driveX - driveTurn) * speedMultiplier; + double blPower = (driveY - driveX + driveTurn) * speedMultiplier; + double brPower = (driveY + driveX - driveTurn) * speedMultiplier; + + // Normalization (prevents math outputting numbers > 1.0) + double max = Math.max(Math.abs(flPower), Math.abs(frPower)); + max = Math.max(max, Math.abs(blPower)); + max = Math.max(max, Math.abs(brPower)); + + if (max > 1.0) { + flPower /= max; frPower /= max; + blPower /= max; brPower /= max; + } + + frontLeft.setPower(flPower); + frontRight.setPower(frPower); + backLeft.setPower(blPower); + backRight.setPower(brPower); + } + + @Override + public void publishTelemetry(Telemetry telemetry) { + if (Constants.GLOBAL.DEBUG_MODE) { + telemetry.addData("Drive State", currentState.toString()); + telemetry.addData("Inputs (Y/X/T)", String.format("%.2f / %.2f / %.2f", driveY, driveX, driveTurn)); + } + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleOp/Constants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleOp/Constants.java new file mode 100644 index 0000000..ac81a53 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleOp/Constants.java @@ -0,0 +1,27 @@ +package org.firstinspires.ftc.teamcode.teleOp; + +import com.bylazar.configurables.annotations.Configurable; + +@Configurable +public class Constants { + + @Configurable + public static class GLOBAL { + public static double TELEMETRY_DELAY_MS = 250.0; + public static boolean DEBUG_MODE = true; + public static int TARGET_FPS = 0; // No target FPS (UNCAPPED) + } + + @Configurable + public static class DRIVE { + // Hardware Names + public static final String FL_NAME = "fl"; + public static final String FR_NAME = "fr"; + public static final String BL_NAME = "bl"; + public static final String BR_NAME = "br"; + + // Speed Constants + public static double NORMAL_SPEED = 0.6; + public static double PRECISION_SPEED = 0.35; + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleOp/teleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleOp/teleOp.java new file mode 100644 index 0000000..9d8e830 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/teleOp/teleOp.java @@ -0,0 +1,56 @@ +package org.firstinspires.ftc.teamcode.teleOp; + +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import org.firstinspires.ftc.teamcode.lib.BaseOpMode; +import org.firstinspires.ftc.teamcode.subsys.Drivetrain; +import org.firstinspires.ftc.teamcode.util.AutoTransfer; + +@TeleOp(name = "Competition TeleOp", group = "Main") +public class teleOp extends BaseOpMode { + + public enum GlobalState { + IDLE, + SCORING + } + + private GlobalState robotState = GlobalState.IDLE; + private Drivetrain drive; + + @Override + protected void setupSubsystems() { + drive = new Drivetrain(); + manager.register(drive); + } + + @Override + protected void onInit() { + if (AutoTransfer.hasValidData()) { + unifiedTelemetry.addData("Loaded Alliance", AutoTransfer.getAllianceString()); + unifiedTelemetry.update(); + } + } + + @Override + protected void stateMachineUpdate() { + drive.setDriveVectors(-gamepad1.left_stick_y, gamepad1.left_stick_x, gamepad1.right_stick_x); + + switch (robotState) { + + case IDLE: + drive.setTargetState(Drivetrain.DriveState.NORMAL); + + if (gamepad1.aWasPressed()) { + robotState = GlobalState.SCORING; + } + break; + + case SCORING: + drive.setTargetState(Drivetrain.DriveState.PRECISION); + + if (gamepad1.bWasPressed()) { + robotState = GlobalState.IDLE; + } + break; + } + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/AutoTransfer.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/AutoTransfer.java new file mode 100755 index 0000000..fc91998 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/AutoTransfer.java @@ -0,0 +1,55 @@ +package org.firstinspires.ftc.teamcode.util; + +import com.pedropathing.geometry.Pose; + +public class AutoTransfer { + + public static Pose transferPose = new Pose(0, 0, 0); + + public static int alliance = 0; + + public static int motifPattern = 0; + + public static boolean isAutonRan = false; + + public static void reset() { + transferPose = new Pose(0, 0, 0); + alliance = 0; + motifPattern = 0; + isAutonRan = false; + } + + public static void initAuton(int allianceColor) { + reset(); + alliance = allianceColor; + isAutonRan = true; + } + + public static void setMotifPattern(int pattern) { + motifPattern = pattern; + } + + public static void updatePose(Pose pose) { + transferPose = pose; + } + + public static String getAllianceString() { + switch (alliance) { + case 1: return "BLUE"; + case 2: return "RED"; + default: return "UNKNOWN"; + } + } + public static String getMotifString() { + switch (motifPattern) { + case 1: return "Pattern 1 (GPP)"; + case 2: return "Pattern 2 (PGP)"; + case 3: return "Pattern 3 (PPG)"; + default: return "UNKNOWN"; + } + } + + public static boolean hasValidData() { + return isAutonRan && alliance != 0; + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/FPSCounter.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/FPSCounter.java new file mode 100755 index 0000000..659813e --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/FPSCounter.java @@ -0,0 +1,62 @@ +package org.firstinspires.ftc.teamcode.util; + +import com.qualcomm.robotcore.util.ElapsedTime; + +public class FPSCounter { + private final ElapsedTime loopTimer = new ElapsedTime(); + private double loopTimeSum = 0; + private int loopCount = 0; + private double currentLoopTime = 0; + private double lastTime = 0; + + public FPSCounter() { + reset(); + } + + public void startLoop() { + double now = loopTimer.milliseconds(); + if (loopCount > 0) { + currentLoopTime = now - lastTime; + loopTimeSum += currentLoopTime; + } + lastTime = now; + loopCount++; + } + + public long getSyncTime(int targetFps) { + if (targetFps <= 0) return 0; + + double targetMs = 1000.0 / targetFps; + double now = loopTimer.milliseconds(); + double elapsedWorkMs = now - lastTime; + double remainingMs = targetMs - elapsedWorkMs; + + return (remainingMs > 0) ? (long) remainingMs : 0; + } + + public double getCurrentFps() { + return (currentLoopTime > 0) ? 1000.0 / currentLoopTime : 0; + } + + public double getAverageFps() { + if (loopCount <= 1) return 0; + return 1000.0 / (loopTimeSum / (loopCount - 1)); + } + + public double getCurrentLoopTime() { + return currentLoopTime; + } + + public double getAverageLoopTime() { + if (loopCount <= 1) return 0; + return loopTimeSum / (loopCount - 1); + } + + public void reset() { + loopTimer.reset(); + lastTime = 0; + loopTimeSum = 0; + loopCount = 0; + currentLoopTime = 0; + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/OATDrawing.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/OATDrawing.java new file mode 100755 index 0000000..0c42eb0 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/OATDrawing.java @@ -0,0 +1,58 @@ +package org.firstinspires.ftc.teamcode.util; + +import com.bylazar.field.FieldManager; +import com.bylazar.field.PanelsField; +import com.bylazar.field.Style; +import com.pedropathing.geometry.*; +import com.pedropathing.math.*; +import com.pedropathing.paths.*; + +public class OATDrawing { + public static final double ROBOT_RADIUS = 9; + private static final FieldManager panelsField = PanelsField.INSTANCE.getField(); + + private static final Style robotLook = new Style("", "#3F51B5", 0.75); + + private static final Style goalLook = new Style("", "#F44336", 0.9); + + private static final Style vectorLook = new Style("", "#4CAF50", 0.8); + + private static final Style visionLook = new Style("", "#FFD700", 0.9); + + public static void init() { + panelsField.setOffsets(PanelsField.INSTANCE.getPresets().getPEDRO_PATHING()); + } + public static void drawOATState(Pose robotPose, Pose goalPose, boolean hasVisionLock) { + if (robotPose == null || goalPose == null) return; + + drawRobot(robotPose, robotLook); + + panelsField.setStyle(goalLook); + panelsField.moveCursor(goalPose.getX(), goalPose.getY()); + panelsField.circle(4); + + panelsField.setStyle(hasVisionLock ? visionLook : vectorLook); + panelsField.moveCursor(robotPose.getX(), robotPose.getY()); + panelsField.line(goalPose.getX(), goalPose.getY()); + + panelsField.update(); + } + + private static void drawRobot(Pose pose, Style style) { + if (pose == null || Double.isNaN(pose.getX()) || Double.isNaN(pose.getY()) || Double.isNaN(pose.getHeading())) { + return; + } + + panelsField.setStyle(style); + panelsField.moveCursor(pose.getX(), pose.getY()); + panelsField.circle(ROBOT_RADIUS); + + Vector v = pose.getHeadingAsUnitVector(); + v.setMagnitude(v.getMagnitude() * ROBOT_RADIUS); + double x2 = pose.getX() + v.getXComponent(); + double y2 = pose.getY() + v.getYComponent(); + + panelsField.moveCursor(pose.getX(), pose.getY()); + panelsField.line(x2, y2); + } +} \ No newline at end of file diff --git a/TeamCode/src/main/res/raw/readme.md b/TeamCode/src/main/res/raw/readme.md new file mode 100644 index 0000000..355a3c4 --- /dev/null +++ b/TeamCode/src/main/res/raw/readme.md @@ -0,0 +1 @@ +Place your sound files in this folder to use them as project resources. \ No newline at end of file diff --git a/TeamCode/src/main/res/values/strings.xml b/TeamCode/src/main/res/values/strings.xml new file mode 100644 index 0000000..d781ec5 --- /dev/null +++ b/TeamCode/src/main/res/values/strings.xml @@ -0,0 +1,4 @@ + + + + diff --git a/TeamCode/src/main/res/xml/teamwebcamcalibrations.xml b/TeamCode/src/main/res/xml/teamwebcamcalibrations.xml new file mode 100644 index 0000000..22ae7a8 --- /dev/null +++ b/TeamCode/src/main/res/xml/teamwebcamcalibrations.xml @@ -0,0 +1,161 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/build.common.gradle b/build.common.gradle new file mode 100644 index 0000000..160f84f --- /dev/null +++ b/build.common.gradle @@ -0,0 +1,117 @@ +/** + * build.common.gradle + * + * Try to avoid editing this file, as it may be updated from time to time as the FTC SDK + * evolves. Rather, if it is necessary to customize the build process, do those edits in + * the build.gradle file in TeamCode. + * + * This file contains the necessary content of the 'build.gradle' files for robot controller + * applications built using the FTC SDK. Each individual 'build.gradle' in those applications + * can simply contain the one line: + * + * apply from: '../build.common.gradle' + * + * which will pick up this file here. This approach allows makes it easier to integrate + * updates to the FTC SDK into your code. + */ + +import java.util.regex.Pattern + +apply plugin: 'com.android.application' + +android { + + compileSdk 34 + + signingConfigs { + release { + def apkStoreFile = System.getenv("APK_SIGNING_STORE_FILE") + if (apkStoreFile != null) { + keyAlias System.getenv("APK_SIGNING_KEY_ALIAS") + keyPassword System.getenv("APK_SIGNING_KEY_PASSWORD") + storeFile file(System.getenv("APK_SIGNING_STORE_FILE")) + storePassword System.getenv("APK_SIGNING_STORE_PASSWORD") + } else { + keyAlias 'androiddebugkey' + keyPassword 'android' + storeFile rootProject.file('libs/ftc.debug.keystore') + storePassword 'android' + } + } + + debug { + keyAlias 'androiddebugkey' + keyPassword 'android' + storeFile rootProject.file('libs/ftc.debug.keystore') + storePassword 'android' + } + } + + defaultConfig { + signingConfig signingConfigs.debug + applicationId 'com.qualcomm.ftcrobotcontroller' + minSdkVersion 24 + //noinspection ExpiredTargetSdkVersion + targetSdkVersion 28 + + /** + * We keep the versionCode and versionName of robot controller applications in sync with + * the master information published in the AndroidManifest.xml file of the FtcRobotController + * module. This helps avoid confusion that might arise from having multiple versions of + * a robot controller app simultaneously installed on a robot controller device. + * + * We accomplish this with the help of a funky little Groovy script that maintains that + * correspondence automatically. + * + * @see Configure Your Build + * @see Versioning Your App + */ + def manifestFile = project(':FtcRobotController').file('src/main/AndroidManifest.xml'); + def manifestText = manifestFile.getText() + // + def vCodePattern = Pattern.compile("versionCode=\"(\\d+(\\.\\d+)*)\"") + def matcher = vCodePattern.matcher(manifestText) + matcher.find() + def vCode = Integer.parseInt(matcher.group(1)) + // + def vNamePattern = Pattern.compile("versionName=\"(.*)\"") + matcher = vNamePattern.matcher(manifestText); + matcher.find() + def vName = matcher.group(1) + // + versionCode vCode + versionName vName + } + + // http://google.github.io/android-gradle-dsl/current/com.android.build.gradle.internal.dsl.BuildType.html + buildTypes { + release { + signingConfig signingConfigs.release + + ndk { + abiFilters "armeabi-v7a", "arm64-v8a" + } + } + debug { + debuggable true + jniDebuggable true + ndk { + abiFilters "armeabi-v7a", "arm64-v8a" + } + } + } + + compileOptions { + sourceCompatibility JavaVersion.VERSION_1_8 + targetCompatibility JavaVersion.VERSION_1_8 + } + + packagingOptions { + pickFirst '**/*.so' + } + ndkVersion '21.3.6528147' +} + +repositories { +} + diff --git a/build.dependencies.gradle b/build.dependencies.gradle new file mode 100644 index 0000000..8a0a2b4 --- /dev/null +++ b/build.dependencies.gradle @@ -0,0 +1,22 @@ +repositories { + mavenCentral() + google() // Needed for androidx + maven { url = "https://mymaven.bylazar.com/releases" } +} + +dependencies { + implementation 'org.firstinspires.ftc:Inspection:11.1.0' + implementation 'org.firstinspires.ftc:Blocks:11.1.0' + implementation 'org.firstinspires.ftc:RobotCore:11.1.0' + implementation 'org.firstinspires.ftc:RobotServer:11.1.0' + implementation 'org.firstinspires.ftc:OnBotJava:11.1.0' + implementation 'org.firstinspires.ftc:Hardware:11.1.0' + implementation 'org.firstinspires.ftc:FtcCommon:11.1.0' + implementation 'org.firstinspires.ftc:Vision:11.1.0' + implementation 'androidx.appcompat:appcompat:1.2.0' + + implementation 'com.pedropathing:ftc:2.1.0' + implementation 'com.pedropathing:telemetry:1.0.0' + implementation 'com.bylazar:fullpanels:1.0.12' +} + diff --git a/build.gradle b/build.gradle new file mode 100644 index 0000000..e70f209 --- /dev/null +++ b/build.gradle @@ -0,0 +1,29 @@ +/** + * Top-level build file for ftc_app project. + * + * It is extraordinarily rare that you will ever need to edit this file. + */ + +buildscript { + repositories { + mavenCentral() + google() + } + dependencies { + // Note for FTC Teams: Do not modify this yourself. + classpath 'com.android.tools.build:gradle:8.7.0' + } +} + +// This is now required because aapt2 has to be downloaded from the +// google() repository beginning with version 3.2 of the Android Gradle Plugin +allprojects { + repositories { + mavenCentral() + google() + } +} + +repositories { + mavenCentral() +} diff --git a/doc/legal/AudioBlocksSounds.txt b/doc/legal/AudioBlocksSounds.txt new file mode 100644 index 0000000..4eab3bc --- /dev/null +++ b/doc/legal/AudioBlocksSounds.txt @@ -0,0 +1,21 @@ +The sound files listed below in this SDK were purchased from www.audioblocks.com under the +following license. + + http://support.audioblocks.com/customer/en/portal/topics/610636-licensing-faq-s/articles + + How am I allowed to use your content? + Last Updated: Aug 11, 2016 01:51PM EDT + Our content may be used for nearly any project, commercial or otherwise, including feature + films, broadcast, commercial, industrial, educational video, print projects, multimedia, + games, and the internet, so long as substantial value is added to the content. (For example, + incorporating an audio clip into a commercial qualifies, while reposting our audio clip on + YouTube with no modification or no visual component does not.) Once you download a file it is + yours to keep and use forever, royalty- free, even if you change your subscription or cancel + your account. + +List of applicable sound files + + chimeconnect.wav + chimedisconnect.wav + errormessage.wav + warningmessage.wav \ No newline at end of file diff --git a/doc/legal/Exhibit A - LEGO Open Source License Agreement.txt b/doc/legal/Exhibit A - LEGO Open Source License Agreement.txt new file mode 100644 index 0000000..10c13b9 --- /dev/null +++ b/doc/legal/Exhibit A - LEGO Open Source License Agreement.txt @@ -0,0 +1,15 @@ +EXHIBIT A - LEGO® Open Source License Agreement + +The contents of the file 'nxtstartupsound.wav' contained in this SDK are subject to the +LEGO® Open Source License Agreement Version 1.0 (the "License"); you may not use this +file except in compliance with the License. You may obtain a copy of the License +at "LEGO Open Source License.pdf" contained in the same directory as this exhibit. + +Software distributed under the License is distributed on an "AS IS" basis, WITHOUT +WARRANTY OF ANY KIND, either express or implied. See the License for the specific +language governing rights and limitations under the License. + +The Original Code is \AT91SAM7S256\Resource\SOUNDS\!Startup.rso. +LEGO is the owner of the Original Code. Portions created by Robert Atkinson are +Copyright (C) 2015. All Rights Reserved. +Contributor(s): Robert Atkinson. \ No newline at end of file diff --git a/doc/legal/LEGO Open Source License.pdf b/doc/legal/LEGO Open Source License.pdf new file mode 100644 index 0000000..9188498 Binary files /dev/null and b/doc/legal/LEGO Open Source License.pdf differ diff --git a/doc/media/PullRequest.PNG b/doc/media/PullRequest.PNG new file mode 100644 index 0000000..8cba3a9 Binary files /dev/null and b/doc/media/PullRequest.PNG differ diff --git a/gradle.properties b/gradle.properties new file mode 100644 index 0000000..f5935e9 --- /dev/null +++ b/gradle.properties @@ -0,0 +1,12 @@ +# AndroidX package structure to make it clearer which packages are bundled with the +# Android operating system, and which are packaged with your app's APK +# https://developer.android.com/topic/libraries/support-library/androidx-rn +android.useAndroidX=true + +# We no longer need to auto-convert third-party libraries to use AndroidX, which slowed down the build +android.enableJetifier=false + +# Allow Gradle to use up to 1 GB of RAM +org.gradle.jvmargs=-Xmx1024M + +android.nonTransitiveRClass=false \ No newline at end of file diff --git a/gradle/wrapper/gradle-wrapper.jar b/gradle/wrapper/gradle-wrapper.jar new file mode 100644 index 0000000..f3d88b1 Binary files /dev/null and b/gradle/wrapper/gradle-wrapper.jar differ diff --git a/gradle/wrapper/gradle-wrapper.properties b/gradle/wrapper/gradle-wrapper.properties new file mode 100644 index 0000000..19cfad9 --- /dev/null +++ b/gradle/wrapper/gradle-wrapper.properties @@ -0,0 +1,5 @@ +distributionBase=GRADLE_USER_HOME +distributionPath=wrapper/dists +distributionUrl=https\://services.gradle.org/distributions/gradle-8.9-bin.zip +zipStoreBase=GRADLE_USER_HOME +zipStorePath=wrapper/dists diff --git a/gradlew b/gradlew new file mode 100755 index 0000000..91a7e26 --- /dev/null +++ b/gradlew @@ -0,0 +1,164 @@ +#!/usr/bin/env bash + +############################################################################## +## +## Gradle start up script for UN*X +## +############################################################################## + +# Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script. +DEFAULT_JVM_OPTS="" + +APP_NAME="Gradle" +APP_BASE_NAME=`basename "$0"` + +# Use the maximum available, or set MAX_FD != -1 to use that value. +MAX_FD="maximum" + +warn ( ) { + echo "$*" +} + +die ( ) { + echo + echo "$*" + echo + exit 1 +} + +# OS specific support (must be 'true' or 'false'). +cygwin=false +msys=false +darwin=false +case "`uname`" in + CYGWIN* ) + cygwin=true + ;; + Darwin* ) + darwin=true + ;; + MINGW* ) + msys=true + ;; +esac + +# For Cygwin, ensure paths are in UNIX format before anything is touched. +if $cygwin ; then + [ -n "$JAVA_HOME" ] && JAVA_HOME=`cygpath --unix "$JAVA_HOME"` +fi + +# Attempt to set APP_HOME +# Resolve links: $0 may be a link +PRG="$0" +# Need this for relative symlinks. +while [ -h "$PRG" ] ; do + ls=`ls -ld "$PRG"` + link=`expr "$ls" : '.*-> \(.*\)$'` + if expr "$link" : '/.*' > /dev/null; then + PRG="$link" + else + PRG=`dirname "$PRG"`"/$link" + fi +done +SAVED="`pwd`" +cd "`dirname \"$PRG\"`/" >&- +APP_HOME="`pwd -P`" +cd "$SAVED" >&- + +CLASSPATH=$APP_HOME/gradle/wrapper/gradle-wrapper.jar + +# Determine the Java command to use to start the JVM. +if [ -n "$JAVA_HOME" ] ; then + if [ -x "$JAVA_HOME/jre/sh/java" ] ; then + # IBM's JDK on AIX uses strange locations for the executables + JAVACMD="$JAVA_HOME/jre/sh/java" + else + JAVACMD="$JAVA_HOME/bin/java" + fi + if [ ! -x "$JAVACMD" ] ; then + die "ERROR: JAVA_HOME is set to an invalid directory: $JAVA_HOME + +Please set the JAVA_HOME variable in your environment to match the +location of your Java installation." + fi +else + JAVACMD="java" + which java >/dev/null 2>&1 || die "ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. + +Please set the JAVA_HOME variable in your environment to match the +location of your Java installation." +fi + +# Increase the maximum file descriptors if we can. +if [ "$cygwin" = "false" -a "$darwin" = "false" ] ; then + MAX_FD_LIMIT=`ulimit -H -n` + if [ $? -eq 0 ] ; then + if [ "$MAX_FD" = "maximum" -o "$MAX_FD" = "max" ] ; then + MAX_FD="$MAX_FD_LIMIT" + fi + ulimit -n $MAX_FD + if [ $? -ne 0 ] ; then + warn "Could not set maximum file descriptor limit: $MAX_FD" + fi + else + warn "Could not query maximum file descriptor limit: $MAX_FD_LIMIT" + fi +fi + +# For Darwin, add options to specify how the application appears in the dock +if $darwin; then + GRADLE_OPTS="$GRADLE_OPTS \"-Xdock:name=$APP_NAME\" \"-Xdock:icon=$APP_HOME/media/gradle.icns\"" +fi + +# For Cygwin, switch paths to Windows format before running java +if $cygwin ; then + APP_HOME=`cygpath --path --mixed "$APP_HOME"` + CLASSPATH=`cygpath --path --mixed "$CLASSPATH"` + + # We build the pattern for arguments to be converted via cygpath + ROOTDIRSRAW=`find -L / -maxdepth 1 -mindepth 1 -type d 2>/dev/null` + SEP="" + for dir in $ROOTDIRSRAW ; do + ROOTDIRS="$ROOTDIRS$SEP$dir" + SEP="|" + done + OURCYGPATTERN="(^($ROOTDIRS))" + # Add a user-defined pattern to the cygpath arguments + if [ "$GRADLE_CYGPATTERN" != "" ] ; then + OURCYGPATTERN="$OURCYGPATTERN|($GRADLE_CYGPATTERN)" + fi + # Now convert the arguments - kludge to limit ourselves to /bin/sh + i=0 + for arg in "$@" ; do + CHECK=`echo "$arg"|egrep -c "$OURCYGPATTERN" -` + CHECK2=`echo "$arg"|egrep -c "^-"` ### Determine if an option + + if [ $CHECK -ne 0 ] && [ $CHECK2 -eq 0 ] ; then ### Added a condition + eval `echo args$i`=`cygpath --path --ignore --mixed "$arg"` + else + eval `echo args$i`="\"$arg\"" + fi + i=$((i+1)) + done + case $i in + (0) set -- ;; + (1) set -- "$args0" ;; + (2) set -- "$args0" "$args1" ;; + (3) set -- "$args0" "$args1" "$args2" ;; + (4) set -- "$args0" "$args1" "$args2" "$args3" ;; + (5) set -- "$args0" "$args1" "$args2" "$args3" "$args4" ;; + (6) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" ;; + (7) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" ;; + (8) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" "$args7" ;; + (9) set -- "$args0" "$args1" "$args2" "$args3" "$args4" "$args5" "$args6" "$args7" "$args8" ;; + esac +fi + +# Split up the JVM_OPTS And GRADLE_OPTS values into an array, following the shell quoting and substitution rules +function splitJvmOpts() { + JVM_OPTS=("$@") +} +eval splitJvmOpts $DEFAULT_JVM_OPTS $JAVA_OPTS $GRADLE_OPTS +JVM_OPTS[${#JVM_OPTS[*]}]="-Dorg.gradle.appname=$APP_BASE_NAME" + +exec "$JAVACMD" "${JVM_OPTS[@]}" -classpath "$CLASSPATH" org.gradle.wrapper.GradleWrapperMain "$@" diff --git a/gradlew.bat b/gradlew.bat new file mode 100644 index 0000000..8a0b282 --- /dev/null +++ b/gradlew.bat @@ -0,0 +1,90 @@ +@if "%DEBUG%" == "" @echo off +@rem ########################################################################## +@rem +@rem Gradle startup script for Windows +@rem +@rem ########################################################################## + +@rem Set local scope for the variables with windows NT shell +if "%OS%"=="Windows_NT" setlocal + +@rem Add default JVM options here. You can also use JAVA_OPTS and GRADLE_OPTS to pass JVM options to this script. +set DEFAULT_JVM_OPTS= + +set DIRNAME=%~dp0 +if "%DIRNAME%" == "" set DIRNAME=. +set APP_BASE_NAME=%~n0 +set APP_HOME=%DIRNAME% + +@rem Find java.exe +if defined JAVA_HOME goto findJavaFromJavaHome + +set JAVA_EXE=java.exe +%JAVA_EXE% -version >NUL 2>&1 +if "%ERRORLEVEL%" == "0" goto init + +echo. +echo ERROR: JAVA_HOME is not set and no 'java' command could be found in your PATH. +echo. +echo Please set the JAVA_HOME variable in your environment to match the +echo location of your Java installation. + +goto fail + +:findJavaFromJavaHome +set JAVA_HOME=%JAVA_HOME:"=% +set JAVA_EXE=%JAVA_HOME%/bin/java.exe + +if exist "%JAVA_EXE%" goto init + +echo. +echo ERROR: JAVA_HOME is set to an invalid directory: %JAVA_HOME% +echo. +echo Please set the JAVA_HOME variable in your environment to match the +echo location of your Java installation. + +goto fail + +:init +@rem Get command-line arguments, handling Windowz variants + +if not "%OS%" == "Windows_NT" goto win9xME_args +if "%@eval[2+2]" == "4" goto 4NT_args + +:win9xME_args +@rem Slurp the command line arguments. +set CMD_LINE_ARGS= +set _SKIP=2 + +:win9xME_args_slurp +if "x%~1" == "x" goto execute + +set CMD_LINE_ARGS=%* +goto execute + +:4NT_args +@rem Get arguments from the 4NT Shell from JP Software +set CMD_LINE_ARGS=%$ + +:execute +@rem Setup the command line + +set CLASSPATH=%APP_HOME%\gradle\wrapper\gradle-wrapper.jar + +@rem Execute Gradle +"%JAVA_EXE%" %DEFAULT_JVM_OPTS% %JAVA_OPTS% %GRADLE_OPTS% "-Dorg.gradle.appname=%APP_BASE_NAME%" -classpath "%CLASSPATH%" org.gradle.wrapper.GradleWrapperMain %CMD_LINE_ARGS% + +:end +@rem End local scope for the variables with windows NT shell +if "%ERRORLEVEL%"=="0" goto mainEnd + +:fail +rem Set variable GRADLE_EXIT_CONSOLE if you need the _script_ return code instead of +rem the _cmd.exe /c_ return code! +if not "" == "%GRADLE_EXIT_CONSOLE%" exit 1 +exit /b 1 + +:mainEnd +if "%OS%"=="Windows_NT" endlocal + +:omega diff --git a/libs/README.txt b/libs/README.txt new file mode 100644 index 0000000..3d34852 --- /dev/null +++ b/libs/README.txt @@ -0,0 +1 @@ +Location of external libs diff --git a/libs/ftc.debug.keystore b/libs/ftc.debug.keystore new file mode 100644 index 0000000..a7204e6 Binary files /dev/null and b/libs/ftc.debug.keystore differ diff --git a/settings.gradle b/settings.gradle new file mode 100644 index 0000000..9e2cfb3 --- /dev/null +++ b/settings.gradle @@ -0,0 +1,2 @@ +include ':FtcRobotController' +include ':TeamCode'