Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Allow use of non-integer and custom encoders #7

Closed
wants to merge 3 commits into from
Closed

Allow use of non-integer and custom encoders #7

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
7ed5464
Allow you to use non-integer encoders
j5155 Aug 29, 2024
2b01177
Merge remote-tracking branch 'j5155/master' into j5155
j5155 Sep 22, 2024
37c918c
Make encoder interface no longer sealed and change unwrap to work for…
j5155 Sep 22, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
20 changes: 10 additions & 10 deletions RoadRunner/src/main/java/com/acmerobotics/roadrunner/ftc/Encoders.kt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -9,11 +9,11 @@ import kotlin.math.min
import kotlin.math.round

class PositionVelocityPair(
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I intend for this class to represent encoder measurements in tick units (and really measurements made through a Rev hub given the raw fields). I would rather create new types and plumbing for sensors that don't fit this model.

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Makes sense. It may still be worth having some way of expanding it for other ways of reading, such as Octoquad or the pinpoint's raw encoder passthrough system.

@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 {
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Are the odometry sensors modeled as a different kind of encoder? What alternate implementations are you pondering and why shouldn't they be included in this module?

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Currently in my road-runner-ftc fork I am representing the OTOS position and the Pinpoint raw encoder data as encoder implementations only for tuning. https://github.com/jdhs-ftc/road-runner-ftc-otos/blob/master/RoadRunner/src/main/java/com/acmerobotics/roadrunner/ftc/OTOS.kt and https://github.com/jdhs-ftc/road-runner-ftc-otos/blob/pinpoint/RoadRunner/src/main/java/com/acmerobotics/roadrunner/ftc/Pinpoint.kt (note: for the pinpoint I am currently using the raw odometry encoder passthrough for tuning, not the calculated pose, and the comments reflect that)
I mainly did this because it seemed the easiest way to use them for tuning. I have not included them in this PR because honestly I'm not sure they're up to roadrunner standards for quality and testing, but making this no longer sealed would allow me to much more easily develop and/or distribute them.
For actual following I'm using modifications to updatePoseEstimate to set the field centric pose directly, as previously discussed; that does not use these encoder modifications at all.

interface Encoder {
fun getPositionAndVelocity(): PositionVelocityPair

val controller: DcMotorController
Expand All @@ -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 {
rbrott marked this conversation as resolved.
Show resolved Hide resolved
var x = x
if (m.direction == DcMotorSimple.Direction.REVERSE) {
x = -x
Expand All @@ -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),
Expand Down Expand Up @@ -70,19 +70,19 @@ 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
// because the velocity is always a multiple of 20 cps due to Expansion Hub's 50ms measurement window
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()
Expand All @@ -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,
)
Expand Down
15 changes: 3 additions & 12 deletions RoadRunner/src/main/java/com/acmerobotics/roadrunner/ftc/Tuning.kt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2,23 +2,14 @@ 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
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
Expand Down Expand Up @@ -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 {
Expand Down
8 changes: 7 additions & 1 deletion web/tuning/common.ts
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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

Expand Down Expand Up @@ -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)
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

What do you intend for this to do? I'm assuming you don't want the comma operator here (or maybe I'm missing something).

And doesn't this conflate "double positions" with "missing velocities"?

Copy link
Author

@j5155 j5155 Sep 23, 2024 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

My only goal with this change is to not run inverseOverflow on the data if it isn't from an integer source. I am honestly uncertain that this is a great way of detecting and doing that, and very open to feedback on better ways.

});
}

return numDerivOffline(ts, xs).map((est, i) => inverseOverflow(vs[i + 1], est));
}
Expand Down