00asdf/VisionFiveHeatControl
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

untested motor controller

Browse Source
This commit is contained in:
00asdf 2025-04-12 00:22:18 +02:00
parent b24ebba1f6
commit 754275d7b2
1 changed files with 77 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

88
src/main/kotlin/dev/asdf00/visionfive/hc/MotorControl.kt
View File

@ -1,5 +1,7 @@
package dev.asdf00.visionfive.hc
import kotlin.math.abs
class Controller(private val senseBot: GpioPin, private val senseTop: GpioPin, private val motor: PinGroup) {
constructor(senseBot: GpioPin, senseTop: GpioPin, m0: GpioPin, m1: GpioPin, m2: GpioPin, m3: GpioPin) : this(
senseBot,
@ -8,7 +10,7 @@ class Controller(private val senseBot: GpioPin, private val senseTop: GpioPin, p
)
init {
motor.intState = initialStep
motor.intState = INITIAL_STEP
}
private var internalState = -1
@ -18,11 +20,71 @@ class Controller(private val senseBot: GpioPin, private val senseTop: GpioPin, p
get() = if (internalState < 0) Double.NaN else internalState.toDouble() / maxStep.toDouble()
set(target) {
if (internalState < 0)
throw IllegalStateException("call Controller#runInitSequence before using it")
TODO()
throw IllegalStateException("call Controller#runCalibrationSequence before using it")
if (target < 0 || target > 1)
throw IllegalArgumentException("state must always be between 0 and 1")
if (target isCloseTo 0) {
// full close
runToBot()
internalState = 0
} else if (target isCloseTo 1) {
// full open
runToTop()
internalState = maxStep
} else {
// normal case
val dif = (maxStep.toDouble() * target).toInt() - internalState
if (dif > 0) {
val realDif = stepAnti(dif)
if (realDif != dif) {
// maybe log correction
// recalibrate if deviation was too much
if (abs(internalState.toDouble() / maxStep.toDouble() - target) > RECALIB_DATA) {
// turn to middle
val mDif = stepClock(maxStep / 2)
if (mDif != maxStep / 2) {
// something has gone VERY wrong here
internalState = -1
throw IllegalStateException("unrecoverable state")
}
runCalibrationSequence()
// and try again after calibration
@Suppress("RecursivePropertyAccessor")
this.state = target
return
}
internalState = maxStep
} else {
internalState += realDif
}
} else if (dif < 0) {
val realDif = stepClock(dif)
if (realDif != dif) {
// maybe log correction
// recalibrate if deviation was too much
if (abs(internalState.toDouble() / maxStep.toDouble() - target) > RECALIB_DATA) {
// turn to middle
val mDif = stepAnti(maxStep / 2)
if (mDif != maxStep / 2) {
// something has gone VERY wrong here
internalState = -1
throw IllegalStateException("unrecoverable state")
}
runCalibrationSequence()
// and try again after calibration
@Suppress("RecursivePropertyAccessor")
this.state = target
return
}
internalState = 0
} else {
internalState -= realDif
}
}
}
}
fun runInitSequence() {
fun runCalibrationSequence() {
val prevState = runToBot()
val range = runToTop()
stepClock(range - prevState)
@ -77,18 +139,18 @@ class Controller(private val senseBot: GpioPin, private val senseTop: GpioPin, p
}
private fun doStep(step: LongArray) {
assert(motor.intState == initialStep, { "step must ALWAYS start on initial step state" })
assert(motor.intState == INITIAL_STEP) { "step must ALWAYS start on initial step state" }
for (l in step) {
motor.intState = l
Thread.sleep(stepDelay)
Thread.sleep(STEP_DELAY)
}
assert(motor.intState == initialStep, { "step must ALWAYS end on initial step state" })
assert(motor.intState == INITIAL_STEP) { "step must ALWAYS end on initial step state" }
}
companion object {
private val stepDelay = 10L
private val initialStep = 0b1001L
private const val STEP_DELAY = 10L
private const val INITIAL_STEP = 0b1001L
private const val RECALIB_DATA = 0.01
private val clockStep = longArrayOf(
0b0001,
@ -112,4 +174,8 @@ class Controller(private val senseBot: GpioPin, private val senseTop: GpioPin, p
0b1001,
)
}
}
}
infix fun Double.isCloseTo(other: Double) = this.isCloseTo(other, 0e-6)
infix fun Double.isCloseTo(other: Int) = this isCloseTo other.toDouble()
fun Double.isCloseTo(other: Double, epsilon: Double) = this >= other - epsilon && this <= other + epsilon