From 8663bfbe8723e841eb14e56856e14ae9e5439982 Mon Sep 17 00:00:00 2001 From: j5155 <54331556+j5155@users.noreply.github.com> Date: Sun, 23 Jun 2024 20:09:56 -0800 Subject: [PATCH] various fixes --- TeamCode/build.gradle | 2 +- .../ftc/teamcode/OTOSOpMode.java | 151 ++++++++++++++++++ .../ftc/teamcode/RROtosTest.java | 50 ++++++ .../teamcode/tuning/ManualFeedbackTuner.java | 15 +- .../ftc/teamcode/tuning/TuningOpModes.java | 4 +- 5 files changed, 218 insertions(+), 4 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OTOSOpMode.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RROtosTest.java diff --git a/TeamCode/build.gradle b/TeamCode/build.gradle index 900763eab666..7d0edb4624f2 100644 --- a/TeamCode/build.gradle +++ b/TeamCode/build.gradle @@ -36,7 +36,7 @@ dependencies { implementation project(':FtcRobotController') annotationProcessor files('lib/OpModeAnnotationProcessor.jar') - implementation 'com.github.jdhs-ftc:road-runner-ftc-otos:5f1b6c5a05' + implementation "com.github.jdhs-ftc:road-runner-ftc-otos:7c00759558" implementation "com.acmerobotics.roadrunner:core:1.0.0-beta8" implementation "com.acmerobotics.roadrunner:actions:1.0.0-beta8" implementation "com.acmerobotics.dashboard:dashboard:0.4.15" diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OTOSOpMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OTOSOpMode.java new file mode 100644 index 000000000000..e927a271e2f0 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OTOSOpMode.java @@ -0,0 +1,151 @@ +/* + SPDX-License-Identifier: MIT + + Copyright (c) 2024 SparkFun Electronics +*/ +package org.firstinspires.ftc.teamcode; + +import com.acmerobotics.roadrunner.ftc.SparkFunOTOS; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +/* + * 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 OTOSOpMode 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 + // stored in the sensor, it's part of the library, so you need to set at the + // start of all your programs. + // myOtos.setLinearUnit(SparkFunOTOS.LinearUnit.METERS); + myOtos.setLinearUnit(SparkFunOTOS.LinearUnit.INCHES); + // myOtos.setAngularUnit(SparkFunOTOS.AngularUnit.RADIANS); + myOtos.setAngularUnit(SparkFunOTOS.AngularUnit.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, 180); + 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 programs. 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(); + } +} \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RROtosTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RROtosTest.java new file mode 100644 index 000000000000..c1c979c2324f --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RROtosTest.java @@ -0,0 +1,50 @@ +package org.firstinspires.ftc.teamcode; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +import com.acmerobotics.dashboard.telemetry.TelemetryPacket; +import com.acmerobotics.roadrunner.Pose2d; +import com.acmerobotics.roadrunner.PoseVelocity2d; +import com.acmerobotics.roadrunner.Vector2d; +import com.acmerobotics.roadrunner.ftc.SparkFunOTOS; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +public class RROtosTest extends LinearOpMode { + @Override + public void runOpMode() throws InterruptedException { + telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); + + SparkFunOTOSDrive drive = new SparkFunOTOSDrive(hardwareMap, new Pose2d(0, 0, 0)); + + waitForStart(); + + while (opModeIsActive()) { + drive.setDrivePowers(new PoseVelocity2d( + new Vector2d( + -gamepad1.left_stick_y, + -gamepad1.left_stick_x + ), + -gamepad1.right_stick_x + )); + + drive.updatePoseEstimate(); + + telemetry.addData("x", drive.pose.position.x); + telemetry.addData("y", drive.pose.position.y); + telemetry.addData("heading (deg)", Math.toDegrees(drive.pose.heading.toDouble())); + + SparkFunOTOS.Pose2D oPose = drive.otos.getPosition(); + telemetry.addData("oX",oPose.x); + telemetry.addData("oY", oPose.y); + telemetry.addData("oH", oPose.h); + telemetry.addData("oH (deg)", Math.toDegrees(oPose.h)); + + telemetry.update(); + + TelemetryPacket packet = new TelemetryPacket(); + packet.fieldOverlay().setStroke("#3F51B5"); + Drawing.drawRobot(packet.fieldOverlay(), drive.pose); + FtcDashboard.getInstance().sendTelemetryPacket(packet); + } + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/ManualFeedbackTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/ManualFeedbackTuner.java index 0b6f1933e04d..ce1fc45228a5 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/ManualFeedbackTuner.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/ManualFeedbackTuner.java @@ -5,6 +5,7 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import org.firstinspires.ftc.teamcode.MecanumDrive; +import org.firstinspires.ftc.teamcode.SparkFunOTOSDrive; import org.firstinspires.ftc.teamcode.TankDrive; import org.firstinspires.ftc.teamcode.ThreeDeadWheelLocalizer; import org.firstinspires.ftc.teamcode.TwoDeadWheelLocalizer; @@ -14,7 +15,19 @@ public final class ManualFeedbackTuner extends LinearOpMode { @Override public void runOpMode() throws InterruptedException { - if (TuningOpModes.DRIVE_CLASS.equals(MecanumDrive.class)) { + if (TuningOpModes.DRIVE_CLASS.equals(SparkFunOTOSDrive.class)) { + SparkFunOTOSDrive drive = new SparkFunOTOSDrive(hardwareMap, new Pose2d(0, 0, 0)); + + waitForStart(); + + while (opModeIsActive()) { + Actions.runBlocking( + drive.actionBuilder(new Pose2d(0, 0, 0)) + .lineToX(DISTANCE) + .lineToX(0) + .build()); + } + } else if (TuningOpModes.DRIVE_CLASS.equals(MecanumDrive.class)) { MecanumDrive drive = new MecanumDrive(hardwareMap, new Pose2d(0, 0, 0)); if (drive.localizer instanceof TwoDeadWheelLocalizer) { diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TuningOpModes.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TuningOpModes.java index 15b9b2832396..67fbe6a4d476 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TuningOpModes.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/TuningOpModes.java @@ -60,8 +60,8 @@ public static void register(OpModeManager manager) { List leftEncs = new ArrayList<>(), rightEncs = new ArrayList<>(); List parEncs = new ArrayList<>(), perpEncs = new ArrayList<>(); - parEncs.add(new OtosEncoder(od.otos,true, od.leftBack)); - perpEncs.add(new OtosEncoder(od.otos,false, od.leftBack)); + parEncs.add(new OtosEncoder(od.otos,false, od.leftBack)); + perpEncs.add(new OtosEncoder(od.otos,true, od.leftBack)); return new DriveView( DriveType.MECANUM,