Skip to content

Commit

Permalink
Last bits before moving the main servo control to Marvin
Browse files Browse the repository at this point in the history
Running out of servo channels, plus the CRICKIT is a bit easier to test things with.

TODO can't move displays because the multiplexer uses the same I2C address as the servo controller and both are "solder closed" to change addresses.
  • Loading branch information
EAGrahamJr committed Apr 6, 2024
1 parent 3d32961 commit fa4a0e3
Show file tree
Hide file tree
Showing 11 changed files with 247 additions and 267 deletions.
110 changes: 110 additions & 0 deletions armthing/src/main/kotlin/crackers/kobots/app/RotoRegulator.kt
Original file line number Diff line number Diff line change
@@ -0,0 +1,110 @@
/*
* 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.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express
* or implied. See the License for the specific language governing
* permissions and limitations under the License.
*/

package crackers.kobots.app

/**
* Rotary encoder stuff.
*/
object RotoRegulator {
// val encoder by lazy { QwiicTwist(i2cMultiplexer.getI2CDevice(2, QwiicTwist.DEFAULT_I2C_ADDRESS)) }
//
// init {
// encoder.count = 0 // always start at zero
// encoder.pixel.brightness = 0.25f
// encoder.pixel + PURPLE
// encoder.colorConnection = Triple(0, 0, 0)
// }
//
// private var buttonDown = false
//
// fun setPixelColor() {
// encoder.pixel +
// when (currentMode) {
// Mode.NONE -> Color.BLACK
// Mode.NIGHT -> Color.RED
// Mode.MORNING -> ORANGISH
// Mode.DAYTIME -> GOLDENROD
// Mode.EVENING -> Color.BLUE.darker()
// Mode.MANUAL -> Color.GREEN
// }
// encoder.pixel.brightness =
// when (currentMode) {
// Mode.NIGHT -> 0.01f
// Mode.MORNING -> 0.05f
// Mode.DAYTIME -> 0.25f
// Mode.EVENING -> 0.05f
// else -> 0.01f
// }
// // TODO only do this when it's "adjusting" something
//// encoder.colorConnection = Triple(25, 25, 25)
// }
//
// val midRange = -2..2
// val lowRange = -100..-3
// val highRange = 3..100
//
// @Volatile
// private var adjustingEntityId: String? = null
// private var lastCount = 0
//
// fun readTwist() {
// // pressing the button allows switching to and from "manual" mode
// buttonDown =
// (encoder button PRESSED).also { pressed ->
// if (pressed && !buttonDown) {
// // killAllTheThings()
// currentMode = if (currentMode == Mode.MANUAL) Mode.NONE else Mode.MANUAL
// }
// }
//
// lastCount =
// encoder.count.let {
// if (currentMode == Mode.MANUAL && it > lastCount) {
// BackBenchPicker.updateMenu()
// FrontBenchPicker.updateMenu()
// }
// it
// }
// }
//
// fun mangageLight(
// entityId: String?,
// currentBrightness: Int,
// ) {
// if (entityId != null) {
// lastCount = encoder.count
// encoder.count = currentBrightness
// } else {
// encoder.count = lastCount
// }
// adjustingEntityId = entityId
// }
//
// fun adjustLight(count: Int) {
// try {
// val adjusted = count.coerceIn(0, 100)
// encoder.count = adjusted
// with(AppCommon.hasskClient) {
// light(adjustingEntityId!!.substringAfter("light.")) set adjusted
// }
// } catch (e: Exception) {
// // TODO screw it - take it out of manual mode
// currentMode = Mode.DAYTIME
// adjustingEntityId = null
// }
// }
}
Original file line number Diff line number Diff line change
Expand Up @@ -90,45 +90,44 @@ object ManualController : Startable {
if (selectDebounce) return

if (armSelected) {
armThing()
// armThing()
} else {
otherThing()
}
}

private fun otherThing() = with(gamepad) {
if (xButton) {
+thermoStepper
// +thermoStepper
} else if (bButton) {
-thermoStepper
// -thermoStepper
} else if (aButton) thermoStepper.reset()
}

private fun armThing() {
with(TheArm) {
// val xAxis = gamepad.xAxis
// if (gpZeroX == null) gpZeroX = xAxis
// else {
// val diff = gpZeroX!! - xAxis
// if (diff > 45f) waist -= 2
// if (diff < -45f) waist += 2
// }
val xAxis = gamepad.xAxis
if (gpZeroX == null) gpZeroX = xAxis
else {
val diff = gpZeroX!! - xAxis
// if (diff > 45f) +boomLink
// if (diff < -45f) -boomLink
}
val yAxis = gamepad.yAxis
if (gpZeroY == null) {
gpZeroY = yAxis
} else {
val diff = gpZeroY!! - yAxis
// if (diff > 45f) elevator.extendTo(100)
// if (diff < -45f) elevator.extendTo(0)
// elevator.release()
// if (diff > 45f) +armLink
// if (diff < -45f) -armLink
}
//
// if (gamepad.aButton) +extender
// if (gamepad.yButton) -extender

// if (gamepad.aButton) +swing
// if (gamepad.yButton) -swing
// if (gamepad.xButton) -gripper
// if (gamepad.bButton) +gripper
//
// updateCurrentState()

updateCurrentState()
}
}

