eaglesfe
/
robotcode
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Compare commits

base: eaglesfe:master
Branches Tags
eaglesfe:master
eaglesfe:optimus_dev
eaglesfe:Sihan_Phoenix_Branch
eaglesfe:bumblebee_dev
eaglesfe:Sihan's_Branch
eaglesfe:Khangs_Branch
..
compare: eaglesfe:Khangs_Branch
Branches Tags
eaglesfe:optimus_dev
eaglesfe:Sihan_Phoenix_Branch
eaglesfe:master
eaglesfe:bumblebee_dev
eaglesfe:Sihan's_Branch
eaglesfe:Khangs_Branch

19 Commits
master ... Khangs_Bra

Author SHA1 Message Date
Scott Barnes 22a2c6627e Merge branch 'Khangs_Branch' of https://github.com/IronEaglesRobotics/CenterStage into Khangs_Branch 2023-12-01 20:40:28 -06:00
Scott Barnes 59fed0a5f7 Add drone launcher 2023-12-01 20:39:55 -06:00
ImposterVarunoverlord 2ccea78af4 For khang 2023-12-01 11:22:31 -06:00
Scott Barnes 0fc58a36e8 Fix height adjust with intake 2023-11-30 20:28:37 -06:00
Scott Barnes b308fcd59a Put arm on P controller 2023-11-30 19:54:26 -06:00
ImposterVarunoverlord afc4ae7316 new push 2023-11-30 19:47:38 -06:00
ImposterVarunoverlord 4ea48d6479 I updated the drive path for BlueRightAuto_Drivepathonly_ 2023-11-30 00:41:04 -06:00
ImposterVarunoverlord 2076df39e3 This is the new code with updated values for the new intake ramp with the new arm and hopper. 2023-11-29 19:18:13 -06:00
ImposterVarunoverlord a13a1d584b I added the new door to the macros and a potential blue right auto drive path. 2023-11-28 11:20:55 -06:00
Robert Teal 5329732517 Tuned roadrunner, refactored a bunch of things, added macros, added a basic auto, added a new teleop 2023-11-20 23:36:55 -06:00
UntitledRice 7610f97542 switched to slow button instead of turbo 2023-11-16 17:15:43 -06:00
UntitledRice 5ba6de512f added opmode jsut for drivebase to driver pratice one at a time moved turbo to driver A button and works 2023-11-16 16:56:51 -06:00
UntitledRice 252ba82597 added turbo mode on driver 1 right bumper, started adding pid stuff and added runtime check to start working on macros. pid and advanced macro in ExperimentalKhangMain and the turbo and variable that keeps track fo time is in KhangMain. Do not mix these two opmodes very bad things will happen that im still smoothing out. 2023-11-15 05:06:55 -06:00
UntitledRice 76d8b67c01 fixed broken code due to auto upload merge error. added variable to keep track of run time. fixed some macros, commented most the code for future drivers to learn controls and what the code does easily. I will be trying to make macros use time but I am committing and pushing this right now because I know it will go horribly wrong. 2023-11-14 11:57:15 -06:00
UntitledRice dc48b5ebf3 Fix all errors, first working code used for meet 1, after meet added new keybinds and some minor changes plus lots fo math, logic, adn macros added. 2023-11-11 18:39:00 -06:00
Justin d7c89b9a23 park autos? 2023-11-11 10:47:48 -06:00
Justin ebd299fafa blue left auto path 2023-11-09 16:27:38 -06:00
UntitledRice edd589fb79 Add drive base, add intake, and other small details. 2023-10-26 19:11:12 -05:00
Scott Barnes 49d4939366 Working red detection 2023-10-14 16:27:44 -05:00
90 changed files with 8514 additions and 614 deletions
Show Stats Expand all files Collapse all files

4
FtcRobotController/src/main/AndroidManifest.xml
View File

@ -1,8 +1,8 @@
<?xml version="1.0" encoding="utf-8"?>
<manifest xmlns:android="http://schemas.android.com/apk/res/android"
xmlns:tools="http://schemas.android.com/tools"
android:versionCode="53"
android:versionName="9.1">
android:versionCode="51"
android:versionName="9.0">
<uses-permission android:name="android.permission.RECEIVE_BOOT_COMPLETED" />

406
FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTag.java vendored
View File

@ -1,217 +1,189 @@
/* 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 Play to start OpMode");
telemetry.update();
waitForStart();
if (opModeIsActive()) {
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<AprilTagDetection> 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
///* 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,
// * including Java Builder structures for specifying Vision 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.
// */
//@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 Play to start OpMode");
// telemetry.update();
// waitForStart();
//
// if (opModeIsActive()) {
// 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()
// //.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();
//
// // 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.enableCameraMonitoring(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<AprilTagDetection> 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

312
FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagEasy.java vendored
View File

@ -1,163 +1,149 @@
/* Copyright (c) 2023 FIRST. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted (subject to the limitations in the disclaimer below) provided that
* the following conditions are met:
*
* Redistributions of source code must retain the above copyright notice, this list
* of conditions and the following disclaimer.
*
* Redistributions in binary form must reproduce the above copyright notice, this
* list of conditions and the following disclaimer in the documentation and/or
* other materials provided with the distribution.
*
* Neither the name of FIRST nor the names of its contributors may be used to endorse or
* promote products derived from this software without specific prior written permission.
*
* NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
* LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
* THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
package org.firstinspires.ftc.robotcontroller.external.samples;
import com.qualcomm.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 Play to start OpMode");
telemetry.update();
waitForStart();
if (opModeIsActive()) {
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<AprilTagDetection> 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
///* 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.
// *
// * 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 Play to start OpMode");
// telemetry.update();
// waitForStart();
//
// if (opModeIsActive()) {
// 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<AprilTagDetection> 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

23
FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptTensorFlowObjectDetection.java vendored
View File

@ -29,7 +29,6 @@
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;
@ -54,17 +53,6 @@ public class ConceptTensorFlowObjectDetection extends LinearOpMode {
private static final boolean USE_WEBCAM = true; // true for webcam, false for phone camera
// TFOD_MODEL_ASSET points to a model file stored in the project Asset location,
// this is only used for Android Studio when using models in Assets.
private static final String TFOD_MODEL_ASSET = "MyModelStoredAsAsset.tflite";
// TFOD_MODEL_FILE points to a model file stored onboard the Robot Controller's storage,
// this is used when uploading models directly to the RC using the model upload interface.
private static final String TFOD_MODEL_FILE = "/sdcard/FIRST/tflitemodels/myCustomModel.tflite";
// Define the labels recognized in the model for TFOD (must be in training order!)
private static final String[] LABELS = {
"Pixel",
};
/**
* The variable to store our instance of the TensorFlow Object Detection processor.
*/
@ -119,16 +107,11 @@ public class ConceptTensorFlowObjectDetection extends LinearOpMode {
// Create the TensorFlow processor by using a builder.
tfod = new TfodProcessor.Builder()
// With the following lines commented out, the default TfodProcessor Builder
// will load the default model for the season. To define a custom model to load,
// choose one of the following:
// Use setModelAssetName() if the custom TF Model is built in as an asset (AS only).
// Use setModelFileName() if you have downloaded a custom team model to the Robot Controller.
// Use setModelAssetName() if the TF Model is built in as an asset.
// Use setModelFileName() if you have downloaded a custom team model to the Robot Controller.
//.setModelAssetName(TFOD_MODEL_ASSET)
//.setModelFileName(TFOD_MODEL_FILE)
// The following default settings are available to un-comment and edit as needed to
// set parameters for custom models.
//.setModelLabels(LABELS)
//.setIsModelTensorFlow2(true)
//.setIsModelQuantized(true)
@ -151,7 +134,7 @@ public class ConceptTensorFlowObjectDetection extends LinearOpMode {
//builder.setCameraResolution(new Size(640, 480));
// Enable the RC preview (LiveView). Set "false" to omit camera monitoring.
//builder.enableLiveView(true);
//builder.enableCameraMonitoring(true);
// Set the stream format; MJPEG uses less bandwidth than default YUY2.
//builder.setStreamFormat(VisionPortal.StreamFormat.YUY2);

44
FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveToAprilTagOmni.java vendored
View File

@ -49,13 +49,6 @@ 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)
@ -109,7 +102,7 @@ public class RobotAutoDriveToAprilTagOmni extends LinearOpMode
private DcMotor rightBackDrive = null; // Used to control the right back drive wheel
private static final boolean USE_WEBCAM = true; // Set true to use a webcam, or false for a phone camera
private static final int DESIRED_TAG_ID = -1; // Choose the tag you want to approach or set to -1 for ANY tag.
private static final int DESIRED_TAG_ID = 0; // Choose the tag you want to approach or set to -1 for ANY tag.
private VisionPortal visionPortal; // Used to manage the video source.
private AprilTagProcessor aprilTag; // Used for managing the AprilTag detection process.
private AprilTagDetection desiredTag = null; // Used to hold the data for a detected AprilTag
@ -157,33 +150,25 @@ public class RobotAutoDriveToAprilTagOmni extends LinearOpMode
// Step through the list of detected tags and look for a matching tag
List<AprilTagDetection> 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);
}
if ((detection.metadata != null) &&
((DESIRED_TAG_ID < 0) || (detection.id == DESIRED_TAG_ID)) ){
targetFound = true;
desiredTag = detection;
break; // don't look any further.
} 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);
telemetry.addData("Unknown Target", "Tag ID %d is not in TagLibrary\n", detection.id);
}
}
// Tell the driver what we see, and what to do.
if (targetFound) {
telemetry.addData("\n>","HOLD Left-Bumper to Drive to Target\n");
telemetry.addData("Found", "ID %d (%s)", desiredTag.id, desiredTag.metadata.name);
telemetry.addData(">","HOLD Left-Bumper to Drive to Target\n");
telemetry.addData("Target", "ID %d (%s)", desiredTag.id, desiredTag.metadata.name);
telemetry.addData("Range", "%5.1f inches", desiredTag.ftcPose.range);
telemetry.addData("Bearing","%3.0f degrees", desiredTag.ftcPose.bearing);
telemetry.addData("Yaw","%3.0f degrees", desiredTag.ftcPose.yaw);
} else {
telemetry.addData("\n>","Drive using joysticks to find valid target\n");
telemetry.addData(">","Drive using joysticks to find valid target\n");
}
// If Left Bumper is being pressed, AND we have found the desired target, Drive to target Automatically .
@ -258,15 +243,6 @@ public class RobotAutoDriveToAprilTagOmni extends LinearOpMode
// Create the AprilTag processor by using a builder.
aprilTag = new AprilTagProcessor.Builder().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
// 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()

44
FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/RobotAutoDriveToAprilTagTank.java vendored
View File

@ -49,13 +49,6 @@ 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.
@ -104,7 +97,7 @@ public class RobotAutoDriveToAprilTagTank extends LinearOpMode
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 static final int DESIRED_TAG_ID = 0; // Choose the tag you want to approach or set to -1 for ANY tag.
private VisionPortal visionPortal; // Used to manage the video source.
private AprilTagProcessor aprilTag; // Used for managing the AprilTag detection process.
private AprilTagDetection desiredTag = null; // Used to hold the data for a detected AprilTag
@ -147,32 +140,24 @@ public class RobotAutoDriveToAprilTagTank extends LinearOpMode
// Step through the list of detected tags and look for a matching tag
List<AprilTagDetection> 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);
}
if ((detection.metadata != null) &&
((DESIRED_TAG_ID < 0) || (detection.id == DESIRED_TAG_ID)) ){
targetFound = true;
desiredTag = detection;
break; // don't look any further.
} 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);
telemetry.addData("Unknown Target", "Tag ID %d is not in TagLibrary\n", detection.id);
}
}
// Tell the driver what we see, and what to do.
if (targetFound) {
telemetry.addData("\n>","HOLD Left-Bumper to Drive to Target\n");
telemetry.addData("Found", "ID %d (%s)", desiredTag.id, desiredTag.metadata.name);
telemetry.addData(">","HOLD Left-Bumper to Drive to Target\n");
telemetry.addData("Target", "ID %d (%s)", desiredTag.id, desiredTag.metadata.name);
telemetry.addData("Range", "%5.1f inches", desiredTag.ftcPose.range);
telemetry.addData("Bearing","%3.0f degrees", desiredTag.ftcPose.bearing);
} else {
telemetry.addData("\n>","Drive using joysticks to find valid target\n");
telemetry.addData(">","Drive using joysticks to find valid target\n");
}
// If Left Bumper is being pressed, AND we have found the desired target, Drive to target Automatically .
@ -233,15 +218,6 @@ public class RobotAutoDriveToAprilTagTank extends LinearOpMode
// Create the AprilTag processor by using a builder.
aprilTag = new AprilTagProcessor.Builder().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
// 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()

78
FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorDigitalTouch.java vendored
View File

@ -1,78 +0,0 @@
/* 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();
}
}
}

52
README.md
View File

@ -59,58 +59,6 @@ The readme.md file located in the [/TeamCode/src/main/java/org/firstinspires/ftc
# Release Information
## Version 9.1 (20240215-115542)
### Enhancements
* Fixes a problem with Blocks: if the user closes a Block's warning balloon, it will still be closed next time the project is opened in the Blocks editor.
* In the Blocks editor, an alert concerning missing hardware devices is not shown if all the Blocks that use the missing hardware devices are disabled.
* Adds Blocks to support comparing property values CRServo.Direction, DCMotor.Direction, DCMotor.Mode, DCMotor.ZeroPowerBehavior, DigitalChannel.Mode, GyroSensor.HeadingMode, IrSeekerSensor.Mode, and Servo.Direction, to the corresponding enum Block.
* Improves OnBotJava auto-import to correctly import classes when used in certain situations.
* Improves OnBotJava autocomplete to provide better completion options in most cases.
* This fixes an issue where autocomplete would fail if a method with two or more formal parameters was defined.
* In OnBotJava, code folding support was added to expand and collapse code sections
* In OnBotJava, the copyright header is now automatically collapsed loading new files
* For all Blocks OpMode samples, intro comments have been moved to the RunOpMode comment balloon.
* The Clean up Blocks command in the Blocks editor now positions function Blocks so their comment balloons don't overlap other function Blocks.
* Added Blocks OpMode sample SensorTouch.
* Added Java OpMode sample SensorDigitalTouch.
* Several improvements to VisionPortal
* Adds option to control whether the stream is automatically started following a `.build()` call on a VisionPortal Builder
* Adds option to control whether the vision processing statistics overlay is rendered or not
* VisionPortals now implement the `CameraStreamSource` interface, allowing multiportal users to select which portal is routed to the DS in INIT by calling CameraStreamServer.getInstance().setSource(visionPortal). Can be selected via gamepad, between Camera Stream sessions.
* Add option to `AprilTagProcessor` to suppress calibration warnings
* Improves camera calibration warnings
* If a calibration is scaled, the resolution it was scaled from will be listed
* If calibrations exist with the wrong aspect ratio, the calibrated resolutions will be listed
* Fixes race condition which caused app crash when calling `stopStreaming()` immediately followed by `close()` on a VisionPortal
* Fixes IllegalStateException when calling `stopStreaming()` immediately after building a VisionPortal
* Added FTC Blocks counterparts to new Java methods:
* VisionPortal.Builder.setAutoStartStreamOnBuild
* VisionPortal.Builder.setShowStatsOverlay
* AprilTagProcessor.Builder.setSuppressCalibrationWarnings
* CameraStreamServer.setSource​
### Bug Fixes
* Fixes a problem where OnBotJava does not apply font size settings to the editor.
* Updates EasyOpenCV dependency to v1.7.1
* Fixes inability to use EasyOpenCV CameraFactory in OnBotJava
* Fixes entire RC app crash when user pipeline throws an exception
* Fixes entire RC app crash when user user canvas annotator throws an exception
* Use the modern stacktrace display when handling user exceptions instead of the legacy ESTOP telemetry message
## Version 9.0.1 (20230929-083754)
### Enhancements
* Updates AprilTag samples to include Decimation and additional Comments. Also corrects misleading tag ID warnings
* Increases maximum size of Blocks inline comments to 140 characters
* Adds Blocks sample BasicOmniOpMode.
* Updated CENTERSTAGE library AprilTag orientation quaternions
* Thanks [@FromenActual](https://github.com/FromenActual)
* Updated Java Sample ConceptTensorFlowObjectDetection.java to include missing elements needed for custom model support.
### Bug Fixes
* Fixes a problem where after October 1 the Driver Station will report as obsolete on v9.0 and prompt the user to update.
## Version 9.0 (20230830-154348)
### Breaking Changes

8
TeamCode/build.gradle
View File

@ -10,8 +10,6 @@
// Custom definitions may go here
// Include common definitions from above.
apply from: '../build.common.gradle'
apply from: '../build.dependencies.gradle'
@ -26,4 +24,10 @@ android {
dependencies {
implementation project(':FtcRobotController')
annotationProcessor files('lib/OpModeAnnotationProcessor.jar')
implementation 'org.openftc:easyopencv:1.7.0'
implementation 'org.ftclib.ftclib:vision:2.0.1'
implementation 'org.ftclib.ftclib:core:2.0.1'
implementation 'org.apache.commons:commons-math3:3.6.1'
implementation 'com.fasterxml.jackson.core:jackson-databind:2.12.7'
implementation 'com.acmerobotics.roadrunner:core:0.5.6'
}

30
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/controller/Button.java Normal file
View File

@ -0,0 +1,30 @@
package org.firstinspires.ftc.teamcode.controller;
public class Button {
boolean currentState = false;
boolean lastState = false;
boolean justPressed = false;
boolean justReleased = false;
public Button() {}
public void update(boolean pressed) {
lastState = currentState;
currentState = pressed;
justPressed = currentState && !lastState;
justReleased = !currentState && lastState;
}
public boolean isPressed() {
return currentState;
}
public boolean isJustPressed() {
return justPressed;
}
public boolean isJustReleased() {
return justReleased;
}
}

197
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/controller/Controller.java Normal file
View File

@ -0,0 +1,197 @@
package org.firstinspires.ftc.teamcode.controller;
import com.qualcomm.robotcore.hardware.Gamepad;
public class Controller {
private final Gamepad gamepad;
private final Joystick leftStick;
private final Joystick rightStick;
private final Button leftStickButton;
private final Button rightStickButton;
private final Button dLeft;
private final Button dRight;
private final Button dUp;
private final Button dDown;
private final Button a;
private final Button b;
private final Button x;
private final Button y;
private final Button leftBumper;
private final Button rightBumper;
private final Button back;
private final Button start;
private final Trigger leftTrigger;
private final Trigger rightTrigger;
private final Button touchpad;
private final Button allButtons;
public Controller(Gamepad gamepad) {
this.gamepad = gamepad;
leftStick = new Joystick();
rightStick = new Joystick();
leftStickButton = new Button();
rightStickButton = new Button();
dLeft = new Button();
dRight = new Button();
dUp = new Button();
dDown = new Button();
a = new Button();
b = new Button();
x = new Button();
y = new Button();
leftBumper = new Button();
rightBumper = new Button();
back = new Button();
start = new Button();
leftTrigger = new Trigger();
rightTrigger = new Trigger();
touchpad = new Button();
allButtons = new Button();
}
public void update() {
leftStick.update(gamepad.left_stick_x, -gamepad.left_stick_y);
rightStick.update(gamepad.right_stick_x, -gamepad.right_stick_y);
leftStickButton.update(gamepad.left_stick_button);
rightStickButton.update(gamepad.right_stick_button);
dLeft.update(gamepad.dpad_left);
dRight.update(gamepad.dpad_right);
dUp.update(gamepad.dpad_up);
dDown.update(gamepad.dpad_down);
a.update(gamepad.a);
b.update(gamepad.b);
x.update(gamepad.x);
y.update(gamepad.y);
leftBumper.update(gamepad.left_bumper);
rightBumper.update(gamepad.right_bumper);
back.update(gamepad.back);
start.update(gamepad.start);
leftTrigger.update(gamepad.left_trigger);
rightTrigger.update(gamepad.right_trigger);
touchpad.update(gamepad.touchpad);
allButtons.update(leftStickButton.isPressed() || rightStickButton.isPressed() || dLeft.isPressed() || dRight.isPressed() || dUp.isPressed() || dDown.isPressed() || a.isPressed() || b.isPressed() || x.isPressed() || y.isPressed() || leftBumper.isPressed() || rightBumper.isPressed() || back.isPressed() || start.isPressed() || leftTrigger.getValue() > 0 || rightTrigger.getValue() > 0 || touchpad.isPressed());
}
public Joystick getLeftStick() {
return leftStick;
}
public Joystick getRightStick() {
return rightStick;
}
public Button getLeftStickButton() {
return leftStickButton;
}
public Button getRightStickButton() {
return rightStickButton;
}
public Button getDLeft() {
return dLeft;
}
public Button getDRight() {
return dRight;
}
public Button getDUp() {
return dUp;
}
public Button getDDown() {
return dDown;
}
public Button getA() {
return a;
}
public Button getB() {
return b;
}
public Button getX() {
return x;
}
public Button getY() {
return y;
}
public Button getLeftBumper() {
return leftBumper;
}
public Button getRightBumper() {
return rightBumper;
}
public Button getBack() {
return back;
}
public Button getStart() {
return start;
}
public Trigger getLeftTrigger() {
return leftTrigger;
}
public Trigger getRightTrigger() {
return rightTrigger;
}
public Button getTouchpad() {
return touchpad;
}
public Button getAllButtons() {
return allButtons;
}
public void rumble() {
gamepad.rumble(Gamepad.RUMBLE_DURATION_CONTINUOUS);
}
public void rumble(int milliseconds) {
gamepad.rumble(milliseconds);
}
public void rumbleBlips(int blips) {
gamepad.rumbleBlips(blips);
}
public void stopRumble() {
gamepad.stopRumble();
}
public boolean isRumbling() {
return gamepad.isRumbling();
}
public void setColor(int r, int g, int b) {
gamepad.setLedColor(r, g, b, -1);
}
public void setColor(int r, int g, int b, int milliseconds) {
gamepad.setLedColor(r, g, b, milliseconds);
}
}

913
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/controller/Gamepad2.java Normal file
View File

@ -0,0 +1,913 @@
package org.firstinspires.ftc.teamcode.controller;///*
// * 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 com.qualcomm.robotcore.hardware;
//
//import android.os.SystemClock;
//
//import com.qualcomm.robotcore.eventloop.opmode.OpModeManagerImpl;
//import com.qualcomm.robotcore.exception.RobotCoreException;
//import com.qualcomm.robotcore.robocol.RobocolParsable;
//import com.qualcomm.robotcore.robocol.RobocolParsableBase;
//import com.qualcomm.robotcore.util.Range;
//import com.qualcomm.robotcore.util.RobotLog;
//
//import org.firstinspires.ftc.robotcore.internal.collections.EvictingBlockingQueue;
//import org.firstinspires.ftc.robotcore.internal.collections.SimpleGson;
//import org.firstinspires.ftc.robotcore.internal.network.SendOnceRunnable;
//import org.firstinspires.ftc.robotcore.internal.ui.GamepadUser;
//
//import java.nio.BufferOverflowException;
//import java.nio.ByteBuffer;
//import java.util.ArrayList;
//import java.util.concurrent.ArrayBlockingQueue;
//
///**
// * Monitor a hardware gamepad.
// * <p>
// * The buttons, analog sticks, and triggers are represented a public
// * member variables that can be read from or written to directly.
// * <p>
// * Analog sticks are represented as floats that range from -1.0 to +1.0. They will be 0.0 while at
// * rest. The horizontal axis is labeled x, and the vertical axis is labeled y.
// * <p>
// * Triggers are represented as floats that range from 0.0 to 1.0. They will be at 0.0 while at
// * rest.
// * <p>
// * Buttons are boolean values. They will be true if the button is pressed, otherwise they will be
// * false.
// * <p>
// * The codes KEYCODE_BUTTON_SELECT and KEYCODE_BACK are both be handled as a "back" button event.
// * Older Android devices (Kit Kat) map a Logitech F310 "back" button press to a KEYCODE_BUTTON_SELECT event.
// * Newer Android devices (Marshmallow or greater) map this "back" button press to a KEYCODE_BACK event.
// * Also, the REV Robotics Gamepad (REV-31-1159) has a "select" button instead of a "back" button on the gamepad.
// * <p>
// * The dpad is represented as 4 buttons, dpad_up, dpad_down, dpad_left, and dpad_right
// */
//@SuppressWarnings("unused")
//public class Gamepad2 extends RobocolParsableBase {
//
// /**
// * A gamepad with an ID equal to ID_UNASSOCIATED has not been associated with any device.
// */
// public static final int ID_UNASSOCIATED = -1;
//
// /**
// * A gamepad with a phantom id a synthetic one made up by the system
// */
// public static final int ID_SYNTHETIC = -2;
//
// public enum Type {
// // Do NOT change the order/names of existing entries,
// // you will break backwards compatibility!!
// UNKNOWN(LegacyType.UNKNOWN),
// LOGITECH_F310(LegacyType.LOGITECH_F310),
// XBOX_360(LegacyType.XBOX_360),
// SONY_PS4(LegacyType.SONY_PS4), // This indicates a PS4-compatible controller that is being used through our compatibility mode
// SONY_PS4_SUPPORTED_BY_KERNEL(LegacyType.SONY_PS4); // This indicates a PS4-compatible controller that is being used through the DualShock 4 Linux kernel driver.
//
// private final LegacyType correspondingLegacyType;
// Type(LegacyType correspondingLegacyType) {
// this.correspondingLegacyType = correspondingLegacyType;
// }
// }
//
// // LegacyType is necessary because robocol gamepad version 3 was written in a way that was not
// // forwards-compatible, so we have to keep sending V3-compatible values.
// public enum LegacyType {
// // Do NOT change the order or names of existing entries, or add new entries.
// // You will break backwards compatibility!!
// UNKNOWN,
// LOGITECH_F310,
// XBOX_360,
// SONY_PS4;
// }
//
// @SuppressWarnings("UnusedAssignment")
// public volatile Type type = Type.UNKNOWN; // IntelliJ thinks this is redundant, but it is NOT. Must be a bug in the analyzer?
//
// /**
// * left analog stick horizontal axis
// */
// public volatile float left_stick_x = 0f;
//
// /**
// * left analog stick vertical axis
// */
// public volatile float left_stick_y = 0f;
//
// /**
// * right analog stick horizontal axis
// */
// public volatile float right_stick_x = 0f;
//
// /**
// * right analog stick vertical axis
// */
// public volatile float right_stick_y = 0f;
//
// /**
// * dpad up
// */
// public volatile boolean dpad_up = false;
//
// /**
// * dpad down
// */
// public volatile boolean dpad_down = false;
//
// /**
// * dpad left
// */
// public volatile boolean dpad_left = false;
//
// /**
// * dpad right
// */
// public volatile boolean dpad_right = false;
//
// /**
// * button a
// */
// public volatile boolean a = false;
//
// /**
// * button b
// */
// public volatile boolean b = false;
//
// /**
// * button x
// */
// public volatile boolean x = false;
//
// /**
// * button y
// */
// public volatile boolean y = false;
//
// /**
// * button guide - often the large button in the middle of the controller. The OS may
// * capture this button before it is sent to the app; in which case you'll never
// * receive it.
// */
// public volatile boolean guide = false;
//
// /**
// * button start
// */
// public volatile boolean start = false;
//
// /**
// * button back
// */
// public volatile boolean back = false;
//
// /**
// * button left bumper
// */
// public volatile boolean left_bumper = false;
//
// /**
// * button right bumper
// */
// public volatile boolean right_bumper = false;
//
// /**
// * left stick button
// */
// public volatile boolean left_stick_button = false;
//
// /**
// * right stick button
// */
// public volatile boolean right_stick_button = false;
//
// /**
// * left trigger
// */
// public volatile float left_trigger = 0f;
//
// /**
// * right trigger
// */
// public volatile float right_trigger = 0f;
//
// /**
// * PS4 Support - Circle
// */
// public volatile boolean circle = false;
//
// /**
// * PS4 Support - cross
// */
// public volatile boolean cross = false;
//
// /**
// * PS4 Support - triangle
// */
// public volatile boolean triangle = false;
//
// /**
// * PS4 Support - square
// */
// public volatile boolean square = false;
//
// /**
// * PS4 Support - share
// */
// public volatile boolean share = false;
//
// /**
// * PS4 Support - options
// */
// public volatile boolean options = false;
//
// /**
// * PS4 Support - touchpad
// */
// public volatile boolean touchpad = false;
// public volatile boolean touchpad_finger_1;
// public volatile boolean touchpad_finger_2;
// public volatile float touchpad_finger_1_x;
// public volatile float touchpad_finger_1_y;
// public volatile float touchpad_finger_2_x;
// public volatile float touchpad_finger_2_y;
//
// /**
// * PS4 Support - PS Button
// */
// public volatile boolean ps = false;
//
// /**
// * Which user is this gamepad used by
// */
// protected volatile byte user = ID_UNASSOCIATED;
// //
// public GamepadUser getUser() {
// return GamepadUser.from(user);
// }
// //
// public void setUser(GamepadUser user) {
// this.user = user.id;
// }
//
// /**
// * See {@link OpModeManagerImpl#runActiveOpMode(Gamepad[])}
// */
// protected volatile byte userForEffects = ID_UNASSOCIATED;
// public void setUserForEffects(byte userForEffects) {
// this.userForEffects = userForEffects;
// }
//
// /**
// * ID assigned to this gamepad by the OS. This value can change each time the device is plugged in.
// */
// public volatile int id = ID_UNASSOCIATED; // public only for historical reasons
//
// public void setGamepadId(int id) {
// this.id = id;
// }
// public int getGamepadId() {
// return this.id;
// }
//
// /**
// * Relative timestamp of the last time an event was detected
// */
// public volatile long timestamp = 0;
//
// /**
// * Sets the time at which this Gamepad last changed its state,
// * in the {@link android.os.SystemClock#uptimeMillis} time base.
// */
// public void setTimestamp(long timestamp) {
// this.timestamp = timestamp;
// }
//
// /**
// * Refreshes the Gamepad's timestamp to be the current time.
// */
// public void refreshTimestamp() {
// setTimestamp(SystemClock.uptimeMillis());
// }
//
// // private static values used for packaging the gamepad state into a byte array
// private static final short PAYLOAD_SIZE = 60;
// private static final short BUFFER_SIZE = PAYLOAD_SIZE + RobocolParsable.HEADER_LENGTH;
//
// private static final byte ROBOCOL_GAMEPAD_VERSION = 5;
//
// public Gamepad() {
// this.type = type();
// }
//
// /**
// * Copy the state of a gamepad into this gamepad
// * @param gamepad state to be copied from
// * @throws RobotCoreException if the copy fails - gamepad will be in an unknown
// * state if this exception is thrown
// */
// public void copy(Gamepad gamepad) throws RobotCoreException {
// // reuse the serialization code; since that reduces the chances of bugs
// fromByteArray(gamepad.toByteArray());
// }
//
// /**
// * Reset this gamepad into its initial state
// */
// public void reset() {
// try {
// copy(new Gamepad());
// } catch (RobotCoreException e) {
// // we should never hit this
// RobotLog.e("Gamepad library in an invalid state");
// throw new IllegalStateException("Gamepad library in an invalid state");
// }
// }
//
// @Override
// public MsgType getRobocolMsgType() {
// return RobocolParsable.MsgType.GAMEPAD;
// }
//
// @Override
// public byte[] toByteArray() throws RobotCoreException {
//
// ByteBuffer buffer = getWriteBuffer(PAYLOAD_SIZE);
//
// try {
// int buttons = 0;
//
// buffer.put(ROBOCOL_GAMEPAD_VERSION);
// buffer.putInt(id);
// buffer.putLong(timestamp).array();
// buffer.putFloat(left_stick_x).array();
// buffer.putFloat(left_stick_y).array();
// buffer.putFloat(right_stick_x).array();
// buffer.putFloat(right_stick_y).array();
// buffer.putFloat(left_trigger).array();
// buffer.putFloat(right_trigger).array();
//
// buttons = (buttons << 1) + (touchpad_finger_1 ? 1 : 0);
// buttons = (buttons << 1) + (touchpad_finger_2 ? 1 : 0);
// buttons = (buttons << 1) + (touchpad ? 1 : 0);
// buttons = (buttons << 1) + (left_stick_button ? 1 : 0);
// buttons = (buttons << 1) + (right_stick_button ? 1 : 0);
// buttons = (buttons << 1) + (dpad_up ? 1 : 0);
// buttons = (buttons << 1) + (dpad_down ? 1 : 0);
// buttons = (buttons << 1) + (dpad_left ? 1 : 0);
// buttons = (buttons << 1) + (dpad_right ? 1 : 0);
// buttons = (buttons << 1) + (a ? 1 : 0);
// buttons = (buttons << 1) + (b ? 1 : 0);
// buttons = (buttons << 1) + (x ? 1 : 0);
// buttons = (buttons << 1) + (y ? 1 : 0);
// buttons = (buttons << 1) + (guide ? 1 : 0);
// buttons = (buttons << 1) + (start ? 1 : 0);
// buttons = (buttons << 1) + (back ? 1 : 0);
// buttons = (buttons << 1) + (left_bumper ? 1 : 0);
// buttons = (buttons << 1) + (right_bumper ? 1 : 0);
// buffer.putInt(buttons);
//
// // Version 2
// buffer.put(user);
//
// // Version 3
// buffer.put((byte) legacyType().ordinal());
//
// // Version 4
// buffer.put((byte) type.ordinal());
//
// // Version 5
// buffer.putFloat(touchpad_finger_1_x);
// buffer.putFloat(touchpad_finger_1_y);
// buffer.putFloat(touchpad_finger_2_x);
// buffer.putFloat(touchpad_finger_2_y);
// } catch (BufferOverflowException e) {
// RobotLog.logStacktrace(e);
// }
//
// return buffer.array();
// }
//
// @Override
// public void fromByteArray(byte[] byteArray) throws RobotCoreException {
// if (byteArray.length < BUFFER_SIZE) {
// throw new RobotCoreException("Expected buffer of at least " + BUFFER_SIZE + " bytes, received " + byteArray.length);
// }
//
// ByteBuffer byteBuffer = getReadBuffer(byteArray);
//
// int buttons = 0;
//
// byte version = byteBuffer.get();
//
// // TODO(Noah): Reset version to 1
// // extract version 1 values
// if (version >= 1) {
// id = byteBuffer.getInt();
// timestamp = byteBuffer.getLong();
// left_stick_x = byteBuffer.getFloat();
// left_stick_y = byteBuffer.getFloat();
// right_stick_x = byteBuffer.getFloat();
// right_stick_y = byteBuffer.getFloat();
// left_trigger = byteBuffer.getFloat();
// right_trigger = byteBuffer.getFloat();
//
// buttons = byteBuffer.getInt();
// touchpad_finger_1 = (buttons & 0x20000) != 0;
// touchpad_finger_2 = (buttons & 0x10000) != 0;
// touchpad = (buttons & 0x08000) != 0;
// left_stick_button = (buttons & 0x04000) != 0;
// right_stick_button = (buttons & 0x02000) != 0;
// dpad_up = (buttons & 0x01000) != 0;
// dpad_down = (buttons & 0x00800) != 0;
// dpad_left = (buttons & 0x00400) != 0;
// dpad_right = (buttons & 0x00200) != 0;
// a = (buttons & 0x00100) != 0;
// b = (buttons & 0x00080) != 0;
// x = (buttons & 0x00040) != 0;
// y = (buttons & 0x00020) != 0;
// guide = (buttons & 0x00010) != 0;
// start = (buttons & 0x00008) != 0;
// back = (buttons & 0x00004) != 0;
// left_bumper = (buttons & 0x00002) != 0;
// right_bumper = (buttons & 0x00001) != 0;
// }
//
// // extract version 2 values
// if (version >= 2) {
// user = byteBuffer.get();
// }
//
// // extract version 3 values
// if (version >= 3) {
// type = Type.values()[byteBuffer.get()];
// }
//
// if (version >= 4) {
// byte v4TypeValue = byteBuffer.get();
// if (v4TypeValue < Type.values().length) {
// // Yes, this will replace the version 3 value. That is a good thing, since the version 3
// // value was not forwards-compatible.
// type = Type.values()[v4TypeValue];
// } // Else, we don't know what the number means, so we just stick with the value we got from the v3 type field
// }
//
// if(version >= 5) {
// touchpad_finger_1_x = byteBuffer.getFloat();
// touchpad_finger_1_y = byteBuffer.getFloat();
// touchpad_finger_2_x = byteBuffer.getFloat();
// touchpad_finger_2_y = byteBuffer.getFloat();
// }
//
// updateButtonAliases();
// }
//
// /**
// * Are all analog sticks and triggers in their rest position?
// * @return true if all analog sticks and triggers are at rest; otherwise false
// */
// public boolean atRest() {
// return (
// left_stick_x == 0f && left_stick_y == 0f &&
// right_stick_x == 0f && right_stick_y == 0f &&
// left_trigger == 0f && right_trigger == 0f);
// }
//
// /**
// * Get the type of gamepad as a {@link Type}. This method defaults to "UNKNOWN".
// * @return gamepad type
// */
// public Type type() {
// return type;
// }
//
// /**
// * Get the type of gamepad as a {@link LegacyType}. This method defaults to "UNKNOWN".
// * @return gamepad type
// */
// private LegacyType legacyType() {
// return type.correspondingLegacyType;
// }
//
//
// /**
// * Display a summary of this gamepad, including the state of all buttons, analog sticks, and triggers
// * @return a summary
// */
// @Override
// public String toString() {
//
// switch (type) {
// case SONY_PS4:
// case SONY_PS4_SUPPORTED_BY_KERNEL:
// return ps4ToString();
//
// case UNKNOWN:
// case LOGITECH_F310:
// case XBOX_360:
// default:
// return genericToString();
// }
// }
//
//
// protected String ps4ToString() {
// String buttons = new String();
// if (dpad_up) buttons += "dpad_up ";
// if (dpad_down) buttons += "dpad_down ";
// if (dpad_left) buttons += "dpad_left ";
// if (dpad_right) buttons += "dpad_right ";
// if (cross) buttons += "cross ";
// if (circle) buttons += "circle ";
// if (square) buttons += "square ";
// if (triangle) buttons += "triangle ";
// if (ps) buttons += "ps ";
// if (share) buttons += "share ";
// if (options) buttons += "options ";
// if (touchpad) buttons += "touchpad ";
// if (left_bumper) buttons += "left_bumper ";
// if (right_bumper) buttons += "right_bumper ";
// if (left_stick_button) buttons += "left stick button ";
// if (right_stick_button) buttons += "right stick button ";
//
// return String.format("ID: %2d user: %2d lx: % 1.2f ly: % 1.2f rx: % 1.2f ry: % 1.2f lt: %1.2f rt: %1.2f %s",
// id, user, left_stick_x, left_stick_y,
// right_stick_x, right_stick_y, left_trigger, right_trigger, buttons);
// }
//
// protected String genericToString() {
// String buttons = new String();
// if (dpad_up) buttons += "dpad_up ";
// if (dpad_down) buttons += "dpad_down ";
// if (dpad_left) buttons += "dpad_left ";
// if (dpad_right) buttons += "dpad_right ";
// if (a) buttons += "a ";
// if (b) buttons += "b ";
// if (x) buttons += "x ";
// if (y) buttons += "y ";
// if (guide) buttons += "guide ";
// if (start) buttons += "start ";
// if (back) buttons += "back ";
// if (left_bumper) buttons += "left_bumper ";
// if (right_bumper) buttons += "right_bumper ";
// if (left_stick_button) buttons += "left stick button ";
// if (right_stick_button) buttons += "right stick button ";
//
// return String.format("ID: %2d user: %2d lx: % 1.2f ly: % 1.2f rx: % 1.2f ry: % 1.2f lt: %1.2f rt: %1.2f %s",
// id, user, left_stick_x, left_stick_y,
// right_stick_x, right_stick_y, left_trigger, right_trigger, buttons);
// }
//
//
// // To prevent blowing up the command queue if the user tries to send an LED command in a tight loop,
// // we have a 1-element evicting blocking queue for the outgoing LED effect and the event loop periodically
// // just grabs the effect out of it (if any)
// public EvictingBlockingQueue<LedEffect> ledQueue = new EvictingBlockingQueue<>(new ArrayBlockingQueue<LedEffect>(1));
//
// public static final int LED_DURATION_CONTINUOUS = -1;
//
// public static class LedEffect {
// public static class Step {
// public int r;
// public int g;
// public int b;
// public int duration;
// }
//
// public final ArrayList<Step> steps;
// public final boolean repeating;
// public int user;
//
// private LedEffect(ArrayList<Step> steps, boolean repeating) {
// this.steps = steps;
// this.repeating = repeating;
// }
//
// public String serialize() {
// return SimpleGson.getInstance().toJson(this);
// }
//
// public static LedEffect deserialize(String serialized) {
// return SimpleGson.getInstance().fromJson(serialized, LedEffect.class);
// }
//
// public static class Builder {
// private ArrayList<Step> steps = new ArrayList<>();
// private boolean repeating;
//
// /**
// * Add a "step" to this LED effect. A step basically just means to set
// * the LED to a certain color (r,g,b) for a certain duration. By creating a chain of
// * steps, you can create unique effects.
// *
// * @param r the red LED intensity (0.0 - 1.0)
// * @param g the green LED intensity (0.0 - 1.0)
// * @param b the blue LED intensity (0.0 - 1.0)
// * @return the builder object, to follow the builder pattern
// */
// public Builder addStep(double r, double g, double b, int durationMs) {
// return addStepInternal(r, g, b, Math.max(durationMs, 0));
// }
//
// /**
// * Set whether this LED effect should loop after finishing,
// * unless the LED is otherwise commanded differently.
// * @param repeating whether to loop
// * @return the builder object, to follow the builder pattern
// */
// public Builder setRepeating(boolean repeating) {
// this.repeating = repeating;
// return this;
// }
//
// private Builder addStepInternal(double r, double g, double b, int duration) {
//
// r = Range.clip(r, 0, 1);
// g = Range.clip(g, 0, 1);
// b = Range.clip(b, 0, 1);
//
// Step step = new Step();
// step.r = (int) Math.round(Range.scale(r, 0.0, 1.0, 0, 255));
// step.g = (int) Math.round(Range.scale(g, 0.0, 1.0, 0, 255));
// step.b = (int) Math.round(Range.scale(b, 0.0, 1.0, 0, 255));
// step.duration = duration;
// steps.add(step);
// return this;
// }
//
// /**
// * After you've added your steps, call this to get an LedEffect object
// * that you can then pass to {@link #runLedEffect(LedEffect)}
// * @return an LedEffect object, built from previously added steps
// */
// public LedEffect build() {
// return new LedEffect(steps, repeating);
// }
// }
// }
//
// public void setLedColor(double r, double g, double b, int durationMs) {
//
// if (durationMs != LED_DURATION_CONTINUOUS) {
// durationMs = Math.max(0, durationMs);
// }
//
// LedEffect effect = new LedEffect.Builder().addStepInternal(r,g,b, durationMs).build();
// queueEffect(effect);
// }
//
// /**
// * Run an LED effect built using {@link LedEffect.Builder}
// * The LED effect will be run asynchronously; your OpMode will
// * not halt execution while the effect is running.
// *
// * Calling this will displace any currently running LED effect
// */
// public void runLedEffect(LedEffect effect) {
// queueEffect(effect);
// }
//
// private void queueEffect(LedEffect effect) {
// // We need to make a new object here, because since the effect is queued and
// // not set immediately, if you called this function for two different gamepads
// // but passed in the same instance of an LED effect object, then it could happen
// // that both effect commands would be directed to the *same* gamepad. (Because of
// // the "user" field being modified before the queued command was serialized)
// LedEffect copy = new LedEffect(effect.steps, effect.repeating);
// copy.user = userForEffects;
// ledQueue.offer(copy);
// }
//
// // To prevent blowing up the command queue if the user tries to send a rumble command in a tight loop,
// // we have a 1-element evicting blocking queue for the outgoing rumble effect and the event loop periodically
// // just grabs the effect out of it (if any)
// public EvictingBlockingQueue<RumbleEffect> rumbleQueue = new EvictingBlockingQueue<>(new ArrayBlockingQueue<RumbleEffect>(1));
// public long nextRumbleApproxFinishTime = RUMBLE_FINISH_TIME_FLAG_NOT_RUMBLING;
//
// public static final int RUMBLE_DURATION_CONTINUOUS = -1;
//
// public static class RumbleEffect {
// public static class Step {
// public int large;
// public int small;
// public int duration;
// }
//
// public int user;
// public final ArrayList<Step> steps;
//
// private RumbleEffect(ArrayList<Step> steps) {
// this.steps = steps;
// }
//
// public String serialize() {
// return SimpleGson.getInstance().toJson(this);
// }
//
// public static RumbleEffect deserialize(String serialized) {
// return SimpleGson.getInstance().fromJson(serialized, RumbleEffect.class);
// }
//
// public static class Builder {
// private ArrayList<Step> steps = new ArrayList<>();
//
// /**
// * Add a "step" to this rumble effect. A step basically just means to rumble
// * at a certain power level for a certain duration. By creating a chain of
// * steps, you can create unique effects. See {@link #rumbleBlips(int)} for a
// * a simple example.
// * @param rumble1 rumble power for rumble motor 1 (0.0 - 1.0)
// * @param rumble2 rumble power for rumble motor 2 (0.0 - 1.0)
// * @param durationMs milliseconds this step lasts
// * @return the builder object, to follow the builder pattern
// */
// public Builder addStep(double rumble1, double rumble2, int durationMs) {
// return addStepInternal(rumble1, rumble2, Math.max(durationMs, 0));
// }
//
// private Builder addStepInternal(double rumble1, double rumble2, int durationMs) {
//
// rumble1 = Range.clip(rumble1, 0, 1);
// rumble2 = Range.clip(rumble2, 0, 1);
//
// Step step = new Step();
// step.large = (int) Math.round(Range.scale(rumble1, 0.0, 1.0, 0, 255));
// step.small = (int) Math.round(Range.scale(rumble2, 0.0, 1.0, 0, 255));
// step.duration = durationMs;
// steps.add(step);
//
// return this;
// }
//
// /**
// * After you've added your steps, call this to get a RumbleEffect object
// * that you can then pass to {@link #runRumbleEffect(RumbleEffect)}
// * @return a RumbleEffect object, built from previously added steps
// */
// public RumbleEffect build() {
// return new RumbleEffect(steps);
// }
// }
// }
//
// /**
// * Run a rumble effect built using {@link RumbleEffect.Builder}
// * The rumble effect will be run asynchronously; your OpMode will
// * not halt execution while the effect is running.
// *
// * Calling this will displace any currently running rumble effect
// */
// public void runRumbleEffect(RumbleEffect effect) {
// queueEffect(effect);
// }
//
// /**
// * Rumble the gamepad's first rumble motor at maximum power for a certain duration.
// * Calling this will displace any currently running rumble effect.
// * @param durationMs milliseconds to rumble for, or {@link #RUMBLE_DURATION_CONTINUOUS}
// */
// public void rumble(int durationMs) {
//
// if (durationMs != RUMBLE_DURATION_CONTINUOUS) {
// durationMs = Math.max(0, durationMs);
// }
//
// RumbleEffect effect = new RumbleEffect.Builder().addStepInternal(1.0, 0, durationMs).build();
// queueEffect(effect);
// }
//
// /**
// * Rumble the gamepad at a fixed rumble power for a certain duration
// * Calling this will displace any currently running rumble effect
// * @param rumble1 rumble power for rumble motor 1 (0.0 - 1.0)
// * @param rumble2 rumble power for rumble motor 2 (0.0 - 1.0)
// * @param durationMs milliseconds to rumble for, or {@link #RUMBLE_DURATION_CONTINUOUS}
// */
// public void rumble(double rumble1, double rumble2, int durationMs) {
//
// if (durationMs != RUMBLE_DURATION_CONTINUOUS) {
// durationMs = Math.max(0, durationMs);
// }
//
// RumbleEffect effect = new RumbleEffect.Builder().addStepInternal(rumble1, rumble2, durationMs).build();
// queueEffect(effect);
// }
//
// /**
// * Cancel the currently running rumble effect, if any
// */
// public void stopRumble() {
// rumble(0,0,RUMBLE_DURATION_CONTINUOUS);
// }
//
// /**
// * Rumble the gamepad for a certain number of "blips" using predetermined blip timing
// * This will displace any currently running rumble effect.
// * @param count the number of rumble blips to perform
// */
// public void rumbleBlips(int count) {
// RumbleEffect.Builder builder = new RumbleEffect.Builder();
//
// for(int i = 0; i < count; i++) {
// builder.addStep(1.0,0,50).addStep(0,0,50);
// }
//
// queueEffect(builder.build());
// }
//
// private void queueEffect(RumbleEffect effect) {
// // We need to make a new object here, because since the effect is queued and
// // not set immediately, if you called this function for two different gamepads
// // but passed in the same instance of a rumble effect object, then it could happen
// // that both rumble commands would be directed to the *same* gamepad. (Because of
// // the "user" field being modified before the queued command was serialized)
// RumbleEffect copy = new RumbleEffect(effect.steps);
// copy.user = userForEffects;
// rumbleQueue.offer(copy);
// nextRumbleApproxFinishTime = calcApproxRumbleFinishTime(copy);
// }
//
// private static final long RUMBLE_FINISH_TIME_FLAG_NOT_RUMBLING = -1;
// private static final long RUMBLE_FINISH_TIME_FLAG_INFINITE = Long.MAX_VALUE;
//
// /**
// * Returns an educated guess about whether there is a rumble action ongoing on this gamepad
// * @return an educated guess about whether there is a rumble action ongoing on this gamepad
// */
// public boolean isRumbling() {
// if(nextRumbleApproxFinishTime == RUMBLE_FINISH_TIME_FLAG_NOT_RUMBLING) {
// return false;
// } else if(nextRumbleApproxFinishTime == RUMBLE_FINISH_TIME_FLAG_INFINITE) {
// return true;
// } else {
// return System.currentTimeMillis() < nextRumbleApproxFinishTime;
// }
// }
//
// private long calcApproxRumbleFinishTime(RumbleEffect effect) {
// // If the effect is only 1 step long and has an infinite duration...
// if(effect.steps.size() == 1 && effect.steps.get(0).duration == RUMBLE_DURATION_CONTINUOUS) {
// // If the power is zero, then that means the gamepad is being commanded to cease rumble
// if (effect.steps.get(0).large == 0 && effect.steps.get(0).small == 0) {
// return RUMBLE_FINISH_TIME_FLAG_NOT_RUMBLING;
// } else { // But if not, that means it's an infinite (continuous) rumble command
// return RUMBLE_FINISH_TIME_FLAG_INFINITE;
// }
// } else { // If the effect has more than one step (or one step with non-infinite duration) we need to sum the step times
// long time = System.currentTimeMillis();
// long overhead = 50 /* rumbleGamepadsInterval in FtcEventLoopHandler */ +
// SendOnceRunnable.MS_BATCH_TRANSMISSION_INTERVAL +
// 5 /* Slop */;
// for(RumbleEffect.Step step : effect.steps) {
// time += step.duration;
// }
// time += overhead;
// return time;
// }
// }
//
// /**
// * Alias buttons so that XBOX &amp; PS4 native button labels can be used in use code.
// * Should allow a team to program with whatever controllers they prefer, but
// * be able to swap controllers easily without changing code.
// */
// protected void updateButtonAliases(){
// // There is no assignment for touchpad because there is no equivalent on XBOX controllers.
// circle = b;
// cross = a;
// triangle = y;
// square = x;
// share = back;
// options = start;
// ps = guide;
// }
//}

