From e92c4f24a0a994ffb1d3ebea39b314ae7bec2ad7 Mon Sep 17 00:00:00 2001 From: "E. A. Graham Jr" <10370165+EAGrahamJr@users.noreply.github.com> Date: Wed, 15 Nov 2023 14:44:59 -0800 Subject: [PATCH] Doing some sonar-like things Tweaks and stuff --- .../kotlin/crackers/kobots/app/Thingie.kt | 4 ++ .../crackers/kobots/app/arm/ArmMonitor.kt | 46 +++++++++++++------ .../kotlin/crackers/kobots/app/arm/TheArm.kt | 2 +- .../kobots/app/execution/MoveStuffAround.kt | 4 +- .../crackers/kobots/app/execution/QuickRun.kt | 4 +- .../crackers/kobots/app/execution/Sensei.kt | 41 ++++++++++++----- 6 files changed, 72 insertions(+), 29 deletions(-) diff --git a/armthing/src/main/kotlin/crackers/kobots/app/Thingie.kt b/armthing/src/main/kotlin/crackers/kobots/app/Thingie.kt index 86e8dd9..f86766e 100644 --- a/armthing/src/main/kotlin/crackers/kobots/app/Thingie.kt +++ b/armthing/src/main/kotlin/crackers/kobots/app/Thingie.kt @@ -25,6 +25,7 @@ import crackers.kobots.app.enviro.DieAufseherin import crackers.kobots.app.enviro.RosetteStatus import crackers.kobots.app.enviro.Segmenter import crackers.kobots.app.execution.homeSequence +import crackers.kobots.app.execution.toffle import crackers.kobots.devices.expander.CRICKITHat import org.slf4j.LoggerFactory import java.util.concurrent.atomic.AtomicBoolean @@ -53,6 +54,9 @@ fun main(args: Array? = null) { Segmenter.start() ManualController.start() + // fire off the sensor just to make sure it's working + toffle.distanceCm + crickitHat.use { // start all the things that require the CRICKIT TheArm.start() diff --git a/armthing/src/main/kotlin/crackers/kobots/app/arm/ArmMonitor.kt b/armthing/src/main/kotlin/crackers/kobots/app/arm/ArmMonitor.kt index 89224e4..60ff21a 100644 --- a/armthing/src/main/kotlin/crackers/kobots/app/arm/ArmMonitor.kt +++ b/armthing/src/main/kotlin/crackers/kobots/app/arm/ArmMonitor.kt @@ -50,7 +50,10 @@ private const val MAX_HT = 32 /** * Shows where the arm is on a timed basis. */ -object ArmMonitor : StatusColumnDisplay by StatusColumnDelegate(MAX_WD, MAX_HT) { +object ArmMonitor : StatusColumnDisplay by StatusColumnDelegate(MAX_WD, MAX_HT), KobotRadar by SimpleRadar( + Point(0, 0), + MAX_HT.toDouble() +) { private val logger = LoggerFactory.getLogger("ArmMonitor") private val screenGraphics: Graphics2D @@ -102,9 +105,9 @@ object ArmMonitor : StatusColumnDisplay by StatusColumnDelegate(MAX_WD, MAX_HT) } ) screenOnAt = Instant.now() + var lastMode = Mode.IDLE future = AppCommon.executor.scheduleWithFixedDelay(10.milliseconds, 10.milliseconds) { - var lastScanLocation = 0 whileRunning { // manual mode, show where the arm is at @@ -122,12 +125,16 @@ object ArmMonitor : StatusColumnDisplay by StatusColumnDelegate(MAX_WD, MAX_HT) } Mode.SCAN -> { + // just make sure the screen is on + if (lastMode != Mode.SCAN) { + screen.setDisplayOn(true) + clearImage() + } screenOnAt = Instant.now() - // if the waist location is greater than the last location, look to the right otherwise left - val current = TheArm.state.position.waist.angle - if (current > lastScanLocation) showEyes(LOOK_RIGHT) - else showEyes(LOOK_LEFT) - lastScanLocation = current + lastChanged = Instant.now() + // update the radar image + screenGraphics.paintRadar() + screen.display(screenImage) } Mode.MOVING -> { @@ -135,7 +142,7 @@ object ArmMonitor : StatusColumnDisplay by StatusColumnDelegate(MAX_WD, MAX_HT) } Mode.COMPLETED -> { - showEyes(Expression(lidPosition = Eye.LidPosition.ONE_QUARTER), true) + showEyes(Expression(lidPosition = Eye.LidPosition.ONE_QUARTER), lastMode != Mode.SCAN) mode = Mode.IDLE } @@ -147,13 +154,21 @@ object ArmMonitor : StatusColumnDisplay by StatusColumnDelegate(MAX_WD, MAX_HT) else -> mode = Mode.IDLE } } + lastMode = mode } } } - private fun showLastStatus() = with(screenGraphics) { + fun ping(angle: Int, distance: Float) { + // TODO make this not so hard-waired + // scale the distance based on percentage of max (25.5) + val scaled = MAX_HT * (distance / 25.5f) + updateScan(KobotRadar.RadarScan(angle + 45, scaled)) + } + + private fun showLastStatus() { // show last recorded status - displayStatuses(TheArm.state.position.mapped()) + screenGraphics.displayStatuses(TheArm.state.position.mapped()) screen.display(screenImage) } @@ -169,7 +184,8 @@ object ArmMonitor : StatusColumnDisplay by StatusColumnDelegate(MAX_WD, MAX_HT) * Show the eyes -- if time has elapsed for showing anything, turn off the screen unless forced. */ @Synchronized - fun showEyes(expression: Expression, force: Boolean = false) { + private fun showEyes(expression: Expression, force: Boolean = false) { + require(mode != Mode.SCAN) if (!force) { screenOnAt.elapsed().also { elapsed -> if (elapsed > TURN_OFF) { @@ -189,8 +205,7 @@ object ArmMonitor : StatusColumnDisplay by StatusColumnDelegate(MAX_WD, MAX_HT) if (force) screenOnAt = Instant.now() screen.setDisplayOn(true) - screenGraphics.color = Color.BLACK - screenGraphics.fillRect(0, 0, MAX_WD, MAX_HT) + clearImage() eyes(CannedExpressions.CLOSED.expression) drawEyes() @@ -201,6 +216,11 @@ object ArmMonitor : StatusColumnDisplay by StatusColumnDelegate(MAX_WD, MAX_HT) lastChanged = Instant.now() } + private fun clearImage() { + screenGraphics.color = Color.BLACK + screenGraphics.fillRect(0, 0, MAX_WD, MAX_HT) + } + private fun drawEyes() { eyes.draw(screenGraphics) diff --git a/armthing/src/main/kotlin/crackers/kobots/app/arm/TheArm.kt b/armthing/src/main/kotlin/crackers/kobots/app/arm/TheArm.kt index 329b2e3..72f032f 100644 --- a/armthing/src/main/kotlin/crackers/kobots/app/arm/TheArm.kt +++ b/armthing/src/main/kotlin/crackers/kobots/app/arm/TheArm.kt @@ -96,7 +96,7 @@ object TheArm : SequenceExecutor("TheArm", AppCommon.mqttClient) { } val waist by lazy { val servoRange = IntRange(0, 180) - val physicalRange = IntRange(0, 140) + val physicalRange = IntRange(0, 130) ServoRotator(waistServo, physicalRange, servoRange) } diff --git a/armthing/src/main/kotlin/crackers/kobots/app/execution/MoveStuffAround.kt b/armthing/src/main/kotlin/crackers/kobots/app/execution/MoveStuffAround.kt index 4757853..fab43d2 100644 --- a/armthing/src/main/kotlin/crackers/kobots/app/execution/MoveStuffAround.kt +++ b/armthing/src/main/kotlin/crackers/kobots/app/execution/MoveStuffAround.kt @@ -26,6 +26,8 @@ import crackers.kobots.parts.movement.sequence /** * Defines how to pick something up from a place and put it somewhere else, with the reverse also available. + * + * TODO rethink the approach and grab/drop motions: more interim steps? */ class MoveStuffAround( val closeOnItem: Int = 90, // how much to close the gripper to grab the item - this stresses the technic a bit @@ -70,7 +72,7 @@ class MoveStuffAround( // assumes something is in the gripper private val returnToPickupLocation = sequence { action { waist rotate waistForPickupTarget } -// action { elbow rotate elbowForPickupTarget + 25 } + action { elbow rotate elbowForPickupTarget + 25 } action { extender goTo extenderToPickupTarget } action { elbow rotate elbowForPickupTarget } action { gripper goTo GRIPPER_OPEN } diff --git a/armthing/src/main/kotlin/crackers/kobots/app/execution/QuickRun.kt b/armthing/src/main/kotlin/crackers/kobots/app/execution/QuickRun.kt index 25fa33b..d1814b8 100644 --- a/armthing/src/main/kotlin/crackers/kobots/app/execution/QuickRun.kt +++ b/armthing/src/main/kotlin/crackers/kobots/app/execution/QuickRun.kt @@ -37,10 +37,10 @@ import crackers.kobots.parts.movement.sequence object PickUpAndMoveStuff { val dropMover = MoveStuffAround( closeOnItem = 93, - extenderToPickupTarget = 80, + extenderToPickupTarget = 70, elbowForPickupTarget = 10, dropOffElbow = 0, - dropOffExtender = 15 + dropOffExtender = 0 ) val moveEyeDropsToDropZone = dropMover.moveObjectToTarget() val returnDropsToStorage = dropMover.pickupAndReturn() diff --git a/armthing/src/main/kotlin/crackers/kobots/app/execution/Sensei.kt b/armthing/src/main/kotlin/crackers/kobots/app/execution/Sensei.kt index 6a9ca20..05a9d2b 100644 --- a/armthing/src/main/kotlin/crackers/kobots/app/execution/Sensei.kt +++ b/armthing/src/main/kotlin/crackers/kobots/app/execution/Sensei.kt @@ -16,7 +16,8 @@ package crackers.kobots.app.execution -import crackers.kobots.app.arm.TheArm.elbow +import crackers.kobots.app.arm.ArmMonitor +import crackers.kobots.app.arm.TheArm.waist import crackers.kobots.app.arm.TheArm.waistServo import crackers.kobots.devices.at import crackers.kobots.devices.sensors.VL6180X @@ -35,24 +36,40 @@ val simpleScan by lazy { sequence { name = "Simple Scan" this += homeSequence - // drop the arm horizontally - action { elbow rotate 0 } // sweep the waist to 90 degrees, one degree at a time, then back to 0 the same way action { - execute { - for (i in 0..140 step 2) snapShot(i) - for (i in 140 downTo 0 step 2) snapShot(i) - true +// execute { runWithServo() } + waist forwardUntil { + KobotSleep.millis(5) + println("Angle ${waistServo.angle} waist ${waist.current()}- ${toffle.distance()}") + ArmMonitor.ping(waist.current(), toffle.distance()) + KobotSleep.millis(15) + waist.current() == 90 + } + } + action { + waist backwardUntil { + KobotSleep.millis(5) + println("Angle ${waistServo.angle} waist ${waist.current()}- ${toffle.distance()}") + ArmMonitor.ping(waist.current(), toffle.distance()) + KobotSleep.millis(15) + waist.current() == 0 } } this += homeSequence } } -private fun snapShot(i: Int) { - waistServo at i - KobotSleep.millis(5) - println("Angle $i - ${toffle.distance()}") - KobotSleep.millis(15) +private fun runWithServo(): Boolean { + fun snapShot(i: Int) { + waistServo at i + KobotSleep.millis(5) + println("Angle $i waist ${waist.current()}- ${toffle.distance()}") + KobotSleep.millis(15) + } + + for (i in 0..140 step 2) snapShot(i) + for (i in 140 downTo 0 step 2) snapShot(i) + return true }