FTC Lift Code

KEncoder.kt

package com.asiankoala.koawalib.hardware.motor

import com.asiankoala.koawalib.logger.Logger
import com.asiankoala.koawalib.math.epsilonEquals
import com.asiankoala.koawalib.util.Clock
import com.qualcomm.robotcore.util.MovingStatistics
import kotlin.math.abs
import kotlin.math.max
import kotlin.math.sign

// @Suppress("unused")
class KEncoder internal constructor(
    private val motor: KMotor,
    private val ticksPerUnit: Double,
    private val isRevEncoder: Boolean
) {
    private var offset = 0.0
    private var multiplier = 1.0
    private var _pos = 0.0
    private var _vel = 0.0
    private val prevPos = ArrayList<Pair<Double, Double>>()
    private val prevVel = ArrayList<Pair<Double, Double>>()
    private val velStats = MovingStatistics(LOOK_BEHIND)

    val pos get() = (_pos + offset) / ticksPerUnit
    val vel get() = velStats.mean / ticksPerUnit
    val delta get() = (
        prevPos[prevPos.size - 1].second -
            prevPos[prevPos.size - 2].second
        ) / ticksPerUnit

    val reverse: KEncoder
        get() {
            multiplier *= -1.0
            Logger.logInfo("encoder associated with ${motor.deviceName} reversed")
            return this
        }

    private fun estimateDerivative(): Pair<Double, Boolean> {
        if (prevPos.size < 2) return Pair(0.0, false)
        val oldIndex = max(0, prevPos.size - 2)
        val oldPosition = prevPos[oldIndex]
        val currPosition = prevPos.last()
        val dt = (currPosition.first - oldPosition.first)
        if (dt epsilonEquals 0.0) throw Exception("failed derivative")
        return Pair((currPosition.second - oldPosition.second) / dt, true)
    }

    private fun attemptVelUpdate() {
        val ret = estimateDerivative()
        if (!ret.second) return
        var estimatedVel = ret.first

        if (isRevEncoder) {
            estimatedVel = inverseOverflow(motor.rawMotorVelocity * multiplier, _vel)
        }

        velStats.add(estimatedVel)
    }

    private fun internalReset() {
        _pos = motor.rawMotorPosition * multiplier
        prevPos.clear()
        prevPos.add(Pair(Clock.seconds, _pos))
        prevPos.add(Pair(Clock.seconds - 1e6, _pos))
        prevVel.clear()
        velStats.clear()
        _vel = 0.0
    }

    fun update() {
        val seconds = Clock.seconds
        _pos = motor.rawMotorPosition * multiplier
        prevPos.add(Pair(seconds, _pos))
        attemptVelUpdate()
        prevVel.add(Pair(seconds, _vel))
    }

    fun zero(newPosition: Double = 0.0): KEncoder {
        internalReset()
        offset = newPosition * ticksPerUnit - _pos
        return this
    }

    companion object {
        private const val LOOK_BEHIND = 5
        private const val CPS_STEP = 0x10000

        private fun inverseOverflow(input: Double, estimate: Double): Double {
            var real = input
            while (abs(estimate - real) > CPS_STEP / 2.0) {
                real += sign(estimate - real) * CPS_STEP
            }
            return real
        }
    }
}

KMotor.kt

package com.asiankoala.koawalib.hardware.motor

import com.asiankoala.koawalib.command.commands.LoopCmd
import com.asiankoala.koawalib.control.motor.MotionProfileMotorController
import com.asiankoala.koawalib.control.motor.MotorControlModes
import com.asiankoala.koawalib.control.motor.MotorController
import com.asiankoala.koawalib.control.motor.PositionMotorController
import com.asiankoala.koawalib.control.profile.MotionState
import com.asiankoala.koawalib.hardware.KDevice
import com.asiankoala.koawalib.logger.Logger
import com.asiankoala.koawalib.math.clamp
import com.asiankoala.koawalib.math.d
import com.asiankoala.koawalib.math.epsilonNotEqual
import com.qualcomm.robotcore.hardware.DcMotor
import com.qualcomm.robotcore.hardware.DcMotorEx
import com.qualcomm.robotcore.hardware.DcMotorSimple
import kotlin.math.absoluteValue
import kotlin.math.sign

