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

Compare commits

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

4 Commits
optimus_de ... master

Author SHA1 Message Date
Cal Kestis 1da45a36c6
Merge pull request #941 from FIRST-Tech-Challenge/20240215-115542-release-candidate 
FtcRobotController v9.1
 2024-02-16 14:13:01 -08:00
Cal Kestis c303962469 FtcRobotController v9.1 2024-02-15 13:10:49 -08:00
Cal Kestis f326c0d033
Merge pull request #731 from FIRST-Tech-Challenge/20230929-083754-release-candidate 
FtcRobotController v9.0.1
 2023-09-29 19:13:38 +09:00
Cal Kestis c023e97a6a FtcRobotController v9.0.1 2023-09-29 19:11:41 +09:00
93 changed files with 605 additions and 10737 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="51"
android:versionName="9.0">
android:versionCode="53"
android:versionName="9.1">
<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,189 +1,217 @@
///* 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
/* 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

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

@ -1,149 +1,163 @@
///* 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
/* 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

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

@ -29,6 +29,7 @@
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;
@ -53,6 +54,17 @@ 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.
*/
@ -107,11 +119,16 @@ public class ConceptTensorFlowObjectDetection extends LinearOpMode {
// Create the TensorFlow processor by using a builder.
tfod = new TfodProcessor.Builder()
// 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.
// 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.
//.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)
@ -134,7 +151,7 @@ public class ConceptTensorFlowObjectDetection extends LinearOpMode {
//builder.setCameraResolution(new Size(640, 480));
// Enable the RC preview (LiveView). Set "false" to omit camera monitoring.
//builder.enableCameraMonitoring(true);
//builder.enableLiveView(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,6 +49,13 @@ 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)
@ -102,7 +109,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 = 0; // Choose the tag you want to approach or set to -1 for ANY tag.
private static final int DESIRED_TAG_ID = -1; // Choose the tag you want to approach or set to -1 for ANY tag.
private VisionPortal visionPortal; // Used to manage the video source.
private AprilTagProcessor aprilTag; // Used for managing the AprilTag detection process.
private AprilTagDetection desiredTag = null; // Used to hold the data for a detected AprilTag
@ -150,25 +157,33 @@ 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) {
if ((detection.metadata != null) &&
((DESIRED_TAG_ID < 0) || (detection.id == DESIRED_TAG_ID)) ){
targetFound = true;
desiredTag = detection;
break; // don't look any further.
// Look to see if we have size info on this tag.
if (detection.metadata != null) {
// Check to see if we want to track towards this tag.
if ((DESIRED_TAG_ID < 0) || (detection.id == DESIRED_TAG_ID)) {
// Yes, we want to use this tag.
targetFound = true;
desiredTag = detection;
break; // don't look any further.
} else {
// This tag is in the library, but we do not want to track it right now.
telemetry.addData("Skipping", "Tag ID %d is not desired", detection.id);
}
} else {
telemetry.addData("Unknown Target", "Tag ID %d is not in TagLibrary\n", detection.id);
// This tag is NOT in the library, so we don't have enough information to track to it.
telemetry.addData("Unknown", "Tag ID %d is not in TagLibrary", detection.id);
}
}
// Tell the driver what we see, and what to do.
if (targetFound) {
telemetry.addData(">","HOLD Left-Bumper to Drive to Target\n");
telemetry.addData("Target", "ID %d (%s)", desiredTag.id, desiredTag.metadata.name);
telemetry.addData("\n>","HOLD Left-Bumper to Drive to Target\n");
telemetry.addData("Found", "ID %d (%s)", desiredTag.id, desiredTag.metadata.name);
telemetry.addData("Range", "%5.1f inches", desiredTag.ftcPose.range);
telemetry.addData("Bearing","%3.0f degrees", desiredTag.ftcPose.bearing);
telemetry.addData("Yaw","%3.0f degrees", desiredTag.ftcPose.yaw);
} else {
telemetry.addData(">","Drive using joysticks to find valid target\n");
telemetry.addData("\n>","Drive using joysticks to find valid target\n");
}
// If Left Bumper is being pressed, AND we have found the desired target, Drive to target Automatically .
@ -243,6 +258,15 @@ 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,6 +49,13 @@ 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.
@ -97,7 +104,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 = 0; // Choose the tag you want to approach or set to -1 for ANY tag.
private static final int DESIRED_TAG_ID = -1; // Choose the tag you want to approach or set to -1 for ANY tag.
private VisionPortal visionPortal; // Used to manage the video source.
private AprilTagProcessor aprilTag; // Used for managing the AprilTag detection process.
private AprilTagDetection desiredTag = null; // Used to hold the data for a detected AprilTag
@ -140,24 +147,32 @@ 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) {
if ((detection.metadata != null) &&
((DESIRED_TAG_ID < 0) || (detection.id == DESIRED_TAG_ID)) ){
targetFound = true;
desiredTag = detection;
break; // don't look any further.
// Look to see if we have size info on this tag.
if (detection.metadata != null) {
// Check to see if we want to track towards this tag.
if ((DESIRED_TAG_ID < 0) || (detection.id == DESIRED_TAG_ID)) {
// Yes, we want to use this tag.
targetFound = true;
desiredTag = detection;
break; // don't look any further.
} else {
// This tag is in the library, but we do not want to track it right now.
telemetry.addData("Skipping", "Tag ID %d is not desired", detection.id);
}
} else {
telemetry.addData("Unknown Target", "Tag ID %d is not in TagLibrary\n", detection.id);
// This tag is NOT in the library, so we don't have enough information to track to it.
telemetry.addData("Unknown", "Tag ID %d is not in TagLibrary", detection.id);
}
}
// Tell the driver what we see, and what to do.
if (targetFound) {
telemetry.addData(">","HOLD Left-Bumper to Drive to Target\n");
telemetry.addData("Target", "ID %d (%s)", desiredTag.id, desiredTag.metadata.name);
telemetry.addData("\n>","HOLD Left-Bumper to Drive to Target\n");
telemetry.addData("Found", "ID %d (%s)", desiredTag.id, desiredTag.metadata.name);
telemetry.addData("Range", "%5.1f inches", desiredTag.ftcPose.range);
telemetry.addData("Bearing","%3.0f degrees", desiredTag.ftcPose.bearing);
} else {
telemetry.addData(">","Drive using joysticks to find valid target\n");
telemetry.addData("\n>","Drive using joysticks to find valid target\n");
}
// If Left Bumper is being pressed, AND we have found the desired target, Drive to target Automatically .
@ -218,6 +233,15 @@ 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 Normal file
View File

@ -0,0 +1,78 @@
/* Copyright (c) 2024 FIRST. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted (subject to the limitations in the disclaimer below) provided that
* the following conditions are met:
*
* Redistributions of source code must retain the above copyright notice, this list
* of conditions and the following disclaimer.
*
* Redistributions in binary form must reproduce the above copyright notice, this
* list of conditions and the following disclaimer in the documentation and/or
* other materials provided with the distribution.
*
* Neither the name of FIRST nor the names of its contributors may be used to endorse or
* promote products derived from this software without specific prior written permission.
*
* NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
* LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
* THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
package org.firstinspires.ftc.robotcontroller.external.samples;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DigitalChannel;
/*
* This OpMode demonstrates how to use a digital channel.
*
* The OpMode assumes that the digital channel is configured with a name of "digitalTouch".
*
* Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name.
* Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list.
*/
@TeleOp(name = "Sensor: digital channel", group = "Sensor")
@Disabled
public class SensorDigitalTouch extends LinearOpMode {
DigitalChannel digitalTouch; // Digital channel Object
@Override
public void runOpMode() {
// get a reference to our touchSensor object.
digitalTouch = hardwareMap.get(DigitalChannel.class, "digitalTouch");
digitalTouch.setMode(DigitalChannel.Mode.INPUT);
telemetry.addData("DigitalTouchSensorExample", "Press start to continue...");
telemetry.update();
// wait for the start button to be pressed.
waitForStart();
// while the OpMode is active, loop and read the digital channel.
// Note we use opModeIsActive() as our loop condition because it is an interruptible method.
while (opModeIsActive()) {
// button is pressed if value returned is LOW or false.
// send the info back to driver station using telemetry function.
if (digitalTouch.getState() == false) {
telemetry.addData("Button", "PRESSED");
} else {
telemetry.addData("Button", "NOT PRESSED");
}
telemetry.update();
}
}
}

1
MeepMeepTesting/.gitignore vendored
View File

@ -1 +0,0 @@
/build

17
MeepMeepTesting/build.gradle
View File

@ -1,17 +0,0 @@
plugins {
id 'java-library'
}
java {
sourceCompatibility = JavaVersion.VERSION_1_8
targetCompatibility = JavaVersion.VERSION_1_8
}
repositories {
maven { url = 'https://jitpack.io' }
maven { url = 'https://maven.brott.dev/' }
}
dependencies {
implementation 'com.github.NoahBres:MeepMeep:2.0.3'
}

131
MeepMeepTesting/src/main/java/com/example/meepmeeptesting/MeepMeepTesting.java
View File

@ -1,131 +0,0 @@
package com.example.meepmeeptesting;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.geometry.Vector2d;
import com.noahbres.meepmeep.MeepMeep;
import com.noahbres.meepmeep.roadrunner.DefaultBotBuilder;
import com.noahbres.meepmeep.roadrunner.entity.RoadRunnerBotEntity;
public class MeepMeepTesting {
public static void main(String[] args) {
MeepMeep meepMeep = new MeepMeep(800);
RoadRunnerBotEntity myBot = new DefaultBotBuilder(meepMeep)
// Set bot constraints: maxVel, maxAccel, maxAngVel, maxAngAccel, track width
.setConstraints(60, 60, Math.toRadians(180), Math.toRadians(180), 15)
.followTrajectorySequence(drive ->
drive.trajectorySequenceBuilder(new Pose2d(-51.5, 33.6,Math.toRadians(180)))
// .lineToConstantHeading(new Vector2d(-45,59.5))
.setTangent(Math.toRadians(90))
.splineToConstantHeading(new Pose2d(-48,59.5).vec(),Math.toRadians(0))
.lineToConstantHeading(new Pose2d(33,59.5).vec())
.splineToConstantHeading(new Pose2d(52.5, 36).vec(),Math.toRadians(0))
.build()
);
RoadRunnerBotEntity BlueFrontStage = new DefaultBotBuilder(meepMeep)
.setConstraints(60,60,Math.toRadians(180),Math.toRadians(180),15)
.followTrajectorySequence(drive ->
drive.trajectorySequenceBuilder(new Pose2d(-36, 61.5, Math.toRadians(-90)))
//Score then pick up 1 white
.lineToLinearHeading(new Pose2d(-39.7, 33.5, Math.toRadians(-90)))
.lineToLinearHeading(new Pose2d(-51.5, 33.6, Math.toRadians(180)).plus(new Pose2d(-5.4,-1.5))).waitSeconds(.01)
//Spline to board
.setTangent(Math.toRadians(90))
.splineToConstantHeading(new Pose2d(-33, 59.5, Math.toRadians(180)).vec(),Math.toRadians(0))
.lineToConstantHeading(new Pose2d(33, 59.5, Math.toRadians(180)).vec())
.splineToConstantHeading(new Pose2d(52, 34, Math.toRadians(8)).vec(),Math.toRadians(0))
//Go back to white stack
.splineToConstantHeading(new Pose2d(33, 59.5, Math.toRadians(180)).plus(new Pose2d(0,-2)).vec(), Math.toRadians(180))
.lineToConstantHeading(new Pose2d(-45, 59.5, Math.toRadians(180)).plus(new Pose2d(0,-2)).vec())
.splineToConstantHeading(new Pose2d(-51.5, 33.6, Math.toRadians(180)).plus(new Pose2d(-3.9, -3.7)).vec(), Math.toRadians(180))
//Go back to board
.setTangent(Math.toRadians(90))
.splineToConstantHeading(new Pose2d(-33, 59.5, Math.toRadians(180)).vec(),Math.toRadians(0))
.lineToConstantHeading(new Pose2d(33, 59.5, Math.toRadians(180)).vec())
.splineToConstantHeading(new Pose2d(52, 34, Math.toRadians(8)).vec(),Math.toRadians(0))
.build()
);
RoadRunnerBotEntity BlueFrontStage3 = new DefaultBotBuilder(meepMeep)
.setConstraints(60,60,Math.toRadians(180),Math.toRadians(180),15)
.followTrajectorySequence(drive ->
drive.trajectorySequenceBuilder(new Pose2d(-36, 61.5, Math.toRadians(-90)))
//Score then pick up 1 white
.lineToLinearHeading(new Pose2d(-46.7, 39.5, Math.toRadians(-90)))
.lineToLinearHeading(new Pose2d(-46.7,50.5,Math.toRadians(-90)))
.lineToLinearHeading(new Pose2d(-51.5, 33.6, Math.toRadians(180)).plus(new Pose2d(-5.4,-1.5))).waitSeconds(.01)
//Spline to board
.setTangent(Math.toRadians(90))
.splineToConstantHeading(new Pose2d(-33, 59.5, Math.toRadians(180)).vec(),Math.toRadians(0))
.lineToConstantHeading(new Pose2d(33, 59.5, Math.toRadians(180)).vec())
.splineToConstantHeading(new Pose2d(53.6, 32, Math.toRadians(8)).vec(),Math.toRadians(0))
//Park
.lineToLinearHeading(new Pose2d(45,58,Math.toRadians(-180)))
.build()
);
RoadRunnerBotEntity BlueFrontStage1 = new DefaultBotBuilder(meepMeep)
.setConstraints(60,60,Math.toRadians(180),Math.toRadians(180),15)
.followTrajectorySequence(drive ->
drive.trajectorySequenceBuilder(new Pose2d(-36, 61.5, Math.toRadians(-90)))
//Score then pick up 1 white
.lineToLinearHeading(new Pose2d(-36, 45.5, Math.toRadians(-90)))
.lineToLinearHeading(new Pose2d(-36,33.5,Math.toRadians(0)))
.lineToLinearHeading(new Pose2d(-32,33.5,Math.toRadians(0)))
.lineToLinearHeading(new Pose2d(-51.5, 33.6, Math.toRadians(180)).plus(new Pose2d(-5.4,-1.5))).waitSeconds(.01)
//Spline to board
.setTangent(Math.toRadians(90))
.splineToConstantHeading(new Pose2d(-33, 59.5, Math.toRadians(180)).vec(),Math.toRadians(0))
.lineToConstantHeading(new Pose2d(33, 59.5, Math.toRadians(180)).vec())
.splineToConstantHeading(new Pose2d(53.6, 32, Math.toRadians(8)).vec(),Math.toRadians(0))
//Park
.lineToLinearHeading(new Pose2d(45,58,Math.toRadians(-180)))
.build()
);
RoadRunnerBotEntity RedFrontStage = new DefaultBotBuilder(meepMeep)
.setConstraints(60,60,Math.toRadians(180),Math.toRadians(180),15)
.followTrajectorySequence(drive ->
drive.trajectorySequenceBuilder(new Pose2d(-36, -61.5, Math.toRadians(90)))
//Score then pick up 1 white
.lineToLinearHeading(new Pose2d(-39.7, -33.5, Math.toRadians(90)))
.lineToLinearHeading(new Pose2d(-51.5, -33.6, Math.toRadians(180)).plus(new Pose2d(-5.4,1.5))).waitSeconds(.01)
//Spline to board
.setTangent(Math.toRadians(-90))
.splineToConstantHeading(new Pose2d(-33, -59.5, Math.toRadians(180)).vec(),Math.toRadians(0))
.lineToConstantHeading(new Pose2d(33, -59.5, Math.toRadians(180)).vec())
.splineToConstantHeading(new Pose2d(52, -34, Math.toRadians(8)).vec(),Math.toRadians(0))
//Go back to white stack
.splineToConstantHeading(new Pose2d(33, -59.5, Math.toRadians(180)).plus(new Pose2d(0,-2)).vec(), Math.toRadians(180))
.lineToConstantHeading(new Pose2d(-45, -59.5, Math.toRadians(180)).plus(new Pose2d(0,-2)).vec())
.splineToConstantHeading(new Pose2d(-51.5, -33.6, Math.toRadians(180)).plus(new Pose2d(-3.9, -3.7)).vec(), Math.toRadians(180))
//Go back to board
.setTangent(Math.toRadians(-90))
.splineToConstantHeading(new Pose2d(-33, -59.5, Math.toRadians(180)).vec(),Math.toRadians(0))
.lineToConstantHeading(new Pose2d(33, -59.5, Math.toRadians(180)).vec())
.splineToConstantHeading(new Pose2d(52, -34, Math.toRadians(8)).vec(),Math.toRadians(0))
.build()
);
meepMeep.setBackground(MeepMeep.Background.FIELD_CENTERSTAGE_JUICE_DARK)
.setDarkMode(true)
.setBackgroundAlpha(0.95f)
//.addEntity(myBot)
//.addEntity(BlueFrontStage)
//.addEntity(BlueFrontStage3)
//.addEntity(BlueFrontStage1)
.addEntity(RedFrontStage)
.start();
}
}

52
README.md
View File

@ -59,6 +59,58 @@ 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,6 +10,8 @@
// Custom definitions may go here
// Include common definitions from above.
apply from: '../build.common.gradle'
apply from: '../build.dependencies.gradle'
@ -24,10 +26,4 @@ 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
View File

@ -1,30 +0,0 @@
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
View File

@ -1,197 +0,0 @@
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
View File

@ -1,913 +0,0 @@
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
View File

@ -1,32 +0,0 @@
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
View File

@ -1,33 +0,0 @@
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;
}
}

171
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Arm.java
View File

