Add localization, drive classes, and field centric (#1)

* Add initial quickstart files * Bump dash * Remove AccelLogger from TuningOpModes (FIRST-Tech-Challenge#177) * Make Data fields in Logger OpModes public (FIRST-Tech-Challenge#179) * Fix CW turns * Hopefully fix track width regression * Fix DriveView setDrivePowers() * Bump dash * Fix action drawing * Update for new actions * Bump RR * Bump dash * Migrate to IMU interface * Add setDirection() to Encoder interface (fixes FIRST-Tech-Challenge#203) * Add logging behind the scenes * Move logging stuff * Use IMU heading in mecanum drive localizer (fixes FIRST-Tech-Challenge#215) * No intercept regression * Regression improvements * Fix drive powers * Correct velocity in ManualFeedforwardTuner * Record signals on Java side * Adapt regression to signals * Bump dash * Add link to quickstart2 docs (fixes FIRST-Tech-Challenge#217) * Fix tick units * Add localization test Also shorten some longer method names. * Bump RR * Move quickstart files to separate module * Remove tuning web files * Use RR FTC module * Update README.md * Bump RR FTC * Bump RR FTC * Bump RR FTC * Finish tank params refactor * Add more unit comments * Remove mecanum forward kinematics rotation factors * Rename dead wheel params * Bump deps * Bump RR FTC * Dead wheel overflow encoders by default * Bump RR FTC * Bump dash * Apply heading velocity correction in localizer * Change default `par1YTicks` This avoids populating `NaN` in odometry-derived values. * Remove outdated TODO * Fix feedback tuner start * Bump RR FTC * Add more dash tunables * Re-create feedforward objects * Pipe feedforward into drive view factory * Drive view use feedforward factory (fixes FIRST-Tech-Challenge#258) * Replace rear with back * Ensure 2 samples when drawing paths * Bump dash * Require odometry wheel positions to run ManualFeedbackTuner (FIRST-Tech-Challenge#292) * Add bug template * Fix yaml * Localization test degrees heading * Add missing imports * Add more debug messages * Replace `log()` with `toDouble()` * Log power commands * Bump RR FTC * Add TODO comments * Move drive encoder reversal before first get Changing the direction of an encoder after reading it the first time will create a potentially massive delta in the first odometry update. * Bump RR * Bump RR FTC * Always throw `RuntimeException` The FTC SDK only catches `Exception` thrown by user code, so switch all throwables to `RuntimeException`. * Add encoder reverse directions to dead wheel localizers Direction doesn't matter for position regression, but it does matter for `inPerTick`, `lateralInPerTick`. And everything makes more sense if the directions are correct. * Better spline test * Bump RR FTC * Set better default `lateralInPerTick` * Update to lazy IMU (fixes FIRST-Tech-Challenge#346) * Draw robot in LocalizationTest (fixes FIRST-Tech-Challenge#360) * Add drawing class * Stop reading localizer inputs on construction This ensures that any motion between construction and first update will be ignored (this is usually the period between init and start of an op mode). If teams want to track pose during that period, they can call `updatePoseEstimate()` explicitly. This matches the behavior of the 0.5.x localizers (without the annoying reset on every pose estimate set). The localizers also now log much more data to help troubleshoot localization issues in the future. * Rename `LazyImu` drive members Also makes sure the mecanum drive localizer fetches the IMU on construction. * Use new params * Bump RR * Bump RR * Bump dash * Added example code and driver to teamcode * Update README.md * Update README.md * Update README.md * Update README.md * Bump RR FTC (fixes #405) * Fix angular velocity Add workaround for FIRST-Tech-Challenge#1070 * Merge FTC SDK v10.1 Co-authored-by: Cal Kestis <[email protected]> Co-authored-by: Cal Kestis <[email protected]> * Added example code and driver to teamcode * Update README.md * Update README.md * Update README.md * Removed duplicate Pose2D class, added imports for FTC SDK Pose2D. * Update README.md changed README to reflect removing the duplicate Pose2D * Changed i2c read speed to 400khz, added an "update only pos" function. Needs more work to determine what the best "power user setup" is. * Added Overflow for update() to allow user to specify when only certain data gets updated. * Fixed typo on velocity read registers * Landed on overloading update function with the ability to only pull heading. Consider V2 having the ability to read just Position and just Velocity. * Added overflow to the update() method to allow only updating the IMU heading * Finalized faster i2c read speed improvements * Bugfix for Encoder Direction * Removed Incorrect Example * Update SensorGoBildaPinpointExample.java Removed a new line which was accidentally added * Fix tank drive turn min ang accel (fixes FIRST-Tech-Challenge#420) * Update README.md * Move DriveLocalizer classes out of the actual drive classes * move all of the localizers and then change them to implement updatePositionEstimate and updateVelocityEstimate instead of simply "update" * change MecanumDrive and TankDrive to use the new Localizer methods * update to follow recommendations from PR comments * add `pose` property back to not break backwards compatibility * remove OTOS example + change drive localizer class names to pre-PR versions * add janky checks to check if the `pose` property of the drive classes was changed directly and if so, setPose to the pose property so the localizer pose property is updated to match * Revert teamwebcamcalibrations.xml * move DriveLocalizers to their original spot in the drive class, delete localization subpackage; fold calculatePoseDelta into the localizer update methods * remove pose variable from drive class and delegate it solely to the localizer object * delete unneeded files * delete unneeded messages * cleaning up of localizer update methods as per latest review * add pinpoint localizer * make mecanumdrive use pinpoint localizer * add parameters to pinpoint localizer * add field centric powers function to mecanumdrive * make field centric accurate * refactor clutter to a single folder and fix errors --------- Co-authored-by: Ryan Brott <[email protected]> Co-authored-by: abidingabi <[email protected]> Co-authored-by: Ethan Doak <[email protected]> Co-authored-by: Ethan <[email protected]> Co-authored-by: Cal Kestis <[email protected]> Co-authored-by: Cal Kestis <[email protected]> Co-authored-by: Zach Harel <[email protected]> Co-authored-by: 0verkil <[email protected]>
escape-velocity-14343 · Dec 27, 2024 · 5f8f97a · 5f8f97a
1 parent ca18860
commit 5f8f97a
Show file tree

Hide file tree

Showing 16 changed files with 1,614 additions and 3 deletions.
diff --git a/.github/ISSUE_TEMPLATE/bug.yml b/.github/ISSUE_TEMPLATE/bug.yml
@@ -0,0 +1,33 @@
+name: Bug Report
+description: File a bug report
+body:
+  - type: input
+    id: version
+    attributes:
+      label: RR FTC Version
+      description: Open `TeamCode/build.gradle` and read the version from the
+        `implementation "com.acmerobotics.roadrunner:ftc:LIBRARY_VERSION_HERE"`
+        line
+      placeholder: 0.1.8
+    validations:
+      required: true
+  - type: textarea
+    id: repro
+    attributes:
+      label: Observed Behavior
+    validations:
+      required: true
+  - type: textarea
+    id: tuning
+    attributes:
+      label: Tuning Files
+      description: Click the "Download" button on the tuning page and attach the file
+        (if applicable)
+  - type: textarea
+    id: logs
+    attributes:
+      label: Robot Logs
+      description: Download the relevant logs from `http://192.168.43.1:8080/logs` and
+        attach them here
+
+
diff --git a/.gitignore b/.gitignore
@@ -78,4 +78,7 @@ lint/intermediates/
 lint/generated/
 lint/outputs/
 lint/tmp/
-# lint/reports/
+# lint/reports/
+
+.DS_Store
+/.idea
diff --git a/TeamCode/build.gradle b/TeamCode/build.gradle
@@ -23,6 +23,17 @@ android {
     }
 }
 
+repositories {
+    maven {
+        url = 'https://maven.brott.dev/'
+    }
+}
+
 dependencies {
     implementation project(':FtcRobotController')
+
+    implementation "com.acmerobotics.roadrunner:ftc:0.1.14"
+    implementation "com.acmerobotics.roadrunner:core:1.0.0"
+    implementation "com.acmerobotics.roadrunner:actions:1.0.0"
+    implementation "com.acmerobotics.dashboard:dashboard:0.4.16"
 }
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SensorGoBildaPinpointExample.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SensorGoBildaPinpointExample.java
@@ -0,0 +1,199 @@
+/*   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.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 org.firstinspires.ftc.robotcore.external.navigation.Pose2D;
+import org.firstinspires.ftc.teamcode.lib.drivers.GoBildaPinpointDriver;
+
+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 {
+
+    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 an update from the Pinpoint odometry computer. This checks almost all outputs
+            from the device in a single I2C read.
+             */
+            odo.update();
+
+            /*
+            Optionally, you can update only the heading of the device. This takes less time to read, but will not
+            pull any other data. Only the heading (which you can pull with getHeading() or in getPosition().
+             */
+            //odo.update(GoBildaPinpointDriver.readData.ONLY_UPDATE_HEADING);
+
+
+            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 I²C 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.getX(DistanceUnit.MM), pos.getY(DistanceUnit.MM), pos.getHeading(AngleUnit.DEGREES));
+            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.getX(DistanceUnit.MM), vel.getY(DistanceUnit.MM), vel.getHeading(AngleUnit.DEGREES));
+            telemetry.addData("Velocity", velocity);
+
+
+            /*
+            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("Pinpoint Frequency", odo.getFrequency()); //prints/gets the current refresh rate of the Pinpoint
+
+            telemetry.addData("REV Hub Frequency: ", frequency); //prints the control system refresh rate
+            telemetry.update();
+
+        }
+    }}
+
diff --git a/.../teamcode/controllers/PIDFController.java → ...mcode/lib/controllers/PIDFController.java b/.../teamcode/controllers/PIDFController.java → ...mcode/lib/controllers/PIDFController.java
@@ -1,4 +1,4 @@
-package org.firstinspires.ftc.teamcode.controllers;
+package org.firstinspires.ftc.teamcode.lib.controllers;
 
 
 /**

diff --git a/...teamcode/controllers/SquIDController.java → ...code/lib/controllers/SquIDController.java b/...teamcode/controllers/SquIDController.java → ...code/lib/controllers/SquIDController.java
@@ -1,4 +1,4 @@
-package org.firstinspires.ftc.teamcode.controllers;
+package org.firstinspires.ftc.teamcode.lib.controllers;
 
 /**
  * The classic SquID controller. Operates off the control law of response being proportional to the (signed) square root of the error.

diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/dash/Drawing.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/lib/dash/Drawing.java
@@ -0,0 +1,23 @@
+package org.firstinspires.ftc.teamcode.lib.dash;
+
+import com.acmerobotics.dashboard.canvas.Canvas;
+import com.acmerobotics.roadrunner.Pose2d;
+import com.acmerobotics.roadrunner.Vector2d;
+
+public final class Drawing {
+    private Drawing() {}
+
+
+    public static void drawRobot(Canvas c, Pose2d t) {
+        final double ROBOT_RADIUS = 9;
+
+        c.setStrokeWidth(1);
+        c.strokeCircle(t.position.x, t.position.y, ROBOT_RADIUS);
+
+        Vector2d halfv = t.heading.vec().times(0.5 * ROBOT_RADIUS);
+        Vector2d p1 = t.position.plus(halfv);
+        Vector2d p2 = p1.plus(halfv);
+        c.strokeLine(p1.x, p1.y, p2.x, p2.y);
+    }
+
+}