diff --git a/servomatic/src/main/kotlin/crackers/kobots/app/HAJunk.kt b/servomatic/src/main/kotlin/crackers/kobots/app/HAJunk.kt index aada8f6..4231cfe 100644 --- a/servomatic/src/main/kotlin/crackers/kobots/app/HAJunk.kt +++ b/servomatic/src/main/kotlin/crackers/kobots/app/HAJunk.kt @@ -20,27 +20,47 @@ import crackers.kobots.app.SuzerainOfServos.ARM_DOWN import crackers.kobots.app.SuzerainOfServos.ARM_UP import crackers.kobots.app.SuzerainOfServos.BOOM_DOWN import crackers.kobots.app.SuzerainOfServos.BOOM_UP +import crackers.kobots.app.SuzerainOfServos.BUCKET_HOME +import crackers.kobots.app.SuzerainOfServos.BUCKET_MAX import crackers.kobots.app.SuzerainOfServos.SWING_HOME import crackers.kobots.app.SuzerainOfServos.SWING_MAX import crackers.kobots.app.SuzerainOfServos.armLink import crackers.kobots.app.SuzerainOfServos.boomLink +import crackers.kobots.app.SuzerainOfServos.bucketLink import crackers.kobots.app.SuzerainOfServos.gripper import crackers.kobots.app.SuzerainOfServos.swing import crackers.kobots.mqtt.homeassistant.DeviceIdentifier import crackers.kobots.mqtt.homeassistant.KobotAnalogSensor import crackers.kobots.mqtt.homeassistant.KobotNumberEntity import crackers.kobots.mqtt.homeassistant.KobotSelectEntity -import crackers.kobots.parts.movement.ActionSpeed -import crackers.kobots.parts.movement.LinearActuator -import crackers.kobots.parts.movement.Rotator -import crackers.kobots.parts.movement.SequenceRequest +import crackers.kobots.parts.enumValue +import crackers.kobots.parts.movement.* import kotlin.math.roundToInt /** * HA entities, etc. */ object HAJunk : AutoCloseable { - val haIdentifier = DeviceIdentifier("Kobots", "Servomatic") + private val haIdentifier = DeviceIdentifier("Kobots", "Servomatic") + + private val selectHandler = object : KobotSelectEntity.Companion.SelectHandler { + override val options = Mode.entries.map { it.name } + override fun executeOption(select: String) { + when (enumValue(select)) { + Mode.IDLE -> { + } + + Mode.STOP -> AppCommon.applicationRunning = false + Mode.CLUCK -> { + } + + Mode.HOME -> SuzerainOfServos.handleRequest(SequenceRequest(Predestination.homeSequence)) + Mode.SAY_HI -> SuzerainOfServos.handleRequest(SequenceRequest(Predestination.sayHi)) + + else -> logger.warn("No clue what to do with $select") + } + } + } val commandSelectEntity = KobotSelectEntity(selectHandler, "servo_selector", "Servo Selector", haIdentifier) @@ -57,7 +77,7 @@ object HAJunk : AutoCloseable { crackers.kobots.parts.movement.sequence { name = "HA Move $thing" action { - requestedSpeed = ActionSpeed.SLOW + requestedSpeed = DefaultActionSpeed.SLOW rotator rotate target.roundToInt() } }.run { @@ -71,10 +91,10 @@ object HAJunk : AutoCloseable { override fun currentState() = linear.current().toFloat() override fun move(target: Float) { - crackers.kobots.parts.movement.sequence { + sequence { name = "HA Move $thing" action { - requestedSpeed = ActionSpeed.SLOW + requestedSpeed = DefaultActionSpeed.SLOW linear goTo target.roundToInt() } }.run { @@ -138,6 +158,16 @@ object HAJunk : AutoCloseable { haIdentifier ) {} + val bucketEntity = object : KobotNumberEntity( + ArmRotateHandler(bucketLink, "Bucket"), + "arm_bucket", + "Arm: Bucket", + haIdentifier, + min = BUCKET_HOME, + max = BUCKET_MAX, + unitOfMeasurement = "degrees" + ) {} + /** * LET'S LIGHT THIS THING UP!!! */ @@ -146,10 +176,19 @@ object HAJunk : AutoCloseable { boomEntity.start() armEntity.start() gripperEntity.start() + bucketEntity.start() commandSelectEntity.start() tofSensor.start() } + fun sendUpdatedStates() { + swingEntity.sendCurrentState() + boomEntity.sendCurrentState() + armEntity.sendCurrentState() + gripperEntity.sendCurrentState() + bucketEntity.sendCurrentState() + } + override fun close() { } } diff --git a/servomatic/src/main/kotlin/crackers/kobots/app/Predestination.kt b/servomatic/src/main/kotlin/crackers/kobots/app/Predestination.kt index 29d19bf..4faaa96 100644 --- a/servomatic/src/main/kotlin/crackers/kobots/app/Predestination.kt +++ b/servomatic/src/main/kotlin/crackers/kobots/app/Predestination.kt @@ -17,15 +17,18 @@ package crackers.kobots.app import crackers.kobots.app.SuzerainOfServos.ARM_DOWN -import crackers.kobots.app.SuzerainOfServos.ARM_UP import crackers.kobots.app.SuzerainOfServos.BOOM_UP +import crackers.kobots.app.SuzerainOfServos.BUCKET_HOME +import crackers.kobots.app.SuzerainOfServos.GRIPPER_CLOSED import crackers.kobots.app.SuzerainOfServos.GRIPPER_OPEN import crackers.kobots.app.SuzerainOfServos.SWING_HOME import crackers.kobots.app.SuzerainOfServos.armLink import crackers.kobots.app.SuzerainOfServos.boomLink +import crackers.kobots.app.SuzerainOfServos.bucketLink import crackers.kobots.app.SuzerainOfServos.gripper import crackers.kobots.app.SuzerainOfServos.swing -import crackers.kobots.parts.movement.ActionSpeed +import crackers.kobots.parts.movement.DefaultActionSpeed +import crackers.kobots.parts.movement.sequence /** * Pre-canned sequences, esp the home one. @@ -35,22 +38,25 @@ object Predestination { * Wave hello kinda. */ val sayHi by lazy { - crackers.kobots.parts.movement.sequence { + sequence { name = "Say Hi" action { armLink rotate 45 boomLink rotate 45 gripper goTo GRIPPER_OPEN swing rotate 90 + bucketLink rotate 45 } (1..4).forEach { action { - armLink rotate ARM_UP - requestedSpeed = ActionSpeed.FAST + gripper goTo GRIPPER_CLOSED + bucketLink rotate 30 + requestedSpeed = DefaultActionSpeed.FAST } action { - armLink rotate 30 - requestedSpeed = ActionSpeed.FAST + gripper goTo GRIPPER_OPEN + bucketLink rotate 45 + requestedSpeed = DefaultActionSpeed.FAST } } this += homeSequence @@ -61,7 +67,7 @@ object Predestination { * Send it home - should be "0" state for servos. */ val homeSequence by lazy { - crackers.kobots.parts.movement.sequence { + sequence { name = "Home" // make sure the gripper is out of the way of anything action { @@ -69,9 +75,13 @@ object Predestination { gripper goTo GRIPPER_OPEN } action { - armLink rotate ARM_DOWN + bucketLink rotate BUCKET_HOME + armLink rotate ARM_DOWN + 10 swing rotate SWING_HOME } + action { + armLink rotate ARM_DOWN + } } } diff --git a/servomatic/src/main/kotlin/crackers/kobots/app/ServoThing.kt b/servomatic/src/main/kotlin/crackers/kobots/app/ServoThing.kt index e96f876..461c810 100644 --- a/servomatic/src/main/kotlin/crackers/kobots/app/ServoThing.kt +++ b/servomatic/src/main/kotlin/crackers/kobots/app/ServoThing.kt @@ -19,11 +19,10 @@ package crackers.kobots.app import com.diozero.devices.Button import crackers.kobots.app.AppCommon.REMOTE_PI import crackers.kobots.app.AppCommon.mqttClient +import crackers.kobots.app.HAJunk.commandSelectEntity import crackers.kobots.app.SuzerainOfServos.INTERNAL_TOPIC -import crackers.kobots.mqtt.homeassistant.KobotSelectEntity import crackers.kobots.parts.app.KobotSleep import crackers.kobots.parts.app.publishToTopic -import crackers.kobots.parts.enumValue import crackers.kobots.parts.movement.ActionSequence import crackers.kobots.parts.movement.SequenceRequest import crackers.kobots.parts.scheduleWithFixedDelay @@ -40,7 +39,7 @@ import kotlin.time.Duration.Companion.milliseconds val logger = LoggerFactory.getLogger("Servomatic") enum class Mode { - IDLE, STOP, CLUCK, TEXT + IDLE, STOP, CLUCK, TEXT, HOME, SAY_HI } internal interface Startable { @@ -58,8 +57,10 @@ internal var systemState: SystemState get() = state.get() set(v) { if (v != state.get()) { + logger.warn("State is $v") state.set(v) // TODO trigger things? + commandSelectEntity.sendCurrentState() } } @@ -101,19 +102,3 @@ fun stopEverything() { } internal fun servoRequest(sequence: ActionSequence) = publishToTopic(INTERNAL_TOPIC, SequenceRequest(sequence)) - -internal val selectHandler = object : KobotSelectEntity.Companion.SelectHandler { - override val options = Mode.entries.map { it.name } - override fun executeOption(select: String) { - when (enumValue(select)) { - Mode.IDLE -> { - } - - Mode.STOP -> AppCommon.applicationRunning = false - Mode.CLUCK -> { - } - - else -> logger.warn("No clue what to do with $select") - } - } -} diff --git a/servomatic/src/main/kotlin/crackers/kobots/app/SuzerainOfServos.kt b/servomatic/src/main/kotlin/crackers/kobots/app/SuzerainOfServos.kt index 611b865..74bfc94 100644 --- a/servomatic/src/main/kotlin/crackers/kobots/app/SuzerainOfServos.kt +++ b/servomatic/src/main/kotlin/crackers/kobots/app/SuzerainOfServos.kt @@ -16,9 +16,11 @@ package crackers.kobots.app +import com.diozero.api.ServoTrim import com.diozero.devices.ServoController -import crackers.kobots.devices.MG90S_TRIM +import crackers.kobots.parts.movement.LimitedRotator.Companion.rotator import crackers.kobots.parts.movement.SequenceExecutor +import crackers.kobots.parts.movement.SequenceRequest import crackers.kobots.parts.movement.ServoLinearActuator import crackers.kobots.parts.movement.ServoRotator import java.util.concurrent.CountDownLatch @@ -39,13 +41,15 @@ object SuzerainOfServos : SequenceExecutor("Suzie", AppCommon.mqttClient), Start private lateinit var stopLatch: CountDownLatch override fun stop() { + // forces everything to stop + super.stop() + + logger.info("Setting latch") stopLatch = CountDownLatch(1) - // TODO request all things "home" + handleRequest(SequenceRequest(Predestination.homeSequence)) if (!stopLatch.await(30, TimeUnit.SECONDS)) { logger.error("Arm not homed in 30 seconds") } - // forces everything to stop - super.stop() } override fun canRun() = AppCommon.applicationRunning || ::stopLatch.isInitialized @@ -55,54 +59,55 @@ object SuzerainOfServos : SequenceExecutor("Suzie", AppCommon.mqttClient), Start } override fun postExecution() { - with(HAJunk) { - swingEntity.sendCurrentState() - boomEntity.sendCurrentState() - armEntity.sendCurrentState() - gripperEntity.sendCurrentState() - } + HAJunk.sendUpdatedStates() super.postExecution() systemState = SystemState.IDLE if (::stopLatch.isInitialized) stopLatch.countDown() } const val SWING_HOME = 0 - const val SWING_MAX = 140 + const val SWING_MAX = 133 const val BOOM_UP = 0 - const val BOOM_DOWN = 90 + const val BOOM_DOWN = 70 - const val ARM_DOWN = -53 - const val ARM_UP = 65 + const val ARM_DOWN = 0 + const val ARM_UP = 90 const val GRIPPER_OPEN = 0 const val GRIPPER_CLOSED = 100 + const val BUCKET_HOME = 0 + const val BUCKET_MAX = 180 + // hardware! ===================================================================================================== - private val maxServoRange = IntRange(0, 180) + private val maxServoRange = 0..ServoTrim.MG90S.maxAngle + val servo1 = hat.getServo(0, ServoTrim.MG90S, 0) val swing by lazy { - val servo = hat.getServo(0, MG90S_TRIM, 0) - val physicalRange = IntRange(SWING_HOME, SWING_MAX) - ServoRotator(servo, physicalRange, maxServoRange) + servo1.rotator(SWING_HOME..SWING_MAX, maxServoRange) } val boomLink by lazy { - val servo = hat.getServo(1, MG90S_TRIM, 0) - val physicalRange = IntRange(BOOM_UP, BOOM_DOWN) - ServoRotator(servo, physicalRange, maxServoRange) + hat.getServo(1, ServoTrim.MG90S, 0).rotator(BOOM_UP..BOOM_DOWN, 0..140) } val armLink by lazy { - val servo = hat.getServo(2, MG90S_TRIM, 0) - val physicalRange = IntRange(ARM_DOWN, ARM_UP) - ServoRotator(servo, physicalRange, maxServoRange) + hat.getServo(2, ServoTrim.MG90S, 0).rotator(ARM_DOWN..ARM_UP, maxServoRange) } val gripper by lazy { - val servo = hat.getServo(3, MG90S_TRIM, 0) + val servo = hat.getServo(3, ServoTrim.MG90S, 0) ServoLinearActuator(servo, 0f, 80f) } + + val bucketLink by lazy { + val servo = hat.getServo(4, ServoTrim.MG90S, 0) + val range = 0..BUCKET_MAX + object : ServoRotator(servo, range) { + override fun rotateTo(angle: Int) = (armLink.current() <= 5) || super.rotateTo(angle) + } + } }