@ -1,171 +0,0 @@
package org.firstinspires.ftc.teamcode.hardware;
import com.acmerobotics.dashboard.config.Config;
import com.arcrobotics.ftclib.controller.PController;
import com.qualcomm.hardware.rev.RevColorSensorV3;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.Servo;
@Config
public class Arm {
//make each servo
private Servo rAServo;
private Servo lAServo;
private Servo doorServo;
private Servo wristServo;
private Servo elbow;
private Slides.Position pos = Slides.Position.DOWN;
private PController armController;
private double armControllerTarget;
public static double armTolerance = 0.03;
public static double armSpeed = 0.022;
private double armPos;
private double armTempPos;
private double doorPos;
private double wristPos;
private double elbowPos;
public static double ARM_KP = 0.18;
public static double doorOpenPos = 0.36;
public static double doorClosePos = 0.61;
public static double armStart = 0.24;
public static double armScore = 0.95;
public static double armScore_highMacro = 0.95;
public static double wristStart = 0.8;
public static double wristScore = 0.31;
public static double elbow_mid = 0.5;
public static double elbow_right = 0.29;
public static double elbow_left = 0.95;
public static double wristScore_highMacro = 0.3;
public double counter;
public enum Position {
INTAKE, SCORE, SCOREHI, SCORELEFT, SCORERIGHT
}
public enum DoorPosition {
OPEN, CLOSE
}
public enum Elbowpos{
}
public Arm(HardwareMap hardwareMap) {
wristServo = hardwareMap.get(Servo.class, "Wrist");
doorServo = hardwareMap.get(Servo.class, "Door");
lAServo = hardwareMap.get(Servo.class, "LeftArm");
rAServo = hardwareMap.get(Servo.class, "RightArm");
elbow = hardwareMap.get(Servo.class, "Elbow");
// lAServo.setDirection(Servo.Direction.REVERSE);
rAServo.setDirection(Servo.Direction.REVERSE);
doorServo.setDirection(Servo.Direction.REVERSE);
// wristServo.setDirection(Servo.Direction.REVERSE);
this.armController = new PController(ARM_KP);
setArmPos(Position.INTAKE);
lAServo.setPosition(armStart);
rAServo.setPosition(armStart);
// armTempPos = armPos;
setWristPos(Position.INTAKE);
setDoor(DoorPosition.CLOSE);
setElbowPos(1);
}
public void setArmPos(Position pos) {
this.armControllerTarget = pos == Position.INTAKE
? armStart
: armScore;
// if (pos == Position.INTAKE) {
// armPos = armStart;
// } else if (pos == Position.SCORE) {
// armPos = armScore;
// }
}
public boolean armAtPosition() {
return armController.atSetPoint();
// return armTempPos == armPos;
}
public void setWristPos(Position pos) {
if (pos == Position.INTAKE) {
wristPos = wristStart;
} else if (pos == Position.SCORE) {
wristPos = wristScore;
} else if (pos == Position.SCOREHI) {
wristPos = wristScore_highMacro;
}
}
public void setElbowPos (int counter){
if (counter == 1) {
elbowPos = elbow_mid;
} else if (counter == 2) {
elbowPos = elbow_mid;
} else if (counter == 3) {
elbowPos = elbow_left;
} else if (counter == 4) {
elbowPos = elbow_right;
}
}
public void setDoor(DoorPosition pos) {
if (pos == DoorPosition.OPEN) {
doorPos = doorOpenPos;
} else if (pos == DoorPosition.CLOSE) {
doorPos = doorClosePos;
}
}
public String getTelemetry() {
return String.format("Wrist: %s\nDoor: %s\nLeft Arm: %s\nRight Arm: %s\nError: %s",
wristServo.getPosition(), doorServo.getPosition(), lAServo.getPosition(), rAServo.getPosition(), armController.getPositionError());
//return "Wrist Servo: "+wristServo.getPosition()+"\nLeft Arm Servo: "+lAServo.getPosition()+"\nRight Arm Servo: "+rAServo.getPosition()+"\nDoor Servo: "+ doorServo.getPosition();
}
public void update() {
this.armController.setSetPoint(this.armControllerTarget);
this.armController.setTolerance(armTolerance);
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);
} else {
lAServo.setPosition(armControllerTarget);
rAServo.setPosition(armControllerTarget);
}
// if (Math.abs(armTempPos-armPos) <= armTolerance) {
// armTempPos = armPos;
// } else {
// if (armTempPos < armPos) {
// armTempPos += armSpeed;
// } else if (armTempPos > armPos) {
// armTempPos -= armSpeed;
// }
// }
// lAServo.setPosition(armTempPos);
// rAServo.setPosition(armTempPos);
doorServo.setPosition(doorPos);
wristServo.setPosition(wristPos);
elbow.setPosition(elbowPos);
}
}

106
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Camera.java
View File

@ -1,106 +0,0 @@
package org.firstinspires.ftc.teamcode.hardware;
import static org.firstinspires.ftc.teamcode.util.Constants.INVALID_DETECTION;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.tearabite.ielib.localization.AprilTagPoseEstimator;
import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
import org.firstinspires.ftc.teamcode.vision.Detection;
import org.firstinspires.ftc.teamcode.vision.PropDetectionPipeline;
import org.firstinspires.ftc.vision.VisionPortal;
import org.firstinspires.ftc.vision.apriltag.AprilTagDetection;
import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor;
@Config
public class Camera {
private static final String REAR_WEBCAM_NAME = "rearWebcam";
public static final String FRONT_WEBCAM_NAME = "frontWebcam";
private static final Pose2d REAR_CAMERA_OFFSETS = new Pose2d(6.5, 0, Math.PI);
public static float PROP_REJECTION_VERTICAL_UPPER = 480f * 0.33f;
public static float PROP_REJECTION_VERTICAL_LOWER = 440;
private PropDetectionPipeline prop;
private AprilTagProcessor aprilTag;
private VisionPortal aprilTagPortal;
private VisionPortal propPortal;
private boolean initialized;
private AprilTagPoseEstimator aprilTagPoseEstimator;
public Camera(HardwareMap hardwareMap) {
this.init(hardwareMap);
}
private void init(HardwareMap hardwareMap) {
this.aprilTagPoseEstimator = AprilTagPoseEstimator.builder()
.robotOffset(REAR_CAMERA_OFFSETS)
.build();
this.aprilTag = new AprilTagProcessor.Builder()
.setDrawAxes(true)
.setDrawCubeProjection(true)
.setDrawTagID(true)
.setDrawTagOutline(true)
.build();
int[] viewportIds = VisionPortal.makeMultiPortalView(2, VisionPortal.MultiPortalLayout.HORIZONTAL);
VisionPortal.Builder aprilTagVisionPortalBuilder = new VisionPortal.Builder();
aprilTagVisionPortalBuilder.setCamera(hardwareMap.get(WebcamName.class, REAR_WEBCAM_NAME));
aprilTagVisionPortalBuilder.setLiveViewContainerId(viewportIds[0]);
aprilTagVisionPortalBuilder.setAutoStopLiveView(true);
aprilTagVisionPortalBuilder.addProcessor(aprilTag);
this.aprilTagPortal = aprilTagVisionPortalBuilder.build();
this.prop = new PropDetectionPipeline();
VisionPortal.Builder propVisionPortalBuilder = new VisionPortal.Builder();
propVisionPortalBuilder.setCamera(hardwareMap.get(WebcamName.class, FRONT_WEBCAM_NAME));
propVisionPortalBuilder.setLiveViewContainerId(viewportIds[1]);
propVisionPortalBuilder.setAutoStopLiveView(true);
propVisionPortalBuilder.addProcessor(prop);
this.propPortal = propVisionPortalBuilder.build();
this.propPortal.resumeLiveView();
this.aprilTagPortal.resumeLiveView();
this.initialized = true;
}
public Detection getProp() {
if (!initialized || getAlliance() == null) {
return INVALID_DETECTION;
}
switch (getAlliance()) {
case Blue:
Detection blue = this.prop.getBlue();
return blue != null && blue.isValid() ? blue : INVALID_DETECTION;
case Red:
Detection red = this.prop.getRed();
return red != null && red.isValid() ? red : INVALID_DETECTION;
}
return INVALID_DETECTION;
}
public void setAlliance(CenterStageCommon.Alliance alliance) {
this.prop.setAlliance(alliance);
}
public CenterStageCommon.Alliance getAlliance() {
return this.prop != null ? this.prop.getAlliance() : null;
}
public AprilTagDetection getBestAprilTagDetection() {
return this.aprilTag.getDetections()
.stream().max((d1, d2) -> Float.compare(d2.decisionMargin, d1.decisionMargin))
.orElse(null);
}
public Pose2d estimatePoseFromAprilTag() {
AprilTagDetection detection = this.getBestAprilTagDetection();
if (detection == null) {
return null;
}
return this.aprilTagPoseEstimator.estimatePose(detection);
}
}

120
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Intake.java
View File

@ -1,120 +0,0 @@
package org.firstinspires.ftc.teamcode.hardware;
import static org.firstinspires.ftc.teamcode.hardware.Arm.DoorPosition.CLOSE;
import static org.firstinspires.ftc.teamcode.hardware.Arm.DoorPosition.OPEN;
import com.acmerobotics.dashboard.config.Config;
import com.qualcomm.robotcore.hardware.CRServo;
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 CRServo WheelServo;
private DcMotor dcMotor;
private Position pos = Position.UP;
public enum Position {
STACK1, STACK2, STACK3, STACK4, STACK5, STACK6, 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.015;
public static double stack2 = 0.029;
public static double stack3 = 0.065;
public static double stack4 = 0.09;
public static double stack5 = 0.1;
public static double stack6 = 0.13;
public static double up = 0.30;
public static double motorPower = 0;
public Intake(HardwareMap hardwareMap, Position up) {
lServo = hardwareMap.get(Servo.class, "LeftIntake");
lServo.setDirection(Servo.Direction.REVERSE);
rServo = hardwareMap.get(Servo.class, "Right Intake Servo");
dcMotor = hardwareMap.get(DcMotor.class, "Intakemotor");
WheelServo = hardwareMap.get(CRServo.class, "Wheel");
}
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.STACK6) {
lServo.setPosition(stack6);
rServo.setPosition(stack6);
}
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 void wheel_spit(){
WheelServo.setPower(1);
}
public void wheel_swallow(){
WheelServo.setPower(-1);
}
public void wheel_stop(){
WheelServo.setPower(0);
}
public double getPower() {
return dcMotor.getPower();
}
public String getTelemetry() {
return "lServo: "+lServo.getPosition()+"rServo: "+rServo.getPosition()+"dcMotor: "+dcMotor.getPower();
}
}

59
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/LEDs.java
View File

@ -1,59 +0,0 @@
package org.firstinspires.ftc.teamcode.hardware;
import com.acmerobotics.dashboard.config.Config;
import com.qualcomm.hardware.rev.RevBlinkinLedDriver;
import com.qualcomm.robotcore.hardware.HardwareMap;
import java.util.Locale;
@Config
public class LEDs {
public static int NUMBER = 1;//80 solid
public static int RED = 80;
public static int REDFULL = 3;
public static int REDSCORING = 5;//5
public static int REDINIT = 79;
public static int BLUE = 93;
public static int BLUEFULL = 2;
public static int BLUESCORING = 5;//5
public static int BLUEINIT = 99;
public static int yellow = 81;
public static int lime = 87;
public static int white = 97;
public static int purple = 78;
//yellow:81, green: 87, white:97 , purple:78 ,
private RevBlinkinLedDriver blinkinLedDriver;
private RevBlinkinLedDriver.BlinkinPattern pattern;
public LEDs(HardwareMap hardwareMap) {
blinkinLedDriver = hardwareMap.get(RevBlinkinLedDriver.class, "LEDs");
setPattern();
}
public void setPattern() {
pattern = RevBlinkinLedDriver.BlinkinPattern.fromNumber(NUMBER);
blinkinLedDriver.setPattern(pattern);
}
public void setPattern(int patternNumber) {
pattern = RevBlinkinLedDriver.BlinkinPattern.fromNumber(patternNumber);
blinkinLedDriver.setPattern(pattern);
}
public void nextPattern() {
pattern = pattern.next();
blinkinLedDriver.setPattern(pattern);
}
public void previousPattern() {
pattern = pattern.previous();
blinkinLedDriver.setPattern(pattern);
}
public String getTelemetry() {
return String.format(Locale.US, "Pattern: %s", pattern.toString());
}
}

246
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Robot.java
View File

@ -1,246 +0,0 @@
package org.firstinspires.ftc.teamcode.hardware;
import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.UP;
import static org.firstinspires.ftc.teamcode.hardware.Arm.DoorPosition.CLOSE;
import static org.firstinspires.ftc.teamcode.hardware.Arm.DoorPosition.OPEN;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.qualcomm.hardware.rev.RevBlinkinLedDriver;
import com.qualcomm.robotcore.hardware.HardwareMap;
import org.firstinspires.ftc.teamcode.roadrunner.drive.SampleMecanumDrive;
import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
@Config
public class Robot {
//Create each object
public SampleMecanumDrive drive;
public Camera camera;
public Intake intake;
public Slides slides;
public Arm arm;
public LEDs leds;
public endGame_Mechs endGameMechs;
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;
public static double slideWait = 1.5;
public static double scoreWait = 0.65;
public static double armWait = 2.0;
public int alliance = 0;
//Name the objects
public Robot(HardwareMap hardwareMap) {
drive = new SampleMecanumDrive(hardwareMap);
camera = new Camera(hardwareMap);
intake = new Intake(hardwareMap, UP);
slides = new Slides(hardwareMap);
arm = new Arm(hardwareMap);
endGameMechs = new endGame_Mechs(hardwareMap);
camEnabled = true;
leds = new LEDs(hardwareMap);
}
public void extendMacro(int slidePos, double runTime) {
if (alliance == 1){
leds.setPattern(2);
}else{
leds.setPattern(3);
}
switch(macroState) {
case(0):
macroStartTime = runTime;
slides.setTarget(slidePos);
arm.setDoor(CLOSE);
macroState ++;
break;
case(1):
if (runTime > macroStartTime + slideWait || slides.atTarget()) {
macroState ++;
}
break;
case(2):
arm.setArmPos(Arm.Position.SCORE);
arm.setWristPos(Arm.Position.SCORE);
macroState++;
break;
case (3):
if(arm.armAtPosition()){
macroState = 0;
lastMacro = runningMacro;
runningMacro = 0;
if (alliance == 1){
leds.setPattern(93);
}else{
leds.setPattern(80);
}
}
break;
}
}
public void resetMacro(int slidePos, double runTime) {
if (alliance == 1){
leds.setPattern(2);
}else{
leds.setPattern(3);
}
switch(macroState) {
case(0):
macroStartTime = runTime;
arm.setDoor(OPEN);
macroState++;
break;
case(1):
if (runTime > macroStartTime + scoreWait) {
macroState ++;
}
break;
case(2):
macroStartTime = runTime;
arm.setArmPos(Arm.Position.INTAKE);
arm.setWristPos(Arm.Position.INTAKE);
macroState++;
break;
case(3):
if (/*runTime > macroStartTime + armWait || */arm.armAtPosition()) {
macroState ++;
}
break;
case(4):
slides.setTarget(slidePos);
macroState++;
break;
case (5):
if (slides.atTarget()){
macroState = 0;
lastMacro = runningMacro;
runningMacro = 0;
if (alliance == 1){
leds.setPattern(93);
}else{
leds.setPattern(80);
}
}
break;
}
}
public void extendMacro_auto(int slidePos, double runTime, int slidepos2 ) {
switch(macroState) {
case(0):
macroStartTime = runTime;
slides.setTarget(slidePos);
arm.setDoor(CLOSE);
macroState ++;
break;
case(1):
if (runTime > macroStartTime + slideWait || slides.atTarget()) {
macroState ++;
}
break;
case(2):
arm.setArmPos(Arm.Position.SCORE);
arm.setWristPos(Arm.Position.SCORE);
macroState++;
break;
case (3):
if(arm.armAtPosition()){
macroState = 0;
lastMacro = runningMacro;
runningMacro = 0;
macroState++;
}
break;
case (4):
slides.setTarget(slidepos2);
break;
}
}
public void resetMacro_auto(int slidePos2, double runTime, int slidePos) {
switch(macroState) {
case(0):
macroStartTime = runTime;
arm.setDoor(OPEN);
macroState++;
break;
case(1):
if (runTime > macroStartTime + scoreWait) {
slides.setTarget(slidePos);
macroState ++;
}
break;
case(2):
macroStartTime = runTime;
arm.setArmPos(Arm.Position.INTAKE);
arm.setWristPos(Arm.Position.INTAKE);
macroState++;
break;
case(3):
if (/*runTime > macroStartTime + armWait || */arm.armAtPosition()) {
macroState ++;
}
break;
case(4):
slides.setTarget(slidePos2);
macroState++;
break;
case (5):
if (slides.atTarget()){
macroState = 0;
lastMacro = runningMacro;
runningMacro = 0;
}
break;
}
}
public void update(double runTime) {
// drive.getPoseEstimate().getX();
// = estimatedPose.getX();
Pose2d estimatedPose = null;
// if (camera != null) {
// estimatedPose = this.camera.estimatePoseFromAprilTag();
// }
//drive.update(estimatedPose);
// Pose2d p1 = drive.getPoseEstimate();
//
// Pose2d p2 = this.camera.estimatePoseFromAprilTag();
//
// double D = Math.sqrt(Math.pow((p2.getX()-p1.getX()),2) + Math.pow((p2.getY()-p1.getY()),2));
//
// if (D >= 6 || D <= 1){
// estimatedPose = null;
// }else{
// estimatedPose = this.camera.estimatePoseFromAprilTag();
//
// }
drive.update(/*estimatedPose*/);
slides.update(runTime);
arm.update();
//leds.setPattern();
}
public String getTelemetry() {
return String.format("Arm:\n------------\n%s\nSlides:\n------------\n%s\nIMU:\n------------\n%s" , arm.getTelemetry(), slides.getTelemetry(), drive.getExternalHeadingVelocity());
}
public TrajectorySequenceBuilder getTrajectorySequenceBuilder() {
return this.drive.trajectorySequenceBuilder(this.drive.getPoseEstimate());
}
}

127
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/Slides.java
View File

