SCDS/FtcTemplate
7
0
Fork 0
mirror of https://github.com/trc492/FtcTemplate.git synced 2025-07-01 21:11:23 -07:00
Code Issues Packages Projects Releases Wiki Activity

Updated to FTC SDK 9.2.0.

Browse Source 
Reorganized libraries.
This commit is contained in:
Titan Robotics Club
2024-07-12 06:13:04 -07:00
parent 6634d26140
commit c44d44b230
27 changed files with 1558 additions and 2082 deletions
Download Patch File Download Diff File 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"?> <?xml version="1.0" encoding="utf-8"?>
<manifest xmlns:android="http://schemas.android.com/apk/res/android" <manifest xmlns:android="http://schemas.android.com/apk/res/android"
xmlns:tools="http://schemas.android.com/tools" xmlns:tools="http://schemas.android.com/tools"
android:versionCode="53" android:versionCode="54"
android:versionName="9.1"> android:versionName="9.2">
<uses-permission android:name="android.permission.RECEIVE_BOOT_COMPLETED" /> <uses-permission android:name="android.permission.RECEIVE_BOOT_COMPLETED" />

104
FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptAprilTagMultiPortal.java vendored Normal file
View File

@ -0,0 +1,104 @@
/* Copyright (c) 2024 FIRST. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted (subject to the limitations in the disclaimer below) provided that
* the following conditions are met:
*
* Redistributions of source code must retain the above copyright notice, this list
* of conditions and the following disclaimer.
*
* Redistributions in binary form must reproduce the above copyright notice, this
* list of conditions and the following disclaimer in the documentation and/or
* other materials provided with the distribution.
*
* Neither the name of FIRST nor the names of its contributors may be used to endorse or
* promote products derived from this software without specific prior written permission.
*
* NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
* LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
* THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
package org.firstinspires.ftc.robotcontroller.external.samples;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import org.firstinspires.ftc.robotcore.external.hardware.camera.BuiltinCameraDirection;
import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
import org.firstinspires.ftc.vision.VisionPortal;
import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor;
/**
* This OpMode demonstrates the basics of using multiple vision portals simultaneously
*/
@TeleOp(name = "Concept: AprilTagMultiPortal", group = "Concept")
@Disabled
public class ConceptAprilTagMultiPortal extends LinearOpMode
{
VisionPortal portal1;
VisionPortal portal2;
AprilTagProcessor aprilTagProcessor1;
AprilTagProcessor aprilTagProcessor2;
@Override
public void runOpMode() throws InterruptedException
{
// Because we want to show two camera feeds simultaneously, we need to inform
// the SDK that we want it to split the camera monitor area into two smaller
// areas for us. It will then give us View IDs which we can pass to the individual
// vision portals to allow them to properly hook into the UI in tandem.
int[] viewIds = VisionPortal.makeMultiPortalView(2, VisionPortal.MultiPortalLayout.VERTICAL);
// We extract the two view IDs from the array to make our lives a little easier later.
// NB: the array is 2 long because we asked for 2 portals up above.
int portal1ViewId = viewIds[0];
int portal2ViewId = viewIds[1];
// If we want to run AprilTag detection on two portals simultaneously,
// we need to create two distinct instances of the AprilTag processor,
// one for each portal. If you want to see more detail about different
// options that you have when creating these processors, go check out
// the ConceptAprilTag OpMode.
aprilTagProcessor1 = AprilTagProcessor.easyCreateWithDefaults();
aprilTagProcessor2 = AprilTagProcessor.easyCreateWithDefaults();
// Now we build both portals. The CRITICAL thing to notice here is the call to
// setLiveViewContainerId(), where we pass in the IDs we received earlier from
// makeMultiPortalView().
portal1 = new VisionPortal.Builder()
.setCamera(hardwareMap.get(WebcamName.class, "Webcam 1"))
.setLiveViewContainerId(portal1ViewId)
.addProcessor(aprilTagProcessor1)
.build();
portal2 = new VisionPortal.Builder()
.setCamera(hardwareMap.get(WebcamName.class, "Webcam 2"))
.setLiveViewContainerId(portal2ViewId)
.addProcessor(aprilTagProcessor2)
.build();
waitForStart();
// Main Loop
while (opModeIsActive())
{
// Just show some basic telemetry to demonstrate both processors are working in parallel
// on their respective cameras. If you want to see more detail about the information you
// can get back from the processor, you should look at ConceptAprilTag.
telemetry.addData("Number of tags in Camera 1", aprilTagProcessor1.getDetections().size());
telemetry.addData("Number of tags in Camera 2", aprilTagProcessor2.getDetections().size());
telemetry.update();
sleep(20);
}
}
}

202
FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptDoubleVision.java vendored
View File

@ -1,202 +0,0 @@
/* 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.robotcore.external.tfod.Recognition;
import org.firstinspires.ftc.vision.VisionPortal;
import org.firstinspires.ftc.vision.apriltag.AprilTagDetection;
import org.firstinspires.ftc.vision.apriltag.AprilTagProcessor;
import org.firstinspires.ftc.vision.tfod.TfodProcessor;
import java.util.List;
/*
* This OpMode illustrates the basics of using both AprilTag recognition and TensorFlow
* Object Detection.
*
* 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: Double Vision", group = "Concept")
@Disabled
public class ConceptDoubleVision 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 TensorFlow Object Detection processor.
*/
private TfodProcessor tfod;
/**
* The variable to store our instance of the vision portal.
*/
private VisionPortal myVisionPortal;
@Override
public void runOpMode() {
initDoubleVision();
// This OpMode loops continuously, allowing the user to switch between
// AprilTag and TensorFlow Object Detection (TFOD) image processors.
while (!isStopRequested()) {
if (opModeInInit()) {
telemetry.addData("DS preview on/off","3 dots, Camera Stream");
telemetry.addLine();
telemetry.addLine("----------------------------------------");
}
if (myVisionPortal.getProcessorEnabled(aprilTag)) {
// User instructions: Dpad left or Dpad right.
telemetry.addLine("Dpad Left to disable AprilTag");
telemetry.addLine();
telemetryAprilTag();
} else {
telemetry.addLine("Dpad Right to enable AprilTag");
}
telemetry.addLine();
telemetry.addLine("----------------------------------------");
if (myVisionPortal.getProcessorEnabled(tfod)) {
telemetry.addLine("Dpad Down to disable TFOD");
telemetry.addLine();
telemetryTfod();
} else {
telemetry.addLine("Dpad Up to enable TFOD");
}
// Push telemetry to the Driver Station.
telemetry.update();
if (gamepad1.dpad_left) {
myVisionPortal.setProcessorEnabled(aprilTag, false);
} else if (gamepad1.dpad_right) {
myVisionPortal.setProcessorEnabled(aprilTag, true);
}
if (gamepad1.dpad_down) {
myVisionPortal.setProcessorEnabled(tfod, false);
} else if (gamepad1.dpad_up) {
myVisionPortal.setProcessorEnabled(tfod, true);
}
sleep(20);
} // end while loop
} // end method runOpMode()
/**
* Initialize AprilTag and TFOD.
*/
private void initDoubleVision() {
// -----------------------------------------------------------------------------------------
// AprilTag Configuration
// -----------------------------------------------------------------------------------------
aprilTag = new AprilTagProcessor.Builder()
.build();
// -----------------------------------------------------------------------------------------
// TFOD Configuration
// -----------------------------------------------------------------------------------------
tfod = new TfodProcessor.Builder()
.build();
// -----------------------------------------------------------------------------------------
// Camera Configuration
// -----------------------------------------------------------------------------------------
if (USE_WEBCAM) {
myVisionPortal = new VisionPortal.Builder()
.setCamera(hardwareMap.get(WebcamName.class, "Webcam 1"))
.addProcessors(tfod, aprilTag)
.build();
} else {
myVisionPortal = new VisionPortal.Builder()
.setCamera(BuiltinCameraDirection.BACK)
.addProcessors(tfod, aprilTag)
.build();
}
} // end initDoubleVision()
/**
* 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
} // end method telemetryAprilTag()
/**
* Add telemetry about TensorFlow Object Detection (TFOD) recognitions.
*/
private void telemetryTfod() {
List<Recognition> currentRecognitions = tfod.getRecognitions();
telemetry.addData("# Objects Detected", currentRecognitions.size());
// Step through the list of recognitions and display info for each one.
for (Recognition recognition : currentRecognitions) {
double x = (recognition.getLeft() + recognition.getRight()) / 2 ;
double y = (recognition.getTop() + recognition.getBottom()) / 2 ;
telemetry.addData(""," ");
telemetry.addData("Image", "%s (%.0f %% Conf.)", recognition.getLabel(), recognition.getConfidence() * 100);
telemetry.addData("- Position", "%.0f / %.0f", x, y);
telemetry.addData("- Size", "%.0f x %.0f", recognition.getWidth(), recognition.getHeight());
} // end for() loop
} // end method telemetryTfod()
} // end class

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

@ -1,199 +0,0 @@
/* Copyright (c) 2019 FIRST. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted (subject to the limitations in the disclaimer below) provided that
* the following conditions are met:
*
* Redistributions of source code must retain the above copyright notice, this list
* of conditions and the following disclaimer.
*
* Redistributions in binary form must reproduce the above copyright notice, this
* list of conditions and the following disclaimer in the documentation and/or
* other materials provided with the distribution.
*
* Neither the name of FIRST nor the names of its contributors may be used to endorse or
* promote products derived from this software without specific prior written permission.
*
* NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
* LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
* THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
package org.firstinspires.ftc.robotcontroller.external.samples;
import 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.robotcore.external.tfod.Recognition;
import org.firstinspires.ftc.vision.VisionPortal;
import org.firstinspires.ftc.vision.tfod.TfodProcessor;
import java.util.List;
/*
* This OpMode illustrates the basics of TensorFlow Object Detection,
* 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: TensorFlow Object Detection", group = "Concept")
@Disabled
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.
*/
private TfodProcessor tfod;
/**
* The variable to store our instance of the vision portal.
*/
private VisionPortal visionPortal;
@Override
public void runOpMode() {
initTfod();
// 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()) {
telemetryTfod();
// 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 runOpMode()
/**
* Initialize the TensorFlow Object Detection processor.
*/
private void initTfod() {
// Create the TensorFlow processor by using a builder.
tfod = new TfodProcessor.Builder()
// With the following lines commented out, the default TfodProcessor Builder
// will load the default model for the season. To define a custom model to load,
// choose one of the following:
// Use setModelAssetName() if the custom TF Model is built in as an asset (AS only).
// Use setModelFileName() if you have downloaded a custom team model to the Robot Controller.
//.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)
//.setModelInputSize(300)
//.setModelAspectRatio(16.0 / 9.0)
.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.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(tfod);
// Build the Vision Portal, using the above settings.
visionPortal = builder.build();
// Set confidence threshold for TFOD recognitions, at any time.
//tfod.setMinResultConfidence(0.75f);
// Disable or re-enable the TFOD processor at any time.
//visionPortal.setProcessorEnabled(tfod, true);
} // end method initTfod()
/**
* Add telemetry about TensorFlow Object Detection (TFOD) recognitions.
*/
private void telemetryTfod() {
List<Recognition> currentRecognitions = tfod.getRecognitions();
telemetry.addData("# Objects Detected", currentRecognitions.size());
// Step through the list of recognitions and display info for each one.
for (Recognition recognition : currentRecognitions) {
double x = (recognition.getLeft() + recognition.getRight()) / 2 ;
double y = (recognition.getTop() + recognition.getBottom()) / 2 ;
telemetry.addData(""," ");
telemetry.addData("Image", "%s (%.0f %% Conf.)", recognition.getLabel(), recognition.getConfidence() * 100);
telemetry.addData("- Position", "%.0f / %.0f", x, y);
telemetry.addData("- Size", "%.0f x %.0f", recognition.getWidth(), recognition.getHeight());
} // end for() loop
} // end method telemetryTfod()
} // end class

142
FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptTensorFlowObjectDetectionEasy.java vendored
View File

@ -1,142 +0,0 @@
/* Copyright (c) 2019 FIRST. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted (subject to the limitations in the disclaimer below) provided that
* the following conditions are met:
*
* Redistributions of source code must retain the above copyright notice, this list
* of conditions and the following disclaimer.
*
* Redistributions in binary form must reproduce the above copyright notice, this
* list of conditions and the following disclaimer in the documentation and/or
* other materials provided with the distribution.
*
* Neither the name of FIRST nor the names of its contributors may be used to endorse or
* promote products derived from this software without specific prior written permission.
*
* NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
* LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
* THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
package org.firstinspires.ftc.robotcontroller.external.samples;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import org.firstinspires.ftc.robotcore.external.hardware.camera.BuiltinCameraDirection;
import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
import org.firstinspires.ftc.robotcore.external.tfod.Recognition;
import org.firstinspires.ftc.vision.VisionPortal;
import org.firstinspires.ftc.vision.tfod.TfodProcessor;
import java.util.List;
/*
* This OpMode illustrates the basics of TensorFlow Object Detection, using
* the easiest 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: TensorFlow Object Detection Easy", group = "Concept")
@Disabled
public class ConceptTensorFlowObjectDetectionEasy extends LinearOpMode {
private static final boolean USE_WEBCAM = true; // true for webcam, false for phone camera
/**
* The variable to store our instance of the TensorFlow Object Detection processor.
*/
private TfodProcessor tfod;
/**
* The variable to store our instance of the vision portal.
*/
private VisionPortal visionPortal;
@Override
public void runOpMode() {
initTfod();
// 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()) {
telemetryTfod();
// 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 runOpMode()
/**
* Initialize the TensorFlow Object Detection processor.
*/
private void initTfod() {
// Create the TensorFlow processor the easy way.
tfod = TfodProcessor.easyCreateWithDefaults();
// Create the vision portal the easy way.
if (USE_WEBCAM) {
visionPortal = VisionPortal.easyCreateWithDefaults(
hardwareMap.get(WebcamName.class, "Webcam 1"), tfod);
} else {
visionPortal = VisionPortal.easyCreateWithDefaults(
BuiltinCameraDirection.BACK, tfod);
}
} // end method initTfod()
/**
* Add telemetry about TensorFlow Object Detection (TFOD) recognitions.
*/
private void telemetryTfod() {
List<Recognition> currentRecognitions = tfod.getRecognitions();
telemetry.addData("# Objects Detected", currentRecognitions.size());
// Step through the list of recognitions and display info for each one.
for (Recognition recognition : currentRecognitions) {
double x = (recognition.getLeft() + recognition.getRight()) / 2 ;
double y = (recognition.getTop() + recognition.getBottom()) / 2 ;
telemetry.addData(""," ");
telemetry.addData("Image", "%s (%.0f %% Conf.)", recognition.getLabel(), recognition.getConfidence() * 100);
telemetry.addData("- Position", "%.0f / %.0f", x, y);
telemetry.addData("- Size", "%.0f x %.0f", recognition.getWidth(), recognition.getHeight());
} // end for() loop
} // end method telemetryTfod()
} // end class

