install gobilda odo

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/MecanumDrive.java

@@ -117,7 +117,7 @@ kinematics.new WheelVelConstraint(PARAMS.maxWheelVel),
 
     public final VoltageSensor voltageSensor;
 
-    public final LazyImu lazyImu;
+    public LazyImu lazyImu;
 
     public final Localizer localizer;
     public Pose2d pose;

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/PinpointDrive.java

@@ -0,0 +1,165 @@
+package org.firstinspires.ftc.teamcode.roadrunner;
+
+
+import static com.qualcomm.hardware.rev.RevHubOrientationOnRobot.zyxOrientation;
+
+import com.acmerobotics.dashboard.config.Config;
+import com.acmerobotics.roadrunner.Pose2d;
+import com.acmerobotics.roadrunner.PoseVelocity2d;
+import com.acmerobotics.roadrunner.ftc.FlightRecorder;
+import com.acmerobotics.roadrunner.ftc.GoBildaPinpointDriver;
+import com.acmerobotics.roadrunner.ftc.GoBildaPinpointDriverRR;
+import com.acmerobotics.roadrunner.ftc.LazyImu;
+import com.qualcomm.hardware.rev.RevHubOrientationOnRobot;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
+import org.firstinspires.ftc.robotcore.external.navigation.Pose2D;
+import org.firstinspires.ftc.teamcode.roadrunner.messages.PoseMessage;
+
+/**
+ * Experimental extension of MecanumDrive that uses the Gobilda Pinpoint sensor for localization.
+ * <p>
+ * 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
+ */
+@Config
+public class PinpointDrive extends MecanumDrive {
+    public static class Params {
+        /*
+        Set this to the name that your Pinpoint is configured as in your hardware config.
+         */
+        public String pinpointDeviceName = "pinpoint";
+        /*
+        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 the 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 the 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 for 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 = GoBildaPinpointDriverRR.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;
+
+        /*
+        Use the pinpoint IMU for tuning
+        If true, overrides any IMU setting in MecanumDrive and uses exclusively Pinpoint for tuning
+        You can also use the pinpoint directly in MecanumDrive if this doesn't work for some reason;
+         replace "imu" with "pinpoint" or whatever your pinpoint is called in config.
+         Note: Pinpoint IMU is always used for base localization
+         */
+        public boolean usePinpointIMUForTuning = true;
+    }
+
+    public static Params PARAMS = new Params();
+    public GoBildaPinpointDriverRR pinpoint;
+    private Pose2d lastPinpointPose = pose;
+
+    public PinpointDrive(HardwareMap hardwareMap, Pose2d pose) {
+        super(hardwareMap, pose);
+        FlightRecorder.write("PINPOINT_PARAMS", PARAMS);
+        pinpoint = hardwareMap.get(GoBildaPinpointDriverRR.class, PARAMS.pinpointDeviceName);
+
+        if (PARAMS.usePinpointIMUForTuning) {
+            lazyImu = new LazyImu(hardwareMap, PARAMS.pinpointDeviceName, new RevHubOrientationOnRobot(zyxOrientation(0, 0, 0)));
+        }
+
+        // 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);
+
+        pinpoint.setEncoderDirections(PARAMS.xDirection, PARAMS.yDirection);
+
+        /*
+        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();
+        // wait for pinpoint to finish calibrating
+        try {
+            Thread.sleep(300);
+        } catch (InterruptedException e) {
+            throw new RuntimeException(e);
+        }
+
+        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 the sensor's 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.update();
+        pose = pinpoint.getPositionRR();
+        lastPinpointPose = pose;
+
+        // RR standard
+        poseHistory.add(pose);
+        while (poseHistory.size() > 100) {
+            poseHistory.removeFirst();
+        }
+
+        FlightRecorder.write("ESTIMATED_POSE", new PoseMessage(pose));
+        FlightRecorder.write("PINPOINT_RAW_POSE", new FTCPoseMessage(pinpoint.getPosition()));
+        FlightRecorder.write("PINPOINT_STATUS", pinpoint.getDeviceStatus());
+
+        return pinpoint.getVelocityRR();
+    }
+
+
+    // for debug logging
+    public static final class FTCPoseMessage {
+        public long timestamp;
+        public double x;
+        public double y;
+        public double heading;
+
+        public FTCPoseMessage(Pose2D pose) {
+            this.timestamp = System.nanoTime();
+            this.x = pose.getX(DistanceUnit.INCH);
+            this.y = pose.getY(DistanceUnit.INCH);
+            this.heading = pose.getHeading(AngleUnit.RADIANS);
+        }
+    }
+
+
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/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.roadrunner;
+
+import com.acmerobotics.roadrunner.Pose2d;
+import com.acmerobotics.roadrunner.PoseVelocity2d;
+import com.acmerobotics.roadrunner.ftc.GoBildaPinpointDriver;
+import com.acmerobotics.roadrunner.ftc.GoBildaPinpointDriverRR;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+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 [email protected]
+
+-Ethan Doak
+ */
+
+@TeleOp(name = "goBILDA® PinPoint Odometry Example", group = "Linear OpMode")
+//@Disabled
+public class SensorGoBildaPinpointExample extends LinearOpMode {
+
+    GoBildaPinpointDriverRR 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(GoBildaPinpointDriverRR.class, "pinpoint");
+
+        /*
+        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.update();
+
+
+            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 inches, and heading in radians) of the robot, and prints it.
+             */
+            Pose2d pos = odo.getPositionRR();
+            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 inches/sec and heading in radians/sec) and prints it.
+             */
+            PoseVelocity2d vel = odo.getVelocityRR();
+            String velocity = String.format(Locale.US, "{XVel: %.3f, YVel: %.3f, HVel: %.3f}", vel.linearVel.x, vel.linearVel.y, vel.angVel);
+            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();
+        }
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/roadrunner/tuning/LocalizationTest.java

@@ -9,16 +9,14 @@
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
 
 import org.firstinspires.ftc.teamcode.roadrunner.Drawing;
-import org.firstinspires.ftc.teamcode.roadrunner.MecanumDrive;
-import org.firstinspires.ftc.teamcode.roadrunner.SparkFunOTOSDrive;
-import org.firstinspires.ftc.teamcode.roadrunner.TankDrive;
+import org.firstinspires.ftc.teamcode.roadrunner.PinpointDrive;
 
 public class LocalizationTest 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));
+        PinpointDrive drive = new PinpointDrive(hardwareMap, new Pose2d(0, 0, 0));
 
         waitForStart();