@ -1,127 +0,0 @@
package org.firstinspires.ftc.teamcode.hardware;
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;
import com.qualcomm.robotcore.hardware.PIDFCoefficients;
@Config
public class Slides {
public DcMotor slide;
public 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;
//p was 0.0014
public static PIDFCoefficients coefficients = new PIDFCoefficients(0.0015,0.04,0,0.01);
public static double pTolerance = 20;
private PIDController controller = new PIDController(coefficients.p, coefficients.i, coefficients.d);
public static int targetMin = -10;
public static int targetMax = 830;
public static int down = 0;
public static int micro_tier1 = 50;
public static int mini_tier1 = 250;
public static int tier1 = 350;
public static int tier2 = 500;
public static int tier3 = 720;
private int target = 0;
public static int manualSpeed = 50;
public enum Position { DOWN, mini_tier1, TIER1, TIER2, TIER3 }
public Slides(HardwareMap hardwareMap) {
slide = hardwareMap.get(DcMotor.class, "Rightslide");
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, "Leftslide");
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.mini_tier1) {
target = Math.min(Math.max(mini_tier1, 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 (slide.getCurrentPosition() <= pTolerance && target <= pTolerance) {
// slide.setPower(0);
// slide2.setPower(0);
// } else {
double pid, ff;
controller.setPID(coefficients.p, coefficients.i, coefficients.d);
controller.setTolerance(pTolerance);
pid = controller.calculate(slide.getCurrentPosition(), target);
ff = coefficients.f;
slide.setPower(pid + ff);
pid = controller.calculate(slide2.getCurrentPosition(), target);
ff = coefficients.f;
slide2.setPower(pid + ff);
// }
// }
}
public String getTelemetry() {
return String.format("Position: %s %s\nTarget: %s %s\nPower: %s %s", slide.getCurrentPosition(), slide2.getCurrentPosition(), target, target, slide.getPower(), slide2.getPower());
}
}

81
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/hardware/endGame_Mechs.java
View File

@ -1,81 +0,0 @@
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;
import com.qualcomm.robotcore.hardware.Servo;
@Config
public class endGame_Mechs {
private Servo servo;
private Servo hang1;
private Servo hang2;
private DcMotor hang;
public static double initPos = 0.42;
public static double launchPos = 0.8;
public static double finger_out = 0.48;
public static double finger_in = 0.2;
public static int initHang = -1000;
public static double hold = 0.8;
public static double release = 0.5;
public static double hold2 = 0.8;
public static double release2 = 0.8;
public static int HangUp = -860;
public static int Hanged = -30;
private int target = 0;
public static int down = 0;
public static int manualSpeed = 50;
public endGame_Mechs(HardwareMap hardwareMap) {
this.servo = hardwareMap.get(Servo.class, "Drone");
this.servo.setPosition(initPos);
// this.servo = hardwareMap.get(Servo.class, "Finger");
// this.servo.setPosition(finger_out);
this.hang = hardwareMap.get(DcMotor.class, "Hang");
this.hang.setTargetPosition(0);
this.hang.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
this.hang.setMode(DcMotor.RunMode.RUN_TO_POSITION);
this.hang.setDirection(DcMotorSimple.Direction.REVERSE);
this.hang.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
// this.hang1 = hardwareMap.get(Servo.class, "Hanger 1");
// this.hang1.setPosition(hold);
// this.hang2 = hardwareMap.get(Servo.class, "Hanger 2");
// this.hang2.setPosition(hold);
}
public void launch() {
this.servo.setPosition(launchPos);
}
public void reset() {
this.servo.setPosition(initPos);
}
// public void Finger_in() {this.servo.setPosition(finger_in);}
//
// public void Finger_out () {this.servo.setPosition(finger_out);}
public void hang_init_pos(){
this.hang.setTargetPosition(0);
this.hang.setPower(1);
}
public void hang_final_pos(){
this.hang.setTargetPosition(2250);
this.hang.setPower(1);
}
}

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

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

80
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/AutoBase.java
View File

@ -1,80 +0,0 @@
package org.firstinspires.ftc.teamcode.opmode.autonomous;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import org.firstinspires.ftc.teamcode.hardware.Robot;
import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
import org.firstinspires.ftc.teamcode.vision.Detection;
@Config
public abstract class AutoBase extends LinearOpMode {
public Robot robot;
protected int macroState;
protected double macroTime;
protected int teamPropLocation = 2; // 1 is left, 2 is middle, 3 is right, perhaps these could be enums instead
// abstract methods for each auto to implement
public abstract void createTrajectories();
public abstract void followTrajectories();
public static int DetectionTest = 20;
@Override
public void runOpMode() {
// create telemetry object for both driver hub and dashboard
// check SampleMecanumDrive to make sure it doesn't override the dashboard telemetry
telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry());
// initialize robot
robot = new Robot(hardwareMap);
//robot.camera.setAlliance(CenterStageCommon.Alliance.Red);
// create trajectories
createTrajectories();
// wait for start
while (!isStarted() && !isStopRequested()) {
// if (!robot.camera.isCameraReady()) {
// telemetry.addLine("Initializing...");
// } else {
telemetry.addLine("Initialized");
// Detection vndafds = robot.camera.getProp() <- returns a detection
// int fdsf = detection.getCenter().x <- x value on the screen between -50,50
double PropDetection = robot.camera.getProp().getCenter().x;
if (PropDetection <= -DetectionTest ) {
teamPropLocation = 1;
} else if (PropDetection >= -DetectionTest && PropDetection <= DetectionTest) {
teamPropLocation = 2;
} else if (PropDetection >= DetectionTest) {
teamPropLocation = 3;
}
// teamPropLocation = 1,2,3
//teamPropLocation = robot.camera.getMarkerId(); // or whatever method you end up using
telemetry.addData("Team Prop Location", teamPropLocation);
// }
telemetry.update();
}
// run auto
while (macroState != -1 && !isStopRequested()) {
// state machine to follow trajectories
followTrajectories();
// update robot
robot.update(getRuntime());
// update telemetry
telemetry.addData("Macro State", macroState);
telemetry.addLine(robot.getTelemetry());
telemetry.update();
}
// stop camera
//robot.camera.stopTargetingCamera();
}
}

309
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueBackStageAuto.java
View File

@ -1,309 +0,0 @@
package org.firstinspires.ftc.teamcode.opmode.autonomous;
import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK1;
import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK2;
import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK3;
import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK4;
import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK5;
import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK6;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.geometry.Vector2d;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import org.firstinspires.ftc.teamcode.hardware.Arm;
import org.firstinspires.ftc.teamcode.hardware.Slides;
import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
@Config
@Autonomous(name = "Blue Backstage Auto(2+4)", group = "Competition", preselectTeleOp = "Main TeleOp")
public class BlueBackStageAuto extends AutoBase {
public static final Pose2d DROP_3 = new Pose2d(18, 32, Math.toRadians(-180));
public static final Pose2d DROP_3M = new Pose2d(13.8, 32, Math.toRadians(-180));
public static final Pose2d DROP_2 = new Pose2d(14, 34, Math.toRadians(-90));
public static final Pose2d ALINE = new Pose2d(51,35, Math.toRadians(-180));
public static final Pose2d DROP_1 = new Pose2d(24.5, 45, Math.toRadians(-90));
public static final Pose2d DEPOSIT_PRELOAD_3 = new Pose2d(53.4, 28.7, Math.toRadians(-180));
public static final Pose2d DEPOSIT_PRELOAD_2 = new Pose2d(51.5, 34.5, Math.toRadians(-180));
public static final Pose2d DEPOSIT_PRELOAD_1 = new Pose2d(51.5, 39.3, Math.toRadians(-180));
public static final Pose2d DEPOSIT_WHITE_STACKS_3 = new Pose2d(53.2, 35.6, Math.toRadians(-180));
public static final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(51, 32.6, Math.toRadians(-180));
public static final Pose2d DEPOSIT_WHITE_STACKS_1 = new Pose2d(53, 33.5, Math.toRadians(-180));
//public static final Vector2d POST_SCORING_SPLINE_END = new Vector2d(24, -8.5);//-36
public static final Pose2d POST_SCORING_SPLINE_END = new Pose2d(26, 11.1 , Math.toRadians(-180));//-36
public static final Pose2d STACK_LOCATION1 = new Pose2d(-56.2, 11.1, Math.toRadians(-180));
public static final Pose2d STACK_LOCATION2 = new Pose2d(-58, 11.1, Math.toRadians(-180));
public static final Pose2d STACK_LOCATION3 = new Pose2d(-55.6, 11.1, Math.toRadians(-180));
@Override
public void createTrajectories() {
// set start position
Pose2d start = new Pose2d(12, 61.5, Math.toRadians(-90));
robot.drive.setPoseEstimate(start);
robot.camera.setAlliance(CenterStageCommon.Alliance.Blue);
}
@Override
public void followTrajectories() {
TrajectorySequenceBuilder builder;
switch (macroState) {
case 0:
builder = this.robot.getTrajectorySequenceBuilder();
switch (teamPropLocation) {
case 1:
builder.lineToLinearHeading(DROP_1);
break;
case 2:
builder.lineToLinearHeading(DROP_2);
break;
case 3:
builder.lineToLinearHeading(DROP_3);
builder.lineToLinearHeading(DROP_3M);
break;
}
this.robot.drive.followTrajectorySequenceAsync(builder.build());
macroState++;
break;
// DRIVE TO TAPE
case 1:
// if drive is done move on
if (!robot.drive.isBusy()) {
macroTime = getRuntime();
macroState++;
}
break;
// RUN INTAKE
case 2:
// intake
// if (getRuntime() < macroTime + 0.05) {
//
// }
// // if intake is done move on
// else {
robot.intake.setDcMotor(0);
robot.arm.setDoor(Arm.DoorPosition.CLOSE);
robot.extendMacro_auto(Slides.mini_tier1, getRuntime(), Slides.micro_tier1);
builder = this.robot.getTrajectorySequenceBuilder();
//Scores yellow pixel
switch (teamPropLocation) {
case 1:
builder.lineToLinearHeading(DEPOSIT_PRELOAD_1);
break;
case 2:
builder.lineToLinearHeading(DEPOSIT_PRELOAD_2);
break;
case 3:
builder.lineToLinearHeading(DEPOSIT_PRELOAD_3);
break;
}
this.robot.drive.followTrajectorySequenceAsync(builder.build());
macroState++;
// }
break;
// EXTEND AND MOVE TO BACKBOARD
case 3:
// extend macro
if (robot.macroState != 0) {
robot.extendMacro(Slides.mini_tier1, getRuntime());
}
// if macro and drive are done, move on
if (robot.macroState == 0 && !robot.drive.isBusy()) {
robot.resetMacro(0, getRuntime());
macroState++;
macroTime = getRuntime();
}
break;
// STACK RUN 1 -------------------------
case 4:
robot.resetMacro(0, getRuntime());
if (getRuntime() > macroTime + 1.4 || robot.macroState >= 4) {
builder = this.robot.getTrajectorySequenceBuilder();
//builder.lineToConstantHeading(POST_SCORING_SPLINE_END);
builder.splineToConstantHeading(POST_SCORING_SPLINE_END.vec(),Math.toRadians(180));
switch (teamPropLocation) {
case 1:
builder.lineToConstantHeading(STACK_LOCATION1.vec().plus(new Vector2d(0)));
break;
case 2:
builder.lineToConstantHeading(STACK_LOCATION2.vec().plus(new Vector2d(0)));
break;
case 3:
builder.lineToConstantHeading(STACK_LOCATION3.vec().plus(new Vector2d()));
break;
}
this.robot.drive.followTrajectorySequenceAsync(builder.build());
macroState++;
}
break;
//waits for the robot to fin the trajectory
case 5:
robot.resetMacro(0, getRuntime());
robot.intake.setDcMotor(0.58);
robot.intake.setpos(STACK6);
if (!robot.drive.isBusy() && getRuntime() > macroTime + 0.4) {
//robot.intake.setDcMotor(0.5);
macroState++;
}
break;
//First 2 pixels off the stack are intaken by this
case 6:
robot.intake.setDcMotor(0.48);
robot.intake.setpos(STACK5);
macroTime = getRuntime();
macroState++;
break;
//goes back to the easel
case 7:
if (getRuntime() > macroTime + 0.7) {
//robot.intake.setDcMotor(-0.0);
robot.arm.setDoor(Arm.DoorPosition.CLOSE);
robot.intake.setDcMotor(-0.35);
builder = this.robot.getTrajectorySequenceBuilder();
//builder.lineToConstantHeading(POST_SCORING_SPLINE_END);
builder.lineToConstantHeading(POST_SCORING_SPLINE_END.vec());
switch (teamPropLocation) {
case 1:
builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec(),Math.toRadians(0));
break;
case 2:
builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec(),Math.toRadians(0));
break;
case 3:
builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_3.vec(),Math.toRadians(0));
break;
}
this.robot.drive.followTrajectorySequenceAsync(builder.build());
macroState++;
macroTime = getRuntime();
}
break;
case 8:
// if drive is done move on
if (getRuntime() > macroTime + 1.4 || !robot.drive.isBusy()) {
macroTime = getRuntime();
robot.macroState = 0;
robot.extendMacro(Slides.mini_tier1 + 60, getRuntime());
macroState++;
}
break;
//extending the macro and about to score
case 9:
if (robot.macroState != 0) {
robot.extendMacro(Slides.mini_tier1 + 60, getRuntime());
}
if (robot.macroState == 0 && !robot.drive.isBusy()) {
robot.resetMacro(0, getRuntime());
macroState++;
macroTime = getRuntime();
}
break;
// STACK RUN 2
case 10:
robot.resetMacro(0, getRuntime());
if (getRuntime() > macroTime + 1.4 || robot.macroState >= 4) {
builder = this.robot.getTrajectorySequenceBuilder();
robot.intake.setDcMotor(0);
//builder.lineToConstantHeading(POST_SCORING_SPLINE_END);
builder.splineToConstantHeading(POST_SCORING_SPLINE_END.vec(),Math.toRadians(180));
switch (teamPropLocation) {
case 1:
builder.lineToConstantHeading(STACK_LOCATION1.vec().plus(new Vector2d(-2)));
break;
case 2:
builder.lineToConstantHeading(STACK_LOCATION2.vec().plus(new Vector2d(0)));
break;
case 3:
builder.lineToConstantHeading(STACK_LOCATION3.vec().plus(new Vector2d()));
break;
}
this.robot.drive.followTrajectorySequenceAsync(builder.build());
macroState++;
}
break;
//waits for the robot to fin the trajectory
case 11:
robot.resetMacro(0, getRuntime());
robot.intake.setDcMotor(0.58);
robot.intake.setpos(STACK3);
if (!robot.drive.isBusy() && getRuntime() > macroTime + 0.4) {
//robot.intake.setDcMotor(0.68);
macroState++;
}
break;
//Third and 4th pixels off the stack are intaken by this
case 12:
robot.intake.setDcMotor(0.48);
robot.intake.setpos(STACK2);
macroTime = getRuntime();
macroState++;
break;
//goes back to the easel
case 13:
if (getRuntime() > macroTime + 0.7) {
robot.arm.setDoor(Arm.DoorPosition.CLOSE);
robot.intake.setDcMotor(-0.35);
builder = this.robot.getTrajectorySequenceBuilder();
//builder.lineToConstantHeading(POST_SCORING_SPLINE_END);
builder.lineToConstantHeading(POST_SCORING_SPLINE_END.vec());
switch (teamPropLocation) {
case 1:
builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec(),Math.toRadians(0));
break;
case 2:
builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec(),Math.toRadians(0));
break;
case 3:
builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_3.vec(),Math.toRadians(0));
break;
}
this.robot.drive.followTrajectorySequenceAsync(builder.build());
macroState++;
macroTime = getRuntime();
}
break;
case 14:
// if drive is done move on
if (getRuntime() > macroTime + 1.4 || !robot.drive.isBusy()) {
macroTime = getRuntime();
robot.macroState = 0;
robot.extendMacro(Slides.mini_tier1 + 80, getRuntime());
macroState++;
}
break;
//extending the macro and about to score
case 15:
if (robot.macroState != 0) {
robot.extendMacro(Slides.mini_tier1 + 80, getRuntime());
}
if (robot.macroState == 0 && !robot.drive.isBusy()) {
robot.resetMacro(0, getRuntime());
macroState++;
}
break;
// PARK ROBOT
case 16:
robot.resetMacro(0, getRuntime());
if (robot.macroState == 0) {
macroState = -1;
}
// PARK ROBOT
//
}
}
}

313
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueBackStagePark.java
View File

@ -1,313 +0,0 @@
package org.firstinspires.ftc.teamcode.opmode.autonomous;
import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK2;
import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK3;
import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK4;
import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK5;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.geometry.Vector2d;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import org.firstinspires.ftc.teamcode.hardware.Arm;
import org.firstinspires.ftc.teamcode.hardware.Slides;
import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
@Config
@Autonomous(name = "Blue Backstage Auto(2 and Park)", group = "Competition", preselectTeleOp = "Main TeleOp")
public class BlueBackStagePark extends AutoBase {
public static final Pose2d DROP_3 = new Pose2d(18, 32, Math.toRadians(-180));
public static final Pose2d DROP_3M = new Pose2d(13.8, 32, Math.toRadians(-180));
public static final Pose2d DROP_2 = new Pose2d(14, 34, Math.toRadians(-90));
public static final Pose2d ALINE = new Pose2d(51,35, Math.toRadians(-180));
public static final Pose2d DROP_1 = new Pose2d(24.5, 43, Math.toRadians(-90));
public static final Pose2d DEPOSIT_PRELOAD_3 = new Pose2d(55.4, 28.7, Math.toRadians(-180));
public static final Pose2d DEPOSIT_PRELOAD_2 = new Pose2d(51.6, 34.5, Math.toRadians(-180));
public static final Pose2d DEPOSIT_PRELOAD_1 = new Pose2d(51.5, 39.3, Math.toRadians(-180));
public static final Pose2d DEPOSIT_WHITE_STACKS_3 = new Pose2d(53.2, 35.6, Math.toRadians(-187));
public static final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(52.4, 32.6, Math.toRadians(-187));
public static final Pose2d DEPOSIT_WHITE_STACKS_1 = new Pose2d(53, 33.5, Math.toRadians(-187));
//public static final Vector2d POST_SCORING_SPLINE_END = new Vector2d(24, -8.5);//-36
public static final Pose2d POST_SCORING_SPLINE_END = new Pose2d(26, 11.1 , Math.toRadians(-180));//-36
public static final Pose2d STACK_LOCATION1 = new Pose2d(-56.2, 11.1, Math.toRadians(-180));
public static final Pose2d STACK_LOCATION2 = new Pose2d(-54.5, 11.1, Math.toRadians(-180));
public static final Pose2d STACK_LOCATION3 = new Pose2d(-55.6, 11.1, Math.toRadians(-180));
public static final Pose2d PARK_1 = new Pose2d(-53,58,Math.toRadians(-180));
public static final Pose2d PARK_2 = new Pose2d(-58,58,Math.toRadians(-180));
@Override
public void createTrajectories() {
// set start position
Pose2d start = new Pose2d(12, 61.5, Math.toRadians(-90));
robot.drive.setPoseEstimate(start);
robot.camera.setAlliance(CenterStageCommon.Alliance.Blue);
}
@Override
public void followTrajectories() {
TrajectorySequenceBuilder builder = null;
switch (macroState) {
case 0:
builder = this.robot.getTrajectorySequenceBuilder();
switch (teamPropLocation) {
case 1:
builder.lineToLinearHeading(DROP_1);
break;
case 2:
builder.lineToLinearHeading(DROP_2);
break;
case 3:
builder.lineToLinearHeading(DROP_3);
builder.lineToLinearHeading(DROP_3M);
break;
}
this.robot.drive.followTrajectorySequenceAsync(builder.build());
macroState++;
break;
// DRIVE TO TAPE
case 1:
// if drive is done move on
if (!robot.drive.isBusy()) {
macroTime = getRuntime();
macroState++;
}
break;
// RUN INTAKE
case 2:
// intake
if (getRuntime() < macroTime + 0.1) {
robot.intake.setDcMotor(-0.24);
}
// if intake is done move on
else {
robot.intake.setDcMotor(0);
robot.arm.setDoor(Arm.DoorPosition.CLOSE);
robot.extendMacro(Slides.mini_tier1, getRuntime());
builder = this.robot.getTrajectorySequenceBuilder();
//Scores yellow pixel
switch (teamPropLocation) {
case 1:
builder.lineToLinearHeading(DEPOSIT_PRELOAD_1);
break;
case 2:
builder.lineToLinearHeading(DEPOSIT_PRELOAD_2);
break;
case 3:
builder.lineToLinearHeading(DEPOSIT_PRELOAD_3);
break;
}
this.robot.drive.followTrajectorySequenceAsync(builder.build());
macroState++;
}
break;
// EXTEND AND MOVE TO BACKBOARD
case 3:
// extend macro
if (robot.macroState != 0) {
robot.extendMacro(Slides.mini_tier1, getRuntime());
}
// if macro and drive are done, move on
if (robot.macroState == 0 && !robot.drive.isBusy()) {
robot.resetMacro(0, getRuntime());
macroState++;
macroTime = getRuntime();
}
break;
// STACK RUN 1 -------------------------
case 4:
robot.resetMacro(0, getRuntime());
if (getRuntime() > macroTime + 1.4 || robot.macroState >= 4) {
// //builder.lineToConstantHeading(POST_SCORING_SPLINE_END);
// builder.splineToConstantHeading(POST_SCORING_SPLINE_END.vec(),Math.toRadians(180));
builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(PARK_1);
builder.lineToLinearHeading(PARK_2);
// switch (teamPropLocation) {
// case 1:
// builder.lineToConstantHeading(STACK_LOCATION1.vec().plus(new Vector2d(0)));
// break;
// case 2:
// builder.lineToConstantHeading(STACK_LOCATION2.vec().plus(new Vector2d(-0)));
// break;
// case 3:
// builder.lineToConstantHeading(STACK_LOCATION3.vec().plus(new Vector2d(0.5)));
// break;
// }
// this.robot.drive.followTrajectorySequenceAsync(builder.build());
// macroState++;
}
break;
// //waits for the robot to fin the trajectory
// case 5:
// robot.resetMacro(0, getRuntime());
// robot.intake.setDcMotor(0.68);
// robot.intake.setpos(STACK5);
// if (!robot.drive.isBusy()) {
// macroState++;
// }
// break;
// //First 2 pixels off the stack are intaken by this
// case 6:
// robot.intake.setDcMotor(0.68);
// robot.intake.setpos(STACK5);
// macroTime = getRuntime();
// macroState++;
// break;
// //goes back to the easel
// case 7:
// if (getRuntime() > macroTime + 0.03) {
// //robot.intake.setDcMotor(-0.0);
// robot.intake.setDcMotor(-0.35);
// builder = this.robot.getTrajectorySequenceBuilder();
// //builder.lineToConstantHeading(POST_SCORING_SPLINE_END);
// builder.lineToConstantHeading(POST_SCORING_SPLINE_END.vec());
// switch (teamPropLocation) {
// case 1:
// builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec(),Math.toRadians(0));
// break;
// case 2:
// builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec(),Math.toRadians(0));
// break;
// case 3:
// builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_3.vec(),Math.toRadians(0));
// break;
// }
// this.robot.drive.followTrajectorySequenceAsync(builder.build());
// macroState++;
// macroTime = getRuntime();
// }
// break;
// case 8:
// // if drive is done move on
// if (getRuntime() > macroTime + 1.4 || !robot.drive.isBusy()) {
// macroTime = getRuntime();
// robot.macroState = 0;
// robot.extendMacro(Slides.mini_tier1 + 80, getRuntime());
// macroState++;
// }
// break;
// //extending the macro and about to score
// case 9:
// if (robot.macroState != 0) {
// robot.extendMacro(Slides.mini_tier1 + 80, getRuntime());
// }
// if (robot.macroState == 0 && !robot.drive.isBusy()) {
// robot.resetMacro(0, getRuntime());
// macroState++;
// macroTime = getRuntime();
// }
// break;
// // STACK RUN 2
// case 10:
// robot.resetMacro(0, getRuntime());
// if (getRuntime() > macroTime + 1.4 || robot.macroState >= 4) {
// builder = this.robot.getTrajectorySequenceBuilder();
// //builder.lineToConstantHeading(POST_SCORING_SPLINE_END);
// builder.splineToConstantHeading(POST_SCORING_SPLINE_END.vec(),Math.toRadians(180));
// switch (teamPropLocation) {
// case 1:
// builder.lineToConstantHeading(STACK_LOCATION1.vec().plus(new Vector2d(-2)));
// break;
// case 2:
// builder.lineToConstantHeading(STACK_LOCATION2.vec().plus(new Vector2d(-2)));
// break;
// case 3:
// builder.lineToConstantHeading(STACK_LOCATION3.vec().plus(new Vector2d(0.5
// )));
// break;
// }
// this.robot.drive.followTrajectorySequenceAsync(builder.build());
// macroState++;
// }
// break;
// //waits for the robot to fin the trajectory
// case 11:
// robot.resetMacro(0, getRuntime());
// robot.intake.setDcMotor(0.68);
// robot.intake.setpos(STACK3);
// if (!robot.drive.isBusy()) {
// macroState++;
// }
// break;
// //Third and 4th pixels off the stack are intaken by this
// case 12:
// robot.intake.setDcMotor(0.68);
// robot.intake.setpos(STACK2);
// macroTime = getRuntime();
// macroState++;
// break;
// //goes back to the easel
// case 13:
// if (getRuntime() > macroTime + 0.03) {
// robot.intake.setDcMotor(-0.35);
// builder = this.robot.getTrajectorySequenceBuilder();
// //builder.lineToConstantHeading(POST_SCORING_SPLINE_END);
// builder.lineToConstantHeading(POST_SCORING_SPLINE_END.vec());
// switch (teamPropLocation) {
// case 1:
// builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec(),Math.toRadians(0));
// break;
// case 2:
// builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec(),Math.toRadians(0));
// break;
// case 3:
// builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_3.vec(),Math.toRadians(0));
// break;
// }
// this.robot.drive.followTrajectorySequenceAsync(builder.build());
// macroState++;
// macroTime = getRuntime();
// }
// break;
// case 14:
// // if drive is done move on
// if (getRuntime() > macroTime + 1.4 || !robot.drive.isBusy()) {
// macroTime = getRuntime();
// robot.macroState = 0;
// robot.extendMacro(Slides.mini_tier1 + 140, getRuntime());
// macroState++;
// }
// break;
// //extending the macro and about to score
// case 15:
// if (robot.macroState != 0) {
// robot.extendMacro(Slides.mini_tier1 + 80, getRuntime());
// }
// if (robot.macroState == 0 && !robot.drive.isBusy()) {
// robot.resetMacro(0, getRuntime());
// macroState++;
// }
// break;
//
// // PARK ROBOT
case 5:
robot.resetMacro(0, getRuntime());
if (robot.macroState == 0) {
macroState = -1;
// builder.lineToLinearHeading(PARK_1);
// builder.lineToLinearHeading(PARK_2);
// builder = this.robot.getTrajectorySequenceBuilder();
}
//
// // PARK ROBOT
////
}
}
}