186
FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/ConceptTensorFlowObjectDetectionSwitchableCameras.java vendored
View File

@ -1,186 +0,0 @@
/* Copyright (c) 2020 FIRST. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted (subject to the limitations in the disclaimer below) provided that
* the following conditions are met:
*
* Redistributions of source code must retain the above copyright notice, this list
* of conditions and the following disclaimer.
*
* Redistributions in binary form must reproduce the above copyright notice, this
* list of conditions and the following disclaimer in the documentation and/or
* other materials provided with the distribution.
*
* Neither the name of FIRST nor the names of its contributors may be used to endorse or
* promote products derived from this software without specific prior written permission.
*
* NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
* LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
* THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
package org.firstinspires.ftc.robotcontroller.external.samples;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import org.firstinspires.ftc.robotcore.external.ClassFactory;
import org.firstinspires.ftc.robotcore.external.hardware.camera.CameraName;
import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
import org.firstinspires.ftc.robotcore.external.tfod.Recognition;
import org.firstinspires.ftc.vision.VisionPortal;
import org.firstinspires.ftc.vision.VisionPortal.CameraState;
import org.firstinspires.ftc.vision.tfod.TfodProcessor;
import java.util.List;
/*
* This OpMode illustrates the basics of TensorFlow Object Detection, using
* two webcams.
*
* Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name.
* Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list.
*/
@TeleOp(name = "Concept: TensorFlow Object Detection Switchable Cameras", group = "Concept")
@Disabled
public class ConceptTensorFlowObjectDetectionSwitchableCameras extends LinearOpMode {
/**
* Variables used for switching cameras.
*/
private WebcamName webcam1, webcam2;
private boolean oldLeftBumper;
private boolean oldRightBumper;
/**
* The variable to store our instance of the TensorFlow Object Detection processor.
*/
private TfodProcessor tfod;
/**
* The variable to store our instance of the vision portal.
*/
private VisionPortal visionPortal;
@Override
public void runOpMode() {
initTfod();
// 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()) {
telemetryCameraSwitching();
telemetryTfod();
// Push telemetry to the Driver Station.
telemetry.update();
// Save CPU resources; can resume streaming when needed.
if (gamepad1.dpad_down) {
visionPortal.stopStreaming();
} else if (gamepad1.dpad_up) {
visionPortal.resumeStreaming();
}
doCameraSwitching();
// Share the CPU.
sleep(20);
}
}
// Save more CPU resources when camera is no longer needed.
visionPortal.close();
} // end runOpMode()
/**
* Initialize the TensorFlow Object Detection processor.
*/
private void initTfod() {
// Create the TensorFlow processor by using a builder.
tfod = new TfodProcessor.Builder().build();
webcam1 = hardwareMap.get(WebcamName.class, "Webcam 1");
webcam2 = hardwareMap.get(WebcamName.class, "Webcam 2");
CameraName switchableCamera = ClassFactory.getInstance()
.getCameraManager().nameForSwitchableCamera(webcam1, webcam2);
// Create the vision portal by using a builder.
visionPortal = new VisionPortal.Builder()
.setCamera(switchableCamera)
.addProcessor(tfod)
.build();
} // end method initTfod()
/**
* Add telemetry about camera switching.
*/
private void telemetryCameraSwitching() {
if (visionPortal.getActiveCamera().equals(webcam1)) {
telemetry.addData("activeCamera", "Webcam 1");
telemetry.addData("Press RightBumper", "to switch to Webcam 2");
} else {
telemetry.addData("activeCamera", "Webcam 2");
telemetry.addData("Press LeftBumper", "to switch to Webcam 1");
}
} // end method telemetryCameraSwitching()
/**
* Add telemetry about TensorFlow Object Detection (TFOD) recognitions.
*/
private void telemetryTfod() {
List<Recognition> currentRecognitions = tfod.getRecognitions();
telemetry.addData("# Objects Detected", currentRecognitions.size());
// Step through the list of recognitions and display info for each one.
for (Recognition recognition : currentRecognitions) {
double x = (recognition.getLeft() + recognition.getRight()) / 2 ;
double y = (recognition.getTop() + recognition.getBottom()) / 2 ;
telemetry.addData(""," ");
telemetry.addData("Image", "%s (%.0f %% Conf.)", recognition.getLabel(), recognition.getConfidence() * 100);
telemetry.addData("- Position", "%.0f / %.0f", x, y);
telemetry.addData("- Size", "%.0f x %.0f", recognition.getWidth(), recognition.getHeight());
} // end for() loop
} // end method telemetryTfod()
/**
* Set the active camera according to input from the gamepad.
*/
private void doCameraSwitching() {
if (visionPortal.getCameraState() == CameraState.STREAMING) {
// If the left bumper is pressed, use Webcam 1.
// If the right bumper is pressed, use Webcam 2.
boolean newLeftBumper = gamepad1.left_bumper;
boolean newRightBumper = gamepad1.right_bumper;
if (newLeftBumper && !oldLeftBumper) {
visionPortal.setActiveCamera(webcam1);
} else if (newRightBumper && !oldRightBumper) {
visionPortal.setActiveCamera(webcam2);
}
oldLeftBumper = newLeftBumper;
oldRightBumper = newRightBumper;
}
} // end method doCameraSwitching()
} // end class

141
FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorOctoQuad.java vendored Normal file
View File

@ -0,0 +1,141 @@
/*
* Copyright (c) 2024 DigitalChickenLabs
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
package org.firstinspires.ftc.robotcontroller.external.samples;
import com.qualcomm.hardware.digitalchickenlabs.OctoQuad;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import org.firstinspires.ftc.robotcore.external.Telemetry;
/*
* This OpMode illustrates how to use the DigitalChickenLabs OctoQuad Quadrature Encoder & Pulse Width Interface Module
*
* The OctoQuad has 8 input channels that can used to read either Relative Quadrature Encoders or Pulse-Width Absolute Encoder inputs.
* Relative Quadrature encoders are found on most FTC motors, and some stand-alone position sensors like the REV Thru-Bore encoder.
* Pulse-Width encoders are less common. The REV Thru-Bore encoder can provide its absolute position via a variable pulse width,
* as can several sonar rangefinders such as the MaxBotix MB1000 series.
*
* This basic sample shows how an OctoQuad can be used to read the position three Odometry pods fitted
* with REV Thru-Bore encoders. For a more advanced example showing additional OctoQuad capabilities, see the SensorOctoQuadAdv sample.
*
* This OpMode assumes that the OctoQuad is attached to an I2C interface named "octoquad" in the robot configuration.
*
* The code assumes the first three OctoQuad inputs are connected as follows
* - Chan 0: for measuring forward motion on the left side of the robot.
* - Chan 1: for measuring forward motion on the right side of the robot.
* - Chan 2: for measuring Lateral (strafing) motion.
*
* The encoder values may be reset to zero by pressing the X (left most) button on Gamepad 1.
*
* This sample does not show how to interpret these readings, just how to obtain and display them.
*
* Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name.
* Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list
*
* See the sensor's product page: https://www.tindie.com/products/digitalchickenlabs/octoquad-8ch-quadrature-pulse-width-decoder/
*/
@TeleOp(name = "OctoQuad Basic", group="OctoQuad")
@Disabled
public class SensorOctoQuad extends LinearOpMode {
// Identify which encoder OctoQuad inputs are connected to each odometry pod.
private final int ODO_LEFT = 0; // Facing forward direction on left side of robot (Axial motion)
private final int ODO_RIGHT = 1; // Facing forward direction on right side or robot (Axial motion )
private final int ODO_PERP = 2; // Facing perpendicular direction at the center of the robot (Lateral motion)
// Declare the OctoQuad object and members to store encoder positions and velocities
private OctoQuad octoquad;
private int posLeft;
private int posRight;
private int posPerp;
/**
* This function is executed when this OpMode is selected from the Driver Station.
*/
@Override
public void runOpMode() {
// Connect to OctoQuad by referring to its name in the Robot Configuration.
octoquad = hardwareMap.get(OctoQuad.class, "octoquad");
// Read the Firmware Revision number from the OctoQuad and display it as telemetry.
telemetry.addData("OctoQuad Firmware Version ", octoquad.getFirmwareVersion());
// Reverse the count-direction of any encoder that is not what you require.
// Eg: if you push the robot forward and the left encoder counts down, then reverse it so it counts up.
octoquad.setSingleEncoderDirection(ODO_LEFT, OctoQuad.EncoderDirection.REVERSE);
octoquad.setSingleEncoderDirection(ODO_RIGHT, OctoQuad.EncoderDirection.FORWARD);
octoquad.setSingleEncoderDirection(ODO_PERP, OctoQuad.EncoderDirection.FORWARD);
// Any changes that are made should be saved in FLASH just in case there is a sensor power glitch.
octoquad.saveParametersToFlash();
telemetry.addLine("\nPress Play to read encoder values");
telemetry.update();
waitForStart();
// Configure the telemetry for optimal display of data.
telemetry.setDisplayFormat(Telemetry.DisplayFormat.MONOSPACE);
telemetry.setMsTransmissionInterval(50);
// Set all the encoder inputs to zero
octoquad.resetAllPositions();
// Loop while displaying the odometry pod positions.
while (opModeIsActive()) {
telemetry.addData(">", "Press X to Reset Encoders\n");
// Check for X button to reset encoders
if (gamepad1.x) {
// Reset the position of all encoders to zero.
octoquad.resetAllPositions();
}
// Read all the encoder data. Load into local members
readOdometryPods();
// Display the values
telemetry.addData("Left ", "%8d counts", posLeft);
telemetry.addData("Right", "%8d counts", posRight);
telemetry.addData("Perp ", "%8d counts", posPerp);
telemetry.update();
}
}
private void readOdometryPods() {
// For best performance, we should only perform ONE transaction with the OctoQuad each cycle.
// Since this example only needs to read positions from a few channels, we could use either
// readPositionRange(idxFirst, idxLast) to get a select number of sequential channels
// or
// readAllPositions() to get all 8 encoder readings
//
// Since both calls take almost the same amount of time, and the actual channels may not end up being sequential,
// we will read all of the encoder positions, and then pick out the ones we need.
int[] positions = octoquad.readAllPositions();
posLeft = positions[ODO_LEFT];
posRight = positions[ODO_RIGHT];
posPerp = positions[ODO_PERP];
}
}

278
FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorOctoQuadAdv.java vendored Normal file
View File

