Localization features

.gitignore

@@ -78,4 +78,7 @@ lint/intermediates/
 lint/generated/
 lint/outputs/
 lint/tmp/
-# lint/reports/
+# lint/reports/
+
+#Kotlin
+*.salive

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/EncoderTrackingTest.java

@@ -0,0 +1,68 @@
+package org.firstinspires.ftc.teamcode;
+
+import static java.lang.StrictMath.PI;
+
+import com.qualcomm.hardware.lynx.LynxModule;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.util.ElapsedTime;
+
+import org.firstinspires.ftc.teamcode.localization.EncoderTracking;
+import org.firstinspires.ftc.teamcode.localization.Pose;
+
+import java.util.List;
+
+@TeleOp
+public class EncoderTrackingTest extends LinearOpMode {
+    private static final double ticksPerRotation = 8192.0;
+    private static final double radiusInches = 0.69;
+    private static final double trackWidth = 14 + 7 / 16.; // distance between two parallel encoders
+    private static final double forwardOffset = -(6 + 3 / 4.);
+
+    private double tick2inch(int ticks) {
+        return (ticks / ticksPerRotation) * 2 * PI * radiusInches;
+    }
+
+    @Override
+    public void runOpMode() throws InterruptedException {
+        Hardware hardware = new Hardware(hardwareMap);
+        List<LynxModule> allHubs = hardwareMap.getAll(LynxModule.class);
+        allHubs.forEach(consumer -> {
+            consumer.setBulkCachingMode(LynxModule.BulkCachingMode.AUTO);
+        });
+
+        EncoderTracking encTrack = new EncoderTracking(hardware);
+
+        waitForStart();
+        if (isStopRequested()) return;
+
+        ElapsedTime timer = new ElapsedTime();
+
+        timer.reset();
+        final double pow = 0.25;
+        final double duration = 4.0;
+        hardware.frontLeft.setPower(pow);
+        hardware.frontRight.setPower(pow);
+        hardware.backLeft.setPower(pow);
+        hardware.backRight.setPower(pow);
+        while (opModeIsActive()) {
+            if (timer.time() > duration) {
+                hardware.frontLeft.setPower(0);
+                hardware.frontRight.setPower(0);
+                hardware.backLeft.setPower(0);
+                hardware.backRight.setPower(0);
+            }
+            encTrack.step();
+            Pose pose = encTrack.getPose();
+
+//            telemetry.addData("enc:left", currentLeft);
+//            telemetry.addData("enc:center", currentCenter);
+//            telemetry.addData("enc:right", currentRight);
+//            telemetry.addLine();
+            telemetry.addData("x", pose.x());
+            telemetry.addData("y", pose.y());
+            telemetry.addData("heading deg", pose.heading() * 180 / PI);
+            telemetry.update();
+        }
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/MecanumTeleOp.java

@@ -9,13 +9,11 @@
 public class MecanumTeleOp extends LinearOpMode {
     @Override
     public void runOpMode() {
-        DcMotor frontLeft = hardwareMap.dcMotor.get("frontLeft");
-        DcMotor backLeft = hardwareMap.dcMotor.get("backLeft");
-        DcMotor frontRight = hardwareMap.dcMotor.get("frontRight");
-        DcMotor backRight = hardwareMap.dcMotor.get("backRight");
-
-        frontLeft.setDirection(DcMotorSimple.Direction.REVERSE);
-        backLeft.setDirection(DcMotorSimple.Direction.REVERSE);
+        Hardware hardware = new Hardware(hardwareMap);
+        DcMotor frontLeft = hardware.frontLeft;
+        DcMotor backLeft = hardware.backLeft;
+        DcMotor frontRight = hardware.frontRight;
+        DcMotor backRight = hardware.backRight;
 
         waitForStart();
         if (isStopRequested()) return;

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/localization/EncoderTracking.java

@@ -0,0 +1,69 @@
+package org.firstinspires.ftc.teamcode.localization;
+
+import static java.lang.Math.cos;
+import static java.lang.Math.sin;
+import static java.lang.StrictMath.PI;
+
+import com.qualcomm.robotcore.hardware.DcMotor;
+
+import org.firstinspires.ftc.teamcode.Hardware;
+
+public class EncoderTracking {
+
+    private double tick2inch(int ticks) {
+        return (ticks / ticksPerRotation) * 2 * PI * radiusInches;
+    }
+
+    private static final double ticksPerRotation = 8192.0;
+    private static final double radiusInches = 0.69;
+    private static final double trackWidth = 14 + 7 / 16.; // distance between two parallel encoders
+    private static final double forwardOffset = -(6 + 3 / 4.);
+
+    double heading = 0;
+    double x = 0;
+    double y = 0;
+
+    double lastLeft, lastCenter, lastRight;
+    final DcMotor leftEncoder, centerEncoder, rightEncoder;
+
+    public EncoderTracking(Hardware hardware) {
+        leftEncoder = hardware.encoderLeft;
+        centerEncoder = hardware.encoderCenter;
+        rightEncoder = hardware.encoderRight;
+
+        lastLeft = tick2inch(leftEncoder.getCurrentPosition());
+        lastCenter = tick2inch(centerEncoder.getCurrentPosition());
+        lastRight = tick2inch(rightEncoder.getCurrentPosition());
+    }
+
+    public void step() {
+        double currentLeft = tick2inch(leftEncoder.getCurrentPosition());
+        double currentCenter = tick2inch(centerEncoder.getCurrentPosition());
+        double currentRight = tick2inch(rightEncoder.getCurrentPosition());
+
+        double deltaLeft = currentLeft - lastLeft;
+        double deltaCenter = currentCenter - lastCenter;
+        double deltaRight = currentRight - lastRight;
+
+        double deltaTurn = (deltaRight - deltaLeft) / trackWidth;
+        double deltaForward = (deltaLeft + deltaRight) / 2.;
+        double deltaStrafe = deltaCenter - forwardOffset * deltaTurn;
+
+        double deltaX = deltaForward * cos(heading) - deltaStrafe * sin(heading);
+        double deltaY = deltaForward * sin(heading) + deltaStrafe * cos(heading);
+
+        x += deltaX;
+        y += deltaY;
+        heading += deltaTurn;
+
+        lastLeft = currentLeft;
+        lastCenter = currentCenter;
+        lastRight = currentRight;
+    }
+
+    public Pose getPose() {
+        return new Pose(
+                x, y, heading
+        );
+    }
+}

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/localization/Pose.kt

@@ -0,0 +1,28 @@
+package org.firstinspires.ftc.teamcode.localization
+
+data class Pose(
+    @get:JvmName("x") val x: Double,
+    @get:JvmName("y") val y: Double,
+    @get:JvmName("heading") val heading: Double,
+) {
+    /**
+     * vector addition (i.e. [x1, y1, heading1] + [x2, y2, heading2] = [x1 + x2, y1 + y2, heading1 + heading2])
+     */
+    @JvmName("add")
+    operator fun plus(other: Pose) = Pose(x + other.x, y + other.y, heading + other.heading)
+
+    @JvmOverloads
+    constructor(x: Number, y: Number, heading: Number = 0.0) : this(
+        x.toDouble(),
+        y.toDouble(),
+        heading.toDouble()
+    )
+}
+
+data class Motion(
+    val forward: Double,
+    val right: Double,
+    val turn: Double
+) {
+
+}

build.dependencies.gradle

@@ -1,7 +1,9 @@
 repositories {
     mavenCentral()
     google() // Needed for androidx
-
+    maven {
+        url = 'https://maven.brott.dev/'
+    }
 }
 
 dependencies {
@@ -21,5 +23,7 @@ dependencies {
     implementation 'androidx.appcompat:appcompat:1.2.0'
     testImplementation 'org.junit.jupiter:junit-jupiter:5.9.2'
     testRuntimeOnly 'org.junit.platform:junit-platform-launcher'
+
+    implementation 'com.acmerobotics.dashboard:dashboard:0.4.15'
 }