236
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueFrontPreload.java
View File

@ -1,236 +0,0 @@
package org.firstinspires.ftc.teamcode.opmode.autonomous;
import static org.firstinspires.ftc.teamcode.hardware.Arm.DoorPosition.CLOSE;
import static org.firstinspires.ftc.teamcode.hardware.Arm.DoorPosition.OPEN;
import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK1;
import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK2;
import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK3;
import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK4;
import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK5;
import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK6;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import org.firstinspires.ftc.teamcode.hardware.Slides;
import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
@Config
@Autonomous(name = "Blue FrontPreload (2+1)", group = "Competition", preselectTeleOp = "Main TeleOp")
public class BlueFrontPreload extends AutoBase {
public static final Pose2d DROP_1_PART_2 = new Pose2d(-33, 33.5, Math.toRadians(0));
public static final Pose2d DROP_1 = new Pose2d(-33,33.5,Math.toRadians(0));
public static final Pose2d DROP_2 = new Pose2d(-39.7, 33.5, Math.toRadians(-90));
public static final Pose2d DROP_2_PART_2 = new Pose2d(-39.7,36.5, Math.toRadians(-90));
public static final Pose2d DROP_3 = new Pose2d(-51, 50.5, Math.toRadians(-90));
public static final Pose2d DROP_1M = new Pose2d(-36, 45, Math.toRadians(-90));
public static final Pose2d DROP_2M = new Pose2d(-48.5, 30, Math.toRadians(-90));
public static final Pose2d DROP_3M = new Pose2d(-53.7, 35.9, Math.toRadians(-90));
public static final Pose2d DEPOSIT_WHITE_STACKS_1 = new Pose2d(53.3, 38.3, Math.toRadians(0));//187
public static final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(53, 34.5, Math.toRadians(0));//187
public static final Pose2d DEPOSIT_WHITE_STACKS_3 = new Pose2d(55, 25, Math.toRadians(0));//817
public static final Pose2d STACK_LOCATION_1 = new Pose2d(-54.5, 32.6, Math.toRadians(180));
public static final Pose2d STACK_LOCATION_2 = new Pose2d(-52, 29.6, Math.toRadians(180));
public static final Pose2d STACK_LOCATION_3 = new Pose2d(-52, 29.6, Math.toRadians(180));
public static final Pose2d POST_SCORING_SPLINE_END = new Pose2d(24, 12.4, Math.toRadians(10));//-36
public static final Pose2d POST_DROP_POS = new Pose2d(-45, 59.5, Math.toRadians(180));
public static final Pose2d POST_DROP_POS_PART2 = new Pose2d(-33, 58.5, Math.toRadians(180));
public static final Pose2d PRE_DEPOSIT_POS = new Pose2d(33, 58.5, Math.toRadians(180));
public static final Pose2d PARK_1 = new Pose2d(45,58,Math.toRadians(-180));
public static final Pose2d PARK_2 = new Pose2d(58,58,Math.toRadians(-180));
@Override
public void createTrajectories() {
// set start position
Pose2d start = new Pose2d(-36, 61.5, Math.toRadians(-90));
robot.drive.setPoseEstimate(start);
robot.camera.setAlliance(CenterStageCommon.Alliance.Blue);
}
@Override
public void followTrajectories() {
TrajectorySequenceBuilder builder = null;
switch (macroState) {
case 0:
builder = this.robot.getTrajectorySequenceBuilder();
switch (teamPropLocation) {
case 1:
builder.lineToLinearHeading(DROP_1M);
builder.lineToLinearHeading(DROP_1);
break;
case 2:
builder.lineToLinearHeading(DROP_2);
break;
case 3:
builder.lineToLinearHeading(DROP_3M);
// builder.lineToLinearHeading(DROP_3);
break;
}
this.robot.drive.followTrajectorySequenceAsync(builder.build());
macroState++;
break;
// DRIVE TO TAPE
case 1:
// if drive is done move on
if (!robot.drive.isBusy()) {
macroTime = getRuntime();
macroState++;
}
break;
// RUN INTAKE
case 2:
// intake
if (getRuntime() < macroTime + 0.18) {
robot.intake.setDcMotor(-0.23);
}
else{
builder = this.robot.getTrajectorySequenceBuilder();
robot.intake.setDcMotor(0);
switch (teamPropLocation) {
case 1:
builder.lineToLinearHeading(DROP_1_PART_2);
break;
case 2:
builder.lineToLinearHeading(DROP_2_PART_2);
break;
case 3:
builder.lineToLinearHeading(DROP_3);
break;
}
robot.arm.setDoor(OPEN);
switch (teamPropLocation) {
case (1):
//team prop location 1
builder.lineToLinearHeading(STACK_LOCATION_1.plus(new Pose2d(0))).waitSeconds(.01);
break;
case (2):
//team prop location 2
builder.lineToLinearHeading(STACK_LOCATION_2.plus(new Pose2d(-5.8,2))).waitSeconds(.01);
break;
case (3):
//team prop location 3
builder.lineToLinearHeading(STACK_LOCATION_3.plus(new Pose2d(-4.8,2))).waitSeconds(.01);
break;
}
robot.intake.setDcMotor(0.44);
robot.intake.setpos(STACK6);
macroTime = getRuntime();
this.robot.drive.followTrajectorySequenceAsync(builder.build());
macroState++;
}
// if intake is done move on
break;
case 3:
// if drive is done move on
if (!robot.drive.isBusy()) {
macroTime = getRuntime();
macroState++;
}
break;
case 4:
if (getRuntime() > macroTime + 0.06) {
robot.arm.setDoor(CLOSE);
robot.intake.setDcMotor(-0.5);
builder = this.robot.getTrajectorySequenceBuilder();
switch (teamPropLocation) {
case 1:
builder.setTangent(Math.toRadians(90));
builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0));
builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec(),Math.toRadians(0));
break;
case 2:
builder.setTangent(Math.toRadians(90));
builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0));
builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec(),Math.toRadians(0));
break;
case 3:
builder.setTangent(Math.toRadians(90));
builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0));
builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_3.vec(),Math.toRadians(0));
break;
}
this.robot.drive.followTrajectorySequenceAsync(builder.build());
macroState++;
macroTime = getRuntime();
}
break;
case 5:
// if drive is done move on
if (getRuntime() > macroTime + 2.4 || !robot.drive.isBusy()) {
macroTime = getRuntime();
robot.macroState = 0;
robot.extendMacro(Slides.mini_tier1-30, getRuntime());
macroState++;
}
break;
//extending the macro and about to score
case 6:
if (robot.macroState != 0) {
robot.extendMacro(Slides.mini_tier1, getRuntime());
}
if (robot.macroState == 0 && !robot.drive.isBusy()) {
robot.resetMacro(0, getRuntime());
macroState++;
}
break;
//Park
case 7:
robot.resetMacro(0, getRuntime());
if (getRuntime() > macroTime + 3.4 || robot.macroState >= 4) {
builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(PARK_1);
this.robot.drive.followTrajectorySequenceAsync(builder.build());
macroState++;
}
break;
case 8:
// if drive is done move on
if (getRuntime() > macroTime + 4.4 || !robot.drive.isBusy()) {
macroState++;
}
break;
case 9:
robot.resetMacro(0, getRuntime());
if (robot.macroState == 0) {
macroState = -1;
}
case 19:
robot.resetMacro(0, getRuntime());
if (robot.macroState == 0) {
macroState = -1;
}
}
}
}

292
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/BlueFrontStageAuto.java
View File

@ -1,292 +0,0 @@
package org.firstinspires.ftc.teamcode.opmode.autonomous;
import static org.firstinspires.ftc.teamcode.hardware.Arm.DoorPosition.CLOSE;
import static org.firstinspires.ftc.teamcode.hardware.Arm.DoorPosition.OPEN;
import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK1;
import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK2;
import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK3;
import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK4;
import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK5;
import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK6;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import org.firstinspires.ftc.teamcode.hardware.Slides;
import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
@Config
@Autonomous(name = "Blue FrontPreload (2+3)", group = "Competition", preselectTeleOp = "Main TeleOp")
public class BlueFrontStageAuto extends AutoBase {
public static final Pose2d DROP_1_PART_2 = new Pose2d(-33, 33.5, Math.toRadians(0));
public static final Pose2d DROP_1 = new Pose2d(-33,33.5,Math.toRadians(0));
public static final Pose2d DROP_2 = new Pose2d(-39.7, 33.5, Math.toRadians(-90));
public static final Pose2d DROP_2_PART_2 = new Pose2d(-39.7,36.5, Math.toRadians(-90));
public static final Pose2d DROP_3 = new Pose2d(-49, 50.5, Math.toRadians(-90));
public static final Pose2d DROP_1M = new Pose2d(-36, 45, Math.toRadians(-90));
public static final Pose2d DROP_2M = new Pose2d(-48.5, 30, Math.toRadians(-90));
public static final Pose2d DROP_3M = new Pose2d(-53.7, 35.9, Math.toRadians(-90));
public static final Pose2d DEPOSIT_WHITE_STACKS_1 = new Pose2d(53.3, 38.3, Math.toRadians(0));//187
public static final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(53, 34.5, Math.toRadians(0));//187
public static final Pose2d DEPOSIT_WHITE_STACKS_3 = new Pose2d(55, 25, Math.toRadians(0));//817
public static final Pose2d STACK_LOCATION_1 = new Pose2d(-54.5, 32.6, Math.toRadians(180));
public static final Pose2d STACK_LOCATION_2 = new Pose2d(-52, 29.6, Math.toRadians(180));
public static final Pose2d STACK_LOCATION_3 = new Pose2d(-52, 29.6, Math.toRadians(180));
public static final Pose2d POST_SCORING_SPLINE_END = new Pose2d(24, 12.4, Math.toRadians(10));//-36
public static final Pose2d POST_DROP_POS = new Pose2d(-45, 59.5, Math.toRadians(180));
public static final Pose2d POST_DROP_POS_PART2 = new Pose2d(-33, 58.5, Math.toRadians(180));
public static final Pose2d PRE_DEPOSIT_POS = new Pose2d(33, 58.5, Math.toRadians(180));
public static final Pose2d PARK_1 = new Pose2d(45,58,Math.toRadians(-180));
public static final Pose2d PARK_2 = new Pose2d(58,58,Math.toRadians(-180));
@Override
public void createTrajectories() {
// set start position
Pose2d start = new Pose2d(-36, 61.5, Math.toRadians(-90));
robot.drive.setPoseEstimate(start);
robot.camera.setAlliance(CenterStageCommon.Alliance.Blue);
}
@Override
public void followTrajectories() {
TrajectorySequenceBuilder builder = null;
switch (macroState) {
case 0:
builder = this.robot.getTrajectorySequenceBuilder();
switch (teamPropLocation) {
case 1:
builder.lineToLinearHeading(DROP_1M);
builder.lineToLinearHeading(DROP_1);
break;
case 2:
builder.lineToLinearHeading(DROP_2);
break;
case 3:
builder.lineToLinearHeading(DROP_3M);
// builder.lineToLinearHeading(DROP_3);
break;
}
this.robot.drive.followTrajectorySequenceAsync(builder.build());
macroState++;
break;
// DRIVE TO TAPE
case 1:
// if drive is done move on
if (!robot.drive.isBusy()) {
macroTime = getRuntime();
macroState++;
}
break;
// RUN INTAKE
case 2:
// intake
if (getRuntime() < macroTime + 0.18) {
robot.intake.setDcMotor(-0.23);
}
else{
builder = this.robot.getTrajectorySequenceBuilder();
robot.intake.setDcMotor(0);
switch (teamPropLocation) {
case 1:
builder.lineToLinearHeading(DROP_1_PART_2);
break;
case 2:
builder.lineToLinearHeading(DROP_2_PART_2);
break;
case 3:
builder.lineToLinearHeading(DROP_3);
break;
}
robot.arm.setDoor(OPEN);
switch (teamPropLocation) {
case (1):
//team prop location 1
builder.lineToLinearHeading(STACK_LOCATION_1.plus(new Pose2d(0))).waitSeconds(.01);
break;
case (2):
//team prop location 2
builder.lineToLinearHeading(STACK_LOCATION_2.plus(new Pose2d(-5.8,2))).waitSeconds(.01);
case (3):
//team prop location 3
builder.lineToLinearHeading(STACK_LOCATION_3.plus(new Pose2d(-4.8,2))).waitSeconds(.01);
}
robot.intake.setDcMotor(0.44);
robot.intake.setpos(STACK6);
macroTime = getRuntime();
this.robot.drive.followTrajectorySequenceAsync(builder.build());
macroState++;
}
// if intake is done move on
break;
case 3:
// if drive is done move on
if (!robot.drive.isBusy()) {
macroTime = getRuntime();
macroState++;
}
break;
case 4:
if (getRuntime() > macroTime + 0.06) {
robot.arm.setDoor(CLOSE);
robot.intake.setDcMotor(-0.5);
builder = this.robot.getTrajectorySequenceBuilder();
switch (teamPropLocation) {
case 1:
builder.setTangent(Math.toRadians(90));
builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0));
builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec(),Math.toRadians(0));
break;
case 2:
builder.setTangent(Math.toRadians(90));
builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0));
builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec(),Math.toRadians(0));
break;
case 3:
builder.setTangent(Math.toRadians(90));
builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0));
builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_3.vec(),Math.toRadians(0));
break;
}
this.robot.drive.followTrajectorySequenceAsync(builder.build());
macroState++;
macroTime = getRuntime();
}
break;
case 5:
// if drive is done move on
if (getRuntime() > macroTime + 2.4 || !robot.drive.isBusy()) {
macroTime = getRuntime();
robot.macroState = 0;
robot.extendMacro(Slides.mini_tier1-30, getRuntime());
macroState++;
}
break;
//extending the macro and about to score
case 6:
if (robot.macroState != 0) {
robot.extendMacro(Slides.mini_tier1, getRuntime());
}
if (robot.macroState == 0 && !robot.drive.isBusy()) {
robot.resetMacro(0, getRuntime());
macroState++;
}
break;
case 7:
robot.resetMacro(0,getRuntime());
if (getRuntime() > macroTime + 3.4 || robot.macroState >= 4) {
builder = this.robot.getTrajectorySequenceBuilder();
builder.splineToConstantHeading(PRE_DEPOSIT_POS.vec(),Math.toRadians(0));
builder.lineToConstantHeading(POST_DROP_POS_PART2.vec());
builder.splineToConstantHeading(STACK_LOCATION_2.vec(),Math.toRadians(0));
this.robot.drive.followTrajectorySequenceAsync(builder.build());
macroState++;
}
break;
case 8:
robot.arm.setDoor(OPEN);
robot.intake.setDcMotor(0.44);
robot.intake.setpos(STACK4);
macroTime = getRuntime();
macroState++;
break;
case 9:
if (getRuntime() > macroTime + 0.06) {
robot.arm.setDoor(CLOSE);
robot.intake.setDcMotor(-0.5);
builder = this.robot.getTrajectorySequenceBuilder();
switch (teamPropLocation) {
case 1:
builder.setTangent(Math.toRadians(90));
builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0));
builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec(),Math.toRadians(0));
break;
case 2:
builder.setTangent(Math.toRadians(90));
builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0));
builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec(),Math.toRadians(0));
break;
case 3:
builder.setTangent(Math.toRadians(90));
builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0));
builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec(),Math.toRadians(0));
break;
}
this.robot.drive.followTrajectorySequenceAsync(builder.build());
macroState++;
macroTime = getRuntime();
}
break;
case 10:
// if drive is done move on
if (getRuntime() > macroTime + 2.4 || !robot.drive.isBusy()) {
macroTime = getRuntime();
robot.macroState = 0;
robot.extendMacro(Slides.mini_tier1-30, getRuntime());
macroState++;
}
break;
case 11:
if (robot.macroState != 0) {
robot.extendMacro(Slides.mini_tier1, getRuntime());
}
if (robot.macroState == 0 && !robot.drive.isBusy()) {
robot.resetMacro(0, getRuntime());
macroState++;
}
break;
//Park
case 12:
robot.resetMacro(0, getRuntime());
if (getRuntime() > macroTime + 3.4 || robot.macroState >= 4) {
builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(PARK_1);
this.robot.drive.followTrajectorySequenceAsync(builder.build());
macroState++;
}
break;
}
}
}

