Doing some sonar-like things

Tweaks and stuff

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<String>? = 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()

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,20 +125,24 @@ 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 -> {
                             // above
                         }
 
                         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)

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)
     }
 

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 }

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()

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
 }