@ -0,0 +1,278 @@
/*
* Copyright (c) 2024 DigitalChickenLabs
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
package org.firstinspires.ftc.robotcontroller.external.samples;
import com.qualcomm.hardware.digitalchickenlabs.OctoQuad;
import com.qualcomm.hardware.digitalchickenlabs.OctoQuadBase;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.util.ElapsedTime;
import com.qualcomm.robotcore.util.MovingStatistics;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import org.firstinspires.ftc.robotcore.external.Telemetry;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import java.util.ArrayList;
import java.util.List;
/*
* This OpMode illustrates how to use advanced features of the DigitalChickenLabs OctoQuad Quadrature Encoder & Pulse Width Interface Module
*
* The OctoQuad has 8 input channels that can used to read either Quadrature Encoder signals (like with most FTC motors)
* or Pulse-Width style Absolute Encoder inputs (eg: REV Through Bore encoder pulse width output).
*
* This OpMode illustrates several of the more advanced features of an OctoQuad, including Pulse Width measurement and velocity measurement.
* For a more basic OpMode just showing how to read encoder inputs, see the SensorOctoQuad sample.
*
* This OpMode assumes that the OctoQuad is attached to an I2C interface named "octoquad" in the robot configuration.
*
* One system that requires a lot of encoder inputs is a Swerve Drive system.
* Such a drive requires two encoders per drive module: one for position & velocity of the Drive motor/wheel, and one for position/angle of the Steering motor.
* The Drive motor usually requires a quadrature encoder, and the Steering motor requires an Absolute encoder.
* This quantity and combination of encoder inputs is a challenge, but a single OctoQuad could be used to read them all.
*
* This sample will configure an OctoQuad for a swerve drive, as follows
* - Configure 4 Relative Quadrature Encoder inputs and 4 Absolute Pulse-Width Encoder inputs
* - Configure a velocity sample interval of 25 mSec
* - Configure a pulse width input range of 1-1024 uSec for a REV Through Bore Encoder's Absolute Pulse output.
*
* An OctoSwerveDrive class will be created to initialize the OctoQuad, and manage the 4 swerve modules.
* An OctoSwerveModule class will be created to configure and read a single swerve module.
*
* Wiring:
* The OctoQuad will be configured to accept Quadrature encoders on the first four channels and Absolute (pulse width) encoders on the last four channels.
*
* The standard 4-pin to 4-pin cable can be used to connect each Driver Motor encoder to the OctoQuad. (channels 0-3)
*
* A modified version of the REV 6-4 pin cable (shipped with the encoder) connects the steering encoder to the OctoQuad. (channels 4-7)
* To connect the Absolute position signal from a REV Thru-Bore encoder to the OctoQuad, the Provided 6-pin to 4-pin cable will need to be modified.
* At the 6-pin connector end, move the yellow wire from its initial pin-3 position to the ABS pin-5 position. This can be done easily
* by using a small flathead screwdriver to lift the small white tab holding the metal wire crimp in place and gently pulling the wire out.
* See the OctoSwerveDrive() constructor below for the correct wheel/channel assignment.
*
* Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name.
* Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list
*
* Note: If you prefer, you can move the two support classes from this file, and place them in their own files.
* But leaving them in place is simpler for this example.
*
* See the sensor's product page: https://www.tindie.com/products/digitalchickenlabs/octoquad-8ch-quadrature-pulse-width-decoder/
*/
@TeleOp(name="OctoQuad Advanced", group="OctoQuad")
@Disabled
public class SensorOctoQuadAdv extends LinearOpMode {
@Override
public void runOpMode() throws InterruptedException {
// Connect to the OctoQuad by looking up its name in the hardwareMap.
OctoQuad octoquad = hardwareMap.get(OctoQuad.class, "octoquad");
// Create the interface for the Swerve Drive Encoders
OctoSwerveDrive octoSwerveDrive = new OctoSwerveDrive(octoquad);
// Display the OctoQuad firmware revision
telemetry.addLine("OctoQuad Firmware v" + octoquad.getFirmwareVersion());
telemetry.addLine("\nPress Play to read encoder values");
telemetry.update();
waitForStart();
// Configure the telemetry for optimal display of data.
telemetry.setDisplayFormat(Telemetry.DisplayFormat.MONOSPACE);
telemetry.setMsTransmissionInterval(50);
// Run stats to determine cycle times.
MovingStatistics avgTime = new MovingStatistics(100);
ElapsedTime elapsedTime = new ElapsedTime();
while (opModeIsActive()) {
telemetry.addData(">", "Press X to Reset Encoders\n");
if(gamepad1.x) {
octoquad.resetAllPositions();
}
// read all the swerve drive encoders and update positions and velocities.
octoSwerveDrive.updateModules();
octoSwerveDrive.show(telemetry);
// Update cycle time stats
avgTime.add(elapsedTime.nanoseconds());
elapsedTime.reset();
telemetry.addData("Loop time", "%.1f mS", avgTime.getMean()/1000000);
telemetry.update();
}
}
}
// ============================ Internal (Inner) Classes =============================
/***
* OctoSwerveDrive class manages 4 Swerve Modules
* - Performs general OctoQuad initialization
* - Creates 4 module classes, one for each swerve module
* - Updates swerve drive status by reading all data from OctoQuad and Updating each module
* - Displays all swerve drive data as telemetry
*/
class OctoSwerveDrive {
private final OctoQuad octoquad;
private final List<OctoSwerveModule> allModules = new ArrayList<>();
// members to hold encoder data for each module.
public final OctoSwerveModule LeftFront;
public final OctoSwerveModule RightFront;
public final OctoSwerveModule LeftBack;
public final OctoSwerveModule RightBack;
// Prepare an object to hold an entire OctoQuad encoder readable register bank (pos and vel)
private final OctoQuad.EncoderDataBlock encoderDataBlock = new OctoQuad.EncoderDataBlock();
public OctoSwerveDrive(OctoQuad octoquad) {
this.octoquad = octoquad;
// Clear out all prior settings and encoder data before setting up desired configuration
octoquad.resetEverything();
// Assume first 4 channels are relative encoders and the next 4 are absolute encoders
octoquad.setChannelBankConfig(OctoQuad.ChannelBankConfig.BANK1_QUADRATURE_BANK2_PULSE_WIDTH);
// Create the four OctoSwerveModules, and add them to a 'list' for easier reference.
// Note: The wheel/channel order is set here. Ensure your encoder cables match.
//
// Note: The angleOffset must be set for each module so a forward facing wheel shows a steer angle of 0 degrees.
// The process for determining the correct angleOffsets is as follows:
// 1) Set all the angleValues (below) to zero then build and deploy the code.
// 2) Physically rotate all the swerve drives so the wheels are facing forward in the desired 0 degree position
// 3) Run the OpMode, view the telemetry and record the "Degrees" value for each module.
// 4) Update the code by entering the recorded Degrees value for each module as the angleOffset (last) parameter in the lines below.
//
// Rebuild and deploy the new code. Verify that the telemetry now indicates 0 degrees when the wheels are facing forward.
// Also verify that the correct module values change appropriately when you manually spin (drive) and rotate (steer) a wheel.
allModules.add(LeftFront = new OctoSwerveModule(octoquad, "LF ",0,0));// Drive = 0, Steer = 4
allModules.add(RightFront = new OctoSwerveModule(octoquad, "RF ",1,0));// Drive = 1, Steer = 5
allModules.add(LeftBack = new OctoSwerveModule(octoquad, "LB ",2,0));// Drive = 2, Steer = 6
allModules.add(RightBack = new OctoSwerveModule(octoquad, "RB ",3,0));// Drive = 3, Steer = 7
// now make sure the settings persist through any power glitches.
octoquad.saveParametersToFlash();
}
/**
* Updates all 4 swerve modules
*/
public void updateModules() {
// Read full OctoQuad data block and then pass DataBlock to each swerve module to update themselves.
octoquad.readAllEncoderData(encoderDataBlock);
for (OctoSwerveModule module : allModules) {
module.updateModule(encoderDataBlock);
}
}
/**
* Generate telemetry data for all modules.
* @param telemetry OpMode Telemetry object
*/
public void show(Telemetry telemetry) {
// create general header block and then have each module add its own telemetry
telemetry.addData("pos", " Count CPS Degree DPS");
for (OctoSwerveModule module : allModules) {
module.show(telemetry);
}
}
}
/***
* The OctoSwerveModule class manages a single swerve module
*/
class OctoSwerveModule {
public double driveCounts;
public double driveCountsPerSec;
public double steerDegrees;
public double steerDegreesPerSec;
private final String name;
private final int channel;
private final double angleOffset;
private final double steerDirMult;
private static final int VELOCITY_SAMPLE_INTERVAL_MS = 25; // To provide 40 updates per second.
private static final double DEGREES_PER_US = (360.0 / 1024.0); // based on REV Through Bore Encoder
private static final double VELOCITY_SAMPLES_PER_S = (1000.0 / VELOCITY_SAMPLE_INTERVAL_MS);
// The correct drive and turn directions must be set for the Swerve Module based on the specific hardware geometry.
// Forward motion must generate an increasing drive count.
// Counter Clockwise steer rotation must generate an increasing Steer Angle (degrees)
private static final boolean INVERT_DRIVE_ENCODER = false; // Set true if forward motion decreases drive "Count"
private static final boolean INVERT_STEER_ENCODER = false; // Set true if counter clockwise steer action decreases steer "Degree"
/***
* @param octoquad provide access to configure OctoQuad
* @param name name used for telemetry display
* @param quadChannel Quadrature encoder channel. Pulse Width channel is this + 4
* @param angleOffset Angle to subtract from absolute encoder to calibrate zero position. (see comments above)
*/
public OctoSwerveModule (OctoQuad octoquad, String name, int quadChannel, double angleOffset) {
this.name = name;
this.channel = quadChannel;
this.angleOffset = angleOffset;
this.steerDirMult = INVERT_STEER_ENCODER ? -1 : 1 ; // create a multiplier to flip the steer angle.
// Set the drive encoder direction. Note the absolute encoder does not have built-in direction inversion.
octoquad.setSingleEncoderDirection(channel, INVERT_DRIVE_ENCODER ? OctoQuad.EncoderDirection.REVERSE : OctoQuad.EncoderDirection.FORWARD);
// Set the velocity sample interval on both encoders
octoquad.setSingleVelocitySampleInterval(channel, VELOCITY_SAMPLE_INTERVAL_MS);
octoquad.setSingleVelocitySampleInterval(channel + 4, VELOCITY_SAMPLE_INTERVAL_MS);
// Setup Absolute encoder pulse range to match REV Through Bore encoder.
octoquad.setSingleChannelPulseWidthParams (channel + 4, new OctoQuad.ChannelPulseWidthParams(1,1024));
}
/***
* Calculate the Swerve module's position and velocity values
* @param encoderDataBlock most recent full data block read from OctoQuad.
*/
public void updateModule(OctoQuad.EncoderDataBlock encoderDataBlock) {
driveCounts = encoderDataBlock.positions[channel]; // get Counts.
driveCountsPerSec = encoderDataBlock.velocities[channel] * VELOCITY_SAMPLES_PER_S; // convert counts/interval to counts/sec
// convert uS to degrees. Add in any possible direction flip.
steerDegrees = AngleUnit.normalizeDegrees((encoderDataBlock.positions[channel+ 4] * DEGREES_PER_US * steerDirMult) - angleOffset);
// convert uS/interval to deg per sec. Add in any possible direction flip.
steerDegreesPerSec = encoderDataBlock.velocities[channel + 4] * DEGREES_PER_US * steerDirMult * VELOCITY_SAMPLES_PER_S;
}
/**
* Display the Swerve module's state as telemetry
* @param telemetry OpMode Telemetry object
*/
public void show(Telemetry telemetry) {
telemetry.addData(name, "%8.0f %7.0f %7.0f %6.0f", driveCounts, driveCountsPerSec, steerDegrees, steerDegreesPerSec);
}
}

156
FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorSparkFunOTOS.java vendored Normal file
View File

@ -0,0 +1,156 @@
/*
SPDX-License-Identifier: MIT
Copyright (c) 2024 SparkFun Electronics
*/
package org.firstinspires.ftc.robotcontroller.external.samples;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.hardware.sparkfun.SparkFunOTOS;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
/*
* This OpMode illustrates how to use the SparkFun Qwiic Optical Tracking Odometry Sensor (OTOS)
*
* The OpMode assumes that the sensor is configured with a name of "sensor_otos".
*
* Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name.
* Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list
*
* See the sensor's product page: https://www.sparkfun.com/products/24904
*/
@TeleOp(name = "Sensor: SparkFun OTOS", group = "Sensor")
@Disabled
public class SensorSparkFunOTOS extends LinearOpMode {
// Create an instance of the sensor
SparkFunOTOS myOtos;
@Override
public void runOpMode() throws InterruptedException {
// Get a reference to the sensor
myOtos = hardwareMap.get(SparkFunOTOS.class, "sensor_otos");
// All the configuration for the OTOS is done in this helper method, check it out!
configureOtos();
// Wait for the start button to be pressed
waitForStart();
// Loop until the OpMode ends
while (opModeIsActive()) {
// Get the latest position, which includes the x and y coordinates, plus the
// heading angle
SparkFunOTOS.Pose2D pos = myOtos.getPosition();
// Reset the tracking if the user requests it
if (gamepad1.y) {
myOtos.resetTracking();
}
// Re-calibrate the IMU if the user requests it
if (gamepad1.x) {
myOtos.calibrateImu();
}
// Inform user of available controls
telemetry.addLine("Press Y (triangle) on Gamepad to reset tracking");
telemetry.addLine("Press X (square) on Gamepad to calibrate the IMU");
telemetry.addLine();
// Log the position to the telemetry
telemetry.addData("X coordinate", pos.x);
telemetry.addData("Y coordinate", pos.y);
telemetry.addData("Heading angle", pos.h);
// Update the telemetry on the driver station
telemetry.update();
}
}
private void configureOtos() {
telemetry.addLine("Configuring OTOS...");
telemetry.update();
// Set the desired units for linear and angular measurements. Can be either
// meters or inches for linear, and radians or degrees for angular. If not
// set, the default is inches and degrees. Note that this setting is not
// persisted in the sensor, so you need to set at the start of all your
// OpModes if using the non-default value.
// myOtos.setLinearUnit(DistanceUnit.METER);
myOtos.setLinearUnit(DistanceUnit.INCH);
// myOtos.setAngularUnit(AnguleUnit.RADIANS);
myOtos.setAngularUnit(AngleUnit.DEGREES);
// Assuming you've mounted your sensor to a robot and it's not centered,
// you can specify the offset for the sensor relative to the center of the
// robot. The units default to inches and degrees, but if you want to use
// different units, specify them before setting the offset! Note that as of
// firmware version 1.0, these values will be lost after a power cycle, so
// you will need to set them each time you power up the sensor. For example, if
// the sensor is mounted 5 inches to the left (negative X) and 10 inches
// forward (positive Y) of the center of the robot, and mounted 90 degrees
// clockwise (negative rotation) from the robot's orientation, the offset
// would be {-5, 10, -90}. These can be any value, even the angle can be
// tweaked slightly to compensate for imperfect mounting (eg. 1.3 degrees).
SparkFunOTOS.Pose2D offset = new SparkFunOTOS.Pose2D(0, 0, 0);
myOtos.setOffset(offset);
// Here we can set the linear and angular scalars, which can compensate for
// scaling issues with the sensor measurements. Note that as of firmware
// version 1.0, these values will be lost after a power cycle, so you will
// need to set them each time you power up the sensor. They can be any value
// from 0.872 to 1.127 in increments of 0.001 (0.1%). It is recommended to
// first set both scalars to 1.0, then calibrate the angular scalar, then
// the linear scalar. To calibrate the angular scalar, spin the robot by
// multiple rotations (eg. 10) to get a precise error, then set the scalar
// to the inverse of the error. Remember that the angle wraps from -180 to
// 180 degrees, so for example, if after 10 rotations counterclockwise
// (positive rotation), the sensor reports -15 degrees, the required scalar
// would be 3600/3585 = 1.004. To calibrate the linear scalar, move the
// robot a known distance and measure the error; do this multiple times at
// multiple speeds to get an average, then set the linear scalar to the
// inverse of the error. For example, if you move the robot 100 inches and
// the sensor reports 103 inches, set the linear scalar to 100/103 = 0.971
myOtos.setLinearScalar(1.0);
myOtos.setAngularScalar(1.0);
// The IMU on the OTOS includes a gyroscope and accelerometer, which could
// have an offset. Note that as of firmware version 1.0, the calibration
// will be lost after a power cycle; the OTOS performs a quick calibration
// when it powers up, but it is recommended to perform a more thorough
// calibration at the start of all your OpModes. Note that the sensor must
// be completely stationary and flat during calibration! When calling
// calibrateImu(), you can specify the number of samples to take and whether
// to wait until the calibration is complete. If no parameters are provided,
// it will take 255 samples and wait until done; each sample takes about
// 2.4ms, so about 612ms total
myOtos.calibrateImu();
// Reset the tracking algorithm - this resets the position to the origin,
// but can also be used to recover from some rare tracking errors
myOtos.resetTracking();
// After resetting the tracking, the OTOS will report that the robot is at
// the origin. If your robot does not start at the origin, or you have
// another source of location information (eg. vision odometry), you can set
// the OTOS location to match and it will continue to track from there.
SparkFunOTOS.Pose2D currentPosition = new SparkFunOTOS.Pose2D(0, 0, 0);
myOtos.setPosition(currentPosition);
// Get the hardware and firmware version
SparkFunOTOS.Version hwVersion = new SparkFunOTOS.Version();
SparkFunOTOS.Version fwVersion = new SparkFunOTOS.Version();
myOtos.getVersionInfo(hwVersion, fwVersion);
telemetry.addLine("OTOS configured! Press start to get position data!");
telemetry.addLine();
telemetry.addLine(String.format("OTOS Hardware Version: v%d.%d", hwVersion.major, hwVersion.minor));
telemetry.addLine(String.format("OTOS Firmware Version: v%d.%d", fwVersion.major, fwVersion.minor));
telemetry.update();
}
}

812
FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/UtilityOctoQuadConfigMenu.java vendored Normal file
View File