32
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/controller/Joystick.java Normal file
View File

@ -0,0 +1,32 @@
package org.firstinspires.ftc.teamcode.controller;
public class Joystick {
public static double DEADZONE = 0.05; //small 5% deadzone for the new controlers
double x = 0;
double y = 0;
public Joystick() {}
public void update(double x, double y) {
this.x = x;
this.y = y;
}
public double getX() {
if(x<DEADZONE && x>-DEADZONE) {
return 0;
}
else{
return x;
}
}
public double getY() {
if(y<DEADZONE && y>-DEADZONE) {
return 0;
}
else{
return y;
}
}
}

33
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/controller/Trigger.java Normal file
View File

@ -0,0 +1,33 @@
package org.firstinspires.ftc.teamcode.controller;
public class Trigger {
double value = 0;
double currentState;
double lastState;
boolean justPressed = false;
boolean justReleased = false;
public Trigger() {}
public void update(double value) {
lastState = currentState;
currentState = value;
justPressed = currentState != 0 && lastState == 0;
justReleased = currentState == 0 && lastState != 0;
this.value = value;
}
public double getValue() {
return value;
}
public boolean isJustPressed() {
return justPressed;
}
public boolean isJustReleased() {
return justReleased;
}
}

91
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Arm.java Normal file
View File

@ -0,0 +1,91 @@
package org.firstinspires.ftc.teamcode.hardware;
import com.acmerobotics.dashboard.config.Config;
import com.arcrobotics.ftclib.controller.PController;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.Servo;
import org.firstinspires.ftc.teamcode.hardware.robby.Slides;
@Config
public class Arm {
//make each servo
private Servo dServo;
private Servo rAServo;
private Servo lAServo;
private Servo wristServo;
private Slides.Position pos = Slides.Position.DOWN;
private PController armController;
private double armControllerTarget;
public static double ARM_KP = 0.01;
public enum Position {
INTAKE, SCORE;
}
public Arm(HardwareMap hardwareMap) {
wristServo = hardwareMap.get(Servo.class, "Wrist Servo");
dServo = hardwareMap.get(Servo.class, "Door Servo");
lAServo = hardwareMap.get(Servo.class, "Left Arm Servo");
rAServo = hardwareMap.get(Servo.class, "Right Arm Servo ");
// lAServo.setDirection(Servo.Direction.REVERSE);
rAServo.setDirection(Servo.Direction.REVERSE);
dServo.setDirection(Servo.Direction.REVERSE);
// wristServo.setDirection(Servo.Direction.REVERSE);
this.armController = new PController(ARM_KP);
setArmPos(Position.INTAKE);
setWristPos(Position.INTAKE);
}
public static double doorOpenpos = 0.1;
public static double doorClosePos = 0.85;
public static double armStart = 0.15;
public static double armScore = 1;
public static double wristStart = 0.93;
public static double wristScore = 0.98;
public void setArmPos(Position pos) {
this.armControllerTarget = pos == Position.INTAKE
? armStart
: armScore;
}
public void setWristPos(Position pos) {
if (pos == Position.INTAKE) {
wristServo.setPosition(wristStart);
} else if (pos == Position.SCORE) {
wristServo.setPosition(wristScore);
}
}
public void setDoor(DoorPos.DoorPosition pos) {
if (pos == DoorPos.DoorPosition.OPEN) {
dServo.setPosition(doorOpenpos);
} else if (pos == DoorPos.DoorPosition.CLOSE) {
dServo.setPosition(doorClosePos);
}
}
public String getTelemetry() {
return "Wrist Servo: "+wristServo.getPosition()+"Left Arm Servo: "+lAServo.getPosition()+"Right Arm Servo: "+rAServo.getPosition()+"Door Servo: "+dServo.getPosition();
}
public void update() {
this.armController.setSetPoint(this.armControllerTarget);
this.armController.setTolerance(0.001);
this.armController.setP(ARM_KP);
double output = 0;
if (!this.armController.atSetPoint()) {
output = Math.max(-1 * armScore, Math.min(armScore, this.armController.calculate(lAServo.getPosition())));
this.lAServo.setPosition(this.lAServo.getPosition() + output);
this.rAServo.setPosition(this.lAServo.getPosition() + output);
}
}
}

8
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/ArmPos.java Normal file
View File

@ -0,0 +1,8 @@
package org.firstinspires.ftc.teamcode.hardware;
public class ArmPos {
public enum APosition {
SDOWN, SCORE, SAFTEYDOWN, SAFTEYUP;
}
}

126
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Camera.java Normal file
View File

@ -0,0 +1,126 @@
package org.firstinspires.ftc.teamcode.hardware;
import static org.firstinspires.ftc.teamcode.util.Constants.INVALID_DETECTION;
import static org.firstinspires.ftc.teamcode.util.Constants.TARGETING_WEBCAM;
import static org.firstinspires.ftc.teamcode.util.Constants.WEBCAM_HEIGHT;
import static org.firstinspires.ftc.teamcode.util.Constants.WEBCAM_ROTATION;
import static org.firstinspires.ftc.teamcode.util.Constants.WEBCAM_WIDTH;
import com.qualcomm.robotcore.hardware.HardwareMap;
import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
import org.firstinspires.ftc.teamcode.util.CameraPosition;
import org.firstinspires.ftc.teamcode.vision.AprilTagDetectionPipeline;
import org.firstinspires.ftc.teamcode.vision.Detection;
import org.firstinspires.ftc.teamcode.vision.TargetingPipeline;
import org.openftc.apriltag.AprilTagDetection;
import org.openftc.easyopencv.OpenCvCamera;
import org.openftc.easyopencv.OpenCvCameraFactory;
import java.util.ArrayList;
// Class for the camera
public class Camera {
private float decimation;
private boolean needToSetDecimation;
int numFramesWithoutDetection = 0;
private boolean signalWebcamInitialized;
private OpenCvCamera signalWebcam;
private HardwareMap hardwareMap;
private OpenCvCamera targetingCamera;
private TargetingPipeline targetingPipeline;
private boolean targetingCameraInitialized;
AprilTagDetectionPipeline aprilTagDetectionPipeline;
ArrayList<AprilTagDetection> detections;
static final double FEET_PER_METER = 3.28084;
public CameraPosition cameraPosition;
private final Object decimationSync = new Object();
final float DECIMATION_HIGH = 3;
final float DECIMATION_LOW = 2;
final float THRESHOLD_HIGH_DECIMATION_RANGE_METERS = 1.0f;
final int THRESHOLD_NUM_FRAMES_NO_DETECTION_BEFORE_LOW_DECIMATION = 4;
// Constructor
public Camera(HardwareMap hardwareMap) {
this.hardwareMap = hardwareMap;
}
// Initiate the Targeting Camera
public void initTargetingCamera() {
int targetingCameraMonitorViewId = hardwareMap.appContext.getResources().getIdentifier("cameraMonitorViewId", "id", hardwareMap.appContext.getPackageName());
this.targetingCamera = OpenCvCameraFactory.getInstance().createWebcam(hardwareMap.get(WebcamName.class, TARGETING_WEBCAM), targetingCameraMonitorViewId);
this.targetingPipeline = new TargetingPipeline();
targetingCamera.setPipeline(targetingPipeline);
targetingCamera.openCameraDeviceAsync(new OpenCvCamera.AsyncCameraOpenListener() {
@Override
public void onOpened() {
targetingCamera.startStreaming(WEBCAM_WIDTH, WEBCAM_HEIGHT, WEBCAM_ROTATION);
targetingCameraInitialized = true;
}
@Override
public void onError(int errorCode) {
}
});
}
// Close the Targeting Camera
public void stopTargetingCamera() {
if (targetingCameraInitialized) {
targetingCamera.closeCameraDeviceAsync(() -> targetingCameraInitialized = false);
}
}
// detect colors
public Detection getRed() {
return (targetingCameraInitialized ? targetingPipeline.getRed() : INVALID_DETECTION);
}
public Detection getBlue() {
return (targetingCameraInitialized ? targetingPipeline.getBlue() : INVALID_DETECTION);
}
//return frame rate
public int getFrameCount() {
if (signalWebcamInitialized) {
return signalWebcam.getFrameCount();
} else {
return 0;
}
}
public int getMarkerId() {
detections = aprilTagDetectionPipeline.getLatestDetections();
// If there's been a new frame...
if (detections != null) {
// If we don't see any tags
if (detections.size() == 0) {
numFramesWithoutDetection++;
// If we haven't seen a tag for a few frames, lower the decimation
// so we can hopefully pick one up if we're e.g. far back
if (numFramesWithoutDetection >= THRESHOLD_NUM_FRAMES_NO_DETECTION_BEFORE_LOW_DECIMATION) {
aprilTagDetectionPipeline.setDecimation(DECIMATION_LOW);
}
}
// We do see tags!
else {
numFramesWithoutDetection = 0;
// If the target is within 1 meter, turn on high decimation to
// increase the frame rate
if (detections.get(0).pose.z < THRESHOLD_HIGH_DECIMATION_RANGE_METERS) {
aprilTagDetectionPipeline.setDecimation(DECIMATION_HIGH);
}
return detections.get(0).id;
}
}
return -1;
}
}

7
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/DoorPos.java Normal file
View File

@ -0,0 +1,7 @@
package org.firstinspires.ftc.teamcode.hardware;
public class DoorPos {
public enum DoorPosition {
OPEN, CLOSE
}
}

7
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Down.java Normal file
View File

@ -0,0 +1,7 @@
package org.firstinspires.ftc.teamcode.hardware;
public class Down {
public enum DownArm {
YES, NO
}
}

25
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/DroneLauncher.java Normal file
View File

@ -0,0 +1,25 @@
package org.firstinspires.ftc.teamcode.hardware;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.Servo;
import org.firstinspires.ftc.teamcode.util.Constants;
public class DroneLauncher {
private Servo servo;
public static double initPos = 0;
public static double launchPos = 0.5;
public DroneLauncher(HardwareMap hardwareMap) {
this.servo = hardwareMap.get(Servo.class, Constants.droneLauncher);
this.servo.setPosition(initPos);
}
public void launch() {
this.servo.setPosition(launchPos);
}
public void reset() {
this.servo.setPosition(initPos);
}
}

7
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Height.java Normal file
View File

@ -0,0 +1,7 @@
package org.firstinspires.ftc.teamcode.hardware;
public class Height {
public enum height {
ASCEND, HOLD, DESCEND, RESET
}
}

7
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/HopperPos.java Normal file
View File

@ -0,0 +1,7 @@
package org.firstinspires.ftc.teamcode.hardware;
public class HopperPos {
public enum hopperPos {
UP, DOWN
}
}

92
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Intake.java Normal file
View File

@ -0,0 +1,92 @@
package org.firstinspires.ftc.teamcode.hardware;
import com.acmerobotics.dashboard.config.Config;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.Servo;
@Config
public class Intake {
private Servo rServo;
private Servo lServo;
private DcMotor dcMotor;
private Position pos = Position.UP;
public enum Position {
STACK1, STACK2, STACK3, STACK4, STACK5, UP;
public Position nextPosition() {
int nextOne = (this.ordinal() + 1) % Position.values().length;
return Position.values()[nextOne];
}
public Position previousPosition() {
int backOne =Math.max(0,(this.ordinal() - 1) % Position.values().length);
return Position.values()[backOne];
}
}
//Position
public static double stack1 = 0;
public static double stack2 = 0.03;
public static double stack3 = 0.06;
public static double stack4 = 0.09;
public static double stack5 = 0.12;
public static double up = 0.30;
public static double motorPower = 0;
public Intake(HardwareMap hardwareMap, Position up) {
lServo = hardwareMap.get(Servo.class, "Left Servo");
lServo.setDirection(Servo.Direction.REVERSE);
rServo = hardwareMap.get(Servo.class, "Right Servo");
dcMotor = hardwareMap.get(DcMotor.class, "Intake Motor");
}
public void setpos(Position stack) {
if (stack == Position.STACK1) {
lServo.setPosition(stack1);
rServo.setPosition(stack1);
} else if (stack == Position.STACK2) {
lServo.setPosition(stack2);
rServo.setPosition(stack2);
} else if (stack == Position.STACK3) {
lServo.setPosition(stack3);
rServo.setPosition(stack3);
} else if (stack == Position.STACK4) {
lServo.setPosition(stack4);
rServo.setPosition(stack4);
} else if (stack == Position.STACK5) {
lServo.setPosition(stack5);
rServo.setPosition(stack5);
} else if (stack == Position.UP) {
lServo.setPosition(up);
rServo.setPosition(up);
}
}
public void incrementPos() {
pos = pos.nextPosition();
}
public void decrementPos() {
pos = pos.previousPosition();
}
public void setDcMotor(double pwr) {
dcMotor.setPower(pwr);
if (pwr >= 0.01) {
this.setpos(this.pos);
}
else {
this.setpos(Position.UP);
}
}
public double getPower() {
return dcMotor.getPower();
}
public String getTelemetry() {
return "lServo: "+lServo.getPosition()+"rServo: "+rServo.getPosition()+"dcMotor: "+dcMotor.getPower();
}
}

138
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/KhangSlides.java Normal file
View File

@ -0,0 +1,138 @@
package org.firstinspires.ftc.teamcode.hardware;
import com.acmerobotics.dashboard.config.Config;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.HardwareMap;
@Config
public class KhangSlides {
//Create and assign motors
public KhangSlides(HardwareMap hardwareMap) {
rSMotor = hardwareMap.get(DcMotor.class, "Right Slide Motor");
// rSMotor.setDirection(DcMotorSimple.Direction.REVERSE);
rSMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
rSMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
lSMotor = hardwareMap.get(DcMotor.class, "Left Slide Motor");
lSMotor.setDirection(DcMotorSimple.Direction.REVERSE);
lSMotor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
lSMotor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
}
//make each motor
private DcMotor rSMotor;
private DcMotor lSMotor;
//Value for motor power and speed slides go at
public static double rSPower = 0.5;
public static double lSPower = 0.5;
public static double downpos = 0;
public static double tape1pos = 200;
public static double tape2pos = 350;
public static double tape3pos = 500;
public static double maxpos = 1000;
//Set default slide height to 0 aka starting position
public static double sHeight = 0;
public static double newSHeight = 0;
// Do stuff to get to a position when asked to go to a position
public void setSPos(SlidePos.SPosition height) {
if (height == SlidePos.SPosition.DOWN) {
sHeight = 0;
lSMotor.setTargetPosition((int) downpos);
lSMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
lSMotor.setPower(lSPower);
rSMotor.setTargetPosition((int) downpos);
rSMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
rSMotor.setPower(rSPower);
} else if (height == SlidePos.SPosition.TAPE1) {
sHeight = 300;
lSMotor.setTargetPosition((int) tape1pos);
lSMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
lSMotor.setPower(lSPower);
rSMotor.setTargetPosition((int) tape1pos);
rSMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
rSMotor.setPower(rSPower);
} else if (height == SlidePos.SPosition.TAPE2) {
sHeight = 500;
lSMotor.setTargetPosition((int) tape2pos);
lSMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
lSMotor.setPower(lSPower);
rSMotor.setTargetPosition((int) tape2pos);
rSMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
rSMotor.setPower(rSPower);
} else if (height == SlidePos.SPosition.TAPE3) {
sHeight = 700;
lSMotor.setTargetPosition((int) tape3pos);
lSMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
lSMotor.setPower(lSPower);
rSMotor.setTargetPosition((int) tape3pos);
rSMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
rSMotor.setPower(rSPower);
} else if (height == SlidePos.SPosition.MAX) {
sHeight = 1000;
lSMotor.setTargetPosition((int) maxpos);
lSMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
lSMotor.setPower(lSPower);
rSMotor.setTargetPosition((int) maxpos);
rSMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
rSMotor.setPower(rSPower);
}
}
// public void setHeight(Hieght.height send) {
// if (send == Hieght.height.ASCEND) {
// if (newSHeight <= 790) {
// newSHeight = newSHeight + 10;
// lSMotor.setTargetPosition((int) (newSHeight));
// lSMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
// lSMotor.setPower(lSPower);
// rSMotor.setTargetPosition((int) (newSHeight));
// rSMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
// rSMotor.setPower(rSPower);
// } else if (newSHeight >= 800) {
// newSHeight = 800;
// lSMotor.setTargetPosition((int) (newSHeight));
// lSMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
// lSMotor.setPower(lSPower);
// rSMotor.setTargetPosition((int) (newSHeight));
// rSMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
// rSMotor.setPower(rSPower);
// }
// } else if (send == Hieght.height.HOLD) {
// newSHeight = newSHeight + 0;
// lSMotor.setTargetPosition((int) (newSHeight));
// lSMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
// lSMotor.setPower(lSPower);
// rSMotor.setTargetPosition((int) (newSHeight));
// rSMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
// rSMotor.setPower(rSPower);
// } else if (send == Hieght.height.DESCEND) {
// if (newSHeight >= 10) {
// newSHeight = newSHeight - 10;
// lSMotor.setTargetPosition((int) (newSHeight));
// lSMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
// lSMotor.setPower(lSPower);
// rSMotor.setTargetPosition((int) (newSHeight));
// rSMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
// rSMotor.setPower(rSPower);
// } else if (newSHeight <= 0) {
// newSHeight = 0;
// lSMotor.setTargetPosition((int) (newSHeight));
// lSMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
// lSMotor.setPower(lSPower);
// rSMotor.setTargetPosition((int) (newSHeight));
// rSMotor.setMode(DcMotor.RunMode.RUN_TO_POSITION);
// rSMotor.setPower(rSPower);
// }
// }
// }
//Return feed to the control hub for easy debug
public String getTelemetry() {
return "Left Slide Motor: "+lSMotor.getPower()+"Right Slide Motor: "+rSMotor.getPower();
}
}

109
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java Normal file
View File

@ -0,0 +1,109 @@
package org.firstinspires.ftc.teamcode.hardware;
import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.UP;
import static org.firstinspires.ftc.teamcode.hardware.DoorPos.DoorPosition.CLOSE;
import static org.firstinspires.ftc.teamcode.hardware.DoorPos.DoorPosition.OPEN;
import com.acmerobotics.dashboard.config.Config;
import com.qualcomm.robotcore.hardware.HardwareMap;
import org.firstinspires.ftc.teamcode.hardware.robby.Slides;
import org.firstinspires.ftc.teamcode.roadrunner.drive.SampleMecanumDrive;
import org.firstinspires.ftc.teamcode.roadrunner.util.Encoder;
@Config
public class Robot {
//Create each object
public SampleMecanumDrive drive;
public Camera camera;
public Intake intake;
public Slides slides;
public Arm arm;
public DroneLauncher droneLauncher;
public double macroStartTime = 0;
public int macroState = 0;
public int runningMacro = 0; // 0 = no macro | 1 = tier 1 | 2 = tier 2 | 3 = tier 3 | 4 = return
public int lastMacro = 0;
private boolean camEnabled = true;
//Name the objects
public Robot(HardwareMap hardwareMap) {
drive = new SampleMecanumDrive(hardwareMap);
camera = new Camera(hardwareMap);
camera.initTargetingCamera();
intake = new Intake(hardwareMap, UP);
slides = new Slides(hardwareMap);
arm = new Arm(hardwareMap);
droneLauncher = new DroneLauncher(hardwareMap);
camEnabled = true;
}
public void extendMacro(Slides.Position slidePos, double runTime) {
switch(macroState) {
case(0):
macroStartTime = runTime;
slides.setTarget(slidePos);
arm.setDoor(CLOSE);
macroState ++;
break;
case(1):
if (runTime > macroStartTime + 1) {
macroState ++;
}
break;
case(2):
macroStartTime = runTime;
arm.setArmPos(Arm.Position.SCORE);
arm.setWristPos(Arm.Position.SCORE);
macroState = 0;
lastMacro = runningMacro;
runningMacro = 0;
break;
}
}
public void resetMacro(Slides.Position pos, double runTime) {
switch(macroState) {
case(0):
macroStartTime = runTime;
arm.setDoor(OPEN);
macroState++;
break;
//Ind_sleeper
case(1):
if (runTime > macroStartTime + 1) {
macroState ++;
}
break;
case(2):
macroStartTime = runTime;
arm.setArmPos(Arm.Position.INTAKE);
arm.setWristPos(Arm.Position.INTAKE);
macroState++;
break;
case(3):
if (runTime > macroStartTime + 1.1) {
macroState ++;
}
break;
case(4):
macroStartTime = runTime;
slides.setTarget(pos);
macroState = 0;
lastMacro = runningMacro;
runningMacro = 0;
}
}
public void update(double runTime) {
drive.update();
slides.update(runTime);
arm.update();
}
public String getTelemetry() {
return "Telemetry?";
}
}

8
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/SlidePos.java Normal file
View File

@ -0,0 +1,8 @@
package org.firstinspires.ftc.teamcode.hardware;
public class SlidePos {
public enum SPosition {
DOWN, TAPE1, TAPE2, TAPE3, MAX, MANUAL;
}
}

119
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/robby/Slides.java Normal file
View File

