-
-
Notifications
You must be signed in to change notification settings - Fork 2
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
- Loading branch information
Showing
32 changed files
with
691 additions
and
557 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
118 changes: 118 additions & 0 deletions
118
api/src/main/kotlin/nebulosa/api/focusers/BacklashCompensator.kt
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,118 @@ | ||
| package nebulosa.api.focusers | ||
|
|
||
| import nebulosa.log.d | ||
| import nebulosa.log.loggerFor | ||
| import nebulosa.log.w | ||
| import kotlin.math.max | ||
| import kotlin.math.min | ||
|
|
||
| // https://bitbucket.org/Isbeorn/nina | ||
|
|
||
| data class BacklashCompensator( | ||
| @JvmField val compensation: BacklashCompensation, | ||
| @JvmField val maxPosition: Int, | ||
| ) { | ||
|
|
||
| enum class OvershootDirection { | ||
| NONE, | ||
| IN, | ||
| OUT, | ||
| } | ||
|
|
||
| @Volatile var lastDirection = OvershootDirection.NONE | ||
| private set | ||
|
|
||
| @Volatile var offset = 0 | ||
| private set | ||
|
|
||
| fun compute(targetPosition: Int, currentPosition: Int): IntArray { | ||
| var newPosition = targetPosition | ||
|
|
||
| when (compensation.mode) { | ||
| BacklashCompensationMode.ABSOLUTE -> { | ||
| val adjustedTargetPosition = targetPosition + offset | ||
|
|
||
| if (adjustedTargetPosition < 0) { | ||
| offset = 0 | ||
| newPosition = 0 | ||
| } else if (adjustedTargetPosition > maxPosition) { | ||
| offset = 0 | ||
| newPosition = maxPosition | ||
| } else { | ||
| val backlashCompensation = calculateAbsoluteBacklashCompensation(currentPosition, adjustedTargetPosition) | ||
| offset += backlashCompensation | ||
| newPosition = max(0, min(adjustedTargetPosition + backlashCompensation, maxPosition)) | ||
| } | ||
| } | ||
| BacklashCompensationMode.OVERSHOOT -> { | ||
| val backlashCompensation = calculateOvershootBacklashCompensation(currentPosition, targetPosition) | ||
|
|
||
| if (backlashCompensation != 0) { | ||
| val overshoot = targetPosition + backlashCompensation | ||
|
|
||
| if (overshoot < 0) { | ||
| LOG.w("overshooting position is below minimum 0, skipping overshoot") | ||
| } else if (overshoot > maxPosition) { | ||
| LOG.w("overshooting position is above maximum {}, skipping overshoot", maxPosition) | ||
| } else { | ||
| LOG.d("overshooting from {} to overshoot position {} using a compensation of {}. Moving back to position {}", currentPosition, overshoot, backlashCompensation, newPosition) | ||
|
|
||
| move(currentPosition, overshoot) | ||
| move(overshoot, newPosition) | ||
|
|
||
| return intArrayOf(overshoot, newPosition) | ||
| } | ||
| } | ||
| } | ||
| BacklashCompensationMode.NONE -> Unit | ||
| } | ||
|
|
||
| move(currentPosition, newPosition) | ||
|
|
||
| return intArrayOf(newPosition) | ||
| } | ||
|
|
||
| fun adjustedPosition(currentPosition: Int): Int { | ||
| return currentPosition - offset | ||
| } | ||
|
|
||
|
|
||
| @Suppress("NOTHING_TO_INLINE") | ||
| private inline fun move(currentPosition: Int, newPosition: Int) { | ||
| lastDirection = determineMovingDirection(currentPosition, newPosition) | ||
| LOG.d("moving to position {} using {} backlash compensation", newPosition, compensation.mode) | ||
| } | ||
|
|
||
| private fun determineMovingDirection(prevPosition: Int, newPosition: Int): OvershootDirection { | ||
| return if (newPosition > prevPosition) OvershootDirection.OUT | ||
| else if (newPosition < prevPosition) OvershootDirection.IN | ||
| else lastDirection | ||
| } | ||
|
|
||
| private fun calculateAbsoluteBacklashCompensation(lastPosition: Int, newPosition: Int): Int { | ||
| val direction = determineMovingDirection(lastPosition, newPosition) | ||
|
|
||
| return if (direction == OvershootDirection.IN && lastDirection == OvershootDirection.OUT) { | ||
| LOG.d("focuser is reversing direction from outwards to inwards") | ||
| -compensation.backlashIn | ||
| } else if (direction == OvershootDirection.OUT && lastDirection === OvershootDirection.IN) { | ||
| LOG.d("focuser is reversing direction from inwards to outwards") | ||
| compensation.backlashOut | ||
| } else { | ||
| 0 | ||
| } | ||
| } | ||
|
|
||
| private fun calculateOvershootBacklashCompensation(lastPosition: Int, newPosition: Int): Int { | ||
| val direction = determineMovingDirection(lastPosition, newPosition) | ||
|
|
||
| return if (direction == OvershootDirection.IN && compensation.backlashIn != 0) -compensation.backlashIn | ||
| else if (direction == OvershootDirection.OUT && compensation.backlashOut != 0) compensation.backlashOut | ||
| else 0 | ||
| } | ||
|
|
||
| companion object { | ||
|
|
||
| @JvmStatic private val LOG = loggerFor<BacklashCompensator>() | ||
| } | ||
| } |
Oops, something went wrong.