From ab39f1c5eb53af2e492a1be10a985653d70378cf Mon Sep 17 00:00:00 2001 From: j5155 <54331556+j5155@users.noreply.github.com> Date: Sun, 30 Jun 2024 22:49:52 -0800 Subject: [PATCH] Add tuners and docs --- README.md | 80 +++++++++- .../ftc/teamcode/OTOSOpMode.java | 151 ------------------ .../ftc/teamcode/RROtosTest.java | 50 ------ .../tuning/otos/OTOSAngularScalar.java | 32 ++++ .../tuning/otos/OTOSHeadingOffsetTuner.java | 28 ++++ .../tuning/otos/OTOSPositionOffsetTuner.java | 33 ++++ 6 files changed, 171 insertions(+), 203 deletions(-) delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OTOSOpMode.java delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RROtosTest.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/otos/OTOSAngularScalar.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/otos/OTOSHeadingOffsetTuner.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/otos/OTOSPositionOffsetTuner.java diff --git a/README.md b/README.md index 25b856010be0..58162f446941 100644 --- a/README.md +++ b/README.md @@ -1,4 +1,80 @@ -# Road Runner Quickstart +# SparkFun OTOS Quickstart for Roadrunner 1.0 + +The SparkFun OTOS or Optical Tracking Odometry Sensor is an optical-based odometry sensor with an integrated IMU. +This repository allows teams to integrate it into Roadrunner as a drop-in replacement. + +## Notes and Warnings +Ensure that your sensor is properly mounted 10mm above the ground using the directions on the product page. + +The OTOS sensor is designed to ONLY work on official field tiles. +Ensure that all tuning is performed on them. +(If you have nothing else to test on, it seems to also be able to track a hardwood floor as well. +However, tuning numbers will likely be different between them.) + +The custom localization is implemented using the SparkFunOTOSDrive class, which *extends* MecanumDrive. +This means that all of RoadRunner's standard tuning should remain in MecanumDrive, but you should use SparkFunOTOSDrive +in your OpModes. + +I eventually plan to PR this in some form once it's been more extensively tested. + +## When things go wrong… +This quickstart has not been extensively tested, and you are likely to encounter bugs and issues. +If this happens, or if there's anything you're confused about or don't understand, the best way to get help is +making a post in roadrunner-help on the FTC Discord with your MecanumDrive and SparkFunOTOSDrive attached and pinging me +(@j5155). +If you're certain that you've found a bug, +or you have a feature request, you may also make an issue in the Issues tab above. + +Do NOT make an issue on the official Roadrunner quickstart while you are using this one. + +## Tuning + +### Configure Hardware + +Configure your drive motors in MecanumDrive + as explained [here](https://rr.brott.dev/docs/v1-0/tuning/#drive-classes). +Make sure to properly reverse them using MecanumDirectionDebugger by following the official docs. + +Tuning currently also requires a properly configured Control Hub or Expansion Hub IMU in MecanumDrive. +This will be fixed in the future, but for now make sure your hub orientation is properly defined. + +Also, make sure to configure the OTOS in your hardware config. +By default, SparkFunOTOSDrive will look for a sensor named sensor_otos, +but you can change this in SparkFunOTOSDrive line 70. +### Tune Scalars and Offsets +First, tune the Angular Scalar by running the OTOSAngularScalar OpMode and following the instructions. +This will allow you to get the maximum accuracy from the OTOS IMU. + +After you have tuned the angular scalar, the IMU will be accurate enough to tune the heading offset. +Run OTOSHeadingOffsetTuner and follow the instructions. + +Next, tune the Linear Scalar using the LocalizationTest OpMode. +Again, ensure that you perform this tuning on field tiles so that the OTOS gives accurate data. +SparkFun's official instructions to do are as follows: +> 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 + +Set the Linear Scalar at line 56 of SparkFunOTOSDrive. + +Next, tune the Position Offset using OTOSPositionOffsetTuner by following the instructions. +This should ensure that the OTOS is properly aware of its location on the robot. + +As a final step to ensure everything is working properly, +run LocalizationTest again and try driving around, spinning in place, etc. +to make sure everything is working properly. + +### Begin Roadrunner Tuning +Follow the [official RoadRunner docs](https://rr.brott.dev/docs/v1-0/tuning/#forwardramplogger-dead-wheels-only) for the remaining tuning steps. +However, make sure to start at ForwardRampLogger and follow the steps labeled "dead wheels." + +Additionally, when you run AngularRampLogger, +you can ignore the odometry position graphs and only use the trackWidthTicks one. + +Once you have completed the official docs for tuning, you should be good to go to use Roadrunner as normal! + -Check out the [docs](https://rr.brott.dev/docs/v1-0/tuning/). diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OTOSOpMode.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OTOSOpMode.java deleted file mode 100644 index e927a271e2f0..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OTOSOpMode.java +++ /dev/null @@ -1,151 +0,0 @@ -/* - 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 deleted file mode 100644 index c1c979c2324f..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RROtosTest.java +++ /dev/null @@ -1,50 +0,0 @@ -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/otos/OTOSAngularScalar.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/otos/OTOSAngularScalar.java new file mode 100644 index 000000000000..19035296e526 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/otos/OTOSAngularScalar.java @@ -0,0 +1,32 @@ +package org.firstinspires.ftc.teamcode.tuning.otos; + +import com.acmerobotics.roadrunner.Pose2d; +import com.acmerobotics.roadrunner.Rotation2d; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import org.firstinspires.ftc.teamcode.SparkFunOTOSDrive; +@TeleOp +public class OTOSAngularScalar extends LinearOpMode { + + @Override + public void runOpMode() throws InterruptedException { + SparkFunOTOSDrive drive = new SparkFunOTOSDrive(hardwareMap, new Pose2d(0,0,0)); + double radsTurned = 0; + Rotation2d lastHeading = Rotation2d.fromDouble(0); + telemetry.addLine("OTOS Angular Scalar Tuner"); + telemetry.addLine("Press START, then rotate the robot on the ground 10 times (3600 degrees)."); + telemetry.addLine("Then copy the scalar into SparkFunOTOSDrive."); + telemetry.update(); + waitForStart(); + while (opModeIsActive()) { + radsTurned += drive.pose.heading.minus(lastHeading); + lastHeading = drive.pose.heading; + telemetry.addData("Uncorrected Degrees Turned", Math.toDegrees(radsTurned)); + telemetry.addData("Calculated Angular Scalar", 3600 / Math.toDegrees(radsTurned)); + telemetry.update(); + } + + + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/otos/OTOSHeadingOffsetTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/otos/OTOSHeadingOffsetTuner.java new file mode 100644 index 000000000000..ed0b405a7a96 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/otos/OTOSHeadingOffsetTuner.java @@ -0,0 +1,28 @@ +package org.firstinspires.ftc.teamcode.tuning.otos; + +import com.acmerobotics.roadrunner.Pose2d; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import org.firstinspires.ftc.teamcode.SparkFunOTOSDrive; +@TeleOp +public class OTOSHeadingOffsetTuner extends LinearOpMode { + + @Override + public void runOpMode() throws InterruptedException { + SparkFunOTOSDrive drive = new SparkFunOTOSDrive(hardwareMap, new Pose2d(0,0,0)); + telemetry.addLine("OTOS Heading Offset Tuner"); + telemetry.addLine("Line the side of the robot against a wall and Press START."); + telemetry.addLine("Then push the robot forward some distance."); + telemetry.addLine("Finally, copy the heading offset into line 38 of SparkFunOTOSDrive"); + telemetry.update(); + waitForStart(); + while (opModeIsActive()) { + telemetry.addData("Heading Offset (radians, enter this one into SparkFunOTOSDrive!)",Math.atan2(drive.pose.position.x,drive.pose.position.y)); + telemetry.addData("Heading Offset (degrees)",Math.toRadians(Math.atan2(drive.pose.position.x,drive.pose.position.y))); + telemetry.update(); + } + + + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/otos/OTOSPositionOffsetTuner.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/otos/OTOSPositionOffsetTuner.java new file mode 100644 index 000000000000..3e722da308ab --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/tuning/otos/OTOSPositionOffsetTuner.java @@ -0,0 +1,33 @@ +package org.firstinspires.ftc.teamcode.tuning.otos; + +import com.acmerobotics.roadrunner.Pose2d; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; + +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import org.firstinspires.ftc.teamcode.SparkFunOTOSDrive; +@TeleOp +public class OTOSPositionOffsetTuner extends LinearOpMode { + + @Override + public void runOpMode() throws InterruptedException { + SparkFunOTOSDrive drive = new SparkFunOTOSDrive(hardwareMap, new Pose2d(0,0,0)); + telemetry.addLine("OTOS Position Offset Tuner"); + telemetry.addLine("Line the robot against the corner of two walls facing forward and Press START."); + telemetry.addLine("Then rotate the robot exactly 180 degrees and press it back into the corner."); + telemetry.addLine("Finally, copy the pose offset into line 38 of SparkFunOTOSDrive."); + telemetry.update(); + waitForStart(); + while (opModeIsActive()) { + telemetry.addData("Heading (deg)",Math.toDegrees(drive.pose.heading.toDouble())); + if (Math.abs(Math.toDegrees(drive.pose.heading.toDouble())) > 175) { + telemetry.addData("X Offset", drive.pose.position.x / 2); + telemetry.addData("Y Offset", drive.pose.position.y / 2); + } else { + telemetry.addLine("Rotate the robot 180 degrees and align it to the corner again."); + } + telemetry.update(); + } + + + } +}