@ -0,0 +1,119 @@
package org.firstinspires.ftc.teamcode.hardware.robby;
import com.acmerobotics.dashboard.config.Config;
import com.arcrobotics.ftclib.controller.PIDController;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.HardwareMap;
@Config
public class Slides {
private DcMotor slide;
private DcMotor slide2;
public static double p = 0.0014;
public static double i = 0.02;
public static double d = 0;
public static double f = 0.01;
public static double pTolerance = 20;
public static PIDController controller = new PIDController(p, i, d);
public static int targetMin = -10;
public static int targetMax = 1000;
public static int down = 0;
public static int tier1 = 200;
public static int tier2 = 350;
public static int tier3 = 500;
private int target = 0;
public static int manualSpeed = 20;
public enum Position { DOWN, TIER1, TIER2, TIER3 }
public Slides(HardwareMap hardwareMap) {
slide = hardwareMap.get(DcMotor.class, "Right Slide Motor");
slide.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
slide.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
// slide.setDirection(DcMotorSimple.Direction.REVERSE);
slide.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
slide2 = hardwareMap.get(DcMotor.class, "Left Slide Motor");
slide2.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
slide2.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
slide2.setDirection(DcMotorSimple.Direction.REVERSE);
slide.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
}
public void setTarget(int pos) {
target = Math.min(Math.max(pos, targetMin), targetMax);
}
public void setTarget(Position pos) {
if (pos == Position.DOWN) {
target = Math.min(Math.max(down, targetMin), targetMax);
} else if (pos == Position.TIER1) {
target = Math.min(Math.max(tier1, targetMin), targetMax);
} else if (pos == Position.TIER2) {
target = Math.min(Math.max(tier2, targetMin), targetMax);
} else if (pos == Position.TIER3) {
target = Math.min(Math.max(tier3, targetMin), targetMax);
}
}
public void increaseTarget(double increase) {
target += (int) (increase * manualSpeed);
target = Math.min(targetMax, Math.max(targetMin, target));
}
public int getTarget() {
return target;
}
public boolean atTarget() {
return controller.atSetPoint();
}
public void cancel() {
target = slide.getCurrentPosition();
}
public void targetReset() {
target = targetMin;
}
public void update(double runTime) {
// highPos = 720 + heightOffset;
// midPos = 350 + heightOffset;
// lowPos = heightOffset;
// pickupPos = 20 + heightOffset;
// downPos = heightOffset;// TODO add these back in
// if (target == 0) {
// slide.setPower(0);
// slide2.setPower(0);
// } else {
// if (target < 5) {
// slide.setPower(0);
// slide2.setPower(0);
// } else {
double pid, ff;
controller.setPID(p, i, d);
controller.setTolerance(pTolerance);
pid = controller.calculate(slide.getCurrentPosition(), target);
ff = f;
slide.setPower(pid + ff);
pid = controller.calculate(slide2.getCurrentPosition(), target);
ff = f;
slide2.setPower(pid + ff);
// }
// }
}
public String getTelemetry() {
return String.format("Position: %s %s\nTarget: %s %s\nPower: %s %s\nHeightOffset: %s", slide.getCurrentPosition(), slide2.getCurrentPosition(), target, target, slide.getPower(), slide2.getPower());
}
}

291
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/AbstractAuto.java Normal file
View File

@ -0,0 +1,291 @@
package org.firstinspires.ftc.teamcode.opmode.autonomous;
import com.acmerobotics.roadrunner.trajectory.Trajectory;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.firstinspires.ftc.teamcode.hardware.Robot;
import org.firstinspires.ftc.teamcode.hardware.robby.Slides;
import org.firstinspires.ftc.teamcode.util.CameraPosition;
import java.util.ArrayList;
import java.util.Locale;
public abstract class AbstractAuto extends LinearOpMode {
public Robot robot;
private int teamElementLocation = 2;
private ArrayList<Step> steps;
private double currentRuntime;
public CameraPosition cameraPosition;
// Main method to run all the steps for autonomous
@Override
public void runOpMode() {
// telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
telemetry.addLine("Initializing Robot...");
telemetry.update();
setCameraPosition();
robot = new Robot(hardwareMap);
makeTrajectories();
// while (robot.camera.getFrameCount() < 1) {
// idle();
// }
// wait for start
while (!(isStarted() || isStopRequested())) {
currentRuntime = getRuntime();
robot.update(currentRuntime);
// int newLocation = robot.camera.getMarkerId();
// if (newLocation != -1) {
// teamElementLocation = newLocation;
// }
telemetry.addLine("Initialized");
telemetry.addLine("Randomization: "+teamElementLocation);
telemetry.addLine(robot.getTelemetry());
telemetry.update();
}
resetRuntime();
// build the first step
steps = new ArrayList<>();
// stopCamera();
initializeSteps(teamElementLocation);
int stepNumber = 0;
double stepTimeout;
Step step = steps.get(stepNumber);
stepTimeout = step.getTimeout() != -1 ? currentRuntime + step.getTimeout() : Double.MAX_VALUE;
step.start();
// run the remaining steps
while (opModeIsActive()) {
currentRuntime = getRuntime();
// once a step finishes
if (step.isFinished() || currentRuntime >= stepTimeout) {
// do the finishing move
step.end();
stepNumber++;
// if it was the last step break out of the while loop
if (stepNumber > steps.size() - 1) {
break;
}
// else continue to the next step
step = steps.get(stepNumber);
stepTimeout = step.getTimeout() != -1 ? currentRuntime + step.getTimeout() : Double.MAX_VALUE;
step.start();
}
// // update turret and slides position
// PoseStorage.slidesPosition = robot.actuators.getSlides();
// PoseStorage.turretPosition = robot.actuators.getTurret();
// while the step is running display telemetry
step.whileRunning();
robot.update(currentRuntime);
telemetry.addLine(String.format(Locale.US, "Runtime: %.0f", currentRuntime));
telemetry.addLine("Step " + (stepNumber + 1) + " of " + steps.size() + ", " + step.getTelemetry() + "\n");
telemetry.addLine(robot.getTelemetry());
telemetry.update();
}
}
// Load up all of the steps for the autonomous: to be overridden with the specific steps in the specific auto
public abstract void initializeSteps(int location);
//methods to be implemented in the specific autos
public abstract void setAlliance();
public abstract void setCameraPosition();
public abstract boolean useCamera();
public abstract void makeTrajectories();
// public abstract void setArm(Arm.Position armPos, Claw.Position clawPos);
//other methods that do certain tasks
public void turn(double degrees) {
steps.add(new Step("Following a trajectory") {
@Override
public void start() {
robot.drive.turn(degrees);
}
@Override
public void whileRunning() {
robot.drive.update();
}
@Override
public void end() {
}
@Override
public boolean isFinished() {
return !robot.drive.isBusy();
}
});
}
public void followTrajectory(Trajectory trajectory) {
steps.add(new Step("Following a trajectory") {
@Override
public void start() {
robot.drive.followTrajectoryAsync(trajectory);
}
@Override
public void whileRunning() {
robot.drive.update();
}
@Override
public void end() {
}
@Override
public boolean isFinished() {
return !robot.drive.isBusy();
}
});
}
public void followAndExtend(Trajectory trajectory, Slides.Position slidePos) {
steps.add(new Step("Following a trajectory") {
@Override
public void start() {
robot.drive.followTrajectoryAsync(trajectory);
if (slidePos == Slides.Position.TIER1) {
robot.runningMacro = 1;
} else if (slidePos == Slides.Position.TIER2) {
robot.runningMacro = 2;
} else if (slidePos == Slides.Position.TIER3) {
robot.runningMacro = 3;
}
robot.extendMacro(slidePos, currentRuntime);
}
@Override
public void whileRunning() {
if (robot.runningMacro != 0) {
robot.extendMacro(slidePos, currentRuntime);
}
robot.drive.update();
}
@Override
public void end() {
}
@Override
public boolean isFinished() {
return !robot.drive.isBusy() && robot.runningMacro == 0;
}
});
}
public void followAndRetract(Trajectory trajectory, Slides.Position slidePos) {
steps.add(new Step("Following a trajectory") {
@Override
public void start() {
robot.drive.followTrajectoryAsync(trajectory);
robot.runningMacro = 4;
robot.resetMacro(slidePos, currentRuntime);
}
@Override
public void whileRunning() {
if (robot.runningMacro != 0) {
robot.resetMacro(slidePos, currentRuntime);
}
robot.drive.update();
}
@Override
public void end() {
}
@Override
public boolean isFinished() {
return !robot.drive.isBusy() && robot.runningMacro == 0;
}
});
}
public void runIntake(double power, double timeout) {
steps.add(new Step("Running Intake" + timeout + " seconds", timeout) {
@Override
public void start() {
robot.intake.setDcMotor(power);
}
@Override
public void whileRunning() {
}
@Override
public void end() {
robot.intake.setDcMotor(0);
}
@Override
public boolean isFinished() {
return false;
}
});
}
// Functions to add steps
public void addDelay(double timeout) {
steps.add(new Step("Waiting for " + timeout + " seconds", timeout) {
@Override
public void start() {
}
@Override
public void whileRunning() {
}
@Override
public void end() {
}
@Override
public boolean isFinished() {
return false;
}
});
}
public void stopCamera() {
steps.add(new Step("Stopping Signal Camera") {
@Override
public void start() {
robot.camera.stopTargetingCamera();
}
@Override
public void whileRunning() {
}
@Override
public void end() {
}
@Override
public boolean isFinished() {
return true;
}
});
}
}

59
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueLeftAuto.java Normal file
View File

@ -0,0 +1,59 @@
package org.firstinspires.ftc.teamcode.opmode.autonomous;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.trajectory.Trajectory;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import org.firstinspires.ftc.teamcode.hardware.robby.Slides;
@Autonomous(name = "Blue Left Auto")
public class BlueLeftAuto extends AbstractAuto {
public Trajectory scorePurple;
public Trajectory scoreYellow;
public Trajectory parkRobot;
@Override
public void initializeSteps(int location) {
followTrajectory(scorePurple);
runIntake(-0.3, 1);
followAndExtend(scoreYellow, Slides.Position.TIER1);
followAndRetract(parkRobot, Slides.Position.DOWN);
}
@Override
public void setAlliance() {
}
@Override
public void setCameraPosition() {
}
@Override
public boolean useCamera() {
return false;
}
@Override
public void makeTrajectories() {
Pose2d start = new Pose2d(24, 61.5, Math.toRadians(-90));
// Pose2d dropLeft = new Pose2d(24, 60, Math.toRadians(-90));
Pose2d dropMiddle = new Pose2d(24, 40.5, Math.toRadians(-90));
// Pose2d dropRight = new Pose2d(24, 60, Math.toRadians(-90));
Pose2d score = new Pose2d(62, 36, Math.toRadians(180));
Pose2d park = new Pose2d(62, 60, Math.toRadians(180));
scorePurple = robot.drive.trajectoryBuilder(start)
.lineToLinearHeading(dropMiddle)
.build();
scoreYellow = robot.drive.trajectoryBuilder(scorePurple.end())
.lineToLinearHeading(score)
.build();
parkRobot = robot.drive.trajectoryBuilder(scoreYellow.end())
.lineToLinearHeading(park)
.build();
robot.drive.setPoseEstimate(start);
}
}

79
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueRightAuto_Drivepathonly_.java Normal file
View File

@ -0,0 +1,79 @@
package org.firstinspires.ftc.teamcode.opmode.autonomous;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.trajectory.Trajectory;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.firstinspires.ftc.teamcode.roadrunner.drive.SampleMecanumDrive;
@Autonomous(name = "BlueRightAuto(Drive only)")
public class BlueRightAuto_Drivepathonly_ extends LinearOpMode {
@Override
public void runOpMode() throws InterruptedException {
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
// add more trajectories here
Pose2d start = new Pose2d(-36, 61.5, Math.toRadians(-90));
// Pose2d dropLeft = new Pose2d(24, 60, Math.toRadians(-90));
Pose2d dropMiddle = new Pose2d(-36, 40.5, Math.toRadians(-90));
//
// Pose2d nine = new Pose2d(-36, 40.5, Math.toRadians(-180));
Pose2d stack1 = new Pose2d(-50, 40.5, Math.toRadians(-180));
Pose2d dropMiddle2 = new Pose2d(-36, 40.5, Math.toRadians(-180));
Pose2d bmid = new Pose2d(-36, 5, Math.toRadians(-180));
Pose2d bmid2 = new Pose2d(36, 5, Math.toRadians(-180));
Pose2d alimb = new Pose2d(54, 40.5, Math.toRadians(-180));
Pose2d score = new Pose2d(54, 36, Math.toRadians(180));
drive.setPoseEstimate(start);
// add this per trajectories
Trajectory scorePurple = drive.trajectoryBuilder(start)
.lineToLinearHeading(dropMiddle)
.build();
Trajectory mid_drop = drive.trajectoryBuilder(dropMiddle)
.lineToLinearHeading(stack1)
.build();
Trajectory back_to_mid = drive.trajectoryBuilder(stack1)
.lineToLinearHeading(dropMiddle2)
.build();
Trajectory front_gate = drive.trajectoryBuilder(dropMiddle2)
.lineToLinearHeading(bmid)
.build();
Trajectory front_gate_pt_2 = drive.trajectoryBuilder(bmid)
.lineToLinearHeading(bmid2)
.build();
Trajectory front_gate_pt_3 = drive.trajectoryBuilder(bmid2)
.lineToLinearHeading(alimb)
.build();
Trajectory about_to_score = drive.trajectoryBuilder(alimb)
.lineToLinearHeading(score)
.build();
// the scorePurple. should be whatever the start pose2d thing was
Trajectory scoreYellow = drive.trajectoryBuilder(scorePurple.end())
.lineToLinearHeading(score)
.build();
waitForStart();
if(isStopRequested()) return;
// add this per trajectories
drive.followTrajectory(scorePurple);
drive.turn(-90);
drive.followTrajectory(mid_drop);
// drive.followTrajectory(stack);
drive.followTrajectory(back_to_mid);
drive.followTrajectory(front_gate);
drive.followTrajectory(front_gate_pt_2);
drive.followTrajectory(front_gate_pt_3);
drive.followTrajectory(about_to_score);
drive.followTrajectory(scoreYellow);
}
}

42
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/SimpleBlueLeftAuto_ignore.java Normal file
View File

@ -0,0 +1,42 @@
package org.firstinspires.ftc.teamcode.opmode.autonomous;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.trajectory.Trajectory;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.firstinspires.ftc.teamcode.roadrunner.drive.SampleMecanumDrive;
@Autonomous(name = "Simple Blue Left Auto")
public class SimpleBlueLeftAuto_ignore extends LinearOpMode {
@Override
public void runOpMode() throws InterruptedException {
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
// add more trajectories here
Pose2d start = new Pose2d(24, 61.5, Math.toRadians(-90));
// Pose2d dropLeft = new Pose2d(24, 60, Math.toRadians(-90));
Pose2d dropMiddle = new Pose2d(24, 40.5, Math.toRadians(-90));
// Pose2d dropRight = new Pose2d(24, 60, Math.toRadians(-90));
Pose2d score = new Pose2d(60, 36, Math.toRadians(180));
drive.setPoseEstimate(start);
// add this per trajectories
Trajectory scorePurple = drive.trajectoryBuilder(start)
.lineToLinearHeading(dropMiddle)
.build();
// the scorePurple. should be whatever the start pose@d thing was
Trajectory scoreYellow = drive.trajectoryBuilder(scorePurple.end())
.lineToLinearHeading(score)
.build();
waitForStart();
if(isStopRequested()) return;
// add this per trajectories
drive.followTrajectory(scorePurple);
// drive.wait(1);
drive.followTrajectory(scoreYellow);
}
}

53
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/Step.java Normal file
View File

@ -0,0 +1,53 @@
package org.firstinspires.ftc.teamcode.opmode.autonomous;
import org.firstinspires.ftc.teamcode.vision.Detection;
// Class for every step that the autonomous program will take
public abstract class Step {
private final double timeout;
private String telemetry;
// variables when moving
public double x;
public double y;
public double stepTime;
double tempTime = stepTime;
// variables when shooting
public Detection teamProp;
// Constructors
public Step(String telemetry) {
this.telemetry = telemetry;
this.timeout = -1;
}
public Step(String telemetry, double timeout) {
this.telemetry = telemetry;
this.timeout = timeout;
}
// Abstract methods to be overrode
public abstract void start();
public abstract void whileRunning();
public abstract void end();
public abstract boolean isFinished();
// Return the timeout for the step
public double getTimeout() {
return timeout;
}
public void setTelemetry(String telemetry) {
this.telemetry = telemetry;
}
// Return the Telemetry for the step
public String getTelemetry() {
return telemetry;
}
}

304
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/AbstractTeleOp.java Normal file
View File

@ -0,0 +1,304 @@
//package org.firstinspires.ftc.teamcode.opmode.teleop;
//
//import static org.firstinspires.ftc.teamcode.opmode.teleop.Team.BLUE;
//import static org.firstinspires.ftc.teamcode.opmode.teleop.Team.RED;
//import static java.lang.Math.PI;
//
//import com.acmerobotics.dashboard.config.Config;
//import com.acmerobotics.roadrunner.geometry.Pose2d;
//import com.arcrobotics.ftclib.controller.PIDController;
//import com.qualcomm.robotcore.eventloop.opmode.OpMode;
//
//import org.firstinspires.ftc.teamcode.controller.Controller;
//import org.firstinspires.ftc.teamcode.hardware.Arm;
//import org.firstinspires.ftc.teamcode.hardware.Robot;
//import org.firstinspires.ftc.teamcode.hardware.robby.Slides;
//
//@Config
//public abstract class AbstractTeleOp extends OpMode {
// private Robot robot;
// public Team team;
// Controller driver1;
// Controller driver2;
//
// public static double normal = 0.5;
// public static double vazrooooming = 1.0;
// public static int heightIncrement = 20;
//
// Pose2d robot_pos;
// double robot_x, robot_y, robot_heading;
//
// // auto align variables
// double headingPID;
// public static double headingP = 0.01;
// public static double headingI = 0.03;
// public static double headingD = 0.0005;
// public static PIDController headingController = new PIDController(headingP, headingI, headingD);
//
// double strafePID;
// public static double strafeP = 0.05;
// public static double strafeI = 0;
// public static double strafeD = 0.01;
// public static PIDController strafeController = new PIDController(strafeP, strafeI, strafeD);
//
// double robot_y_pos;
// boolean fixed90Toggle = false;
// boolean fixed0Toggle = false;
//
//// public static double robot_width = 12;
//// public static double robot_length = 12;
//// public static double robot_radius = 6;
////
//// public static double groundJuncRadius = 6;
//// public static double coneRadius = 4;
////
//// public static double armWait = 0.2;
//
//// private double timeSinceOpened = 0; // for claw
//// private double timeSinceClosed = 0;
//
// // private int delayState = 0; // for arm
//// private double delayStart = 0;
//// private boolean doArmDelay = false; // for arm
// private boolean isAutoClose = true;
//
// @Override
// public void init() {
// robot = new Robot(hardwareMap);
// driver1 = new Controller(gamepad1);
// driver2 = new Controller(gamepad2);
// if (team == RED) {
// driver1.setColor(255, 0, 0);
// driver2.setColor(255, 0, 0);
// } else if (team == BLUE) {
// driver1.setColor(0, 0, 255);
// driver2.setColor(0, 0, 255);
// }
//
// PoseStorage.AutoJustEnded = false;
// }
//
// @Override
// public void init_loop() {
// ////init when camera is on and responding
//// if (robot.camera.getFrameCount() < 1) {
//// telemetry.addLine("Initializing Robot...");
//// } else {
//// telemetry.addLine("Initialized");
//// }
// telemetry.addLine("Initialized");
// telemetry.addLine(robot.getTelemetry());
// telemetry.update();
// }
//
// //find out what quarter you are in
// private int getQuadrant(double angle) {
// if (0 < angle && angle < PI/2.0) {
// return 1;
// } else if (PI/2.0 < angle && angle < PI) {
// return 2;
// } else if (PI < angle && angle < 3*PI/2.0) {
// return 3;
// } else {
// return 4;
// }
// }
//
// @Override
// public void loop() {
// // robot position update
// robot_pos = robot.drive.getPoseEstimate();
// robot_x = robot_pos.getX();
// robot_y = robot_pos.getY();
// robot_heading = robot_pos.getHeading(); // in radians
//
// // driver 1 controls
// driver1.update();
// driver2.update();
//
// //create and set X, Y, and Z for drive inputs
// double x = -gamepad1.left_stick_y;
// double y = -gamepad1.left_stick_x;
// double z = -gamepad1.right_stick_x;
//
//// figure out if a toggle is present
// if (driver1.getRightBumper().isPressed()) {
// if (!fixed90Toggle) {
// robot_y_pos = robot.drive.getPoseEstimate().getX();
// }
// fixed90Toggle = true;
// } else {
// fixed90Toggle = false;
// }
//
// if (!fixed90Toggle && driver1.getLeftBumper().isPressed()) {
// if (!fixed0Toggle) {
// robot_y_pos = robot.drive.getPoseEstimate().getY();
// }
// fixed0Toggle = true;
// } else {
// fixed0Toggle = false;
// }
//
// // heading pid
// headingController.setPID(headingP, headingI, headingD);
// int quadrant = getQuadrant(robot_heading);
// if (fixed0Toggle) {
// switch (quadrant) {
// case 1:
// headingPID = headingController.calculate(Math.toDegrees(robot_heading), 0);//- right
// break;
// case 2:
// headingPID = headingController.calculate(Math.toDegrees(robot_heading), 180);//+ left
// break;
// case 3:
// headingPID = headingController.calculate(Math.toDegrees(robot_heading), 180);//- right
// break;
// case 4:
// headingPID = headingController.calculate(Math.toDegrees(robot_heading), 360);//+ left
// break;
// }
// } else if (fixed90Toggle) {
// switch (quadrant) {
// case 1:
// headingPID = headingController.calculate(Math.toDegrees(robot_heading), 90);//- right
// break;
// case 2:
// headingPID = headingController.calculate(Math.toDegrees(robot_heading), 90);//+ left
// break;
// case 3:
// headingPID = headingController.calculate(Math.toDegrees(robot_heading), 270);//- right
// break;
// case 4:
// headingPID = headingController.calculate(Math.toDegrees(robot_heading), 270);//+ left
// break;
// }
// } else {
// headingPID = 0;
// }
//
// // strafe pid
// strafeController.setPID(strafeP, strafeI, strafeD);
// if (fixed0Toggle) {
// strafePID = strafeController.calculate(robot_y, robot_y_pos);
// } else if (fixed90Toggle) {
// strafePID = strafeController.calculate(robot_x, robot_y_pos);
// } else {
// strafePID = 0;
// }
//
// telemetry.addLine("Wanted Position: "+robot_y_pos);
// telemetry.addLine("Actual Position: "+robot_y);
// telemetry.addLine("PID: "+strafePID);
//
// // turbo
// if (driver1.getLeftBumper().isPressed() || driver1.getRightBumper().isPressed()) {
// x *= vazrooooming;
// y *= vazrooooming;
// z *= vazrooooming;
// } else {
// x *= normal;
// y *= normal;
// z *= normal;
// }
//
// // actually set power
// if (fixed0Toggle || fixed90Toggle) {
// robot.drive.setWeightedDrivePower(new Pose2d(x, 0, headingPID));
// } else {
// robot.drive.setWeightedDrivePower(new Pose2d(x, y, z));
// }
//
// // driver 2 controls
// // increment heights
// if (driver2.getDUp().isJustPressed()) {
// Slides.heightOffset += heightIncrement;
// robot.slides.setTarget(robot.slides.getTarget() + heightIncrement);
// } else if (driver2.getDDown().isJustPressed()) {
// Slides.heightOffset -= heightIncrement;
// robot.slides.setTarget(robot.slides.getTarget() - heightIncrement);
// }
//
// switch (robot.runningMacro) {
// case(0): // manual mode
//
// //track pad is rad
// // left right dpad is turn wrist
// robot.slides.increaseTarget(driver2.getLeftStick().getY());
//// if (Math.abs(driver2.getRightStick().getY()) > 0.05) { // close claw if anything is moved
//// robot.claw.close();
//// }
//
//// if (driver2.getRightBumper().isJustPressed()) {
//// robot.arm.goToScore();
//// }
//// if (driver2.getLeftBumper().isJustPressed()) {
//// robot.arm.goToIntake();
//// }
//
// // retract all the time
// if (driver2.getLeftStickButton().isJustPressed()) {
// robot.runningMacro = 5;
// }
//
// // to cancel macro and allow manual movement
//// if (driver2.getLeftStick().getY() > 0.05 || driver2.getRightStick().getY() > 0.05) {
//// robot.runningMacro = 0;
//// robot.lastMacro = 0;
//// }
//
// if (driver2.getTouchpad().isJustPressed()) {
// robot.runningMacro = 4;
// } else if (driver2.getX().isJustPressed()) { // high position [closed, bring up, bring out]
// robot.runningMacro = 3;
// } else if (driver2.getY().isJustPressed()) { // middle position [middle goal level]
// robot.runningMacro = 2;
// } else if (driver2.getB().isJustPressed() && !driver2.getStart().isJustPressed()) { // low position [low goal level]
// robot.runningMacro = 1;
// } else if (driver2.getA().isJustPressed()) {
// if (robot.lastMacro == 0 || robot.lastMacro == 4) { // if not running any macros or picking up cones
// } else { // otherwise, I need to undo a macro
// robot.runningMacro = 5;
// }
// } else {
// if (driver2.getRightBumper().isJustPressed()) {
// isAutoClose = false;
// }
// if (driver2.getLeftBumper().isJustPressed()) {
// }
// }
// break;
// case(1):
//// robot.extendMacro(Slides.Position.LOW, Arm.APosition.SCORE, getRuntime());
// break;
// case(2):
//// robot.extendMacro(Slides.Position.MEDIUM, Arm.APosition.SCORE, getRuntime());
// break;
// case(3):
//// robot.extendMacro(Slides.Position.HIGH, Arm.APosition.SCORE, getRuntime());
// break;
// case(4):
//// robot.extendMacro(Slides.Position.PICKUP, Arm.APosition.SDOWN, getRuntime());
// break;
// case (5): // macro reset
// robot.resetMacroNoDunk(0, getRuntime());
// driver1.rumble(100);
// break;
// }
//
// // cancel the macros
// if (driver2.getRightStickButton().isJustPressed()) {
// robot.runningMacro = 0;
// robot.lastMacro = 0;
// robot.macroState = 0;
// robot.slides.cancel();
// }
//
// // update and telemetry
// robot.update(getRuntime());
//
// telemetry.addLine(robot.getTelemetry());
// telemetry.addLine(String.format("Last Macro: %s\nRunning Macro: %s\nMacroState: %s", robot.lastMacro, robot.runningMacro, robot.macroState));
// telemetry.update();
// }
//}

11
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/BlueTeam.java Normal file
View File

@ -0,0 +1,11 @@
//package org.firstinspires.ftc.teamcode.opmode.teleop;
//import com.qualcomm.robotcore.eventloop.opmode.Disabled;
//import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
//
//@TeleOp(name = "Opmode For Team Blue", group = "Competition")
//public class BlueTeam extends AbstractTeleOp {
//
// public BlueTeam() {
// team = Team.BLUE;
// }
//}

47
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/Drivebase.java Normal file
View File

@ -0,0 +1,47 @@
package org.firstinspires.ftc.teamcode.opmode.teleop;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import org.firstinspires.ftc.teamcode.controller.Controller;
import org.firstinspires.ftc.teamcode.hardware.Robot;
@TeleOp(name = "Drivebase Only", group = "OpModes")
public class Drivebase extends OpMode {
//turbo mode
public static double normal = 1;
public static double turbo = 0.5;
//create robot instance
private Robot robot;
//create controller 1 (driver)
private Controller controller1;
@Override
public void init() {
this.robot = new Robot(hardwareMap);
controller1 = new Controller(gamepad1);
telemetry.addLine("Started");
//update to make sure we receive last line of code
telemetry.update();
}
@Override
public void loop() {
//create and set X, Y, and Z for drive inputs
double x = -gamepad1.left_stick_y;
double y = -gamepad1.left_stick_x;
double z = -gamepad1.right_stick_x;
robot.drive.setWeightedDrivePower(new Pose2d(x, y, z));
//turbo activation
if (controller1.getA().isJustPressed()){
x *= turbo;
y *= turbo;
z *= turbo;
} else if (controller1.getA().isJustReleased()){
x *= normal;
y *= normal;
z *= normal;
}
}
}

257
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/ExperimentalKhangMain.java Normal file
View File

@ -0,0 +1,257 @@
//package org.firstinspires.ftc.teamcode.opmode.teleop;
//
//import com.acmerobotics.dashboard.config.Config;
//import com.acmerobotics.roadrunner.geometry.Pose2d;
//import com.qualcomm.robotcore.eventloop.opmode.OpMode;
//import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
//import com.qualcomm.robotcore.hardware.Servo;
//import com.qualcomm.robotcore.util.ElapsedTime;
//
//import org.firstinspires.ftc.teamcode.controller.Controller;
//import org.firstinspires.ftc.teamcode.hardware.DoorPos;
//import org.firstinspires.ftc.teamcode.hardware.Down;
//import org.firstinspires.ftc.teamcode.hardware.Hieght;
//import org.firstinspires.ftc.teamcode.hardware.HopperPos;
//import org.firstinspires.ftc.teamcode.hardware.Intake;
//import org.firstinspires.ftc.teamcode.hardware.Robot;
//
//@Config
//@TeleOp(name = "experimental opmode", group = "OpModes")
//public class ExperimentalKhangMain extends OpMode {
//
// //keep track of runtime for advanced macros
// private ElapsedTime runTime = new ElapsedTime();
//
// //create and set start position of intake
// private Intake.Position Currentpos = Intake.Position.UP;
// //inform if slides are in lowest position or not
// private Down.DownArm DownQuestion = Down.DownArm.YES;
// //create and set start position of slides
// private SlidePos.SPosition CurrentSpos = SlidePos.SPosition.DOWN;
// //create and set start position of arm
// private ArmPos.APosition CurrentApos = ArmPos.APosition.SDOWN;
// //create and set start position of door, default close
// private DoorPos.DoorPosition CurrentDpos = DoorPos.DoorPosition.CLOSE;
// //create and set start position of hopper
// private HopperPos.hopperPos hopperpos = HopperPos.hopperPos.DOWN;
// //create manual slide height varable and set it to hold position as start
// private Hieght.height CurrentHeight = Hieght.height.HOLD;
//
// //robot position stuff
// Pose2d robot_pos;
// double robot_x, robot_y, robot_heading;
//
// //create robot instance
// private Robot robot;
// //create servo for plane
// private Servo planeServo;
// //create controller 1 (driver)
// private Controller controller1;
// //create controller 2 (macro user and one with hard life)
// private Controller controller2;
// //set starting slide height to 0
// private double sHeight = 0;
//
// @Override
// public void init() {
//
// //reset runtime when opmode is initialized
// runTime.reset();
//
// //make each servo, motor, and controller and configure them
// planeServo = hardwareMap.get(Servo.class, "Plane Servo");
// this.robot = new Robot(hardwareMap);
// controller1 = new Controller(gamepad1);
// controller2 = new Controller(gamepad2);
// //feedback to driver hub to know if init was successful
// telemetry.addLine("Started");
// //update to amke sure we receive last lien of code
// telemetry.update();
// }
//
// @Override
// public void loop() {
//
// // robot position update
// robot_pos = robot.drive.getPoseEstimate();
// robot_x = robot_pos.getX();
// robot_y = robot_pos.getY();
// robot_heading = robot_pos.getHeading(); // in radians
//
// // Calculate the runtime in seconds
// double currentTime = runTime.seconds();
//
// //set start of macro runtime
// double macroStartTime = 0;
//
// //create and set X, Y, and Z for drive inputs
// double x = -gamepad1.left_stick_y;
// double y = -gamepad1.left_stick_x;
// double z = -gamepad1.right_stick_x;
// robot.drive.setWeightedDrivePower(new Pose2d(x, y, z));
//
// //set intake to be pressure reactant to right trigger
// robot.intake.setDcMotor(gamepad2.right_trigger);
// //manual slide height control
// double sHeight = gamepad2.right_stick_y;
// //activate intake or not
// double intakeON = gamepad2.right_trigger;
// //graph input of Y joystick to make macros for slides
// double sY = -gamepad2.left_stick_y;
// //graph of X
// double sX = gamepad2.left_stick_x;
//
// //reset value to set slides to starting value
// double reset = gamepad2.left_trigger;
//
// //variable to determine shoot drone or not
// double shoot = gamepad1.right_trigger;
//
// //update variables to constantly feed position each component should be in
// //intake
// robot.intake.setpos(Currentpos);
// //slide macro
// robot.slides.setSPos(CurrentSpos);
// //slide manual
//// robot.slides.setHeight(CurrentHeight);
// //arm position
// robot.arm.setPos(CurrentApos);
// //door open or not
// robot.arm.openDoor(CurrentDpos);
// //update
// controller2.update();
//
// //manual slide input reader
//// if (sHeight >= 0.2) {
//// CurrentHeight = Hieght.height.ASCEND;
//// } else if (sHeight <= -0.2) {
//// CurrentHeight = Hieght.height.DESCEND;
//// } else {
//// CurrentHeight = Hieght.height.HOLD;
//// }
//
// //read values to determine if plane should shoot or not
// if (shoot >= 0.9) {
// planeServo.setPosition(0.2);
// } else {
// planeServo.setPosition(0);
// }
//
// //make door rise as intake goes on
// if (intakeON >= 0.01) {
// CurrentDpos = DoorPos.DoorPosition.OPEN;
// } else {
// CurrentDpos = DoorPos.DoorPosition.CLOSE;
// }
//
// if (intakeON >= 0.35) {
// Currentpos = Intake.Position.STACK1;
// } else {
// Currentpos = Intake.Position.UP;
// }
//
// //reset slide position
// if (reset >= 0.2) {
// CurrentSpos = CurrentSpos.DOWN;
// }
//
//// //control position of hopper
//// if (controller2.getLeftBumper().isJustPressed()) {
//// hopperpos = HopperPos.hopperPos.UP;
//// } else if (controller2.getLeftBumper().isJustReleased()) {
//// hopperpos = HopperPos.hopperPos.DOWN;
//// }
//
// //shift intake up one pixel
// if (controller2.getDLeft().isJustPressed()) {
// //prevent from going higher than highest legal value
// if (Currentpos != Intake.Position.UP) {
// Currentpos = Currentpos.nextPosition();
// }
// }
//
// //shift intake down one pixel
// if (controller2.getDRight().isJustPressed()) {
// //prevent from going lower than lowest value
// if (Currentpos != Intake.Position.STACK1) {
// Currentpos = Currentpos.previousPosition();
// }
// }
//
// //set intake to max position
// if (controller2.getDUp().isJustPressed()) {
// Currentpos = Currentpos.UP;
// }
//
// //set intake to lowest position
// if (controller2.getDDown().isJustPressed()) {
// Currentpos = Currentpos.STACK1;
// }
//
//// //arm safety pause going up
//// if (controller2.getA().isJustPressed()){
//// CurrentApos = CurrentApos.SAFTEYUP;
//// DownQuestion = DownQuestion.NO;
//// }
//
// if (controller2.getA().isJustPressed()) {
// macroStartTime = currentTime;
// CurrentSpos = CurrentSpos.TAPE1;
// if (macroStartTime + 1000 <= currentTime) {
// CurrentApos = CurrentApos.SCORE;
// }
// }
//
// //arm safety pause going down
// if (controller2.getX().isJustPressed()) {
// CurrentApos = CurrentApos.SAFTEYDOWN;
// DownQuestion = DownQuestion.NO;
// }
//
// //arm all the way up
// if (controller2.getB().isJustPressed()) {
// CurrentApos = CurrentApos.SCORE;
// DownQuestion = DownQuestion.NO;
// }
//
// //arm all the way down
// if (controller2.getY().isJustPressed()) {
// CurrentApos = CurrentApos.SDOWN;
// DownQuestion = DownQuestion.YES;
// }
//
// //set slides to max when left joystick is up
// if (sY >= 0.5) {
// CurrentSpos = CurrentSpos.MAX;
// }
//
// //set slides to tape 1 level when left joystick is down
// if (sY <= -0.5) {
// CurrentSpos = CurrentSpos.TAPE1;
// }
//
// //set slides to tape 3 when left joystick is right
// if (sX >= 0.2) {
// CurrentSpos = CurrentSpos.TAPE3;
// }
//
// //set slides to tape 2 when left joystick is left
// if (sX <= -0.2) {
// CurrentSpos = CurrentSpos.TAPE2;
// }
//
// //set slides all the way down when left bumper gets pressed
// if (controller2.getLeftBumper().isJustPressed()) {
// CurrentSpos = CurrentSpos.DOWN;
// }
//
// //set intake all teh way up when right bumper is pressed
// if (controller2.getRightBumper().isJustPressed()) {
// Currentpos = Currentpos.UP;
// }
//
// // update the runtime on the telemetry
// telemetry.addData("Runtime", currentTime);
// telemetry.update();
// }
//}