320
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/RedBackStageAuto.java
View File

@ -1,320 +0,0 @@
package org.firstinspires.ftc.teamcode.opmode.autonomous;
import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK1;
import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK2;
import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK3;
import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK4;
import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK5;
import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK6;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.geometry.Vector2d;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import org.firstinspires.ftc.teamcode.hardware.Arm;
import org.firstinspires.ftc.teamcode.hardware.Slides;
import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
@Config
@Autonomous(name = "Red Backstage Auto(2+4)", group = "Competition", preselectTeleOp = "Main TeleOp")
public class RedBackStageAuto extends AutoBase {
public static final Pose2d DROP_1 = new Pose2d(16, -32.5, Math.toRadians(180));
public static final Pose2d DROP_1M = new Pose2d(11.5, -32.5, Math.toRadians(180));
public static final Pose2d DROP_2 = new Pose2d(15.6, -33.5, Math.toRadians(90));
public static final Pose2d ALINE = new Pose2d(51,-32.5, Math.toRadians(180));
public static final Pose2d DROP_3 = new Pose2d(25, -42.5, Math.toRadians(90));
public static final Pose2d DEPOSIT_PRELOAD_1 = new Pose2d(52, -26.3, Math.toRadians(180));
public static final Pose2d DEPOSIT_PRELOAD_2 = new Pose2d(51.7, -32.5, Math.toRadians(180));
public static final Pose2d DEPOSIT_PRELOAD_3 = new Pose2d(52.3, -39, Math.toRadians(180));
public static final Pose2d DEPOSIT_WHITE_STACKS_1 = new Pose2d(52, -35.3, Math.toRadians(180));//187
public static final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(51.6, -30.5, Math.toRadians(180));//187
public static final Pose2d DEPOSIT_WHITE_STACKS_3 = new Pose2d(52.4, -34.5, Math.toRadians(180));//187
//public static final Vector2d POST_SCORING_SPLINE_END = new Vector2d(24, -8.5);//-36
public static final Pose2d POST_SCORING_SPLINE_END = new Pose2d(27, -11, Math.toRadians(180));//-36
public static final Pose2d STACK_LOCATION_1 = new Pose2d(-56, -11, Math.toRadians(180));
public static final Pose2d STACK_LOCATION_2 = new Pose2d(-56.2, -11, Math.toRadians(180));
public static final Pose2d STACK_LOCATION_3 = new Pose2d(-56.2, -11, Math.toRadians(185));
@Override
public void createTrajectories() {
// set start position
Pose2d start = new Pose2d(12, -61.5, Math.toRadians(90));
robot.drive.setPoseEstimate(start);
robot.camera.setAlliance(CenterStageCommon.Alliance.Red);
}
@Override
public void followTrajectories() {
TrajectorySequenceBuilder builder;
switch (macroState) {
case 0:
builder = this.robot.getTrajectorySequenceBuilder();
switch (teamPropLocation) {
case 1:
builder.lineToLinearHeading(DROP_1);
builder.lineToLinearHeading(DROP_1M);
break;
case 2:
builder.lineToLinearHeading(DROP_2);
break;
case 3:
builder.lineToLinearHeading(DROP_3);
break;
}
this.robot.drive.followTrajectorySequenceAsync(builder.build());
macroState++;
break;
// DRIVE TO TAPE
case 1:
// if drive is done move on
if (!robot.drive.isBusy()) {
macroTime = getRuntime();
macroState++;
}
break;
// RUN INTAKE
case 2:
// intake
if (getRuntime() < macroTime + 0.26) {
robot.intake.setDcMotor(-0.18);
}
// if intake is done move on
else {
robot.intake.setDcMotor(0);
robot.arm.setDoor(Arm.DoorPosition.CLOSE);
robot.extendMacro_auto(0, getRuntime(),Slides.mini_tier1);
builder = this.robot.getTrajectorySequenceBuilder();
//Scores yellow pixel
switch (teamPropLocation) {
case 1:
builder.lineToLinearHeading(DEPOSIT_PRELOAD_1);
break;
case 2:
//robot.extendMacro(Slides.mini_tier1 -40, getRuntime());
builder.lineToLinearHeading(DEPOSIT_PRELOAD_2);
break;
case 3:
builder.lineToLinearHeading(DEPOSIT_PRELOAD_3);
break;
}
this.robot.drive.followTrajectorySequenceAsync(builder.build());
macroState++;
}
break;
// EXTEND AND MOVE TO BACKBOARD
case 3:
// extend macro
if (robot.macroState != 0) {
robot.extendMacro(Slides.mini_tier1 - 20, getRuntime());
}
// if macro and drive are done, move on
if (robot.macroState == 0 && !robot.drive.isBusy()) {
robot.resetMacro(0, getRuntime());
macroState++;
macroTime = getRuntime();
}
break;
// STACK RUN 1 -------------------------
case 4:
robot.resetMacro(0, getRuntime());
if (getRuntime() > macroTime + 1.5 || robot.macroState >= 4) {
builder = this.robot.getTrajectorySequenceBuilder();
robot.intake.setDcMotor(0);
//builder.lineToConstantHeading(POST_SCORING_SPLINE_END);
builder.splineToConstantHeading(POST_SCORING_SPLINE_END.vec(),Math.toRadians(180));
switch (teamPropLocation) {
case 1:
builder.lineToConstantHeading(STACK_LOCATION_1.vec().plus(new Vector2d(-2)));
robot.intake.setpos(STACK5);
break;
case 2:
builder.lineToConstantHeading(STACK_LOCATION_2.vec().plus(new Vector2d(-2)));
robot.intake.setpos(STACK5);
break;
case 3:
builder.lineToConstantHeading(STACK_LOCATION_3.vec().plus(new Vector2d(-2)));
robot.intake.setpos(STACK5);
break;
}
this.robot.drive.followTrajectorySequenceAsync(builder.build());
macroState++;
macroTime = getRuntime();
}
break;
//waits for the robot to fin the trajectory
case 5:
robot.resetMacro(0, getRuntime());
robot.intake.setDcMotor(0.58);
robot.intake.setpos(STACK6);
if (!robot.drive.isBusy() && getRuntime() > macroTime + 0.2) {
macroState++;
}
break;
//First 2 pixels off the stack are intaken by this
case 6:
robot.intake.setDcMotor(0.48);
robot.intake.setpos(STACK5);
macroTime = getRuntime();
macroState++;
break;
//goes back to the easel
case 7:
if (getRuntime() > macroTime + 0.6) {
robot.arm.setDoor(Arm.DoorPosition.CLOSE);
builder = this.robot.getTrajectorySequenceBuilder();
//builder.lineToConstantHeading(POST_SCORING_SPLINE_END);
builder.lineToConstantHeading(POST_SCORING_SPLINE_END.vec());
robot.intake.setDcMotor(-0.35);
switch (teamPropLocation) {
case 1:
builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec(),Math.toRadians(0));
break;
case 2:
builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec(),Math.toRadians(0));
break;
case 3:
builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_3.vec(),Math.toRadians(0));
break;
}
this.robot.drive.followTrajectorySequenceAsync(builder.build());
macroState++;
macroTime = getRuntime();
}
break;
case 8:
// if drive is done move on
if (getRuntime() > macroTime + 1.4 || !robot.drive.isBusy()) {
macroTime = getRuntime();
robot.macroState = 0;
robot.extendMacro(Slides.mini_tier1 , getRuntime());
macroState++;
}
break;
//extending the macro and about to score
case 9:
if (robot.macroState != 0) {
robot.extendMacro(Slides.mini_tier1 , getRuntime());
}
if (robot.macroState == 0 && !robot.drive.isBusy()) {
robot.resetMacro(0, getRuntime());
macroState++;
macroTime = getRuntime();
}
break;
// STACK RUN 2
case 10:
robot.resetMacro(0, getRuntime());
if (getRuntime() > macroTime + 1.5 || robot.macroState >= 4) {
robot.intake.setDcMotor(0);
builder = this.robot.getTrajectorySequenceBuilder();
builder.splineToConstantHeading(POST_SCORING_SPLINE_END.vec(),Math.toRadians(180));
switch (teamPropLocation) {
case 1:
builder.lineToConstantHeading(STACK_LOCATION_1.vec().plus(new Vector2d(-1.75)));
robot.intake.setpos(STACK3);
break;
case 2:
builder.lineToConstantHeading(STACK_LOCATION_2.vec().plus(new Vector2d(-3.3)));
robot.intake.setpos(STACK3);
break;
case 3:
builder.lineToConstantHeading(STACK_LOCATION_3.vec().plus(new Vector2d(-2.8)));
robot.intake.setpos(STACK3);
break;
}
this.robot.drive.followTrajectorySequenceAsync(builder.build());
macroState++;
macroTime = getRuntime();
}
break;
//waits for the robot to fin the trajectory
case 11:
robot.resetMacro(0, getRuntime());
robot.intake.setDcMotor(0.58);
robot.intake.setpos(STACK3);
if (!robot.drive.isBusy() && getRuntime() > macroTime + 0.2) {
macroState++;
}
break;
//3rd and 4th pixels off the stack are intaken by this
case 12:
robot.intake.setDcMotor(0.48);
robot.intake.setpos(STACK2);
macroTime = getRuntime();
macroState++;
break;
//goes back to the easel
case 13:
if (getRuntime() > macroTime + 0.7) {
robot.arm.setDoor(Arm.DoorPosition.CLOSE);
builder = this.robot.getTrajectorySequenceBuilder();
//builder.lineToConstantHeading(POST_SCORING_SPLINE_END);
builder.lineToConstantHeading(POST_SCORING_SPLINE_END.vec());
robot.intake.setDcMotor(-0.35);
switch (teamPropLocation) {
case 1:
builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec(),Math.toRadians(0));
break;
case 2:
builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec(),Math.toRadians(0));
break;
case 3:
builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_3.vec(),Math.toRadians(0));
break;
}
this.robot.drive.followTrajectorySequenceAsync(builder.build());
macroState++;
macroTime = getRuntime();
}
break;
case 14:
// if drive is done move on
if (getRuntime() > macroTime + 1.4 || !robot.drive.isBusy()) {
macroTime = getRuntime();
robot.macroState = 0;
robot.extendMacro(Slides.mini_tier1 + 100, getRuntime());
macroState++;
}
break;
//extending the macro and about to score
case 15:
if (robot.macroState != 0) {
robot.extendMacro(Slides.mini_tier1 + 100, getRuntime());
}
if (robot.macroState == 0 && !robot.drive.isBusy()) {
robot.resetMacro(0, getRuntime());
macroState++;
}
break;
// PARK ROBOT
case 16:
robot.resetMacro(0, getRuntime());
if (robot.macroState == 0) {
macroState = -1;
}
// PARK ROBOT
//
}
}
}

235
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/RedFrontPreload.java
View File

@ -1,235 +0,0 @@
package org.firstinspires.ftc.teamcode.opmode.autonomous;
import static org.firstinspires.ftc.teamcode.hardware.Arm.DoorPosition.CLOSE;
import static org.firstinspires.ftc.teamcode.hardware.Arm.DoorPosition.OPEN;
import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK1;
import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK2;
import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK3;
import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK4;
import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK5;
import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK6;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.geometry.Vector2d;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import org.firstinspires.ftc.teamcode.hardware.Slides;
import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
@Config
@Autonomous(name = "Red FrontPreload (2+1)", group = "Competition", preselectTeleOp = "Red Teleop")
public class RedFrontPreload extends AutoBase {
public static final Pose2d DROP_1_PART_2 = new Pose2d(-33, -33.5, Math.toRadians(180));
public static final Pose2d DROP_1 = new Pose2d(-33,-33.5,Math.toRadians(180));
public static final Pose2d DROP_2 = new Pose2d(-35.7, -33.5, Math.toRadians(90));
public static final Pose2d DROP_2_PART_2 = new Pose2d(-37.7,-42, Math.toRadians(90));
public static final Pose2d DROP_3 = new Pose2d(-51, -50.5, Math.toRadians(90));
public static final Pose2d DROP_1M = new Pose2d(-36, -45, Math.toRadians(90));
public static final Pose2d DROP_2M = new Pose2d(-48.5, -30, Math.toRadians(90));
public static final Pose2d DROP_3M = new Pose2d(-53.7, -35.9, Math.toRadians(90));
public static final Pose2d DEPOSIT_WHITE_STACKS_1 = new Pose2d(53.3, -38.3, Math.toRadians(180));//187
public static final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(47, -32, Math.toRadians(187));//187
public static final Pose2d DEPOSIT_WHITE_STACKS_3 = new Pose2d(55, -25, Math.toRadians(180));//817
public static final Pose2d STACK_LOCATION_1 = new Pose2d(-54.5, -32.6, Math.toRadians(180));
public static final Pose2d STACK_LOCATION_2 = new Pose2d(-57, -37, Math.toRadians(180));
public static final Pose2d STACK_LOCATION_3 = new Pose2d(-52, -29.6, Math.toRadians(180));
public static final Pose2d POST_SCORING_SPLINE_END = new Pose2d(24, -12.4, Math.toRadians(190));//-36
public static final Pose2d POST_DROP_POS = new Pose2d(-45, -59.5, Math.toRadians(0));
public static final Pose2d POST_DROP_POS_PART2 = new Pose2d(-40, -58.5, Math.toRadians(0));
public static final Pose2d PRE_DEPOSIT_POS = new Pose2d(8, -58.5, Math.toRadians(0));
public static final Pose2d PARK_1 = new Pose2d(45,-58,Math.toRadians(180));
public static final Pose2d PARK_2 = new Pose2d(58,-58,Math.toRadians(180));
@Override
public void createTrajectories() {
// set start position
Pose2d start = new Pose2d(-36, -61.5, Math.toRadians(90));
robot.drive.setPoseEstimate(start);
robot.camera.setAlliance(CenterStageCommon.Alliance.Red);
}
@Override
public void followTrajectories() {
TrajectorySequenceBuilder builder = null;
switch (macroState) {
case 0:
builder = this.robot.getTrajectorySequenceBuilder();
switch (teamPropLocation) {
case 1:
builder.lineToLinearHeading(DROP_1M);
builder.lineToLinearHeading(DROP_1);
break;
case 2:
builder.lineToLinearHeading(DROP_2);
break;
case 3:
builder.lineToLinearHeading(DROP_3M);
// builder.lineToLinearHeading(DROP_3);
break;
}
this.robot.drive.followTrajectorySequenceAsync(builder.build());
macroState++;
break;
// DRIVE TO TAPE
case 1:
// if drive is done move on
if (!robot.drive.isBusy()) {
macroTime = getRuntime();
macroState++;
}
break;
// RUN INTAKE
case 2:
// intake
if (getRuntime() < macroTime + 0.1) {
}
else{
builder = this.robot.getTrajectorySequenceBuilder();
robot.intake.setDcMotor(0);
switch (teamPropLocation) {
case 1:
builder.lineToLinearHeading(DROP_1_PART_2);
break;
case 2:
builder.lineToLinearHeading(DROP_2_PART_2);
break;
case 3:
builder.lineToLinearHeading(DROP_3);
break;
}
robot.arm.setDoor(OPEN);
switch (teamPropLocation) {
case (1):
//team prop location 1
builder.lineToLinearHeading(STACK_LOCATION_1.plus(new Pose2d(0))).waitSeconds(.01);
break;
case (2):
//team prop location 2
builder.lineToLinearHeading(STACK_LOCATION_2).waitSeconds(.01);
break;
case (3):
//team prop location 3
builder.lineToLinearHeading(STACK_LOCATION_3.plus(new Pose2d(-4.8,-2))).waitSeconds(.01);
break;
}
robot.intake.setDcMotor(0.44);
robot.intake.setpos(STACK6);
macroTime = getRuntime();
this.robot.drive.followTrajectorySequenceAsync(builder.build());
macroState++;
}
// if intake is done move on
break;
case 3:
// if drive is done move on
if (!robot.drive.isBusy()) {
macroTime = getRuntime();
macroState++;
}
break;
case 4:
if (getRuntime() > macroTime + 0.1) {
robot.arm.setDoor(CLOSE);
robot.intake.setDcMotor(-0.1);
builder = this.robot.getTrajectorySequenceBuilder();
switch (teamPropLocation) {
case 1:
builder.setTangent(Math.toRadians(-90));
builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(180));
builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec(),Math.toRadians(180));
break;
case 2:
builder.setTangent(Math.toRadians(-90));
builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(180));
builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec().plus(new Vector2d((3.8))),Math.toRadians(180));
break;
case 3:
builder.setTangent(Math.toRadians(-90));
builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(180));
builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_3.vec(),Math.toRadians(180));
break;
}
this.robot.drive.followTrajectorySequenceAsync(builder.build());
macroState++;
macroTime = getRuntime();
}
break;
case 5:
// if drive is done move on
if (getRuntime() > macroTime + 4.4 || !robot.drive.isBusy()) {
macroTime = getRuntime();
robot.macroState = 0;
robot.extendMacro(Slides.mini_tier1-30, getRuntime());
macroState++;
}
break;
//extending the macro and about to score
case 6:
if (robot.macroState != 0) {
robot.extendMacro(Slides.mini_tier1, getRuntime());
}
if (robot.macroState == 0 && !robot.drive.isBusy()) {
robot.resetMacro(0, getRuntime());
macroState++;
}
break;
//Park
case 7:
robot.resetMacro(0, getRuntime());
if (getRuntime() > macroTime + 3.4 || robot.macroState >= 4) {
builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(PARK_1);
this.robot.drive.followTrajectorySequenceAsync(builder.build());
macroState++;
}
break;
case 8:
// if drive is done move on
if (getRuntime() > macroTime + 4.4 || !robot.drive.isBusy()) {
macroState++;
}
break;
case 9:
robot.resetMacro(0, getRuntime());
if (robot.macroState == 0) {
macroState = -1;
}
case 19:
robot.resetMacro(0, getRuntime());
if (robot.macroState == 0) {
macroState = -1;
}
}
}
}

365
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/RedFrontStageAuto.java
View File

