diff --git a/RoadRunner/src/main/java/com/acmerobotics/roadrunner/ftc/Encoders.kt b/RoadRunner/src/main/java/com/acmerobotics/roadrunner/ftc/Encoders.kt index 6e4d7b4..2cde0c6 100644 --- a/RoadRunner/src/main/java/com/acmerobotics/roadrunner/ftc/Encoders.kt +++ b/RoadRunner/src/main/java/com/acmerobotics/roadrunner/ftc/Encoders.kt @@ -9,11 +9,11 @@ import kotlin.math.min import kotlin.math.round class PositionVelocityPair( - @JvmField val position: Int, @JvmField val velocity: Int, - @JvmField val rawPosition: Int, @JvmField val rawVelocity: Int + @JvmField val position: Double, @JvmField val velocity: Double, + @JvmField val rawPosition: Double, @JvmField val rawVelocity: Double ) -sealed interface Encoder { +interface Encoder { fun getPositionAndVelocity(): PositionVelocityPair val controller: DcMotorController @@ -23,7 +23,7 @@ sealed interface Encoder { class RawEncoder(private val m: DcMotorEx) : Encoder { override var direction: DcMotorSimple.Direction = DcMotorSimple.Direction.FORWARD - private fun applyDirection(x: Int): Int { + private fun applyDirection(x: Double): Double { var x = x if (m.direction == DcMotorSimple.Direction.REVERSE) { x = -x @@ -37,8 +37,8 @@ class RawEncoder(private val m: DcMotorEx) : Encoder { } override fun getPositionAndVelocity(): PositionVelocityPair { - val rawPosition = m.currentPosition - val rawVelocity = m.velocity.toInt() + val rawPosition = m.currentPosition.toDouble() + val rawVelocity = m.velocity return PositionVelocityPair( applyDirection(rawPosition), applyDirection(rawVelocity), @@ -70,7 +70,7 @@ class RollingThreeMedian { // by the time they reach here, they are widened into an int and possibly negated private const val CPS_STEP = 0x10000 -private fun inverseOverflow(input: Int, estimate: Double): Int { +private fun inverseOverflow(input: Int, estimate: Double): Double { // convert to uint16 var real = input and 0xFFFF // initial, modulo-based correction: it can recover the remainder of 5 of the upper 16 bits @@ -78,11 +78,11 @@ private fun inverseOverflow(input: Int, estimate: Double): Int { real += real % 20 / 4 * CPS_STEP // estimate-based correction: it finds the nearest multiple of 5 to correct the upper bits by real += round((estimate - real) / (5 * CPS_STEP)).toInt() * 5 * CPS_STEP - return real + return real.toDouble() } class OverflowEncoder(@JvmField val encoder: RawEncoder) : Encoder { - private var lastPosition: Int = encoder.getPositionAndVelocity().position + private var lastPosition: Double = encoder.getPositionAndVelocity().position private val lastUpdate = ElapsedTime() private val velEstimate = RollingThreeMedian() @@ -97,7 +97,7 @@ class OverflowEncoder(@JvmField val encoder: RawEncoder) : Encoder { return PositionVelocityPair( p.position, - inverseOverflow(p.velocity, v), + inverseOverflow(p.velocity.toInt(), v), p.rawPosition, p.rawVelocity, ) diff --git a/RoadRunner/src/main/java/com/acmerobotics/roadrunner/ftc/Tuning.kt b/RoadRunner/src/main/java/com/acmerobotics/roadrunner/ftc/Tuning.kt index 45a4fbf..333977a 100644 --- a/RoadRunner/src/main/java/com/acmerobotics/roadrunner/ftc/Tuning.kt +++ b/RoadRunner/src/main/java/com/acmerobotics/roadrunner/ftc/Tuning.kt @@ -2,15 +2,7 @@ package com.acmerobotics.roadrunner.ftc import com.acmerobotics.dashboard.FtcDashboard import com.acmerobotics.dashboard.telemetry.MultipleTelemetry -import com.acmerobotics.roadrunner.MecanumKinematics -import com.acmerobotics.roadrunner.MotorFeedforward -import com.acmerobotics.roadrunner.PoseVelocity2d -import com.acmerobotics.roadrunner.PoseVelocity2dDual -import com.acmerobotics.roadrunner.TankKinematics -import com.acmerobotics.roadrunner.Time -import com.acmerobotics.roadrunner.TimeProfile -import com.acmerobotics.roadrunner.Vector2d -import com.acmerobotics.roadrunner.constantProfile +import com.acmerobotics.roadrunner.* import com.google.gson.annotations.SerializedName import com.qualcomm.hardware.lynx.LynxDcMotorController import com.qualcomm.hardware.lynx.LynxModule @@ -18,7 +10,6 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode import com.qualcomm.robotcore.hardware.DcMotor import com.qualcomm.robotcore.hardware.DcMotorEx import com.qualcomm.robotcore.hardware.HardwareMap -import com.qualcomm.robotcore.hardware.IMU import com.qualcomm.robotcore.hardware.VoltageSensor import com.qualcomm.robotcore.util.SerialNumber import org.firstinspires.ftc.robotcore.external.Telemetry @@ -55,10 +46,10 @@ enum class DriveType { TANK } -private fun unwrap(e: Encoder): RawEncoder = +private fun unwrap(e: Encoder): Encoder = when (e) { is OverflowEncoder -> e.encoder - is RawEncoder -> e + else -> e } fun interface FeedforwardFactory { diff --git a/web/tuning/common.ts b/web/tuning/common.ts index 959956e..121f0d4 100644 --- a/web/tuning/common.ts +++ b/web/tuning/common.ts @@ -1,5 +1,5 @@ import Plotly from 'plotly.js-dist-min'; -import regression, { DataPoint } from 'regression'; +import regression, {DataPoint} from 'regression'; // TODO: time-interpolate data @@ -78,6 +78,12 @@ function fixVels(ts: number[], xs: number[], vs: number[]) { if (ts.length !== xs.length || ts.length !== vs.length) { throw new Error(`${ts.length} !== ${xs.length} !== ${vs.length}`); } + // check if the second to last number in the data is non-integer + if (xs[-2] !== Math.round(xs[-2])) { + return numDerivOffline(ts, xs).map((est, i) => { + return (vs[i + 1], est) + }); + } return numDerivOffline(ts, xs).map((est, i) => inverseOverflow(vs[i + 1], est)); }