@ -0,0 +1,812 @@
/*
* Copyright (c) 2024 DigitalChickenLabs
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
package org.firstinspires.ftc.robotcontroller.external.samples;
import com.qualcomm.hardware.digitalchickenlabs.OctoQuad;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.Gamepad;
import org.firstinspires.ftc.robotcore.external.Telemetry;
import java.util.ArrayList;
import java.util.Stack;
/*
* This OpMode illustrates how to use the DigitalChickenLabs OctoQuad Quadrature Encoder & Pulse Width Interface Module.
*
* The OctoQuad has 8 input channels that can used to read either Relative Quadrature Encoders or Pulse-Width Absolute Encoder inputs.
* Relative Quadrature encoders are found on most FTC motors, and some stand-alone position sensors like the REV Thru-Bore encoder.
* Pulse-Width encoders are less common. The REV Thru-Bore encoder can provide its absolute position via a variable pulse width,
* as can several sonar rangefinders such as the MaxBotix MB1000 series.
*
* This OpMode assumes that the OctoQuad is attached to an I2C interface named "octoquad" in the robot configuration.
*
* Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list
*
* Select, Init and run the "OctoQuad Configuration Tool" OpMode
* Read the blue User-Interface tips at the top of the telemetry screen.
* Use the UI buttons to navigate the menu and make any desired changes to the OctoQuad configuration.
* Use the Program Settings To FLASH option to make any changes permanent.
*
* See the sensor's product page: https://www.tindie.com/products/digitalchickenlabs/octoquad-8ch-quadrature-pulse-width-decoder/
*/
@TeleOp(name = "OctoQuad Configuration Tool", group="OctoQuad")
@Disabled
public class UtilityOctoQuadConfigMenu extends LinearOpMode
{
TelemetryMenu.MenuElement rootMenu = new TelemetryMenu.MenuElement("OctoQuad Config Menu", true);
TelemetryMenu.MenuElement menuHwInfo = new TelemetryMenu.MenuElement("Hardware Information", false);
TelemetryMenu.EnumOption optionI2cResetMode;
TelemetryMenu.EnumOption optionChannelBankConfig;
TelemetryMenu.MenuElement menuEncoderDirections = new TelemetryMenu.MenuElement("Set Encoder Directions", false);
TelemetryMenu.BooleanOption[] optionsEncoderDirections = new TelemetryMenu.BooleanOption[OctoQuad.NUM_ENCODERS];
TelemetryMenu.MenuElement menuVelocityIntervals = new TelemetryMenu.MenuElement("Velocity Measurement Intervals", false);
TelemetryMenu.IntegerOption[] optionsVelocityIntervals = new TelemetryMenu.IntegerOption[OctoQuad.NUM_ENCODERS];
TelemetryMenu.MenuElement menuAbsParams = new TelemetryMenu.MenuElement("Abs. Encoder Pulse Width Params", false);
TelemetryMenu.IntegerOption[] optionsAbsParamsMax = new TelemetryMenu.IntegerOption[OctoQuad.NUM_ENCODERS];
TelemetryMenu.IntegerOption[] optionsAbsParamsMin = new TelemetryMenu.IntegerOption[OctoQuad.NUM_ENCODERS];
TelemetryMenu.OptionElement optionProgramToFlash;
TelemetryMenu.OptionElement optionSendToRAM;
TelemetryMenu.StaticClickableOption optionExit;
OctoQuad octoquad;
boolean error = false;
@Override
public void runOpMode()
{
octoquad = hardwareMap.getAll(OctoQuad.class).get(0);
if(octoquad.getChipId() != OctoQuad.OCTOQUAD_CHIP_ID)
{
telemetry.addLine("Error: cannot communicate with OctoQuad. Check your wiring and configuration and try again");
telemetry.update();
error = true;
}
else
{
if(octoquad.getFirmwareVersion().maj != OctoQuad.SUPPORTED_FW_VERSION_MAJ)
{
telemetry.addLine("Error: The OctoQuad is running a different major firmware version than this driver was built for. Cannot run configuration tool");
telemetry.update();
error = true;
}
}
if(error)
{
waitForStart();
return;
}
telemetry.addLine("Retrieving current configuration from OctoQuad");
telemetry.update();
optionExit = new TelemetryMenu.StaticClickableOption("Exit configuration menu")
{
@Override
void onClick() // called on OpMode thread
{
requestOpModeStop();
}
};
optionI2cResetMode = new TelemetryMenu.EnumOption("I2C Reset Mode", OctoQuad.I2cRecoveryMode.values(), octoquad.getI2cRecoveryMode());
optionChannelBankConfig = new TelemetryMenu.EnumOption("Channel Bank Modes", OctoQuad.ChannelBankConfig.values(), octoquad.getChannelBankConfig());
menuHwInfo.addChild(new TelemetryMenu.StaticItem("Board Firmware: v" + octoquad.getFirmwareVersion()));
//menuHwInfo.addChild(new TelemetryMenu.StaticItem("Board unique ID: FIXME"));
for(int i = 0; i < OctoQuad.NUM_ENCODERS; i++)
{
optionsEncoderDirections[i] = new TelemetryMenu.BooleanOption(
String.format("Encoder %d direction", i),
octoquad.getSingleEncoderDirection(i) == OctoQuad.EncoderDirection.REVERSE,
"-",
"+");
}
menuEncoderDirections.addChildren(optionsEncoderDirections);
for(int i = 0; i < OctoQuad.NUM_ENCODERS; i++)
{
optionsVelocityIntervals[i] = new TelemetryMenu.IntegerOption(
String.format("Chan %d velocity intvl", i),
OctoQuad.MIN_VELOCITY_MEASUREMENT_INTERVAL_MS,
OctoQuad.MAX_VELOCITY_MEASUREMENT_INTERVAL_MS,
octoquad.getSingleVelocitySampleInterval(i));
}
menuVelocityIntervals.addChildren(optionsVelocityIntervals);
for(int i = 0; i < OctoQuad.NUM_ENCODERS; i++)
{
OctoQuad.ChannelPulseWidthParams params = octoquad.getSingleChannelPulseWidthParams(i);
optionsAbsParamsMax[i] = new TelemetryMenu.IntegerOption(
String.format("Chan %d max pulse length", i),
OctoQuad.MIN_PULSE_WIDTH_US,
OctoQuad.MAX_PULSE_WIDTH_US,
params.max_length_us);
optionsAbsParamsMin[i] = new TelemetryMenu.IntegerOption(
String.format("Chan %d min pulse length", i),
OctoQuad.MIN_PULSE_WIDTH_US,
OctoQuad.MAX_PULSE_WIDTH_US,
params.min_length_us);
}
menuAbsParams.addChildren(optionsAbsParamsMin);
menuAbsParams.addChildren(optionsAbsParamsMax);
optionProgramToFlash = new TelemetryMenu.OptionElement()
{
String name = "Program Settings to FLASH";
long lastClickTime = 0;
@Override
protected String getDisplayText()
{
if(lastClickTime == 0)
{
return name;
}
else
{
if(System.currentTimeMillis() - lastClickTime < 1000)
{
return name + " **OK**";
}
else
{
lastClickTime = 0;
return name;
}
}
}
@Override
void onClick()
{
sendSettingsToRam();
octoquad.saveParametersToFlash();
lastClickTime = System.currentTimeMillis();
}
};
optionSendToRAM = new TelemetryMenu.OptionElement()
{
String name = "Send Settings to RAM";
long lastClickTime = 0;
@Override
protected String getDisplayText()
{
if(lastClickTime == 0)
{
return name;
}
else
{
if(System.currentTimeMillis() - lastClickTime < 1000)
{
return name + " **OK**";
}
else
{
lastClickTime = 0;
return name;
}
}
}
@Override
void onClick()
{
sendSettingsToRam();
lastClickTime = System.currentTimeMillis();
}
};
rootMenu.addChild(menuHwInfo);
rootMenu.addChild(optionI2cResetMode);
rootMenu.addChild(optionChannelBankConfig);
rootMenu.addChild(menuEncoderDirections);
rootMenu.addChild(menuVelocityIntervals);
rootMenu.addChild(menuAbsParams);
rootMenu.addChild(optionProgramToFlash);
rootMenu.addChild(optionSendToRAM);
rootMenu.addChild(optionExit);
TelemetryMenu menu = new TelemetryMenu(telemetry, rootMenu);
while (!isStopRequested())
{
menu.loop(gamepad1);
telemetry.update();
sleep(20);
}
}
void sendSettingsToRam()
{
for(int i = 0; i < OctoQuad.NUM_ENCODERS; i++)
{
octoquad.setSingleEncoderDirection(i, optionsEncoderDirections[i].getValue() ? OctoQuad.EncoderDirection.REVERSE : OctoQuad.EncoderDirection.FORWARD);
octoquad.setSingleVelocitySampleInterval(i, optionsVelocityIntervals[i].getValue());
OctoQuad.ChannelPulseWidthParams params = new OctoQuad.ChannelPulseWidthParams();
params.max_length_us = optionsAbsParamsMax[i].getValue();
params.min_length_us = optionsAbsParamsMin[i].getValue();
octoquad.setSingleChannelPulseWidthParams(i, params);
}
octoquad.setI2cRecoveryMode((OctoQuad.I2cRecoveryMode) optionI2cResetMode.getValue());
octoquad.setChannelBankConfig((OctoQuad.ChannelBankConfig) optionChannelBankConfig.getValue());
}
/*
* Copyright (c) 2023 OpenFTC Team
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
public static class TelemetryMenu
{
private final MenuElement root;
private MenuElement currentLevel;
private boolean dpadUpPrev;
private boolean dpadDnPrev;
private boolean dpadRightPrev;
private boolean dpadLeftPrev;
private boolean xPrev;
private boolean lbPrev;
private int selectedIdx = 0;
private Stack<Integer> selectedIdxStack = new Stack<>();
private final Telemetry telemetry;
/**
* TelemetryMenu constructor
* @param telemetry pass in 'telemetry' from your OpMode
* @param root the root menu element
*/
public TelemetryMenu(Telemetry telemetry, MenuElement root)
{
this.root = root;
this.currentLevel = root;
this.telemetry = telemetry;
telemetry.setDisplayFormat(Telemetry.DisplayFormat.HTML);
telemetry.setMsTransmissionInterval(50);
}
/**
* Call this from inside your loop to put the current menu state into
* telemetry, and process gamepad inputs for navigating the menu
* @param gamepad the gamepad you want to use to navigate the menu
*/
public void loop(Gamepad gamepad)
{
// Capture current state of the gamepad buttons we care about;
// We can only look once or we risk a race condition
boolean dpadUp = gamepad.dpad_up;
boolean dpadDn = gamepad.dpad_down;
boolean dpadRight = gamepad.dpad_right;
boolean dpadLeft = gamepad.dpad_left;
boolean x = gamepad.x;
boolean lb = gamepad.left_bumper;
// Figure out who our children our at this level
// and figure out which item is currently highlighted
// with the selection pointer
ArrayList<Element> children = currentLevel.children();
Element currentSelection = children.get(selectedIdx);
// Left and right are inputs to the selected item (if it's an Option)
if (currentSelection instanceof OptionElement)
{
if (dpadRight && !dpadRightPrev) // rising edge
{
((OptionElement) currentSelection).onRightInput();
}
else if (dpadLeft && !dpadLeftPrev) // rising edge
{
((OptionElement) currentSelection).onLeftInput();
}
}
// Up and down navigate the current selection pointer
if (dpadUp && !dpadUpPrev) // rising edge
{
selectedIdx--; // Move selection pointer up
}
else if (dpadDn && !dpadDnPrev) // rising edge
{
selectedIdx++; // Move selection pointer down
}
// Make selected index sane (don't let it go out of bounds) :eyes:
if (selectedIdx >= children.size())
{
selectedIdx = children.size()-1;
}
else if (selectedIdx < 0)
{
selectedIdx = 0;
}
// Select: either enter submenu or input to option
else if (x && !xPrev) // rising edge
{
// Select up element
if (currentSelection instanceof SpecialUpElement)
{
// We can only go up if we're not at the root level
if (currentLevel != root)
{
// Restore selection pointer to where it was before
selectedIdx = selectedIdxStack.pop();
// Change to the parent level
currentLevel = currentLevel.parent();
}
}
// Input to option
else if (currentSelection instanceof OptionElement)
{
((OptionElement) currentSelection).onClick();
}
// Enter submenu
else if (currentSelection instanceof MenuElement)
{
// Save our current selection pointer so we can restore it
// later if the user navigates back up a level
selectedIdxStack.push(selectedIdx);
// We have no idea what's in the submenu :monkey: so best to
// just set the selection pointer to the first element
selectedIdx = 0;
// Now the current level becomes the submenu that the selection
// pointer was on
currentLevel = (MenuElement) currentSelection;
}
}
// Go up a level
else if (lb && !lbPrev)
{
// We can only go up if we're not at the root level
if (currentLevel != root)
{
// Restore selection pointer to where it was before
selectedIdx = selectedIdxStack.pop();
// Change to the parent level
currentLevel = currentLevel.parent();
}
}
// Save the current button states so that we can look for
// the rising edge the next time around the loop :)
dpadUpPrev = dpadUp;
dpadDnPrev = dpadDn;
dpadRightPrev = dpadRight;
dpadLeftPrev = dpadLeft;
xPrev = x;
lbPrev = lb;
// Start building the text display.
// First, we add the static directions for gamepad operation
StringBuilder builder = new StringBuilder();
builder.append("<font color='#119af5' face=monospace>");
builder.append("Navigate items.....dpad up/down\n")
.append("Select.............X\n")
.append("Edit option........dpad left/right\n")
.append("Up one level.......left bumper\n");
builder.append("</font>");
builder.append("\n");
// Now actually add the menu options. We start by adding the name of the current menu level.
builder.append("<font face=monospace>");
builder.append("Current Menu: ").append(currentLevel.name).append("\n");
// Now we loop through all the child elements of this level and add them
for (int i = 0; i < children.size(); i++)
{
// If the selection pointer is at this index, put a green dot in the box :)
if (selectedIdx == i)
{
builder.append("[<font color=green face=monospace>•</font>] ");
}
// Otherwise, just put an empty box
else
{
builder.append("[ ] ");
}
// Figure out who the selection pointer is pointing at :eyes:
Element e = children.get(i);
// If it's pointing at a submenu, indicate that it's a submenu to the user
// by prefixing "> " to the name.
if (e instanceof MenuElement)
{
builder.append("> ");
}
// Finally, add the element's name
builder.append(e.getDisplayText());
// We musn't forget the newline
builder.append("\n");
}
// Don't forget to close the font tag either
builder.append("</font>");
// Build the string!!!! :nerd:
String menu = builder.toString();
// Add it to telemetry
telemetry.addLine(menu);
}
public static class MenuElement extends Element
{
private String name;
private ArrayList<Element> children = new ArrayList<>();
/**
* Create a new MenuElement; may either be the root menu, or a submenu (set isRoot accordingly)
* @param name the name for this menu
* @param isRoot whether this is a root menu, or a submenu
*/
public MenuElement(String name, boolean isRoot)
{
this.name = name;
// If it's not the root menu, we add the up one level option as the first element
if (!isRoot)
{
children.add(new SpecialUpElement());
}
}
/**
* Add a child element to this menu (may either be an Option or another menu)
* @param child the child element to add
*/
public void addChild(Element child)
{
child.setParent(this);
children.add(child);
}
/**
* Add multiple child elements to this menu (may either be option, or another menu)
* @param children the children to add
*/
public void addChildren(Element[] children)
{
for (Element e : children)
{
e.setParent(this);
this.children.add(e);
}
}
@Override
protected String getDisplayText()
{
return name;
}
private ArrayList<Element> children()
{
return children;
}
}
public static abstract class OptionElement extends Element
{
/**
* Override this to get notified when the element is clicked
*/
void onClick() {}
/**
* Override this to get notified when the element gets a "left edit" input
*/
protected void onLeftInput() {}
/**
* Override this to get notified when the element gets a "right edit" input
*/
protected void onRightInput() {}
}
public static class EnumOption extends OptionElement
{
protected int idx = 0;
protected Enum[] e;
protected String name;
public EnumOption(String name, Enum[] e)
{
this.e = e;
this.name = name;
}
public EnumOption(String name, Enum[] e, Enum def)
{
this(name, e);
idx = def.ordinal();
}
@Override
public void onLeftInput()
{
idx++;
if(idx > e.length-1)
{
idx = 0;
}
}
@Override
public void onRightInput()
{
idx--;
if(idx < 0)
{
idx = e.length-1;
}
}
@Override
public void onClick()
{
onRightInput();
}
@Override
protected String getDisplayText()
{
return String.format("%s: <font color='#e37c07' face=monospace>%s</font>", name, e[idx].name());
}
public Enum getValue()
{
return e[idx];
}
}
public static class IntegerOption extends OptionElement
{
protected int i;
protected int min;
protected int max;
protected String name;
public IntegerOption(String name, int min, int max, int def)
{
this.name = name;
this.min = min;
this.max = max;
this.i = def;
}
@Override
public void onLeftInput()
{
i--;
if(i < min)
{
i = max;
}
}
@Override
public void onRightInput()
{
i++;
if(i > max)
{
i = min;
}
}
@Override
public void onClick()
{
onRightInput();
}
@Override
protected String getDisplayText()
{
return String.format("%s: <font color='#e37c07' face=monospace>%d</font>", name, i);
}
public int getValue()
{
return i;
}
}
static class BooleanOption extends OptionElement
{
private String name;
private boolean val = true;
private String customTrue;
private String customFalse;
BooleanOption(String name, boolean def)
{
this.name = name;
this.val = def;
}
BooleanOption(String name, boolean def, String customTrue, String customFalse)
{
this(name, def);
this.customTrue = customTrue;
this.customFalse = customFalse;
}
@Override
public void onLeftInput()
{
val = !val;
}
@Override
public void onRightInput()
{
val = !val;
}
@Override
public void onClick()
{
val = !val;
}
@Override
protected String getDisplayText()
{
String valStr;
if(customTrue != null && customFalse != null)
{
valStr = val ? customTrue : customFalse;
}
else
{
valStr = val ? "true" : "false";
}
return String.format("%s: <font color='#e37c07' face=monospace>%s</font>", name, valStr);
}
public boolean getValue()
{
return val;
}
}
/**
*
*/
public static class StaticItem extends OptionElement
{
private String name;
public StaticItem(String name)
{
this.name = name;
}
@Override
protected String getDisplayText()
{
return name;
}
}
public static abstract class StaticClickableOption extends OptionElement
{
private String name;
public StaticClickableOption(String name)
{
this.name = name;
}
abstract void onClick();
@Override
protected String getDisplayText()
{
return name;
}
}
private static abstract class Element
{
private MenuElement parent;
protected void setParent(MenuElement parent)
{
this.parent = parent;
}
protected MenuElement parent()
{
return parent;
}
protected abstract String getDisplayText();
}
private static class SpecialUpElement extends Element
{
@Override
protected String getDisplayText()
{
return "<font color='#119af5' face=monospace>.. ↰ Up One Level</font>";
}
}
}
}

