From 7d6ccd90a8bde7ac75aad645a77464b2b63fa7d0 Mon Sep 17 00:00:00 2001 From: j5155 <54331556+j5155@users.noreply.github.com> Date: Thu, 19 Sep 2024 17:26:35 -0800 Subject: [PATCH] Update library, add initial draft of PinpointDrive and example class --- TeamCode/build.gradle | 2 +- .../ftc/teamcode/PinpointDrive.java | 122 +++++++++++ .../SensorGoBildaPinpointExample.java | 192 ++++++++++++++++++ 3 files changed, 315 insertions(+), 1 deletion(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PinpointDrive.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SensorGoBildaPinpointExample.java diff --git a/TeamCode/build.gradle b/TeamCode/build.gradle index 2ff491a8c852..479308666e8c 100644 --- a/TeamCode/build.gradle +++ b/TeamCode/build.gradle @@ -38,5 +38,5 @@ dependencies { implementation "com.acmerobotics.roadrunner:core:1.0.0" implementation "com.acmerobotics.roadrunner:actions:1.0.0" implementation "com.acmerobotics.dashboard:dashboard:0.4.16" - implementation "com.github.jdhs-ftc:road-runner-ftc-otos:a425f97d46" + implementation "com.github.jdhs-ftc:road-runner-ftc-otos:14c7f4ca9a" } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PinpointDrive.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PinpointDrive.java new file mode 100644 index 000000000000..7ad0fd2e1a8c --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PinpointDrive.java @@ -0,0 +1,122 @@ +package org.firstinspires.ftc.teamcode; + + + +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; +import com.acmerobotics.roadrunner.Vector2d; +import com.acmerobotics.roadrunner.ftc.FlightRecorder; +import com.acmerobotics.roadrunner.ftc.GoBildaPinpointDriver; +import com.qualcomm.hardware.sparkfun.SparkFunOTOS; +import com.qualcomm.robotcore.hardware.HardwareMap; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.teamcode.messages.PoseMessage; + +/** + * Experimental extension of MecanumDrive that uses the Gobilda Pinpoint sensor for localization. + *

+ * Released under the BSD 3-Clause Clear License by j5155 from 12087 Capital City Dynamics + * Portions of this code made and released under the MIT License by Gobilda (Base 10 Assets, LLC) + * Unless otherwise noted, comments are from Gobilda + */ +public class PinpointDrive extends MecanumDrive { + public static class Params { + /* + Set the odometry pod positions relative to the point that the odometry computer tracks around. + The X pod offset refers to how far sideways from the tracking point the + X (forward) odometry pod is. Left of the center is a positive number, + right of center is a negative number. the Y pod offset refers to how far forwards from + the tracking point the Y (strafe) odometry pod is. forward of center is a positive number, + backwards is a negative number. + */ + //these are tuned for 3110-0002-0001 Product Insight #1 + // RR localizer note: These units are inches, presets are converted from mm (which is why they are inexact) + public double xOffset = -3.3071; + public double yOffset = -6.6142; + + /* + Set the kind of pods used by your robot. If you're using goBILDA odometry pods, select either + the goBILDA_SWINGARM_POD, or the goBILDA_4_BAR_POD. + If you're using another kind of odometry pod, input the number of ticks per millimeter of that pod. + + RR LOCALIZER NOTE: this is ticks per MILLIMETER, NOT inches per tick. + This value should be more than one; the value for the Gobilda 4 Bar Pod is approximately 20. + To get this value from inPerTick, first convert the value to millimeters (multiply by 25.4) + and then take its inverse (one over the value) + */ + public double encoderResolution = GoBildaPinpointDriver.goBILDA_4_BAR_POD; + + /* + Set the direction that each of the two odometry pods count. The X (forward) pod should + increase when you move the robot forward. And the Y (strafe) pod should increase when + you move the robot to the left. + */ + public GoBildaPinpointDriver.EncoderDirection xDirection = GoBildaPinpointDriver.EncoderDirection.FORWARD; + public GoBildaPinpointDriver.EncoderDirection yDirection = GoBildaPinpointDriver.EncoderDirection.FORWARD; + } + + public static Params PARAMS = new Params(); + public GoBildaPinpointDriver pinpoint; + private Pose2d lastPinpointPose = pose; + + public PinpointDrive(HardwareMap hardwareMap, Pose2d pose) { + super(hardwareMap, pose); + pinpoint = hardwareMap.get(GoBildaPinpointDriver.class,"pinpoint"); + + // RR localizer note: don't love this conversion (change driver?) + pinpoint.setOffsets(DistanceUnit.MM.fromInches(PARAMS.xOffset), DistanceUnit.MM.fromInches(PARAMS.yOffset)); + + + pinpoint.setEncoderResolution(PARAMS.encoderResolution); + + /* + Before running the robot, recalibrate the IMU. This needs to happen when the robot is stationary + The IMU will automatically calibrate when first powered on, but recalibrating before running + the robot is a good idea to ensure that the calibration is "good". + resetPosAndIMU will reset the position to 0,0,0 and also recalibrate the IMU. + This is recommended before you run your autonomous, as a bad initial calibration can cause + an incorrect starting value for x, y, and heading. + */ + //pinpoint.recalibrateIMU(); + pinpoint.resetPosAndIMU(); + + pinpoint.setPosition(pose); + } + @Override + public PoseVelocity2d updatePoseEstimate() { + if (lastPinpointPose != pose) { + // RR localizer note: + // Something else is modifying our pose (likely for relocalization), + // so we override otos pose with the new pose. + // This could potentially cause up to 1 loop worth of drift. + // I don't like this solution at all, but it preserves compatibility. + // The only alternative is to add getter and setters, but that breaks compat. + // Potential alternate solution: timestamp the pose set and backtrack it based on speed? + pinpoint.setPosition(pose); + } + pinpoint.bulkUpdate(); // RR LOCALIZER NOTE: this is not ideal for loop times. + // Driver needs update to be optimized + pose = pinpoint.getPosition(); + lastPinpointPose = pose; + + // RR standard + poseHistory.add(pose); + while (poseHistory.size() > 100) { + poseHistory.removeFirst(); + } + + FlightRecorder.write("ESTIMATED_POSE", new PoseMessage(pose)); + + Pose2d velocity = pinpoint.getVelocity(); + // RR localizer note + // "velocity.position" is a little weird + // it's because velocity is stored as a roadrunner pose2d right now + // maybe not ideal for this, probably I should change it to use a PoseVelocity2d directly instead + return new PoseVelocity2d(new Vector2d(velocity.position.x, velocity.position.y),velocity.heading.toDouble()); + } + + +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SensorGoBildaPinpointExample.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SensorGoBildaPinpointExample.java new file mode 100644 index 000000000000..197e3f342713 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SensorGoBildaPinpointExample.java @@ -0,0 +1,192 @@ +/* MIT License + * Copyright (c) [2024] [Base 10 Assets, LLC] + * + * 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.teamcode; + +import com.acmerobotics.roadrunner.Pose2d; +import com.acmerobotics.roadrunner.ftc.GoBildaPinpointDriver; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.eventloop.opmode.TeleOp; + +import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; + +import java.util.Locale; + +/* +This opmode shows how to use the goBILDA® Pinpoint Odometry Computer. +The goBILDA Odometry Computer is a device designed to solve the Pose Exponential calculation +commonly associated with Dead Wheel Odometry systems. It reads two encoders, and an integrated +system of senors to determine the robot's current heading, X position, and Y position. + +it uses an ESP32-S3 as a main cpu, with an STM LSM6DSV16X IMU. +It is validated with goBILDA "Dead Wheel" Odometry pods, but should be compatible with any +quadrature rotary encoder. The ESP32 PCNT peripheral is speced to decode quadrature encoder signals +at a maximum of 40mhz per channel. Though the maximum in-application tested number is 130khz. + +The device expects two perpendicularly mounted Dead Wheel pods. The encoder pulses are translated +into mm and their readings are transformed by an "offset", this offset describes how far away +the pods are from the "tracking point", usually the center of rotation of the robot. + +Dead Wheel pods should both increase in count when moved forwards and to the left. +The gyro will report an increase in heading when rotated counterclockwise. + +The Pose Exponential algorithm used is described on pg 181 of this book: +https://github.com/calcmogul/controls-engineering-in-frc + +For support, contact tech@gobilda.com + +-Ethan Doak + */ + +@TeleOp(name="goBILDA® PinPoint Odometry Example", group="Linear OpMode") +//@Disabled +public class SensorGoBildaPinpointExample extends LinearOpMode { + + GoBildaPinpointDriver odo; // Declare OpMode member for the Odometry Computer + + double oldTime = 0; + + + @Override + public void runOpMode() { + + // Initialize the hardware variables. Note that the strings used here must correspond + // to the names assigned during the robot configuration step on the DS or RC devices. + + odo = hardwareMap.get(GoBildaPinpointDriver.class,"odo"); + + /* + Set the odometry pod positions relative to the point that the odometry computer tracks around. + The X pod offset refers to how far sideways from the tracking point the + X (forward) odometry pod is. Left of the center is a positive number, + right of center is a negative number. the Y pod offset refers to how far forwards from + the tracking point the Y (strafe) odometry pod is. forward of center is a positive number, + backwards is a negative number. + */ + odo.setOffsets(-84.0, -168.0); //these are tuned for 3110-0002-0001 Product Insight #1 + + /* + Set the kind of pods used by your robot. If you're using goBILDA odometry pods, select either + the goBILDA_SWINGARM_POD, or the goBILDA_4_BAR_POD. + If you're using another kind of odometry pod, uncomment setEncoderResolution and input the + number of ticks per mm of your odometry pod. + */ + odo.setEncoderResolution(GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_4_BAR_POD); + //odo.setEncoderResolution(13.26291192); + + + /* + Set the direction that each of the two odometry pods count. The X (forward) pod should + increase when you move the robot forward. And the Y (strafe) pod should increase when + you move the robot to the left. + */ + odo.setEncoderDirections(GoBildaPinpointDriver.EncoderDirection.FORWARD, GoBildaPinpointDriver.EncoderDirection.FORWARD); + + + /* + Before running the robot, recalibrate the IMU. This needs to happen when the robot is stationary + The IMU will automatically calibrate when first powered on, but recalibrating before running + the robot is a good idea to ensure that the calibration is "good". + resetPosAndIMU will reset the position to 0,0,0 and also recalibrate the IMU. + This is recommended before you run your autonomous, as a bad initial calibration can cause + an incorrect starting value for x, y, and heading. + */ + //odo.recalibrateIMU(); + odo.resetPosAndIMU(); + + telemetry.addData("Status", "Initialized"); + telemetry.addData("X offset", odo.getXOffset()); + telemetry.addData("Y offset", odo.getYOffset()); + telemetry.addData("Device Version Number:", odo.getDeviceVersion()); + telemetry.addData("Device SCalar", odo.getYawScalar()); + telemetry.update(); + + // Wait for the game to start (driver presses START) + waitForStart(); + resetRuntime(); + + + // run until the end of the match (driver presses STOP) + while (opModeIsActive()) { + + /* + Request a bulk update from the Pinpoint odometry computer. This checks almost all outputs + from the device in a single I2C read. + */ + odo.bulkUpdate(); + + + if (gamepad1.a){ + odo.resetPosAndIMU(); //resets the position to 0 and recalibrates the IMU + } + + if (gamepad1.b){ + odo.recalibrateIMU(); //recalibrates the IMU without resetting position + } + + /* + This code prints the loop frequency of the REV Control Hub. This frequency is effected + by I2C reads/writes. So it's good to keep an eye on. This code calculates the amount + of time each cycle takes and finds the frequency (number of updates per second) from + that cycle time. + */ + double newTime = getRuntime(); + double loopTime = newTime-oldTime; + double frequency = 1/loopTime; + oldTime = newTime; + + + /* + gets the current Position (x & y in mm, and heading in degrees) of the robot, and prints it. + */ + Pose2d pos = odo.getPosition(); + String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.position.x, pos.position.y, pos.heading.toDouble()); + telemetry.addData("Position", data); + + + /* + gets the current Velocity (x & y in mm/sec and heading in degrees/sec) and prints it. + */ + Pose2d vel = odo.getVelocity(); + String velocity = String.format(Locale.US,"{XVel: %.3f, YVel: %.3f, HVel: %.3f}", vel.position.x, vel.position.y, vel.heading.toDouble()); + telemetry.addData("Velocity", velocity); + + telemetry.addData("X Encoder:", odo.getEncoderX()); //gets the raw data from the X encoder + telemetry.addData("Y Encoder:",odo.getEncoderY()); //gets the raw data from the Y encoder + telemetry.addData("Pinpoint Frequency", odo.getFrequency()); //prints/gets the current refresh rate of the Pinpoint + + /* + Gets the Pinpoint device status. Pinpoint can reflect a few states. But we'll primarily see + READY: the device is working as normal + CALIBRATING: the device is calibrating and outputs are put on hold + NOT_READY: the device is resetting from scratch. This should only happen after a power-cycle + FAULT_NO_PODS_DETECTED - the device does not detect any pods plugged in + FAULT_X_POD_NOT_DETECTED - The device does not detect an X pod plugged in + FAULT_Y_POD_NOT_DETECTED - The device does not detect a Y pod plugged in + */ + telemetry.addData("Status", odo.getDeviceStatus()); + + telemetry.addData("REV Hub Frequency: ", frequency); //prints the control system refresh rate + telemetry.update(); + } + }}