initial
This commit is contained in:
@@ -0,0 +1,90 @@
|
||||
package dev.asdf00.visionfive.hc
|
||||
|
||||
import java.nio.file.Files
|
||||
import java.nio.file.Path
|
||||
|
||||
class GpioPin(val num: Int, val isInput: Boolean) : AutoCloseable {
|
||||
var state: Boolean = false
|
||||
get() {
|
||||
if (isInput)
|
||||
field = Files.readString(valuePath) == "1\n"
|
||||
return field
|
||||
}
|
||||
set(newVal) {
|
||||
if (isInput)
|
||||
throw IllegalStateException("can not write state of input")
|
||||
if (field == newVal)
|
||||
return
|
||||
if (!isInput)
|
||||
Files.writeString(valuePath, if (newVal) "1" else "0")
|
||||
field = newVal
|
||||
}
|
||||
|
||||
private val basePath get() = Path.of("/sys/class/gpio")
|
||||
private val pinPath get() = basePath.resolve("gpio$num")
|
||||
private val valuePath get() = pinPath.resolve("value")
|
||||
|
||||
init {
|
||||
if (num !in ALLOWED_PINS)
|
||||
throw IllegalArgumentException("GPIO$num does not exist!")
|
||||
println("initializing with 'echo $num > ${basePath.resolve("export")}'")
|
||||
Files.writeString(basePath.resolve("export"), "$num")
|
||||
Files.writeString(basePath.resolve("gpio$num").resolve("direction"), if (isInput) "in" else "out")
|
||||
}
|
||||
|
||||
override fun close() {
|
||||
if (!isInput)
|
||||
state = false
|
||||
Files.writeString(basePath.resolve("unexport"), "$num")
|
||||
}
|
||||
|
||||
companion object {
|
||||
private val ALLOWED_PINS = setOf(58, 57, 55, 5, 6, 42, 38, 43, 47, 54, 51, 52, 53, 50, 48, 49, 56, 45, 40, 37, 39, 46, 59, 63, 36, 60, 61, 44)
|
||||
}
|
||||
}
|
||||
|
||||
class PinGroup(vararg val pins: GpioPin) {
|
||||
var state: BooleanArray
|
||||
get() = BooleanArray(pins.size) { pins[it].state }
|
||||
set(nuStates) {
|
||||
if (nuStates.size != pins.size)
|
||||
throw IllegalArgumentException("state count mismatch, expected ${pins.size}, got ${nuStates.size}")
|
||||
for (i in nuStates.indices)
|
||||
pins[i].state = nuStates[i]
|
||||
}
|
||||
|
||||
var intState: Long
|
||||
get() {
|
||||
if (pins.size > 64)
|
||||
throw UnsupportedOperationException("can not use intState for PinGroup with more than 64 pins")
|
||||
var l = 0L
|
||||
for (i in pins.indices.reversed()) {
|
||||
l = l shl 1
|
||||
l = l or if (pins[i].state) 1 else 0
|
||||
}
|
||||
return l
|
||||
}
|
||||
set(value) {
|
||||
var l = value
|
||||
if (pins.size > 64)
|
||||
throw UnsupportedOperationException("can not use intState for PinGroup with more than 64 pins")
|
||||
for (i in pins.indices) {
|
||||
pins[i].state = l and 1 == 1L
|
||||
l = l ushr 1
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
fun usingPins(vararg pins: Pair<Int, Boolean>, body: (Array<GpioPin>) -> Unit) {
|
||||
fun recUsingPins(i: Int, pins: Array<GpioPin?>, pinIds: Array<out Pair<Int, Boolean>>, body: (Array<GpioPin>) -> Unit) {
|
||||
if (i < pins.size) {
|
||||
GpioPin(pinIds[i].first, pinIds[i].second).use {
|
||||
pins[i] = it
|
||||
recUsingPins(i + 1, pins, pinIds, body)
|
||||
}
|
||||
} else {
|
||||
body(Array(pins.size) { pins[it]!! })
|
||||
}
|
||||
}
|
||||
recUsingPins(0, arrayOfNulls(pins.size), pins, body)
|
||||
}
|
||||
@@ -0,0 +1,115 @@
|
||||
package dev.asdf00.visionfive.hc
|
||||
|
||||
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,
|
||||
senseTop,
|
||||
PinGroup(m0, m1, m2, m3)
|
||||
)
|
||||
|
||||
init {
|
||||
motor.intState = initialStep
|
||||
}
|
||||
|
||||
private var internalState = -1
|
||||
private var maxStep = -1
|
||||
|
||||
var state: Double
|
||||
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()
|
||||
}
|
||||
|
||||
fun runInitSequence() {
|
||||
val prevState = runToBot()
|
||||
val range = runToTop()
|
||||
stepClock(range - prevState)
|
||||
internalState = prevState
|
||||
maxStep = range
|
||||
}
|
||||
|
||||
private fun stepClock(cnt: Int = 1): Int {
|
||||
var actual = cnt
|
||||
for (i in 0..<cnt) {
|
||||
if (!senseBot.state) {
|
||||
doStep(clockStep)
|
||||
} else {
|
||||
actual = i
|
||||
break
|
||||
}
|
||||
}
|
||||
return actual
|
||||
}
|
||||
|
||||
private fun stepAnti(cnt: Int = 1): Int {
|
||||
var actual = cnt
|
||||
for (i in 0..<cnt) {
|
||||
if (!senseTop.state) {
|
||||
doStep(antiStep)
|
||||
} else {
|
||||
actual = i
|
||||
break
|
||||
}
|
||||
}
|
||||
return actual
|
||||
}
|
||||
|
||||
private fun runToBot(): Int {
|
||||
var i = 0
|
||||
while (true) {
|
||||
if (senseBot.state)
|
||||
return i
|
||||
doStep(clockStep)
|
||||
i++
|
||||
}
|
||||
}
|
||||
|
||||
private fun runToTop(): Int {
|
||||
var i = 0
|
||||
while (true) {
|
||||
if (senseTop.state)
|
||||
return i
|
||||
doStep(antiStep)
|
||||
i++
|
||||
}
|
||||
}
|
||||
|
||||
private fun doStep(step: LongArray) {
|
||||
assert(motor.intState == initialStep, { "step must ALWAYS start on initial step state" })
|
||||
for (l in step) {
|
||||
motor.intState = l
|
||||
Thread.sleep(stepDelay)
|
||||
}
|
||||
assert(motor.intState == initialStep, { "step must ALWAYS end on initial step state" })
|
||||
}
|
||||
|
||||
companion object {
|
||||
private val stepDelay = 10L
|
||||
|
||||
private val initialStep = 0b1001L
|
||||
|
||||
private val clockStep = longArrayOf(
|
||||
0b0001,
|
||||
0b0011,
|
||||
0b0010,
|
||||
0b0110,
|
||||
0b0100,
|
||||
0b1100,
|
||||
0b1000,
|
||||
0b1001,
|
||||
)
|
||||
|
||||
private val antiStep = longArrayOf(
|
||||
0b1000,
|
||||
0b1100,
|
||||
0b0100,
|
||||
0b0110,
|
||||
0b0010,
|
||||
0b0011,
|
||||
0b0001,
|
||||
0b1001,
|
||||
)
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user