// @Suppress("unused")
class KMotor internal constructor(name: String) : KDevice<DcMotorEx>(name) {
    enum class Priority { HIGH, LOW }
    private val cmd = LoopCmd(this::update).withName("$name motor")
    private var powerMultiplier = 1.0
    private var lastUpdateIter = 0
    internal var controller: MotorController? = null
    var encoder: KEncoder? = null
    internal var mode = MotorControlModes.OPEN_LOOP
    internal var encoderCreated = false
    internal var isVoltageCorrected = false
    internal val rawMotorPosition get() = device.currentPosition.d
    internal val rawMotorVelocity get() = device.velocity
    internal var priority = Priority.HIGH
    internal var ks = 0.0

    val pos: Double get() = encoder?.pos ?: throw Exception("queried motor $deviceName's pos without paired encoder")
    val vel: Double get() = encoder?.vel ?: throw Exception("queried motor $deviceName's vel without paired encoder")

    internal var zeroPowerBehavior: DcMotor.ZeroPowerBehavior = DcMotor.ZeroPowerBehavior.FLOAT
        set(value) {
            device.zeroPowerBehavior = value
            field = value
        }

    internal var direction: DcMotorSimple.Direction = DcMotorSimple.Direction.FORWARD
        set(value) {
            powerMultiplier = if (value == DcMotorSimple.Direction.FORWARD) {
                1.0
            } else {
                -1.0
            }
            field = value
        }

    var power: Double = 0.0
        set(value) {
            val clamped = clamp(value * (if (isVoltageCorrected) VOLTAGE_CONSTANT / lastVoltageRead else 1.0), -1.0, 1.0) * powerMultiplier
            if (clamped epsilonNotEqual field &&
                (clamped == 0.0 || clamped.absoluteValue == 1.0 || (clamped - field).absoluteValue > 0.005) &&
                (priority == Priority.HIGH || iter - lastUpdateIter > 3)
            ) {
                field = clamped
                device.power = clamped + clamped.sign * ks
                lastUpdateIter = iter
            }
        }
        get() = field * powerMultiplier

    val setpoint: MotionState get() {
        if (controller !is MotionProfileMotorController) throw Exception("controller not motion profile controller")
        return (controller as MotionProfileMotorController).setpoint
    }

    val targetState: MotionState get() {
        if (controller !is PositionMotorController) throw Exception("not position controller")
        return (controller as PositionMotorController).targetState
    }

    val currState: MotionState get() = controller?.currentState ?: throw Exception("fuck")

    private fun update() {
        encoder?.let { enc ->
            enc.update()
            controller?.let {
                it.currentState = MotionState(enc.pos, enc.vel)
                it.update()
                power = it.output
                Logger.put("updating motor controller for $deviceName")
            }
        }
    }

    fun setPositionTarget(x: Double) {
        controller?.setTargetPosition(x) ?: throw Exception("motor must be position controlled")
        Logger.logInfo("set motor $deviceName's position target to $x")
    }

    fun setVelocityTarget(v: Double) {
        controller?.setTargetVelocity(v) ?: throw Exception("motor must be velocity controlled")
    }

    fun setProfileTarget(x: Double) {
        controller?.setProfileTarget(x) ?: throw Exception("motor must be motion profiled")
    }

    fun isAtTarget() = controller?.isAtTarget() ?: throw Exception("motor must not be open loop")

    fun zero(pos: Double = 0.0) {
        encoder?.zero(pos) ?: throw Exception("motor must have paired encoder")
    }

    fun enable() {
        power = 0.0
        cmd.schedule()
    }

    fun disable() {
        power = 0.0
        cmd.cancel()
    }

    init {
        device.mode = DcMotor.RunMode.RUN_WITHOUT_ENCODER
        device.zeroPowerBehavior = DcMotor.ZeroPowerBehavior.FLOAT
    }

    companion object {
        private var iter = 0
        private var VOLTAGE_CONSTANT = 12.0
        internal var lastVoltageRead = Double.NaN

        internal fun updatePriorityIter() {
            iter++
        }

        @JvmStatic
        fun setVoltageConstant(x: Double) {
            VOLTAGE_CONSTANT = x
        }
    }
}

Lift.kt

package asiankoala.ftc2022.sunmi.subsystems

import asiankoala.ftc2022.sunmi.constants.LiftConstants
import com.asiankoala.koawalib.control.controller.PIDGains
import com.asiankoala.koawalib.control.motor.FFGains
import com.asiankoala.koawalib.control.profile.MotionConstraints
import com.asiankoala.koawalib.hardware.motor.EncoderFactory
import com.asiankoala.koawalib.hardware.motor.KMotor
import com.asiankoala.koawalib.hardware.motor.MotorFactory
import com.asiankoala.koawalib.hardware.sensor.KLimitSwitch
import com.asiankoala.koawalib.logger.Logger
import com.asiankoala.koawalib.subsystem.KSubsystem