@ -1,365 +0,0 @@
package org.firstinspires.ftc.teamcode.opmode.autonomous;
import static org.firstinspires.ftc.teamcode.hardware.Arm.DoorPosition.CLOSE;
import static org.firstinspires.ftc.teamcode.hardware.Arm.DoorPosition.OPEN;
import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK1;
import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK2;
import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK3;
import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK4;
import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK5;
import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK6;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import org.firstinspires.ftc.teamcode.hardware.Slides;
import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
@Config
@Autonomous(name = "JANK Red FrontStage Auto(2+3)", group = "Competition", preselectTeleOp = "Main TeleOp")
public class RedFrontStageAuto extends AutoBase {
public static final Pose2d DROP_1 = new Pose2d(-35, -32.5, Math.toRadians(180)); //THIS ANGLE NEEDS TO BE CHANGED
public static final Pose2d DROP_2 = new Pose2d(-39.7, -33.5, Math.toRadians(90));
public static final Pose2d DROP_3 = new Pose2d(-49, -33.5, Math.toRadians(90));
public static final Pose2d DROP_1M = new Pose2d(-48.5, -30, Math.toRadians(90));
public static final Pose2d DROP_2M = new Pose2d(-48.5, -30, Math.toRadians(90));
public static final Pose2d DROP_3M = new Pose2d(-48.5, -30, Math.toRadians(90));
public static final Pose2d DEPOSIT_WHITE_STACKS_1 = new Pose2d(53.3, -38.3, Math.toRadians(188));//187
public static final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(52, -34, Math.toRadians(188));//187
public static final Pose2d DEPOSIT_WHITE_STACKS_3 = new Pose2d(53.6, -32, Math.toRadians(188));//817
public static final Pose2d STACK_LOCATION = new Pose2d(-51.5, -33.6, Math.toRadians(0));
public static final Pose2d POST_SCORING_SPLINE_END = new Pose2d(24, -12.4, Math.toRadians(190));//-36
public static final Pose2d POST_DROP_POS = new Pose2d(-45, -59.5, Math.toRadians(0));
public static final Pose2d POST_DROP_POS_PART2 = new Pose2d(-33, -59.5, Math.toRadians(0));
public static final Pose2d PRE_DEPOSIT_POS = new Pose2d(33, -59.5, Math.toRadians(0));
@Override
public void createTrajectories() {
// set start position
Pose2d start = new Pose2d(-36, -61.5, Math.toRadians(90));
robot.drive.setPoseEstimate(start);
robot.camera.setAlliance(CenterStageCommon.Alliance.Blue);
}
@Override
public void followTrajectories() {
TrajectorySequenceBuilder builder = null;
switch (macroState) {
case 0:
builder = this.robot.getTrajectorySequenceBuilder();
switch (teamPropLocation) {
case 1:
builder.lineToLinearHeading(DROP_1);
break;
case 2:
builder.lineToLinearHeading(DROP_2);
break;
case 3:
builder.lineToLinearHeading(DROP_3);
break;
}
this.robot.drive.followTrajectorySequenceAsync(builder.build());
macroState++;
break;
// DRIVE TO TAPE
case 1:
// if drive is done move on
if (!robot.drive.isBusy()) {
macroTime = getRuntime();
macroState++;
}
break;
// RUN INTAKE
case 2:
// intake
if (getRuntime() < macroTime + 0.32) {
robot.intake.setDcMotor(-0.18);
}
else{
builder = this.robot.getTrajectorySequenceBuilder();
robot.intake.setDcMotor(0);
robot.arm.setDoor(OPEN);
builder.lineToLinearHeading(STACK_LOCATION.plus(new Pose2d(-5.4,1.5))).waitSeconds(.01);
robot.intake.setDcMotor(0.44);
robot.intake.setpos(STACK6);
macroTime = getRuntime();
this.robot.drive.followTrajectorySequenceAsync(builder.build());
macroState++;
}
// if intake is done move on
break;
case 3:
// if drive is done move on
if (!robot.drive.isBusy()) {
macroTime = getRuntime();
macroState++;
}
break;
case 4:
if (getRuntime() > macroTime + 0.06) {
robot.arm.setDoor(CLOSE);
robot.intake.setDcMotor(-0.8);
builder = this.robot.getTrajectorySequenceBuilder();
// //builder.lineToConstantHeading(POST_SCORING_SPLINE_END);
// builder.lineToConstantHeading(POST_DROP_POS.vec());
// builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(), Math.toRadians(0));
switch (teamPropLocation) {
case 1:
builder.setTangent(Math.toRadians(-90));
builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(180));
builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec(),Math.toRadians(180));
break;
case 2:
builder.setTangent(Math.toRadians(-90));
builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(180));
builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec(),Math.toRadians(180));
break;
case 3:
builder.setTangent(Math.toRadians(-90));
builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(180));
builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_3.vec(),Math.toRadians(180));
break;
// case 1:
// builder.setTangent(Math.toRadians(90))
// builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0));
// builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
// builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec(),Math.toRadians(0));
// break;
// case 2:
// builder.lineToLinearHeading(POST_DROP_POS);
// builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0));
// builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
// builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec(), Math.toRadians(0));
// break;
// case 3:
// builder.lineToLinearHeading(POST_DROP_POS);
// builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0));
// builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
// builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_3.vec(), Math.toRadians(0));
// break;
}
this.robot.drive.followTrajectorySequenceAsync(builder.build());
macroState++;
macroTime = getRuntime();
}
break;
case 5:
// if drive is done move on
if (getRuntime() > macroTime + 4.4 || !robot.drive.isBusy()) {
macroTime = getRuntime();
robot.macroState = 0;
robot.extendMacro(Slides.mini_tier1-70, getRuntime());
macroState++;
}
break;
//extending the macro and about to score
case 6:
if (robot.macroState != 0) {
robot.extendMacro(Slides.mini_tier1, getRuntime());
}
if (robot.macroState == 0 && !robot.drive.isBusy()) {
robot.resetMacro(0, getRuntime());
macroState++;
}
break;
//Stack run 2
case 7:
robot.resetMacro(0, getRuntime());
if (getRuntime() > macroTime + 3.4 || robot.macroState >= 4) {
builder = this.robot.getTrajectorySequenceBuilder();
builder.splineToConstantHeading(PRE_DEPOSIT_POS.plus(new Pose2d(0,2)).vec(), Math.toRadians(0));
builder.lineToConstantHeading(POST_DROP_POS.plus(new Pose2d(0,2)).vec());
builder.splineToConstantHeading(STACK_LOCATION.plus(new Pose2d(-3.9, 3.7)).vec(), Math.toRadians(0));
this.robot.drive.followTrajectorySequenceAsync(builder.build());
macroState++;
}
break;
//waits for the robot to fin the trajectory
case 8:
robot.resetMacro(0, getRuntime());
robot.arm.setDoor(OPEN);
robot.intake.setDcMotor(0.54);
robot.intake.setpos(STACK4);
if (!robot.drive.isBusy()) {
macroState++;
}
break;
//3rd and 4th pixels off the stack are in-taken by this
case 9:
robot.intake.setDcMotor(0.54);
robot.arm.setDoor(OPEN);
robot.intake.setpos(STACK3);
macroTime = getRuntime();
macroState++;
break;
case 10:
if (getRuntime() > macroTime + 0.6) {
robot.arm.setDoor(CLOSE);
robot.intake.setDcMotor(-0.45);
builder = this.robot.getTrajectorySequenceBuilder();
//builder.lineToConstantHeading(POST_SCORING_SPLINE_END);
//builder.splineToLinearHeading(POST_DROP_POS, Math.toRadians(0));
//builder.lineToLinearHeading(POST_DROP_POS_PART2.plus(new Pose2d(0,2)));
switch (teamPropLocation) {
case 1:
builder.setTangent(Math.toRadians(90));
builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0));
builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec(),Math.toRadians(0));
break;
case 2:
builder.setTangent(Math.toRadians(90));
builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0));
builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec(),Math.toRadians(0));
break;
case 3:
builder.setTangent(Math.toRadians(90));
builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0));
builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_3.vec(),Math.toRadians(0));
break;
}
this.robot.drive.followTrajectorySequenceAsync(builder.build());
macroState++;
macroTime = getRuntime();
}
break;
case 11:
// if drive is done move on
if (getRuntime() > macroTime + 6.4 || !robot.drive.isBusy()) {
macroTime = getRuntime();
robot.macroState = 0;
robot.extendMacro(Slides.mini_tier1 + 20, getRuntime());
macroState++;
}
break;
//extending the macro and about to score
case 12:
if (robot.macroState != 0) {
robot.extendMacro(Slides.mini_tier1 + 80, getRuntime());
}
if (robot.macroState == 0 && !robot.drive.isBusy()) {
robot.resetMacro(0, getRuntime());
macroState++;
}
break;
case 13:
robot.resetMacro(0, getRuntime());
if (robot.macroState == 0) {
macroState = -1;
}
//stack run 3
// case 13:
// robot.resetMacro(0, getRuntime());
// if (getRuntime() > macroTime + 2.4 || robot.macroState >= 4) {
// builder = this.robot.getTrajectorySequenceBuilder();
// builder.splineToConstantHeading(PRE_DEPOSIT_POS.vec(), Math.toRadians(180));
// builder.lineToLinearHeading(POST_DROP_POS);
// builder.splineToConstantHeading(STACK_LOCATION.plus(new Pose2d(-1.5)).vec(), Math.toRadians(180));
// this.robot.drive.followTrajectorySequenceAsync(builder.build());
// macroState++;
// }
// break;
//
// //waits for the robot to fin the trajectory
// case 14:
// robot.resetMacro(0, getRuntime());
// robot.arm.setDoor(OPEN);
// robot.intake.setDcMotor(0.54);
// robot.intake.setpos(STACK2);
// if (!robot.drive.isBusy()) {
// macroState++;
// }
// break;
// //3rd and 4th pixels off the stack are intaken by this
// case 15:
// robot.intake.setDcMotor(0.54);
// robot.arm.setDoor(OPEN);
// robot.intake.setpos(STACK1);
// macroTime = getRuntime();
// macroState++;
// break;
//
// case 16:
// if (getRuntime() > macroTime + 0.6) {
// robot.intake.setDcMotor(0);
// builder = this.robot.getTrajectorySequenceBuilder();
// //builder.lineToConstantHeading(POST_SCORING_SPLINE_END);
// builder.splineToSplineHeading(POST_DROP_POS, Math.toRadians(0));
//
// switch (teamPropLocation) {
// case 1:
// builder.lineToLinearHeading(PRE_DEPOSIT_POS);
// builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec(), Math.toRadians(0));
// break;
// case 2:
// builder.lineToLinearHeading(PRE_DEPOSIT_POS);
// builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec(), Math.toRadians(0));
// break;
// case 3:
// builder.lineToLinearHeading(PRE_DEPOSIT_POS);
// builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_3.vec(), Math.toRadians(0));
// break;
// }
// this.robot.drive.followTrajectorySequenceAsync(builder.build());
// macroState++;
// macroTime = getRuntime();
// }
// break;
// case 17:
// // if drive is done move on
// if (getRuntime() > macroTime + 4.4 || !robot.drive.isBusy()) {
// macroTime = getRuntime();
// robot.macroState = 0;
// robot.extendMacro(Slides.mini_tier1 + 180, getRuntime());
// macroState++;
// }
// break;
// //extending the macro and about to score
// case 18:
// if (robot.macroState != 0) {
// robot.extendMacro(Slides.mini_tier1 + 80, getRuntime());
// }
// if (robot.macroState == 0 && !robot.drive.isBusy()) {
// robot.resetMacro(0, getRuntime());
// macroState++;
// }
// break;
case 19:
robot.resetMacro(0, getRuntime());
if (robot.macroState == 0) {
macroState = -1;
}
}
}
}

304
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/autonomous/RedFrontStageme.java
View File

@ -1,304 +0,0 @@
package org.firstinspires.ftc.teamcode.opmode.autonomous;
import static org.firstinspires.ftc.teamcode.hardware.Arm.DoorPosition.CLOSE;
import static org.firstinspires.ftc.teamcode.hardware.Arm.DoorPosition.OPEN;
import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK1;
import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK2;
import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK3;
import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK4;
import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK5;
import static org.firstinspires.ftc.teamcode.hardware.Intake.Position.STACK6;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.roadrunner.geometry.Pose2d;
import com.acmerobotics.roadrunner.geometry.Vector2d;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import org.firstinspires.ftc.teamcode.hardware.Slides;
import org.firstinspires.ftc.teamcode.roadrunner.trajectorysequence.TrajectorySequenceBuilder;
import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
@Config
@Autonomous(name = "Red FrontPreload (2+2)", group = "Competition", preselectTeleOp = "Red Teleop")
public class RedFrontStageme extends AutoBase {
public static final Pose2d DROP_1_PART_2 = new Pose2d(-33, -33.5, Math.toRadians(180));
public static final Pose2d DROP_1 = new Pose2d(-33,-33.5,Math.toRadians(180));
public static final Pose2d DROP_2 = new Pose2d(-35.7, -33.5, Math.toRadians(90));
public static final Pose2d DROP_2_PART_2 = new Pose2d(-35.7,-41, Math.toRadians(90));
public static final Pose2d DROP_2_PART_3 = new Pose2d(-40.7,-42, Math.toRadians(180));
public static final Pose2d DROP_3 = new Pose2d(-51, -50.5, Math.toRadians(90));
public static final Pose2d DROP_1M = new Pose2d(-36, -45, Math.toRadians(90));
public static final Pose2d DROP_2M = new Pose2d(-48.5, -30, Math.toRadians(90));
public static final Pose2d DROP_3M = new Pose2d(-53.7, -35.9, Math.toRadians(90));
public static final Pose2d DEPOSIT_WHITE_STACKS_1 = new Pose2d(53.3, -38.3, Math.toRadians(180));//187
public static final Pose2d DEPOSIT_WHITE_STACKS_2 = new Pose2d(52, -32, Math.toRadians(187));//187
public static final Pose2d DEPOSIT_WHITE_STACKS_3 = new Pose2d(55, -25, Math.toRadians(180));//817
public static final Pose2d STACK_LOCATION_1 = new Pose2d(-54.5, -32.6, Math.toRadians(180));
public static final Pose2d STACK_LOCATION_2 = new Pose2d(-57, -37, Math.toRadians(180));
public static final Pose2d STACK_LOCATION_3 = new Pose2d(-52, -29.6, Math.toRadians(180));
public static final Pose2d POST_SCORING_SPLINE_END = new Pose2d(24, -12.4, Math.toRadians(190));//-36
public static final Pose2d POST_DROP_POS = new Pose2d(-45, -59.5, Math.toRadians(0));
public static final Pose2d POST_DROP_POS_PART2 = new Pose2d(-40, -58.5, Math.toRadians(180));
public static final Pose2d PRE_DEPOSIT_POS = new Pose2d(8, -58.5, Math.toRadians(180));
public static final Pose2d PARK_1 = new Pose2d(45,-58,Math.toRadians(180));
public static final Pose2d PARK_2 = new Pose2d(58,-58,Math.toRadians(180));
@Override
public void createTrajectories() {
// set start position
Pose2d start = new Pose2d(-36, -61.5, Math.toRadians(90));
robot.drive.setPoseEstimate(start);
robot.camera.setAlliance(CenterStageCommon.Alliance.Red);
}
@Override
public void followTrajectories() {
TrajectorySequenceBuilder builder = null;
switch (macroState) {
case 0:
builder = this.robot.getTrajectorySequenceBuilder();
switch (teamPropLocation) {
case 1:
builder.lineToLinearHeading(DROP_1M);
builder.lineToLinearHeading(DROP_1);
break;
case 2:
builder.lineToLinearHeading(DROP_2);
break;
case 3:
builder.lineToLinearHeading(DROP_3M);
// builder.lineToLinearHeading(DROP_3);
break;
}
this.robot.drive.followTrajectorySequenceAsync(builder.build());
macroState++;
break;
// DRIVE TO TAPE
case 1:
// if drive is done move on
if (!robot.drive.isBusy()) {
macroTime = getRuntime();
macroState++;
}
break;
// RUN INTAKE
case 2:
// intake
if (getRuntime() < macroTime + 0.1) {
}
else{
builder = this.robot.getTrajectorySequenceBuilder();
robot.intake.setDcMotor(0);
switch (teamPropLocation) {
case 1:
builder.lineToLinearHeading(DROP_1_PART_2);
break;
case 2:
builder.lineToLinearHeading(DROP_2_PART_2);
builder.lineToLinearHeading(DROP_2_PART_3);
break;
case 3:
builder.lineToLinearHeading(DROP_3);
break;
}
//robot.arm.setDoor(OPEN);
macroTime = getRuntime();
this.robot.drive.followTrajectorySequenceAsync(builder.build());
macroState++;
}
// if intake is done move on
break;
case 3:
// if drive is done move on
if (!robot.drive.isBusy()) {
macroTime = getRuntime();
macroState++;
}
break;
case 4:
if (getRuntime() > macroTime + 0.1) {
//robot.arm.setDoor(CLOSE);
robot.intake.setDcMotor(-0.1);
builder = this.robot.getTrajectorySequenceBuilder();
switch (teamPropLocation) {
case 1:
builder.setTangent(Math.toRadians(-90));
builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0));
builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec(),Math.toRadians(0));
break;
case 2:
builder.setTangent(Math.toRadians(-90));
builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0));
builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec(),Math.toRadians(0));
break;
case 3:
builder.setTangent(Math.toRadians(-90));
builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0));
builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_3.vec(),Math.toRadians(0));
break;
}
this.robot.drive.followTrajectorySequenceAsync(builder.build());
macroState++;
macroTime = getRuntime();
}
break;
case 5:
// if drive is done move on
if (getRuntime() > macroTime + 4.4 || !robot.drive.isBusy()) {
macroTime = getRuntime();
robot.macroState = 0;
robot.extendMacro(Slides.mini_tier1-30, getRuntime());
macroState++;
}
break;
//extending the macro and about to score
case 6:
if (robot.macroState != 0) {
robot.extendMacro(Slides.mini_tier1, getRuntime());
}
if (robot.macroState == 0 && !robot.drive.isBusy()) {
robot.resetMacro(0, getRuntime());
macroState++;
}
break;
case 7:
robot.resetMacro(0,getRuntime());
if (getRuntime() > macroTime + 3.4 || robot.macroState >= 4) {
builder = this.robot.getTrajectorySequenceBuilder();
builder.splineToConstantHeading(PRE_DEPOSIT_POS.vec(),Math.toRadians(0));
builder.lineToConstantHeading(POST_DROP_POS_PART2.vec());
builder.splineToConstantHeading(STACK_LOCATION_2.vec(),Math.toRadians(0));
this.robot.drive.followTrajectorySequenceAsync(builder.build());
macroState++;
}
break;
case 8:
robot.arm.setDoor(OPEN);
robot.intake.setDcMotor(0.44);
robot.intake.setpos(STACK5);
macroTime = getRuntime();
macroState++;
break;
case 9:
if (getRuntime() > macroTime + 0.06) {
robot.arm.setDoor(CLOSE);
robot.intake.setDcMotor(-0.5);
builder = this.robot.getTrajectorySequenceBuilder();
switch (teamPropLocation) {
case 1:
builder.setTangent(Math.toRadians(-90));
builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0));
builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_2.vec(),Math.toRadians(0));
break;
case 2:
builder.setTangent(Math.toRadians(-90));
builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0));
builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec(),Math.toRadians(0));
break;
case 3:
builder.setTangent(Math.toRadians(-90));
builder.splineToConstantHeading(POST_DROP_POS_PART2.vec(),Math.toRadians(0));
builder.lineToConstantHeading(PRE_DEPOSIT_POS.vec());
builder.splineToConstantHeading(DEPOSIT_WHITE_STACKS_1.vec(),Math.toRadians(0));
break;
}
this.robot.drive.followTrajectorySequenceAsync(builder.build());
macroState++;
macroTime = getRuntime();
}
break;
case 10:
// if drive is done move on
if (getRuntime() > macroTime + 2.4 || !robot.drive.isBusy()) {
macroTime = getRuntime();
robot.macroState = 0;
robot.extendMacro(Slides.mini_tier1-30, getRuntime());
macroState++;
}
break;
case 11:
if (robot.macroState != 0) {
robot.extendMacro(Slides.mini_tier1, getRuntime());
}
if (robot.macroState == 0 && !robot.drive.isBusy()) {
robot.resetMacro(0, getRuntime());
macroState++;
}
break;
//Park
case 12:
robot.resetMacro(0, getRuntime());
if (getRuntime() > macroTime + 3.4 || robot.macroState >= 4) {
builder = this.robot.getTrajectorySequenceBuilder();
builder.lineToLinearHeading(PARK_1);
this.robot.drive.followTrajectorySequenceAsync(builder.build());
macroState++;
}
break;
//Park
// case 7:
// robot.resetMacro(0, getRuntime());
// if (getRuntime() > macroTime + 3.4 || robot.macroState >= 4) {
// builder = this.robot.getTrajectorySequenceBuilder();
// builder.lineToLinearHeading(PARK_1);
// this.robot.drive.followTrajectorySequenceAsync(builder.build());
// macroState++;
// }
// break;
//
// case 8:
// // if drive is done move on
// if (getRuntime() > macroTime + 4.4 || !robot.drive.isBusy()) {
// macroState++;
// }
// break;
//
// case 9:
// robot.resetMacro(0, getRuntime());
// if (robot.macroState == 0) {
// macroState = -1;
// }
case 19:
robot.resetMacro(0, getRuntime());
if (robot.macroState == 0) {
macroState = -1;
}
}
}
}