5
FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/sample_conventions.md vendored
View File

@ -29,6 +29,11 @@ Concept: This is a sample OpMode that illustrates performing a specific function
Each OpMode should try to only demonstrate a single concept so they are easy to Each OpMode should try to only demonstrate a single concept so they are easy to
locate based on their name. These OpModes may not produce a drivable robot. locate based on their name. These OpModes may not produce a drivable robot.
Utility: This sample type is provided as a useful tool, or aide, to perform some specific development task.
It is not expected to be something you would include in your own robot code.
To use the tool, comment out the @Disabled annotation and build the App.
Read the comments found in the sample for an explanation of its intended use.
After the prefix, other conventions will apply: After the prefix, other conventions will apply:
* Sensor class names should constructed as: Sensor - Company - Type * Sensor class names should constructed as: Sensor - Company - Type

4
FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/internal/FtcOpModeRegister.java
View File

@ -34,8 +34,6 @@ package org.firstinspires.ftc.robotcontroller.internal;
import com.qualcomm.robotcore.eventloop.opmode.OpModeManager; import com.qualcomm.robotcore.eventloop.opmode.OpModeManager;
import com.qualcomm.robotcore.eventloop.opmode.OpModeRegister; import com.qualcomm.robotcore.eventloop.opmode.OpModeRegister;
import org.firstinspires.ftc.robotcontroller.external.samples.ConceptNullOp;
/** /**
* {@link FtcOpModeRegister} is responsible for registering OpModes for use in an FTC game. * {@link FtcOpModeRegister} is responsible for registering OpModes for use in an FTC game.
* @see #register(OpModeManager) * @see #register(OpModeManager)
@ -49,7 +47,7 @@ public class FtcOpModeRegister implements OpModeRegister {
* There are two mechanisms by which an OpMode may be registered. * There are two mechanisms by which an OpMode may be registered.
* *
* 1) The preferred method is by means of class annotations in the OpMode itself. * 1) The preferred method is by means of class annotations in the OpMode itself.
* See, for example the class annotations in {@link ConceptNullOp}. * See, for example the class annotations in {@link org.firstinspires.ftc.robotcontroller.external.samples.ConceptNullOp}.
* *
* 2) The other, retired, method is to modify this {@link #register(OpModeManager)} * 2) The other, retired, method is to modify this {@link #register(OpModeManager)}
* method to include explicit calls to OpModeManager.register(). * method to include explicit calls to OpModeManager.register().

32
README.md
View File

@ -254,7 +254,7 @@ The Javadoc reference documentation for the TRC Robotics Framework Library can b
### Online User Forum ### Online User Forum
For technical questions regarding our Framework Library, please post questions on the FTC Forums [here](https://ftcforum.firstinspires.org/forum/ftc-technology/android-studio). For technical questions regarding our Framework Library, please post questions on the FTC Forums [here](https://ftcforum.firstinspires.org/forum/ftc-technology/android-studio).
### Sample OpModes ### TRC Sample OpModes
In addition, we provide a large selection of sample OpModes (sample robot code) that show you how to use various features of our library. You can find them in a separate GitHub repository [here](https://github.com/trc492/TrcFtcSamples). In addition, we provide a large selection of sample OpModes (sample robot code) that show you how to use various features of our library. You can find them in a separate GitHub repository [here](https://github.com/trc492/TrcFtcSamples).
# FTC SDK Release Information # FTC SDK Release Information
@ -312,10 +312,38 @@ For technical questions regarding the Control System or the FTC SDK, please visi
&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;[FIRST Tech Challenge Community](https://ftc-community.firstinspires.org/) &nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;[FIRST Tech Challenge Community](https://ftc-community.firstinspires.org/)
### Sample OpModes ### Sample OpModes
In addition, we provide a large selection of Sample OpModes (robot code examples) that show you how to use various features of our library. You can find them in a separate GitHub repository [here](https://github.com/trc492/TrcFtcSamples) This project contains a large selection of Sample OpModes (robot code examples) which can be cut and pasted into your /teamcode folder to be used as-is, or modified to suit your team's needs.
Samples Folder: &nbsp;&nbsp; [/FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples](FtcRobotController/src/main/java/org/firstinspires/ftc/robotcontroller/external/samples)
The readme.md file located in the [/TeamCode/src/main/java/org/firstinspires/ftc/teamcode](TeamCode/src/main/java/org/firstinspires/ftc/teamcode) folder contains an explanation of the sample naming convention, and instructions on how to copy them to your own project space.
# Release Information # Release Information
## Version 9.2 (20240701-085519)
### Important Notes
* Java classes and Blocks for TensorFlow Object Detection have been deprecated and will be removed in Version 10.0.
* The samples that use TensorFlow Object Detection have been removed.
### Enhancements
* Adds explanatory text to failed items on the inspection activities. To view the explanatory text tap the red warning icon for a failed item.
* In the Blocks editor: added a new kind of variable set block that sets the variable and also returns the new value.
* Changes the way that camera controls behave for a SwitchableCamera. Now, each method (such as getExposure, getMinExposure, getMaxExposure, setExposure for ExposureControl) acts on the currently active camera.
* Adds support for the REV USB PS4 Compatible Gamepad (REV-31-2983)
* Adds ConceptAprilTagMultiPortal OpMode
* Adds support for OctoQuad Quadrature Encoder & Pulse Width Interface Module
* Adds the ExportAprilTagLibraryToBlocks annotation that indicates that a static method that returns an AprilTagLibrary is exported to the Blocks programming environment. The corresponding block will appear in the Blocks toolbox along with the built-in tag libraries.
* Adds Blocks OpMode ConceptAprilTagOptimizeExposure.
* Adds support for the SparkFun Optical Tracking Odometry sensor.
### Bug Fixes
* Fixes https://github.com/FIRST-Tech-Challenge/FtcRobotController/issues/942 where visionPortal.close() can cause an IndexOutOfBoundsError.
* Fixes a bug in the blocks editor where collapsed function blocks show a warning "Collapsed blocks contain warnings." when the Blocks OpMode is reopened.
* Fixes a bug where the blocks editor wouldn't warn you that you have unsaved changes when you try to leave. This bug was introduced due to a behavior change in Chrome 119.
* [Issue #764](https://github.com/FIRST-Tech-Challenge/FtcRobotController/issues/764) - Get gain control returns a null pointer for a switchable camera
* Fixes a bug where the correct deadzone for certain gamepads was not applied when Advanced Gamepad Features was enabled
## Version 9.1 (20240215-115542) ## Version 9.1 (20240215-115542)
### Enhancements ### Enhancements

81
TeamCode/src/main/java/com/qualcomm/hardware/digitalchickenlabs/CachingOctoQuad.java
View File

@ -1,81 +0,0 @@
/*
* Copyright (c) 2022 DigitalChickenLabs
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
package com.qualcomm.hardware.digitalchickenlabs;
public interface CachingOctoQuad extends OctoQuadBase
{
/**
* Controls how data is cached to reduce the number
* of Lynx transactions needed to read the encoder data
*/
enum CachingMode
{
/*
* No caching at all
*/
NONE,
/**
* The cache is only updated when you call {@link #setCachingMode(CachingMode)}
*/
MANUAL,
/**
* The cache is updated the 2nd time you call {@link #readSinglePosition_Caching(int)} (int)}
* or {@link #readSingleVelocity_Caching(int)} (int)} for the same encoder index.
*/
AUTO
}
/**
* Set the data caching mode for the OctoQuad
* @param mode mode to use
*/
void setCachingMode(CachingMode mode);
/**
* Manually refresh the position and velocity data cache
*/
void refreshCache();
/**
* Read a single position from the OctoQuad
* Note this call may return cached data based on
* the {@link CachingMode} you selected!
* Depending on the channel bank configuration, this may
* either be quadrature step count, or pulse width.
* @param idx the index of the encoder to read
* @return the position for the specified encoder
*/
int readSinglePosition_Caching(int idx);
/**
* Read a single velocity from the OctoQuad
* Note this call may return cached data based on
* the {@link CachingMode} you selected!
* NOTE: if using an absolute pulse width encoder, in order to get sane
* velocity data, you must set the channel min/max pulse width parameter.
* @param idx the index of the encoder to read
* @return the velocity for the specified encoder
*/
short readSingleVelocity_Caching(int idx);
}

225
TeamCode/src/main/java/com/qualcomm/hardware/digitalchickenlabs/OctoQuad.java
View File