260
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/Meet1TeleOp.java Normal file
View File

@ -0,0 +1,260 @@
//package org.firstinspires.ftc.teamcode.opmode.teleop;
//import com.acmerobotics.dashboard.config.Config;
//import com.acmerobotics.roadrunner.geometry.Pose2d;
//import com.qualcomm.robotcore.eventloop.opmode.OpMode;
//import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
//import com.qualcomm.robotcore.hardware.Servo;
//
//import org.firstinspires.ftc.teamcode.controller.Controller;
//import org.firstinspires.ftc.teamcode.hardware.Arm;
//import org.firstinspires.ftc.teamcode.hardware.DoorPos;
//import org.firstinspires.ftc.teamcode.hardware.Down;
//import org.firstinspires.ftc.teamcode.hardware.Hieght;
//import org.firstinspires.ftc.teamcode.hardware.HopperPos;
//import org.firstinspires.ftc.teamcode.hardware.Intake;
//import org.firstinspires.ftc.teamcode.hardware.Robot;
//import org.firstinspires.ftc.teamcode.hardware.SlidePos;
//import com.qualcomm.robotcore.util.ElapsedTime;
//
//@Config
//@TeleOp(name = "Meet 1 TeleOp", group = "OpModes")
//public class Meet1TeleOp extends OpMode {
//
// //turbo mode
// public static double normal = 0.5;
// public static double turbo = 1;
//
// //keep track of runtime for advanced macros
// private ElapsedTime runTime = new ElapsedTime();
//
// //create and set start position of intake
// private Intake.Position Currentpos = Intake.Position.UP;
// //inform if slides are in lowest position or not
// private Down.DownArm DownQuestion = Down.DownArm.YES;
// //create and set start position of slides
// private SlidePos.SPosition CurrentSpos = SlidePos.SPosition.DOWN;
// //create and set start position of arm
// private Arm.Position CurrentApos = Arm.Position.SDOWN;
// //create and set start position of door, default close
// private DoorPos.DoorPosition CurrentDpos = DoorPos.DoorPosition.CLOSE;
// //create and set start position of hopper
// private HopperPos.hopperPos hopperpos = HopperPos.hopperPos.DOWN;
// //create manual slide height varable and set it to hold position as start
// private Hieght.height CurrentHeight = Hieght.height.HOLD;
// //create robot instance
// private Robot robot;
// //create servo for plane
// private Servo planeServo;
// //create controller 1 (driver)
// private Controller controller1;
// //create controller 2 (macro user and one with hard life)
// private Controller controller2;
// //set starting slide height to 0
// private double sHeight = 0;
//
// @Override
// public void init() {
//
// //reset runtime when opmode is initialized
// runTime.reset();
//
// //make each servo, motor, and controller and configure them
// planeServo = hardwareMap.get(Servo.class, "Plane Servo");
// this.robot = new Robot(hardwareMap);
// controller1 = new Controller(gamepad1);
// controller2 = new Controller(gamepad2);
// //feedback to driver hub to know if init was successful
// telemetry.addLine("Started");
// //update to make sure we receive last line of code
// telemetry.update();
// }
//
// @Override
// public void loop() {
//
// // Calculate the runtime in seconds
// double currentTime = runTime.seconds();
//
// //set start of macro runtime
// double macroStartTime = 0;
//
// //create and set X, Y, and Z for drive inputs
// double x = -gamepad1.left_stick_y;
// double y = -gamepad1.left_stick_x;
// double z = -gamepad1.right_stick_x;
// robot.drive.setWeightedDrivePower(new Pose2d(x, y, z));
//
// //turbo activation
// if (controller1.getA().isJustPressed()){
// x *= turbo;
// y *= turbo;
// z *= turbo;
// } else if (controller1.getA().isJustReleased()){
// x *= normal;
// y *= normal;
// z *= normal;
// }
//
// //set intake to be pressure reactant to right trigger
// robot.intake.setDcMotor(gamepad2.right_trigger);
// //manual slide height control
// double sHeight = gamepad2.right_stick_y;
// //activate intake or not
// double intakeON = gamepad2.right_trigger;
// //graph input of Y joystick to make macros for slides
// double sY = -gamepad2.left_stick_y;
// //graph of X
// double sX = gamepad2.left_stick_x;
// double manualSY = gamepad2.right_stick_y;
//
// //reset value to set slides to starting value
// double reset = gamepad2.left_trigger;
//
// //variable to determine shoot drone or not
// double shoot = gamepad1.right_trigger;
//
// //update variables to constantly feed position each component should be in
// //intake
// robot.intake.setpos(Currentpos);
// //slide macro
// robot.slides.setSPos(CurrentSpos);
// //slide manual
//// robot.slides.setHeight(CurrentHeight);
// //arm position
// robot.arm.setArmPos(CurrentApos);
// //door open or not
// robot.arm.openDoor(CurrentDpos);
// //update
// controller2.update();
//
// //manual slide input reader
//// if (sHeight >= 0.2) {
//// CurrentHeight = Hieght.height.ASCEND;
//// } else if (sHeight <= -0.2) {
//// CurrentHeight = Hieght.height.DESCEND;
//// } else {
//// CurrentHeight = Hieght.height.HOLD;
//// }
//
// //read values to determine if plane should shoot or not
// if (shoot >= 0.9) {
// planeServo.setPosition(0.2);
// } else {
// planeServo.setPosition(0);
// }
//
// //make door rise as intake goes on
// if (intakeON >= 0.01) {
// CurrentDpos = DoorPos.DoorPosition.OPEN;
// } else {
// CurrentDpos = DoorPos.DoorPosition.CLOSE;
// }
//
// //make intake go to stack 1 when pressed down
// if (intakeON >= 0.25) {
// Currentpos = Intake.Position.STACK1;
// } else {
// Currentpos = Intake.Position.UP;
// }
//
// //reset slide position
// if (reset >= 0.2) {
// CurrentSpos = CurrentSpos.DOWN;
// }
//
//// //control position of hopper
//// if (controller2.getLeftBumper().isJustPressed()) {
//// hopperpos = HopperPos.hopperPos.UP;
//// } else if (controller2.getLeftBumper().isJustReleased()) {
//// hopperpos = HopperPos.hopperPos.DOWN;
//// }
//
// //shift intake up one pixel
// if (controller2.getDLeft().isJustPressed()) {
// //prevent from going higher than highest legal value
// if (Currentpos != Intake.Position.UP) {
// Currentpos = Currentpos.nextPosition();
// }
// }
//
// //shift intake down one pixel
// if (controller2.getDRight().isJustPressed()) {
// //prevent from going lower than lowest value
// if (Currentpos != Intake.Position.STACK1) {
// Currentpos = Currentpos.previousPosition();
// }
// }
//
// //set intake to max position
// if (controller2.getDUp().isJustPressed()) {
// Currentpos = Currentpos.UP;
// }
//
// //set intake to lowest position
// if (controller2.getDDown().isJustPressed()) {
// Currentpos = Currentpos.STACK1;
// }
//
// //arm safety pause going up
// if (controller2.getA().isJustPressed()){
// CurrentApos = CurrentApos.SAFTEYUP;
// DownQuestion = DownQuestion.NO;
// }
//
// //arm safety pause going down
// if (controller2.getX().isJustPressed()) {
// CurrentApos = CurrentApos.SAFTEYDOWN;
// DownQuestion = DownQuestion.NO;
// }
//
// //arm all the way up
// if (controller2.getB().isJustPressed()) {
// CurrentApos = CurrentApos.SCORE;
// DownQuestion = DownQuestion.NO;
// }
//
// //arm all the way down
// if (controller2.getY().isJustPressed()) {
// CurrentApos = CurrentApos.SDOWN;
// DownQuestion = DownQuestion.YES;
// }
//
// //set slides to max when left joystick is up
// if (sY >= 0.5) {
// CurrentSpos = CurrentSpos.MAX;
// }
//
// //set slides to tape 1 level when left joystick is down
// if (sY <= -0.5) {
// CurrentSpos = CurrentSpos.TAPE1;
// }
//
// //set slides to tape 3 when left joystick is right
// if (sX >= 0.2) {
// CurrentSpos = CurrentSpos.TAPE3;
// }
//
// //set slides to tape 2 when left joystick is left
// if (sX <= -0.2) {
// CurrentSpos = CurrentSpos.TAPE2;
// }
//
// if (manualSY >=0.1) {
// CurrentSpos = CurrentSpos.MANUAL;
// }
//
//// //set slides all the way down when left bumper gets pressed
//// if (controller2.getLeftBumper().isJustPressed()) {
//// CurrentSpos = CurrentSpos.DOWN;
//// }
////
//// //set intake all teh way up when right bumper is pressed
//// if (controller2.getRightBumper().isJustPressed()) {
//// Currentpos = Currentpos.UP;
//// }
//
// // update the runtime on the telemetry
// telemetry.addData("Runtime", currentTime);
// telemetry.update();
// }
//}

113
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/NewTeleop.java Normal file
View File

@ -0,0 +1,113 @@
package org.firstinspires.ftc.teamcode.opmode.teleop;
import static org.firstinspires.ftc.teamcode.hardware.DoorPos.DoorPosition.CLOSE;
import static org.firstinspires.ftc.teamcode.hardware.DoorPos.DoorPosition.OPEN;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import org.firstinspires.ftc.teamcode.controller.Controller;
import org.firstinspires.ftc.teamcode.hardware.Intake;
import org.firstinspires.ftc.teamcode.hardware.Robot;
import org.firstinspires.ftc.teamcode.hardware.robby.Slides;
@Config
@TeleOp(name = "Main TeleOp", group = "OpModes")
public class NewTeleop extends OpMode {
public static double normal = 0.5;
public static double turbo = 1;
private Robot robot;
private Controller controller1;
private Controller controller2;
@Override
public void init() {
controller1 = new Controller(gamepad1);
controller2 = new Controller(gamepad2);
this.robot = new Robot(hardwareMap);
robot.intake.setpos(Intake.Position.STACK1);
telemetry.addLine("Started");
telemetry.update();
}
@Override
public void loop() {
// Driver 1
double x = -gamepad1.left_stick_y;
double y = -gamepad1.left_stick_x;
double z = -gamepad1.right_stick_x;
if (controller1.getRightTrigger().getValue() > 0.1) {
x *= turbo;
y *= turbo;
z *= turbo;
} else {
x *= normal;
y *= normal;
z *= normal;
}
robot.drive.setWeightedDrivePower(new Pose2d(x, y, z));
// Driver 2
robot.intake.setDcMotor(gamepad2.right_trigger);
if (controller2.getRightBumper().isJustPressed()) {
robot.intake.incrementPos();
}
if (controller2.getLeftBumper().isJustPressed()) {
robot.intake.decrementPos();
}
// Drone launcher
if (controller1.getA().isPressed()) {
this.robot.droneLauncher.launch();
} else {
this.robot.droneLauncher.reset();
}
// macros
switch (robot.runningMacro) {
case (0): // manual mode
robot.slides.increaseTarget(controller2.getLeftStick().getY());
if (controller2.getX().isJustPressed()) {
robot.runningMacro = 1;
} else if (controller2.getY().isJustPressed()) {
robot.runningMacro = 2;
} else if (controller2.getB().isJustPressed()) {
robot.runningMacro = 3;
} else if (controller2.getA().isJustPressed()) {
robot.runningMacro = 4;
}
if (robot.intake.getPower() >= 0.01) {
robot.arm.setDoor(OPEN);
} else {
robot.arm.setDoor(CLOSE);
}
break;
case (1):
robot.extendMacro(Slides.Position.TIER1, getRuntime());
break;
case (2):
robot.extendMacro(Slides.Position.TIER2, getRuntime());
break;
case (3):
robot.extendMacro(Slides.Position.TIER3, getRuntime());
break;
case (4):
robot.resetMacro(Slides.Position.DOWN, getRuntime());
break;
}
// update and telemetry
robot.update(getRuntime());
controller1.update();
controller2.update();
telemetry.addLine(robot.getTelemetry());
telemetry.update();
}
}

9
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/PoseStorage.java Normal file
View File

@ -0,0 +1,9 @@
package org.firstinspires.ftc.teamcode.opmode.teleop;
import com.acmerobotics.roadrunner.geometry.Pose2d;
public class PoseStorage {
public static Pose2d currentPose = new Pose2d();
public static boolean AutoJustEnded = false;
}

11
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/RedTeam.java Normal file
View File

@ -0,0 +1,11 @@
//package org.firstinspires.ftc.teamcode.opmode.teleop;
//
//import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
//
//@TeleOp(name = "Opmode For Team Red", group = "Competition")
//public class RedTeam extends AbstractTeleOp {
//
// public RedTeam() {
// team = Team.RED;
// }
//}

6
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/Team.java Normal file
View File

@ -0,0 +1,6 @@
package org.firstinspires.ftc.teamcode.opmode.teleop;
public enum Team {
RED, BLUE
}

89
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/DriveConstants.java Normal file
View File

@ -0,0 +1,89 @@
package org.firstinspires.ftc.teamcode.roadrunner.drive;
import com.acmerobotics.dashboard.config.Config;
import com.qualcomm.robotcore.hardware.PIDFCoefficients;
/*
* Constants shared between multiple drive types.
*
* TODO: Tune or adjust the following constants to fit your robot. Note that the non-final
* fields may also be edited through the dashboard (connect to the robot's WiFi network and
* navigate to https://192.168.49.1:8080/dash). Make sure to save the values here after you
* adjust them in the dashboard; **config variable changes don't persist between app restarts**.
*
* These are not the only parameters; some are located in the localizer classes, drive base classes,
* and op modes themselves.
*/
@Config
public class DriveConstants {
/*
* These are motor constants that should be listed online for your motors.
*/
public static final double TICKS_PER_REV = 384.5;
public static final double MAX_RPM = 435;
/*
* Set RUN_USING_ENCODER to true to enable built-in hub velocity control using drive encoders.
* Set this flag to false if drive encoders are not present and an alternative localization
* method is in use (e.g., tracking wheels).
*
* If using the built-in motor velocity PID, update MOTOR_VELO_PID with the tuned coefficients
* from DriveVelocityPIDTuner.
*/
public static final boolean RUN_USING_ENCODER = false;
public static PIDFCoefficients MOTOR_VELO_PID = new PIDFCoefficients(0, 0, 0,
getMotorVelocityF(MAX_RPM / 60 * TICKS_PER_REV));
/*
* These are physical constants that can be determined from your robot (including the track
* width; it will be tune empirically later although a rough estimate is important). Users are
* free to chose whichever linear distance unit they would like so long as it is consistently
* used. The default values were selected with inches in mind. Road runner uses radians for
* angular distances although most angular parameters are wrapped in Math.toRadians() for
* convenience. Make sure to exclude any gear ratio included in MOTOR_CONFIG from GEAR_RATIO.
*/
public static double WHEEL_RADIUS = 1.889765; // in
public static double GEAR_RATIO = 1; // output (wheel) speed / input (motor) speed
public static double TRACK_WIDTH = 13.5; // in
/*
* These are the feedforward parameters used to model the drive motor behavior. If you are using
* the built-in velocity PID, *these values are fine as is*. However, if you do not have drive
* motor encoders or have elected not to use them for velocity control, these values should be
* empirically tuned.
*/
// public static double kV = 1.0 / rpmToVelocity(MAX_RPM);
public static double kV = 0.012;
public static double kA = 0.004;
public static double kStatic = 0;
/*
* These values are used to generate the trajectories for you robot. To ensure proper operation,
* the constraints should never exceed ~80% of the robot's actual capabilities. While Road
* Runner is designed to enable faster autonomous motion, it is a good idea for testing to start
* small and gradually increase them later after everything is working. All distance units are
* inches.
*/
// old is 35 35 120 120
// new is 70 45 90 90
public static double MAX_VEL = 60;
public static double MAX_ACCEL = 60;
public static double MAX_ANG_VEL = Math.toRadians(120);
public static double MAX_ANG_ACCEL = Math.toRadians(120);
public static double encoderTicksToInches(double ticks) {
return WHEEL_RADIUS * 2 * Math.PI * GEAR_RATIO * ticks / TICKS_PER_REV;
}
public static double rpmToVelocity(double rpm) {
return rpm * GEAR_RATIO * 2 * Math.PI * WHEEL_RADIUS / 60.0;
}
public static double getMotorVelocityF(double ticksPerSecond) {
// see https://docs.google.com/document/d/1tyWrXDfMidwYyP_5H4mZyVgaEswhOC35gvdmP-V-5hA/edit#heading=h.61g9ixenznbx
return 32767 / ticksPerSecond;
}
}

5
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/MouseOdometry.java Normal file
View File

@ -0,0 +1,5 @@
package org.firstinspires.ftc.teamcode.roadrunner.drive;//package org.firstinspires.ftc.teamcode.roadrunner.drive;
//import java.awt.
//
//public class MouseOdometry {
//}

318
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/SampleMecanumDrive.java Normal file
View File

@ -0,0 +1,318 @@
package org.firstinspires.ftc.teamcode.roadrunner.drive;
import androidx.annotation.NonNull;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.roadrunner.control.PIDCoefficients;
import com.acmerobotics.roadrunner.drive.DriveSignal;
import com.acmerobotics.roadrunner.drive.MecanumDrive;
import com.acmerobotics.roadrunner.followers.HolonomicPIDVAFollower;
import com.acmerobotics.roadrunner.followers.TrajectoryFollower;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.trajectory.Trajectory;
import com.acmerobotics.roadrunner.trajectory.TrajectoryBuilder;
import com.acmerobotics.roadrunner.trajectory.constraints.AngularVelocityConstraint;
import com.acmerobotics.roadrunner.trajectory.constraints.MecanumVelocityConstraint;
import com.acmerobotics.roadrunner.trajectory.constraints.MinVelocityConstraint;
import com.acmerobotics.roadrunner.trajectory.constraints.ProfileAccelerationConstraint;
import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryAccelerationConstraint;
import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryVelocityConstraint;
import com.qualcomm.hardware.bosch.BNO055IMU;
import com.qualcomm.hardware.lynx.LynxModule;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.hardware.Gamepad;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.PIDFCoefficients;
import com.qualcomm.robotcore.hardware.VoltageSensor;
import com.qualcomm.robotcore.hardware.configuration.typecontainers.MotorConfigurationType;
import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.TrajectorySequence;
import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.TrajectorySequenceRunner;
import org.firstinspires.ftc.teamcode.roadrunner.util.LynxModuleUtil;
import java.util.ArrayList;
import java.util.Arrays;
import java.util.List;
@Config
public class SampleMecanumDrive extends MecanumDrive {
public static PIDCoefficients TRANSLATIONAL_PID = new PIDCoefficients(8, 0, 0);
public static PIDCoefficients HEADING_PID = new PIDCoefficients(8, 0, 0);
public static double LATERAL_MULTIPLIER = 1;
public static double VX_WEIGHT = 1;
public static double VY_WEIGHT = 1;
public static double OMEGA_WEIGHT = 1;
private TrajectorySequenceRunner trajectorySequenceRunner;
private static final TrajectoryVelocityConstraint VEL_CONSTRAINT = getVelocityConstraint(DriveConstants.MAX_VEL, DriveConstants.MAX_ANG_VEL, DriveConstants.TRACK_WIDTH);
private static final TrajectoryAccelerationConstraint ACCEL_CONSTRAINT = getAccelerationConstraint(DriveConstants.MAX_ACCEL);
private TrajectoryFollower follower;
private DcMotorEx leftFront, leftRear, rightRear, rightFront;
private List<DcMotorEx> motors;
private BNO055IMU imu;
private VoltageSensor batteryVoltageSensor;
public SampleMecanumDrive(HardwareMap hardwareMap) {
super(DriveConstants.kV, DriveConstants.kA, DriveConstants.kStatic, DriveConstants.TRACK_WIDTH, DriveConstants.TRACK_WIDTH, LATERAL_MULTIPLIER);
follower = new HolonomicPIDVAFollower(TRANSLATIONAL_PID, TRANSLATIONAL_PID, HEADING_PID,
new Pose2d(0.5, 0.5, Math.toRadians(5)), 0.5);
LynxModuleUtil.ensureMinimumFirmwareVersion(hardwareMap);
batteryVoltageSensor = hardwareMap.voltageSensor.iterator().next();
for (LynxModule module : hardwareMap.getAll(LynxModule.class)) {
module.setBulkCachingMode(LynxModule.BulkCachingMode.AUTO);
}
// TODO: adjust the names of the following hardware devices to match your configuration
imu = hardwareMap.get(BNO055IMU.class, "imu");
BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
parameters.angleUnit = BNO055IMU.AngleUnit.RADIANS;
imu.initialize(parameters);
// TODO: If the hub containing the IMU you are using is mounted so that the "REV" logo does
// not face up, remap the IMU axes so that the z-axis points upward (normal to the floor.)
//
// | +Z axis
// |
// |
// |
// _______|_____________ +Y axis
// / |_____________/|__________
// / REV / EXPANSION //
// / / HUB //
// /_______/_____________//
// |_______/_____________|/
// /
// / +X axis
//
// This diagram is derived from the axes in section 3.4 https://www.bosch-sensortec.com/media/boschsensortec/downloads/datasheets/bst-bno055-ds000.pdf
// and the placement of the dot/orientation from https://docs.revrobotics.com/rev-control-system/control-system-overview/dimensions#imu-location
//
// For example, if +Y in this diagram faces downwards, you would use AxisDirection.NEG_Y.
// BNO055IMUUtil.remapZAxis(imu, AxisDirection.NEG_Y);
leftFront = hardwareMap.get(DcMotorEx.class, "frontLeft");
leftRear = hardwareMap.get(DcMotorEx.class, "backLeft");
rightRear = hardwareMap.get(DcMotorEx.class, "backRight");
rightFront = hardwareMap.get(DcMotorEx.class, "frontRight");
motors = Arrays.asList(leftFront, leftRear, rightRear, rightFront);
for (DcMotorEx motor : motors) {
MotorConfigurationType motorConfigurationType = motor.getMotorType().clone();
motorConfigurationType.setAchieveableMaxRPMFraction(1.0);
motor.setMotorType(motorConfigurationType);
}
if (DriveConstants.RUN_USING_ENCODER) {
setMode(DcMotor.RunMode.RUN_USING_ENCODER);
}
setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
if (DriveConstants.RUN_USING_ENCODER && DriveConstants.MOTOR_VELO_PID != null) {
setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, DriveConstants.MOTOR_VELO_PID);
}
// TODO: reverse any motors using DcMotor.setDirection()
rightFront.setDirection(DcMotorSimple.Direction.REVERSE);
rightRear.setDirection(DcMotorSimple.Direction.REVERSE);
leftRear.setDirection(DcMotorSimple.Direction.REVERSE);
leftFront.setDirection(DcMotorSimple.Direction.REVERSE);
// TODO: if desired, use setLocalizer() to change the localization method
// for instance, setLocalizer(new ThreeTrackingWheelLocalizer(...));
setLocalizer(new TwoWheelTrackingLocalizer(hardwareMap, this));
// setLocalizer(new StandardTrackingWheelLocalizer(hardwareMap));
trajectorySequenceRunner = new TrajectorySequenceRunner(follower, HEADING_PID);
}
public TrajectoryBuilder trajectoryBuilder(Pose2d startPose) {
return new TrajectoryBuilder(startPose, VEL_CONSTRAINT, ACCEL_CONSTRAINT);
}
public TrajectoryBuilder trajectoryBuilder(Pose2d startPose, boolean reversed) {
return new TrajectoryBuilder(startPose, reversed, VEL_CONSTRAINT, ACCEL_CONSTRAINT);
}
public TrajectoryBuilder trajectoryBuilder(Pose2d startPose, double startHeading) {
return new TrajectoryBuilder(startPose, startHeading, VEL_CONSTRAINT, ACCEL_CONSTRAINT);
}
public TrajectorySequenceBuilder trajectorySequenceBuilder(Pose2d startPose) {
return new TrajectorySequenceBuilder(
startPose,
VEL_CONSTRAINT, ACCEL_CONSTRAINT,
DriveConstants.MAX_ANG_VEL, DriveConstants.MAX_ANG_ACCEL
);
}
public void turnAsync(double angle) {
trajectorySequenceRunner.followTrajectorySequenceAsync(
trajectorySequenceBuilder(getPoseEstimate())
.turn(angle)
.build()
);
}
public void turn(double angle) {
turnAsync(angle);
waitForIdle();
}
public void followTrajectoryAsync(Trajectory trajectory) {
trajectorySequenceRunner.followTrajectorySequenceAsync(
trajectorySequenceBuilder(trajectory.start())
.addTrajectory(trajectory)
.build()
);
}
public void followTrajectory(Trajectory trajectory) {
followTrajectoryAsync(trajectory);
waitForIdle();
}
public void followTrajectorySequenceAsync(TrajectorySequence trajectorySequence) {
trajectorySequenceRunner.followTrajectorySequenceAsync(trajectorySequence);
}
public void followTrajectorySequence(TrajectorySequence trajectorySequence) {
followTrajectorySequenceAsync(trajectorySequence);
waitForIdle();
}
public Pose2d getLastError() {
return trajectorySequenceRunner.getLastPoseError();
}
public void update() {
updatePoseEstimate();
DriveSignal signal = trajectorySequenceRunner.update(getPoseEstimate(), getPoseVelocity());
if (signal != null) setDriveSignal(signal);
}
public void waitForIdle() {
while (!Thread.currentThread().isInterrupted() && isBusy())
update();
}
public boolean isBusy() {
return trajectorySequenceRunner.isBusy();
}
public void setMode(DcMotor.RunMode runMode) {
for (DcMotorEx motor : motors) {
motor.setMode(runMode);
}
}
public void setZeroPowerBehavior(DcMotor.ZeroPowerBehavior zeroPowerBehavior) {
for (DcMotorEx motor : motors) {
motor.setZeroPowerBehavior(zeroPowerBehavior);
}
}
public void setPIDFCoefficients(DcMotor.RunMode runMode, PIDFCoefficients coefficients) {
PIDFCoefficients compensatedCoefficients = new PIDFCoefficients(
coefficients.p, coefficients.i, coefficients.d,
coefficients.f * 12 / batteryVoltageSensor.getVoltage()
);
for (DcMotorEx motor : motors) {
motor.setPIDFCoefficients(runMode, compensatedCoefficients);
}
}
public void setWeightedDrivePower(Pose2d drivePower) {
Pose2d vel = drivePower;
if (Math.abs(drivePower.getX()) + Math.abs(drivePower.getY())
+ Math.abs(drivePower.getHeading()) > 1) {
// re-normalize the powers according to the weights
double denom = VX_WEIGHT * Math.abs(drivePower.getX())
+ VY_WEIGHT * Math.abs(drivePower.getY())
+ OMEGA_WEIGHT * Math.abs(drivePower.getHeading());
vel = new Pose2d(
VX_WEIGHT * drivePower.getX(),
VY_WEIGHT * drivePower.getY(),
OMEGA_WEIGHT * drivePower.getHeading()
).div(denom);
}
setDrivePower(vel);
}
@NonNull
@Override
public List<Double> getWheelPositions() {
List<Double> wheelPositions = new ArrayList<>();
for (DcMotorEx motor : motors) {
wheelPositions.add(DriveConstants.encoderTicksToInches(motor.getCurrentPosition()));
}
return wheelPositions;
}
@Override
public List<Double> getWheelVelocities() {
List<Double> wheelVelocities = new ArrayList<>();
for (DcMotorEx motor : motors) {
wheelVelocities.add(DriveConstants.encoderTicksToInches(motor.getVelocity()));
}
return wheelVelocities;
}
@Override
public void setMotorPowers(double v, double v1, double v2, double v3) {
leftFront.setPower(v);
leftRear.setPower(v1);
rightRear.setPower(-v2);
rightFront.setPower(-v3);
}
@Override
public double getRawExternalHeading() {
return imu.getAngularOrientation().firstAngle;
}
@Override
public Double getExternalHeadingVelocity() {
// To work around an SDK bug, use -zRotationRate in place of xRotationRate
// and -xRotationRate in place of zRotationRate (yRotationRate behaves as
// expected). This bug does NOT affect orientation.
//
// See https://github.com/FIRST-Tech-Challenge/FtcRobotController/issues/251 for details.
return (double) -imu.getAngularVelocity().zRotationRate;
}
public static TrajectoryVelocityConstraint getVelocityConstraint(double maxVel, double maxAngularVel, double trackWidth) {
return new MinVelocityConstraint(Arrays.asList(
new AngularVelocityConstraint(maxAngularVel),
new MecanumVelocityConstraint(maxVel, trackWidth)
));
}
public static TrajectoryAccelerationConstraint getAccelerationConstraint(double maxAccel) {
return new ProfileAccelerationConstraint(maxAccel);
}
public String getTelemetry() {
return "Drive: "+getPoseEstimate();
}
}

82
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/StandardTrackingWheelLocalizer.java Normal file
View File

@ -0,0 +1,82 @@
package org.firstinspires.ftc.teamcode.roadrunner.drive;
import androidx.annotation.NonNull;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.localization.ThreeTrackingWheelLocalizer;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.HardwareMap;
import org.firstinspires.ftc.teamcode.roadrunner.util.Encoder;
import java.util.Arrays;
import java.util.List;
/*
* Sample tracking wheel localizer implementation assuming the standard configuration:
*
* /--------------\
* | ____ |
* | ---- |
* | || || |
* | || || |
* | |
* | |
* \--------------/
*
*/
public class StandardTrackingWheelLocalizer extends ThreeTrackingWheelLocalizer {
public static double TICKS_PER_REV = 8192;
public static double WHEEL_RADIUS = 0.6889764; // in
public static double GEAR_RATIO = 1; // output (wheel) speed / input (encoder) speed
public static double LATERAL_DISTANCE = 11.686023622; // in; distance between the left and right wheels
public static double FORWARD_OFFSET = 4.89812992126; // in; offset of the lateral wheel
private Encoder leftEncoder, rightEncoder, frontEncoder;
public StandardTrackingWheelLocalizer(HardwareMap hardwareMap) {
super(Arrays.asList(
new Pose2d(0, LATERAL_DISTANCE / 2, 0), // left
new Pose2d(0, -LATERAL_DISTANCE / 2, 0), // right
new Pose2d(FORWARD_OFFSET, 0, Math.toRadians(90)) // front
));
leftEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "backRight"));
rightEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "frontLeft"));
frontEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "frontRight"));
// TODO: reverse any encoders using Encoder.setDirection(Encoder.Direction.REVERSE)
leftEncoder.setDirection(Encoder.Direction.REVERSE);
// rightEncoder.setDirection(Encoder.Direction.REVERSE);
// frontEncoder.setDirection(Encoder.Direction.REVERSE);
}
public static double encoderTicksToInches(double ticks) {
return WHEEL_RADIUS * 2 * Math.PI * GEAR_RATIO * ticks / TICKS_PER_REV;
}
@NonNull
@Override
public List<Double> getWheelPositions() {
return Arrays.asList(
encoderTicksToInches(leftEncoder.getCurrentPosition()),
encoderTicksToInches(rightEncoder.getCurrentPosition()),
encoderTicksToInches(frontEncoder.getCurrentPosition())
);
}
@NonNull
@Override
public List<Double> getWheelVelocities() {
// TODO: If your encoder velocity can exceed 32767 counts / second (such as the REV Through Bore and other
// competing magnetic encoders), change Encoder.getRawVelocity() to Encoder.getCorrectedVelocity() to enable a
// compensation method
return Arrays.asList(
encoderTicksToInches(leftEncoder.getCorrectedVelocity()),
encoderTicksToInches(rightEncoder.getCorrectedVelocity()),
encoderTicksToInches(frontEncoder.getCorrectedVelocity())
);
}
}

108
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/TwoWheelTrackingLocalizer.java Normal file
View File

@ -0,0 +1,108 @@
package org.firstinspires.ftc.teamcode.roadrunner.drive;
import androidx.annotation.NonNull;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.localization.TwoTrackingWheelLocalizer;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.HardwareMap;
import org.firstinspires.ftc.teamcode.roadrunner.util.Encoder;
import java.util.Arrays;
import java.util.List;
/*
* Sample tracking wheel localizer implementation assuming the standard configuration:
*
* ^
* |
* | ( x direction)
* |
* v
* <----( y direction )---->
* (forward)
* /--------------\
* | ____ |
* | ---- | <- Perpendicular Wheel
* | || |
* | || | <- Parallel Wheel
* | |
* | |
* \--------------/
*
*/
public class TwoWheelTrackingLocalizer extends TwoTrackingWheelLocalizer {
public static double TICKS_PER_REV = 2000;
public static double WHEEL_RADIUS = 0.944882; // in
public static double GEAR_RATIO = 1; // output (wheel) speed / input (encoder) speed
public static double PARALLEL_X = -1.5; // X is the up and down direction
public static double PARALLEL_Y = 7.25; // Y is the strafe direction
public static double PERPENDICULAR_X = -6.1;
public static double PERPENDICULAR_Y = 0;
public static double MULTIPLIER_X = 1.0;
public static double MULTIPLIER_Y = 1.0;
// Parallel/Perpendicular to the forward axis
// Parallel wheel is parallel to the forward axis
// Perpendicular is perpendicular to the forward axis
private Encoder parallelEncoder, perpendicularEncoder;
private SampleMecanumDrive drive;
public TwoWheelTrackingLocalizer(HardwareMap hardwareMap, SampleMecanumDrive drive) {
super(Arrays.asList(
new Pose2d(PARALLEL_X, PARALLEL_Y, 0),
new Pose2d(PERPENDICULAR_X, PERPENDICULAR_Y, Math.toRadians(90))
));
this.drive = drive;
parallelEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "backLeft"));
// parallelEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "backRight"));
perpendicularEncoder = new Encoder(hardwareMap.get(DcMotorEx.class, "Intake Motor"));
// TODO: reverse any encoders using Encoder.setDirection(Encoder.Direction.REVERSE)
perpendicularEncoder.setDirection(Encoder.Direction.REVERSE);
// parallelEncoder.setDirection(Encoder.Direction.REVERSE);
}
public static double encoderTicksToInches(double ticks) {
return WHEEL_RADIUS * 2 * Math.PI * GEAR_RATIO * ticks / TICKS_PER_REV;
}
@Override
public double getHeading() {
return drive.getRawExternalHeading();
}
@Override
public Double getHeadingVelocity() {
return drive.getExternalHeadingVelocity();
}
@NonNull
@Override
public List<Double> getWheelPositions() {
return Arrays.asList(
encoderTicksToInches(parallelEncoder.getCurrentPosition()),
encoderTicksToInches(perpendicularEncoder.getCurrentPosition())
);
}
@NonNull
@Override
public List<Double> getWheelVelocities() {
// TODO: If your encoder velocity can exceed 32767 counts / second (such as the REV Through Bore and other
// competing magnetic encoders), change Encoder.getRawVelocity() to Encoder.getCorrectedVelocity() to enable a
// compensation method
return Arrays.asList(
encoderTicksToInches(parallelEncoder.getCorrectedVelocity()),
encoderTicksToInches(perpendicularEncoder.getCorrectedVelocity())
);
}
}

203
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/AutomaticFeedforwardTuner.java Normal file
View File