class Lift(br: KMotor) : KSubsystem() {
    private val lt = MotorFactory("lt")
        .reverse
        .pairEncoder(
            br,
            EncoderFactory(367.0 / 12.0).reverse.zero()
        )
        .withMotionProfileControl(
            PIDGains(
                LiftConstants.kP,
                LiftConstants.kI,
                LiftConstants.kD
            ),
            FFGains(kG = LiftConstants.kG),
            MotionConstraints(LiftConstants.vel, LiftConstants.accel, LiftConstants.deccel),
            0.1,
            0.0
        )
        .float
        .build()

    private val lb = MotorFactory("lb").float.build()
    private val rt = MotorFactory("rt").float.build()
    private val rb = MotorFactory("rb").float.reverse.build()
    private val limit = KLimitSwitch("limit")
    private val chainedMotors = listOf(rt, lb, rb)
    private val allMotors = listOf(lt, *chainedMotors.toTypedArray())
    var isAttemptingZero = false
    var isFailed = false
    val pos get() = lt.pos
    val vel get() = lt.vel
    val setpoint get() = lt.setpoint
    val power get() = lt.power

    fun setTarget(pos: Double) {
        lt.setProfileTarget(pos)
    }

    fun startAttemptingZero() {
        isAttemptingZero = true
    }

    fun startFailsafeZero() {
        allMotors.forEach(KMotor::disable)
        allMotors.forEach { it.power = LiftConstants.failsafePow }
        isAttemptingZero = true
        isFailed = true
    }

    private fun checkFailsafeZero() {
        if(isFailed) {
            allMotors.forEach(KMotor::enable)
            Logger.logInfo("lift unfailed")
        }
    }

    override fun periodic() {
        chainedMotors.forEach { it.power = lt.power }
        if(isAttemptingZero && limit.invoke()) {
            checkFailsafeZero()
            lt.zero()
            isAttemptingZero = false
            setTarget(LiftConstants.home)
            Logger.logInfo("zeroed")
        }
    }
}

MotionProfileMotorController.kt

package com.asiankoala.koawalib.control.motor

import com.acmerobotics.roadrunner.profile.MotionProfile
import com.acmerobotics.roadrunner.profile.MotionProfileGenerator
import com.asiankoala.koawalib.control.controller.Bounds
import com.asiankoala.koawalib.control.controller.PIDGains
import com.asiankoala.koawalib.control.profile.MotionConstraints
import com.asiankoala.koawalib.control.profile.MotionState
import com.asiankoala.koawalib.control.profile.disp.Constraints
import com.asiankoala.koawalib.control.profile.disp.DispState
import com.asiankoala.koawalib.control.profile.disp.Lmao
import com.asiankoala.koawalib.control.profile.disp.OnlineProfile
import com.asiankoala.koawalib.logger.Logger
import com.qualcomm.robotcore.util.ElapsedTime
import kotlin.math.abs

typealias stat = com.acmerobotics.roadrunner.profile.MotionState
internal class MotionProfileMotorController(
    pid: PIDGains,
    ff: FFGains,
    bounds: Bounds,
    allowedPositionError: Double,
    disabledPosition: DisabledPosition,
    private val constraints: MotionConstraints,
) : PositionMotorController(pid, ff, bounds, allowedPositionError, disabledPosition) {
    private var profile: MotionProfile? = null
    private val timer = ElapsedTime()
    internal var setpoint = MotionState(currentState.x)
        private set

    override fun setTarget(requestedState: MotionState) {
        controller.reset()
        timer.reset()
        targetState = requestedState
        profile = MotionProfileGenerator.generateSimpleMotionProfile(
            stat(currentState.x, 0.0),
            stat(targetState.x, 0.0),
            constraints.maxV,
            constraints.accel
        )
//        Logger.logInfo("created profile with startState: $currentState and endstate: $targetState")
    }

    override fun isAtTarget() = profile?.let { timer.seconds() > it.duration() && super.isAtTarget() } ?: false

    override fun update() {
        profile?.let { p -> setpoint = p[timer.seconds()].let { MotionState(it.x, it.v, it.a) } }
        controller.apply {
            targetPosition = setpoint.x
            targetVelocity = setpoint.v
            targetAcceleration = setpoint.a
        }
        super.update()
    }
}