Expand Down
10 changes: 5 additions & 5 deletions armthing/src/main/kotlin/crackers/kobots/app/arm/Messages.kt
Original file line number Diff line number Diff line change
@@ -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.
Expand Down Expand Up @@ -29,14 +29,14 @@ data class JointPosition(val angle: Int, val radius: Float = 0f)
*/
data class ArmPosition(
val waist: JointPosition,
val extender: JointPosition,
val boomLink: JointPosition,
val gripper: JointPosition,
val elbow: JointPosition
val armLink: JointPosition
) {
fun mapped(): Map<String, Int> = mapOf(
"WST" to waist.angle,
"XTN" to extender.angle,
"ELB" to elbow.angle,
"BM" to boomLink.angle,
"ARM" to armLink.angle,
"GRP" to gripper.angle
)
}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -14,57 +14,40 @@
* permissions and limitations under the License.
*/

package crackers.kobots.app.execution
package crackers.kobots.app.arm

import crackers.kobots.app.arm.TheArm.ELBOW_DOWN
import crackers.kobots.app.arm.TheArm.ELBOW_UP
import crackers.kobots.app.arm.TheArm.EXTENDER_FULL
import crackers.kobots.app.arm.TheArm.EXTENDER_HOME
import crackers.kobots.app.arm.TheArm.ARM_DOWN
import crackers.kobots.app.arm.TheArm.ARM_UP
import crackers.kobots.app.arm.TheArm.BOOM_UP
import crackers.kobots.app.arm.TheArm.GRIPPER_CLOSED
import crackers.kobots.app.arm.TheArm.GRIPPER_OPEN
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.SWING_HOME
import crackers.kobots.app.arm.TheArm.armLink
import crackers.kobots.app.arm.TheArm.boomLink
import crackers.kobots.app.arm.TheArm.gripper
import crackers.kobots.app.arm.TheArm.waist
import crackers.kobots.app.arm.TheArm.swing
import crackers.kobots.parts.movement.ActionSpeed
import crackers.kobots.parts.movement.sequence

/**
* Picks up stuff from the places and returns it.
*/
object PickUpAndMoveStuff {
val dropMover = MoveStuffAround(
closeOnItem = 93,
extenderToPickupTarget = 70,
elbowForPickupTarget = 10,
dropOffElbow = 0,
dropOffExtender = 0
)
val moveEyeDropsToDropZone = dropMover.moveObjectToTarget()
val returnDropsToStorage = dropMover.pickupAndReturn()
}

/**
* Wave hello kinda.
*/
val sayHi by lazy {
sequence {
name = "Say Hi"
action {
elbow rotate 45
extender goTo EXTENDER_FULL
armLink rotate 45
boomLink rotate 45
gripper goTo GRIPPER_OPEN
waist rotate 90
swing rotate 90
}
(1..4).forEach {
action {
elbow rotate ELBOW_UP
armLink rotate ARM_UP
requestedSpeed = ActionSpeed.FAST
}
action {
elbow rotate 30
armLink rotate 30
requestedSpeed = ActionSpeed.FAST
}
}
Expand All @@ -80,8 +63,8 @@ val excuseMe by lazy {
name = "Excuse Me"
this += homeSequence
action {
waist rotate 90
elbow rotate 45
swing rotate 90
armLink rotate 45
}
(1..5).forEach {
action {
Expand All @@ -104,15 +87,15 @@ val armSleep by lazy {
sequence {
name = "Go To Sleep"
this += homeSequence
action {
waist rotate WAIST_MAX
requestedSpeed = ActionSpeed.SLOW
}
action {
requestedSpeed = ActionSpeed.VERY_SLOW
elbow rotate ELBOW_DOWN
extender goTo 50
}
// action {
// waist rotate WAIST_MAX
// requestedSpeed = ActionSpeed.SLOW
// }
// action {
// requestedSpeed = ActionSpeed.VERY_SLOW
// elbow rotate ELBOW_DOWN
// extender goTo 50
// }
}
}

Expand All @@ -124,18 +107,12 @@ val homeSequence by lazy {
name = "Home"
// make sure the gripper is out of the way of anything
action {
requestedSpeed = ActionSpeed.SLOW
extender goTo EXTENDER_HOME
}
action {
requestedSpeed = ActionSpeed.SLOW
elbow rotate ELBOW_DOWN
boomLink rotate BOOM_UP
gripper goTo GRIPPER_OPEN
}
// now we can close and finish
action {
requestedSpeed = ActionSpeed.SLOW
waist rotate WAIST_HOME
gripper goTo GRIPPER_OPEN
armLink rotate ARM_DOWN
swing rotate SWING_HOME
}
}
}
Loading

0 comments on commit fa4a0e3

Please sign in to comment.