@ -0,0 +1,203 @@
package org.firstinspires.ftc.teamcode.roadrunner.drive.opmode;
import static org.firstinspires.ftc.teamcode.roadrunner.drive.DriveConstants.MAX_RPM;
import static org.firstinspires.ftc.teamcode.roadrunner.drive.DriveConstants.RUN_USING_ENCODER;
import static org.firstinspires.ftc.teamcode.roadrunner.drive.DriveConstants.rpmToVelocity;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.util.NanoClock;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.util.RobotLog;
import org.firstinspires.ftc.robotcore.internal.system.Misc;
import org.firstinspires.ftc.teamcode.roadrunner.drive.SampleMecanumDrive;
import org.firstinspires.ftc.teamcode.roadrunner.util.LoggingUtil;
import java.util.ArrayList;
import java.util.List;
/*
* Op mode for computing kV, kStatic, and kA from various drive routines. For the curious, here's an
* outline of the procedure:
* 1. Slowly ramp the motor power and record encoder values along the way.
* 2. Run a linear regression on the encoder velocity vs. motor power plot to obtain a slope (kV)
* and an optional intercept (kStatic).
* 3. Accelerate the robot (apply constant power) and record the encoder counts.
* 4. Adjust the encoder data based on the velocity tuning data and find kA with another linear
* regression.
*/
//@Config
@Disabled
@Autonomous(group = "drive")
public class AutomaticFeedforwardTuner extends LinearOpMode {
public static double MAX_POWER = 0.7;
public static double DISTANCE = 100; // in
@Override
public void runOpMode() throws InterruptedException {
if (RUN_USING_ENCODER) {
RobotLog.setGlobalErrorMsg("Feedforward constants usually don't need to be tuned " +
"when using the built-in drive motor velocity PID.");
}
telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
NanoClock clock = NanoClock.system();
telemetry.addLine("Press play to begin the feedforward tuning routine");
telemetry.update();
waitForStart();
if (isStopRequested()) return;
telemetry.clearAll();
telemetry.addLine("Would you like to fit kStatic?");
telemetry.addLine("Press (Y/Δ) for yes, (B/O) for no");
telemetry.update();
boolean fitIntercept = false;
while (!isStopRequested()) {
if (gamepad1.y) {
fitIntercept = true;
while (!isStopRequested() && gamepad1.y) {
idle();
}
break;
} else if (gamepad1.b) {
while (!isStopRequested() && gamepad1.b) {
idle();
}
break;
}
idle();
}
telemetry.clearAll();
telemetry.addLine(Misc.formatInvariant(
"Place your robot on the field with at least %.2f in of room in front", DISTANCE));
telemetry.addLine("Press (Y/Δ) to begin");
telemetry.update();
while (!isStopRequested() && !gamepad1.y) {
idle();
}
while (!isStopRequested() && gamepad1.y) {
idle();
}
telemetry.clearAll();
telemetry.addLine("Running...");
telemetry.update();
double maxVel = rpmToVelocity(MAX_RPM);
double finalVel = MAX_POWER * maxVel;
double accel = (finalVel * finalVel) / (2.0 * DISTANCE);
double rampTime = Math.sqrt(2.0 * DISTANCE / accel);
List<Double> timeSamples = new ArrayList<>();
List<Double> positionSamples = new ArrayList<>();
List<Double> powerSamples = new ArrayList<>();
drive.setPoseEstimate(new Pose2d());
double startTime = clock.seconds();
while (!isStopRequested()) {
double elapsedTime = clock.seconds() - startTime;
if (elapsedTime > rampTime) {
break;
}
double vel = accel * elapsedTime;
double power = vel / maxVel;
timeSamples.add(elapsedTime);
positionSamples.add(drive.getPoseEstimate().getX());
powerSamples.add(power);
drive.setDrivePower(new Pose2d(power, 0.0, 0.0));
drive.updatePoseEstimate();
}
drive.setDrivePower(new Pose2d(0.0, 0.0, 0.0));
telemetry.clearAll();
telemetry.addLine("Quasi-static ramp up test complete");
telemetry.addLine("Would you like to fit kA?");
telemetry.addLine("Press (Y/Δ) for yes, (B/O) for no");
telemetry.update();
boolean fitAccelFF = false;
while (!isStopRequested()) {
if (gamepad1.y) {
fitAccelFF = true;
while (!isStopRequested() && gamepad1.y) {
idle();
}
break;
} else if (gamepad1.b) {
while (!isStopRequested() && gamepad1.b) {
idle();
}
break;
}
idle();
}
if (fitAccelFF) {
telemetry.clearAll();
telemetry.addLine("Place the robot back in its starting position");
telemetry.addLine("Press (Y/Δ) to continue");
telemetry.update();
while (!isStopRequested() && !gamepad1.y) {
idle();
}
while (!isStopRequested() && gamepad1.y) {
idle();
}
telemetry.clearAll();
telemetry.addLine("Running...");
telemetry.update();
double maxPowerTime = DISTANCE / maxVel;
timeSamples.clear();
positionSamples.clear();
powerSamples.clear();
drive.setPoseEstimate(new Pose2d());
drive.setDrivePower(new Pose2d(MAX_POWER, 0.0, 0.0));
startTime = clock.seconds();
while (!isStopRequested()) {
double elapsedTime = clock.seconds() - startTime;
if (elapsedTime > maxPowerTime) {
break;
}
timeSamples.add(elapsedTime);
positionSamples.add(drive.getPoseEstimate().getX());
powerSamples.add(MAX_POWER);
drive.updatePoseEstimate();
}
drive.setDrivePower(new Pose2d(0.0, 0.0, 0.0));
telemetry.clearAll();
telemetry.addLine("Constant power test complete");
telemetry.addLine(Misc.formatInvariant("kA = %.5f (R^2 = %.2f)",
telemetry.update()));
}
while (!isStopRequested()) {
idle();
}
}
}

51
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/BackAndForth.java Normal file
View File

@ -0,0 +1,51 @@
package org.firstinspires.ftc.teamcode.roadrunner.drive.opmode;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.trajectory.Trajectory;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.firstinspires.ftc.teamcode.roadrunner.drive.SampleMecanumDrive;
/*
* Op mode for preliminary tuning of the follower PID coefficients (located in the drive base
* classes). The robot drives back and forth in a straight line indefinitely. Utilization of the
* dashboard is recommended for this tuning routine. To access the dashboard, connect your computer
* to the RC's WiFi network. In your browser, navigate to https://192.168.49.1:8080/dash if you're
* using the RC phone or https://192.168.43.1:8080/dash if you are using the Control Hub. Once
* you've successfully connected, start the program, and your robot will begin moving forward and
* backward. You should observe the target position (green) and your pose estimate (blue) and adjust
* your follower PID coefficients such that you follow the target position as accurately as possible.
* If you are using SampleMecanumDrive, you should be tuning TRANSLATIONAL_PID and HEADING_PID.
* If you are using SampleTankDrive, you should be tuning AXIAL_PID, CROSS_TRACK_PID, and HEADING_PID.
* These coefficients can be tuned live in dashboard.
*
* This opmode is designed as a convenient, coarse tuning for the follower PID coefficients. It
* is recommended that you use the FollowerPIDTuner opmode for further fine tuning.
*/
//@Config
@Autonomous(group = "drive")
public class BackAndForth extends LinearOpMode {
public static double DISTANCE = 50;
@Override
public void runOpMode() throws InterruptedException {
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
Trajectory trajectoryForward = drive.trajectoryBuilder(new Pose2d())
.forward(DISTANCE)
.build();
Trajectory trajectoryBackward = drive.trajectoryBuilder(trajectoryForward.end())
.back(DISTANCE)
.build();
waitForStart();
while (opModeIsActive() && !isStopRequested()) {
drive.followTrajectory(trajectoryForward);
drive.followTrajectory(trajectoryBackward);
}
}
}

171
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/DriveVelocityPIDTuner.java Normal file
View File

@ -0,0 +1,171 @@
package org.firstinspires.ftc.teamcode.roadrunner.drive.opmode;
import static org.firstinspires.ftc.teamcode.roadrunner.drive.DriveConstants.MAX_ACCEL;
import static org.firstinspires.ftc.teamcode.roadrunner.drive.DriveConstants.MAX_VEL;
import static org.firstinspires.ftc.teamcode.roadrunner.drive.DriveConstants.MOTOR_VELO_PID;
import static org.firstinspires.ftc.teamcode.roadrunner.drive.DriveConstants.RUN_USING_ENCODER;
import static org.firstinspires.ftc.teamcode.roadrunner.drive.DriveConstants.kV;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.profile.MotionProfile;
import com.acmerobotics.roadrunner.profile.MotionProfileGenerator;
import com.acmerobotics.roadrunner.profile.MotionState;
import com.acmerobotics.roadrunner.util.NanoClock;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.util.RobotLog;
import org.firstinspires.ftc.teamcode.roadrunner.drive.SampleMecanumDrive;
import java.util.List;
/*
* This routine is designed to tune the PID coefficients used by the REV Expansion Hubs for closed-
* loop velocity control. Although it may seem unnecessary, tuning these coefficients is just as
* important as the positional parameters. Like the other manual tuning routines, this op mode
* relies heavily upon the dashboard. To access the dashboard, connect your computer to the RC's
* WiFi network. In your browser, navigate to https://192.168.49.1:8080/dash if you're using the RC
* phone or https://192.168.43.1:8080/dash if you are using the Control Hub. Once you've successfully
* connected, start the program, and your robot will begin moving forward and backward according to
* a motion profile. Your job is to graph the velocity errors over time and adjust the PID
* coefficients (note: the tuning variable will not appear until the op mode finishes initializing).
* Once you've found a satisfactory set of gains, add them to the DriveConstants.java file under the
* MOTOR_VELO_PID field.
*
* Recommended tuning process:
*
* 1. Increase kP until any phase lag is eliminated. Concurrently increase kD as necessary to
* mitigate oscillations.
* 2. Add kI (or adjust kF) until the steady state/constant velocity plateaus are reached.
* 3. Back off kP and kD a little until the response is less oscillatory (but without lag).
*
* Pressing Y/Δ (Xbox/PS4) will pause the tuning process and enter driver override, allowing the
* user to reset the position of the bot in the event that it drifts off the path.
* Pressing B/O (Xbox/PS4) will cede control back to the tuning process.
*/
//@Config
@Disabled
@Autonomous(group = "drive")
public class DriveVelocityPIDTuner extends LinearOpMode {
public static double DISTANCE = 72; // in
enum Mode {
DRIVER_MODE,
TUNING_MODE
}
private static MotionProfile generateProfile(boolean movingForward) {
MotionState start = new MotionState(movingForward ? 0 : DISTANCE, 0, 0, 0);
MotionState goal = new MotionState(movingForward ? DISTANCE : 0, 0, 0, 0);
return MotionProfileGenerator.generateSimpleMotionProfile(start, goal, MAX_VEL, MAX_ACCEL);
}
@Override
public void runOpMode() {
if (!RUN_USING_ENCODER) {
RobotLog.setGlobalErrorMsg("%s does not need to be run if the built-in motor velocity" +
"PID is not in use", getClass().getSimpleName());
}
telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
Mode mode = Mode.TUNING_MODE;
double lastKp = MOTOR_VELO_PID.p;
double lastKi = MOTOR_VELO_PID.i;
double lastKd = MOTOR_VELO_PID.d;
double lastKf = MOTOR_VELO_PID.f;
drive.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, MOTOR_VELO_PID);
NanoClock clock = NanoClock.system();
telemetry.addLine("Ready!");
telemetry.update();
telemetry.clearAll();
waitForStart();
if (isStopRequested()) return;
boolean movingForwards = true;
MotionProfile activeProfile = generateProfile(true);
double profileStart = clock.seconds();
while (!isStopRequested()) {
telemetry.addData("mode", mode);
switch (mode) {
case TUNING_MODE:
if (gamepad1.y) {
mode = Mode.DRIVER_MODE;
drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
}
// calculate and set the motor power
double profileTime = clock.seconds() - profileStart;
if (profileTime > activeProfile.duration()) {
// generate a new profile
movingForwards = !movingForwards;
activeProfile = generateProfile(movingForwards);
profileStart = clock.seconds();
}
MotionState motionState = activeProfile.get(profileTime);
double targetPower = kV * motionState.getV();
drive.setDrivePower(new Pose2d(targetPower, 0, 0));
List<Double> velocities = drive.getWheelVelocities();
// update telemetry
telemetry.addData("targetVelocity", motionState.getV());
for (int i = 0; i < velocities.size(); i++) {
telemetry.addData("measuredVelocity" + i, velocities.get(i));
telemetry.addData(
"error" + i,
motionState.getV() - velocities.get(i)
);
}
break;
case DRIVER_MODE:
if (gamepad1.b) {
drive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
mode = Mode.TUNING_MODE;
movingForwards = true;
activeProfile = generateProfile(movingForwards);
profileStart = clock.seconds();
}
drive.setWeightedDrivePower(
new Pose2d(
-gamepad1.left_stick_y,
-gamepad1.left_stick_x,
-gamepad1.right_stick_x
)
);
break;
}
if (lastKp != MOTOR_VELO_PID.p || lastKd != MOTOR_VELO_PID.d
|| lastKi != MOTOR_VELO_PID.i || lastKf != MOTOR_VELO_PID.f) {
drive.setPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER, MOTOR_VELO_PID);
lastKp = MOTOR_VELO_PID.p;
lastKi = MOTOR_VELO_PID.i;
lastKd = MOTOR_VELO_PID.d;
lastKf = MOTOR_VELO_PID.f;
}
telemetry.update();
}
}
}

56
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/FollowerPIDTuner.java Normal file
View File

@ -0,0 +1,56 @@
package org.firstinspires.ftc.teamcode.roadrunner.drive.opmode;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.firstinspires.ftc.teamcode.roadrunner.drive.SampleMecanumDrive;
import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.TrajectorySequence;
/*
* Op mode for preliminary tuning of the follower PID coefficients (located in the drive base
* classes). The robot drives in a DISTANCE-by-DISTANCE square indefinitely. Utilization of the
* dashboard is recommended for this tuning routine. To access the dashboard, connect your computer
* to the RC's WiFi network. In your browser, navigate to https://192.168.49.1:8080/dash if you're
* using the RC phone or https://192.168.43.1:8080/dash if you are using the Control Hub. Once
* you've successfully connected, start the program, and your robot will begin driving in a square.
* You should observe the target position (green) and your pose estimate (blue) and adjust your
* follower PID coefficients such that you follow the target position as accurately as possible.
* If you are using SampleMecanumDrive, you should be tuning TRANSLATIONAL_PID and HEADING_PID.
* If you are using SampleTankDrive, you should be tuning AXIAL_PID, CROSS_TRACK_PID, and HEADING_PID.
* These coefficients can be tuned live in dashboard.
*/
//@Config
@Disabled
@Autonomous(group = "drive")
public class FollowerPIDTuner extends LinearOpMode {
public static double DISTANCE = 48; // in
@Override
public void runOpMode() throws InterruptedException {
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
Pose2d startPose = new Pose2d(-DISTANCE / 2, -DISTANCE / 2, 0);
drive.setPoseEstimate(startPose);
waitForStart();
if (isStopRequested()) return;
while (!isStopRequested()) {
TrajectorySequence trajSeq = drive.trajectorySequenceBuilder(startPose)
.forward(DISTANCE)
.turn(Math.toRadians(90))
.forward(DISTANCE)
.turn(Math.toRadians(90))
.forward(DISTANCE)
.turn(Math.toRadians(90))
.forward(DISTANCE)
.turn(Math.toRadians(90))
.build();
drive.followTrajectorySequence(trajSeq);
}
}
}

45
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/LocalizationTest.java Normal file
View File

@ -0,0 +1,45 @@
package org.firstinspires.ftc.teamcode.roadrunner.drive.opmode;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotor;
import org.firstinspires.ftc.teamcode.roadrunner.drive.SampleMecanumDrive;
/**
* This is a simple teleop routine for testing localization. Drive the robot around like a normal
* teleop routine and make sure the robot's estimated pose matches the robot's actual pose (slight
* errors are not out of the ordinary, especially with sudden drive motions). The goal of this
* exercise is to ascertain whether the localizer has been configured properly (note: the pure
* encoder localizer heading may be significantly off if the track width has not been tuned).
*/
@TeleOp(group = "drive")
public class LocalizationTest extends LinearOpMode {
@Override
public void runOpMode() throws InterruptedException {
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
waitForStart();
while (!isStopRequested()) {
drive.setWeightedDrivePower(
new Pose2d(
-gamepad1.left_stick_y,
-gamepad1.left_stick_x,
-gamepad1.right_stick_x
)
);
drive.update();
Pose2d poseEstimate = drive.getPoseEstimate();
telemetry.addData("x", poseEstimate.getX());
telemetry.addData("y", poseEstimate.getY());
telemetry.addData("heading", poseEstimate.getHeading());
telemetry.update();
}
}
}

143
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/ManualFeedforwardTuner.java Normal file
View File

@ -0,0 +1,143 @@
package org.firstinspires.ftc.teamcode.roadrunner.drive.opmode;
import static org.firstinspires.ftc.teamcode.roadrunner.drive.DriveConstants.MAX_ACCEL;
import static org.firstinspires.ftc.teamcode.roadrunner.drive.DriveConstants.MAX_VEL;
import static org.firstinspires.ftc.teamcode.roadrunner.drive.DriveConstants.kA;
import static org.firstinspires.ftc.teamcode.roadrunner.drive.DriveConstants.kStatic;
import static org.firstinspires.ftc.teamcode.roadrunner.drive.DriveConstants.kV;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.kinematics.Kinematics;
import com.acmerobotics.roadrunner.profile.MotionProfile;
import com.acmerobotics.roadrunner.profile.MotionProfileGenerator;
import com.acmerobotics.roadrunner.profile.MotionState;
import com.acmerobotics.roadrunner.util.NanoClock;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.firstinspires.ftc.teamcode.roadrunner.drive.SampleMecanumDrive;
import java.util.Objects;
/*
* This routine is designed to tune the open-loop feedforward coefficients. Although it may seem unnecessary,
* tuning these coefficients is just as important as the positional parameters. Like the other
* manual tuning routines, this op mode relies heavily upon the dashboard. To access the dashboard,
* connect your computer to the RC's WiFi network. In your browser, navigate to
* https://192.168.49.1:8080/dash if you're using the RC phone or https://192.168.43.1:8080/dash if
* you are using the Control Hub. Once you've successfully connected, start the program, and your
* robot will begin moving forward and backward according to a motion profile. Your job is to graph
* the velocity errors over time and adjust the feedforward coefficients. Once you've found a
* satisfactory set of gains, add them to the appropriate fields in the DriveConstants.java file.
*
* Pressing Y/Δ (Xbox/PS4) will pause the tuning process and enter driver override, allowing the
* user to reset the position of the bot in the event that it drifts off the path.
* Pressing B/O (Xbox/PS4) will cede control back to the tuning process.
*/
//@Config
@Autonomous(group = "drive")
public class ManualFeedforwardTuner extends LinearOpMode {
public static double DISTANCE = 72; // in
private FtcDashboard dashboard = FtcDashboard.getInstance();
private SampleMecanumDrive drive;
enum Mode {
DRIVER_MODE,
TUNING_MODE
}
private Mode mode;
private static MotionProfile generateProfile(boolean movingForward) {
MotionState start = new MotionState(movingForward ? 0 : DISTANCE, 0, 0, 0);
MotionState goal = new MotionState(movingForward ? DISTANCE : 0, 0, 0, 0);
return MotionProfileGenerator.generateSimpleMotionProfile(start, goal, MAX_VEL, MAX_ACCEL);
}
@Override
public void runOpMode() {
// if (RUN_USING_ENCODER) {
// RobotLog.setGlobalErrorMsg("Feedforward constants usually don't need to be tuned " +
// "when using the built-in drive motor velocity PID.");
// }
telemetry = new MultipleTelemetry(telemetry, dashboard.getTelemetry());
drive = new SampleMecanumDrive(hardwareMap);
mode = Mode.TUNING_MODE;
NanoClock clock = NanoClock.system();
telemetry.addLine("Ready!");
telemetry.update();
telemetry.clearAll();
waitForStart();
if (isStopRequested()) return;
boolean movingForwards = true;
MotionProfile activeProfile = generateProfile(true);
double profileStart = clock.seconds();
while (!isStopRequested()) {
telemetry.addData("mode", mode);
switch (mode) {
case TUNING_MODE:
if (gamepad1.y) {
mode = Mode.DRIVER_MODE;
}
// calculate and set the motor power
double profileTime = clock.seconds() - profileStart;
if (profileTime > activeProfile.duration()) {
// generate a new profile
movingForwards = !movingForwards;
activeProfile = generateProfile(movingForwards);
profileStart = clock.seconds();
}
MotionState motionState = activeProfile.get(profileTime);
double targetPower = Kinematics.calculateMotorFeedforward(motionState.getV(), motionState.getA(), kV, kA, kStatic);
drive.setDrivePower(new Pose2d(targetPower, 0, 0));
drive.updatePoseEstimate();
Pose2d poseVelo = Objects.requireNonNull(drive.getPoseVelocity(), "poseVelocity() must not be null. Ensure that the getWheelVelocities() method has been overridden in your localizer.");
double currentVelo = poseVelo.getX();
// update telemetry
telemetry.addData("targetVelocity", motionState.getV());
telemetry.addData("measuredVelocity", currentVelo);
telemetry.addData("error", motionState.getV() - currentVelo);
break;
case DRIVER_MODE:
if (gamepad1.b) {
mode = Mode.TUNING_MODE;
movingForwards = true;
activeProfile = generateProfile(movingForwards);
profileStart = clock.seconds();
}
drive.setWeightedDrivePower(
new Pose2d(
-gamepad1.left_stick_y,
-gamepad1.left_stick_x,
-gamepad1.right_stick_x
)
);
break;
}
telemetry.update();
}
}
}

71
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/MaxAngularVeloTuner.java Normal file
View File

@ -0,0 +1,71 @@
package org.firstinspires.ftc.teamcode.roadrunner.drive.opmode;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.util.ElapsedTime;
import org.firstinspires.ftc.teamcode.roadrunner.drive.SampleMecanumDrive;
import java.util.Objects;
/**
* This routine is designed to calculate the maximum angular velocity your bot can achieve under load.
* <p>
* Upon pressing start, your bot will turn at max power for RUNTIME seconds.
* <p>
* Further fine tuning of MAX_ANG_VEL may be desired.
*/
//@Config
@Disabled
@Autonomous(group = "drive")
public class MaxAngularVeloTuner extends LinearOpMode {
public static double RUNTIME = 4.0;
private ElapsedTime timer;
private double maxAngVelocity = 0.0;
@Override
public void runOpMode() throws InterruptedException {
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
telemetry.addLine("Your bot will turn at full speed for " + RUNTIME + " seconds.");
telemetry.addLine("Please ensure you have enough space cleared.");
telemetry.addLine("");
telemetry.addLine("Press start when ready.");
telemetry.update();
waitForStart();
telemetry.clearAll();
telemetry.update();
drive.setDrivePower(new Pose2d(0, 0, 1));
timer = new ElapsedTime();
while (!isStopRequested() && timer.seconds() < RUNTIME) {
drive.updatePoseEstimate();
Pose2d poseVelo = Objects.requireNonNull(drive.getPoseVelocity(), "poseVelocity() must not be null. Ensure that the getWheelVelocities() method has been overridden in your localizer.");
maxAngVelocity = Math.max(poseVelo.getHeading(), maxAngVelocity);
}
drive.setDrivePower(new Pose2d());
telemetry.addData("Max Angular Velocity (rad)", maxAngVelocity);
telemetry.addData("Max Angular Velocity (deg)", Math.toDegrees(maxAngVelocity));
telemetry.update();
while (!isStopRequested()) idle();
}
}

83
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/MaxVelocityTuner.java Normal file
View File

@ -0,0 +1,83 @@
package org.firstinspires.ftc.teamcode.roadrunner.drive.opmode;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.VoltageSensor;
import com.qualcomm.robotcore.util.ElapsedTime;
import org.firstinspires.ftc.teamcode.roadrunner.drive.DriveConstants;
import org.firstinspires.ftc.teamcode.roadrunner.drive.SampleMecanumDrive;
import java.util.Objects;
/**
* This routine is designed to calculate the maximum velocity your bot can achieve under load. It
* will also calculate the effective kF value for your velocity PID.
* <p>
* Upon pressing start, your bot will run at max power for RUNTIME seconds.
* <p>
* Further fine tuning of kF may be desired.
*/
//@Config
@Disabled
@Autonomous(group = "drive")
public class MaxVelocityTuner extends LinearOpMode {
public static double RUNTIME = 2.0;
private ElapsedTime timer;
private double maxVelocity = 0.0;
private VoltageSensor batteryVoltageSensor;
@Override
public void runOpMode() throws InterruptedException {
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
drive.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
batteryVoltageSensor = hardwareMap.voltageSensor.iterator().next();
telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
telemetry.addLine("Your bot will go at full speed for " + RUNTIME + " seconds.");
telemetry.addLine("Please ensure you have enough space cleared.");
telemetry.addLine("");
telemetry.addLine("Press start when ready.");
telemetry.update();
waitForStart();
telemetry.clearAll();
telemetry.update();
drive.setDrivePower(new Pose2d(1, 0, 0));
timer = new ElapsedTime();
while (!isStopRequested() && timer.seconds() < RUNTIME) {
drive.updatePoseEstimate();
Pose2d poseVelo = Objects.requireNonNull(drive.getPoseVelocity(), "poseVelocity() must not be null. Ensure that the getWheelVelocities() method has been overridden in your localizer.");
maxVelocity = Math.max(poseVelo.vec().norm(), maxVelocity);
}
drive.setDrivePower(new Pose2d());
double effectiveKf = DriveConstants.getMotorVelocityF(veloInchesToTicks(maxVelocity));
telemetry.addData("Max Velocity", maxVelocity);
telemetry.addData("Voltage Compensated kF", effectiveKf * batteryVoltageSensor.getVoltage() / 12);
telemetry.update();
while (!isStopRequested() && opModeIsActive()) idle();
}
private double veloInchesToTicks(double inchesPerSec) {
return inchesPerSec / (2 * Math.PI * DriveConstants.WHEEL_RADIUS) / DriveConstants.GEAR_RATIO * DriveConstants.TICKS_PER_REV;
}
}

90
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/MotorDirectionDebugger.java Normal file
View File

@ -0,0 +1,90 @@
package org.firstinspires.ftc.teamcode.roadrunner.drive.opmode;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import org.firstinspires.ftc.robotcore.external.Telemetry;
import org.firstinspires.ftc.teamcode.roadrunner.drive.SampleMecanumDrive;
/**
* This is a simple teleop routine for debugging your motor configuration.
* Pressing each of the buttons will power its respective motor.
*
* Button Mappings:
*
* Xbox/PS4 Button - Motor
* X / ▢ - Front Left
* Y / Δ - Front Right
* B / O - Rear Right
* A / X - Rear Left
* The buttons are mapped to match the wheels spatially if you
* were to rotate the gamepad 45deg°. x/square is the front left
* ________ and each button corresponds to the wheel as you go clockwise
* / ______ \
* ------------.-' _ '-..+ Front of Bot
* / _ ( Y ) _ \ ^
* | ( X ) _ ( B ) | Front Left \ Front Right
* ___ '. ( A ) /| Wheel \ Wheel
* .' '. '-._____.-' .' (x/▢) \ (Y/Δ)
* | | | \
* '.___.' '. | Rear Left \ Rear Right
* '. / Wheel \ Wheel
* \. .' (A/X) \ (B/O)
* \________/
*
* Uncomment the @Disabled tag below to use this opmode.
*/
//@Config
@TeleOp(group = "drive")
public class MotorDirectionDebugger extends LinearOpMode {
public static double MOTOR_POWER = 0.7;
@Override
public void runOpMode() throws InterruptedException {
telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
telemetry.addLine("Press play to begin the debugging opmode");
telemetry.update();
waitForStart();
if (isStopRequested()) return;
telemetry.clearAll();
telemetry.setDisplayFormat(Telemetry.DisplayFormat.HTML);
while (!isStopRequested()) {
telemetry.addLine("Press each button to turn on its respective motor");
telemetry.addLine();
telemetry.addLine("<font face=\"monospace\">Xbox/PS4 Button - Motor</font>");
telemetry.addLine("<font face=\"monospace\">&nbsp;&nbsp;X / ▢&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;- Front Left</font>");
telemetry.addLine("<font face=\"monospace\">&nbsp;&nbsp;Y / Δ&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;- Front Right</font>");
telemetry.addLine("<font face=\"monospace\">&nbsp;&nbsp;B / O&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;- Rear&nbsp;&nbsp;Right</font>");
telemetry.addLine("<font face=\"monospace\">&nbsp;&nbsp;A / X&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;- Rear&nbsp;&nbsp;Left</font>");
telemetry.addLine();
if(gamepad1.x) {
drive.setMotorPowers(MOTOR_POWER, 0, 0, 0);
telemetry.addLine("Running Motor: Front Left");
} else if(gamepad1.y) {
drive.setMotorPowers(0, 0, 0, MOTOR_POWER);
telemetry.addLine("Running Motor: Front Right");
} else if(gamepad1.b) {
drive.setMotorPowers(0, 0, MOTOR_POWER, 0);
telemetry.addLine("Running Motor: Rear Right");
} else if(gamepad1.a) {
drive.setMotorPowers(0, MOTOR_POWER, 0, 0);
telemetry.addLine("Running Motor: Rear Left");
} else {
drive.setMotorPowers(0, 0, 0, 0);
telemetry.addLine("Running Motor: None");
}
telemetry.update();
}
}
}

40
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/SplineTest.java Normal file
View File

@ -0,0 +1,40 @@
package org.firstinspires.ftc.teamcode.roadrunner.drive.opmode;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.geometry.Vector2d;
import com.acmerobotics.roadrunner.trajectory.Trajectory;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.firstinspires.ftc.teamcode.roadrunner.drive.SampleMecanumDrive;
/*
* This is an example of a more complex path to really test the tuning.
*/
@Disabled
@Autonomous(group = "drive")
public class SplineTest extends LinearOpMode {
@Override
public void runOpMode() throws InterruptedException {
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
waitForStart();
if (isStopRequested()) return;
Trajectory traj = drive.trajectoryBuilder(new Pose2d())
.splineTo(new Vector2d(30, 30), 0)
.build();
drive.followTrajectory(traj);
sleep(2000);
drive.followTrajectory(
drive.trajectoryBuilder(traj.end(), true)
.splineTo(new Vector2d(0, 0), Math.toRadians(180))
.build()
);
}
}

46
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/StrafeTest.java Normal file
View File

@ -0,0 +1,46 @@
package org.firstinspires.ftc.teamcode.roadrunner.drive.opmode;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.trajectory.Trajectory;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.firstinspires.ftc.teamcode.roadrunner.drive.SampleMecanumDrive;
/*
* This is a simple routine to test translational drive capabilities.
*/
//@Config
@Disabled
@Autonomous(group = "drive")
public class StrafeTest extends LinearOpMode {
public static double DISTANCE = 60; // in
@Override
public void runOpMode() throws InterruptedException {
telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
Trajectory trajectory = drive.trajectoryBuilder(new Pose2d())
.strafeRight(DISTANCE)
.build();
waitForStart();
if (isStopRequested()) return;
drive.followTrajectory(trajectory);
Pose2d poseEstimate = drive.getPoseEstimate();
telemetry.addData("finalX", poseEstimate.getX());
telemetry.addData("finalY", poseEstimate.getY());
telemetry.addData("finalHeading", poseEstimate.getHeading());
telemetry.update();
while (!isStopRequested() && opModeIsActive()) ;
}
}

44
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/StraightTest.java Normal file
View File

@ -0,0 +1,44 @@
package org.firstinspires.ftc.teamcode.roadrunner.drive.opmode;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.trajectory.Trajectory;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.firstinspires.ftc.teamcode.roadrunner.drive.SampleMecanumDrive;
/*
* This is a simple routine to test translational drive capabilities.
*/
//@Config
@Autonomous(group = "drive")
public class StraightTest extends LinearOpMode {
public static double DISTANCE = 60; // in
@Override
public void runOpMode() throws InterruptedException {
telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
Trajectory trajectory = drive.trajectoryBuilder(new Pose2d())
.forward(DISTANCE)
.build();
waitForStart();
if (isStopRequested()) return;
drive.followTrajectory(trajectory);
Pose2d poseEstimate = drive.getPoseEstimate();
telemetry.addData("finalX", poseEstimate.getX());
telemetry.addData("finalY", poseEstimate.getY());
telemetry.addData("finalHeading", poseEstimate.getHeading());
telemetry.update();
while (!isStopRequested() && opModeIsActive()) ;
}
}

88
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/TrackWidthTuner.java Normal file
View File

@ -0,0 +1,88 @@
package org.firstinspires.ftc.teamcode.roadrunner.drive.opmode;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.util.Angle;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.util.MovingStatistics;
import org.firstinspires.ftc.robotcore.internal.system.Misc;
import org.firstinspires.ftc.teamcode.roadrunner.drive.DriveConstants;
import org.firstinspires.ftc.teamcode.roadrunner.drive.SampleMecanumDrive;
/*
* This routine determines the effective track width. The procedure works by executing a point turn
* with a given angle and measuring the difference between that angle and the actual angle (as
* indicated by an external IMU/gyro, track wheels, or some other localizer). The quotient
* given angle / actual angle gives a multiplicative adjustment to the estimated track width
* (effective track width = estimated track width * given angle / actual angle). The routine repeats
* this procedure a few times and averages the values for additional accuracy. Note: a relatively
* accurate track width estimate is important or else the angular constraints will be thrown off.
*/
//@Config
@Disabled
@Autonomous(group = "drive")
public class TrackWidthTuner extends LinearOpMode {
public static double ANGLE = 180; // deg
public static int NUM_TRIALS = 5;
public static int DELAY = 1000; // ms
@Override
public void runOpMode() throws InterruptedException {
telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
// TODO: if you haven't already, set the localizer to something that doesn't depend on
// drive encoders for computing the heading
telemetry.addLine("Press play to begin the track width tuner routine");
telemetry.addLine("Make sure your robot has enough clearance to turn smoothly");
telemetry.update();
waitForStart();
if (isStopRequested()) return;
telemetry.clearAll();
telemetry.addLine("Running...");
telemetry.update();
MovingStatistics trackWidthStats = new MovingStatistics(NUM_TRIALS);
for (int i = 0; i < NUM_TRIALS; i++) {
drive.setPoseEstimate(new Pose2d());
// it is important to handle heading wraparounds
double headingAccumulator = 0;
double lastHeading = 0;
drive.turnAsync(Math.toRadians(ANGLE));
while (!isStopRequested() && drive.isBusy()) {
double heading = drive.getPoseEstimate().getHeading();
headingAccumulator += Angle.normDelta(heading - lastHeading);
lastHeading = heading;
drive.update();
}
double trackWidth = DriveConstants.TRACK_WIDTH * Math.toRadians(ANGLE) / headingAccumulator;
trackWidthStats.add(trackWidth);
sleep(DELAY);
}
telemetry.clearAll();
telemetry.addLine("Tuning complete");
telemetry.addLine(Misc.formatInvariant("Effective track width = %.2f (SE = %.3f)",
trackWidthStats.getMean(),
trackWidthStats.getStandardDeviation() / Math.sqrt(NUM_TRIALS)));
telemetry.update();
while (!isStopRequested()) {
idle();
}
}
}

104
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/TrackingWheelForwardOffsetTuner.java Normal file
View File

