Get rest of arm working and hooked up.

Supposedly changed steppers for thermometer again.
EAGrahamJr · Mar 28, 2024 · 3d32961 · 3d32961
1 parent 62bcd8f
commit 3d32961
Show file tree

Hide file tree

Showing 5 changed files with 114 additions and 82 deletions.
diff --git a/armthing/src/main/kotlin/crackers/kobots/app/arm/TheArm.kt b/armthing/src/main/kotlin/crackers/kobots/app/arm/TheArm.kt
@@ -24,7 +24,6 @@ import crackers.kobots.app.enviro.DieAufseherin
 import crackers.kobots.app.enviro.HAStuff
 import crackers.kobots.app.execution.homeSequence
 import crackers.kobots.devices.MG90S_TRIM
-import crackers.kobots.devices.at
 import crackers.kobots.parts.app.publishToTopic
 import crackers.kobots.parts.movement.*
 import java.util.concurrent.CountDownLatch
@@ -41,11 +40,11 @@ object TheArm : SequenceExecutor("TheArm", AppCommon.mqttClient), Startable {
         publishToTopic(REQUEST_TOPIC, SequenceRequest(sequence))
     }
 
-    const val ELBOW_UP = 60
+    const val ELBOW_UP = 80
     const val ELBOW_DOWN = 0
 
     const val WAIST_HOME = 0
-    const val WAIST_MAX = 180
+    const val WAIST_MAX = 140
 
     const val EXTENDER_HOME = 0
     const val EXTENDER_FULL = 100