@ -1,225 +0,0 @@
/*
* Copyright (c) 2022 DigitalChickenLabs
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
package com.qualcomm.hardware.digitalchickenlabs;
public interface OctoQuad extends OctoQuadBase
{
/**
* Class to represent an OctoQuad firmware version
*/
class FirmwareVersion
{
public final int maj;
public final int min;
public final int eng;
public FirmwareVersion(int maj, int min, int eng)
{
this.maj = maj;
this.min = min;
this.eng = eng;
}
@Override
public String toString()
{
return String.format("%d.%d.%d", maj, min, eng);
}
}
/**
* Get the firmware version running on the OctoQuad
* @return the firmware version running on the OctoQuad
*/
FirmwareVersion getFirmwareVersion();
/**
* Read a single position from the OctoQuad
* Depending on the channel bank configuration, this may
* either be quadrature step count, or pulse width.
* @param idx the index of the encoder to read
* @return the position for the specified encoder
*/
int readSinglePosition(int idx);
/**
* Reads all positions from the OctoQuad, writing the data into
* an existing int[] object. The previous values are destroyed.
* Depending on the channel bank configuration, this may
* either be quadrature step count, or pulse width.
* @param out the int[] object to fill with new data
*/
void readAllPositions(int[] out);
/**
* Reads all positions from the OctoQuad
* Depending on the channel bank configuration, this may
* either be quadrature step count, or pulse width.
* @return an int[] object with the new data
*/
int[] readAllPositions();
/**
* Read a selected range of encoders
* Depending on the channel bank configuration, this may
* either be quadrature step count, or pulse width.
* @param idxFirst the first encoder (inclusive)
* @param idxLast the last encoder (inclusive)
* @return an array containing the requested encoder positions
*/
int[] readPositionRange(int idxFirst, int idxLast);
/*
* More Reset methods in Base interface
*/
/**
* Reset multiple encoders in the OctoQuad firmware in one command
* @param resets the encoders to be reset
*/
void resetMultiplePositions(boolean[] resets);
/**
* Reset multiple encoders in the OctoQuad firmware in one command
* @param indices the indices of the encoders to reset
*/
void resetMultiplePositions(int... indices);
/*
* More direction methods in Base interface
*/
/**
* Set the direction for all encoders
* This parameter will NOT be retained across power cycles, unless
* you call {@link #saveParametersToFlash()} ()}
* @param reverse 8-length direction array
*/
void setAllEncoderDirections(boolean[] reverse);
/**
* Read a single velocity from the OctoQuad
* NOTE: if using an absolute pulse width encoder, in order to get sane
* velocity data, you must set the channel min/max pulse width parameter.
* @param idx the index of the encoder to read
* @return the velocity for the specified encoder
*/
short readSingleVelocity(int idx);
/**
* Reads all velocities from the OctoQuad, writing the data into
* an existing short[] object. The previous values are destroyed.
* NOTE: if using an absolute pulse width encoder, in order to get sane
* velocity data, you must set the channel min/max pulse width parameter.
* @param out the short[] object to fill with new data
*/
void readAllVelocities(short[] out);
/**
* Reads all velocities from the OctoQuad
* NOTE: if using an absolute pulse width encoder, in order to get sane
* velocity data, you must set the channel min/max pulse width parameter.
* @return a short[] object with the new data
*/
short[] readAllVelocities();
/**
* Read a selected range of encoder velocities
* NOTE: if using an absolute pulse width encoder, in order to get sane
* velocity data, you must set the channel min/max pulse width parameter.
* @param idxFirst the first encoder (inclusive)
* @param idxLast the last encoder (inclusive)
* @return an array containing the requested velocities
*/
short[] readVelocityRange(int idxFirst, int idxLast);
class EncoderDataBlock
{
public int[] positions = new int[NUM_ENCODERS];
public short[] velocities = new short[NUM_ENCODERS];
}
/**
* Reads all encoder data from the OctoQuad, writing the data into
* an existing {@link EncoderDataBlock} object. The previous values are destroyed.
* @param out the {@link EncoderDataBlock} object to fill with new data
*/
void readAllEncoderData(EncoderDataBlock out);
/**
* Reads all encoder data from the OctoQuad
* @return a {@link EncoderDataBlock} object with the new data
*/
EncoderDataBlock readAllEncoderData();
/*
* More velocity sample interval methods in base interface
*/
/**
* Set the velocity sample intervals for all encoders
* This parameter will NOT be retained across power cycles, unless
* you call {@link #saveParametersToFlash()} ()}
* @param intvlms the sample intervals in milliseconds
*/
void setAllVelocitySampleIntervals(int[] intvlms);
/**
* Reads all velocity sample intervals from the OctoQuad
* @return all velocity sample intervals from the OctoQuad
*/
int[] getAllVelocitySampleIntervals();
class ChannelPulseWidthParams
{
public int min_length_us;
public int max_length_us;
public ChannelPulseWidthParams() {};
public ChannelPulseWidthParams(int min_length_us, int max_length_us)
{
this.min_length_us = min_length_us;
this.max_length_us = max_length_us;
}
}
/**
* Configure the minimum/maximum pulse width reported by an absolute encoder
* which is connected to a given channel, to allow the ability to provide
* accurate velocity data.
* These parameters will NOT be retained across power cycles, unless
* you call {@link #saveParametersToFlash()} ()}
* @param idx the channel in question
* @param params minimum/maximum pulse width
*/
void setSingleChannelPulseWidthParams(int idx, ChannelPulseWidthParams params);
/**
* Queries the OctoQuad to determine the currently set minimum/maxiumum pulse
* width for an encoder channel, to allow sane velocity data.
* @param idx the channel in question
* @return minimum/maximum pulse width
*/
ChannelPulseWidthParams getSingleChannelPulseWidthParams(int idx);
}

207
TeamCode/src/main/java/com/qualcomm/hardware/digitalchickenlabs/OctoQuadBase.java
View File

@ -1,207 +0,0 @@
/*
* Copyright (c) 2022 DigitalChickenLabs
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
package com.qualcomm.hardware.digitalchickenlabs;
public interface OctoQuadBase
{
byte OCTOQUAD_CHIP_ID = 0x51;
int SUPPORTED_FW_VERSION_MAJ = 2;
int ENCODER_FIRST = 0;
int ENCODER_LAST = 7;
int NUM_ENCODERS = 8;
int MIN_VELOCITY_MEASUREMENT_INTERVAL_MS = 1;
int MAX_VELOCITY_MEASUREMENT_INTERVAL_MS = 255;
int MIN_PULSE_WIDTH_LENGTH_µS = 1;
int MAX_PULSE_WIDTH_LENGTH_µS = 0xFFFF;
/**
* Reads the CHIP_ID register of the OctoQuad
* @return the value in the CHIP_ID register of the OctoQuad
*/
byte getChipId();
/**
* Get the firmware version running on the OctoQuad
* @return the firmware version running on the OctoQuad
*/
String getFirmwareVersionString();
/**
* Reset a single encoder in the OctoQuad firmware
* @param idx the index of the encoder to reset
*/
void resetSinglePosition(int idx);
/**
* Reset all encoder counts in the OctoQuad firmware
*/
void resetAllPositions();
enum EncoderDirection
{
FORWARD,
REVERSE
}
/**
* Set the direction for a single encoder
* This parameter will NOT be retained across power cycles, unless
* you call {@link #saveParametersToFlash()} ()}
* @param idx the index of the encoder
* @param dir direction
*/
void setSingleEncoderDirection(int idx, EncoderDirection dir);
/**
* Get the direction for a single encoder
* @param idx the index of the encoder
* @return direction of the encoder in question
*/
EncoderDirection getSingleEncoderDirection(int idx);
/**
* Set the velocity sample interval for a single encoder
* This parameter will NOT be retained across power cycles, unless
* you call {@link #saveParametersToFlash()} ()}
* @param idx the index of the encoder in question
* @param intvlms the sample interval in milliseconds
*/
void setSingleVelocitySampleInterval(int idx, int intvlms);
/**
* Set the velocity sample interval for all encoders
* This parameter will NOT be retained across power cycles, unless
* you call {@link #saveParametersToFlash()} ()}
* @param intvlms the sample interval in milliseconds
*/
void setAllVelocitySampleIntervals(int intvlms);
/**
* Read a single velocity sample interval
* @param idx the index of the encoder in question
* @return the velocity sample interval
*/
int getSingleVelocitySampleInterval(int idx);
/**
* Configure the minimum/maximum pulse width reported by an absolute encoder
* which is connected to a given channel, to allow the ability to provide
* accurate velocity data.
* These parameters will NOT be retained across power cycles, unless
* you call {@link #saveParametersToFlash()} ()}
* @param idx the channel in question
* @param min_length_us minimum pulse width
* @param max_length_us maximum pulse width
*/
void setSingleChannelPulseWidthParams(int idx, int min_length_us, int max_length_us);
/**
* Run the firmware's internal reset routine
*/
void resetEverything();
enum ChannelBankConfig
{
/**
* Both channel banks are configured for Quadrature input
*/
ALL_QUADRATURE(0),
/**
* Both channel banks are configured for pulse width input
*/
ALL_PULSE_WIDTH(1),
/**
* Bank 1 (channels 0-3) is configured for Quadrature input;
* Bank 2 (channels 4-7) is configured for pulse width input.
*/
BANK1_QUADRATURE_BANK2_PULSE_WIDTH(2);
public byte bVal;
ChannelBankConfig(int bVal)
{
this.bVal = (byte) bVal;
}
}
/**
* Configures the OctoQuad's channel banks
* This parameter will NOT be retained across power cycles, unless
* you call {@link #saveParametersToFlash()} ()}
* @param config the channel bank configuration to use
*/
void setChannelBankConfig(ChannelBankConfig config);
/**
* Queries the OctoQuad to determine the current channel bank configuration
* @return the current channel bank configuration
*/
ChannelBankConfig getChannelBankConfig();
enum I2cRecoveryMode
{
/**
* Does not perform any active attempts to recover a wedged I2C bus
*/
NONE(0),
/**
* The OctoQuad will reset its I2C peripheral if 50ms elapses between
* byte transmissions or between bytes and start/stop conditions
*/
MODE_1_PERIPH_RST_ON_FRAME_ERR(1),
/**
* Mode 1 actions + the OctoQuad will toggle the clock line briefly,
* once, after 1500ms of no communications.
*/
MODE_2_M1_PLUS_SCL_IDLE_ONESHOT_TGL(2);
public byte bVal;
I2cRecoveryMode(int bVal)
{
this.bVal = (byte) bVal;
}
}
/**
* Configures the OctoQuad to use the specified I2C recovery mode.
* This parameter will NOT be retained across power cycles, unless
* you call {@link #saveParametersToFlash()} ()}
* @param mode the recovery mode to use
*/
void setI2cRecoveryMode(I2cRecoveryMode mode);
/**
* Queries the OctoQuad to determine the currently configured I2C recovery mode
* @return the currently configured I2C recovery mode
*/
I2cRecoveryMode getI2cRecoveryMode();
/**
* Stores the current state of parameters to flash, to be applied at next boot
*/
void saveParametersToFlash();
}

804
TeamCode/src/main/java/com/qualcomm/hardware/digitalchickenlabs/OctoQuadImpl.java
View File