@ -0,0 +1,104 @@
package org.firstinspires.ftc.teamcode.roadrunner.drive.opmode;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.util.Angle;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.util.MovingStatistics;
import com.qualcomm.robotcore.util.RobotLog;
import org.firstinspires.ftc.robotcore.internal.system.Misc;
import org.firstinspires.ftc.teamcode.roadrunner.drive.SampleMecanumDrive;
import org.firstinspires.ftc.teamcode.roadrunner.drive.StandardTrackingWheelLocalizer;
/**
* This routine determines the effective forward offset for the lateral tracking wheel.
* The procedure executes a point turn at a given angle for a certain number of trials,
* along with a specified delay in milliseconds. The purpose of this is to track the
* change in the y position during the turn. The offset, or distance, of the lateral tracking
* wheel from the center or rotation allows the wheel to spin during a point turn, leading
* to an incorrect measurement for the y position. This creates an arc around around
* the center of rotation with an arc length of change in y and a radius equal to the forward
* offset. We can compute this offset by calculating (change in y position) / (change in heading)
* which returns the radius if the angle (change in heading) is in radians. This is based
* on the arc length formula of length = theta * radius.
*
* To run this routine, simply adjust the desired angle and specify the number of trials
* and the desired delay. Then, run the procedure. Once it finishes, it will print the
* average of all the calculated forward offsets derived from the calculation. This calculated
* forward offset is then added onto the current forward offset to produce an overall estimate
* for the forward offset. You can run this procedure as many times as necessary until a
* satisfactory result is produced.
*/
//@Config
@Disabled
@Autonomous(group="drive")
public class TrackingWheelForwardOffsetTuner extends LinearOpMode {
public static double ANGLE = 180; // deg
public static int NUM_TRIALS = 5;
public static int DELAY = 1000; // ms
@Override
public void runOpMode() throws InterruptedException {
telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
if (!(drive.getLocalizer() instanceof StandardTrackingWheelLocalizer)) {
RobotLog.setGlobalErrorMsg("StandardTrackingWheelLocalizer is not being set in the "
+ "drive class. Ensure that \"setLocalizer(new StandardTrackingWheelLocalizer"
+ "(hardwareMap));\" is called in SampleMecanumDrive.java");
}
telemetry.addLine("Press play to begin the forward offset tuner");
telemetry.addLine("Make sure your robot has enough clearance to turn smoothly");
telemetry.update();
waitForStart();
if (isStopRequested()) return;
telemetry.clearAll();
telemetry.addLine("Running...");
telemetry.update();
MovingStatistics forwardOffsetStats = new MovingStatistics(NUM_TRIALS);
for (int i = 0; i < NUM_TRIALS; i++) {
drive.setPoseEstimate(new Pose2d());
// it is important to handle heading wraparounds
double headingAccumulator = 0;
double lastHeading = 0;
drive.turnAsync(Math.toRadians(ANGLE));
while (!isStopRequested() && drive.isBusy()) {
double heading = drive.getPoseEstimate().getHeading();
headingAccumulator += Angle.norm(heading - lastHeading);
lastHeading = heading;
drive.update();
}
double forwardOffset = StandardTrackingWheelLocalizer.FORWARD_OFFSET +
drive.getPoseEstimate().getY() / headingAccumulator;
forwardOffsetStats.add(forwardOffset);
sleep(DELAY);
}
telemetry.clearAll();
telemetry.addLine("Tuning complete");
telemetry.addLine(Misc.formatInvariant("Effective forward offset = %.2f (SE = %.3f)",
forwardOffsetStats.getMean(),
forwardOffsetStats.getStandardDeviation() / Math.sqrt(NUM_TRIALS)));
telemetry.update();
while (!isStopRequested()) {
idle();
}
}
}

131
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/TrackingWheelLateralDistanceTuner.java Normal file
View File

@ -0,0 +1,131 @@
package org.firstinspires.ftc.teamcode.roadrunner.drive.opmode;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.util.Angle;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.util.RobotLog;
import org.firstinspires.ftc.teamcode.roadrunner.drive.SampleMecanumDrive;
import org.firstinspires.ftc.teamcode.roadrunner.drive.StandardTrackingWheelLocalizer;
/**
* Opmode designed to assist the user in tuning the `StandardTrackingWheelLocalizer`'s
* LATERAL_DISTANCE value. The LATERAL_DISTANCE is the center-to-center distance of the parallel
* wheels.
*
* Tuning Routine:
*
* 1. Set the LATERAL_DISTANCE value in StandardTrackingWheelLocalizer.java to the physical
* measured value. This need only be an estimated value as you will be tuning it anyways.
*
* 2. Make a mark on the bot (with a piece of tape or sharpie or however you wish) and make an
* similar mark right below the indicator on your bot. This will be your reference point to
* ensure you've turned exactly 360°.
*
* 3. Although not entirely necessary, having the bot's pose being drawn in dashbooard does help
* identify discrepancies in the LATERAL_DISTANCE value. To access the dashboard,
* connect your computer to the RC's WiFi network. In your browser, navigate to
* https://192.168.49.1:8080/dash if you're using the RC phone or https://192.168.43.1:8080/dash
* if you are using the Control Hub.
* Ensure the field is showing (select the field view in top right of the page).
*
* 4. Press play to begin the tuning routine.
*
* 5. Use the right joystick on gamepad 1 to turn the bot counterclockwise.
*
* 6. Spin the bot 10 times, counterclockwise. Make sure to keep track of these turns.
*
* 7. Once the bot has finished spinning 10 times, press A to finishing the routine. The indicators
* on the bot and on the ground you created earlier should be lined up.
*
* 8. Your effective LATERAL_DISTANCE will be given. Stick this value into your
* StandardTrackingWheelLocalizer.java class.
*
* 9. If this value is incorrect, run the routine again while adjusting the LATERAL_DISTANCE value
* yourself. Read the heading output and follow the advice stated in the note below to manually
* nudge the values yourself.
*
* Note:
* It helps to pay attention to how the pose on the field is drawn in dashboard. A blue circle with
* a line from the circumference to the center should be present, representing the bot. The line
* indicates forward. If your LATERAL_DISTANCE value is tuned currently, the pose drawn in
* dashboard should keep track with the pose of your actual bot. If the drawn bot turns slower than
* the actual bot, the LATERAL_DISTANCE should be decreased. If the drawn bot turns faster than the
* actual bot, the LATERAL_DISTANCE should be increased.
*
* If your drawn bot oscillates around a point in dashboard, don't worry. This is because the
* position of the perpendicular wheel isn't perfectly set and causes a discrepancy in the
* effective center of rotation. You can ignore this effect. The center of rotation will be offset
* slightly but your heading will still be fine. This does not affect your overall tracking
* precision. The heading should still line up.
*/
//@Config
@Disabled
@TeleOp(group = "drive")
public class TrackingWheelLateralDistanceTuner extends LinearOpMode {
public static int NUM_TURNS = 10;
@Override
public void runOpMode() throws InterruptedException {
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
if (!(drive.getLocalizer() instanceof StandardTrackingWheelLocalizer)) {
RobotLog.setGlobalErrorMsg("StandardTrackingWheelLocalizer is not being set in the "
+ "drive class. Ensure that \"setLocalizer(new StandardTrackingWheelLocalizer"
+ "(hardwareMap));\" is called in SampleMecanumDrive.java");
}
telemetry.addLine("Prior to beginning the routine, please read the directions "
+ "located in the comments of the opmode file.");
telemetry.addLine("Press play to begin the tuning routine.");
telemetry.addLine("");
telemetry.addLine("Press Y/△ to stop the routine.");
telemetry.update();
waitForStart();
if (isStopRequested()) return;
telemetry.clearAll();
telemetry.update();
double headingAccumulator = 0;
double lastHeading = 0;
boolean tuningFinished = false;
while (!isStopRequested() && !tuningFinished) {
Pose2d vel = new Pose2d(0, 0, -gamepad1.right_stick_x);
drive.setDrivePower(vel);
drive.update();
double heading = drive.getPoseEstimate().getHeading();
double deltaHeading = heading - lastHeading;
headingAccumulator += Angle.normDelta(deltaHeading);
lastHeading = heading;
telemetry.clearAll();
telemetry.addLine("Total Heading (deg): " + Math.toDegrees(headingAccumulator));
telemetry.addLine("Raw Heading (deg): " + Math.toDegrees(heading));
telemetry.addLine();
telemetry.addLine("Press Y/△ to conclude routine");
telemetry.update();
if (gamepad1.y)
tuningFinished = true;
}
telemetry.clearAll();
telemetry.addLine("Localizer's total heading: " + Math.toDegrees(headingAccumulator) + "°");
telemetry.addLine("Effective LATERAL_DISTANCE: " +
(headingAccumulator / (NUM_TURNS * Math.PI * 2)) * StandardTrackingWheelLocalizer.LATERAL_DISTANCE);
telemetry.update();
while (!isStopRequested()) idle();
}
}

28
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/opmode/TurnTest.java Normal file
View File

@ -0,0 +1,28 @@
package org.firstinspires.ftc.teamcode.roadrunner.drive.opmode;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.firstinspires.ftc.teamcode.roadrunner.drive.SampleMecanumDrive;
/*
* This is a simple routine to test turning capabilities.
*/
//@Config
@Disabled
@Autonomous(group = "drive")
public class TurnTest extends LinearOpMode {
public static double ANGLE = 90; // deg
@Override
public void runOpMode() throws InterruptedException {
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
waitForStart();
if (isStopRequested()) return;
drive.turn(Math.toRadians(ANGLE));
}
}

4
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/trajectorysequence/EmptySequenceException.java Normal file
View File

@ -0,0 +1,4 @@
package org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence;
public class EmptySequenceException extends RuntimeException { }

44
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/trajectorysequence/TrajectorySequence.java Normal file
View File

@ -0,0 +1,44 @@
package org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.sequencesegment.SequenceSegment;
import java.util.Collections;
import java.util.List;
public class TrajectorySequence {
private final List<SequenceSegment> sequenceList;
public TrajectorySequence(List<SequenceSegment> sequenceList) {
if (sequenceList.size() == 0) throw new EmptySequenceException();
this.sequenceList = Collections.unmodifiableList(sequenceList);
}
public Pose2d start() {
return sequenceList.get(0).getStartPose();
}
public Pose2d end() {
return sequenceList.get(sequenceList.size() - 1).getEndPose();
}
public double duration() {
double total = 0.0;
for (SequenceSegment segment : sequenceList) {
total += segment.getDuration();
}
return total;
}
public SequenceSegment get(int i) {
return sequenceList.get(i);
}
public int size() {
return sequenceList.size();
}
}

711
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/trajectorysequence/TrajectorySequenceBuilder.java Normal file
View File

@ -0,0 +1,711 @@
package org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.geometry.Vector2d;
import com.acmerobotics.roadrunner.path.PathContinuityViolationException;
import com.acmerobotics.roadrunner.profile.MotionProfile;
import com.acmerobotics.roadrunner.profile.MotionProfileGenerator;
import com.acmerobotics.roadrunner.profile.MotionState;
import com.acmerobotics.roadrunner.trajectory.DisplacementMarker;
import com.acmerobotics.roadrunner.trajectory.DisplacementProducer;
import com.acmerobotics.roadrunner.trajectory.MarkerCallback;
import com.acmerobotics.roadrunner.trajectory.SpatialMarker;
import com.acmerobotics.roadrunner.trajectory.TemporalMarker;
import com.acmerobotics.roadrunner.trajectory.TimeProducer;
import com.acmerobotics.roadrunner.trajectory.Trajectory;
import com.acmerobotics.roadrunner.trajectory.TrajectoryBuilder;
import com.acmerobotics.roadrunner.trajectory.TrajectoryMarker;
import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryAccelerationConstraint;
import com.acmerobotics.roadrunner.trajectory.constraints.TrajectoryVelocityConstraint;
import com.acmerobotics.roadrunner.util.Angle;
import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.sequencesegment.SequenceSegment;
import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.sequencesegment.TrajectorySegment;
import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.sequencesegment.TurnSegment;
import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.sequencesegment.WaitSegment;
import java.util.ArrayList;
import java.util.Collections;
import java.util.List;
public class TrajectorySequenceBuilder {
private final double resolution = 0.25;
private final TrajectoryVelocityConstraint baseVelConstraint;
private final TrajectoryAccelerationConstraint baseAccelConstraint;
private TrajectoryVelocityConstraint currentVelConstraint;
private TrajectoryAccelerationConstraint currentAccelConstraint;
private final double baseTurnConstraintMaxAngVel;
private final double baseTurnConstraintMaxAngAccel;
private double currentTurnConstraintMaxAngVel;
private double currentTurnConstraintMaxAngAccel;
private final List<SequenceSegment> sequenceSegments;
private final List<TemporalMarker> temporalMarkers;
private final List<DisplacementMarker> displacementMarkers;
private final List<SpatialMarker> spatialMarkers;
private Pose2d lastPose;
private double tangentOffset;
private boolean setAbsoluteTangent;
private double absoluteTangent;
private TrajectoryBuilder currentTrajectoryBuilder;
private double currentDuration;
private double currentDisplacement;
private double lastDurationTraj;
private double lastDisplacementTraj;
public TrajectorySequenceBuilder(
Pose2d startPose,
Double startTangent,
TrajectoryVelocityConstraint baseVelConstraint,
TrajectoryAccelerationConstraint baseAccelConstraint,
double baseTurnConstraintMaxAngVel,
double baseTurnConstraintMaxAngAccel
) {
this.baseVelConstraint = baseVelConstraint;
this.baseAccelConstraint = baseAccelConstraint;
this.currentVelConstraint = baseVelConstraint;
this.currentAccelConstraint = baseAccelConstraint;
this.baseTurnConstraintMaxAngVel = baseTurnConstraintMaxAngVel;
this.baseTurnConstraintMaxAngAccel = baseTurnConstraintMaxAngAccel;
this.currentTurnConstraintMaxAngVel = baseTurnConstraintMaxAngVel;
this.currentTurnConstraintMaxAngAccel = baseTurnConstraintMaxAngAccel;
sequenceSegments = new ArrayList<>();
temporalMarkers = new ArrayList<>();
displacementMarkers = new ArrayList<>();
spatialMarkers = new ArrayList<>();
lastPose = startPose;
tangentOffset = 0.0;
setAbsoluteTangent = (startTangent != null);
absoluteTangent = startTangent != null ? startTangent : 0.0;
currentTrajectoryBuilder = null;
currentDuration = 0.0;
currentDisplacement = 0.0;
lastDurationTraj = 0.0;
lastDisplacementTraj = 0.0;
}
public TrajectorySequenceBuilder(
Pose2d startPose,
TrajectoryVelocityConstraint baseVelConstraint,
TrajectoryAccelerationConstraint baseAccelConstraint,
double baseTurnConstraintMaxAngVel,
double baseTurnConstraintMaxAngAccel
) {
this(
startPose, null,
baseVelConstraint, baseAccelConstraint,
baseTurnConstraintMaxAngVel, baseTurnConstraintMaxAngAccel
);
}
public TrajectorySequenceBuilder lineTo(Vector2d endPosition) {
return addPath(() -> currentTrajectoryBuilder.lineTo(endPosition, currentVelConstraint, currentAccelConstraint));
}
public TrajectorySequenceBuilder lineTo(
Vector2d endPosition,
TrajectoryVelocityConstraint velConstraint,
TrajectoryAccelerationConstraint accelConstraint
) {
return addPath(() -> currentTrajectoryBuilder.lineTo(endPosition, velConstraint, accelConstraint));
}
public TrajectorySequenceBuilder lineToConstantHeading(Vector2d endPosition) {
return addPath(() -> currentTrajectoryBuilder.lineToConstantHeading(endPosition, currentVelConstraint, currentAccelConstraint));
}
public TrajectorySequenceBuilder lineToConstantHeading(
Vector2d endPosition,
TrajectoryVelocityConstraint velConstraint,
TrajectoryAccelerationConstraint accelConstraint
) {
return addPath(() -> currentTrajectoryBuilder.lineToConstantHeading(endPosition, velConstraint, accelConstraint));
}
public TrajectorySequenceBuilder lineToLinearHeading(Pose2d endPose) {
return addPath(() -> currentTrajectoryBuilder.lineToLinearHeading(endPose, currentVelConstraint, currentAccelConstraint));
}
public TrajectorySequenceBuilder lineToLinearHeading(
Pose2d endPose,
TrajectoryVelocityConstraint velConstraint,
TrajectoryAccelerationConstraint accelConstraint
) {
return addPath(() -> currentTrajectoryBuilder.lineToLinearHeading(endPose, velConstraint, accelConstraint));
}
public TrajectorySequenceBuilder lineToSplineHeading(Pose2d endPose) {
return addPath(() -> currentTrajectoryBuilder.lineToSplineHeading(endPose, currentVelConstraint, currentAccelConstraint));
}
public TrajectorySequenceBuilder lineToSplineHeading(
Pose2d endPose,
TrajectoryVelocityConstraint velConstraint,
TrajectoryAccelerationConstraint accelConstraint
) {
return addPath(() -> currentTrajectoryBuilder.lineToSplineHeading(endPose, velConstraint, accelConstraint));
}
public TrajectorySequenceBuilder strafeTo(Vector2d endPosition) {
return addPath(() -> currentTrajectoryBuilder.strafeTo(endPosition, currentVelConstraint, currentAccelConstraint));
}
public TrajectorySequenceBuilder strafeTo(
Vector2d endPosition,
TrajectoryVelocityConstraint velConstraint,
TrajectoryAccelerationConstraint accelConstraint
) {
return addPath(() -> currentTrajectoryBuilder.strafeTo(endPosition, velConstraint, accelConstraint));
}
public TrajectorySequenceBuilder forward(double distance) {
return addPath(() -> currentTrajectoryBuilder.forward(distance, currentVelConstraint, currentAccelConstraint));
}
public TrajectorySequenceBuilder forward(
double distance,
TrajectoryVelocityConstraint velConstraint,
TrajectoryAccelerationConstraint accelConstraint
) {
return addPath(() -> currentTrajectoryBuilder.forward(distance, velConstraint, accelConstraint));
}
public TrajectorySequenceBuilder back(double distance) {
return addPath(() -> currentTrajectoryBuilder.back(distance, currentVelConstraint, currentAccelConstraint));
}
public TrajectorySequenceBuilder back(
double distance,
TrajectoryVelocityConstraint velConstraint,
TrajectoryAccelerationConstraint accelConstraint
) {
return addPath(() -> currentTrajectoryBuilder.back(distance, velConstraint, accelConstraint));
}
public TrajectorySequenceBuilder strafeLeft(double distance) {
return addPath(() -> currentTrajectoryBuilder.strafeLeft(distance, currentVelConstraint, currentAccelConstraint));
}
public TrajectorySequenceBuilder strafeLeft(
double distance,
TrajectoryVelocityConstraint velConstraint,
TrajectoryAccelerationConstraint accelConstraint
) {
return addPath(() -> currentTrajectoryBuilder.strafeLeft(distance, velConstraint, accelConstraint));
}
public TrajectorySequenceBuilder strafeRight(double distance) {
return addPath(() -> currentTrajectoryBuilder.strafeRight(distance, currentVelConstraint, currentAccelConstraint));
}
public TrajectorySequenceBuilder strafeRight(
double distance,
TrajectoryVelocityConstraint velConstraint,
TrajectoryAccelerationConstraint accelConstraint
) {
return addPath(() -> currentTrajectoryBuilder.strafeRight(distance, velConstraint, accelConstraint));
}
public TrajectorySequenceBuilder splineTo(Vector2d endPosition, double endHeading) {
return addPath(() -> currentTrajectoryBuilder.splineTo(endPosition, endHeading, currentVelConstraint, currentAccelConstraint));
}
public TrajectorySequenceBuilder splineTo(
Vector2d endPosition,
double endHeading,
TrajectoryVelocityConstraint velConstraint,
TrajectoryAccelerationConstraint accelConstraint
) {
return addPath(() -> currentTrajectoryBuilder.splineTo(endPosition, endHeading, velConstraint, accelConstraint));
}
public TrajectorySequenceBuilder splineToConstantHeading(Vector2d endPosition, double endHeading) {
return addPath(() -> currentTrajectoryBuilder.splineToConstantHeading(endPosition, endHeading, currentVelConstraint, currentAccelConstraint));
}
public TrajectorySequenceBuilder splineToConstantHeading(
Vector2d endPosition,
double endHeading,
TrajectoryVelocityConstraint velConstraint,
TrajectoryAccelerationConstraint accelConstraint
) {
return addPath(() -> currentTrajectoryBuilder.splineToConstantHeading(endPosition, endHeading, velConstraint, accelConstraint));
}
public TrajectorySequenceBuilder splineToLinearHeading(Pose2d endPose, double endHeading) {
return addPath(() -> currentTrajectoryBuilder.splineToLinearHeading(endPose, endHeading, currentVelConstraint, currentAccelConstraint));
}
public TrajectorySequenceBuilder splineToLinearHeading(
Pose2d endPose,
double endHeading,
TrajectoryVelocityConstraint velConstraint,
TrajectoryAccelerationConstraint accelConstraint
) {
return addPath(() -> currentTrajectoryBuilder.splineToLinearHeading(endPose, endHeading, velConstraint, accelConstraint));
}
public TrajectorySequenceBuilder splineToSplineHeading(Pose2d endPose, double endHeading) {
return addPath(() -> currentTrajectoryBuilder.splineToSplineHeading(endPose, endHeading, currentVelConstraint, currentAccelConstraint));
}
public TrajectorySequenceBuilder splineToSplineHeading(
Pose2d endPose,
double endHeading,
TrajectoryVelocityConstraint velConstraint,
TrajectoryAccelerationConstraint accelConstraint
) {
return addPath(() -> currentTrajectoryBuilder.splineToSplineHeading(endPose, endHeading, velConstraint, accelConstraint));
}
private TrajectorySequenceBuilder addPath(AddPathCallback callback) {
if (currentTrajectoryBuilder == null) newPath();
try {
callback.run();
} catch (PathContinuityViolationException e) {
newPath();
callback.run();
}
Trajectory builtTraj = currentTrajectoryBuilder.build();
double durationDifference = builtTraj.duration() - lastDurationTraj;
double displacementDifference = builtTraj.getPath().length() - lastDisplacementTraj;
lastPose = builtTraj.end();
currentDuration += durationDifference;
currentDisplacement += displacementDifference;
lastDurationTraj = builtTraj.duration();
lastDisplacementTraj = builtTraj.getPath().length();
return this;
}
public TrajectorySequenceBuilder setTangent(double tangent) {
setAbsoluteTangent = true;
absoluteTangent = tangent;
pushPath();
return this;
}
private TrajectorySequenceBuilder setTangentOffset(double offset) {
setAbsoluteTangent = false;
this.tangentOffset = offset;
this.pushPath();
return this;
}
public TrajectorySequenceBuilder setReversed(boolean reversed) {
return reversed ? this.setTangentOffset(Math.toRadians(180.0)) : this.setTangentOffset(0.0);
}
public TrajectorySequenceBuilder setConstraints(
TrajectoryVelocityConstraint velConstraint,
TrajectoryAccelerationConstraint accelConstraint
) {
this.currentVelConstraint = velConstraint;
this.currentAccelConstraint = accelConstraint;
return this;
}
public TrajectorySequenceBuilder resetConstraints() {
this.currentVelConstraint = this.baseVelConstraint;
this.currentAccelConstraint = this.baseAccelConstraint;
return this;
}
public TrajectorySequenceBuilder setVelConstraint(TrajectoryVelocityConstraint velConstraint) {
this.currentVelConstraint = velConstraint;
return this;
}
public TrajectorySequenceBuilder resetVelConstraint() {
this.currentVelConstraint = this.baseVelConstraint;
return this;
}
public TrajectorySequenceBuilder setAccelConstraint(TrajectoryAccelerationConstraint accelConstraint) {
this.currentAccelConstraint = accelConstraint;
return this;
}
public TrajectorySequenceBuilder resetAccelConstraint() {
this.currentAccelConstraint = this.baseAccelConstraint;
return this;
}
public TrajectorySequenceBuilder setTurnConstraint(double maxAngVel, double maxAngAccel) {
this.currentTurnConstraintMaxAngVel = maxAngVel;
this.currentTurnConstraintMaxAngAccel = maxAngAccel;
return this;
}
public TrajectorySequenceBuilder resetTurnConstraint() {
this.currentTurnConstraintMaxAngVel = baseTurnConstraintMaxAngVel;
this.currentTurnConstraintMaxAngAccel = baseTurnConstraintMaxAngAccel;
return this;
}
public TrajectorySequenceBuilder addTemporalMarker(MarkerCallback callback) {
return this.addTemporalMarker(currentDuration, callback);
}
public TrajectorySequenceBuilder UNSTABLE_addTemporalMarkerOffset(double offset, MarkerCallback callback) {
return this.addTemporalMarker(currentDuration + offset, callback);
}
public TrajectorySequenceBuilder addTemporalMarker(double time, MarkerCallback callback) {
return this.addTemporalMarker(0.0, time, callback);
}
public TrajectorySequenceBuilder addTemporalMarker(double scale, double offset, MarkerCallback callback) {
return this.addTemporalMarker(time -> scale * time + offset, callback);
}
public TrajectorySequenceBuilder addTemporalMarker(TimeProducer time, MarkerCallback callback) {
this.temporalMarkers.add(new TemporalMarker(time, callback));
return this;
}
public TrajectorySequenceBuilder addSpatialMarker(Vector2d point, MarkerCallback callback) {
this.spatialMarkers.add(new SpatialMarker(point, callback));
return this;
}
public TrajectorySequenceBuilder addDisplacementMarker(MarkerCallback callback) {
return this.addDisplacementMarker(currentDisplacement, callback);
}
public TrajectorySequenceBuilder UNSTABLE_addDisplacementMarkerOffset(double offset, MarkerCallback callback) {
return this.addDisplacementMarker(currentDisplacement + offset, callback);
}
public TrajectorySequenceBuilder addDisplacementMarker(double displacement, MarkerCallback callback) {
return this.addDisplacementMarker(0.0, displacement, callback);
}
public TrajectorySequenceBuilder addDisplacementMarker(double scale, double offset, MarkerCallback callback) {
return addDisplacementMarker((displacement -> scale * displacement + offset), callback);
}
public TrajectorySequenceBuilder addDisplacementMarker(DisplacementProducer displacement, MarkerCallback callback) {
displacementMarkers.add(new DisplacementMarker(displacement, callback));
return this;
}
public TrajectorySequenceBuilder turn(double angle) {
return turn(angle, currentTurnConstraintMaxAngVel, currentTurnConstraintMaxAngAccel);
}
public TrajectorySequenceBuilder turn(double angle, double maxAngVel, double maxAngAccel) {
pushPath();
MotionProfile turnProfile = MotionProfileGenerator.generateSimpleMotionProfile(
new MotionState(lastPose.getHeading(), 0.0, 0.0, 0.0),
new MotionState(lastPose.getHeading() + angle, 0.0, 0.0, 0.0),
maxAngVel,
maxAngAccel
);
sequenceSegments.add(new TurnSegment(lastPose, angle, turnProfile, Collections.emptyList()));
lastPose = new Pose2d(
lastPose.getX(), lastPose.getY(),
Angle.norm(lastPose.getHeading() + angle)
);
currentDuration += turnProfile.duration();
return this;
}
public TrajectorySequenceBuilder waitSeconds(double seconds) {
pushPath();
sequenceSegments.add(new WaitSegment(lastPose, seconds, Collections.emptyList()));
currentDuration += seconds;
return this;
}
public TrajectorySequenceBuilder addTrajectory(Trajectory trajectory) {
pushPath();
sequenceSegments.add(new TrajectorySegment(trajectory));
return this;
}
private void pushPath() {
if (currentTrajectoryBuilder != null) {
Trajectory builtTraj = currentTrajectoryBuilder.build();
sequenceSegments.add(new TrajectorySegment(builtTraj));
}
currentTrajectoryBuilder = null;
}
private void newPath() {
if (currentTrajectoryBuilder != null)
pushPath();
lastDurationTraj = 0.0;
lastDisplacementTraj = 0.0;
double tangent = setAbsoluteTangent ? absoluteTangent : Angle.norm(lastPose.getHeading() + tangentOffset);
currentTrajectoryBuilder = new TrajectoryBuilder(lastPose, tangent, currentVelConstraint, currentAccelConstraint, resolution);
}
public TrajectorySequence build() {
pushPath();
List<TrajectoryMarker> globalMarkers = convertMarkersToGlobal(
sequenceSegments,
temporalMarkers, displacementMarkers, spatialMarkers
);
return new TrajectorySequence(projectGlobalMarkersToLocalSegments(globalMarkers, sequenceSegments));
}
private List<TrajectoryMarker> convertMarkersToGlobal(
List<SequenceSegment> sequenceSegments,
List<TemporalMarker> temporalMarkers,
List<DisplacementMarker> displacementMarkers,
List<SpatialMarker> spatialMarkers
) {
ArrayList<TrajectoryMarker> trajectoryMarkers = new ArrayList<>();
// Convert temporal markers
for (TemporalMarker marker : temporalMarkers) {
trajectoryMarkers.add(
new TrajectoryMarker(marker.getProducer().produce(currentDuration), marker.getCallback())
);
}
// Convert displacement markers
for (DisplacementMarker marker : displacementMarkers) {
double time = displacementToTime(
sequenceSegments,
marker.getProducer().produce(currentDisplacement)
);
trajectoryMarkers.add(
new TrajectoryMarker(
time,
marker.getCallback()
)
);
}
// Convert spatial markers
for (SpatialMarker marker : spatialMarkers) {
trajectoryMarkers.add(
new TrajectoryMarker(
pointToTime(sequenceSegments, marker.getPoint()),
marker.getCallback()
)
);
}
return trajectoryMarkers;
}
private List<SequenceSegment> projectGlobalMarkersToLocalSegments(List<TrajectoryMarker> markers, List<SequenceSegment> sequenceSegments) {
if (sequenceSegments.isEmpty()) return Collections.emptyList();
double totalSequenceDuration = 0;
for (SequenceSegment segment : sequenceSegments) {
totalSequenceDuration += segment.getDuration();
}
for (TrajectoryMarker marker : markers) {
SequenceSegment segment = null;
int segmentIndex = 0;
double segmentOffsetTime = 0;
double currentTime = 0;
for (int i = 0; i < sequenceSegments.size(); i++) {
SequenceSegment seg = sequenceSegments.get(i);
double markerTime = Math.min(marker.getTime(), totalSequenceDuration);
if (currentTime + seg.getDuration() >= markerTime) {
segment = seg;
segmentIndex = i;
segmentOffsetTime = markerTime - currentTime;
break;
} else {
currentTime += seg.getDuration();
}
}
SequenceSegment newSegment = null;
if (segment instanceof WaitSegment) {
List<TrajectoryMarker> newMarkers = new ArrayList<>(segment.getMarkers());
newMarkers.addAll(sequenceSegments.get(segmentIndex).getMarkers());
newMarkers.add(new TrajectoryMarker(segmentOffsetTime, marker.getCallback()));
WaitSegment thisSegment = (WaitSegment) segment;
newSegment = new WaitSegment(thisSegment.getStartPose(), thisSegment.getDuration(), newMarkers);
} else if (segment instanceof TurnSegment) {
List<TrajectoryMarker> newMarkers = new ArrayList<>(segment.getMarkers());
newMarkers.addAll(sequenceSegments.get(segmentIndex).getMarkers());
newMarkers.add(new TrajectoryMarker(segmentOffsetTime, marker.getCallback()));
TurnSegment thisSegment = (TurnSegment) segment;
newSegment = new TurnSegment(thisSegment.getStartPose(), thisSegment.getTotalRotation(), thisSegment.getMotionProfile(), newMarkers);
} else if (segment instanceof TrajectorySegment) {
TrajectorySegment thisSegment = (TrajectorySegment) segment;
List<TrajectoryMarker> newMarkers = new ArrayList<>(thisSegment.getTrajectory().getMarkers());
newMarkers.add(new TrajectoryMarker(segmentOffsetTime, marker.getCallback()));
newSegment = new TrajectorySegment(new Trajectory(thisSegment.getTrajectory().getPath(), thisSegment.getTrajectory().getProfile(), newMarkers));
}
sequenceSegments.set(segmentIndex, newSegment);
}
return sequenceSegments;
}
// Taken from Road Runner's TrajectoryGenerator.displacementToTime() since it's private
// note: this assumes that the profile position is monotonic increasing
private Double motionProfileDisplacementToTime(MotionProfile profile, double s) {
double tLo = 0.0;
double tHi = profile.duration();
while (!(Math.abs(tLo - tHi) < 1e-6)) {
double tMid = 0.5 * (tLo + tHi);
if (profile.get(tMid).getX() > s) {
tHi = tMid;
} else {
tLo = tMid;
}
}
return 0.5 * (tLo + tHi);
}
private Double displacementToTime(List<SequenceSegment> sequenceSegments, double s) {
double currentTime = 0.0;
double currentDisplacement = 0.0;
for (SequenceSegment segment : sequenceSegments) {
if (segment instanceof TrajectorySegment) {
TrajectorySegment thisSegment = (TrajectorySegment) segment;
double segmentLength = thisSegment.getTrajectory().getPath().length();
if (currentDisplacement + segmentLength > s) {
double target = s - currentDisplacement;
double timeInSegment = motionProfileDisplacementToTime(
thisSegment.getTrajectory().getProfile(),
target
);
return currentTime + timeInSegment;
} else {
currentDisplacement += segmentLength;
currentTime += thisSegment.getTrajectory().duration();
}
} else {
currentTime += segment.getDuration();
}
}
return 0.0;
}
private Double pointToTime(List<SequenceSegment> sequenceSegments, Vector2d point) {
class ComparingPoints {
private final double distanceToPoint;
private final double totalDisplacement;
private final double thisPathDisplacement;
public ComparingPoints(double distanceToPoint, double totalDisplacement, double thisPathDisplacement) {
this.distanceToPoint = distanceToPoint;
this.totalDisplacement = totalDisplacement;
this.thisPathDisplacement = thisPathDisplacement;
}
}
List<ComparingPoints> projectedPoints = new ArrayList<>();
for (SequenceSegment segment : sequenceSegments) {
if (segment instanceof TrajectorySegment) {
TrajectorySegment thisSegment = (TrajectorySegment) segment;
double displacement = thisSegment.getTrajectory().getPath().project(point, 0.25);
Vector2d projectedPoint = thisSegment.getTrajectory().getPath().get(displacement).vec();
double distanceToPoint = point.minus(projectedPoint).norm();
double totalDisplacement = 0.0;
for (ComparingPoints comparingPoint : projectedPoints) {
totalDisplacement += comparingPoint.totalDisplacement;
}
totalDisplacement += displacement;
projectedPoints.add(new ComparingPoints(distanceToPoint, displacement, totalDisplacement));
}
}
ComparingPoints closestPoint = null;
for (ComparingPoints comparingPoint : projectedPoints) {
if (closestPoint == null) {
closestPoint = comparingPoint;
continue;
}
if (comparingPoint.distanceToPoint < closestPoint.distanceToPoint)
closestPoint = comparingPoint;
}
return displacementToTime(sequenceSegments, closestPoint.thisPathDisplacement);
}
private interface AddPathCallback {
void run();
}
}

271
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/trajectorysequence/TrajectorySequenceRunner.java Normal file
View File