MotorController.kt

package com.asiankoala.koawalib.control.motor

import com.asiankoala.koawalib.control.controller.Bounds
import com.asiankoala.koawalib.control.controller.PIDFController
import com.asiankoala.koawalib.control.controller.PIDGains
import com.asiankoala.koawalib.control.profile.MotionProfile
import com.asiankoala.koawalib.control.profile.MotionState
import kotlin.math.cos

internal abstract class MotorController(
    pidGains: PIDGains,
    ffGains: FFGains,
    bounds: Bounds = Bounds()
) {
    protected abstract fun setTarget(requestedState: MotionState)
    abstract fun isAtTarget(): Boolean
    abstract fun update()

    protected val controller = PIDFController(
        pidGains,
        ffGains.kV,
        ffGains.kA,
        ffGains.kS,
        kF = { pos, _ -> ffGains.kG + (ffGains.kCos?.let { it * cos(pos) } ?: 0.0) }
    ).apply {
        if (bounds.isBounded) {
            this.setInputBounds(bounds.lowerBound!!, bounds.upperBound!!)
        }
    }

    var output = 0.0; protected set
    var targetState = MotionState(0.0); protected set
    var currentState = MotionState(0.0); internal set

    fun setProfile(profile: MotionProfile) {
//        (this as MotionProfileMotorController).setInternalProfile(profile)
    }

    fun setProfileTarget(x: Double) {
        setTarget(MotionState(x))
    }

    fun setTargetPosition(x: Double) {
        setTarget(MotionState(x))
    }

    fun setTargetVelocity(v: Double) {
        setTarget(MotionState(v = v))
    }
}

PIDFController.kt

package com.asiankoala.koawalib.control.controller

import com.asiankoala.koawalib.util.Clock
import kotlin.math.abs
import kotlin.math.sign

// edited version of the PIDFController that got removed in rr 1.0.0
class PIDFController @JvmOverloads constructor(
    private val pid: PIDGains,
    private val kV: Double = 0.0,
    private val kA: Double = 0.0,
    private val kStatic: Double = 0.0,
    private val kF: (Double, Double?) -> Double = { _, _ -> 0.0 },
) {
    private var errorSum: Double = 0.0
    private var lastUpdateTimestamp: Double = Double.NaN
    private var inputBounded: Boolean = false
    private var minInput: Double = 0.0
    private var maxInput: Double = 0.0

    var targetPosition: Double = 0.0
    var targetVelocity: Double = 0.0
    var targetAcceleration: Double = 0.0
    var lastError: Double = 0.0
        private set

    fun setInputBounds(min: Double, max: Double) {
        if (min < max) {
            inputBounded = true
            minInput = min
            maxInput = max
        }
    }

    private fun getPositionError(measuredPosition: Double): Double {
        var error = targetPosition - measuredPosition
        if (inputBounded) {
            val inputRange = maxInput - minInput
            while (abs(error) > inputRange / 2.0) {
                error -= sign(error) * inputRange
            }
        }
        return error
    }

    /**
     * Run a single iteration of the controller.
     *
     * @param measuredPosition measured position (feedback)
     * @param measuredVelocity measured velocity
     */
    @JvmOverloads
    fun update(
        measuredPosition: Double,
        measuredVelocity: Double? = null
    ): Double {
        val currentTimestamp = Clock.seconds
        val error = getPositionError(measuredPosition)
        return if (lastUpdateTimestamp.isNaN()) {
            lastError = error
            lastUpdateTimestamp = currentTimestamp
            0.0
        } else {
            val dt = currentTimestamp - lastUpdateTimestamp
            errorSum += 0.5 * (error + lastError) * dt
            val errorDeriv = (error - lastError) / dt

            lastError = error
            lastUpdateTimestamp = currentTimestamp

            val closed = pid.kP * error + pid.kI * errorSum +
                pid.kD * (measuredVelocity?.let { targetVelocity - it } ?: errorDeriv)
            val open = kV * targetVelocity + kA * targetAcceleration +
                kF(measuredPosition, measuredVelocity) + kStatic * sign(targetVelocity)
            closed + open
        }
    }

    /**
     * Reset the controller's integral sum.
     */
    fun reset() {
        errorSum = 0.0
        lastError = 0.0
        lastUpdateTimestamp = Double.NaN
    }
}