Add pose-pose stuff & tests!!

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

@@ -9,6 +9,7 @@
 
 import org.firstinspires.ftc.teamcode.mmooover.EncoderTracking;
 import org.firstinspires.ftc.teamcode.mmooover.Pose;
+import org.firstinspires.ftc.teamcode.mmooover.PoseFromToProcessor;
 
 import java.util.List;
 
@@ -29,6 +30,7 @@ public void runOpMode() {
         DcMotor backRight = hardware.backRight;
 
         EncoderTracking encTrack = new EncoderTracking(hardware);
+        PoseFromToProcessor pftp = new PoseFromToProcessor(Pose.ORIGIN);
 
         waitForStart();
         if (isStopRequested()) return;
@@ -56,6 +58,9 @@ public void runOpMode() {
             frontRight.setPower(frontRightPower);
             backRight.setPower(backRightPower);
 
+            double diff = (frontLeftPower + backLeftPower) - (frontRightPower + backRightPower);
+            pftp.update(diff, p);
+
             telemetry.addData("x", p.x());
             telemetry.addData("y", p.y());
             telemetry.addData("heading (rad)", p.heading());

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

@@ -13,6 +13,7 @@
 import org.firstinspires.ftc.teamcode.mmooover.EncoderTracking;
 import org.firstinspires.ftc.teamcode.mmooover.Motion;
 import org.firstinspires.ftc.teamcode.mmooover.Pose;
+import org.firstinspires.ftc.teamcode.mmooover.PoseFromToProcessor;
 import org.firstinspires.ftc.teamcode.utilities.LoopStopwatch;
 
 @Autonomous
@@ -39,18 +40,23 @@ public void runOpMode() {
         ElapsedTime timer = new ElapsedTime();
         ElapsedTime targetTime = new ElapsedTime();
         LoopStopwatch ticker = new LoopStopwatch();
+        PoseFromToProcessor pftp = new PoseFromToProcessor(Pose.ORIGIN);
+        Motion lastAction = null;
         waitForStart();
         targetTime.reset();
         timer.reset();
         boolean wait = false;
-        Pose lastPose = Pose.ORIGIN;
         ticker.clear();
         while (opModeIsActive()) {
             ticker.click();
             // Updates pose
             tracker.step();
             // Gets current pose
             Pose p = tracker.getPose();
+            if (lastAction != null)
+                pftp.update(lastAction.getPowerDifferential(), p);
+            else
+                pftp.update(0, p);
             if (wait) {
                 hardware.driveMotors.setAll(0);
                 if (targetIndex >= targets.length) {
@@ -75,9 +81,10 @@ public void runOpMode() {
                     timer.reset();
                     continue;
                 }
-                double speed = min(max(0.5, linear / 18.0 + 0.1), timer.time() / 2);
-                Motion action = p.to(targets[targetIndex], hardware);
-//                action.apply(hardware.driveMotors, CALIBRATION, speed);
+                double speed = min(max(0.75, linear / 18.0 + 0.1), timer.time() / 2);
+                Motion action = pftp.getMotionToTarget(targets[targetIndex], hardware);
+                action.apply(hardware.driveMotors, CALIBRATION, speed);
+                lastAction = action;
             }
             telemetry.addLine("step " + (targetIndex + 1) + " of " + targets.length);
             telemetry.addLine(String.format("Target hit for %.2fs", targetTime.time()));
@@ -104,5 +111,6 @@ public void runOpMode() {
             telemetry.addLine(String.format("While running: %.2fms per loop", ticker.getAvg() * 1000));
             telemetry.update();
         }
+        pftp.dump();
     }
 }

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

@@ -90,6 +90,15 @@ data class Motion(
         turn.toDouble()
     )
 
+    private var lastFL: Double = 0.0;
+    private var lastFR: Double = 0.0;
+    private var lastBL: Double = 0.0;
+    private var lastBR: Double = 0.0;
+
+    fun getPowerDifferential(): Double {
+        return (lastFR + lastBR) - (lastFL + lastBL)
+    }
+
     /**
      * Apply this Motion to a MotorSet.
      * @param motors target set of motors
@@ -115,6 +124,7 @@ data class Motion(
         fr *= factorD
         bl *= factorD
         br *= factorD
+        lastFL = fl; lastFR = fr; lastBL = bl; lastBR = br
         motors.set(fl, fr, bl, br);
     }
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/mmooover/PoseFromToProcessor.kt

@@ -1,7 +1,14 @@
 package org.firstinspires.ftc.teamcode.mmooover
 
+import android.annotation.SuppressLint
+import android.util.Log
+import kotlin.math.abs
+import kotlin.math.cos
+import kotlin.math.sin
+import kotlin.random.Random
+
 /**
- * Combines robot tracking data from EncoderTracking and
+ * Combines robot tracking data from EncoderTracking and Pose-to-Pose outputs...
  * @see EncoderTracking
  */
 class PoseFromToProcessor(origin: Pose) {
@@ -11,22 +18,74 @@ class PoseFromToProcessor(origin: Pose) {
          * The average gets (1 - WEIGHT_NEXT) * old + WEIGHT_NEXT * new
          */
         const val WEIGHT_NEXT = .2
+        const val LIMIT = 100
     }
 
-    var last: Pose = origin
-    var diff: Pose = Pose.ORIGIN
+    var total = 0
+
+    private var lastTick = System.nanoTime()
+    private var last: Pose = origin
+    private var diff: Pose = Pose.ORIGIN
+    private var deltaHeading: Double = 0.0
 
-    var alpha = 0.0
+    val xi: MutableList<Double> = mutableListOf()
+    val yi: MutableList<Double> = mutableListOf()
 
-    fun update(frontLeft: Double, frontRight: Double, backLeft: Double, backRight: Double, nextPose: Pose) {
+    private var alpha = 0.0
+
+    fun update(
+        powerDiff: Double,
+        nextPose: Pose
+    ) {
+        val now = System.nanoTime()
+        val lastTickCache = lastTick
+        lastTick = now
+        val duration = (now - lastTickCache) / 1e9 // <s>
         diff = Pose(
             nextPose.x - last.x,
             nextPose.y - last.y,
             wrapAngle(nextPose.heading - last.heading)
-        )
+        ) // <m, m, rad>
         last = nextPose
-        val beta = (frontRight + backRight) - (frontLeft + backLeft)
-        val next = diff.heading / beta
-        alpha = (1 - WEIGHT_NEXT) * alpha + WEIGHT_NEXT * next
+        val xI = powerDiff * duration
+        val yI = diff.heading
+        deltaHeading = diff.heading
+
+        if (abs(xI) > 1e-3 && abs(yI) > 1e-3) {
+            if (xi.size < LIMIT) {
+                xi.add(xI)
+                yi.add(yI)
+            } else {
+                val prop = LIMIT.toDouble() / total
+                if (Math.random() < prop) {
+                    val ind = Random.Default.nextInt(0, LIMIT)
+                    xi[ind] = xI
+                    yi[ind] = yI
+                }
+            }
+            total++
+        }
+//        val next = diff.heading / powerDiff
+//        alpha = (1 - WEIGHT_NEXT) * alpha + WEIGHT_NEXT * next
+    }
+
+    @SuppressLint("DefaultLocale")
+    fun dump() {
+        StringBuilder().apply {
+            append("x_i: ")
+            xi.forEach { append(String.format("%.4f", it)).append(", ") }
+            append("\ny_i: ")
+            yi.forEach { append(String.format("%.4f", it)).append(", ") }
+        }.also { Log.i("data", it.toString()) }
+    }
+
+    fun getMotionToTarget(target: Pose, robot: TriOdoProvider): Motion {
+        val dh = wrapAngle(target.heading - last.heading)
+        val dx = target.x - last.x
+        val dy = target.y - last.y
+        val estHeading = last.heading + (.5 * deltaHeading)
+        val forward = cos(estHeading) * dx + sin(estHeading) * dy
+        val strafe = sin(estHeading) * dx - cos(estHeading) * dy
+        return Motion(forward, strafe, -dh * robot.forwardOffset)
     }
 }

TeamCode/src/main/java/org/firstinspires/ftc/teamcode/mmooover/Speed2Power.kt

@@ -0,0 +1,18 @@
+package org.firstinspires.ftc.teamcode.mmooover
+
+import kotlin.math.abs
+
+class Speed2Power(val thresh: Double) {
+    companion object {
+        const val EPSILON = 0.001
+    }
+
+    constructor(thresh: Number) : this(thresh.toDouble())
+
+    fun speed2power(speed: Double) = when {
+        abs(speed) < EPSILON -> 0.0
+        speed > 0 -> thresh + ((1 - thresh) * speed)
+        speed < 0 -> -thresh + ((1 - thresh) * speed)
+        else -> throw IllegalArgumentException()
+    }
+}

TeamCode/src/test/java/org/firstinspires/ftc/teamcode/test/FloatUtil.kt

@@ -0,0 +1,17 @@
+package org.firstinspires.ftc.teamcode.test
+
+import kotlin.math.abs
+
+object FloatUtil {
+    const val EPS = 1e-6
+
+    @JvmStatic
+    @JvmOverloads
+    fun assertClose(a: Number, b: Number, lazyMessage: (() -> Any)? = null) {
+        val message = lazyMessage ?: {
+            String.format("Expected %.4f to be approximately %.4f", a.toDouble(), b.toDouble())
+        }
+        val diff = abs(a.toDouble() - b.toDouble())
+        assert(diff < EPS, message)
+    }
+}

TeamCode/src/test/java/org/firstinspires/ftc/teamcode/test/TestSpeedToPower.kt

@@ -0,0 +1,27 @@
+package org.firstinspires.ftc.teamcode.test
+
+import org.firstinspires.ftc.teamcode.mmooover.Speed2Power
+import org.junit.jupiter.api.Test
+
+class TestSpeedToPower {
+    companion object {
+        val zeroThresh = Speed2Power(0.0)
+        val oneThresh = Speed2Power(1.0)
+    }
+    @Test
+    fun zeroThreshZeroSpeed() {
+        FloatUtil.assertClose(zeroThresh.speed2power(0.0), 0.0)
+    }
+    @Test
+    fun zeroThreshFullSpeed() {
+        FloatUtil.assertClose(zeroThresh.speed2power(1.0), 1.0)
+        FloatUtil.assertClose(zeroThresh.speed2power(-1.0), -1.0)
+    }
+    @Test
+    fun oneThreshAlways1() {
+        FloatUtil.assertClose(oneThresh.speed2power(1.0), 1.0)
+        FloatUtil.assertClose(oneThresh.speed2power(.1), 1.0)
+        FloatUtil.assertClose(oneThresh.speed2power(-1.0), -1.0)
+        FloatUtil.assertClose(oneThresh.speed2power(-.1), -1.0)
+    }
+}