@ -0,0 +1,271 @@
package org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence;
import androidx.annotation.Nullable;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.canvas.Canvas;
import com.acmerobotics.dashboard.telemetry.TelemetryPacket;
import com.acmerobotics.roadrunner.control.PIDCoefficients;
import com.acmerobotics.roadrunner.control.PIDFController;
import com.acmerobotics.roadrunner.drive.DriveSignal;
import com.acmerobotics.roadrunner.followers.TrajectoryFollower;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.profile.MotionState;
import com.acmerobotics.roadrunner.trajectory.Trajectory;
import com.acmerobotics.roadrunner.trajectory.TrajectoryMarker;
import com.acmerobotics.roadrunner.util.NanoClock;
import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.sequencesegment.SequenceSegment;
import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.sequencesegment.TrajectorySegment;
import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.sequencesegment.TurnSegment;
import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.sequencesegment.WaitSegment;
import org.firstinspires.ftc.teamcode.roadrunner.util.DashboardUtil;
import java.util.ArrayList;
import java.util.Collections;
import java.util.LinkedList;
import java.util.List;
public class TrajectorySequenceRunner {
public static String COLOR_INACTIVE_TRAJECTORY = "#4caf507a";
public static String COLOR_INACTIVE_TURN = "#7c4dff7a";
public static String COLOR_INACTIVE_WAIT = "#dd2c007a";
public static String COLOR_ACTIVE_TRAJECTORY = "#4CAF50";
public static String COLOR_ACTIVE_TURN = "#7c4dff";
public static String COLOR_ACTIVE_WAIT = "#dd2c00";
public static int POSE_HISTORY_LIMIT = 100;
private final TrajectoryFollower follower;
private final PIDFController turnController;
private final NanoClock clock;
private TrajectorySequence currentTrajectorySequence;
private double currentSegmentStartTime;
private int currentSegmentIndex;
private int lastSegmentIndex;
private Pose2d lastPoseError = new Pose2d();
List<TrajectoryMarker> remainingMarkers = new ArrayList<>();
private final FtcDashboard dashboard;
private final LinkedList<Pose2d> poseHistory = new LinkedList<>();
public TrajectorySequenceRunner(TrajectoryFollower follower, PIDCoefficients headingPIDCoefficients) {
this.follower = follower;
turnController = new PIDFController(headingPIDCoefficients);
turnController.setInputBounds(0, 2 * Math.PI);
clock = NanoClock.system();
dashboard = FtcDashboard.getInstance();
dashboard.setTelemetryTransmissionInterval(25);
}
public void followTrajectorySequenceAsync(TrajectorySequence trajectorySequence) {
currentTrajectorySequence = trajectorySequence;
currentSegmentStartTime = clock.seconds();
currentSegmentIndex = 0;
lastSegmentIndex = -1;
}
public @Nullable
DriveSignal update(Pose2d poseEstimate, Pose2d poseVelocity) {
Pose2d targetPose = null;
DriveSignal driveSignal = null;
TelemetryPacket packet = new TelemetryPacket();
Canvas fieldOverlay = packet.fieldOverlay();
SequenceSegment currentSegment = null;
if (currentTrajectorySequence != null) {
if (currentSegmentIndex >= currentTrajectorySequence.size()) {
for (TrajectoryMarker marker : remainingMarkers) {
marker.getCallback().onMarkerReached();
}
remainingMarkers.clear();
currentTrajectorySequence = null;
}
if (currentTrajectorySequence == null)
return new DriveSignal();
double now = clock.seconds();
boolean isNewTransition = currentSegmentIndex != lastSegmentIndex;
currentSegment = currentTrajectorySequence.get(currentSegmentIndex);
if (isNewTransition) {
currentSegmentStartTime = now;
lastSegmentIndex = currentSegmentIndex;
for (TrajectoryMarker marker : remainingMarkers) {
marker.getCallback().onMarkerReached();
}
remainingMarkers.clear();
remainingMarkers.addAll(currentSegment.getMarkers());
Collections.sort(remainingMarkers, (t1, t2) -> Double.compare(t1.getTime(), t2.getTime()));
}
double deltaTime = now - currentSegmentStartTime;
if (currentSegment instanceof TrajectorySegment) {
Trajectory currentTrajectory = ((TrajectorySegment) currentSegment).getTrajectory();
if (isNewTransition)
follower.followTrajectory(currentTrajectory);
if (!follower.isFollowing()) {
currentSegmentIndex++;
driveSignal = new DriveSignal();
} else {
driveSignal = follower.update(poseEstimate, poseVelocity);
lastPoseError = follower.getLastError();
}
targetPose = currentTrajectory.get(deltaTime);
} else if (currentSegment instanceof TurnSegment) {
MotionState targetState = ((TurnSegment) currentSegment).getMotionProfile().get(deltaTime);
turnController.setTargetPosition(targetState.getX());
double correction = turnController.update(poseEstimate.getHeading());
double targetOmega = targetState.getV();
double targetAlpha = targetState.getA();
lastPoseError = new Pose2d(0, 0, turnController.getLastError());
Pose2d startPose = currentSegment.getStartPose();
targetPose = startPose.copy(startPose.getX(), startPose.getY(), targetState.getX());
driveSignal = new DriveSignal(
new Pose2d(0, 0, targetOmega + correction),
new Pose2d(0, 0, targetAlpha)
);
if (deltaTime >= currentSegment.getDuration()) {
currentSegmentIndex++;
driveSignal = new DriveSignal();
}
} else if (currentSegment instanceof WaitSegment) {
lastPoseError = new Pose2d();
targetPose = currentSegment.getStartPose();
driveSignal = new DriveSignal();
if (deltaTime >= currentSegment.getDuration()) {
currentSegmentIndex++;
}
}
while (remainingMarkers.size() > 0 && deltaTime > remainingMarkers.get(0).getTime()) {
remainingMarkers.get(0).getCallback().onMarkerReached();
remainingMarkers.remove(0);
}
}
poseHistory.add(poseEstimate);
if (POSE_HISTORY_LIMIT > -1 && poseHistory.size() > POSE_HISTORY_LIMIT) {
poseHistory.removeFirst();
}
packet.put("x", poseEstimate.getX());
packet.put("y", poseEstimate.getY());
packet.put("heading (deg)", Math.toDegrees(poseEstimate.getHeading()));
packet.put("xError", getLastPoseError().getX());
packet.put("yError", getLastPoseError().getY());
packet.put("headingError (deg)", Math.toDegrees(getLastPoseError().getHeading()));
draw(fieldOverlay, currentTrajectorySequence, currentSegment, targetPose, poseEstimate);
dashboard.sendTelemetryPacket(packet);
return driveSignal;
}
private void draw(
Canvas fieldOverlay,
TrajectorySequence sequence, SequenceSegment currentSegment,
Pose2d targetPose, Pose2d poseEstimate
) {
if (sequence != null) {
for (int i = 0; i < sequence.size(); i++) {
SequenceSegment segment = sequence.get(i);
if (segment instanceof TrajectorySegment) {
fieldOverlay.setStrokeWidth(1);
fieldOverlay.setStroke(COLOR_INACTIVE_TRAJECTORY);
DashboardUtil.drawSampledPath(fieldOverlay, ((TrajectorySegment) segment).getTrajectory().getPath());
} else if (segment instanceof TurnSegment) {
Pose2d pose = segment.getStartPose();
fieldOverlay.setFill(COLOR_INACTIVE_TURN);
fieldOverlay.fillCircle(pose.getX(), pose.getY(), 2);
} else if (segment instanceof WaitSegment) {
Pose2d pose = segment.getStartPose();
fieldOverlay.setStrokeWidth(1);
fieldOverlay.setStroke(COLOR_INACTIVE_WAIT);
fieldOverlay.strokeCircle(pose.getX(), pose.getY(), 3);
}
}
}
if (currentSegment != null) {
if (currentSegment instanceof TrajectorySegment) {
Trajectory currentTrajectory = ((TrajectorySegment) currentSegment).getTrajectory();
fieldOverlay.setStrokeWidth(1);
fieldOverlay.setStroke(COLOR_ACTIVE_TRAJECTORY);
DashboardUtil.drawSampledPath(fieldOverlay, currentTrajectory.getPath());
} else if (currentSegment instanceof TurnSegment) {
Pose2d pose = currentSegment.getStartPose();
fieldOverlay.setFill(COLOR_ACTIVE_TURN);
fieldOverlay.fillCircle(pose.getX(), pose.getY(), 3);
} else if (currentSegment instanceof WaitSegment) {
Pose2d pose = currentSegment.getStartPose();
fieldOverlay.setStrokeWidth(1);
fieldOverlay.setStroke(COLOR_ACTIVE_WAIT);
fieldOverlay.strokeCircle(pose.getX(), pose.getY(), 3);
}
}
if (targetPose != null) {
fieldOverlay.setStrokeWidth(1);
fieldOverlay.setStroke("#4CAF50");
DashboardUtil.drawRobot(fieldOverlay, targetPose);
}
fieldOverlay.setStroke("#3F51B5");
DashboardUtil.drawPoseHistory(fieldOverlay, poseHistory);
fieldOverlay.setStroke("#3F51B5");
DashboardUtil.drawRobot(fieldOverlay, poseEstimate);
}
public Pose2d getLastPoseError() {
return lastPoseError;
}
public boolean isBusy() {
return currentTrajectorySequence != null;
}
}

40
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/trajectorysequence/sequencesegment/SequenceSegment.java Normal file
View File

@ -0,0 +1,40 @@
package org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.sequencesegment;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.trajectory.TrajectoryMarker;
import java.util.List;
public abstract class SequenceSegment {
private final double duration;
private final Pose2d startPose;
private final Pose2d endPose;
private final List<TrajectoryMarker> markers;
protected SequenceSegment(
double duration,
Pose2d startPose, Pose2d endPose,
List<TrajectoryMarker> markers
) {
this.duration = duration;
this.startPose = startPose;
this.endPose = endPose;
this.markers = markers;
}
public double getDuration() {
return this.duration;
}
public Pose2d getStartPose() {
return startPose;
}
public Pose2d getEndPose() {
return endPose;
}
public List<TrajectoryMarker> getMarkers() {
return markers;
}
}

20
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/trajectorysequence/sequencesegment/TrajectorySegment.java Normal file
View File

@ -0,0 +1,20 @@
package org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.sequencesegment;
import com.acmerobotics.roadrunner.trajectory.Trajectory;
import java.util.Collections;
public final class TrajectorySegment extends SequenceSegment {
private final Trajectory trajectory;
public TrajectorySegment(Trajectory trajectory) {
// Note: Markers are already stored in the `Trajectory` itself.
// This class should not hold any markers
super(trajectory.duration(), trajectory.start(), trajectory.end(), Collections.emptyList());
this.trajectory = trajectory;
}
public Trajectory getTrajectory() {
return this.trajectory;
}
}

36
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/trajectorysequence/sequencesegment/TurnSegment.java Normal file
View File

@ -0,0 +1,36 @@
package org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.sequencesegment;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.profile.MotionProfile;
import com.acmerobotics.roadrunner.trajectory.TrajectoryMarker;
import com.acmerobotics.roadrunner.util.Angle;
import java.util.List;
public final class TurnSegment extends SequenceSegment {
private final double totalRotation;
private final MotionProfile motionProfile;
public TurnSegment(Pose2d startPose, double totalRotation, MotionProfile motionProfile, List<TrajectoryMarker> markers) {
super(
motionProfile.duration(),
startPose,
new Pose2d(
startPose.getX(), startPose.getY(),
Angle.norm(startPose.getHeading() + totalRotation)
),
markers
);
this.totalRotation = totalRotation;
this.motionProfile = motionProfile;
}
public final double getTotalRotation() {
return this.totalRotation;
}
public final MotionProfile getMotionProfile() {
return this.motionProfile;
}
}

12
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/trajectorysequence/sequencesegment/WaitSegment.java Normal file
View File

@ -0,0 +1,12 @@
package org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.sequencesegment;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.trajectory.TrajectoryMarker;
import java.util.List;
public final class WaitSegment extends SequenceSegment {
public WaitSegment(Pose2d pose, double seconds, List<TrajectoryMarker> markers) {
super(seconds, pose, pose, markers);
}
}

70
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/util/AssetsTrajectoryManager.java Normal file
View File

@ -0,0 +1,70 @@
package org.firstinspires.ftc.teamcode.roadrunner.util;
import androidx.annotation.Nullable;
import com.acmerobotics.roadrunner.trajectory.Trajectory;
import com.acmerobotics.roadrunner.trajectory.TrajectoryBuilder;
import com.acmerobotics.roadrunner.trajectory.config.TrajectoryConfig;
import com.acmerobotics.roadrunner.trajectory.config.TrajectoryConfigManager;
import com.acmerobotics.roadrunner.trajectory.config.TrajectoryGroupConfig;
import org.firstinspires.ftc.robotcore.internal.system.AppUtil;
import java.io.IOException;
import java.io.InputStream;
/**
* Set of utilities for loading trajectories from assets (the plugin save location).
*/
public class AssetsTrajectoryManager {
/**
* Loads the group config.
*/
public static @Nullable
TrajectoryGroupConfig loadGroupConfig() {
try {
InputStream inputStream = AppUtil.getDefContext().getAssets().open(
"trajectory/" + TrajectoryConfigManager.GROUP_FILENAME);
return TrajectoryConfigManager.loadGroupConfig(inputStream);
} catch (IOException e) {
return null;
}
}
/**
* Loads a trajectory config with the given name.
*/
public static @Nullable TrajectoryConfig loadConfig(String name) {
try {
InputStream inputStream = AppUtil.getDefContext().getAssets().open(
"trajectory/" + name + ".yaml");
return TrajectoryConfigManager.loadConfig(inputStream);
} catch (IOException e) {
return null;
}
}
/**
* Loads a trajectory builder with the given name.
*/
public static @Nullable TrajectoryBuilder loadBuilder(String name) {
TrajectoryGroupConfig groupConfig = loadGroupConfig();
TrajectoryConfig config = loadConfig(name);
if (groupConfig == null || config == null) {
return null;
}
return config.toTrajectoryBuilder(groupConfig);
}
/**
* Loads a trajectory with the given name.
*/
public static @Nullable Trajectory load(String name) {
TrajectoryBuilder builder = loadBuilder(name);
if (builder == null) {
return null;
}
return builder.build();
}
}

45
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/util/AxesSigns.java Normal file
View File

@ -0,0 +1,45 @@
package org.firstinspires.ftc.teamcode.roadrunner.util;
/**
* IMU axes signs in the order XYZ (after remapping).
*/
public enum AxesSigns {
PPP(0b000),
PPN(0b001),
PNP(0b010),
PNN(0b011),
NPP(0b100),
NPN(0b101),
NNP(0b110),
NNN(0b111);
public final int bVal;
AxesSigns(int bVal) {
this.bVal = bVal;
}
public static AxesSigns fromBinaryValue(int bVal) {
int maskedVal = bVal & 0x07;
switch (maskedVal) {
case 0b000:
return AxesSigns.PPP;
case 0b001:
return AxesSigns.PPN;
case 0b010:
return AxesSigns.PNP;
case 0b011:
return AxesSigns.PNN;
case 0b100:
return AxesSigns.NPP;
case 0b101:
return AxesSigns.NPN;
case 0b110:
return AxesSigns.NNP;
case 0b111:
return AxesSigns.NNN;
default:
throw new IllegalStateException("Unexpected value for maskedVal: " + maskedVal);
}
}
}

8
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/util/AxisDirection.java Normal file
View File

@ -0,0 +1,8 @@
package org.firstinspires.ftc.teamcode.roadrunner.util;
/**
* A direction for an axis to be remapped to
*/
public enum AxisDirection {
POS_X, NEG_X, POS_Y, NEG_Y, POS_Z, NEG_Z
}

128
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/util/BNO055IMUUtil.java Normal file
View File

@ -0,0 +1,128 @@
package org.firstinspires.ftc.teamcode.roadrunner.util;
import com.qualcomm.hardware.bosch.BNO055IMU;
import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder;
/**
* Various utility functions for the BNO055 IMU.
*/
public class BNO055IMUUtil {
/**
* Error for attempting an illegal remapping (lhs or multiple same axes)
*/
public static class InvalidAxisRemapException extends RuntimeException {
public InvalidAxisRemapException(String detailMessage) {
super(detailMessage);
}
}
/**
* Remap BNO055 IMU axes and signs. For reference, the default order is {@link AxesOrder#XYZ}.
* Call after {@link BNO055IMU#initialize(BNO055IMU.Parameters)}. Although this isn't
* mentioned in the datasheet, the axes order appears to affect the onboard sensor fusion.
*
* Adapted from <a href="https://ftcforum.firstinspires.org/forum/ftc-technology/53812-mounting-the-revhub-vertically?p=56587#post56587">this post</a>.
* Reference the <a href="https://www.bosch-sensortec.com/media/boschsensortec/downloads/datasheets/bst-bno055-ds000.pdf">BNO055 Datasheet</a> for details.
*
* NOTE: Remapping axes can be somewhat confusing. Instead, use {@link #remapZAxis}, if appropriate.
*
* @param imu IMU
* @param order axes order
* @param signs axes signs
*/
public static void swapThenFlipAxes(BNO055IMU imu, AxesOrder order, AxesSigns signs) {
try {
// the indices correspond with the 2-bit axis encodings specified in the datasheet
int[] indices = order.indices();
// AxesSign's values align with the datasheet
int axisMapSigns = signs.bVal;
if (indices[0] == indices[1] || indices[0] == indices[2] || indices[1] == indices[2]) {
throw new InvalidAxisRemapException("Same axis cannot be included in AxesOrder twice");
}
// ensure right-handed coordinate system
boolean isXSwapped = indices[0] != 0;
boolean isYSwapped = indices[1] != 1;
boolean isZSwapped = indices[2] != 2;
boolean areTwoAxesSwapped = (isXSwapped || isYSwapped || isZSwapped)
&& (!isXSwapped || !isYSwapped || !isZSwapped);
boolean oddNumOfFlips = (((axisMapSigns >> 2) ^ (axisMapSigns >> 1) ^ axisMapSigns) & 1) == 1;
// != functions as xor
if (areTwoAxesSwapped != oddNumOfFlips) {
throw new InvalidAxisRemapException("Coordinate system is left-handed");
}
// Bit: 7 6 | 5 4 | 3 2 | 1 0 |
// reserved | z axis | y axis | x axis |
int axisMapConfig = indices[2] << 4 | indices[1] << 2 | indices[0];
// Enter CONFIG mode
imu.write8(BNO055IMU.Register.OPR_MODE, BNO055IMU.SensorMode.CONFIG.bVal & 0x0F);
Thread.sleep(100);
// Write the AXIS_MAP_CONFIG register
imu.write8(BNO055IMU.Register.AXIS_MAP_CONFIG, axisMapConfig & 0x3F);
// Write the AXIS_MAP_SIGN register
imu.write8(BNO055IMU.Register.AXIS_MAP_SIGN, axisMapSigns & 0x07);
// Switch back to the previous mode
imu.write8(BNO055IMU.Register.OPR_MODE, imu.getParameters().mode.bVal & 0x0F);
Thread.sleep(100);
} catch (InterruptedException e) {
Thread.currentThread().interrupt();
}
}
/**
* Remaps the IMU coordinate system so that the remapped +Z faces the provided
* {@link AxisDirection}. See {@link #swapThenFlipAxes} for details about the remapping.
*
* @param imu IMU
* @param direction axis direction
*/
public static void remapZAxis(BNO055IMU imu, AxisDirection direction) {
switch (direction) {
case POS_X:
swapThenFlipAxes(imu, AxesOrder.ZYX, AxesSigns.NPP);
break;
case NEG_X:
swapThenFlipAxes(imu, AxesOrder.ZYX, AxesSigns.PPN);
break;
case POS_Y:
swapThenFlipAxes(imu, AxesOrder.XZY, AxesSigns.PNP);
break;
case NEG_Y:
swapThenFlipAxes(imu, AxesOrder.XZY, AxesSigns.PPN);
break;
case POS_Z:
swapThenFlipAxes(imu, AxesOrder.XYZ, AxesSigns.PPP);
break;
case NEG_Z:
swapThenFlipAxes(imu, AxesOrder.XYZ, AxesSigns.PNN);
break;
}
}
/**
* Now deprecated due to unintuitive parameter order.
* Use {@link #swapThenFlipAxes} or {@link #remapZAxis} instead.
*
* @param imu IMU
* @param order axes order
* @param signs axes signs
*/
@Deprecated
public static void remapAxes(BNO055IMU imu, AxesOrder order, AxesSigns signs) {
AxesOrder adjustedAxesOrder = order.reverse();
int[] indices = order.indices();
int axisSignValue = signs.bVal ^ (0b100 >> indices[0]);
AxesSigns adjustedAxesSigns = AxesSigns.fromBinaryValue(axisSignValue);
swapThenFlipAxes(imu, adjustedAxesOrder, adjustedAxesSigns);
}
}

54
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/util/DashboardUtil.java Normal file
View File

@ -0,0 +1,54 @@
package org.firstinspires.ftc.teamcode.roadrunner.util;
import com.acmerobotics.dashboard.canvas.Canvas;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.geometry.Vector2d;
import com.acmerobotics.roadrunner.path.Path;
import java.util.List;
/**
* Set of helper functions for drawing Road Runner paths and trajectories on dashboard canvases.
*/
public class DashboardUtil {
private static final double DEFAULT_RESOLUTION = 2.0; // distance units; presumed inches
private static final double ROBOT_RADIUS = 9; // in
public static void drawPoseHistory(Canvas canvas, List<Pose2d> poseHistory) {
double[] xPoints = new double[poseHistory.size()];
double[] yPoints = new double[poseHistory.size()];
for (int i = 0; i < poseHistory.size(); i++) {
Pose2d pose = poseHistory.get(i);
xPoints[i] = pose.getX();
yPoints[i] = pose.getY();
}
canvas.strokePolyline(xPoints, yPoints);
}
public static void drawSampledPath(Canvas canvas, Path path, double resolution) {
int samples = (int) Math.ceil(path.length() / resolution);
double[] xPoints = new double[samples];
double[] yPoints = new double[samples];
double dx = path.length() / (samples - 1);
for (int i = 0; i < samples; i++) {
double displacement = i * dx;
Pose2d pose = path.get(displacement);
xPoints[i] = pose.getX();
yPoints[i] = pose.getY();
}
canvas.strokePolyline(xPoints, yPoints);
}
public static void drawSampledPath(Canvas canvas, Path path) {
drawSampledPath(canvas, path, DEFAULT_RESOLUTION);
}
public static void drawRobot(Canvas canvas, Pose2d pose) {
canvas.strokeCircle(pose.getX(), pose.getY(), ROBOT_RADIUS);
Vector2d v = pose.headingVec().times(ROBOT_RADIUS);
double x1 = pose.getX() + v.getX() / 2, y1 = pose.getY() + v.getY() / 2;
double x2 = pose.getX() + v.getX(), y2 = pose.getY() + v.getY();
canvas.strokeLine(x1, y1, x2, y2);
}
}

98
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/util/Encoder.java Normal file
View File

@ -0,0 +1,98 @@
package org.firstinspires.ftc.teamcode.roadrunner.util;
import com.acmerobotics.roadrunner.util.NanoClock;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
/**
* Wraps a motor instance to provide corrected velocity counts and allow reversing independently of the corresponding
* slot's motor direction
*/
public class Encoder {
private final static int CPS_STEP = 0x10000;
private static double inverseOverflow(double input, double estimate) {
double real = input;
while (Math.abs(estimate - real) > CPS_STEP / 2.0) {
real += Math.signum(estimate - real) * CPS_STEP;
}
return real;
}
public enum Direction {
FORWARD(1),
REVERSE(-1);
private int multiplier;
Direction(int multiplier) {
this.multiplier = multiplier;
}
public int getMultiplier() {
return multiplier;
}
}
private DcMotorEx motor;
private NanoClock clock;
private Direction direction;
private int lastPosition;
private double velocityEstimate;
private double lastUpdateTime;
public Encoder(DcMotorEx motor, NanoClock clock) {
this.motor = motor;
this.clock = clock;
this.direction = Direction.FORWARD;
this.lastPosition = 0;
this.velocityEstimate = 0.0;
this.lastUpdateTime = clock.seconds();
}
public Encoder(DcMotorEx motor) {
this(motor, NanoClock.system());
}
public Direction getDirection() {
return direction;
}
private int getMultiplier() {
return getDirection().getMultiplier() * (motor.getDirection() == DcMotorSimple.Direction.FORWARD ? 1 : -1);
}
/**
* Allows you to set the direction of the counts and velocity without modifying the motor's direction state
* @param direction either reverse or forward depending on if encoder counts should be negated
*/
public void setDirection(Direction direction) {
this.direction = direction;
}
public int getCurrentPosition() {
int multiplier = getMultiplier();
int currentPosition = motor.getCurrentPosition() * multiplier;
if (currentPosition != lastPosition) {
double currentTime = clock.seconds();
double dt = currentTime - lastUpdateTime;
velocityEstimate = (currentPosition - lastPosition) / dt;
lastPosition = currentPosition;
lastUpdateTime = currentTime;
}
return currentPosition;
}
public double getRawVelocity() {
int multiplier = getMultiplier();
return motor.getVelocity() * multiplier;
}
public double getCorrectedVelocity() {
return inverseOverflow(getRawVelocity(), velocityEstimate);
}
}

60
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/util/LoggingUtil.java Normal file
View File

@ -0,0 +1,60 @@
package org.firstinspires.ftc.teamcode.roadrunner.util;
import org.firstinspires.ftc.robotcore.internal.system.AppUtil;
import java.io.File;
import java.util.ArrayList;
import java.util.Collections;
import java.util.List;
/**
* Utility functions for log files.
*/
public class LoggingUtil {
public static final File ROAD_RUNNER_FOLDER =
new File(AppUtil.ROOT_FOLDER + "/RoadRunner/");
private static final long LOG_QUOTA = 25 * 1024 * 1024; // 25MB log quota for now
private static void buildLogList(List<File> logFiles, File dir) {
for (File file : dir.listFiles()) {
if (file.isDirectory()) {
buildLogList(logFiles, file);
} else {
logFiles.add(file);
}
}
}
private static void pruneLogsIfNecessary() {
List<File> logFiles = new ArrayList<>();
buildLogList(logFiles, ROAD_RUNNER_FOLDER);
Collections.sort(logFiles, (lhs, rhs) ->
Long.compare(lhs.lastModified(), rhs.lastModified()));
long dirSize = 0;
for (File file: logFiles) {
dirSize += file.length();
}
while (dirSize > LOG_QUOTA) {
if (logFiles.size() == 0) break;
File fileToRemove = logFiles.remove(0);
dirSize -= fileToRemove.length();
//noinspection ResultOfMethodCallIgnored
fileToRemove.delete();
}
}
/**
* Obtain a log file with the provided name
*/
public static File getLogFile(String name) {
//noinspection ResultOfMethodCallIgnored
ROAD_RUNNER_FOLDER.mkdirs();
pruneLogsIfNecessary();
return new File(ROAD_RUNNER_FOLDER, name);
}
}

124
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/util/LynxModuleUtil.java Normal file
View File

@ -0,0 +1,124 @@
package org.firstinspires.ftc.teamcode.roadrunner.util;
import com.qualcomm.hardware.lynx.LynxModule;
import com.qualcomm.robotcore.hardware.HardwareMap;
import org.firstinspires.ftc.robotcore.internal.system.Misc;
import java.util.HashMap;
import java.util.Map;
/**
* Collection of utilites for interacting with Lynx modules.
*/
public class LynxModuleUtil {
private static final LynxFirmwareVersion MIN_VERSION = new LynxFirmwareVersion(1, 8, 2);
/**
* Parsed representation of a Lynx module firmware version.
*/
public static class LynxFirmwareVersion implements Comparable<LynxFirmwareVersion> {
public final int major;
public final int minor;
public final int eng;
public LynxFirmwareVersion(int major, int minor, int eng) {
this.major = major;
this.minor = minor;
this.eng = eng;
}
@Override
public boolean equals(Object other) {
if (other instanceof LynxFirmwareVersion) {
LynxFirmwareVersion otherVersion = (LynxFirmwareVersion) other;
return major == otherVersion.major && minor == otherVersion.minor &&
eng == otherVersion.eng;
} else {
return false;
}
}
@Override
public int compareTo(LynxFirmwareVersion other) {
int majorComp = Integer.compare(major, other.major);
if (majorComp == 0) {
int minorComp = Integer.compare(minor, other.minor);
if (minorComp == 0) {
return Integer.compare(eng, other.eng);
} else {
return minorComp;
}
} else {
return majorComp;
}
}
@Override
public String toString() {
return Misc.formatInvariant("%d.%d.%d", major, minor, eng);
}
}
/**
* Retrieve and parse Lynx module firmware version.
* @param module Lynx module
* @return parsed firmware version
*/
public static LynxFirmwareVersion getFirmwareVersion(LynxModule module) {
String versionString = module.getNullableFirmwareVersionString();
if (versionString == null) {
return null;
}
String[] parts = versionString.split("[ :,]+");
try {
// note: for now, we ignore the hardware entry
return new LynxFirmwareVersion(
Integer.parseInt(parts[3]),
Integer.parseInt(parts[5]),
Integer.parseInt(parts[7])
);
} catch (NumberFormatException e) {
return null;
}
}
/**
* Exception indicating an outdated Lynx firmware version.
*/
public static class LynxFirmwareVersionException extends RuntimeException {
public LynxFirmwareVersionException(String detailMessage) {
super(detailMessage);
}
}
/**
* Ensure all of the Lynx modules attached to the robot satisfy the minimum requirement.
* @param hardwareMap hardware map containing Lynx modules
*/
public static void ensureMinimumFirmwareVersion(HardwareMap hardwareMap) {
Map<String, LynxFirmwareVersion> outdatedModules = new HashMap<>();
for (LynxModule module : hardwareMap.getAll(LynxModule.class)) {
LynxFirmwareVersion version = getFirmwareVersion(module);
if (version == null || version.compareTo(MIN_VERSION) < 0) {
for (String name : hardwareMap.getNamesOf(module)) {
outdatedModules.put(name, version);
}
}
}
if (outdatedModules.size() > 0) {
StringBuilder msgBuilder = new StringBuilder();
msgBuilder.append("One or more of the attached Lynx modules has outdated firmware\n");
msgBuilder.append(Misc.formatInvariant("Mandatory minimum firmware version for Road Runner: %s\n",
MIN_VERSION.toString()));
for (Map.Entry<String, LynxFirmwareVersion> entry : outdatedModules.entrySet()) {
msgBuilder.append(Misc.formatInvariant(
"\t%s: %s\n", entry.getKey(),
entry.getValue() == null ? "Unknown" : entry.getValue().toString()));
}
throw new LynxFirmwareVersionException(msgBuilder.toString());
}
}
}

5
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/CameraPosition.java Normal file
View File

@ -0,0 +1,5 @@
package org.firstinspires.ftc.teamcode.util;
public enum CameraPosition {
FRONT, BACK
}

30
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Color.java Normal file
View File

@ -0,0 +1,30 @@
package org.firstinspires.ftc.teamcode.util;
public class Color {
public double h;
public double s;
public double v;
public Color(double h, double s, double v) {
this.h = h;
this.s = s;
this.v = v;
}
public double[] get() {
return new double[]{h, s, v};
}
public double getH() {
return h;
}
public double getS() {
return s;
}
public double getV() {
return v;
}
}

81
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Configurables.java Normal file
View File

@ -0,0 +1,81 @@
package org.firstinspires.ftc.teamcode.util;
import com.qualcomm.robotcore.hardware.PIDFCoefficients;
import org.opencv.core.Point;
import org.opencv.core.Rect;
import org.opencv.core.Size;
public class Configurables {
// Robot Constants
public static double R_ARM_POWER = 0.2;
public static double R_ARM_SPEED = 20;
public static int R_ARM_DEFAULT_POS = 0;
public static int R_ARM_UP_POS = 221;
public static int R_ARM_ALMOST_DOWN_POS = 650;
public static int R_ARM_DOWN_POS = 750;
public static double R_CLAW_CLOSED = 0.13;
public static double R_CLAW_OPEN = 0.7;
public static double R_INTAKE_SPEED = 0.9;
public static double R_INTAKE_SHIELD_UP = 0.17;//0.05
public static double R_INTAKE_SHIELD_DOWN = 0.68;//0.95
public static double R_INTAKE_SHIELD_SPEED = 0.04;
public static double R_SHOOTER_GOAL_POWER = 0.66;
public static double R_SHOOTER_MID_GOAL_POWER = 0.54;
public static double R_SHOOTER_POWERSHOT_POWER = 0.57;
public static double R_PUSHER_CLOSED = 0.35;
public static double R_PUSHER_OPEN = 0.55;
public static double R_PUSHER_DELAY = 0.15;
// Auto Aim Constants
public static double AUTO_AIM_OFFSET_X = 5;
public static double AUTO_AIM_WAIT = 0.2;
public static PIDFCoefficients AUTO_AIM_PID = new PIDFCoefficients(0.009, 0.3, 0.0019, 0);
public static double AUTO_AIM_ACCEPTABLE_ERROR = 2;
public static double AUTO_AIM_MIN_POWER = 0.14;
// CV Color Threshold Constants
public static Color CAMERA_RED_GOAL_LOWER = new Color(165, 80, 80);
public static Color CAMERA_RED_GOAL_UPPER = new Color(15, 255, 255);
public static Color CAMERA_RED_POWERSHOT_LOWER = new Color(165, 80, 80);
public static Color CAMERA_RED_POWERSHOT_UPPER = new Color(15, 255, 255);
public static Color CAMERA_BLUE_GOAL_LOWER = new Color(75, 40, 80);
public static Color CAMERA_BLUE_GOAL_UPPER = new Color(120, 255, 255);
public static Color CAMERA_BLACK_GOAL_LOWER = new Color(0, 0, 0);
public static Color CAMERA_BLACK_GOAL_UPPER = new Color(360, 0, 0);
public static Color CAMERA_BLUE_POWERSHOT_LOWER = new Color(75, 30, 50);
public static Color CAMERA_BLUE_POWERSHOT_UPPER = new Color(120, 255, 255);
public static Color CAMERA_ORANGE_LOWER = new Color(0, 70, 50);
public static Color CAMERA_ORANGE_UPPER = new Color(60, 255, 255);
public static Color CAMERA_WHITE_LOWER = new Color(0, 0, 40);
public static Color CAMERA_WHITE_UPPER = new Color(180, 30, 255);
// CV Detection Constants
public static double CV_MIN_STARTERSTACK_AREA = 0;
public static double CV_MIN_STARTERSTACK_SINGLE_AREA = 0.08;
public static double CV_MIN_STARTERSTACK_QUAD_AREA = 1.3;
public static double CV_MIN_GOAL_AREA = 0;
public static double CV_MAX_GOAL_AREA = 0.3;
public static double CV_MIN_POWERSHOT_AREA = 0.001;
public static double CV_MAX_POWERSHOT_AREA = 0.05;
public static Rect CV_STARTERSTACK_LOCATION = new Rect(75, 50, 190, 90);
public static Point CV_POWERSHOT_OFFSET = new Point(-3, -20); // offset from the bottom left of the goal to the top right of the powershot box (for red)
public static Size CV_POWERSHOT_DIMENSIONS = new Size(100, 50);
public static Size CV_GOAL_PROPER_ASPECT = new Size(11, 8.5);
public static double CV_GOAL_PROPER_AREA = 1.25;
public static double CV_GOAL_ALLOWABLE_AREA_ERROR = 1;
public static double CV_GOAL_ALLOWABLE_SOLIDARITY_ERROR = 1;
public static double CV_GOAL_CUTOFF_Y_LINE = 65;
public static double CV_GOAL_PROPER_HEIGHT = 107;
public static double CV_GOAL_MIN_CONFIDENCE = 3;
public static Color CV_POWERSHOT_OFFSETS_RED = new Color(-40, -30, -19);
public static Color CV_POWERSHOT_OFFSETS_BLUE = new Color(40, 30, 19);
// Old CV Detection Constants
public static double CV_GOAL_SIDE_ALLOWABLE_Y_ERROR = 20;
public static double CV_GOAL_SIDE_ALLOWABLE_SIZE_ERROR = 100;
public static Size CV_GOAL_SIDE_ASPECT_RATIO = new Size(6.5,15.5);
public static double CV_GOAL_SIDE_ALLOWABLE_ASPECT_ERROR = 10;
}

59
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Constants.java Normal file
View File

@ -0,0 +1,59 @@
package org.firstinspires.ftc.teamcode.util;
import androidx.annotation.VisibleForTesting;
import org.firstinspires.ftc.teamcode.vision.Detection;
import org.opencv.core.Mat;
import org.opencv.core.Point;
import org.opencv.core.Scalar;
import org.opencv.core.Size;
import org.opencv.imgproc.Imgproc;
import org.openftc.easyopencv.OpenCvCameraRotation;
public class Constants {
// CV Color Constants
public static Scalar RED = new Scalar(255, 0, 0);
public static Scalar GREEN = new Scalar(0, 255, 0);
public static Scalar BLUE = new Scalar(0, 0, 255);
public static Scalar WHITE = new Scalar(255, 255, 255);
public static Scalar GRAY = new Scalar(80, 80, 80);
public static Scalar BLACK = new Scalar(0, 0, 0);
public static Scalar ORANGE = new Scalar(255, 165, 0);
public static Scalar YELLOW = new Scalar(255, 255, 0);
public static Scalar PURPLE = new Scalar(128, 0, 128);
// CV Structuring Constants
public static final Mat STRUCTURING_ELEMENT = Imgproc.getStructuringElement(Imgproc.MORPH_RECT, new Size(5, 5));
public static final Point ANCHOR = new Point((STRUCTURING_ELEMENT.cols() / 2f), STRUCTURING_ELEMENT.rows() / 2f);
public static final int ERODE_DILATE_ITERATIONS = 2;
public static final Size BLUR_SIZE = new Size(7, 7);
// CV Camera Constants
public static final int WEBCAM_WIDTH = 320;
public static final int WEBCAM_HEIGHT = 240;
public static final OpenCvCameraRotation WEBCAM_ROTATION = OpenCvCameraRotation.UPRIGHT;
// CV Invalid Detection Constants
public static final Point INVALID_POINT = new Point(Double.MIN_VALUE, Double.MIN_VALUE);
public static final double INVALID_AREA = -1;
public static final Detection INVALID_DETECTION = new Detection(new Size(0, 0), 0);
// Hardware Name Constants
public static final String WHEEL_FRONT_LEFT = "frontLeft";
public static final String WHEEL_FRONT_RIGHT = "frontRight";
public static final String WHEEL_BACK_LEFT = "backLeft";
public static final String WHEEL_BACK_RIGHT = "backRight";
public static final String ARM = "wobbler";
public static final String CLAW = "claw";
public static final String INTAKE = "intake";
public static final String INTAKE_SECONDARY = "secondary";
public static final String INTAKE_SHIELD = "shield";
public static final String SHOOTER = "wheel";
public static final String PUSHER = "pusher";
public static final String STACK_WEBCAM = "Stack Webcam";
public static final String TARGETING_WEBCAM = "Targeting Webcam";
public static final String IMU_SENSOR = "imu";
public static final String lServo = "lServo";
public static final String rServo = "rServo";
public static final String droneLauncher = "droneLauncher";
}

