diff --git a/README.md b/README.md index f012196..dcca8fa 100644 --- a/README.md +++ b/README.md @@ -27,7 +27,7 @@ repositories { } dependencies { - implementation "com.github.amarcolini.joos:$module:0.4.5-alpha" + implementation "com.github.amarcolini.joos:$module:0.4.6-alpha" } ``` @@ -52,6 +52,6 @@ java { dependencies { //Gradle will automatically retrieve the correct dependencies based on your operating system - implementation "com.github.amarcolini.joos:gui:0.4.5-alpha" + implementation "com.github.amarcolini.joos:gui:0.4.6-alpha" } ```` \ No newline at end of file diff --git a/build.gradle b/build.gradle index e605782..37078e4 100644 --- a/build.gradle +++ b/build.gradle @@ -1,7 +1,7 @@ buildscript { ext { kotlin_version = "1.6.21" - lib_version = "0.4.5-alpha" + lib_version = "0.4.6-alpha" } repositories { diff --git a/command/annotation/build.gradle b/command/annotation/build.gradle index 72d8a92..0803a97 100644 --- a/command/annotation/build.gradle +++ b/command/annotation/build.gradle @@ -1,7 +1,5 @@ plugins { id "kotlin" - id "java" - id 'com.google.devtools.ksp' version '1.6.21-1.0.5' id 'maven-publish' } @@ -17,6 +15,8 @@ compileKotlin { } } +group = group + ".command" + dependencies { implementation "org.jetbrains.kotlin:kotlin-stdlib:$kotlin_version" implementation "org.jetbrains.kotlin:kotlin-reflect:$kotlin_version" @@ -25,9 +25,6 @@ dependencies { implementation "com.squareup:javapoet:1.13.0" implementation "com.squareup:kotlinpoet:1.11.0" - - ksp 'com.google.auto.service:auto-service:1.0.1' - implementation 'com.google.auto.service:auto-service-annotations:1.0.1' } publishing { @@ -52,7 +49,7 @@ publishing { repositories { maven { name = 'testRepo' - url = '../testRepo' + url = '../../testRepo' } } } \ No newline at end of file diff --git a/command/annotation/src/main/kotlin/com/amarcolini/joos/dashboard/javaProcessor.kt b/command/annotation/src/main/kotlin/com/amarcolini/joos/dashboard/javaProcessor.kt index b70388e..3efcb0e 100644 --- a/command/annotation/src/main/kotlin/com/amarcolini/joos/dashboard/javaProcessor.kt +++ b/command/annotation/src/main/kotlin/com/amarcolini/joos/dashboard/javaProcessor.kt @@ -1,15 +1,12 @@ package com.amarcolini.joos.dashboard -import com.google.auto.service.AutoService import com.squareup.javapoet.* -import com.squareup.kotlinpoet.asTypeName import java.util.* import javax.annotation.processing.* import javax.lang.model.SourceVersion import javax.lang.model.element.* import kotlin.collections.ArrayList -@AutoService(Processor::class) @SupportedSourceVersion(SourceVersion.RELEASE_8) class ConfigProcessor : AbstractProcessor() { private val results = ArrayList>>() @@ -64,9 +61,10 @@ class ConfigElementVisitor : ElementVisitor>> { + if (p0?.modifiers?.contains(Modifier.PUBLIC) != true) return emptyList() val configMembers = ArrayList>>() - val name = getAltName(p0, p1) ?: p0?.simpleName.toString() - p0?.enclosedElements?.forEach { member -> + val name = getAltName(p0, p1) ?: p0.simpleName.toString() + p0.enclosedElements?.forEach { member -> if (member.kind == ElementKind.FIELD && member.modifiers.containsAll( listOf( Modifier.PUBLIC, Modifier.STATIC @@ -98,6 +96,7 @@ class ConfigElementVisitor : ElementVisitor() { override fun visitClassDeclaration(classDeclaration: KSClassDeclaration, data: Resolver) { processedClasses += classDeclaration + if (!classDeclaration.modifiers.contains(Modifier.PUBLIC)) return when (classDeclaration.classKind) { ClassKind.INTERFACE, ClassKind.ENUM_CLASS, ClassKind.ENUM_ENTRY, ClassKind.ANNOTATION_CLASS -> return ClassKind.OBJECT -> { @@ -112,6 +111,7 @@ class ConfigSymbolProcessor( if (closestClass?.classKind != ClassKind.OBJECT && !property.modifiers.contains(Modifier.JAVA_STATIC)) return val parentClass = if (closestClass?.isCompanionObject == true) closestClass.parentDeclaration as KSClassDeclaration else closestClass + if (!property.isPublic()) return val name = property.qualifiedName ?: return val parentClassName = parentClass?.simpleName?.asString()?.ifEmpty { null } val parentClassAltName = parentClass?.let { getAltName(it)?.ifEmpty { null } } @@ -133,7 +133,6 @@ class ConfigSymbolProcessor( } } -@AutoService(SymbolProcessorProvider::class) class ConfigSymbolProcessorProvider : SymbolProcessorProvider { override fun create(environment: SymbolProcessorEnvironment): SymbolProcessor { return ConfigSymbolProcessor(environment) diff --git a/command/annotation/src/main/resources/META-INF/services/com.google.devtools.ksp.processing.SymbolProcessorProvider b/command/annotation/src/main/resources/META-INF/services/com.google.devtools.ksp.processing.SymbolProcessorProvider new file mode 100644 index 0000000..9c0133b --- /dev/null +++ b/command/annotation/src/main/resources/META-INF/services/com.google.devtools.ksp.processing.SymbolProcessorProvider @@ -0,0 +1 @@ +com.amarcolini.joos.dashboard.ConfigSymbolProcessorProvider \ No newline at end of file diff --git a/command/annotation/src/main/resources/META-INF/services/javax.annotation.processing.Processor b/command/annotation/src/main/resources/META-INF/services/javax.annotation.processing.Processor new file mode 100644 index 0000000..809e80d --- /dev/null +++ b/command/annotation/src/main/resources/META-INF/services/javax.annotation.processing.Processor @@ -0,0 +1 @@ +com.amarcolini.joos.dashboard.ConfigProcessor \ No newline at end of file diff --git a/command/build.gradle b/command/build.gradle index 060492e..d16089b 100644 --- a/command/build.gradle +++ b/command/build.gradle @@ -54,7 +54,7 @@ android { test.java.srcDirs += 'src/test/kotlin' } defaultConfig { - minSdkVersion 23 + minSdkVersion 24 } } diff --git a/command/src/main/kotlin/com/amarcolini/joos/command/AbstractComponent.kt b/command/src/main/kotlin/com/amarcolini/joos/command/AbstractComponent.kt index 0bf2f80..cd83352 100644 --- a/command/src/main/kotlin/com/amarcolini/joos/command/AbstractComponent.kt +++ b/command/src/main/kotlin/com/amarcolini/joos/command/AbstractComponent.kt @@ -4,23 +4,12 @@ package com.amarcolini.joos.command * An abstract version of [Component] with more quality of life features. */ abstract class AbstractComponent : Component { - /** - * The scheduler currently using this component. - */ - var scheduler: CommandScheduler? = null - internal set(value) { - subcomponents.forEach { - if (it is AbstractComponent) it.scheduler = value - } - field = value - } - /** * A list of subcomponents that this component may use. All subcomponents are * updated as well, as long as there is a call to `super.update()`. */ @JvmField - protected val subcomponents: MutableList = ArrayList() + protected val subcomponents: MutableSet = HashSet() override fun update() { subcomponents.forEach { it.update() } diff --git a/command/src/main/kotlin/com/amarcolini/joos/command/Command.kt b/command/src/main/kotlin/com/amarcolini/joos/command/Command.kt index 1f33266..a697a70 100644 --- a/command/src/main/kotlin/com/amarcolini/joos/command/Command.kt +++ b/command/src/main/kotlin/com/amarcolini/joos/command/Command.kt @@ -1,16 +1,18 @@ package com.amarcolini.joos.command -import java.util.function.BiConsumer import java.util.function.BooleanSupplier import java.util.function.Consumer import java.util.function.Supplier /** * A state machine representing a complete action to be performed using any number of [Component]s. - * Commands are usually run from a [CommandScheduler], but can be run independently if desired. Commands can be chained + * Commands are usually run from the [CommandScheduler], but can be run independently if desired. Commands can be chained * together to form complex multi-step actions. */ -abstract class Command { +abstract class Command : CommandInterface { + @JvmField + protected val telemetry: SuperTelemetry = CommandScheduler.telemetry + companion object { /** * Creates a [BasicCommand] out of the provided [runnable]. @@ -41,12 +43,6 @@ abstract class Command { */ open val requirements: Set = emptySet() - /** - * An internal property which stores this command's current [CommandScheduler]. - */ - var scheduler: CommandScheduler? = null - internal set - /** * Runs once when first scheduled. */ @@ -76,17 +72,20 @@ abstract class Command { /** * Cancels this command. */ - fun cancel() { - if (scheduler?.isScheduled(this) == true) scheduler?.cancel(this) - } + fun cancel() = cancel(this) + + /** + * Schedules this command. + */ + fun schedule() = schedule(this) /** - * Returns whether this command is currently registered with a [CommandScheduler]. + * Returns whether this command is currently registered with the [CommandScheduler]. */ - fun isScheduled(): Boolean = scheduler?.isScheduled(this) == true + fun isScheduled(): Boolean = isScheduled(this) /** - * Runs this command independently of a [CommandScheduler]. Initializes, executes and ends this command synchronously + * Runs this command independently of the [CommandScheduler]. Initializes, executes and ends this command synchronously * while also updating all of its required components and updating [CommandScheduler.telemetry]. * * *Note*: If this command does not end by itself, this method will run continuously. @@ -109,55 +108,55 @@ abstract class Command { /** * Adds a command to run after this one. */ - infix fun then(other: Command) = + infix fun then(other: Command): SequentialCommand = SequentialCommand(isInterruptable && other.isInterruptable, this, other) /** * Adds a runnable to run after this one. */ - infix fun then(runnable: Runnable) = + infix fun then(runnable: Runnable): SequentialCommand = this then BasicCommand(runnable) /** * Waits [duration] seconds after this command finishes. */ - infix fun wait(duration: Double) = + infix fun wait(duration: Double): SequentialCommand = this then WaitCommand(duration) /** * Adds a command to run in parallel with this one (Both run simultaneously until they finish). */ - infix fun and(other: Command) = + infix fun and(other: Command): ParallelCommand = ParallelCommand(isInterruptable && other.isInterruptable, this, other) /** * Adds a runnable to run in parallel with this one (Both run simultaneously until they finish). */ - infix fun and(runnable: Runnable) = + infix fun and(runnable: Runnable): ParallelCommand = this and BasicCommand(runnable) /** * Adds a command to run in parallel with this one (Both run simultaneously until one finishes). */ - infix fun race(other: Command) = + infix fun race(other: Command): RaceCommand = RaceCommand(isInterruptable && other.isInterruptable, this, other) /** * Adds a runnable to run in parallel with this one (Both run simultaneously until one finishes). */ - infix fun race(runnable: Runnable) = + infix fun race(runnable: Runnable): RaceCommand = this race BasicCommand(runnable) /** * Waits until [condition] returns true after this command finishes. */ - infix fun waitUntil(condition: BooleanSupplier) = + infix fun waitUntil(condition: BooleanSupplier): SequentialCommand = this then FunctionalCommand(isFinished = condition) /** * Overrides this command's [isFinished] function to finish when [condition] returns true. */ - fun runUntil(condition: BooleanSupplier) = + fun runUntil(condition: BooleanSupplier): FunctionalCommand = FunctionalCommand( this::init, this::execute, @@ -168,40 +167,80 @@ abstract class Command { ) /** - * Overrides this command's [isFinished] function to finish when [condition] is true. + * Overrides this command's [isFinished] function to finish when [condition] returns true, or, if it doesn't, + * when this command would normally finish. */ - fun runUntil(condition: Boolean) = - FunctionalCommand( - this::init, - this::execute, - this::end, - { condition }, - isInterruptable, - requirements - ) + fun stopWhen(condition: BooleanSupplier): FunctionalCommand = + runUntil { isFinished() || condition.asBoolean } + + /** + * Overrides this command's [isFinished] function to run until it is cancelled. + */ + fun runForever(): FunctionalCommand = runUntil { false } + + /** + * Overrides this command's [isFinished] function to run only once. + */ + fun runOnce(): FunctionalCommand = runUntil { true } + + /** + * Overrides this command's [init] function. + */ + fun init(action: Runnable): FunctionalCommand = FunctionalCommand( + action, + this::execute, + this::end, + this::isFinished, + isInterruptable, + requirements + ) + + /** + * Overrides this command's [execute] function. + */ + fun execute(action: Runnable): FunctionalCommand = FunctionalCommand( + this::init, + action, + this::end, + this::isFinished, + isInterruptable, + requirements + ) + + /** + * Overrides this command's [end] function. + */ + fun end(action: Consumer): FunctionalCommand = FunctionalCommand( + this::init, + this::execute, + action, + this::isFinished, + isInterruptable, + requirements + ) /** * Returns a [ListenerCommand] that runs the specified action when this command initializes. */ - fun onInit(action: Runnable) = + fun onInit(action: Runnable): ListenerCommand = ListenerCommand(this, action, {}, {}) /** * Returns a [ListenerCommand] that runs the specified action whenever this command updates. */ - fun onExecute(action: Runnable) = + fun onExecute(action: Runnable): ListenerCommand = ListenerCommand(this, {}, action, {}) /** * Returns a [ListenerCommand] that runs the specified action when this command is ended. */ - fun onEnd(action: Consumer) = + fun onEnd(action: Consumer): ListenerCommand = ListenerCommand(this, {}, {}, action) /** * Adds [requirements] to this command's list of required components. */ - fun requires(requirements: Set) = + fun requires(requirements: Set): FunctionalCommand = FunctionalCommand( this::init, this::execute, @@ -214,7 +253,7 @@ abstract class Command { /** * Adds [requirements] to this command's list of required components. */ - fun requires(vararg requirements: Component) = + fun requires(vararg requirements: Component): FunctionalCommand = FunctionalCommand( this::init, this::execute, @@ -227,7 +266,7 @@ abstract class Command { /** * Sets whether this command is interruptable. */ - fun isInterruptable(interruptable: Boolean) = + fun isInterruptable(interruptable: Boolean): FunctionalCommand = FunctionalCommand( this::init, this::execute, diff --git a/command/src/main/kotlin/com/amarcolini/joos/command/CommandGroup.kt b/command/src/main/kotlin/com/amarcolini/joos/command/CommandGroup.kt index 954013b..96de44d 100644 --- a/command/src/main/kotlin/com/amarcolini/joos/command/CommandGroup.kt +++ b/command/src/main/kotlin/com/amarcolini/joos/command/CommandGroup.kt @@ -11,24 +11,16 @@ package com.amarcolini.joos.command */ abstract class CommandGroup @JvmOverloads constructor( requireJoined: Boolean = true, - private vararg val commands: Command + vararg commands: Command ) : Command() { - final override val requirements = HashSet() + final override val requirements: MutableSet = HashSet() init { for (command in commands) { if (requireJoined && (requirements intersect command.requirements).isNotEmpty()) throw IllegalArgumentException( "Multiple commands in a command group cannot require the same components." ) - command.scheduler = scheduler requirements += command.requirements } } - - /** - * Resets the schedulers of each command. Should be called at the end of any overrides. - */ - override fun end(interrupted: Boolean) { - commands.forEach { it.scheduler = null } - } } \ No newline at end of file diff --git a/command/src/main/kotlin/com/amarcolini/joos/command/CommandInterface.kt b/command/src/main/kotlin/com/amarcolini/joos/command/CommandInterface.kt new file mode 100644 index 0000000..a4f9fa8 --- /dev/null +++ b/command/src/main/kotlin/com/amarcolini/joos/command/CommandInterface.kt @@ -0,0 +1,107 @@ +package com.amarcolini.joos.command + +import com.amarcolini.joos.command.CommandScheduler.schedulePolicy +import java.util.function.BooleanSupplier + +/** + * An interface that simplifies access to the [CommandScheduler] by implementing all its methods. + */ +interface CommandInterface { + /** + * Returns whether a command can currently be scheduled. + */ + fun isAvailable(command: Command): Boolean = CommandScheduler.isAvailable(command) + + /** + * Schedules commands for execution. + * + * @return `true` if all commands were successfully scheduled immediately, and `false` if they were not. Note that if + * the scheduler is currently updating, this method will return `false`, but the scheduler will attempt to + * schedule the commands when it can. If [schedulePolicy] is `true`, all commands will be successfully scheduled. + * + * @see CommandScheduler.schedulePolicy + */ + fun schedule(vararg commands: Command): Boolean = CommandScheduler.schedule(*commands) + + /** + * Schedules commands for execution. + * + * @return `true` if all commands were successfully scheduled immediately, and `false` if they were not. Note that if + * the scheduler is currently updating, this method will return `false`, but the scheduler will attempt to + * schedule the commands when it can. If [CommandScheduler.schedulePolicy] is `true`, all commands will be successfully scheduled. + * + * @see CommandScheduler.schedulePolicy + */ + fun schedule(vararg runnables: Runnable): Boolean = CommandScheduler.schedule(*runnables) + + /** + * Schedules commands for execution. + * + * @return `true` if all commands were successfully scheduled immediately, and `false` if they were not. Note that if + * the scheduler is currently updating, this method will return `false`, but the scheduler will attempt to + * schedule the commands when it can. If [CommandScheduler.schedulePolicy] is `true`, all commands will be successfully scheduled. + * + * @param repeat whether the provided [runnable] should run repeatedly or not + * @see CommandScheduler.schedulePolicy + */ + fun schedule(runnable: Runnable, repeat: Boolean): Boolean = CommandScheduler.schedule(runnable, repeat) + + /** + * Registers the given components to this CommandScheduler so that their update functions are called and their default commands are scheduled. + */ + fun register(vararg components: Component) = CommandScheduler.register(*components) + + /** + * Unregisters the given components from this CommandScheduler so that their update functions are no longer called and their default commands are no longer scheduled. + */ + fun unregister(vararg components: Component) = CommandScheduler.unregister(*components) + + /** + * Returns whether the given components are registered with this CommandScheduler. + */ + fun isRegistered(vararg components: Component) = CommandScheduler.isRegistered(*components) + + /** + * Cancels commands. Ignores whether a command is interruptable. + */ + fun cancel(vararg commands: Command) = CommandScheduler.cancel(*commands) + + /** + * Maps a condition to commands. If the condition returns true, the commands are scheduled. + * A command can be mapped to multiple conditions. + */ + fun map(condition: BooleanSupplier, vararg commands: Command) = + CommandScheduler.map(condition, *commands) + + /** + * Maps a condition to commands. If the condition returns true, the commands are scheduled. + * A command can be mapped to multiple conditions. + */ + fun map(condition: BooleanSupplier, vararg runnables: Runnable) = + CommandScheduler.map(condition, *runnables) + + /** + * Removes commands from the list of mappings. + */ + fun unmap(vararg commands: Command) = CommandScheduler.unmap(*commands) + + /** + * Removes a condition from the list of mappings. + */ + fun unmap(condition: BooleanSupplier) = CommandScheduler.unmap(condition) + + /** + * Cancels all currently scheduled commands. Ignores whether a command is interruptable. + */ + fun cancelAll() = CommandScheduler.cancelAll() + + /** + * Returns whether all the given commands are scheduled. + */ + fun isScheduled(vararg commands: Command) = CommandScheduler.isScheduled(*commands) + + /** + * Returns the command currently requiring a given component. + */ + fun requiring(component: Component) = CommandScheduler.requiring(component) +} \ No newline at end of file diff --git a/command/src/main/kotlin/com/amarcolini/joos/command/CommandOpMode.kt b/command/src/main/kotlin/com/amarcolini/joos/command/CommandOpMode.kt new file mode 100644 index 0000000..20b94c2 --- /dev/null +++ b/command/src/main/kotlin/com/amarcolini/joos/command/CommandOpMode.kt @@ -0,0 +1,36 @@ +package com.amarcolini.joos.command + +import com.acmerobotics.dashboard.FtcDashboard +import com.amarcolini.joos.gamepad.MultipleGamepad +import com.qualcomm.robotcore.eventloop.opmode.OpMode + +/** + * An OpMode that uses a [CommandScheduler]. If you're using a robot, try [RobotOpMode]. + */ +abstract class CommandOpMode : OpMode(), CommandInterface { + /** + * The global [SuperTelemetry] instance. + */ + @JvmField + protected val telem: SuperTelemetry = CommandScheduler.telemetry + + /** + * The FtcDashboard instance. + */ + @JvmField + protected val dashboard: FtcDashboard = FtcDashboard.getInstance() + + /** + * A handy [MultipleGamepad]. + */ + @JvmField + protected val gamepad: MultipleGamepad = MultipleGamepad(gamepad1, gamepad2) + + override fun internalPostInitLoop() = CommandScheduler.update() + + override fun internalPostLoop() = CommandScheduler.update() + + abstract override fun start() + + override fun loop() {} +} \ No newline at end of file diff --git a/command/src/main/kotlin/com/amarcolini/joos/command/CommandScheduler.kt b/command/src/main/kotlin/com/amarcolini/joos/command/CommandScheduler.kt index f3af947..a809aa1 100644 --- a/command/src/main/kotlin/com/amarcolini/joos/command/CommandScheduler.kt +++ b/command/src/main/kotlin/com/amarcolini/joos/command/CommandScheduler.kt @@ -1,23 +1,84 @@ package com.amarcolini.joos.command -import com.acmerobotics.dashboard.FtcDashboard -import com.acmerobotics.dashboard.telemetry.TelemetryPacket +import android.content.Context +import com.amarcolini.joos.gamepad.MultipleGamepad +import com.qualcomm.ftccommon.FtcEventLoop +import com.qualcomm.robotcore.eventloop.opmode.* +import com.qualcomm.robotcore.hardware.HardwareMap +import org.firstinspires.ftc.ftccommon.external.OnCreateEventLoop import java.util.function.BooleanSupplier +import kotlin.reflect.full.hasAnnotation /** * The main orchestrator for [Command]s and [Component]s. */ -open class CommandScheduler { +object CommandScheduler : OpModeManagerNotifier.Notifications { + /** + * The global telemetry object used for both FTC Dashboard and the Driver Station. + */ + @JvmField + val telemetry: SuperTelemetry = SuperTelemetry() + + /** + * Resets [telemetry]. + */ + @JvmStatic + fun resetTelemetry(): Unit = telemetry.reset() + + /** + * The current gamepads being used, or null if no OpMode is active. + */ + @JvmStatic + var gamepad: MultipleGamepad? = null + private set + + /** + * The currently active OpMode, or null if no OpMode is active. + */ + @JvmStatic + var opMode: OpMode? = null + internal set(value) { + gamepad = if (value == null) { + reset() + null + } else MultipleGamepad(value.gamepad1, value.gamepad2).also { + gamepad?.unregister() + telemetry.register(value.telemetry) + it.register() + } + field = value + } + + /** + * Whether the current OpMode is a teleop OpMode. + */ + @JvmStatic + val isInTeleOp: Boolean + get() = opMode?.let { it::class.hasAnnotation() } ?: false - companion object Static { - /** - * The global telemetry object used for both FtcDashboard and the Driver Station. - */ - @JvmField - val telemetry = SuperTelemetry() + /** + * Whether the current OpMode is an autonomous OpMode. + */ + @JvmStatic + val isInAutonomous: Boolean + get() = opMode?.let { it::class.hasAnnotation() } ?: false + + /** + * The current hardware map, or null if no OpMode is active. + */ + @JvmStatic + val hMap: HardwareMap? + get() = opMode?.hardwareMap - @JvmStatic - fun resetTelemetry() = telemetry.reset() + /** + * This method attaches itself to the robot controller event loop to automatically + * add/remove telemetries from the global telemetry, register any gamepads or hardware maps, and provide + * other useful features. + */ + @OnCreateEventLoop + @JvmStatic + fun attachEventLoop(context: Context, eventLoop: FtcEventLoop) { + eventLoop.opModeManager.registerListener(this) } private val scheduledCommands = LinkedHashSet() @@ -48,7 +109,6 @@ open class CommandScheduler { .isEmpty() private fun initCommand(command: Command) { - command.scheduler = this command.init() scheduledCommands.add(command) command.requirements.forEach { requirements[it] = command } @@ -75,7 +135,6 @@ open class CommandScheduler { .forEach { (_, command) -> scheduledCommands.remove(command) command.end(true) - command.scheduler = null } initCommand(command) true @@ -95,6 +154,18 @@ open class CommandScheduler { */ fun schedule(vararg runnables: Runnable): Boolean = schedule(*runnables.map { BasicCommand(it) }.toTypedArray()) + /** + * Schedules commands for execution. + * + * @return `true` if all commands were successfully scheduled immediately, and `false` if they were not. Note that if + * the scheduler is currently updating, this method will return `false`, but the scheduler will attempt to + * schedule the commands when it can. If [schedulePolicy] is `true`, all commands will be successfully scheduled. + * + * @param repeat whether the provided [runnable] should run repeatedly or not + * @see schedulePolicy + */ + fun schedule(runnable: Runnable, repeat: Boolean): Boolean = schedule(BasicCommand(runnable).runUntil { !repeat }) + private var isBusy = false fun update() { //Updates all registered components @@ -111,7 +182,6 @@ open class CommandScheduler { if (finished) { requirements.keys.removeAll(it.requirements) it.end(false) - it.scheduler = null } //Removes them if they are finished finished @@ -140,9 +210,6 @@ open class CommandScheduler { * Registers the given components to this CommandScheduler so that their update functions are called and their default commands are scheduled. */ fun register(vararg components: Component) { - components.forEach { - if (it is AbstractComponent) it.scheduler = this - } this.components += components } @@ -150,16 +217,13 @@ open class CommandScheduler { * Unregisters the given components from this CommandScheduler so that their update functions are no longer called and their default commands are no longer scheduled. */ fun unregister(vararg components: Component) { - components.forEach { - if (it is AbstractComponent && it.scheduler == this) it.scheduler = null - } - this.components -= components + this.components -= components.toSet() } /** * Returns whether the given components are registered with this CommandScheduler. */ - fun isRegistered(vararg components: Component) = + fun isRegistered(vararg components: Component): Boolean = this.components.containsAll(components.toList()) /** @@ -178,7 +242,6 @@ open class CommandScheduler { scheduledCommands.remove(command) requirements.keys.removeAll(command.requirements) command.end(true) - command.scheduler = null } } } @@ -195,8 +258,8 @@ open class CommandScheduler { * Maps a condition to commands. If the condition returns true, the commands are scheduled. * A command can be mapped to multiple conditions. */ - fun map(condition: BooleanSupplier, vararg commands: Runnable) { - map(condition, *commands.map { Command.of(it) }.toTypedArray()) + fun map(condition: BooleanSupplier, vararg runnables: Runnable) { + map(condition, *runnables.map { Command.of(it) }.toTypedArray()) } /** @@ -221,6 +284,7 @@ open class CommandScheduler { /** * Resets this [CommandScheduler]. The telemetry is reset, all commands are cancelled, and all commands, components, and conditions are cleared. */ + @JvmStatic fun reset() { val reset = { cancelAll() @@ -238,10 +302,22 @@ open class CommandScheduler { /** * Returns whether all the given commands are scheduled. */ - fun isScheduled(vararg commands: Command) = scheduledCommands.containsAll(commands.asList()) + fun isScheduled(vararg commands: Command): Boolean = scheduledCommands.containsAll(commands.asList()) /** * Returns the command currently requiring a given component. */ - fun requiring(component: Component) = requirements[component] + fun requiring(component: Component): Command? = requirements[component] + + override fun onOpModePreInit(opMode: OpMode) { + this.opMode = opMode + } + + override fun onOpModePreStart(opMode: OpMode) {} + + override fun onOpModePostStop(opMode: OpMode) { + reset() + this.opMode = null + gamepad = null + } } \ No newline at end of file diff --git a/command/src/main/kotlin/com/amarcolini/joos/command/Component.kt b/command/src/main/kotlin/com/amarcolini/joos/command/Component.kt index b4cf76e..3d8dbe5 100644 --- a/command/src/main/kotlin/com/amarcolini/joos/command/Component.kt +++ b/command/src/main/kotlin/com/amarcolini/joos/command/Component.kt @@ -4,17 +4,17 @@ import java.util.function.Supplier /** * A class representing a basic unit of robot organization, encapsulating low-level robot hardware and providing - * methods to be used by [Command]s. [CommandScheduler]s use components to ensure that multiple commands are not using + * methods to be used by [Command]s. The [CommandScheduler] uses components to ensure that multiple commands are not using * the same hardware at the same time. Commands that use a component should include that component in their [Command.requirements] set. */ -interface Component { +interface Component : CommandInterface { companion object { /** * Creates a component using the provided [runnable] and [defaultCommand]. */ @JvmStatic @JvmOverloads - fun of(defaultCommand: Supplier = Supplier { null }, runnable: Runnable) = + fun of(runnable: Runnable, defaultCommand: Supplier = Supplier { null }): Component = object : Component { override fun update() = runnable.run() override fun getDefaultCommand() = defaultCommand.get() @@ -27,7 +27,17 @@ interface Component { fun getDefaultCommand(): Command? = null /** - * This method is called repeatedly by a [CommandScheduler]. + * This method is called repeatedly by the [CommandScheduler]. */ fun update() {} + + /** + * Unregisters this component. + */ + fun unregister() = unregister(this) + + /** + * Registers this component. + */ + fun register() = register(this) } \ No newline at end of file diff --git a/command/src/main/kotlin/com/amarcolini/joos/command/ParallelCommand.kt b/command/src/main/kotlin/com/amarcolini/joos/command/ParallelCommand.kt index ea26ff2..69c7084 100644 --- a/command/src/main/kotlin/com/amarcolini/joos/command/ParallelCommand.kt +++ b/command/src/main/kotlin/com/amarcolini/joos/command/ParallelCommand.kt @@ -1,7 +1,7 @@ package com.amarcolini.joos.command /** - * A command that runs commands in parallel (Runs them all simultaneously until they finish). + * A command that runs commands in parallel until they all finish. */ class ParallelCommand @JvmOverloads constructor( override val isInterruptable: Boolean = true, @@ -34,4 +34,4 @@ class ParallelCommand @JvmOverloads constructor( } override fun isFinished() = commands.values.all { it } -} +} \ No newline at end of file diff --git a/command/src/main/kotlin/com/amarcolini/joos/command/RaceCommand.kt b/command/src/main/kotlin/com/amarcolini/joos/command/RaceCommand.kt index 69f1ffb..d5cc744 100644 --- a/command/src/main/kotlin/com/amarcolini/joos/command/RaceCommand.kt +++ b/command/src/main/kotlin/com/amarcolini/joos/command/RaceCommand.kt @@ -1,7 +1,7 @@ package com.amarcolini.joos.command /** - * A command that runs commands in parallel (Runs them all simultaneously until one of them finishes). + * A command that runs commands in parallel until one of them finishes. */ class RaceCommand @JvmOverloads constructor( override val isInterruptable: Boolean = true, @@ -20,8 +20,7 @@ class RaceCommand @JvmOverloads constructor( override fun end(interrupted: Boolean) { if (!interrupted) commands.filter { !it.isFinished() }.forEach { it.end(true) } else commands.forEach { it.end(true) } - super.end(interrupted) } - override fun isFinished() = commands.any { it.isFinished() } + override fun isFinished(): Boolean = commands.any { it.isFinished() } } \ No newline at end of file diff --git a/command/src/main/kotlin/com/amarcolini/joos/command/Robot.kt b/command/src/main/kotlin/com/amarcolini/joos/command/Robot.kt index d4536bd..f8fd028 100644 --- a/command/src/main/kotlin/com/amarcolini/joos/command/Robot.kt +++ b/command/src/main/kotlin/com/amarcolini/joos/command/Robot.kt @@ -5,54 +5,56 @@ import com.acmerobotics.dashboard.telemetry.MultipleTelemetry import com.amarcolini.joos.gamepad.MultipleGamepad import com.qualcomm.robotcore.eventloop.opmode.Autonomous import com.qualcomm.robotcore.eventloop.opmode.OpMode +import com.qualcomm.robotcore.eventloop.opmode.OpModeRegistrar import com.qualcomm.robotcore.eventloop.opmode.TeleOp import com.qualcomm.robotcore.hardware.HardwareMap +import org.firstinspires.ftc.ftccommon.external.OnCreateEventLoop import kotlin.reflect.full.hasAnnotation /** * A class that makes any command-based robot code a lot smoother * and easier to understand. */ -abstract class Robot(opMode: OpMode) : CommandScheduler() { +abstract class Robot : CommandInterface { /** - * A [MultipleGamepad] storing both OpMode gamepads for convenience. + * The current OpMode. */ @JvmField - val gamepad: MultipleGamepad = MultipleGamepad(opMode.gamepad1, opMode.gamepad2) + val opMode: OpMode = CommandScheduler.opMode + ?: throw IllegalStateException("A Robot cannot be instantiated without an active OpMode.") /** * Whether the current OpMode is a teleop OpMode. */ @JvmField - val isInTeleOp: Boolean = opMode::class.hasAnnotation() + val isInTeleOp: Boolean = CommandScheduler.isInTeleOp /** * Whether the current OpMode is an autonomous OpMode. */ @JvmField - val isInAutonomous: Boolean = opMode::class.hasAnnotation() + val isInAutonomous: Boolean = CommandScheduler.isInAutonomous /** - * The hardware map obtained from the OpMode. + * The current hardware map. */ @JvmField val hMap: HardwareMap = opMode.hardwareMap @JvmField - val dashboard: FtcDashboard = FtcDashboard.getInstance() + val gamepad: MultipleGamepad = CommandScheduler.gamepad + ?: throw IllegalStateException("A Robot cannot be instantiated without an active OpMode.") - init { - register(gamepad) - telemetry.register(opMode.telemetry) - } + @JvmField + val dashboard: FtcDashboard = FtcDashboard.getInstance() /** - * This method is runs when the current OpMode initializes. + * This method is run when the current OpMode initializes. Automatically called by [RobotOpMode]. */ - abstract fun init() + open fun init() {} /** - * This method is run as soon as the current OpMode starts. + * This method is run as soon as the current OpMode starts. Automatically called by [RobotOpMode]. */ - abstract fun start() + open fun start() {} } \ No newline at end of file diff --git a/command/src/main/kotlin/com/amarcolini/joos/command/RobotOpMode.kt b/command/src/main/kotlin/com/amarcolini/joos/command/RobotOpMode.kt index 1b141bb..2e4964d 100644 --- a/command/src/main/kotlin/com/amarcolini/joos/command/RobotOpMode.kt +++ b/command/src/main/kotlin/com/amarcolini/joos/command/RobotOpMode.kt @@ -1,15 +1,16 @@ package com.amarcolini.joos.command +import com.acmerobotics.dashboard.FtcDashboard +import com.amarcolini.joos.gamepad.MultipleGamepad import com.qualcomm.robotcore.eventloop.opmode.* -import kotlin.reflect.KClass -import kotlin.reflect.full.createInstance import kotlin.reflect.full.hasAnnotation import kotlin.reflect.full.primaryConstructor /** * An OpMode made for [Robot]s. */ -abstract class RobotOpMode : OpMode() { +abstract class RobotOpMode : OpMode(), + CommandInterface { /** * Whether this OpMode is a teleop OpMode. */ @@ -28,9 +29,27 @@ abstract class RobotOpMode : OpMode() { protected lateinit var robot: T private set + /** + * The global [SuperTelemetry] instance. + */ + @JvmField + protected val telem: SuperTelemetry = CommandScheduler.telemetry + + /** + * The FtcDashboard instance. + */ + @JvmField + protected val dashboard: FtcDashboard = FtcDashboard.getInstance() + + /** + * A handy [MultipleGamepad]. + */ + @JvmField + protected val gamepad: MultipleGamepad = MultipleGamepad(gamepad1, gamepad2) + /** * This method is called on initialization. Any commands scheduled here will be - * run in the init loop. [initialize] **must** be called here. + * run in the init loop. [initialize] should be called here. */ abstract fun preInit() @@ -40,38 +59,27 @@ abstract class RobotOpMode : OpMode() { */ abstract fun preStart() - final override fun init() = preInit() - final override fun init_loop() = robot.update() + final override fun init() { + preInit() + } + final override fun start() { - robot.start() + CommandScheduler.cancelAll() + if (::robot.isInitialized) robot.start() preStart() } - final override fun loop() = robot.update() - override fun stop() = robot.reset() + override fun internalPostInitLoop() = CommandScheduler.update() + + override fun internalPostLoop() = CommandScheduler.update() + + override fun loop() {} /** - * Initializes the given robot with this OpMode. This method **must** be called - * in order for this OpMode to work correctly. + * Initializes the given robot with this OpMode. This method should be called in init. */ protected fun initialize(robot: T) { this.robot = robot robot.init() } - - /** - * Initializes the given robot with this OpMode. This method **must** be called - * in order for this OpMode to work correctly. - */ - protected fun initialize(robot: (OpMode) -> T) { - initialize(robot(this)) - } - - /** - * Initializes the given robot with this OpMode. This method **must** be called - * in order for this OpMode to work correctly. - */ - protected inline fun initialize() { - initialize(B::class.primaryConstructor?.call(this) ?: return) - } } \ No newline at end of file diff --git a/command/src/main/kotlin/com/amarcolini/joos/command/SequentialCommand.kt b/command/src/main/kotlin/com/amarcolini/joos/command/SequentialCommand.kt index 1a0b924..542aa33 100644 --- a/command/src/main/kotlin/com/amarcolini/joos/command/SequentialCommand.kt +++ b/command/src/main/kotlin/com/amarcolini/joos/command/SequentialCommand.kt @@ -6,7 +6,7 @@ package com.amarcolini.joos.command class SequentialCommand @JvmOverloads constructor( override val isInterruptable: Boolean = true, vararg val commands: Command -) : CommandGroup(false, *commands) { +) : CommandGroup(false, commands = commands) { private var index = -1 override fun init() { @@ -30,7 +30,6 @@ class SequentialCommand @JvmOverloads constructor( override fun end(interrupted: Boolean) { if (index < 0) return - if (interrupted) commands[index].end(interrupted) - super.end(interrupted) + if (interrupted) commands[index].end(true) } } \ No newline at end of file diff --git a/command/src/main/kotlin/com/amarcolini/joos/command/SuperTelemetry.kt b/command/src/main/kotlin/com/amarcolini/joos/command/SuperTelemetry.kt index a0bcdf5..1db4bd8 100644 --- a/command/src/main/kotlin/com/amarcolini/joos/command/SuperTelemetry.kt +++ b/command/src/main/kotlin/com/amarcolini/joos/command/SuperTelemetry.kt @@ -15,7 +15,11 @@ import kotlin.math.ceil /** * A powerful telemetry for both the Driver Station and [FTC Dashboard](https://github.com/acmerobotics/ftc-dashboard). */ -class SuperTelemetry { +class SuperTelemetry() { + constructor(telemetry: Telemetry) : this() { + register(telemetry) + } + private var packet: TelemetryPacket = TelemetryPacket() val lines: MutableList = ArrayList() private val telemetries: MutableList = ArrayList() @@ -62,7 +66,7 @@ class SuperTelemetry { } inner class Item internal constructor(var caption: String, var value: String) : Lineable { - internal lateinit var parent: Line + internal var parent: Line? = null fun setCaption(caption: String): Item { this.caption = caption @@ -88,14 +92,17 @@ class SuperTelemetry { return this } - private fun getIndex(): Int = lines.indexOf(parent) - fun addData(caption: String, format: String, arg1: Any, vararg args: Any): Item = addData(caption, String.format(format, arg1, *args)) fun addData(caption: String, value: Any): Item { + val parent = parent val item = Item(caption, value.toString()) - lines.add(getIndex() + 1, Line(item)) + if (parent == null) { + lines.add(lines.indexOf(this) + 1, item) + } else { + parent.items.add(parent.items.indexOf(this) + 1, item) + } return item } } diff --git a/command/src/main/kotlin/com/amarcolini/joos/command/WaitCommand.kt b/command/src/main/kotlin/com/amarcolini/joos/command/WaitCommand.kt index e1c7ad9..4a46db3 100644 --- a/command/src/main/kotlin/com/amarcolini/joos/command/WaitCommand.kt +++ b/command/src/main/kotlin/com/amarcolini/joos/command/WaitCommand.kt @@ -7,8 +7,8 @@ import com.amarcolini.joos.util.NanoClock * * @param duration the duration in seconds */ -class WaitCommand(var duration: Double) : Command() { - private val clock = NanoClock.system() +class WaitCommand @JvmOverloads constructor(var duration: Double, private val clock: NanoClock = NanoClock.system()) : + Command() { private var start = clock.seconds() override fun init() { diff --git a/command/src/main/kotlin/com/amarcolini/joos/dashboard/ConfigHandler.kt b/command/src/main/kotlin/com/amarcolini/joos/dashboard/ConfigHandler.kt index ee412b2..3cfe4ce 100644 --- a/command/src/main/kotlin/com/amarcolini/joos/dashboard/ConfigHandler.kt +++ b/command/src/main/kotlin/com/amarcolini/joos/dashboard/ConfigHandler.kt @@ -1,5 +1,6 @@ package com.amarcolini.joos.dashboard +import android.content.Context import android.util.Log import com.acmerobotics.dashboard.FtcDashboard import com.acmerobotics.dashboard.config.reflection.FieldProvider @@ -10,6 +11,7 @@ import com.acmerobotics.dashboard.config.variable.VariableType import com.amarcolini.joos.geometry.Angle import com.amarcolini.joos.geometry.Pose2d import com.amarcolini.joos.geometry.Vector2d +import com.qualcomm.ftccommon.FtcEventLoop import org.firstinspires.ftc.ftccommon.external.OnCreateEventLoop import java.lang.reflect.Field import java.lang.reflect.Modifier @@ -48,7 +50,7 @@ object ConfigHandler { @OnCreateEventLoop @JvmStatic - fun init() { + fun init(context: Context, eventLoop: FtcEventLoop) { javaResults?.forEach { (group, field) -> parseField(field, group) } diff --git a/command/src/main/kotlin/com/amarcolini/joos/hardware/Motor.kt b/command/src/main/kotlin/com/amarcolini/joos/hardware/Motor.kt index 499b364..8d0d9d8 100644 --- a/command/src/main/kotlin/com/amarcolini/joos/hardware/Motor.kt +++ b/command/src/main/kotlin/com/amarcolini/joos/hardware/Motor.kt @@ -66,14 +66,63 @@ class Motor @JvmOverloads constructor( constructor( motor: DcMotorEx, maxRPM: Double, - TPR: Double = 1.0, + TPR: Double, wheelRadius: Double, gearRatio: Double, clock: NanoClock = NanoClock.system() ) : this(motor, maxRPM, TPR, clock) { - distancePerRev = wheelRadius * 2 * PI * gearRatio + distancePerOutputRev = 2 * PI * wheelRadius + this.gearRatio = gearRatio } + /** + * @param motor the motor for the wrapper to use + * @param kind the kind of motor + * @param wheelRadius the radius of the wheel the motor is turning + * @param gearRatio the gear ratio from the output shaft to the wheel the motor is turning + */ + @JvmOverloads + constructor( + motor: DcMotorEx, + kind: Kind, + wheelRadius: Double = 1.0, + gearRatio: Double = 1.0, + clock: NanoClock = NanoClock.system() + ) : this(motor, kind.maxRPM, kind.TPR, wheelRadius, gearRatio, clock) + + /** + * @param hMap the hardware map from the OpMode + * @param id the device id from the RC config + * @param kind the kind of motor + * @param wheelRadius the radius of the wheel the motor is turning + * @param gearRatio the gear ratio from the output shaft to the wheel the motor is turning + */ + @JvmOverloads + constructor( + hMap: HardwareMap, + id: String, + kind: Kind, + wheelRadius: Double = 1.0, + gearRatio: Double = 1.0, + clock: NanoClock = NanoClock.system() + ) : this(hMap, id, kind.maxRPM, kind.TPR, wheelRadius, gearRatio, clock) + + /** + * @param motor the motor for the wrapper to use + * @param maxRPM the revolutions per minute of the motor + * @param TPR the ticks per revolution of the motor + * @param gearRatio the gear ratio from the output shaft to the wheel the motor is turning + */ + @JvmOverloads + constructor( + motor: DcMotorEx, + maxRPM: Double, + TPR: Double, + gearRatio: Double, + clock: NanoClock = NanoClock.system() + ) : this(motor, maxRPM, TPR, clock) { + this.gearRatio = gearRatio + } /** * @param hMap the hardware map from the OpMode @@ -88,12 +137,29 @@ class Motor @JvmOverloads constructor( hMap: HardwareMap, id: String, maxRPM: Double, - TPR: Double = 1.0, + TPR: Double, wheelRadius: Double, gearRatio: Double, clock: NanoClock = NanoClock.system() ) : this(hMap.get(DcMotorEx::class.java, id), maxRPM, TPR, wheelRadius, gearRatio, clock) + /** + * @param hMap the hardware map from the OpMode + * @param id the device id from the RC config + * @param maxRPM the revolutions per minute of the motor + * @param TPR the ticks per revolution of the motor + * @param gearRatio the gear ratio from the output shaft to the wheel the motor is turning + */ + @JvmOverloads + constructor( + hMap: HardwareMap, + id: String, + maxRPM: Double, + TPR: Double, + gearRatio: Double, + clock: NanoClock = NanoClock.system() + ) : this(hMap.get(DcMotorEx::class.java, id), maxRPM, TPR, gearRatio, clock) + enum class RunMode { /** * Sets the raw voltage given to the motor, using feedforward if desired. @@ -161,6 +227,36 @@ class Motor @JvmOverloads constructor( RPS } + /** + * A class representing many different motors so you don't have to find their specs. + */ + enum class Kind(val maxRPM: Double, val TPR: Double) { + GOBILDA_30(30.0, 5_281.1), + GOBILDA_43(43.0, 3_895.9), + GOBILDA_60(60.0, 2_786.2), + GOBILDA_84(84.0, 1_992.6), + GOBILDA_117(117.0, 1_425.1), + GOBILDA_223(223.0, 751.8), + GOBILDA_312(312.0, 537.7), + GOBILDA_435(435.0, 384.5), + GOBILDA_1150(1150.0, 145.1), + GOBILDA_1620(1620.0, 103.8), + GOBILDA_6000(5400.0, 28.0), + GOBILDA_MATRIX(5800.0, 28.0), + REV_HEX(6000.0, 28.0), + REV_CORE_HEX(125.0, 288.0), + REV_20_SPUR(300.0, 560.0), + REV_40_SPUR(150.0, 1120.0), + REV_20_PLANETARY(312.5, 537.6), + NEVEREST_20(349.0, 537.6), + NEVEREST_40(160.0, 1120.0), + NEVEREST_60(105.0, 1680.0), + NEVEREST_3_7(1780.0, 103.6), + TETRIX_60(100.0, 1440.0), + TETRIX_40(150.0, 960.0), + TETRIX_20(480.0, 480.0) + } + /** * A wrapper for motor encoders in the FTC SDK. * @@ -170,7 +266,7 @@ class Motor @JvmOverloads constructor( * @param getVelocity the position supplier which points to the * current velocity of the motor in ticks per second */ - class Encoder constructor( + class Encoder internal constructor( private val TPR: Double, private val getPosition: () -> Int, private val getVelocity: () -> Double, @@ -193,7 +289,7 @@ class Motor @JvmOverloads constructor( private var lastPosition = 0 /** - * Whether or not the encoder is reversed. Independent of motor direction. + * Whether the encoder is reversed. Independent of motor direction. */ @JvmField var reversed: Boolean = false @@ -256,7 +352,7 @@ class Motor @JvmOverloads constructor( } @JvmField - val encoder = Encoder(TPR, motor::getCurrentPosition, motor::getVelocity, clock) + val encoder: Encoder = Encoder(TPR, motor::getCurrentPosition, motor::getVelocity, clock) /** * The maximum achievable distance velocity of the motor. @@ -281,19 +377,19 @@ class Motor @JvmOverloads constructor( /** * PID coefficients used in [RunMode.RUN_USING_ENCODER]. */ - var veloCoefficients = PIDCoefficients(1.0) + var veloCoefficients: PIDCoefficients = PIDCoefficients(1.0) /** * PID coefficients used in [RunMode.RUN_TO_POSITION]. */ - var positionCoefficients = PIDCoefficients(1.0) + var positionCoefficients: PIDCoefficients = PIDCoefficients(1.0) /** * Feedforward coefficients used in both [RunMode.RUN_USING_ENCODER] and [RunMode.RUN_WITHOUT_ENCODER]. * Note that these coefficients are applied to desired distance velocity, so not using feedforward means setting * kV to 1 / [maxDistanceVelocity]. */ - var feedforwardCoefficients = FeedforwardCoefficients(1 / maxDistanceVelocity) + var feedforwardCoefficients: FeedforwardCoefficients = FeedforwardCoefficients(1 / maxDistanceVelocity) /** * The target position used by [RunMode.RUN_TO_POSITION]. @@ -303,6 +399,26 @@ class Motor @JvmOverloads constructor( positionController.targetPosition = value.toDouble() field = value } + + /** + * Sets [targetPosition] with the target [angle]. + */ + fun setTargetAngle(angle: Angle) { + targetPosition = (angle / (2 * PI).rad * TPR).roundToInt() + } + + /** + * Sets [targetPosition] with the target [angle], where [angle] is in [Angle.defaultUnits]. + */ + fun setTargetAngle(angle: Double) = setTargetAngle(Angle(angle)) + + /** + * Sets [targetPosition] with the target [distance]. + */ + fun setTargetDistance(distance: Double) { + targetPosition = (distance / distancePerRev * TPR).roundToInt() + } + private var targetVel: Double = 0.0 private var targetAccel: Double = 0.0 @@ -396,7 +512,7 @@ class Motor @JvmOverloads constructor( RunMode.RUN_TO_POSITION -> { positionController.pid = positionCoefficients motor.power = - (speed * positionController.update(currentPosition.toDouble())) / maxTPS + speed * positionController.update(encoder.position.toDouble(), encoder.velocity) } RunMode.RUN_WITHOUT_ENCODER -> { motor.power = Kinematics.calculateMotorFeedforward( @@ -464,11 +580,23 @@ class Motor @JvmOverloads constructor( .onEnd { setSpeed(0.0) } /** - * Returns a command that runs the motor until it has reached the desired distance. + * Returns a command that runs the motor until it has reached the desired [distance]. */ fun goToDistance(distance: Double): Command = goToPosition((distance / distancePerRev * TPR).roundToInt()) + /** + * Returns a command that runs the motor until it has reached the desired [angle]. + */ + fun goToAngle(angle: Angle): Command = + goToPosition((angle / (2 * PI).rad * gearRatio).roundToInt()) + + /** + * Returns a command that runs the motor until it has reached the desired [angle], + * where [angle] is in [Angle.defaultUnits]. + */ + fun goToAngle(angle: Double): Command = goToAngle(Angle(angle)) + /** * The distance per revolution travelled by the motor. * @see distance @@ -476,11 +604,31 @@ class Motor @JvmOverloads constructor( */ var distancePerRev: Double = encoder.distancePerRev get() = encoder.distancePerRev - set(value) { + private set(value) { encoder.distancePerRev = value field = value } + /** + * The distance per revolution of the motor output, excluding any gear ratios (i.e., + * if the motor had a gear ratio of 2:1, and a wheel was attached to the output, this value would + * be the circumference of the wheel). + */ + var distancePerOutputRev: Double = 1.0 + set(value) { + distancePerRev = value * gearRatio + field = value + } + + /** + * The gear ratio on the motor. + */ + var gearRatio: Double = 1.0 + set(value) { + distancePerRev = value * distancePerOutputRev + field = value + } + /** * Returns whether the motor is currently moving towards the desired setpoint using [RunMode.RUN_TO_POSITION]. */ diff --git a/command/src/main/kotlin/com/amarcolini/joos/hardware/MotorGroup.kt b/command/src/main/kotlin/com/amarcolini/joos/hardware/MotorGroup.kt index c6ffc73..5947aa0 100644 --- a/command/src/main/kotlin/com/amarcolini/joos/hardware/MotorGroup.kt +++ b/command/src/main/kotlin/com/amarcolini/joos/hardware/MotorGroup.kt @@ -1,12 +1,17 @@ package com.amarcolini.joos.hardware import com.amarcolini.joos.command.Command +import com.amarcolini.joos.command.Command.Companion.emptyCommand import com.amarcolini.joos.command.Component import com.amarcolini.joos.control.FeedforwardCoefficients import com.amarcolini.joos.control.PIDCoefficients import com.amarcolini.joos.geometry.Angle import com.amarcolini.joos.hardware.Motor.RunMode +import com.amarcolini.joos.util.NanoClock +import com.amarcolini.joos.util.rad import com.qualcomm.robotcore.hardware.HardwareMap +import kotlin.math.PI +import kotlin.math.roundToInt /** * A class that runs multiple motors together as a unit. @@ -20,8 +25,8 @@ class MotorGroup(vararg val motors: Motor) : Component { /** * @param hMap the hardware map from the OpMode - * @param maxRPM the maximum revolutions per minute of the motor - * @param TPR the ticks per revolution of the motor + * @param maxRPM the maximum revolutions per minute of all the motors + * @param TPR the ticks per revolution of all the motors * @param ids the device ids from the RC config */ @JvmOverloads @@ -29,13 +34,64 @@ class MotorGroup(vararg val motors: Motor) : Component { *ids.map { Motor(hMap, it, maxRPM, TPR) }.toTypedArray() ) + /** + * @param hMap the hardware map from the OpMode + * @param kind the kind of all the motors + * @param ids the device ids from the RC config + */ + constructor(hMap: HardwareMap, kind: Motor.Kind, vararg ids: String) : this( + *ids.map { Motor(hMap, it, kind.maxRPM, kind.TPR) }.toTypedArray() + ) + + /** + * @param hMap the hardware map from the OpMode + * @param maxRPM the maximum revolutions per minute of all the motors + * @param TPR the ticks per revolution of all the motors + * @param ids the device ids from the RC config and whether those motors should be reversed + */ + @JvmOverloads + constructor(hMap: HardwareMap, maxRPM: Double, TPR: Double = 1.0, vararg ids: Pair) : this( + *ids.map { Motor(hMap, it.first, maxRPM, TPR).apply { reversed = it.second } }.toTypedArray() + ) + + /** + * @param hMap the hardware map from the OpMode + * @param kind the kind of all the motors + * @param wheelRadius the radius of the wheels each motor is turning + * @param gearRatio the gear ratio from the output shaft to the wheel each motor is turning + * @param ids the device ids from the RC config and whether those motors should be reversed + */ + @JvmOverloads + constructor( + hMap: HardwareMap, + kind: Motor.Kind, + wheelRadius: Double, + gearRatio: Double = 1.0, + vararg ids: Pair + ) : this(hMap, kind.maxRPM, kind.TPR, wheelRadius, gearRatio, *ids) /** * @param hMap the hardware map from the OpMode - * @param maxRPM the revolutions per minute of the motor - * @param TPR the ticks per revolution of the motor - * @param wheelRadius the radius of the wheel the motor is turning - * @param gearRatio the gear ratio from the output shaft to the wheel the motor is turning + * @param kind the kind of all the motors + * @param wheelRadius the radius of the wheels each motor is turning + * @param gearRatio the gear ratio from the output shaft to the wheel each motor is turning + * @param ids the device ids from the RC config + */ + @JvmOverloads + constructor( + hMap: HardwareMap, + kind: Motor.Kind, + wheelRadius: Double, + gearRatio: Double = 1.0, + vararg ids: String + ) : this(hMap, kind.maxRPM, kind.TPR, wheelRadius, gearRatio, *ids) + + /** + * @param hMap the hardware map from the OpMode + * @param maxRPM the revolutions per minute of all the motors + * @param TPR the ticks per revolution of all the motors + * @param wheelRadius the radius of the wheels each motor is turning + * @param gearRatio the gear ratio from the output shaft to the wheel each motor is turning * @param ids the device ids from the RC config */ @JvmOverloads @@ -50,7 +106,28 @@ class MotorGroup(vararg val motors: Motor) : Component { *ids.map { Motor(hMap, it, maxRPM, TPR, wheelRadius, gearRatio) }.toTypedArray() ) - private val states = motors.map { it to it.reversed }.toMap() + /** + * @param hMap the hardware map from the OpMode + * @param maxRPM the revolutions per minute of all the motors + * @param TPR the ticks per revolution of all the motors + * @param wheelRadius the radius of the wheels each motor is turning + * @param gearRatio the gear ratio from the output shaft to the wheel each motor is turning + * @param ids the device ids from the RC config and whether those motors should be reversed + */ + @JvmOverloads + constructor( + hMap: HardwareMap, + maxRPM: Double, + TPR: Double = 1.0, + wheelRadius: Double, + gearRatio: Double, + vararg ids: Pair, + ) : this( + *ids.map { Motor(hMap, it.first, maxRPM, TPR, wheelRadius, gearRatio).apply { reversed = it.second } } + .toTypedArray() + ) + + private val states = motors.associateWith { it.reversed } /** * The maximum revolutions per minute that all motors in the group can achieve. @@ -182,6 +259,26 @@ class MotorGroup(vararg val motors: Motor) : Component { field = value } + /** + * Sets [targetPosition] with the target [angle]. + */ + fun setTargetAngle(angle: Angle) { + motors.forEach { it.setTargetAngle(angle) } + } + + /** + * Sets [targetPosition] with the target [angle], where [angle] is in [Angle.defaultUnits]. + */ + fun setTargetAngle(angle: Double) = setTargetAngle(Angle(angle)) + + /** + * Sets [targetPosition] with the target [distance]. + */ + fun setTargetDistance(distance: Double) { + motors.forEach { it.setTargetDistance(distance) } + } + + /** * The position error considered tolerable for [RunMode.RUN_TO_POSITION] to be considered at the set point. */ @@ -192,12 +289,10 @@ class MotorGroup(vararg val motors: Motor) : Component { } /** - * Returns a command that runs all the motors in the group until all of them have reached the desired position. + * Returns a command that runs all the motors in the group until + * all of them have reached the desired [position]. */ - fun goToPosition(position: Int): Command = Command.of { - runMode = RunMode.RUN_TO_POSITION - targetPosition = position - } + fun goToPosition(position: Int): Command = emptyCommand() .onInit { runMode = RunMode.RUN_TO_POSITION targetPosition = position @@ -207,24 +302,38 @@ class MotorGroup(vararg val motors: Motor) : Component { .onEnd { setSpeed(0.0) } /** - * Returns a command that runs all the motors in the group until all of them have reached the desired distance. + * Returns a command that runs all the motors in the group until + * all of them have reached the desired [distance]. */ - fun goToDistance(distance: Double): Command = Command.of { - runMode = RunMode.RUN_TO_POSITION - motors.forEach { - it.targetPosition = (distance / it.distancePerRev * it.TPR).toInt() + fun goToDistance(distance: Double): Command = emptyCommand() + .onInit { + runMode = RunMode.RUN_TO_POSITION + setTargetDistance(distance) } - } + .requires(this) + .runUntil { !isBusy() } + .onEnd { setSpeed(0.0) } + + /** + * Returns a command that runs all the motors in the group until + * all of them have reached the desired [angle]. + */ + fun goToAngle(angle: Angle): Command = emptyCommand() .onInit { runMode = RunMode.RUN_TO_POSITION - motors.forEach { - it.targetPosition = (distance / it.distancePerRev * it.TPR).toInt() - } + setTargetAngle(angle) } .requires(this) .runUntil { !isBusy() } .onEnd { setSpeed(0.0) } + /** + * Returns a command that runs all the motors in the group until + * all of them have reached the desired [angle], where [angle] is + * in [Angle.defaultUnits]. + */ + fun goToAngle(angle: Double): Command = goToAngle(Angle(angle)) + /** * Resets the encoders of all the motors in the group. */ diff --git a/command/src/main/kotlin/com/amarcolini/joos/hardware/Servo.kt b/command/src/main/kotlin/com/amarcolini/joos/hardware/Servo.kt index 5ec2952..facae53 100644 --- a/command/src/main/kotlin/com/amarcolini/joos/hardware/Servo.kt +++ b/command/src/main/kotlin/com/amarcolini/joos/hardware/Servo.kt @@ -21,13 +21,14 @@ import kotlin.math.min class Servo @JvmOverloads constructor( private val servo: Servo, @JvmField - val range: Angle = 180.deg, + val range: Angle = 300.deg, private val clock: NanoClock = NanoClock.system() ) : Component { /** * @param servo the servo for this wrapper to use * @param range the range of the servo in [Angle.defaultUnits] */ + @JvmOverloads constructor(servo: Servo, range: Double, clock: NanoClock = NanoClock.system()) : this(servo, Angle(range), clock) /** diff --git a/command/src/test/kotlin/CommandTest.kt b/command/src/test/kotlin/CommandTest.kt index 79bd919..025c5a7 100644 --- a/command/src/test/kotlin/CommandTest.kt +++ b/command/src/test/kotlin/CommandTest.kt @@ -8,12 +8,7 @@ import kotlin.math.abs private const val logOutput: Boolean = true class CommandTest { - private lateinit var scheduler: CommandScheduler - - @BeforeEach - fun init() { - scheduler = CommandScheduler() - } + private val scheduler = CommandScheduler @Test fun testCommandLifeCycle() { @@ -49,7 +44,7 @@ class CommandTest { @Test fun testConcurrentModification() { var result = false - val cmd = Command.emptyCommand().runUntil(false) + val cmd = Command.emptyCommand().runForever() .onEnd { result = true } diff --git a/navigation/src/main/kotlin/com/amarcolini/joos/geometry/Vector2d.kt b/navigation/src/main/kotlin/com/amarcolini/joos/geometry/Vector2d.kt index 4202bb6..eb5edd1 100644 --- a/navigation/src/main/kotlin/com/amarcolini/joos/geometry/Vector2d.kt +++ b/navigation/src/main/kotlin/com/amarcolini/joos/geometry/Vector2d.kt @@ -29,13 +29,13 @@ data class Vector2d @JvmOverloads constructor( /** * Returns the angle of this vector. */ - fun angle(): Angle = Angle(FastMath.atan2(y, x), AngleUnit.Radians) + fun angle(): Angle = FastMath.atan2(y, x).rad /** * Calculates the angle between two vectors (in radians). */ infix fun angleBetween(other: Vector2d): Angle = - Angle(acos((this dot other) / (norm() * other.norm())), AngleUnit.Radians) + acos((this dot other) / (norm() * other.norm())).rad /** * Adds two vectors. diff --git a/navigation/src/main/kotlin/com/amarcolini/joos/util/MathUtil.kt b/navigation/src/main/kotlin/com/amarcolini/joos/util/MathUtil.kt index 1e893df..396436f 100644 --- a/navigation/src/main/kotlin/com/amarcolini/joos/util/MathUtil.kt +++ b/navigation/src/main/kotlin/com/amarcolini/joos/util/MathUtil.kt @@ -45,6 +45,33 @@ object MathUtil { fun wrap(n: Int, min: Int, max: Int): Int = if (n < min) max - (min - n) % (max - min) else min + (n - min) % (max - min) + + /** + * Ensures that [n] lies in the range [min]..[max]. + */ + @JvmStatic + fun clamp(n: Double, min: Double, max: Double): Double = n.coerceIn(min, max) + + @JvmStatic + fun cos(angle: Angle): Double = angle.cos() + + @JvmStatic + fun sin(angle: Angle): Double = angle.sin() + + @JvmStatic + fun tan(angle: Angle): Double = angle.tan() + + @JvmStatic + fun abs(angle: Angle): Angle = angle.abs() + + @JvmStatic + fun min(angle1: Angle, angle2: Angle): Angle = min(angle1.radians, angle2.radians).rad + + @JvmStatic + fun max(angle1: Angle, angle2: Angle): Angle = max(angle1.radians, angle2.radians).rad + + @JvmStatic + fun sign(angle: Angle): Double = sign(angle.radians) } fun cos(angle: Angle): Double = angle.cos()