diff --git a/Movements.md b/Movements.md index f68ea1f..7b098c2 100644 --- a/Movements.md +++ b/Movements.md @@ -20,7 +20,6 @@ The sequences and actions are contained within a type-safe DSL. Actions and move Actuators have a small set of _operators_ that are supported: - `+=` and `-+` for relative changes -- `+a` and `-a` (unary plus/minus) for increment and decrement - `%` on a `LinearActuator` for positioning Each `Actuator` type may also contain specific `infix` capable functions for more DSL-like behavior. Note that these functions are "direct drive" and should immediately affect the device. @@ -40,12 +39,17 @@ This `Rotator` uses stepper motors to move to a position by keeping track of "st - "zero" position is entirely arbitrary and is assumed to be pre-calibrated - there are no angular limits imposed, so over-rotation is a possibility +#### LimitedRotator + +A basic interface that defines a rotator with a _physical limit_. This is usually applied to servo motors with limited rotation spans. This class also defines some _extension_ functions to directly create `ServoRotator` objects from `ServoDevice`s. + #### ServoRotator -Models the attachment to a **non-continuous** servo. The physical movement is "mapped" to the servo's angular displacement. +Models the attachment to a **non-continuous** servo. The physical movement is "mapped" to the servo's angular displacement. Accuracy is determined by the gear-ratio: sufficiently large ratios can cause physical angles to be mapped to the same servo angle, resutling in the servo not moving between them -- due to rounding errors, some _physical_ angles may cause servo issues (e.g. jitter) -- some angles may not be reachable (e.g. if the gear ratios work out to a servo being set to a _partial_ angle, it may not actually move) +- mapping is calcuated on object creation and cannot be changed +- overly large servo deltas _may_ cause the servo to ignore smaller movements +- the _current_ position is the closest approximation from the mapping table ### Linear diff --git a/src/main/kotlin/crackers/kobots/parts/app/KobotSleep.kt b/src/main/kotlin/crackers/kobots/parts/app/KobotSleep.kt index dbc9250..2c6c93d 100644 --- a/src/main/kotlin/crackers/kobots/parts/app/KobotSleep.kt +++ b/src/main/kotlin/crackers/kobots/parts/app/KobotSleep.kt @@ -20,7 +20,8 @@ import com.diozero.util.SleepUtil import java.time.Duration /** - * Shortcuts for sleeping, wrapping the `diozero` utils. + * Shortcuts for sleeping, wrapping the `diozero` utils. Sleeps should be used + * **very** judiciously. */ object KobotSleep { fun nanos(nanos: Long) { @@ -42,4 +43,8 @@ object KobotSleep { fun duration(d: Duration) { nanos(d.toNanos()) } + + infix fun of(d: kotlin.time.Duration) { + nanos(d.inWholeNanoseconds) + } } diff --git a/src/main/kotlin/crackers/kobots/parts/movement/ActionSequence.kt b/src/main/kotlin/crackers/kobots/parts/movement/ActionSequence.kt index 582d27c..24927cc 100644 --- a/src/main/kotlin/crackers/kobots/parts/movement/ActionSequence.kt +++ b/src/main/kotlin/crackers/kobots/parts/movement/ActionSequence.kt @@ -16,6 +16,9 @@ package crackers.kobots.parts.movement +import kotlin.time.DurationUnit +import kotlin.time.toDuration + /** * Base parts for all movements. */ @@ -68,6 +71,7 @@ private class ExecutableMovementBuilder(private val function: () -> Boolean) : interface ActionSpeed { val millis: Long + fun duration() = millis.toDuration(DurationUnit.MILLISECONDS) } enum class DefaultActionSpeed(override val millis: Long) : ActionSpeed { @@ -150,7 +154,7 @@ class ActionBuilder { } /** - * Add another's movements to this. + * Add another builder's movements to this. */ operator fun plusAssign(otherBuilder: ActionBuilder) { steps += otherBuilder.steps @@ -213,3 +217,8 @@ class ActionSequence { * Typesafe "builder" (DSL) for creating a sequence of actions. */ fun sequence(init: ActionSequence.() -> Unit): ActionSequence = ActionSequence().apply(init) + +/** + * Typesafe "builder" (DSL) for just the actions. + */ +fun action(init: ActionBuilder.() -> Unit): ActionBuilder = ActionBuilder().apply(init) diff --git a/src/main/kotlin/crackers/kobots/parts/movement/Rotator.kt b/src/main/kotlin/crackers/kobots/parts/movement/Rotator.kt index b43b4c9..7bdf155 100644 --- a/src/main/kotlin/crackers/kobots/parts/movement/Rotator.kt +++ b/src/main/kotlin/crackers/kobots/parts/movement/Rotator.kt @@ -23,9 +23,8 @@ import com.diozero.devices.sandpit.motor.StepperMotorInterface.Direction.BACKWAR import com.diozero.devices.sandpit.motor.StepperMotorInterface.Direction.FORWARD import crackers.kobots.devices.at import crackers.kobots.parts.movement.LimitedRotator.Companion.MAX_ANGLE +import java.util.* import kotlin.math.abs -import kotlin.math.max -import kotlin.math.min import kotlin.math.roundToInt /** @@ -55,11 +54,6 @@ interface Rotator : Actuator { * Current location. */ fun current(): Int - - /** - * Determine if this float is "almost" equal to [another], within the given [wibble]. - */ - fun Float.almostEquals(another: Float, wibble: Float): Boolean = abs(this - another) < wibble } /** @@ -81,32 +75,44 @@ open class BasicStepperRotator( private val maxSteps: Float private val degreesToSteps: Map + private val stepsToDegrees = mutableMapOf>() + init { require(gearRatio > 0f) { "gearRatio '$gearRatio' must be greater than zero." } maxSteps = theStepper.stepsPerRotation / gearRatio // calculate how many steps off of "zero" each degree is - degreesToSteps = (0..359).map { - it to (maxSteps * it / 360).roundToInt() + degreesToSteps = (0..359).map { deg: Int -> + val steps = (maxSteps * deg / 360).roundToInt() + stepsToDegrees.compute(steps) { _, v -> + val theList = v ?: mutableListOf() + theList += deg + theList + } + deg to steps }.toMap() } private val forwardDirection = if (reversed) BACKWARD else FORWARD private val backwardDirection = if (reversed) FORWARD else BACKWARD + /** + * Pass through to release the stepper when it's not directly available. + */ open fun release() = theStepper.release() private var stepsLocation: Int = 0 private var angleLocation: Int = 0 - override fun current(): Int = (360 * stepsLocation / maxSteps).roundToInt() + override fun current(): Int = angleLocation override fun rotateTo(angle: Int): Boolean { // first check to see if the angles already match if (angleLocation == angle) return true // find out where we're supposed to be for steps - val destinationSteps = degreesToSteps[abs(angle % 360)]!! * (if (angle < 0) -1 else 1) + val realAngle = abs(angle % 360) + val destinationSteps = degreesToSteps[realAngle]!! // and if steps match, angles match and everything is good if (destinationSteps == stepsLocation) { angleLocation = angle @@ -117,9 +123,15 @@ open class BasicStepperRotator( if (destinationSteps < stepsLocation) { stepsLocation-- theStepper.step(backwardDirection, stepStyle) + stepsToDegrees[stepsLocation]?.also { anglesForStep -> + angleLocation = if (stepsLocation !in anglesForStep) anglesForStep.max() else anglesForStep.min() + } } else { stepsLocation++ theStepper.step(forwardDirection, stepStyle) + stepsToDegrees[stepsLocation]?.also { anglesForStep -> + angleLocation = if (stepsLocation !in anglesForStep) anglesForStep.min() else anglesForStep.max() + } } // are we there yet? return (destinationSteps == stepsLocation).also { @@ -165,7 +177,7 @@ interface LimitedRotator : Rotator { fun ServoDevice.rotator( physicalRange: IntRange, servoRange: IntRange, - deltaDegrees: Int? = 1 + deltaDegrees: Int = 1 ): LimitedRotator = ServoRotator(this, physicalRange, servoRange, deltaDegrees) const val MAX_ANGLE = Int.MAX_VALUE @@ -174,30 +186,24 @@ interface LimitedRotator : Rotator { } /** - * Servo, with software limits to prevent over-rotation. Each "step" is controlled by the `deltaDegrees` parameter - * (`null` means absolute movement, default 1 degree per "step"). Servo movement is done in "whole degrees" (int), so - * there may be some small rounding errors. + * Servo, with software limits to prevent over-rotation. * - * The `realRange` is the range of the servo _target_ in degrees, and the [servoRange] is the range of the servo + * The [physicalRange] is the range of the servo _target_ in degrees, and the [servoRange] is the range of the servo * itself, in degrees. For example, a servo that rotates its target 180 degrees, but only requires a 90-degree - * rotation of the servo, would have a `realRange` of `0..180` and a [servoRange] of `45..135`. This allows for gear - * ratios and other mechanical linkages. Both ranges must be _increasing_ (systems can flip the values if the motor - * is inverted to a frame of reference). - * - * Note that the target **may** not be exact, due to rounding errors and if the [delta] is large. Example: - * ``` - * delta = 5 - * current = 47 - * target = 50 - * ``` - * This would indicate that the servo _might not_ move, as the target is within the delta given. + * rotation of the servo, would have a [physicalRange] of `0..180` and a [servoRange] of `45..135`. This allows for gear + * ratios and other mechanical linkages. Note that if the [servoRange] is _inverted_, the servo will rotate backwards + * relative to a positive angle. * + * Each "step" is controlled by the [delta] parameter (degrees to move the servo, default `1`). Since this movement is + * done in "whole degrees" (int), there will be rounding errors, especially if the gear ratios are not 1:1. Note that + * using a `[delta] != 1` **may** cause the servo to "seek" when the ranges do not align well and _that_ may + * also cause the servo to never actually reach the target. */ open class ServoRotator( private val theServo: ServoDevice, - realRange: IntRange, + final override val physicalRange: IntRange, private val servoRange: IntRange, - deltaDegrees: Int? = 1 + private val delta: Int = 1 ) : LimitedRotator { /** @@ -211,44 +217,35 @@ open class ServoRotator( 1 ) - private val delta: Float? = if (deltaDegrees == null) null else abs(deltaDegrees).toFloat() + // map physical degrees to where the servo should be + private val degreesToServo: SortedMap - private val PRECISION = 0.1f + init { + require(physicalRange.first < physicalRange.last) { "physicalRange '$physicalRange' must be increasing" } - private val servoLowerLimit: Float - private val servoUpperLimit: Float - private val gearRatio: Float - override val physicalRange = run { - require(realRange.first < realRange.last) { "physicalRange '$realRange' must be increasing" } - realRange - } + val physicalScope = physicalRange.last - physicalRange.first + val servoScope = servoRange.last - servoRange.first - init { - with(servoRange) { - servoLowerLimit = min(first, last).toFloat() - servoUpperLimit = max(first, last).toFloat() - } - gearRatio = (realRange.last - realRange.first).toFloat() / (servoRange.last - servoRange.first).toFloat() - } + degreesToServo = physicalRange.associateWith { angle -> + val normalizedPosition = (angle - physicalRange.first).toDouble() / physicalScope - // translate an angle in the physical range to a servo angle - private fun translate(angle: Int): Float { - val physicalOffset = angle - physicalRange.first - val servoOffset = physicalOffset / gearRatio - return servoOffset + servoRange.first + val servo = (servoRange.first + normalizedPosition * servoScope).roundToInt() + servo + }.toSortedMap() } - // report the current angle, translated to the physical range - override fun current(): Int = theServo.angle.let { - val servoOffset = it - servoRange.first - val physicalOffset = servoOffset * gearRatio - physicalOffset + physicalRange.first - }.roundToInt() + private val availableDegrees = degreesToServo.keys.toList() + + internal var where = physicalRange.first + override fun current(): Int = where /** * Figure out if we need to move or not (and how much) */ override fun rotateTo(angle: Int): Boolean { + val now = current() + if (angle == now) return true + // special case -- if the target is MAXINT, use the physical range as the target // NOTE -- this may cause the servo to jitter or not move at all if (abs(angle) == MAX_ANGLE) { @@ -258,44 +255,39 @@ open class ServoRotator( // angle must be in the physical range require(angle in physicalRange) { "Angle '$angle' is not in physical range '$physicalRange'." } - val currentAngle = theServo.angle - val targetAngle = translate(angle) + // find the "next" angle from now based on where the target is + val nowKeyIndex = availableDegrees.indexOf(now) + val nextAngleIndex = (if (angle < now) nowKeyIndex - 1 else nowKeyIndex + 1) + .coerceIn(0, availableDegrees.size - 1) - // this is an absolute move without any steps, so set it up and fire it - if (delta == null) { - if (reachedTarget(currentAngle, targetAngle, PRECISION)) return true - - // move to the target (trimmed) and we're done - theServo at trimTargetAngle(targetAngle) + val nextAngle = availableDegrees[nextAngleIndex] + // do not move beyond the requested angle + if (nextAngle < angle && angle < now) { return true + } else if (nextAngle > angle && angle > now) return true + + // so where the hell are we going? + val nextServoAngle = degreesToServo[nextAngle]!! + val currentServoAngle = theServo.angle.roundToInt() + + val nextServoTarget: Int + if (nextServoAngle < currentServoAngle) { + val t = currentServoAngle - delta + if (t < nextServoAngle) return true // can't move + nextServoTarget = t + } else if (nextServoAngle > currentServoAngle) { + val t = currentServoAngle + delta + if (t > nextServoAngle) return true // can't move + nextServoTarget = t + } else { + nextServoTarget = currentServoAngle } - // check to see if within the target - if (targetAngle.almostEquals(currentAngle, delta)) return true + theServo at nextServoTarget - // apply the delta and make sure it doesn't go out of range (technically it shouldn't) - val nextAngle = trimTargetAngle( - if (currentAngle > targetAngle) currentAngle - delta else currentAngle + delta - ) + // if we moved to a mapped target, update the position + if (nextServoTarget == nextServoAngle) where = nextAngle - // move it and re-check - theServo at nextAngle - return reachedTarget(theServo.angle, targetAngle, delta) - } - - // determine if the servo is at the target angle or at either of the limits - private fun reachedTarget(servoCurrent: Float, servoTarget: Float, delta: Float): Boolean { - return servoCurrent.almostEquals(servoTarget, delta) || - servoCurrent.almostEquals(servoLowerLimit, PRECISION) || - servoCurrent.almostEquals(servoUpperLimit, PRECISION) - } - - // limit the target angle to the servo limits - private fun trimTargetAngle(targetAngle: Float): Float { - return when { - targetAngle < servoLowerLimit -> servoLowerLimit - targetAngle > servoUpperLimit -> servoUpperLimit - else -> targetAngle - } + return false } } diff --git a/src/test/kotlin/crackers/kobots/parts/movement/RotatorTest.kt b/src/test/kotlin/crackers/kobots/parts/movement/RotatorTest.kt index 64e8348..97f20f1 100644 --- a/src/test/kotlin/crackers/kobots/parts/movement/RotatorTest.kt +++ b/src/test/kotlin/crackers/kobots/parts/movement/RotatorTest.kt @@ -26,11 +26,20 @@ import io.mockk.impl.annotations.MockK import io.mockk.mockkClass import io.mockk.verify +private fun Rotator.test(angle: Int) { + while (!rotateTo(angle)) { + // just count things + } +} + /** * Testing the rotator classes. */ class RotatorTest : FunSpec( { + /** + * Test steppers. + */ context("Stepper") { @MockK lateinit var mockStepper: BasicStepperMotor @@ -39,52 +48,53 @@ class RotatorTest : FunSpec( mockStepper = mockkClass(BasicStepperMotor::class) every { mockStepper.step(any(), any()) } answers { } } + /** - * Test the rotatable with a stepper motor: the gear ratio is 1:1 and the stepper motor has 200 steps per rotation. - * The rotatable is set to move from 0 to 360 degrees, and it's starting position is assumed to be 9 degrees. - * The target angle is 83 degrees. + * The gear ratio is 1:1 and the stepper motor has 200 steps per rotation. The rotator is set to move from + * 0 to 360 degrees, and it's starting position is assumed to be 0 degrees. The target angle is 83 degrees. */ test("max steps < 360, 1:1 gear ratio") { every { mockStepper.stepsPerRotation } answers { 200 } - val rotatable = BasicStepperRotator(mockStepper) + val rotor = BasicStepperRotator(mockStepper) - while (!rotatable.rotateTo(83)) { - // just count things - } + rotor.test(83) verify(exactly = 46) { mockStepper.step(StepperMotorInterface.Direction.FORWARD, any()) } } /** - * Test a stepper rotator with a gear ratio of 1:1.11 and the motor has 200 steps per rotation. The rotator is - * to be moved 90 degrees in a forward direction. The stepper is currently at 0 degrees. + * With a gear ratio of 1:1.11 and the motor has 200 steps per rotation. The rotator is to be moved 90 + * degrees in a forward direction. The stepper is currently at 0 degrees. */ test("max steps < 360, 1:11:1 gear ratio") { every { mockStepper.stepsPerRotation } answers { 200 } - val rotatable = BasicStepperRotator(mockStepper, 1.11f) + val rotor = BasicStepperRotator(mockStepper, 1.11f) - while (!rotatable.rotateTo(90)) { - // just count things - } + rotor.test(90) verify(exactly = 45) { mockStepper.step(StepperMotorInterface.Direction.FORWARD, any()) } } + /** + * Motor has 2048 steps per rotation and a gear ratio of 1.28 (simulated from real-world). The target is + * 90 degrees. + */ test("max steps > 360, 1.28:1 gear ratio") { every { mockStepper.stepsPerRotation } answers { 2048 } - val rotatable = BasicStepperRotator(mockStepper, 1.28f) + val rotor = BasicStepperRotator(mockStepper, 1.28f) - while (!rotatable.rotateTo(90)) { - // just count things - } + rotor.test(90) verify(exactly = 400) { mockStepper.step(StepperMotorInterface.Direction.FORWARD, any()) } } } + /** + * Test servos. + */ context("Servo") { @MockK lateinit var mockServo: ServoDevice @@ -96,56 +106,74 @@ class RotatorTest : FunSpec( } /** - * Test a rotatable with a servo motor. The physical range is 0 to 90 degrees and the servo has a range of 0 to 180. - * Assume the servo angle is at 43 degrees and the target is 87 physical degrees. + * The physical range is 0 to 90 degrees and the servo has a range of 0 to 180. + * Assume the servo angle is at 42 degrees, physical 21, and the target is 87 physical degrees. */ - test("rotatable with servo") { - currentAngle = 43f - val rotatable = ServoRotator(mockServo, IntRange(0, 90), IntRange(0, 180)) - - while (!rotatable.rotateTo(87)) { - // just count things + test("2:1 gear ratio, start at 21, move to 87") { + currentAngle = 42f + val rotor = ServoRotator(mockServo, 0..90, 0..180).apply { + where = 21 } - verify(exactly = 131) { + rotor.test(87) + verify(atLeast = 131) { mockServo.angle = any() } currentAngle shouldBe 174f } /** - * Test a rotatable with a servo motor. The physical range is -10 to 90 degrees and the servo has a range of - * 180 to 0. Assume the servo angle is at 162 degrees and the target is 45 physical degrees. + * The physical range is -10 to 90 degrees and the servo has a range of 180 to 0. + * The physical starting point is 0, servo at 162. */ - test("rotatable with servo reversed") { + test("ratio reversed") { currentAngle = 162f - val rotatable = ServoRotator(mockServo, IntRange(-10, 90), IntRange(180, 0)) - - while (!rotatable.rotateTo(45)) { - // just count things + val rotor = ServoRotator(mockServo, -10..90, IntRange(180, 0)).apply { + where = 0 } - verify(exactly = 80) { + rotor.test(45) + currentAngle shouldBe 81f + verify(atLeast = 80) { mockServo.angle = any() } - currentAngle shouldBe 82f } /** - * Test a rotatable with a servo motor. The physical range is 0 to 90 and the servo has a range of 0 to 180. - * The delta is 5 degrees. The servo is currently at an angle of 60 degrees and the target is 32 degrees. - * Verify the servo does not move. + * The physical range is 0 to 90 and the servo has a range of 0 to 180. The delta is 5 degrees. The servo + * is currently at an angle of 30 degrees and the target is 32 degrees. Verify the servo does not move. */ - test("rotatable with servo delta") { + test("delta prevents movement") { currentAngle = 60f - val rotatable = ServoRotator(mockServo, IntRange(0, 90), IntRange(0, 180), 5) - - while (!rotatable.rotateTo(32)) { - // just count things + val rotor = ServoRotator(mockServo, 0..90, 0..180, 5).apply { + where = 30 } + + rotor.test(32) verify(exactly = 0) { mockServo.angle = any() } currentAngle shouldBe 60f } + + /** + * The physical range is 1 to 133 and the servo range is 0 to 200. This tests that rounding errors will + * still allow the rotator to advance by 1 physical degree -- this was discovered in previous iteration + * trying to "step" from 67 to 68+ degrees. + */ + test("incremental movements") { + val rotor = ServoRotator(mockServo, 0..133, 0..200) + // move it to 68 and then attempt to iterate to 69 (should take 2 moves) + rotor.test(68) + + val startingAngle = rotor.current() + var t = false + var count = 0 + while (rotor.current() == startingAngle && !t && count < 10) { + val c = rotor.current() + t = rotor.rotateTo(c + 1) + count++ + } + count shouldBe 2 + } } } )