Skip to content

Commit

Permalink
Library updates and fixing up some angles
Browse files Browse the repository at this point in the history 
- Swing is correct now
- Added bucket to rotate gripper
  • Loading branch information
@EAGrahamJr
EAGrahamJr committed Apr 17, 2024
1 parent fb958fc commit 91ef4ba
Show file tree
Hide file tree
Showing 4 changed files with 100 additions and 61 deletions.
55 changes: 47 additions & 8 deletions servomatic/src/main/kotlin/crackers/kobots/app/HAJunk.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<Mode>(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)

Expand All @@ -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 {
Expand All @@ -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 {
Expand Down Expand Up @@ -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!!!
*/
Expand All @@ -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() {
}
}
28 changes: 19 additions & 9 deletions servomatic/src/main/kotlin/crackers/kobots/app/Predestination.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand All @@ -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
Expand All @@ -61,17 +67,21 @@ 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 {
boomLink rotate BOOM_UP
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
}
}
}

Expand Down
23 changes: 4 additions & 19 deletions servomatic/src/main/kotlin/crackers/kobots/app/ServoThing.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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 {
Expand All @@ -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()
}
}

Expand Down Expand Up @@ -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<Mode>(select)) {
Mode.IDLE -> {
}

Mode.STOP -> AppCommon.applicationRunning = false
Mode.CLUCK -> {
}

else -> logger.warn("No clue what to do with $select")
}
}
}
55 changes: 30 additions & 25 deletions servomatic/src/main/kotlin/crackers/kobots/app/SuzerainOfServos.kt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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
Expand All @@ -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)
}
}
}

0 comments on commit 91ef4ba

Please sign in to comment.