@ -1,804 +0,0 @@
/*
* Copyright (c) 2022 DigitalChickenLabs
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
package com.qualcomm.hardware.digitalchickenlabs;
import com.qualcomm.hardware.lynx.LynxI2cDeviceSynch;
import com.qualcomm.robotcore.hardware.I2cAddr;
import com.qualcomm.robotcore.hardware.I2cDeviceSynch;
import com.qualcomm.robotcore.hardware.I2cDeviceSynchDevice;
import com.qualcomm.robotcore.hardware.configuration.annotations.DeviceProperties;
import com.qualcomm.robotcore.hardware.configuration.annotations.I2cDeviceType;
import com.qualcomm.robotcore.util.Range;
import com.qualcomm.robotcore.util.RobotLog;
import java.nio.ByteBuffer;
import java.nio.ByteOrder;
import java.util.Arrays;
@I2cDeviceType
@DeviceProperties(xmlTag = "OctoQuadFTC", name = "OctoQuadFTC")
public class OctoQuadImpl extends I2cDeviceSynchDevice<I2cDeviceSynch> implements OctoQuad, CachingOctoQuad
{
private static final int I2C_ADDRESS = 0x30;
private static final ByteOrder OCTOQUAD_ENDIAN = ByteOrder.LITTLE_ENDIAN;
private static final byte CMD_SET_PARAM = 1;
private static final byte CMD_READ_PARAM = 2;
private static final byte CMD_WRITE_PARAMS_TO_FLASH = 3;
private static final byte CMD_RESET_EVERYTHING = 20;
private static final byte CMD_RESET_ENCODERS = 21;
private static final byte PARAM_ENCODER_DIRECTIONS = 0;
private static final byte PARAM_I2C_RECOVERY_MODE = 1;
private static final byte PARAM_CHANNEL_BANK_CONFIG = 2;
private static final byte PARAM_CHANNEL_VEL_INTVL = 3;
private static final byte PARAM_CHANNEL_PULSE_WIDTH_MIN_MAX = 4;
private byte directionRegisterData = 0;
private boolean isInitialized = false;
public class OctoQuadException extends RuntimeException
{
public OctoQuadException(String msg)
{
super(msg);
}
}
public OctoQuadImpl(I2cDeviceSynch deviceClient)
{
super(deviceClient, true);
this.deviceClient.setI2cAddress(I2cAddr.create7bit(I2C_ADDRESS));
super.registerArmingStateCallback(false);
this.deviceClient.engage();
}
@Override
protected boolean doInitialize()
{
//((LynxI2cDeviceSynch) deviceClient).setBusSpeed(LynxI2cDeviceSynch.BusSpeed.FAST_400K);
isInitialized = false;
verifyInitialization();
return true;
}
@Override
public Manufacturer getManufacturer()
{
return Manufacturer.Other;
}
@Override
public String getDeviceName()
{
return "OctoQuadFTC";
}
enum RegisterType
{
uint8_t(1),
int32_t(4),
int16_t(2);
public final int length;
RegisterType(int length)
{
this.length = length;
}
}
enum Register
{
CHIP_ID (0x00, RegisterType.uint8_t),
FIRMWARE_VERSION_MAJOR (0x01, RegisterType.uint8_t),
FIRMWARE_VERSION_MINOR (0x02, RegisterType.uint8_t),
FIRMWARE_VERSION_ENGINEERING (0x03, RegisterType.uint8_t),
COMMAND (0x04, RegisterType.uint8_t),
COMMAND_DAT_0 (0x05, RegisterType.uint8_t),
COMMAND_DAT_1 (0x06, RegisterType.uint8_t),
COMMAND_DAT_2 (0x07, RegisterType.uint8_t),
COMMAND_DAT_3 (0x08, RegisterType.uint8_t),
COMMAND_DAT_4 (0x09, RegisterType.uint8_t),
COMMAND_DAT_5 (0x0A, RegisterType.uint8_t),
COMMAND_DAT_6 (0x0B, RegisterType.uint8_t),
ENCODER_0_POSITION (0x0C, RegisterType.int32_t),
ENCODER_1_POSITION (0x10, RegisterType.int32_t),
ENCODER_2_POSITION (0x14, RegisterType.int32_t),
ENCODER_3_POSITION (0x18, RegisterType.int32_t),
ENCODER_4_POSITION (0x1C, RegisterType.int32_t),
ENCODER_5_POSITION (0x20, RegisterType.int32_t),
ENCODER_6_POSITION (0x24, RegisterType.int32_t),
ENCODER_7_POSITION (0x28, RegisterType.int32_t),
ENCODER_0_VELOCITY (0x2C, RegisterType.int16_t),
ENCODER_1_VELOCITY (0x2E, RegisterType.int16_t),
ENCODER_2_VELOCITY (0x30, RegisterType.int16_t),
ENCODER_3_VELOCITY (0x32, RegisterType.int16_t),
ENCODER_4_VELOCITY (0x34, RegisterType.int16_t),
ENCODER_5_VELOCITY (0x36, RegisterType.int16_t),
ENCODER_6_VELOCITY (0x38, RegisterType.int16_t),
ENCODER_7_VELOCITY (0x3A, RegisterType.int16_t);
public final byte addr;
public final int length;
Register(int addr, RegisterType type)
{
this.addr = (byte) addr;
this.length = type.length;
}
public static final Register[] all = Register.values();
}
// --------------------------------------------------------------------------------------------------------------------------------
// PUBLIC OCTOQUAD API
//---------------------------------------------------------------------------------------------------------------------------------
public byte getChipId()
{
return readRegister(Register.CHIP_ID)[0];
}
public FirmwareVersion getFirmwareVersion()
{
byte[] fw = readContiguousRegisters(Register.FIRMWARE_VERSION_MAJOR, Register.FIRMWARE_VERSION_ENGINEERING);
int maj = fw[0] & 0xFF;
int min = fw[1] & 0xFF;
int eng = fw[2] & 0xFF;
return new FirmwareVersion(maj, min, eng);
}
public String getFirmwareVersionString()
{
return getFirmwareVersion().toString();
}
public int readSinglePosition(int idx)
{
verifyInitialization();
Range.throwIfRangeIsInvalid(idx, ENCODER_FIRST, ENCODER_LAST);
Register register = Register.all[Register.ENCODER_0_POSITION.ordinal()+idx];
return intFromBytes(readRegister(register));
}
public void readAllPositions(int[] out)
{
verifyInitialization();
if(out.length != NUM_ENCODERS)
{
throw new IllegalArgumentException("out.length != 8");
}
byte[] bytes = readContiguousRegisters(Register.ENCODER_0_POSITION, Register.ENCODER_7_POSITION);
ByteBuffer buffer = ByteBuffer.wrap(bytes);
buffer.order(OCTOQUAD_ENDIAN);
for(int i = 0; i < NUM_ENCODERS; i++)
{
out[i] = buffer.getInt();
}
}
public int[] readAllPositions()
{
verifyInitialization();
int[] block = new int[NUM_ENCODERS];
readAllPositions(block);
return block;
}
public int[] readPositionRange(int idxFirst, int idxLast)
{
verifyInitialization();
Range.throwIfRangeIsInvalid(idxFirst, ENCODER_FIRST, ENCODER_LAST);
Range.throwIfRangeIsInvalid(idxLast, ENCODER_FIRST, ENCODER_LAST);
Register registerFirst = Register.all[Register.ENCODER_0_POSITION.ordinal()+idxFirst];
Register registerLast = Register.all[Register.ENCODER_0_POSITION.ordinal()+idxLast];
byte[] data = readContiguousRegisters(registerFirst, registerLast);
ByteBuffer buffer = ByteBuffer.wrap(data);
buffer.order(ByteOrder.LITTLE_ENDIAN);
int numEncodersRead = idxLast-idxFirst+1;
int[] encoderCounts = new int[numEncodersRead];
for(int i = 0; i < numEncodersRead; i++)
{
encoderCounts[i] = buffer.getInt();
}
return encoderCounts;
}
// ALSO USED BY CACHING API !
public void resetSinglePosition(int idx)
{
verifyInitialization();
Range.throwIfRangeIsInvalid(idx, ENCODER_FIRST, ENCODER_LAST);
byte dat = (byte) (1 << idx);
writeContiguousRegisters(Register.COMMAND, Register.COMMAND_DAT_0, new byte[]{CMD_RESET_ENCODERS, dat});
if (cachingMode == CachingMode.AUTO)
{
refreshCache();
}
}
// ALSO USED BY CACHING API !
public void resetAllPositions()
{
verifyInitialization();
writeContiguousRegisters(Register.COMMAND, Register.COMMAND_DAT_0, new byte[] {CMD_RESET_ENCODERS, (byte)0xFF});
if (cachingMode == CachingMode.AUTO)
{
refreshCache();
}
}
public void resetMultiplePositions(boolean[] resets)
{
verifyInitialization();
if(resets.length != NUM_ENCODERS)
{
throw new IllegalArgumentException("resets.length != 8");
}
byte dat = 0;
for(int i = ENCODER_FIRST; i <= ENCODER_LAST; i++)
{
dat |= resets[i] ? (byte)(1 << i) : 0;
}
writeContiguousRegisters(Register.COMMAND, Register.COMMAND_DAT_0, new byte[] {CMD_RESET_ENCODERS, dat});
}
public void resetMultiplePositions(int... indices)
{
verifyInitialization();
for(int idx : indices)
{
Range.throwIfRangeIsInvalid(idx, ENCODER_FIRST, ENCODER_LAST);
}
byte dat = 0;
for(int idx : indices)
{
dat |= 1 << idx;
}
writeContiguousRegisters(Register.COMMAND, Register.COMMAND_DAT_0, new byte[] {CMD_RESET_ENCODERS, dat});
}
public void setSingleEncoderDirection(int idx, EncoderDirection direction)
{
verifyInitialization();
Range.throwIfRangeIsInvalid(idx, ENCODER_FIRST, ENCODER_LAST);
if(direction == EncoderDirection.REVERSE)
{
directionRegisterData |= (byte) (1 << idx);
}
else
{
directionRegisterData &= (byte) ~(1 << idx);
}
writeContiguousRegisters(Register.COMMAND, Register.COMMAND_DAT_1, new byte[]{CMD_SET_PARAM, PARAM_ENCODER_DIRECTIONS, directionRegisterData});
}
public EncoderDirection getSingleEncoderDirection(int idx)
{
verifyInitialization();
Range.throwIfRangeIsInvalid(idx, ENCODER_FIRST, ENCODER_LAST);
writeContiguousRegisters(Register.COMMAND, Register.COMMAND_DAT_0, new byte[]{CMD_READ_PARAM, PARAM_ENCODER_DIRECTIONS});
byte directions = readRegister(Register.COMMAND_DAT_0)[0];
boolean reversed = (directions & (1 << idx)) != 0;
return reversed ? EncoderDirection.REVERSE : EncoderDirection.FORWARD;
}
public void setAllEncoderDirections(boolean[] reverse)
{
verifyInitialization();
if(reverse.length != NUM_ENCODERS)
{
throw new IllegalArgumentException("reverse.length != 8");
}
directionRegisterData = 0;
for(int i = ENCODER_FIRST; i <= ENCODER_LAST; i++)
{
if(reverse[i])
{
directionRegisterData |= (byte) (1 << i);
}
}
writeContiguousRegisters(Register.COMMAND, Register.COMMAND_DAT_1, new byte[]{CMD_SET_PARAM, PARAM_ENCODER_DIRECTIONS, directionRegisterData});
}
public short readSingleVelocity(int idx)
{
verifyInitialization();
Range.throwIfRangeIsInvalid(idx, ENCODER_FIRST, ENCODER_LAST);
Register register = Register.all[Register.ENCODER_0_VELOCITY.ordinal()+idx];
return shortFromBytes(readRegister(register));
}
public void readAllVelocities(short[] out)
{
verifyInitialization();
if(out.length != NUM_ENCODERS)
{
throw new IllegalArgumentException("out.length != 8");
}
byte[] bytes = readContiguousRegisters(Register.ENCODER_0_VELOCITY, Register.ENCODER_7_VELOCITY);
ByteBuffer buffer = ByteBuffer.wrap(bytes);
buffer.order(OCTOQUAD_ENDIAN);
for(int i = 0; i < NUM_ENCODERS; i++)
{
out[i] = buffer.getShort();
}
}
public short[] readAllVelocities()
{
verifyInitialization();
short[] block = new short[NUM_ENCODERS];
readAllVelocities(block);
return block;
}
public short[] readVelocityRange(int idxFirst, int idxLast)
{
verifyInitialization();
Range.throwIfRangeIsInvalid(idxFirst, ENCODER_FIRST, ENCODER_LAST);
Range.throwIfRangeIsInvalid(idxLast, ENCODER_FIRST, ENCODER_LAST);
Register registerFirst = Register.all[Register.ENCODER_0_VELOCITY.ordinal()+idxFirst];
Register registerLast = Register.all[Register.ENCODER_0_VELOCITY.ordinal()+idxLast];
byte[] data = readContiguousRegisters(registerFirst, registerLast);
ByteBuffer buffer = ByteBuffer.wrap(data);
buffer.order(ByteOrder.LITTLE_ENDIAN);
int numVelocitiesRead = idxLast-idxFirst+1;
short[] velocities = new short[numVelocitiesRead];
for(int i = 0; i < numVelocitiesRead; i++)
{
velocities[i] = buffer.getShort();
}
return velocities;
}
public void readAllEncoderData(EncoderDataBlock out)
{
verifyInitialization();
if(out.positions.length != NUM_ENCODERS)
{
throw new IllegalArgumentException("out.counts.length != 8");
}
if(out.velocities.length != NUM_ENCODERS)
{
throw new IllegalArgumentException("out.velocities.length != 8");
}
byte[] bytes = readContiguousRegisters(Register.ENCODER_0_POSITION, Register.ENCODER_7_VELOCITY);
ByteBuffer buffer = ByteBuffer.wrap(bytes);
buffer.order(OCTOQUAD_ENDIAN);
for(int i = 0; i < NUM_ENCODERS; i++)
{
out.positions[i] = buffer.getInt();
}
for(int i = 0; i < NUM_ENCODERS; i++)
{
out.velocities[i] = buffer.getShort();
}
}
public EncoderDataBlock readAllEncoderData()
{
verifyInitialization();
EncoderDataBlock block = new EncoderDataBlock();
readAllEncoderData(block);
return block;
}
public void setSingleVelocitySampleInterval(int idx, int intvlms)
{
verifyInitialization();
Range.throwIfRangeIsInvalid(idx, ENCODER_FIRST, ENCODER_LAST);
Range.throwIfRangeIsInvalid(intvlms, MIN_VELOCITY_MEASUREMENT_INTERVAL_MS, MAX_VELOCITY_MEASUREMENT_INTERVAL_MS);
writeContiguousRegisters(Register.COMMAND, Register.COMMAND_DAT_2, new byte[]{CMD_SET_PARAM, PARAM_CHANNEL_VEL_INTVL, (byte)idx, (byte)intvlms});
}
public void setAllVelocitySampleIntervals(int intvlms)
{
verifyInitialization();
Range.throwIfRangeIsInvalid(intvlms, MIN_VELOCITY_MEASUREMENT_INTERVAL_MS, MAX_VELOCITY_MEASUREMENT_INTERVAL_MS);
for(int i = ENCODER_FIRST; i <= ENCODER_LAST; i++)
{
writeContiguousRegisters(Register.COMMAND, Register.COMMAND_DAT_2, new byte[]{CMD_SET_PARAM, PARAM_CHANNEL_VEL_INTVL, (byte)i, (byte)intvlms});
}
}
public void setAllVelocitySampleIntervals(int[] intvlms)
{
verifyInitialization();
if(intvlms.length != NUM_ENCODERS)
{
throw new IllegalArgumentException("intvls.length != 8");
}
for(int i : intvlms)
{
Range.throwIfRangeIsInvalid(i, MIN_VELOCITY_MEASUREMENT_INTERVAL_MS, MAX_VELOCITY_MEASUREMENT_INTERVAL_MS);
}
for(int i = ENCODER_FIRST; i <= ENCODER_LAST; i++)
{
writeContiguousRegisters(Register.COMMAND, Register.COMMAND_DAT_2, new byte[]{CMD_SET_PARAM, PARAM_CHANNEL_VEL_INTVL, (byte)i, (byte)intvlms[i]});
}
}
public int getSingleVelocitySampleInterval(int idx)
{
verifyInitialization();
Range.throwIfRangeIsInvalid(idx, ENCODER_FIRST, ENCODER_LAST);
writeContiguousRegisters(Register.COMMAND, Register.COMMAND_DAT_1, new byte[]{CMD_READ_PARAM, PARAM_CHANNEL_VEL_INTVL, (byte)idx});
byte ms = readRegister(Register.COMMAND_DAT_0)[0];
return ms & 0xFF;
}
public int[] getAllVelocitySampleIntervals()
{
verifyInitialization();
int[] ret = new int[NUM_ENCODERS];
for(int i = ENCODER_FIRST; i <= ENCODER_FIRST; i++)
{
writeContiguousRegisters(Register.COMMAND, Register.COMMAND_DAT_1, new byte[]{CMD_READ_PARAM, PARAM_CHANNEL_VEL_INTVL, (byte)i});
byte ms = readRegister(Register.COMMAND_DAT_0)[0];
ret[i] = ms & 0xFF;
}
return ret;
}
public void setSingleChannelPulseWidthParams(int idx, int min, int max)
{
setSingleChannelPulseWidthParams(idx, new ChannelPulseWidthParams(min, max));
}
public void setSingleChannelPulseWidthParams(int idx, ChannelPulseWidthParams params)
{
verifyInitialization();
Range.throwIfRangeIsInvalid(idx, ENCODER_FIRST, ENCODER_LAST);
Range.throwIfRangeIsInvalid(params.min_length_us, MIN_PULSE_WIDTH_LENGTH_µS, MAX_PULSE_WIDTH_LENGTH_µS);
Range.throwIfRangeIsInvalid(params.max_length_us, MIN_PULSE_WIDTH_LENGTH_µS, MAX_PULSE_WIDTH_LENGTH_µS);
if(params.max_length_us <= params.min_length_us)
{
throw new RuntimeException("params.max_length_us <= params.min_length_us");
}
ByteBuffer outgoing = ByteBuffer.allocate(7);
outgoing.order(OCTOQUAD_ENDIAN);
outgoing.put(CMD_SET_PARAM);
outgoing.put(PARAM_CHANNEL_PULSE_WIDTH_MIN_MAX);
outgoing.put((byte)idx);
outgoing.putShort((short)params.min_length_us);
outgoing.putShort((short)params.max_length_us);
writeContiguousRegisters(Register.COMMAND, Register.COMMAND_DAT_5, outgoing.array());
}
public ChannelPulseWidthParams getSingleChannelPulseWidthParams(int idx)
{
verifyInitialization();
Range.throwIfRangeIsInvalid(idx, ENCODER_FIRST, ENCODER_LAST);
writeContiguousRegisters(Register.COMMAND, Register.COMMAND_DAT_1, new byte[]{CMD_READ_PARAM, PARAM_CHANNEL_PULSE_WIDTH_MIN_MAX, (byte)idx});
byte[] result = readContiguousRegisters(Register.COMMAND_DAT_0, Register.COMMAND_DAT_3);
ByteBuffer buffer = ByteBuffer.wrap(result);
buffer.order(OCTOQUAD_ENDIAN);
ChannelPulseWidthParams params = new ChannelPulseWidthParams();
params.min_length_us = buffer.getShort() & 0xFFFF;
params.max_length_us = buffer.getShort() & 0xFFFF;
return params;
}
public void resetEverything()
{
verifyInitialization();
writeRegister(Register.COMMAND, new byte[]{CMD_RESET_EVERYTHING});
}
public void setChannelBankConfig(ChannelBankConfig config)
{
verifyInitialization();
writeContiguousRegisters(Register.COMMAND, Register.COMMAND_DAT_1, new byte[]{CMD_SET_PARAM, PARAM_CHANNEL_BANK_CONFIG, config.bVal});
}
public ChannelBankConfig getChannelBankConfig()
{
verifyInitialization();
writeContiguousRegisters(Register.COMMAND, Register.COMMAND_DAT_0, new byte[]{CMD_READ_PARAM, PARAM_CHANNEL_BANK_CONFIG});
byte result = readRegister(Register.COMMAND_DAT_0)[0];
for(ChannelBankConfig c : ChannelBankConfig.values())
{
if(c.bVal == result)
{
return c;
}
}
return ChannelBankConfig.ALL_QUADRATURE;
}
public void setI2cRecoveryMode(I2cRecoveryMode mode)
{
verifyInitialization();
writeContiguousRegisters(Register.COMMAND, Register.COMMAND_DAT_1, new byte[]{CMD_SET_PARAM, PARAM_I2C_RECOVERY_MODE, mode.bVal});
}
public I2cRecoveryMode getI2cRecoveryMode()
{
verifyInitialization();
writeContiguousRegisters(Register.COMMAND, Register.COMMAND_DAT_0, new byte[]{CMD_READ_PARAM, PARAM_I2C_RECOVERY_MODE});
byte result = readRegister(Register.COMMAND_DAT_0)[0];
for(I2cRecoveryMode m : I2cRecoveryMode.values())
{
if(m.bVal == result)
{
return m;
}
}
return I2cRecoveryMode.NONE;
}
public void saveParametersToFlash()
{
verifyInitialization();
writeRegister(Register.COMMAND, new byte[] {CMD_WRITE_PARAMS_TO_FLASH});
try
{
Thread.sleep(100);
}
catch (InterruptedException e)
{
Thread.currentThread().interrupt();
e.printStackTrace();
}
}
// --------------------------------------------------------------------------------------------------------------------------------
// PUBLIC CACHING OCTOQUAD API
//---------------------------------------------------------------------------------------------------------------------------------
protected CachingMode cachingMode = CachingMode.AUTO;
protected EncoderDataBlock cachedData = new EncoderDataBlock();
protected boolean[] posHasBeenRead = new boolean[NUM_ENCODERS];
protected boolean[] velHasBeenRead = new boolean[NUM_ENCODERS];
public void setCachingMode(CachingMode mode)
{
this.cachingMode = mode;
if (cachingMode != CachingMode.NONE)
{
refreshCache();
}
}
public void refreshCache()
{
readAllEncoderData(cachedData);
Arrays.fill(posHasBeenRead, false);
Arrays.fill(velHasBeenRead, false);
}
public int readSinglePosition_Caching(int idx)
{
// If we're caching we're gonna want to read from the cache
if (cachingMode == CachingMode.AUTO || cachingMode == CachingMode.MANUAL)
{
// Update cache if this is the 2nd read
if (cachingMode == CachingMode.AUTO && posHasBeenRead[idx])
{
refreshCache();
}
posHasBeenRead[idx] = true;
return cachedData.positions[idx];
}
// Not caching; read direct
else
{
return readSinglePosition(idx);
}
}
public short readSingleVelocity_Caching(int idx)
{
// If we're caching we're gonna want to read from the cache
if (cachingMode == CachingMode.AUTO || cachingMode == CachingMode.MANUAL)
{
// Update cache if this is the 2nd read
if (cachingMode == CachingMode.AUTO && velHasBeenRead[idx])
{
refreshCache();
}
velHasBeenRead[idx] = true;
return cachedData.velocities[idx];
}
// Not caching; read direct
else
{
return readSingleVelocity(idx);
}
}
// --------------------------------------------------------------------------------------------------------------------------------
// INTERNAL
//---------------------------------------------------------------------------------------------------------------------------------
private void verifyInitialization()
{
if(!isInitialized)
{
byte chipId = getChipId();
if(chipId != OCTOQUAD_CHIP_ID)
{
RobotLog.addGlobalWarningMessage("OctoQuad does not report correct CHIP_ID value; (got 0x%X; expected 0x%X) this likely indicates I2C comms are not working", chipId, OCTOQUAD_CHIP_ID);
}
FirmwareVersion fw = getFirmwareVersion();
if(fw.maj != SUPPORTED_FW_VERSION_MAJ)
{
RobotLog.addGlobalWarningMessage("OctoQuad is running a different major firmware version than this driver was built for (current=%d; expected=%d) IT IS HIGHLY LIKELY THAT NOTHING WILL WORK!", fw.maj, SUPPORTED_FW_VERSION_MAJ);
}
isInitialized = true;
}
}
private static int intFromBytes(byte[] bytes)
{
ByteBuffer byteBuffer = ByteBuffer.wrap(bytes);
byteBuffer.order(OCTOQUAD_ENDIAN);
return byteBuffer.getInt();
}
private static short shortFromBytes(byte[] bytes)
{
ByteBuffer byteBuffer = ByteBuffer.wrap(bytes);
byteBuffer.order(OCTOQUAD_ENDIAN);
return byteBuffer.getShort();
}
private byte[] readRegister(Register reg)
{
return deviceClient.read(reg.addr, reg.length);
}
private byte[] readContiguousRegisters(Register first, Register last)
{
int addrStart = first.addr;
int addrEnd = last.addr + last.length;
int bytesToRead = addrEnd-addrStart;
return deviceClient.read(addrStart, bytesToRead);
}
private void writeRegister(Register reg, byte[] bytes)
{
if(reg.length != bytes.length)
{
throw new IllegalArgumentException("reg.length != bytes.length");
}
deviceClient.write(reg.addr, bytes);
}
private void writeContiguousRegisters(Register first, Register last, byte[] dat)
{
int addrStart = first.addr;
int addrEnd = last.addr + last.length;
int bytesToWrite = addrEnd-addrStart;
if(bytesToWrite != dat.length)
{
throw new IllegalArgumentException("bytesToWrite != dat.length");
}
deviceClient.write(addrStart, dat);
}
}

2
TeamCode/src/main/java/ftclib

Submodule TeamCode/src/main/java/ftclib updated: a819be3b11...c067e94f67

8
TeamCode/src/main/java/teamcode/FtcAuto.java
View File

@ -26,10 +26,10 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import java.util.Locale; import java.util.Locale;
import ftclib.input.FtcChoiceMenu; import ftclib.driverio.FtcChoiceMenu;
import ftclib.input.FtcMatchInfo; import ftclib.driverio.FtcMatchInfo;
import ftclib.input.FtcMenu; import ftclib.driverio.FtcMenu;
import ftclib.input.FtcValueMenu; import ftclib.driverio.FtcValueMenu;
import ftclib.robotcore.FtcOpMode; import ftclib.robotcore.FtcOpMode;
import trclib.command.CmdPidDrive; import trclib.command.CmdPidDrive;
import trclib.command.CmdTimedDrive; import trclib.command.CmdTimedDrive;

4
TeamCode/src/main/java/teamcode/FtcTeleOp.java
View File

@ -26,11 +26,11 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import java.util.Locale; import java.util.Locale;
import ftclib.input.FtcGamepad; import ftclib.driverio.FtcGamepad;
import ftclib.robotcore.FtcOpMode; import ftclib.robotcore.FtcOpMode;
import teamcode.drivebases.SwerveDrive; import teamcode.drivebases.SwerveDrive;
import trclib.drivebase.TrcDriveBase; import trclib.drivebase.TrcDriveBase;
import trclib.input.TrcGameController; import trclib.driverio.TrcGameController;
import trclib.pathdrive.TrcPose2D; import trclib.pathdrive.TrcPose2D;
import trclib.robotcore.TrcDbgTrace; import trclib.robotcore.TrcDbgTrace;
import trclib.robotcore.TrcRobot; import trclib.robotcore.TrcRobot;

10
TeamCode/src/main/java/teamcode/FtcTest.java
View File

@ -29,10 +29,10 @@ import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
import java.util.Arrays; import java.util.Arrays;
import java.util.Locale; import java.util.Locale;
import ftclib.input.FtcChoiceMenu; import ftclib.driverio.FtcChoiceMenu;
import ftclib.input.FtcGamepad; import ftclib.driverio.FtcGamepad;
import ftclib.input.FtcMenu; import ftclib.driverio.FtcMenu;
import ftclib.input.FtcValueMenu; import ftclib.driverio.FtcValueMenu;
import ftclib.robotcore.FtcPidCoeffCache; import ftclib.robotcore.FtcPidCoeffCache;
import teamcode.drivebases.RobotDrive; import teamcode.drivebases.RobotDrive;
import teamcode.drivebases.SwerveDrive; import teamcode.drivebases.SwerveDrive;
@ -40,7 +40,7 @@ import trclib.command.CmdDriveMotorsTest;
import trclib.command.CmdPidDrive; import trclib.command.CmdPidDrive;
import trclib.command.CmdTimedDrive; import trclib.command.CmdTimedDrive;
import trclib.dataprocessor.TrcUtil; import trclib.dataprocessor.TrcUtil;
import trclib.input.TrcGameController; import trclib.driverio.TrcGameController;
import trclib.pathdrive.TrcPose2D; import trclib.pathdrive.TrcPose2D;
import trclib.robotcore.TrcDbgTrace; import trclib.robotcore.TrcDbgTrace;
import trclib.robotcore.TrcPidController; import trclib.robotcore.TrcPidController;

4
TeamCode/src/main/java/teamcode/Robot.java
View File

@ -22,8 +22,8 @@
package teamcode; package teamcode;
import ftclib.input.FtcMatchInfo; import ftclib.driverio.FtcDashboard;
import ftclib.output.FtcDashboard; import ftclib.driverio.FtcMatchInfo;
import ftclib.robotcore.FtcOpMode; import ftclib.robotcore.FtcOpMode;
import ftclib.sensor.FtcRobotBattery; import ftclib.sensor.FtcRobotBattery;
import teamcode.drivebases.MecanumDrive; import teamcode.drivebases.MecanumDrive;

2
TeamCode/src/main/java/teamcode/RobotParams.java
View File

@ -28,7 +28,7 @@ import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
import org.openftc.easyopencv.OpenCvCameraRotation; import org.openftc.easyopencv.OpenCvCameraRotation;
import ftclib.input.FtcGamepad; import ftclib.driverio.FtcGamepad;
import trclib.dataprocessor.TrcUtil; import trclib.dataprocessor.TrcUtil;
import trclib.drivebase.TrcDriveBase.DriveOrientation; import trclib.drivebase.TrcDriveBase.DriveOrientation;
import trclib.pathdrive.TrcPose2D; import trclib.pathdrive.TrcPose2D;

4
TeamCode/src/main/java/teamcode/subsystems/BlinkinLEDs.java
View File

@ -22,9 +22,9 @@
package teamcode.subsystems; package teamcode.subsystems;
import ftclib.output.FtcRevBlinkin; import ftclib.driverio.FtcRevBlinkin;
import trclib.drivebase.TrcDriveBase; import trclib.drivebase.TrcDriveBase;
import trclib.output.TrcRevBlinkin; import trclib.driverio.TrcRevBlinkin;
/** /**
* This class encapsulates the REV Blinkin LED controller to provide a priority indicator showing the status of the * This class encapsulates the REV Blinkin LED controller to provide a priority indicator showing the status of the

2
TeamCode/src/main/java/trclib

Submodule TeamCode/src/main/java/trclib updated: 07ca09c374...e7e2b46faf

18
build.dependencies.gradle
View File

@ -4,15 +4,15 @@ repositories {
} }
dependencies { dependencies {
implementation 'org.firstinspires.ftc:Inspection:9.1.0' implementation 'org.firstinspires.ftc:Inspection:9.2.0'
implementation 'org.firstinspires.ftc:Blocks:9.1.0' implementation 'org.firstinspires.ftc:Blocks:9.2.0'
implementation 'org.firstinspires.ftc:Tfod:9.1.0' implementation 'org.firstinspires.ftc:Tfod:9.2.0'
implementation 'org.firstinspires.ftc:RobotCore:9.1.0' implementation 'org.firstinspires.ftc:RobotCore:9.2.0'
implementation 'org.firstinspires.ftc:RobotServer:9.1.0' implementation 'org.firstinspires.ftc:RobotServer:9.2.0'
implementation 'org.firstinspires.ftc:OnBotJava:9.1.0' implementation 'org.firstinspires.ftc:OnBotJava:9.2.0'
implementation 'org.firstinspires.ftc:Hardware:9.1.0' implementation 'org.firstinspires.ftc:Hardware:9.2.0'
implementation 'org.firstinspires.ftc:FtcCommon:9.1.0' implementation 'org.firstinspires.ftc:FtcCommon:9.2.0'
implementation 'org.firstinspires.ftc:Vision:9.1.0' implementation 'org.firstinspires.ftc:Vision:9.2.0'
implementation 'org.firstinspires.ftc:gameAssets-CenterStage:1.0.0' implementation 'org.firstinspires.ftc:gameAssets-CenterStage:1.0.0'
implementation 'org.tensorflow:tensorflow-lite-task-vision:0.4.3' implementation 'org.tensorflow:tensorflow-lite-task-vision:0.4.3'
runtimeOnly 'org.tensorflow:tensorflow-lite:2.12.0' runtimeOnly 'org.tensorflow:tensorflow-lite:2.12.0'

4
gradle.properties
View File

@ -3,8 +3,8 @@
# https://developer.android.com/topic/libraries/support-library/androidx-rn # https://developer.android.com/topic/libraries/support-library/androidx-rn
android.useAndroidX=true android.useAndroidX=true
# Automatically convert third-party libraries to use AndroidX # We no longer need to auto-convert third-party libraries to use AndroidX, which slowed down the build
android.enableJetifier=true android.enableJetifier=false
# Allow Gradle to use up to 1 GB of RAM # Allow Gradle to use up to 1 GB of RAM
org.gradle.jvmargs=-Xmx1024M org.gradle.jvmargs=-Xmx1024M