153
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/OpenCVUtil.java Normal file
View File

@ -0,0 +1,153 @@
package org.firstinspires.ftc.teamcode.util;
import org.opencv.core.Mat;
import org.opencv.core.MatOfInt;
import org.opencv.core.MatOfPoint;
import org.opencv.core.Point;
import org.opencv.core.Rect;
import org.opencv.core.Scalar;
import org.opencv.imgproc.Imgproc;
import org.opencv.imgproc.Moments;
import java.util.Collections;
import java.util.List;
import java.util.Locale;
import static org.firstinspires.ftc.teamcode.util.Configurables.CAMERA_BLUE_GOAL_LOWER;
import static org.firstinspires.ftc.teamcode.util.Configurables.CAMERA_BLUE_GOAL_UPPER;
import static org.firstinspires.ftc.teamcode.util.Configurables.CAMERA_RED_GOAL_LOWER;
import static org.firstinspires.ftc.teamcode.util.Configurables.CAMERA_RED_GOAL_UPPER;
import static org.firstinspires.ftc.teamcode.util.Configurables.CV_GOAL_ALLOWABLE_AREA_ERROR;
import static org.firstinspires.ftc.teamcode.util.Configurables.CV_GOAL_SIDE_ALLOWABLE_ASPECT_ERROR;
import static org.firstinspires.ftc.teamcode.util.Configurables.CV_GOAL_SIDE_ALLOWABLE_SIZE_ERROR;
import static org.firstinspires.ftc.teamcode.util.Configurables.CV_GOAL_ALLOWABLE_SOLIDARITY_ERROR;
import static org.firstinspires.ftc.teamcode.util.Configurables.CV_GOAL_SIDE_ALLOWABLE_Y_ERROR;
import static org.firstinspires.ftc.teamcode.util.Configurables.CV_GOAL_SIDE_ASPECT_RATIO;
import static org.firstinspires.ftc.teamcode.util.Configurables.CV_GOAL_MIN_CONFIDENCE;
import static org.firstinspires.ftc.teamcode.util.Configurables.CV_GOAL_PROPER_AREA;
import static org.firstinspires.ftc.teamcode.util.Configurables.CV_GOAL_PROPER_ASPECT;
import static org.firstinspires.ftc.teamcode.util.Configurables.CV_GOAL_PROPER_HEIGHT;
import static org.firstinspires.ftc.teamcode.util.Configurables.CV_MAX_GOAL_AREA;
import static org.firstinspires.ftc.teamcode.util.Configurables.CV_MIN_GOAL_AREA;
// CV Helper Functions
public class OpenCVUtil {
public static String telem = "nothing";
// Draw a point
public static void drawPoint(Mat img, Point point, Scalar color) {
Imgproc.circle(img, point, 3, color, -1);
}
// Get the center of a contour
public static Point getCenterOfContour(MatOfPoint contour) {
Moments moments = Imgproc.moments(contour);
return new Point(moments.m10 / moments.m00, moments.m01/ moments.m00);
}
// Get the bottom left of a contour
public static Point getBottomLeftOfContour(MatOfPoint contour) {
Rect boundingRect = Imgproc.boundingRect(contour);
return new Point(boundingRect.x, boundingRect.y+boundingRect.height);
}
// Get the bottom right of a contour
public static Point getBottomRightOfContour(MatOfPoint contour) {
Rect boundingRect = Imgproc.boundingRect(contour);
return new Point(boundingRect.x+boundingRect.width, boundingRect.y+boundingRect.height);
}
// Draw a contour
public static void drawContour(Mat img, MatOfPoint contour, Scalar color) {
Imgproc.drawContours(img, Collections.singletonList(contour), 0, color, 2);
}
// Draw a convex hull around a contour
public static void drawConvexHull(Mat img, MatOfPoint contour, Scalar color) {
MatOfInt hull = new MatOfInt();
Imgproc.convexHull(contour, hull);
Imgproc.drawContours(img, Collections.singletonList(convertIndexesToPoints(contour, hull)), 0, color, 2);
}
// Draw a filled in convex hull around a contour
public static void fillConvexHull(Mat img, MatOfPoint contour, Scalar color) {
MatOfInt hull = new MatOfInt();
Imgproc.convexHull(contour, hull);
Imgproc.drawContours(img, Collections.singletonList(convertIndexesToPoints(contour, hull)), 0, color, -1);
}
// Convert indexes to points that is used in order to draw the contours
public static MatOfPoint convertIndexesToPoints(MatOfPoint contour, MatOfInt indexes) {
int[] arrIndex = indexes.toArray();
Point[] arrContour = contour.toArray();
Point[] arrPoints = new Point[arrIndex.length];
for (int i=0;i<arrIndex.length;i++) {
arrPoints[i] = arrContour[arrIndex[i]];
}
MatOfPoint hull = new MatOfPoint();
hull.fromArray(arrPoints);
return hull;
}
// Get the largest contour out of a list
public static MatOfPoint getLargestContour(List<MatOfPoint> contours) {
if (contours.size() == 0) {
return null;
}
return getLargestContours(contours, 1).get(0);
}
// Get the top largest contours
public static List<MatOfPoint> getLargestContours(List<MatOfPoint> contours, int numContours) {
Collections.sort(contours, (a, b) -> (int) Imgproc.contourArea(b) - (int) Imgproc.contourArea(a));
return contours.subList(0, Math.min(numContours, contours.size()));
}
public static MatOfPoint getHighGoalContour(List<MatOfPoint> contours) {
Collections.sort(contours, (a, b) -> (int) Imgproc.contourArea(b) - (int) Imgproc.contourArea(a));
// return null if nothing
if (contours.size() == 0) {
return null;
}
// check each contour for touching the top, aspect, and size
double properAspect = ((double) CV_GOAL_SIDE_ASPECT_RATIO.height) / ((double) CV_GOAL_SIDE_ASPECT_RATIO.width);
for (int i = 0; i < contours.size() - 1; i++) {
MatOfPoint contour = contours.get(i);
Rect rect = Imgproc.boundingRect(contour);
double area = Imgproc.contourArea(contour);
double aspect = ((double) rect.height) / ((double) rect.width);
if (rect.y <= -100 || Math.abs(properAspect - aspect) > CV_GOAL_SIDE_ALLOWABLE_ASPECT_ERROR ||
area < CV_MIN_GOAL_AREA || area > CV_MAX_GOAL_AREA) {
contours.remove(i);
i--;
}
}
// check for 2 that can be combined
int goalCounter = -1;
for (int i = 0; i < contours.size() - 1; i++) {
MatOfPoint contour1 = contours.get(i);
MatOfPoint contour2 = contours.get(i + 1);
Rect rect1 = Imgproc.boundingRect(contour1);
Rect rect2 = Imgproc.boundingRect(contour2);
double area1 = Imgproc.contourArea(contour1);
double area2 = Imgproc.contourArea(contour2);
if (Math.abs(Math.abs(rect1.y) - Math.abs(rect2.y)) < CV_GOAL_SIDE_ALLOWABLE_Y_ERROR &&
Math.abs(area1 - area2) < CV_GOAL_SIDE_ALLOWABLE_SIZE_ERROR) {
goalCounter = i;
break;
}
}
// return the results
if (goalCounter == -1) {
return contours.get(0);
} else {
MatOfPoint highGoal = new MatOfPoint();
highGoal.push_back(contours.get(goalCounter));
highGoal.push_back(contours.get(goalCounter + 1));
return highGoal;
}
}
}

194
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/AprilTagDetectionPipeline.java Normal file
View File

@ -0,0 +1,194 @@
package org.firstinspires.ftc.teamcode.vision;
import static org.firstinspires.ftc.teamcode.vision.OpenCVUtil.LEFT_BOUNDARY_APRILTAG;
import static org.firstinspires.ftc.teamcode.vision.OpenCVUtil.RIGHT_BOUNDARY_APRILTAG;
import org.opencv.calib3d.Calib3d;
import org.opencv.core.Mat;
import org.opencv.core.MatOfDouble;
import org.opencv.core.MatOfPoint2f;
import org.opencv.core.MatOfPoint3f;
import org.opencv.core.Point;
import org.opencv.core.Point3;
import org.opencv.core.Scalar;
import org.opencv.imgproc.Imgproc;
import org.openftc.apriltag.AprilTagDetection;
import org.openftc.apriltag.AprilTagDetectorJNI;
import org.openftc.easyopencv.OpenCvPipeline;
import java.util.ArrayList;
public class AprilTagDetectionPipeline extends OpenCvPipeline {
double fx;
double fy;
double cx;
double cy;
double tagsize;
double tagsizeX;
double tagsizeY;
Mat cameraMatrix;
private float decimation;
private Mat grey = new Mat();
Scalar blue = new Scalar(7,197,235,255);
Scalar red = new Scalar(255,0,0,255);
Scalar green = new Scalar(0,255,0,255);
Scalar white = new Scalar(255,255,255,255);
private long nativeApriltagPtr;
private boolean needToSetDecimation;
private final Object decimationSync = new Object();
private final Object detectionsUpdateSync = new Object();
private ArrayList<org.openftc.apriltag.AprilTagDetection> detectionsUpdate = new ArrayList<>();
private ArrayList<org.openftc.apriltag.AprilTagDetection> aprilTagDetections = new ArrayList<>();
@Override
public Mat processFrame(Mat input)
{
// Convert to greyscale
Imgproc.cvtColor(input, grey, Imgproc.COLOR_RGBA2GRAY);
synchronized (decimationSync)
{
if(needToSetDecimation)
{
AprilTagDetectorJNI.setApriltagDetectorDecimation(nativeApriltagPtr, decimation);
needToSetDecimation = false;
}
}
// Run AprilTag
aprilTagDetections = AprilTagDetectorJNI.runAprilTagDetectorSimple(nativeApriltagPtr, grey, tagsize, fx, fy, cx, cy);
synchronized (detectionsUpdateSync)
{
detectionsUpdate = aprilTagDetections;
}
for(AprilTagDetection detection : aprilTagDetections)
{
Pose pose = poseFromTrapezoid(detection.corners, cameraMatrix, tagsizeX, tagsizeY);
drawAxisMarker(input, tagsizeY/2.0, 6, pose.rvec, pose.tvec, cameraMatrix);
draw3dCubeMarker(input, tagsizeX, tagsizeX, tagsizeY, 5, pose.rvec, pose.tvec, cameraMatrix);
}
Imgproc.line(input, new Point(LEFT_BOUNDARY_APRILTAG,0), new Point(LEFT_BOUNDARY_APRILTAG,800), green, 2);
Imgproc.line(input, new Point(RIGHT_BOUNDARY_APRILTAG,0), new Point(RIGHT_BOUNDARY_APRILTAG,800), green, 2);
return input;
}
Pose poseFromTrapezoid(Point[] points, Mat cameraMatrix, double tagsizeX , double tagsizeY)
{
// The actual 2d points of the tag detected in the image
MatOfPoint2f points2d = new MatOfPoint2f(points);
// The 3d points of the tag in an 'ideal projection'
Point3[] arrayPoints3d = new Point3[4];
arrayPoints3d[0] = new Point3(-tagsizeX/2, tagsizeY/2, 0);
arrayPoints3d[1] = new Point3(tagsizeX/2, tagsizeY/2, 0);
arrayPoints3d[2] = new Point3(tagsizeX/2, -tagsizeY/2, 0);
arrayPoints3d[3] = new Point3(-tagsizeX/2, -tagsizeY/2, 0);
MatOfPoint3f points3d = new MatOfPoint3f(arrayPoints3d);
// Using this information, actually solve for pose
Pose pose = new Pose();
Calib3d.solvePnP(points3d, points2d, cameraMatrix, new MatOfDouble(), pose.rvec, pose.tvec, false);
return pose;
}
void drawAxisMarker(Mat buf, double length, int thickness, Mat rvec, Mat tvec, Mat cameraMatrix)
{
// The points in 3D space we wish to project onto the 2D image plane.
// The origin of the coordinate space is assumed to be in the center of the detection.
MatOfPoint3f axis = new MatOfPoint3f(
new Point3(0,0,0),
new Point3(length,0,0),
new Point3(0,length,0),
new Point3(0,0,-length)
);
// Project those points
MatOfPoint2f matProjectedPoints = new MatOfPoint2f();
Calib3d.projectPoints(axis, rvec, tvec, cameraMatrix, new MatOfDouble(), matProjectedPoints);
Point[] projectedPoints = matProjectedPoints.toArray();
// Draw the marker!
Imgproc.line(buf, projectedPoints[0], projectedPoints[1], red, thickness);
Imgproc.line(buf, projectedPoints[0], projectedPoints[2], green, thickness);
Imgproc.line(buf, projectedPoints[0], projectedPoints[3], blue, thickness);
Imgproc.circle(buf, projectedPoints[0], thickness, white, -1);
}
void draw3dCubeMarker(Mat buf, double length, double tagWidth, double tagHeight, int thickness, Mat rvec, Mat tvec, Mat cameraMatrix)
{
//axis = np.float32([[0,0,0], [0,3,0], [3,3,0], [3,0,0],
// [0,0,-3],[0,3,-3],[3,3,-3],[3,0,-3] ])
// The points in 3D space we wish to project onto the 2D image plane.
// The origin of the coordinate space is assumed to be in the center of the detection.
MatOfPoint3f axis = new MatOfPoint3f(
new Point3(-tagWidth/2, tagHeight/2,0),
new Point3( tagWidth/2, tagHeight/2,0),
new Point3( tagWidth/2,-tagHeight/2,0),
new Point3(-tagWidth/2,-tagHeight/2,0),
new Point3(-tagWidth/2, tagHeight/2,-length),
new Point3( tagWidth/2, tagHeight/2,-length),
new Point3( tagWidth/2,-tagHeight/2,-length),
new Point3(-tagWidth/2,-tagHeight/2,-length));
// Project those points
MatOfPoint2f matProjectedPoints = new MatOfPoint2f();
Calib3d.projectPoints(axis, rvec, tvec, cameraMatrix, new MatOfDouble(), matProjectedPoints);
Point[] projectedPoints = matProjectedPoints.toArray();
// Pillars
for(int i = 0; i < 4; i++)
{
Imgproc.line(buf, projectedPoints[i], projectedPoints[i+4], blue, thickness);
}
// Base lines
//Imgproc.line(buf, projectedPoints[0], projectedPoints[1], blue, thickness);
//Imgproc.line(buf, projectedPoints[1], projectedPoints[2], blue, thickness);
//Imgproc.line(buf, projectedPoints[2], projectedPoints[3], blue, thickness);
//Imgproc.line(buf, projectedPoints[3], projectedPoints[0], blue, thickness);
// Top lines
Imgproc.line(buf, projectedPoints[4], projectedPoints[5], green, thickness);
Imgproc.line(buf, projectedPoints[5], projectedPoints[6], green, thickness);
Imgproc.line(buf, projectedPoints[6], projectedPoints[7], green, thickness);
Imgproc.line(buf, projectedPoints[4], projectedPoints[7], green, thickness);
}
public ArrayList<AprilTagDetection> getLatestDetections()
{
return aprilTagDetections;
}
public void setDecimation(float decimation)
{
synchronized (decimationSync)
{
this.decimation = decimation;
needToSetDecimation = true;
}
}
class Pose
{
Mat rvec;
Mat tvec;
public Pose(){
rvec = new Mat();
tvec = new Mat();
}
public Pose(Mat rvec, Mat tvec)
{
this.rvec = rvec;
this.tvec = tvec;
}
}
}

136
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/Detection.java Normal file
View File

@ -0,0 +1,136 @@
package org.firstinspires.ftc.teamcode.vision;
import org.opencv.core.Mat;
import org.opencv.core.MatOfPoint;
import org.opencv.core.Point;
import org.opencv.core.Scalar;
import org.opencv.core.Size;
import org.opencv.imgproc.Imgproc;
import static org.firstinspires.ftc.teamcode.util.Constants.GREEN;
import static org.firstinspires.ftc.teamcode.util.Constants.INVALID_AREA;
import static org.firstinspires.ftc.teamcode.util.Constants.INVALID_POINT;
import static org.firstinspires.ftc.teamcode.util.OpenCVUtil.drawConvexHull;
import static org.firstinspires.ftc.teamcode.util.OpenCVUtil.drawPoint;
import static org.firstinspires.ftc.teamcode.util.OpenCVUtil.fillConvexHull;
import static org.firstinspires.ftc.teamcode.util.OpenCVUtil.getBottomLeftOfContour;
import static org.firstinspires.ftc.teamcode.util.OpenCVUtil.getBottomRightOfContour;
import static org.firstinspires.ftc.teamcode.util.OpenCVUtil.getCenterOfContour;
// Class for a Detection
public class Detection {
private double minAreaPx;
private double maxAreaPx;
private final Size maxSizePx;
private double areaPx = INVALID_AREA;
private Point centerPx = INVALID_POINT;
private Point bottomLeftPx = INVALID_POINT;
private Point bottomRightPx = INVALID_POINT;
private MatOfPoint contour;
// Constructor
public Detection(Size frameSize, double minAreaFactor) {
this.maxSizePx = frameSize;
this.minAreaPx = frameSize.area() * minAreaFactor;
this.maxAreaPx = frameSize.area();
}
public Detection(Size frameSize, double minAreaFactor, double maxSizeFactor) {
this.maxSizePx = frameSize;
this.minAreaPx = frameSize.area() * minAreaFactor;
this.maxAreaPx = frameSize.area() * maxSizeFactor;
}
public void setMinArea(double minAreaFactor) {
this.minAreaPx = maxSizePx.area() * minAreaFactor;
}
public void setMaxArea(double maxAreaFactor) {
this.minAreaPx = maxSizePx.area() * maxAreaFactor;
}
// Draw a convex hull around the current detection on the given image
public void draw(Mat img, Scalar color) {
if (isValid()) {
drawConvexHull(img, contour, color);
drawPoint(img, centerPx, GREEN);
// drawPoint(img, bottomLeftPx, GREEN);
// drawPoint(img, bottomRightPx, GREEN);
}
}
// Draw a convex hull around the current detection on the given image
public void fill(Mat img, Scalar color) {
if (isValid()) {
fillConvexHull(img, contour, color);
drawPoint(img, centerPx, GREEN);
// drawPoint(img, bottomLeftPx, GREEN);
// drawPoint(img, bottomRightPx, GREEN);
}
}
// Check if the current Detection is valid
public boolean isValid() {
// return true;
return (this.contour != null) && (this.centerPx != INVALID_POINT) && (this.areaPx != INVALID_AREA);
}
// Get the current contour
public MatOfPoint getContour() {
return contour;
}
// Set the values of the current contour
public void setContour(MatOfPoint contour) {
this.contour = contour;
double area;
if (contour != null && (area = Imgproc.contourArea(contour)) > minAreaPx && area < maxAreaPx) {
this.areaPx = area;
this.centerPx = getCenterOfContour(contour);
this.bottomLeftPx = getBottomLeftOfContour(contour);
this.bottomRightPx = getBottomRightOfContour(contour);
} else {
this.areaPx = INVALID_AREA;
this.centerPx = INVALID_POINT;
this.bottomLeftPx = INVALID_POINT;
this.bottomRightPx = INVALID_POINT;
}
}
// Returns the center of the Detection, normalized so that the width and height of the frame is from [-50,50]
public Point getCenter() {
if (!isValid()) {
return INVALID_POINT;
}
double normalizedX = ((centerPx.x / maxSizePx.width) * 100) - 50;
double normalizedY = ((centerPx.y / maxSizePx.height) * -100) + 50;
return new Point(normalizedX, normalizedY);
}
// Get the center point in pixels
public Point getCenterPx() {
return centerPx;
}
// Get the area of the Detection, normalized so that the area of the frame is 100
public double getArea() {
if (!isValid()) {
return INVALID_AREA;
}
return (areaPx / (maxSizePx.width * maxSizePx.height)) * 100;
}
// Get the leftmost bottom corner of the detection
public Point getBottomLeftCornerPx() {
return bottomLeftPx;
}
// Get the rightmost bottom corner of the detection
public Point getBottomRightCornerPx() {
return bottomRightPx;
}
}

109
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/OpenCVUtil.java Normal file
View File

@ -0,0 +1,109 @@
package org.firstinspires.ftc.teamcode.vision;
import org.firstinspires.ftc.teamcode.util.Color;
import org.opencv.core.Mat;
import org.opencv.core.MatOfInt;
import org.opencv.core.MatOfPoint;
import org.opencv.core.MatOfPoint2f;
import org.opencv.core.Point;
import org.opencv.core.Rect;
import org.opencv.core.RotatedRect;
import org.opencv.core.Scalar;
import org.opencv.imgproc.Imgproc;
import org.opencv.imgproc.Moments;
import java.util.ArrayList;
import java.util.Collections;
import java.util.List;
public class OpenCVUtil {
public static Color YELLOW_LOWER = new Color(38, 25, 0);
public static Color YELLOW_UPPER = new Color(105, 300, 300);
public static int LEFT_BOUNDARY = -25;
public static int RIGHT_BOUNDARY = 25;
public static int LEFT_BOUNDARY_APRILTAG = 0;
public static int RIGHT_BOUNDARY_APRILTAG = 800;
// Draw a point
// Draw a point
public static void drawPoint(Mat img, Point point, Scalar color, int radiusPx) {
Imgproc.circle(img, point, radiusPx, color, -1);
}
// Get the center of a contour
public static Point getCenterOfContour(MatOfPoint contour) {
Moments moments = Imgproc.moments(contour);
return new Point(moments.m10 / moments.m00, moments.m01/ moments.m00);
}
// Get the bottom left of a contour
public static Point getBottomLeftOfContour(MatOfPoint contour) {
Rect boundingRect = Imgproc.boundingRect(contour);
return new Point(boundingRect.x, boundingRect.y+boundingRect.height);
}
// Get the bottom right of a contour
public static Point getBottomRightOfContour(MatOfPoint contour) {
Rect boundingRect = Imgproc.boundingRect(contour);
return new Point(boundingRect.x+boundingRect.width, boundingRect.y+boundingRect.height);
}
// Draw a contour
public static void drawContour(Mat img, MatOfPoint contour, Scalar color) {
Imgproc.drawContours(img, Collections.singletonList(contour), 0, color, 2);
}
// Draw a convex hull around a contour
public static void drawConvexHull(Mat img, MatOfPoint contour, Scalar color) {
MatOfInt hull = new MatOfInt();
Imgproc.convexHull(contour, hull);
Imgproc.drawContours(img, Collections.singletonList(convertIndexesToPoints(contour, hull)), 0, color, 2);
}
// Draw a filled in convex hull around a contour
public static void fillConvexHull(Mat img, MatOfPoint contour, Scalar color) {
MatOfInt hull = new MatOfInt();
Imgproc.convexHull(contour, hull);
Imgproc.drawContours(img, Collections.singletonList(convertIndexesToPoints(contour, hull)), 0, color, -1);
}
// Convert indexes to points that is used in order to draw the contours
public static MatOfPoint convertIndexesToPoints(MatOfPoint contour, MatOfInt indexes) {
int[] arrIndex = indexes.toArray();
Point[] arrContour = contour.toArray();
Point[] arrPoints = new Point[arrIndex.length];
for (int i=0;i<arrIndex.length;i++) {
arrPoints[i] = arrContour[arrIndex[i]];
}
MatOfPoint hull = new MatOfPoint();
hull.fromArray(arrPoints);
return hull;
}
// Get the largest contour out of a list
public static MatOfPoint getLargestContour(List<MatOfPoint> contours) {
if (contours.size() == 0) {
return null;
}
return getLargestContours(contours, 1).get(0);
}
// Get the top largest contours
public static List<MatOfPoint> getLargestContours(List<MatOfPoint> contours, int numContours) {
Collections.sort(contours, (a, b) -> (int) Imgproc.contourArea(b) - (int) Imgproc.contourArea(a));
return contours.subList(0, Math.min(numContours, contours.size()));
}
public static void drawAngledRect(Mat img, MatOfPoint contour, Scalar color, boolean fill) {
RotatedRect rect = Imgproc.minAreaRect(new MatOfPoint2f(contour.toArray()));
Point[] vertices = new Point[4];
rect.points(vertices);
List<MatOfPoint> boxContours = new ArrayList<>();
boxContours.add(new MatOfPoint(vertices));
Imgproc.drawContours(img, boxContours, 0, color, fill ? -1 : 2);
}
}

151
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/TargetingPipeline.java Normal file
View File

@ -0,0 +1,151 @@
package org.firstinspires.ftc.teamcode.vision;
import static org.firstinspires.ftc.teamcode.util.Configurables.CAMERA_BLACK_GOAL_LOWER;
import static org.firstinspires.ftc.teamcode.util.Configurables.CAMERA_BLUE_GOAL_LOWER;
import static org.firstinspires.ftc.teamcode.util.Configurables.CAMERA_BLUE_GOAL_UPPER;
import static org.firstinspires.ftc.teamcode.util.Configurables.CAMERA_RED_GOAL_LOWER;
import static org.firstinspires.ftc.teamcode.util.Configurables.CAMERA_RED_GOAL_UPPER;
import static org.firstinspires.ftc.teamcode.util.Configurables.CV_MAX_GOAL_AREA;
import static org.firstinspires.ftc.teamcode.util.Configurables.CV_MIN_GOAL_AREA;
import static org.firstinspires.ftc.teamcode.util.Constants.ANCHOR;
import static org.firstinspires.ftc.teamcode.util.Constants.BLACK;
import static org.firstinspires.ftc.teamcode.util.Constants.BLUE;
import static org.firstinspires.ftc.teamcode.util.Constants.BLUR_SIZE;
import static org.firstinspires.ftc.teamcode.util.Constants.ERODE_DILATE_ITERATIONS;
import static org.firstinspires.ftc.teamcode.util.Constants.RED;
import static org.firstinspires.ftc.teamcode.util.Constants.STRUCTURING_ELEMENT;
import static org.firstinspires.ftc.teamcode.util.OpenCVUtil.getLargestContour;
import org.firstinspires.ftc.teamcode.util.Color;
import org.opencv.core.Core;
import org.opencv.core.Mat;
import org.opencv.core.MatOfPoint;
import org.opencv.core.Scalar;
import org.opencv.imgproc.Imgproc;
import org.openftc.easyopencv.OpenCvPipeline;
import java.util.ArrayList;
public class TargetingPipeline extends OpenCvPipeline {
Mat blurred = new Mat();
Mat hsv = new Mat();
Mat redMask1 = new Mat();
Mat redMask2 = new Mat();
Mat redMask = new Mat();
Mat blueMask = new Mat();
Mat blackMask = new Mat();
Scalar redGoalLower1;
Scalar redGoalUpper1;
Scalar redGoalLower2;
Scalar redGoalUpper2;
Scalar blueGoalLower1;
Scalar blueGoalUpper1;
Scalar blueGoalLower2;
Scalar blueGoalUpper2;
Scalar blackGoalLower1;
Scalar blackGoalUpper1;
Scalar blackGoalLower2;
Scalar blackGoalUpper2;
private Detection red;
private Detection blue;
private Detection black;
// Init
@Override
public void init(Mat input) {
red = new Detection(input.size(), CV_MIN_GOAL_AREA);
blue = new Detection(input.size(), CV_MIN_GOAL_AREA);
black = new Detection(input.size(), CV_MIN_GOAL_AREA);
}
// Process each frame that is received from the webcam
@Override
public Mat processFrame(Mat input) {
Imgproc.GaussianBlur(input, blurred, BLUR_SIZE, 0);
Imgproc.cvtColor(blurred, hsv, Imgproc.COLOR_RGB2HSV);
updateRed(input);
updateBlue(input);
updateBlack(input);
return input;
}
// Update the Red Goal Detection
private void updateRed(Mat input) {
// take pixels that are in the color range and put them into a mask, eroding and dilating them to remove white noise
redGoalLower1 = new Scalar(CAMERA_RED_GOAL_LOWER.getH(), CAMERA_RED_GOAL_LOWER.getS(), CAMERA_RED_GOAL_LOWER.getV());
redGoalUpper1 = new Scalar(180, CAMERA_RED_GOAL_UPPER.getS(), CAMERA_RED_GOAL_UPPER.getV());
redGoalLower2 = new Scalar(0, CAMERA_RED_GOAL_LOWER.getS(), CAMERA_RED_GOAL_LOWER.getV());
redGoalUpper2 = new Scalar(CAMERA_RED_GOAL_UPPER.getH(), CAMERA_RED_GOAL_UPPER.getS(), CAMERA_RED_GOAL_UPPER.getV());
Core.inRange(hsv, redGoalLower1, redGoalUpper1, redMask1);
Core.inRange(hsv, redGoalLower2, redGoalUpper2, redMask2);
Core.add(redMask1, redMask2, redMask);
Imgproc.erode(redMask, redMask, STRUCTURING_ELEMENT, ANCHOR, ERODE_DILATE_ITERATIONS);
Imgproc.dilate(redMask, redMask, STRUCTURING_ELEMENT, ANCHOR, ERODE_DILATE_ITERATIONS);
ArrayList<MatOfPoint> contours = new ArrayList<>();
Imgproc.findContours(redMask, contours, new Mat(), Imgproc.RETR_LIST, Imgproc.CHAIN_APPROX_SIMPLE);
for (int i = 0; i < contours.size(); i++) {
Detection newDetection = new Detection(input.size(), CV_MIN_GOAL_AREA, CV_MAX_GOAL_AREA);
newDetection.setContour(contours.get(i));
newDetection.draw(input, RED);
}
red.setContour(getLargestContour(contours));
red.fill(input, RED);
}
private void updateBlue(Mat input) {
// take pixels that are in the color range and put them into a mask, eroding and dilating them to remove white noise
blueGoalLower1 = new Scalar(CAMERA_BLUE_GOAL_LOWER.getH(), CAMERA_BLUE_GOAL_LOWER.getS(), CAMERA_BLUE_GOAL_LOWER.getV());
blueGoalUpper1 = new Scalar(180, CAMERA_BLUE_GOAL_UPPER.getS(), CAMERA_BLUE_GOAL_UPPER.getV());
blueGoalLower2 = new Scalar(0, CAMERA_BLUE_GOAL_LOWER.getS(), CAMERA_BLUE_GOAL_LOWER.getV());
blueGoalUpper2 = new Scalar(CAMERA_BLUE_GOAL_UPPER.getH(), CAMERA_BLUE_GOAL_UPPER.getS(), CAMERA_BLUE_GOAL_UPPER.getV());
Core.inRange(hsv, blueGoalLower1, blueGoalUpper1, blueMask);
Core.inRange(hsv, blueGoalLower2, blueGoalUpper2, blueMask);
Imgproc.erode(blueMask, blueMask, STRUCTURING_ELEMENT, ANCHOR, ERODE_DILATE_ITERATIONS);
Imgproc.dilate(blueMask, blueMask, STRUCTURING_ELEMENT, ANCHOR, ERODE_DILATE_ITERATIONS);
ArrayList<MatOfPoint> contours = new ArrayList<>();
Imgproc.findContours(blueMask, contours, new Mat(), Imgproc.RETR_LIST, Imgproc.CHAIN_APPROX_SIMPLE);
for (int i = 0; i < contours.size(); i++) {
Detection newDetection = new Detection(input.size(), CV_MIN_GOAL_AREA, CV_MAX_GOAL_AREA);
newDetection.setContour(contours.get(i));
newDetection.draw(input, BLUE);
}
blue.setContour(getLargestContour(contours));
blue.fill(input, BLUE);
}
private void updateBlack(Mat input) {
// take pixels that are in the color range and put them into a mask, eroding and dilating them to remove white noise
blackGoalLower1 = new Scalar(CAMERA_BLACK_GOAL_LOWER.getH(), CAMERA_BLACK_GOAL_LOWER.getS(), CAMERA_BLACK_GOAL_LOWER.getV());
blackGoalUpper1 = new Scalar(180, CAMERA_BLACK_GOAL_LOWER.getS(), CAMERA_BLACK_GOAL_LOWER.getV());
blackGoalLower2 = new Scalar(0, CAMERA_BLACK_GOAL_LOWER.getS(), CAMERA_BLACK_GOAL_LOWER.getV());
blackGoalUpper2 = new Scalar(CAMERA_BLACK_GOAL_LOWER.getH(), CAMERA_BLACK_GOAL_LOWER.getS(), CAMERA_BLACK_GOAL_LOWER.getV());
Core.inRange(hsv, blackGoalLower1, blackGoalUpper1, blackMask);
Core.inRange(hsv, blackGoalLower2, blackGoalUpper2, blackMask);
Imgproc.erode(blackMask, blackMask, STRUCTURING_ELEMENT, ANCHOR, ERODE_DILATE_ITERATIONS);
Imgproc.dilate(blackMask, blackMask, STRUCTURING_ELEMENT, ANCHOR, ERODE_DILATE_ITERATIONS);
ArrayList<MatOfPoint> contours = new ArrayList<>();
Imgproc.findContours(blackMask, contours, new Mat(), Imgproc.RETR_LIST, Imgproc.CHAIN_APPROX_SIMPLE);
for (int i = 0; i < contours.size(); i++) {
Detection newDetection = new Detection(input.size(), CV_MIN_GOAL_AREA, CV_MAX_GOAL_AREA);
newDetection.setContour(contours.get(i));
newDetection.draw(input, BLACK);
}
black.setContour(getLargestContour(contours));
black.fill(input, BLACK);
}
public Detection getRed() {
return this.red;
}
public Detection getBlue() {
return this.blue;
}
}

5
build.common.gradle
View File

@ -93,14 +93,14 @@ android {
signingConfig signingConfigs.release
ndk {
abiFilters "armeabi-v7a", "arm64-v8a"
abiFilters "armeabi-v7a"
}
}
debug {
debuggable true
jniDebuggable true
ndk {
abiFilters "armeabi-v7a", "arm64-v8a"
abiFilters "armeabi-v7a"
}
}
}
@ -121,5 +121,6 @@ android {
}
repositories {
mavenCentral()
}

20
build.dependencies.gradle
View File

@ -1,21 +1,23 @@
repositories {
mavenCentral()
google() // Needed for androidx
maven { url = 'https://maven.brott.dev/' }
}
dependencies {
implementation 'org.firstinspires.ftc:Inspection:9.1.0'
implementation 'org.firstinspires.ftc:Blocks:9.1.0'
implementation 'org.firstinspires.ftc:Tfod:9.1.0'
implementation 'org.firstinspires.ftc:RobotCore:9.1.0'
implementation 'org.firstinspires.ftc:RobotServer:9.1.0'
implementation 'org.firstinspires.ftc:OnBotJava:9.1.0'
implementation 'org.firstinspires.ftc:Hardware:9.1.0'
implementation 'org.firstinspires.ftc:FtcCommon:9.1.0'
implementation 'org.firstinspires.ftc:Vision:9.1.0'
implementation 'org.firstinspires.ftc:Inspection:9.0.0'
implementation 'org.firstinspires.ftc:Blocks:9.0.0'
implementation 'org.firstinspires.ftc:Tfod:9.0.0'
implementation 'org.firstinspires.ftc:RobotCore:9.0.0'
implementation 'org.firstinspires.ftc:RobotServer:9.0.0'
implementation 'org.firstinspires.ftc:OnBotJava:9.0.0'
implementation 'org.firstinspires.ftc:Hardware:9.0.0'
implementation 'org.firstinspires.ftc:FtcCommon:9.0.0'
implementation 'org.firstinspires.ftc:Vision:9.0.0'
implementation 'org.firstinspires.ftc:gameAssets-CenterStage:1.0.0'
implementation 'org.tensorflow:tensorflow-lite-task-vision:0.4.3'
runtimeOnly 'org.tensorflow:tensorflow-lite:2.12.0'
implementation 'androidx.appcompat:appcompat:1.2.0'
implementation 'com.acmerobotics.dashboard:dashboard:0.4.12'
}

2
build.gradle
View File

@ -11,7 +11,7 @@ buildscript {
}
dependencies {
// Note for FTC Teams: Do not modify this yourself.
classpath 'com.android.tools.build:gradle:7.2.0'
classpath 'com.android.tools.build:gradle:7.2.2'
}
}