@@ -62,45 +61,32 @@ object TheArm : SequenceExecutor("TheArm", AppCommon.mqttClient), Startable {
 
     // hardware! =====================================================================================================
 
-    val gripper by lazy {
-        // traverse gears
-        val _OPEN = 0f
-        val _CLOSE = 65f
-
-        val servo = crickitHat.servo(3, MG90S_TRIM).apply {
-            this at _CLOSE
-        }
-
-        // 0% == CLOSE, 100% == OPEN
-        ServoLinearActuator(servo, _OPEN, _CLOSE)
-    }
-
     val extender by lazy {
-        val _IN = 0f
-        val _OUT = 120f
-        val servo = crickitHat.servo(1, MG90S_TRIM).apply {
-            this at _IN
-        }
-        ServoLinearActuator(servo, _IN, _OUT)
+        val servo = crickitHat.servo(1, MG90S_TRIM).apply { angle = 0f }
+        ServoLinearActuator(servo, 0f, 120f)
     }
 
     val elbow by lazy {
-        val servo2 = crickitHat.servo(2, MG90S_TRIM).apply {
-            this at 0
-        }
+        val servo = crickitHat.servo(2, MG90S_TRIM).apply { angle = 0f }
+
         val physicalRange = IntRange(ELBOW_DOWN, ELBOW_UP)
-        val servoRange = IntRange(0, 145)
-        ServoRotator(servo2, physicalRange, servoRange)
+        val servoRange = IntRange(0, 115)
+
+        ServoRotator(servo, physicalRange, servoRange)
     }
 
-    val waistServo by lazy {
-        crickitHat.servo(4, MG90S_TRIM).apply { this at 0 }
+    val gripper by lazy {
+        val servo = crickitHat.servo(3, MG90S_TRIM).apply { angle = 0f }
+        ServoLinearActuator(servo, 0f, 80f)
     }
 
-    val waistRange = IntRange(0, 180) // TODO check this
     val waist by lazy {
-        val servoRange = IntRange(0, 144)
-        ServoRotator(waistServo, waistRange, servoRange)
+        val servo = crickitHat.servo(4, MG90S_TRIM).apply { angle = 0f }
+
+        val physicalRange = IntRange(WAIST_HOME, WAIST_MAX) // TODO check this
+        val servoRange = IntRange(0, 180)
+
+        ServoRotator(servo, physicalRange, servoRange)
     }
 
     // manage the state of this construct =============================================================================
@@ -144,6 +130,8 @@ object TheArm : SequenceExecutor("TheArm", AppCommon.mqttClient), Startable {
             noodSwitch.handleCommand("OFF")
             waistEntity.sendCurrentState()
             extenderEntity.sendCurrentState()
+            elbowEntity.sendCurrentState()
+            gripperEntity.sendCurrentState()
         }
         // just in case, release steppers
         DieAufseherin.currentMode = DieAufseherin.SystemMode.IDLE

diff --git a/armthing/src/main/kotlin/crackers/kobots/app/enviro/DieAufseherin.kt b/armthing/src/main/kotlin/crackers/kobots/app/enviro/DieAufseherin.kt
@@ -20,12 +20,11 @@ import crackers.kobots.app.AppCommon
 import crackers.kobots.app.Startable
 import crackers.kobots.app.arm.TheArm
 import crackers.kobots.app.display.DisplayDos
-import crackers.kobots.app.enviro.HAStuff.extenderEntity
 import crackers.kobots.app.enviro.HAStuff.noodSwitch
 import crackers.kobots.app.enviro.HAStuff.rosetteStrand
 import crackers.kobots.app.enviro.HAStuff.selector
-import crackers.kobots.app.enviro.HAStuff.textDosEntity
-import crackers.kobots.app.enviro.HAStuff.waistEntity
+import crackers.kobots.app.enviro.HAStuff.startDevices
+import crackers.kobots.app.execution.homeSequence
 import crackers.kobots.app.execution.sayHi
 import crackers.kobots.parts.movement.SequenceRequest
 import org.slf4j.LoggerFactory
@@ -51,8 +50,13 @@ object DieAufseherin : Startable {
         set(v) {
             logger.info("System mode changed to $v")
             theMode.set(v)
+            if (v == SystemMode.IDLE) resetThings()
         }
 
+    private fun resetThings() {
+        selector.sendCurrentState("None")
+    }
+
     enum class GripperActions {
         HOME, SAY_HI, STOP, CLUCK, MANUAL, RANDOM_EYES
     }
@@ -98,7 +102,7 @@ object DieAufseherin : Startable {
     internal fun actionTime(payload: GripperActions?) {
         when (payload) {
             GripperActions.STOP -> AppCommon.applicationRunning = false
-            GripperActions.HOME -> currentMode = SystemMode.IDLE
+            GripperActions.HOME -> TheArm.handleRequest(SequenceRequest(homeSequence))
             GripperActions.MANUAL -> currentMode = SystemMode.MANUAL
             GripperActions.CLUCK -> DisplayDos.cluck()
             GripperActions.RANDOM_EYES -> DisplayDos.randomEye()
@@ -108,13 +112,4 @@ object DieAufseherin : Startable {
         }
     }
 
-    private fun startDevices() {
-        // HA stuff
-        noodSwitch.start()
-        selector.start()
-        rosetteStrand.start()
-        textDosEntity.start()
-        waistEntity.start()
-        extenderEntity.start()
-    }
 }
diff --git a/armthing/src/main/kotlin/crackers/kobots/app/enviro/HAStuff.kt b/armthing/src/main/kotlin/crackers/kobots/app/enviro/HAStuff.kt
@@ -17,16 +17,20 @@
 package crackers.kobots.app.enviro
 
 import crackers.kobots.app.arm.TheArm
+import crackers.kobots.app.arm.TheArm.ELBOW_DOWN
+import crackers.kobots.app.arm.TheArm.ELBOW_UP
+import crackers.kobots.app.arm.TheArm.WAIST_HOME
+import crackers.kobots.app.arm.TheArm.WAIST_MAX
+import crackers.kobots.app.arm.TheArm.elbow
 import crackers.kobots.app.arm.TheArm.extender
+import crackers.kobots.app.arm.TheArm.gripper
 import crackers.kobots.app.arm.TheArm.waist
-import crackers.kobots.app.arm.TheArm.waistRange
 import crackers.kobots.app.crickitHat
 import crackers.kobots.app.display.DisplayDos
+import crackers.kobots.app.enviro.DieAufseherin.currentMode
 import crackers.kobots.mqtt.homeassistant.*
 import crackers.kobots.parts.enumValue
-import crackers.kobots.parts.movement.ActionSpeed
-import crackers.kobots.parts.movement.SequenceRequest
-import crackers.kobots.parts.movement.sequence
+import crackers.kobots.parts.movement.*
 import kotlin.math.roundToInt
 
 object HAStuff {
@@ -70,7 +74,15 @@ object HAStuff {
             DieAufseherin.actionTime(payloadToEnum)
         }
     }
-    val selector = KobotSelectEntity(selectorHandler, "arm_thing", "Arm Thing", deviceIdentifier = haIdentifier)
+    val selector = object : KobotSelectEntity(
+        selectorHandler, "arm_thing", "Arm Thing", deviceIdentifier =
+        haIdentifier
+    ) {
+        override fun sendCurrentState(state: String) {
+            if (currentMode != DieAufseherin.SystemMode.IDLE) super.sendCurrentState(currentState())
+            else super.sendCurrentState("None")
+        }
+    }
 
     /**
      * Run the 8-bit circular NeoPixel thing
@@ -84,59 +96,60 @@ object HAStuff {
      */
     val textDosEntity = KobotTextEntity(DisplayDos::text, "second_display", "Dos Display", haIdentifier)
 
-    /**
-     * Turn it sideways
-     */
-    private val waistHandler = object : KobotNumberEntity.Companion.NumberHandler {
-        override fun currentState() = waist.current().toFloat()
+    private class ArmRotateHandler(val rotator: Rotator, val thing: String) :
+        KobotNumberEntity.Companion.NumberHandler {
+        override fun currentState() = rotator.current().toFloat()
 
         override fun move(target: Float) {
             sequence {
-                name = "HA Move Waist"
+                name = "HA Move $thing"
                 action {
                     requestedSpeed = ActionSpeed.SLOW
-                    waist rotate target.roundToInt()
+                    rotator rotate target.roundToInt()
                 }
             }.let { seq ->
                 TheArm.handleRequest(SequenceRequest(seq))
             }
         }
     }
 
-    val waistEntity = object : KobotNumberEntity(
-        waistHandler,
-        "arm_waist",
-        "Arm: Waist",
-        haIdentifier,
-        min = waistRange.start,
-        max = waistRange.endInclusive,
-        mode = Companion.DisplayMode.box,
-        unitOfMeasurement = "degrees"
-    ) {
-        override val icon = "mdi:rotate-360"
-    }
-
-    /**
-     * In and out
-     */
-    private val extenderHandler = object : KobotNumberEntity.Companion.NumberHandler {
-        override fun currentState() = extender.current().toFloat()
+    private class PctHandler(val linear: LinearActuator, val thing: String) :
+        KobotNumberEntity.Companion.NumberHandler {
+        override fun currentState() = linear.current().toFloat()
 
         override fun move(target: Float) {
             sequence {
-                name = "HA Move Extender"
+                name = "HA Move $thing"
                 action {
                     requestedSpeed = ActionSpeed.SLOW
-                    extender goTo target.toInt()
+                    linear goTo target.roundToInt()
                 }
             }.let { seq ->
                 TheArm.handleRequest(SequenceRequest(seq))
             }
         }
     }
 
+    /**
+     * Turn it sideways
+     */
+    val waistEntity = object : KobotNumberEntity(
+        ArmRotateHandler(waist, "Waist"),
+        "arm_waist",
+        "Arm: Waist",
+        haIdentifier,
+        min = WAIST_HOME,
+        max = WAIST_MAX,
+        unitOfMeasurement = "degrees"
+    ) {
+        override val icon = "mdi:rotate-360"
+    }
+
+    /**
+     * In and oot
+     */
     val extenderEntity = object : KobotNumberEntity(
-        extenderHandler,
+        PctHandler(extender, "Extender"),
         "arm_extender",
         "Arm: Extend",
         haIdentifier,
@@ -145,4 +158,41 @@ object HAStuff {
     ) {
         override val icon = "mdi:hand-extended"
     }
+
+    /**
+     * Hup down
+     */
+    val elbowEntity = object : KobotNumberEntity(
+        ArmRotateHandler(elbow, "Elbow"),
+        "arm_elbow",
+        "Arm: Elbow",
+        haIdentifier,
+        min = ELBOW_DOWN,
+        max = ELBOW_UP,
+        unitOfMeasurement = "degrees"
+    ) {
+        override val icon = "mdi:horizontal-rotate-clockwise"
+    }
+
+    /**
+     * Grabby thing
+     */
+    val gripperEntity = object : KobotNumberEntity(
+        PctHandler(gripper, "Gripper"),
+        "arm_gripper",
+        "Arm: Gripper",
+        haIdentifier
+    ) {}
+
+    internal fun startDevices() {
+        // HA stuff
+        noodSwitch.start()
+        selector.start()
+        rosetteStrand.start()
+        textDosEntity.start()
+        waistEntity.start()
+        extenderEntity.start()
+        elbowEntity.start()
+        gripperEntity.start()
+    }
 }
diff --git a/armthing/src/main/kotlin/crackers/kobots/app/enviro/VeryDumbThermometer.kt b/armthing/src/main/kotlin/crackers/kobots/app/enviro/VeryDumbThermometer.kt
@@ -28,8 +28,8 @@ import org.tinylog.Logger
  */
 object VeryDumbThermometer {
     val thermoStepper by lazy {
-        val stepper = BasicStepperMotor(2048, crickitHat.unipolarStepperPort())
-        BasicStepperRotator(stepper, gearRatio = 1.28f, reversed = true)
+        val stepper = BasicStepperMotor(200, crickitHat.motorStepperPort())
+        BasicStepperRotator(stepper, gearRatio = 1f, reversed = true)
     }
 
     private const val DEGREES_TO_ANGLES = 18f // this comes out to 5 degree temp change == 90 degree stepper change

diff --git a/armthing/src/test/kotlin/crackers/kobots/ImageConvert.kt b/armthing/src/test/kotlin/crackers/kobots/ImageConvert.kt
@@ -1,5 +1,5 @@
 /*
- * Copyright 2022-2023 by E. A. Graham, Jr.
+ * Copyright 2022-2024 by E. A. Graham, Jr.
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
@@ -55,7 +55,6 @@ fun main() {
     }
 }
 
-
 fun BufferedImage.invert(): BufferedImage {
     for (x in 0 until width) {
         for (y in 0 until height) {