From d5a3b14aeb64f9e32940a327268bd9ed7b8702ee Mon Sep 17 00:00:00 2001 From: penguinencounter <49845522+penguinencounter@users.noreply.github.com> Date: Wed, 21 Aug 2024 13:58:03 -0700 Subject: [PATCH] Add pose-pose stuff & tests!! --- .../ftc/teamcode/OdometryTeleOp.java | 5 ++ .../ftc/teamcode/Pose2PoseTest.java | 16 +++- .../ftc/teamcode/mmooover/Pose.kt | 10 +++ .../teamcode/mmooover/PoseFromToProcessor.kt | 77 ++++++++++++++++--- .../ftc/teamcode/mmooover/Speed2Power.kt | 18 +++++ .../ftc/teamcode/test/FloatUtil.kt | 17 ++++ .../ftc/teamcode/test/TestSpeedToPower.kt | 27 +++++++ 7 files changed, 157 insertions(+), 13 deletions(-) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/mmooover/Speed2Power.kt create mode 100644 TeamCode/src/test/java/org/firstinspires/ftc/teamcode/test/FloatUtil.kt create mode 100644 TeamCode/src/test/java/org/firstinspires/ftc/teamcode/test/TestSpeedToPower.kt diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OdometryTeleOp.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OdometryTeleOp.java index 5b750d283aa7..ad570057b9f8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OdometryTeleOp.java +++ b/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()); diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Pose2PoseTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Pose2PoseTest.java index 22fa3e5ea0b6..89463e874f6c 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/Pose2PoseTest.java +++ b/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,11 +40,12 @@ 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(); @@ -51,6 +53,10 @@ public void runOpMode() { 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(); } } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/mmooover/Pose.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/mmooover/Pose.kt index c2653112e897..7f48fff2762d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/mmooover/Pose.kt +++ b/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); } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/mmooover/PoseFromToProcessor.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/mmooover/PoseFromToProcessor.kt index eb15427b4559..6f8cf30e03cf 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/mmooover/PoseFromToProcessor.kt +++ b/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 = mutableListOf() + val yi: MutableList = 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 // diff = Pose( nextPose.x - last.x, nextPose.y - last.y, wrapAngle(nextPose.heading - last.heading) - ) + ) // 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) } } \ No newline at end of file diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/mmooover/Speed2Power.kt b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/mmooover/Speed2Power.kt new file mode 100644 index 000000000000..554ad3d4dd1e --- /dev/null +++ b/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() + } +} \ No newline at end of file diff --git a/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/test/FloatUtil.kt b/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/test/FloatUtil.kt new file mode 100644 index 000000000000..0d44b349068a --- /dev/null +++ b/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) + } +} \ No newline at end of file diff --git a/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/test/TestSpeedToPower.kt b/TeamCode/src/test/java/org/firstinspires/ftc/teamcode/test/TestSpeedToPower.kt new file mode 100644 index 000000000000..4f7de93890f6 --- /dev/null +++ b/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) + } +} \ No newline at end of file