From b6f31bf9d2d684afa2aa2059064eb0c444d7375a Mon Sep 17 00:00:00 2001 From: j5155 <54331556+j5155@users.noreply.github.com> Date: Mon, 3 Jun 2024 23:30:25 -0800 Subject: [PATCH] update library and deal with manual pose setting --- TeamCode/build.gradle | 4 +- .../ftc/teamcode/SensorSparkFunOTOS.java | 151 ------------------ .../ftc/teamcode/SparkFunOTOSDrive.java | 14 ++ 3 files changed, 16 insertions(+), 153 deletions(-) delete mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SensorSparkFunOTOS.java diff --git a/TeamCode/build.gradle b/TeamCode/build.gradle index 13ecf4b651c4..e10397a043c9 100644 --- a/TeamCode/build.gradle +++ b/TeamCode/build.gradle @@ -35,8 +35,8 @@ repositories { dependencies { implementation project(':FtcRobotController') - implementation 'com.github.jdhs-ftc:road-runner-ftc-otos:master-SNAPSHOT' + implementation 'com.github.jdhs-ftc:road-runner-ftc-otos:5f1b6c5a05' 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.14" + implementation "com.acmerobotics.dashboard:dashboard:0.4.15" } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SensorSparkFunOTOS.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SensorSparkFunOTOS.java deleted file mode 100644 index 37e851d11a6e..000000000000 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SensorSparkFunOTOS.java +++ /dev/null @@ -1,151 +0,0 @@ -/* - SPDX-License-Identifier: MIT - - Copyright (c) 2024 SparkFun Electronics -*/ -package org.firstinspires.ftc.teamcode; - -import com.qualcomm.robotcore.eventloop.opmode.Disabled; -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 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 - // 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, 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 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/SparkFunOTOSDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SparkFunOTOSDrive.java index 87a7ecaed90e..7ec3147fd7e7 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SparkFunOTOSDrive.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SparkFunOTOSDrive.java @@ -3,6 +3,7 @@ import static com.acmerobotics.roadrunner.ftc.OTOSKt.OTOSPoseToRRPose; +import static com.acmerobotics.roadrunner.ftc.OTOSKt.RRPoseToOTOSPose; import com.acmerobotics.roadrunner.Pose2d; import com.acmerobotics.roadrunner.PoseVelocity2d; @@ -58,6 +59,7 @@ public static class Params { public static SparkFunOTOSDrive.Params PARAMS = new SparkFunOTOSDrive.Params(); public SparkFunOTOS otos; + private Pose2d lastOtosPose = pose; public SparkFunOTOSDrive(HardwareMap hardwareMap, Pose2d pose) { super(hardwareMap, pose); @@ -69,6 +71,8 @@ public SparkFunOTOSDrive(HardwareMap hardwareMap, Pose2d pose) { otos.setOffset(PARAMS.offset); otos.setLinearScalar(PARAMS.linearScalar); otos.setAngularScalar(PARAMS.angularScalar); + + otos.setPosition(RRPoseToOTOSPose(pose)); // 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 @@ -86,6 +90,16 @@ public SparkFunOTOSDrive(HardwareMap hardwareMap, Pose2d pose) { } @Override public PoseVelocity2d updatePoseEstimate() { + if (lastOtosPose != pose) { + // rr localizer note: + // something other then this function has modified pose + // probably the user + // so we override otos pose with the new pose + // this could potentially cause up to 1 loops worth of drift + // I don't really like this solution at all, but it preserves compatibility + // the only alternative is to add getter and setters but that breaks compat + otos.setPosition(RRPoseToOTOSPose(pose)); + } SparkFunOTOS.Pose2D otosPose = otos.getPosition(); SparkFunOTOS.Pose2D otosVel = otos.getVelocity(); pose = OTOSPoseToRRPose(otosPose);