259
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/BlueTeleop.java
View File

@ -1,259 +0,0 @@
package org.firstinspires.ftc.teamcode.opmode.teleop;
import static org.firstinspires.ftc.teamcode.hardware.Arm.DoorPosition.CLOSE;
import static org.firstinspires.ftc.teamcode.hardware.Arm.DoorPosition.OPEN;
import com.acmerobotics.dashboard.FtcDashboard;
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.DcMotor;
import com.qualcomm.robotcore.hardware.Servo;
import com.qualcomm.robotcore.hardware.VoltageSensor;
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.Slides;
@Config
@TeleOp(name = "Blue TeleOp", group = "OpModes")
public class BlueTeleop extends OpMode {
public static double normal = 0.7;
public static double turbo = 1;
public static double slow_mode = 0.35;
public static double intakeMax = 0.36;
public static double intakeMax2 = -0.70;
private Robot robot;
private Controller controller1;
private Controller controller2;
public boolean hang_counter = false;
private DcMotor[] motors = new DcMotor[8];
private Servo[] servos = new Servo[7];
@Override
public void init() {
this.motors[0] = this.hardwareMap.get(DcMotor.class, "Rightslide");
this.motors[1] = this.hardwareMap.get(DcMotor.class, "Intakemotor");
this.motors[2] = this.hardwareMap.get(DcMotor.class, "FrontRight");
this.motors[3] = this.hardwareMap.get(DcMotor.class, "BackRight");
this.motors[4] = this.hardwareMap.get(DcMotor.class, "BackLeft");
this.motors[5] = this.hardwareMap.get(DcMotor.class, "FrontLeft");
this.motors[6] = this.hardwareMap.get(DcMotor.class, "Leftslide");
this.motors[7] = this.hardwareMap.get(DcMotor.class, "Hang");
this.servos[0] = this.hardwareMap.get(Servo.class, "LeftIntake");
this.servos[1] = this.hardwareMap.get(Servo.class, "Wrist");
this.servos[2] = this.hardwareMap.get(Servo.class, "Elbow");
this.servos[3] = this.hardwareMap.get(Servo.class, "RightArm");
this.servos[4] = this.hardwareMap.get(Servo.class, "LeftArm");
this.servos[5] = this.hardwareMap.get(Servo.class, "Right Intake Servo");
this.servos[6] = this.hardwareMap.get(Servo.class, "Drone");
controller1 = new Controller(gamepad1);
controller2 = new Controller(gamepad2);
this.robot = new Robot(hardwareMap);
// robot.intake.setpos(Intake.Position.STACK1);
// while (robot.camera.getFrameCount() < 1) {
// telemetry.addLine("Initializing...");
// telemetry.update();
// }
telemetry.addLine("Initialized");
this.telemetry = FtcDashboard.getInstance().getTelemetry();
telemetry.update();
this.robot.alliance = 1;
this.robot.leds.setPattern(92);
}
double getBatteryVoltage() {
double result = Double.POSITIVE_INFINITY;
for (VoltageSensor sensor : hardwareMap.voltageSensor) {
double voltage = sensor.getVoltage();
if (voltage > 0) {
result = Math.min(result, voltage);
}
}
return result;
}
@Override
public void loop() {
for (DcMotor motor : this.motors) {
this.telemetry.addData(this.hardwareMap.getNamesOf(motor).stream().findFirst().get(), motor.getPower());
}
for (Servo servo : this.servos) {
this.telemetry.addData(this.hardwareMap.getNamesOf(servo).stream().findFirst().get(), servo.getPosition());
}
this.telemetry.addData("battery", getBatteryVoltage());
this.telemetry.update();
this.robot.leds.setPattern(93);
// 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 if (controller1.getLeftTrigger().getValue() > 0.1) {
x *= slow_mode;
y *= slow_mode;
z *= slow_mode;
} else {
x *= normal;
y *= normal;
z *= normal;
}
robot.drive.setWeightedDrivePower(new Pose2d(x, y, z));
// Drone launcher
if (controller1.getA().isPressed() && !controller1.getStart().isPressed() && !controller2.getStart().isPressed()) {
this.robot.endGameMechs.launch();
} else {
this.robot.endGameMechs.reset();
}
// if (controller1.getRightBumper().isPressed()) {
// this.robot.endGameMechs.Finger_in();
// }else {
// this.robot.endGameMechs.Finger_out();
// }
//Hang Motor
// if (controller1.getB().isJustPressed() && !controller1.getStart().isPressed() && !controller2.getStart().isPressed() && !hang_counter){
// this.robot.endGameMechs.hang_init_pos();
// hang_counter = true;
// }
// else if (controller1.getB().isJustPressed() && !controller1.getStart().isPressed() && !controller2.getStart().isPressed() && hang_counter){
// this.robot.endGameMechs.hang_final_pos();
// hang_counter = false;
// }
if (controller1.getB().isPressed()) {
this.robot.endGameMechs.hang_final_pos();
} else {
this.robot.endGameMechs.hang_init_pos();
}
if (controller1.getX().isPressed()) {
this.robot.intake.wheel_spit();
} else if (controller1.getY().isPressed()) {
this.robot.intake.wheel_swallow();
} else {
this.robot.intake.wheel_stop();
}
// Driver 2
if (controller2.getRightTrigger().getValue()>=0.1){
robot.intake.setDcMotor(controller2.getRightTrigger().getValue()*intakeMax);
}
else if(controller2.getLeftTrigger().getValue()>=0.1){
robot.intake.setDcMotor(controller2.getLeftTrigger().getValue()*intakeMax2);
}
else {
robot.intake.setDcMotor(0);
}
if (controller2.getRightBumper().isJustPressed()) {
robot.intake.incrementPos();
}
if (controller2.getLeftBumper().isJustPressed()) {
robot.intake.decrementPos();
}
//ElbowPos
if (controller2.getDRight().isPressed()){
robot.arm.setElbowPos(4);
} else if (controller2.getDLeft().isPressed()) {
robot.arm.setElbowPos(3);
}else {
robot.arm.setElbowPos(1);
}
// macros
switch (robot.runningMacro) {
case (0): // manual mode
robot.slides.increaseTarget(controller2.getLeftStick().getY());
// if (robot.intake.getPower() >= 0.01) {
// robot.arm.setDoor(OPEN);
// } else if (robot.intake.getPower() <= -0.01) {
// robot.arm.setDoor(OPEN);
// } else if (controller2.getLeftBumper().isPressed()) {
// robot.arm.setDoor(Arm.DoorPosition.OPEN);
// } else {
// robot.arm.setDoor(CLOSE);
// }
if (robot.intake.getPower() >= 0.01) {
robot.intake.wheel_swallow();
} else if (robot.intake.getPower() <= -0.01) {
robot.intake.wheel_swallow();
} else if (controller2.getLeftBumper().isPressed()) {
robot.intake.wheel_swallow();
} else {
robot.intake.wheel_stop();
}
//Start A/B stuff
if (controller2.getX().isJustPressed()) {
robot.runningMacro = 1;
} else if (controller2.getY().isJustPressed()) {
robot.runningMacro = 2;
} else if (controller2.getB().isJustPressed() && !controller1.getStart().isPressed() && !controller2.getStart().isPressed()) {
robot.runningMacro = 3;
} else if (controller2.getA().isJustPressed() && !controller1.getStart().isPressed() && !controller2.getStart().isPressed()) {
robot.runningMacro = 4;
} else if (controller2.getDDown().isJustPressed() ) {
robot.runningMacro = 5;
}
break;
case (1):
robot.extendMacro(Slides.tier1, getRuntime());
break;
case (2):
robot.extendMacro(Slides.tier2, getRuntime());
break;
case (3):
robot.extendMacro(Slides.tier3, getRuntime());
break;
case (4):
robot.resetMacro(0, getRuntime());
break;
case(5):
robot.extendMacro(Slides.mini_tier1, getRuntime());
break;
}
// update and telemetry
robot.update(getRuntime());
controller1.update();
controller2.update();
telemetry.addLine(robot.getTelemetry());
telemetry.update();
}
}

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

@ -1,47 +0,0 @@
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;
}
}
}

235
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/MainTeleOp.java
View File

@ -1,235 +0,0 @@
package org.firstinspires.ftc.teamcode.opmode.teleop;
import static org.firstinspires.ftc.teamcode.hardware.Arm.DoorPosition.CLOSE;
import static org.firstinspires.ftc.teamcode.hardware.Arm.DoorPosition.OPEN;
import com.acmerobotics.dashboard.FtcDashboard;
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.DcMotor;
import com.qualcomm.robotcore.hardware.Servo;
import com.qualcomm.robotcore.hardware.VoltageSensor;
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.Slides;
@Config
@TeleOp(name = "Main TeleOp", group = "OpModes")
public class MainTeleOp extends OpMode {
public static double normal = 0.7;
public static double turbo = 1;
public static double slow_mode = 0.35;
public static double intakeMax = 0.36;
public static double intakeMax2 = -0.70;
private Robot robot;
private Controller controller1;
private Controller controller2;
public boolean hang_counter = false;
private DcMotor[] motors = new DcMotor[8];
private Servo[] servos = new Servo[7];
@Override
public void init() {
this.motors[0] = this.hardwareMap.get(DcMotor.class, "Rightslide");
this.motors[1] = this.hardwareMap.get(DcMotor.class, "Intakemotor");
this.motors[2] = this.hardwareMap.get(DcMotor.class, "FrontRight");
this.motors[3] = this.hardwareMap.get(DcMotor.class, "BackRight");
this.motors[4] = this.hardwareMap.get(DcMotor.class, "BackLeft");
this.motors[5] = this.hardwareMap.get(DcMotor.class, "FrontLeft");
this.motors[6] = this.hardwareMap.get(DcMotor.class, "Leftslide");
this.motors[7] = this.hardwareMap.get(DcMotor.class, "Hang");
this.servos[0] = this.hardwareMap.get(Servo.class, "LeftIntake");
this.servos[1] = this.hardwareMap.get(Servo.class, "Wrist");
this.servos[2] = this.hardwareMap.get(Servo.class, "Door");
this.servos[3] = this.hardwareMap.get(Servo.class, "RightArm");
this.servos[4] = this.hardwareMap.get(Servo.class, "LeftArm");
this.servos[5] = this.hardwareMap.get(Servo.class, "Right Intake Servo");
this.servos[6] = this.hardwareMap.get(Servo.class, "Drone");
controller1 = new Controller(gamepad1);
controller2 = new Controller(gamepad2);
this.robot = new Robot(hardwareMap);
// robot.intake.setpos(Intake.Position.STACK1);
// while (robot.camera.getFrameCount() < 1) {
// telemetry.addLine("Initializing...");
// telemetry.update();
// }
telemetry.addLine("Initialized");
this.telemetry = FtcDashboard.getInstance().getTelemetry();
telemetry.update();
}
double getBatteryVoltage() {
double result = Double.POSITIVE_INFINITY;
for (VoltageSensor sensor : hardwareMap.voltageSensor) {
double voltage = sensor.getVoltage();
if (voltage > 0) {
result = Math.min(result, voltage);
}
}
return result;
}
@Override
public void loop() {
for (DcMotor motor : this.motors) {
this.telemetry.addData(this.hardwareMap.getNamesOf(motor).stream().findFirst().get(), motor.getPower());
}
for (Servo servo : this.servos) {
this.telemetry.addData(this.hardwareMap.getNamesOf(servo).stream().findFirst().get(), servo.getPosition());
}
this.telemetry.addData("battery", getBatteryVoltage());
this.telemetry.update();
// 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 if (controller1.getLeftTrigger().getValue() > 0.1) {
x *= slow_mode;
y *= slow_mode;
z *= slow_mode;
} else {
x *= normal;
y *= normal;
z *= normal;
}
robot.drive.setWeightedDrivePower(new Pose2d(x, y, z));
// Drone launcher
if (controller1.getA().isPressed() && !controller1.getStart().isPressed() && !controller2.getStart().isPressed()) {
this.robot.endGameMechs.launch();
} else {
this.robot.endGameMechs.reset();
}
// if (controller1.getRightBumper().isPressed()) {
// this.robot.endGameMechs.Finger_in();
// }else {
// this.robot.endGameMechs.Finger_out();
// }
//Hang Motor
// if (controller1.getB().isJustPressed() && !controller1.getStart().isPressed() && !controller2.getStart().isPressed() && !hang_counter){
// this.robot.endGameMechs.hang_init_pos();
// hang_counter = true;
// }
// else if (controller1.getB().isJustPressed() && !controller1.getStart().isPressed() && !controller2.getStart().isPressed() && hang_counter){
// this.robot.endGameMechs.hang_final_pos();
// hang_counter = false;
// }
if (controller1.getB().isPressed()) {
this.robot.endGameMechs.hang_final_pos();
} else {
this.robot.endGameMechs.hang_init_pos();
}
if (controller1.getX().isPressed()) {
this.robot.intake.wheel_spit();
} else if (controller1.getY().isPressed()) {
this.robot.intake.wheel_swallow();
} else {
this.robot.intake.wheel_stop();
}
// Driver 2
if (controller2.getRightTrigger().getValue()>=0.1){
robot.intake.setDcMotor(controller2.getRightTrigger().getValue()*intakeMax);
}
else if(controller2.getLeftTrigger().getValue()>=0.1){
robot.intake.setDcMotor(controller2.getLeftTrigger().getValue()*intakeMax2);
}
else {
robot.intake.setDcMotor(0);
}
if (controller2.getRightBumper().isJustPressed()) {
robot.intake.incrementPos();
}
if (controller2.getLeftBumper().isJustPressed()) {
robot.intake.decrementPos();
}
// macros
switch (robot.runningMacro) {
case (0): // manual mode
robot.slides.increaseTarget(controller2.getLeftStick().getY());
if (robot.intake.getPower() >= 0.01) {
robot.arm.setDoor(OPEN);
} else if (robot.intake.getPower() <= -0.01) {
robot.arm.setDoor(OPEN);
} else if (controller2.getLeftBumper().isPressed()) {
robot.arm.setDoor(Arm.DoorPosition.OPEN);
} else {
robot.arm.setDoor(CLOSE);
}
if (controller2.getX().isJustPressed()) {
robot.runningMacro = 1;
} else if (controller2.getY().isJustPressed()) {
robot.runningMacro = 2;
} else if (controller2.getB().isJustPressed() && !controller1.getStart().isPressed() && !controller2.getStart().isPressed()) {
robot.runningMacro = 3;
} else if (controller2.getA().isJustPressed() && !controller1.getStart().isPressed() && !controller2.getStart().isPressed()) {
robot.runningMacro = 4;
} else if (controller2.getDDown().isJustPressed() ) {
robot.runningMacro = 5;
}
break;
case (1):
robot.extendMacro(Slides.tier1, getRuntime());
break;
case (2):
robot.extendMacro(Slides.tier2, getRuntime());
break;
case (3):
robot.extendMacro(Slides.tier3, getRuntime());
break;
case (4):
robot.resetMacro(0, getRuntime());
break;
case(5):
robot.extendMacro(Slides.mini_tier1, getRuntime());
break;
}
// update and telemetry
robot.update(getRuntime());
controller1.update();
controller2.update();
telemetry.addLine(robot.getTelemetry());
telemetry.update();
}
}

250
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/opmode/teleop/RedTeleop.java
View File

