From 7b42c90931c7468414df9cccdb4650be284bd31a Mon Sep 17 00:00:00 2001 From: Strings <168338344+StringsVR@users.noreply.github.com> Date: Tue, 18 Nov 2025 01:20:47 -0500 Subject: [PATCH 1/5] Create flywheels.md --- src/control/examples/flywheels.md | 84 +++++++++++++++++++++++++++++++ 1 file changed, 84 insertions(+) create mode 100644 src/control/examples/flywheels.md diff --git a/src/control/examples/flywheels.md b/src/control/examples/flywheels.md new file mode 100644 index 0000000..da76516 --- /dev/null +++ b/src/control/examples/flywheels.md @@ -0,0 +1,84 @@ +# Flywheel Example +Flywheels are commonly used for launching game elements, +They are typically controlled using a velocity PID feedforward controller. + +With NextControl, that would be implemented like this (using hypothetical constants): + +::: tabs key:code + +== Kotlin + +```kotlin +controlSystem { + velPid(0.005, 0.0, 0.0) + basicFF(0.01, 0.02, 0.03) +} +``` + +== Java + +```java +ControlSystem.builder() + .velPid(0.005, 0.0, 0.0) + .basicFF(0.01, 0.02, 0.03) + .build(); +``` + +::: + +## What can I do with this? +If you're using a command-based framework like NextFTC, +you can create commands that spin the flywheel up or shut it down. + +An example Flywheel subsystem implemented with NextControl looks like: + +::: tabs key:code + +== Kotlin + +```kotlin +object Flywheel : Subsystem { + private val motor = MotorEx("flywheel_motor") + + private val controller: ControlSystem = controlSystem { + velPid(0.005, 0.0, 0.0) + basicFF(0.01, 0.02, 0.03) + } + + val off: Command = RunToVelocity(controller, 0.0).named("FlywheelOff") + val on: Command = RunToVelocity(controller, 500.0).named("FlywheelOn") + + override fun periodic() { + motor.power = controller.calculate(motor.state) + } +} +``` + +== Java + +```java +public class Flywheel implements Subsystem { + public static final Flywheel INSTANCE = new Flywheel(); + private Flywheel() { } + + private final MotorEx motor = new MotorEx("flywheel_motor"); + + private final ControlSystem controller = ControlSystem.builder() + .velPid(0.005, 0, 0) + .basicFF(0.01, 0.02, 0.03) + .build(); + + public final Command off = new RunToVelocity(controller, 0.0) + .requires(this) + .named("FlywheelOff"); + + public final Command on = new RunToVelocity(controller, 500.0) + .requires(this) + .named("FlywheelOn"); + + @Override + public void periodic() { + motor.setPower(controller.calculate(motor.getState())); + } +} +``` From dcd26c271d2faa9907e68a58ca18e5654bbbebc6 Mon Sep 17 00:00:00 2001 From: Strings <168338344+StringsVR@users.noreply.github.com> Date: Tue, 18 Nov 2025 01:30:11 -0500 Subject: [PATCH 2/5] Create flywheel.md --- src/guide/subsystems/flywheel.md | 233 +++++++++++++++++++++++++++++++ 1 file changed, 233 insertions(+) create mode 100644 src/guide/subsystems/flywheel.md diff --git a/src/guide/subsystems/flywheel.md b/src/guide/subsystems/flywheel.md new file mode 100644 index 0000000..e3ae649 --- /dev/null +++ b/src/guide/subsystems/flywheel.md @@ -0,0 +1,233 @@ +# Lift Subsystem + +A subsystem that is found in almost all FTC robots in most seasons is a flywheel, +here will learn how to program your own flywheel subsystem. + +> [!TIP] +> This subsystem can be generalized to any subsystem with a motor. + +## Step 1: Create your subsystem + +The first step to creating your subsystem is setting up the structure for it. +There should only be one instance of each +subsystem. To do this, we will make our subsystem +a [singleton](https://www.geeksforgeeks.org/singleton-design-pattern/). + +:::tabs key:codes + +== Kotlin + +Creating our subsystem with the `object` keyword makes it a singleton: + +```kotlin +object Flywheel : Subsystem { + +} +``` + +== Java +In Java, there are a couple lines of boilerplate you will need to add to the top +of every subsystem you create to +make it a singleton: + +```java +public class Flywheel implements Subsystem { + public static final Flywheel INSTANCE = new Flywheel(); + private Flywheel() { } + +} +``` + +::: + +## Step 2: Create your motor + +Next, we need to set up a motor to power our flywheel. Let's start by creating a +variable to store our motor. It should be +of type `MotorEx`. + +:::tabs key:code + +== Kotlin + +```kotlin +private val motor = MotorEx("lift_motor") +``` + +== Java + +```java +private MotorEx motor = new MotorEx("lift_motor"); +``` + +::: + +We also need a `ControlSystem`, since we want to move our motor. We'll be +using [NextControl](/control), NextFTC's +solution to control in FTC. NextControl makes it very easy to create more +complex controllers if you ever need them. +Let's use a PID controller with a basic feedforward. + +:::tabs key:code +== Kotlin + +```kotlin +private val controller = controlSystem { + velPid(0.005, 0.0, 0.0) + basicFF(0.0, 0.0, 0.0) +} +``` + +== Java + +```java +private ControlSystem controlSystem = ControlSystem.builder() + .velPid(0.005, 0.0, 0.0) + .basicFF(0.0, 0.0, 0.0) + .build(); +``` + +::: + +> [!IMPORTANT] +> Don't forget to tune your controller! + +Now, we must set our motor power to the `ControlSystem`'s output every loop. We +can run code every loop by overriding the `periodic()` function. + +The reason we set the motor power in `periodic()` instead of in our commands is +that the `ControlSystem` needs to be updated every loop to work properly. If we +only set the motor power in our commands, the motor power would only be updated +when a command is running, meaning once a command finishes, the motor power +would stop being updated and thus remain at whatever it was last set to, +pushing it past its target velocity. + +:::tabs key:code + +== Kotlin + +```kotlin +override fun periodic() { + motor.power = controlSystem.calculate(motor.state) +} +``` + +== Java + +```java +@Override +public void periodic() { + motor.setPower(controlSystem.calculate(motor.getState())); +} +``` + +::: + +That's all you need to do to create a motor in NextFTC. + +We're not quite done, though. We still need to create our commands! + +## Step 3: Create commands + +The last step when you create a Subsystem is to create the commands you'll be +using. This process varies with each +subsystem. Here, we'll create three commands that each move +the lift to a different height: `toLow`, +`toMiddle`, and `toHigh`. + +To control our motor's position, we will be using the `RunToPosition` command. + +Let's create our first `RunToVelocity` command. + +:::tabs key:code +== Kotlin + +```kotlin +val on: Command = RunToVelocity(controller, 500.0).named("FlywheelOn") +``` + +== Java + +```java +public final Command on = new RunToVelocity(controller, 500.0) + .requires(this) + .named("FlywheelOn"); +``` + +::: + +Pretty easy, right? Let's duplicate it and update our variable name and target +position to create our other two commands: + +:::tabs key:code +== Kotlin + +```kotlin +val off: Command = RunToVelocity(controller, 0.0).named("FlywheelOff") +``` + +== Java + +```java +public final Command off = new RunToVelocity(controller, 0.0) + .requires(this) + .named("FlywheelOff"); +``` + +::: + +## Final result + +That's it! You've created your first subsystem! Here is the final result: + +:::tabs key:code +== Kotlin + +```kotlin +object Flywheel : Subsystem { + private val motor = MotorEx("flywheel_motor") + + private val controller: ControlSystem = controlSystem { + velPid(0.005, 0.0, 0.0) + basicFF(0.01, 0.02, 0.03) + } + + val off: Command = RunToVelocity(controller, 0.0).named("FlywheelOff") + val on: Command = RunToVelocity(controller, 500.0).named("FlywheelOn") + + override fun periodic() { + motor.power = controller.calculate(motor.state) + } +} +``` + +== Java + +```java +public class Flywheel implements Subsystem { + public static final Flywheel INSTANCE = new Flywheel(); + private Flywheel() { } + + private final MotorEx motor = new MotorEx("flywheel_motor"); + + private final ControlSystem controller = ControlSystem.builder() + .velPid(0.005, 0, 0) + .basicFF(0.01, 0.02, 0.03) + .build(); + + public final Command off = new RunToVelocity(controller, 0.0) + .requires(this) + .named("FlywheelOff"); + + public final Command on = new RunToVelocity(controller, 500.0) + .requires(this) + .named("FlywheelOn"); + + @Override + public void periodic() { + motor.setPower(controller.calculate(motor.getState())); + } +} +``` + +::: From c5478af3c962fdf028e8725b65708d311a3fe890 Mon Sep 17 00:00:00 2001 From: Strings <168338344+StringsVR@users.noreply.github.com> Date: Tue, 18 Nov 2025 01:40:46 -0500 Subject: [PATCH 3/5] Update flywheels.md --- src/control/examples/flywheels.md | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/control/examples/flywheels.md b/src/control/examples/flywheels.md index da76516..fc171dc 100644 --- a/src/control/examples/flywheels.md +++ b/src/control/examples/flywheels.md @@ -30,7 +30,8 @@ ControlSystem.builder() If you're using a command-based framework like NextFTC, you can create commands that spin the flywheel up or shut it down. -An example Flywheel subsystem implemented with NextControl looks like: +An example of a Flywheel subsystem using NextControl can be found +[here](../../guide/subsystems/flywheel.md). ::: tabs key:code From 187c8388553be4b34c9070980512aada00644b9b Mon Sep 17 00:00:00 2001 From: Strings <168338344+StringsVR@users.noreply.github.com> Date: Tue, 18 Nov 2025 01:42:56 -0500 Subject: [PATCH 4/5] Update flywheel.md --- src/guide/subsystems/flywheel.md | 14 ++++++-------- 1 file changed, 6 insertions(+), 8 deletions(-) diff --git a/src/guide/subsystems/flywheel.md b/src/guide/subsystems/flywheel.md index e3ae649..a9ddf56 100644 --- a/src/guide/subsystems/flywheel.md +++ b/src/guide/subsystems/flywheel.md @@ -1,4 +1,4 @@ -# Lift Subsystem +# Flywheel Subsystem A subsystem that is found in almost all FTC robots in most seasons is a flywheel, here will learn how to program your own flywheel subsystem. @@ -51,13 +51,13 @@ of type `MotorEx`. == Kotlin ```kotlin -private val motor = MotorEx("lift_motor") +private val motor = MotorEx("flywheel_motor") ``` == Java ```java -private MotorEx motor = new MotorEx("lift_motor"); +private MotorEx motor = new MotorEx("flywheel_motor"); ``` ::: @@ -131,11 +131,9 @@ We're not quite done, though. We still need to create our commands! The last step when you create a Subsystem is to create the commands you'll be using. This process varies with each -subsystem. Here, we'll create three commands that each move -the lift to a different height: `toLow`, -`toMiddle`, and `toHigh`. +subsystem. Here, we'll create two commands. -To control our motor's position, we will be using the `RunToPosition` command. +To control our motor's position, we will be using the `RunToVelocity` command. Let's create our first `RunToVelocity` command. @@ -157,7 +155,7 @@ public final Command on = new RunToVelocity(controller, 500.0) ::: Pretty easy, right? Let's duplicate it and update our variable name and target -position to create our other two commands: +velocity to create our off command: :::tabs key:code == Kotlin From 10f3bf9c680f199a267858f02605539eb3ee6592 Mon Sep 17 00:00:00 2001 From: Strings <168338344+StringsVR@users.noreply.github.com> Date: Tue, 18 Nov 2025 01:44:58 -0500 Subject: [PATCH 5/5] Update flywheel.md --- src/guide/subsystems/flywheel.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/guide/subsystems/flywheel.md b/src/guide/subsystems/flywheel.md index a9ddf56..0f0480f 100644 --- a/src/guide/subsystems/flywheel.md +++ b/src/guide/subsystems/flywheel.md @@ -133,7 +133,7 @@ The last step when you create a Subsystem is to create the commands you'll be using. This process varies with each subsystem. Here, we'll create two commands. -To control our motor's position, we will be using the `RunToVelocity` command. +To control our motor's velocity , we will be using the `RunToVelocity` command. Let's create our first `RunToVelocity` command.