From 754275d7b236335991060ebc7c7d31a09eaa65b2 Mon Sep 17 00:00:00 2001
From: 00asdf <christoph@00asdf.dev>
Date: Sat, 12 Apr 2025 00:22:18 +0200
Subject: [PATCH] untested motor controller

---
 .../dev/asdf00/visionfive/hc/MotorControl.kt  | 88 ++++++++++++++++---
 1 file changed, 77 insertions(+), 11 deletions(-)

diff --git a/src/main/kotlin/dev/asdf00/visionfive/hc/MotorControl.kt b/src/main/kotlin/dev/asdf00/visionfive/hc/MotorControl.kt
index ece77e0..7812ab6 100644
--- a/src/main/kotlin/dev/asdf00/visionfive/hc/MotorControl.kt
+++ b/src/main/kotlin/dev/asdf00/visionfive/hc/MotorControl.kt
@@ -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,
         )
     }
-}
\ No newline at end of file
+}
+
+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
\ No newline at end of file