@ -1,250 +0,0 @@
package org.firstinspires.ftc.teamcode.opmode.teleop;
import static org.firstinspires.ftc.teamcode.hardware.Arm.DoorPosition.CLOSE;
import static org.firstinspires.ftc.teamcode.hardware.Arm.DoorPosition.OPEN;
import com.acmerobotics.dashboard.FtcDashboard;
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.DcMotor;
import com.qualcomm.robotcore.hardware.Servo;
import com.qualcomm.robotcore.hardware.VoltageSensor;
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.Slides;
@Config
@TeleOp(name = "Red TeleOp", group = "OpModes")
public class RedTeleop extends OpMode {
public static double normal = 0.7;
public static double turbo = 1;
public static double slow_mode = 0.35;
public static double intakeMax = 0.36;
public static double intakeMax2 = -0.70;
private Robot robot;
private Controller controller1;
private Controller controller2;
public boolean hang_counter = false;
private DcMotor[] motors = new DcMotor[8];
private Servo[] servos = new Servo[7];
@Override
public void init() {
this.motors[0] = this.hardwareMap.get(DcMotor.class, "Rightslide");
this.motors[1] = this.hardwareMap.get(DcMotor.class, "Intakemotor");
this.motors[2] = this.hardwareMap.get(DcMotor.class, "FrontRight");
this.motors[3] = this.hardwareMap.get(DcMotor.class, "BackRight");
this.motors[4] = this.hardwareMap.get(DcMotor.class, "BackLeft");
this.motors[5] = this.hardwareMap.get(DcMotor.class, "FrontLeft");
this.motors[6] = this.hardwareMap.get(DcMotor.class, "Leftslide");
this.motors[7] = this.hardwareMap.get(DcMotor.class, "Hang");
this.servos[0] = this.hardwareMap.get(Servo.class, "LeftIntake");
this.servos[1] = this.hardwareMap.get(Servo.class, "Wrist");
this.servos[2] = this.hardwareMap.get(Servo.class, "Door");
this.servos[3] = this.hardwareMap.get(Servo.class, "RightArm");
this.servos[4] = this.hardwareMap.get(Servo.class, "LeftArm");
this.servos[5] = this.hardwareMap.get(Servo.class, "Right Intake Servo");
this.servos[6] = this.hardwareMap.get(Servo.class, "Drone");
controller1 = new Controller(gamepad1);
controller2 = new Controller(gamepad2);
this.robot = new Robot(hardwareMap);
// robot.intake.setpos(Intake.Position.STACK1);
// while (robot.camera.getFrameCount() < 1) {
// telemetry.addLine("Initializing...");
// telemetry.update();
// }
telemetry.addLine("Initialized");
this.telemetry = FtcDashboard.getInstance().getTelemetry();
telemetry.update();
this.robot.alliance = 2;
this.robot.leds.setPattern(79);
}
double getBatteryVoltage() {
double result = Double.POSITIVE_INFINITY;
for (VoltageSensor sensor : hardwareMap.voltageSensor) {
double voltage = sensor.getVoltage();
if (voltage > 0) {
result = Math.min(result, voltage);
}
}
return result;
}
@Override
public void loop() {
for (DcMotor motor : this.motors) {
this.telemetry.addData(this.hardwareMap.getNamesOf(motor).stream().findFirst().get(), motor.getPower());
}
for (Servo servo : this.servos) {
this.telemetry.addData(this.hardwareMap.getNamesOf(servo).stream().findFirst().get(), servo.getPosition());
}
this.telemetry.addData("battery", getBatteryVoltage());
this.telemetry.update();
this.robot.leds.setPattern(80);
// 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 if (controller1.getLeftTrigger().getValue() > 0.1) {
x *= slow_mode;
y *= slow_mode;
z *= slow_mode;
} else {
x *= normal;
y *= normal;
z *= normal;
}
robot.drive.setWeightedDrivePower(new Pose2d(x, y, z));
// Drone launcher
if (controller1.getA().isPressed() && !controller1.getStart().isPressed() && !controller2.getStart().isPressed()) {
this.robot.endGameMechs.launch();
} else {
this.robot.endGameMechs.reset();
}
// if (controller1.getRightBumper().isPressed()) {
// this.robot.endGameMechs.Finger_in();
// }else {
// this.robot.endGameMechs.Finger_out();
// }
//Hang Motor
// if (controller1.getB().isJustPressed() && !controller1.getStart().isPressed() && !controller2.getStart().isPressed() && !hang_counter){
// this.robot.endGameMechs.hang_init_pos();
// hang_counter = true;
// }
// else if (controller1.getB().isJustPressed() && !controller1.getStart().isPressed() && !controller2.getStart().isPressed() && hang_counter){
// this.robot.endGameMechs.hang_final_pos();
// hang_counter = false;
// }
if (controller1.getB().isPressed()) {
this.robot.endGameMechs.hang_final_pos();
} else {
this.robot.endGameMechs.hang_init_pos();
}
if (controller1.getX().isPressed()) {
this.robot.intake.wheel_spit();
} else if (controller1.getY().isPressed()) {
this.robot.intake.wheel_swallow();
} else {
this.robot.intake.wheel_stop();
}
// Driver 2
if (controller2.getRightTrigger().getValue()>=0.1){
robot.intake.setDcMotor(controller2.getRightTrigger().getValue()*intakeMax);
}
else if(controller2.getLeftTrigger().getValue()>=0.1){
robot.intake.setDcMotor(controller2.getLeftTrigger().getValue()*intakeMax2);
}
else {
robot.intake.setDcMotor(0);
}
if (controller2.getRightBumper().isJustPressed()) {
robot.intake.incrementPos();
}
if (controller2.getLeftBumper().isJustPressed()) {
robot.intake.decrementPos();
}
// macros
switch (robot.runningMacro) {
case (0): // manual mode
robot.slides.increaseTarget(controller2.getLeftStick().getY());
// if (robot.intake.getPower() >= 0.01) {
// robot.arm.setDoor(OPEN);
// } else if (robot.intake.getPower() <= -0.01) {
// robot.arm.setDoor(OPEN);
// } else if (controller2.getLeftBumper().isPressed()) {
// robot.arm.setDoor(Arm.DoorPosition.OPEN);
// } else {
// robot.arm.setDoor(CLOSE);
// }
//Elbowpos(
if (controller2.getDRight().isJustPressed()){
robot.arm.setElbowPos(4);
} else if (controller2.getDLeft().isJustPressed()) {
robot.arm.setElbowPos(3);
}else if(controller2.getDUp().isJustPressed()) {
robot.arm.setElbowPos(1);
}
if (controller2.getX().isJustPressed()) {
robot.runningMacro = 1;
} else if (controller2.getY().isJustPressed()) {
robot.runningMacro = 2;
} else if (controller2.getB().isJustPressed() && !controller1.getStart().isPressed() && !controller2.getStart().isPressed()) {
robot.runningMacro = 3;
} else if (controller2.getA().isJustPressed() && !controller1.getStart().isPressed() && !controller2.getStart().isPressed()) {
robot.runningMacro = 4;
} else if (controller2.getDDown().isJustPressed() ) {
robot.runningMacro = 5;
}
break;
case (1):
robot.extendMacro(Slides.tier1, getRuntime());
break;
case (2):
robot.extendMacro(Slides.tier2, getRuntime());
break;
case (3):
robot.extendMacro(Slides.tier3, getRuntime());
break;
case (4):
robot.resetMacro(0, getRuntime());
break;
case(5):
robot.extendMacro(Slides.mini_tier1, getRuntime());
break;
}
// update and telemetry
robot.update(getRuntime());
controller1.update();
controller2.update();
telemetry.addLine(robot.getTelemetry());
telemetry.update();
}
}

86
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/DriveConstants.java
View File

@ -1,86 +0,0 @@
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.
*/
public static double MAX_VEL = 50;
public static double MAX_ACCEL = 50;
public static double MAX_ANG_VEL = 15;
public static double MAX_ANG_ACCEL = 8;
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
View File

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

329
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/drive/SampleMecanumDrive.java
View File

@ -1,329 +0,0 @@
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.AxisDirection;
import org.firstinspires.ftc.teamcode.roadrunner.util.BNO055IMUUtil;
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(7, 0, 1);
public static PIDCoefficients HEADING_PID = new PIDCoefficients(6.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(1)), 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.POS_X);
rightRear = hardwareMap.get(DcMotorEx.class, "BackRight");
rightFront = hardwareMap.get(DcMotorEx.class, "FrontRight");
leftFront = hardwareMap.get(DcMotorEx.class, "FrontLeft");
leftRear = hardwareMap.get(DcMotorEx.class, "BackLeft");
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() {
this.update(null);
}
public void update(Pose2d guess) {
if (guess == null) {
updatePoseEstimate();
} else {
setPoseEstimate(guess);
}
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
View File

@ -1,82 +0,0 @@
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
View File

@ -1,108 +0,0 @@
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, "FrontLeft"));
// 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
View File

@ -1,203 +0,0 @@
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
View File

@ -1,51 +0,0 @@
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
View File

@ -1,171 +0,0 @@
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
View File

@ -1,56 +0,0 @@
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
@Autonomous(group = "drive")
public class FollowerPIDTuner extends LinearOpMode {
public static double DISTANCE = 36; // 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
View File

@ -1,45 +0,0 @@
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
View File

@ -1,143 +0,0 @@
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
View File

@ -1,71 +0,0 @@
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
View File

@ -1,83 +0,0 @@
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
View File

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

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

@ -1,46 +0,0 @@
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 Spinning extends LinearOpMode {
public static double ANGLE = 50;
@Override
public void runOpMode() throws InterruptedException {
SampleMecanumDrive drive = new SampleMecanumDrive(hardwareMap);
waitForStart();
while (opModeIsActive() && !isStopRequested()) {
drive.turn(Math.toRadians(90));
sleep(1000);
drive.turn(Math.toRadians(-90));
sleep(1000);
}
}
}

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

@ -1,40 +0,0 @@
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
View File

@ -1,46 +0,0 @@
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
View File

@ -1,44 +0,0 @@
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
View File

@ -1,88 +0,0 @@
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
View File

@ -1,104 +0,0 @@
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
View File

@ -1,131 +0,0 @@
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
View File

@ -1,28 +0,0 @@
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
View File

@ -1,4 +0,0 @@
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
View File

@ -1,44 +0,0 @@
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
View File

@ -1,711 +0,0 @@
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
View File

@ -1,271 +0,0 @@
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
View File

@ -1,40 +0,0 @@
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
View File

@ -1,20 +0,0 @@
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
View File

@ -1,36 +0,0 @@
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
View File

@ -1,12 +0,0 @@
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
View File

@ -1,70 +0,0 @@
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
View File

@ -1,45 +0,0 @@
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
View File

@ -1,8 +0,0 @@
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
View File

@ -1,128 +0,0 @@
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
View File

@ -1,54 +0,0 @@
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
View File

@ -1,98 +0,0 @@
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
View File

@ -1,60 +0,0 @@
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
View File

@ -1,124 +0,0 @@
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
View File

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

6
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/CenterStageCommon.java
View File

@ -1,6 +0,0 @@
package org.firstinspires.ftc.teamcode.util;
public class CenterStageCommon {
public enum Alliance { Blue, Red }
public enum PropLocation { Unknown, Left, Center, Right }
}

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

@ -1,30 +0,0 @@
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;
}
}

23
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Colors.java
View File

@ -1,23 +0,0 @@
package org.firstinspires.ftc.teamcode.util;
import android.graphics.Color;
import org.opencv.core.Scalar;
public class Colors {
// CV Color Threshold Constants
public static Scalar FTC_RED_LOWER = new Scalar(165, 80, 80);
public static Scalar FTC_RED_UPPER = new Scalar(15, 255, 255);
public static ScalarRange FTC_RED_RANGE_1 = new ScalarRange(new Scalar(180, FTC_RED_UPPER.val[1], FTC_RED_UPPER.val[2]), FTC_RED_LOWER);
public static ScalarRange FTC_RED_RANGE_2 = new ScalarRange(FTC_RED_UPPER, new Scalar(0, FTC_RED_LOWER.val[1], FTC_RED_LOWER.val[2]));
public static Scalar FTC_BLUE_LOWER = new Scalar(75, 40, 80);
public static Scalar FTC_BLUE_UPPER = new Scalar(120, 255, 255);
public static ScalarRange FTC_BLUE_RANGE = new ScalarRange(FTC_BLUE_UPPER, FTC_BLUE_LOWER);
public static Scalar FTC_WHITE_LOWER = new Scalar(0, 0, 40);
public static Scalar FTC_WHITE_UPPER = new Scalar(180, 30, 255);
public static OpenCVUtil.LinePaint RED = new OpenCVUtil.LinePaint(Color.RED);
public static OpenCVUtil.LinePaint BLUE = new OpenCVUtil.LinePaint(Color.BLUE);
public static OpenCVUtil.LinePaint BLACK = new OpenCVUtil.LinePaint(Color.BLACK);
public static OpenCVUtil.LinePaint WHITE = new OpenCVUtil.LinePaint(Color.WHITE);
}

89
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Configurables.java
View File

@ -1,89 +0,0 @@
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;
public static double CAMERA_OFFSET_X = 0f;
public static double DETECTION_AREA_MIN = 0.02f;
public static double DETECTION_AREA_MAX = 0.3f;
public static double DETECTION_LEFT_X = 0;
public static double DETECTION_CENTER_X = .5;
public static double DETECTION_RIGHT_X = 1;
public static double SCORING_DISTANCE_FROM_APRIL_TAG = 6f;
}

60
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/Constants.java
View File

@ -1,60 +0,0 @@
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 WEBCAM_NAME = "FrontWebcam";
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";
}

102
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/OpenCVUtil.java
View File

@ -1,102 +0,0 @@
package org.firstinspires.ftc.teamcode.util;
import android.graphics.Paint;
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;
// 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 class LinePaint extends Paint
{
public LinePaint(int color)
{
setColor(color);
setAntiAlias(true);
setStrokeCap(Paint.Cap.ROUND);
}
}
}

21
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/util/ScalarRange.java
View File

@ -1,21 +0,0 @@
package org.firstinspires.ftc.teamcode.util;
import org.opencv.core.Scalar;
public class ScalarRange {
private Scalar upper;
private Scalar lower;
public ScalarRange(Scalar upper, Scalar lower) {
this.upper = upper;
this.lower = lower;
}
public Scalar getUpper() {
return this.upper;
}
public Scalar getLower() {
return this.lower;
}
}

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

@ -1,194 +0,0 @@
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;
}
}
}

131
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/Detection.java
View File

@ -1,131 +0,0 @@
package org.firstinspires.ftc.teamcode.vision;
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;
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;
// 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 maxAreaFactor) {
this.maxSizePx = frameSize;
this.minAreaPx = frameSize.area() * minAreaFactor;
this.maxAreaPx = frameSize.area() * maxAreaFactor;
}
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);
}
}
// 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);
}
}
// Check if the current Detection is valid
public boolean isValid() {
return (this.contour != null) && (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
View File

@ -1,109 +0,0 @@
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);
}
}

140
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/vision/PropDetectionPipeline.java
View File

@ -1,140 +0,0 @@
package org.firstinspires.ftc.teamcode.vision;
import static org.firstinspires.ftc.teamcode.hardware.Camera.PROP_REJECTION_VERTICAL_LOWER;
import static org.firstinspires.ftc.teamcode.hardware.Camera.PROP_REJECTION_VERTICAL_UPPER;
import static org.firstinspires.ftc.teamcode.util.Colors.FTC_BLUE_RANGE;
import static org.firstinspires.ftc.teamcode.util.Colors.FTC_RED_RANGE_1;
import static org.firstinspires.ftc.teamcode.util.Colors.FTC_RED_RANGE_2;
import static org.firstinspires.ftc.teamcode.util.Colors.RED;
import static org.firstinspires.ftc.teamcode.util.Colors.WHITE;
import static org.firstinspires.ftc.teamcode.util.Configurables.DETECTION_AREA_MAX;
import static org.firstinspires.ftc.teamcode.util.Configurables.DETECTION_AREA_MIN;
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.BLUR_SIZE;
import static org.firstinspires.ftc.teamcode.util.Constants.ERODE_DILATE_ITERATIONS;
import static org.firstinspires.ftc.teamcode.util.Constants.STRUCTURING_ELEMENT;
import static org.firstinspires.ftc.teamcode.util.OpenCVUtil.getLargestContour;
import android.graphics.Canvas;
import org.firstinspires.ftc.robotcore.internal.camera.calibration.CameraCalibration;
import org.firstinspires.ftc.teamcode.util.CenterStageCommon;
import org.firstinspires.ftc.teamcode.util.ScalarRange;
import org.firstinspires.ftc.vision.VisionProcessor;
import org.opencv.core.Core;
import org.opencv.core.Mat;
import org.opencv.core.MatOfPoint;
import org.opencv.core.Point;
import org.opencv.core.Size;
import org.opencv.imgproc.Imgproc;
import java.util.ArrayList;
public class PropDetectionPipeline implements VisionProcessor {
CenterStageCommon.Alliance alliance;
private Mat blurred = new Mat();
private Mat hsv = new Mat();
private Mat mask = new Mat();
private Mat tmpMask = new Mat();
private Detection red;
private Detection blue;
// Init
@Override
public void init(int width, int height, CameraCalibration calibration) {
this.red = new Detection(new Size(width, height), DETECTION_AREA_MIN, DETECTION_AREA_MAX);
this.blue = new Detection(new Size(width, height), DETECTION_AREA_MIN, DETECTION_AREA_MAX);
}
// Process each frame that is received from the webcam
@Override
public Object processFrame(Mat input, long captureTimeNanos) {
Imgproc.GaussianBlur(input, blurred, BLUR_SIZE, 0);
Imgproc.cvtColor(blurred, hsv, Imgproc.COLOR_RGB2HSV);
if (alliance == CenterStageCommon.Alliance.Red) {
red.setContour(detect(FTC_RED_RANGE_1, FTC_RED_RANGE_2));
}
if (alliance == CenterStageCommon.Alliance.Blue) {
blue.setContour(detect(FTC_BLUE_RANGE));
}
return input;
}
private Mat zeros;
private Mat zeros(Size size, int type) {
if (this.zeros == null) {
this.zeros = Mat.zeros(size, type);
}
return this.zeros;
}
private MatOfPoint detect(ScalarRange... colorRanges) {
mask.release();
for (ScalarRange colorRange : colorRanges) {
Core.inRange(hsv, colorRange.getLower(), colorRange.getUpper(), tmpMask);
if (mask.empty() || mask.rows() <= 0) {
Core.inRange(hsv, colorRange.getLower(), colorRange.getUpper(), mask);
}
Core.add(mask, tmpMask, mask);
}
Imgproc.rectangle(mask, new Point(0,0), new Point(mask.cols() - 1, (int)PROP_REJECTION_VERTICAL_UPPER), BLACK, -1);
Imgproc.rectangle(mask, new Point(0,(int)PROP_REJECTION_VERTICAL_LOWER), new Point(mask.cols() - 1, mask.rows() -1), BLACK, -1);
Imgproc.erode(mask, mask, STRUCTURING_ELEMENT, ANCHOR, ERODE_DILATE_ITERATIONS);
Imgproc.dilate(mask, mask, STRUCTURING_ELEMENT, ANCHOR, ERODE_DILATE_ITERATIONS);
ArrayList<MatOfPoint> contours = new ArrayList<>();
Imgproc.findContours(mask, contours, new Mat(), Imgproc.RETR_LIST, Imgproc.CHAIN_APPROX_SIMPLE);
return getLargestContour(contours);
}
@Override
public void onDrawFrame(Canvas canvas, int onscreenWidth, int onscreenHeight, float scaleBmpPxToCanvasPx, float scaleCanvasDensity, Object userContext) {
canvas.drawLine(0, PROP_REJECTION_VERTICAL_LOWER, canvas.getWidth(), PROP_REJECTION_VERTICAL_LOWER, WHITE);
canvas.drawLine(0, PROP_REJECTION_VERTICAL_UPPER, canvas.getWidth(), PROP_REJECTION_VERTICAL_UPPER, WHITE);
if (red != null && red.isValid()) {
Point center = red.getCenterPx();
if (center.y < PROP_REJECTION_VERTICAL_LOWER
&& center.y > PROP_REJECTION_VERTICAL_UPPER) {
canvas.drawCircle((float)red.getCenterPx().x, (float)red.getCenterPx().y, 10, WHITE);
} else {
canvas.drawCircle((float)red.getCenterPx().x, (float)red.getCenterPx().y, 10, RED);
}
}
if (blue != null && blue.isValid()) {
Point center = blue.getCenterPx();
if (center.y < PROP_REJECTION_VERTICAL_LOWER
&& center.y > PROP_REJECTION_VERTICAL_UPPER) {
canvas.drawCircle((float)blue.getCenterPx().x, (float)blue.getCenterPx().y, 10, WHITE);
} else {
canvas.drawCircle((float)blue.getCenterPx().x, (float)blue.getCenterPx().y, 10, RED);
}
}
}
public Detection getRed() {
return this.red;
}
public Detection getBlue() {
return this.blue;
}
public void setAlliance(CenterStageCommon.Alliance alliance) {
this.alliance = alliance;
}
public CenterStageCommon.Alliance getAlliance() {
return this.alliance;
}
}

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

@ -1,151 +0,0 @@
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"
abiFilters "armeabi-v7a", "arm64-v8a"
}
}
debug {
debuggable true
jniDebuggable true
ndk {
abiFilters "armeabi-v7a"
abiFilters "armeabi-v7a", "arm64-v8a"
}
}
}
@ -121,6 +121,5 @@ android {
}
repositories {
mavenCentral()
}

4
build.dependencies.gradle
View File

@ -1,7 +1,6 @@
repositories {
mavenCentral()
google() // Needed for androidx
maven { url = 'https://maven.brott.dev/' }
}
dependencies {
@ -18,8 +17,5 @@ dependencies {
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.15'
implementation project(':ielib-core')
implementation project(':ielib')
}

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.2'
classpath 'com.android.tools.build:gradle:7.2.0'
}
}

6
settings.gradle
View File

@ -1,8 +1,2 @@
include ':FtcRobotController'
include ':TeamCode'
include ':MeepMeepTesting'
include ':MeepMeepTesting'
include ':ielib-core'
project(':ielib-core').projectDir = new File('../ielib-core')
include ':ielib'
project(':ielib').projectDir = new File('../ielib')