maven(url = "https://maven.rowanmcalpin.com/")
maven(url = "https://maven.pedropathing.com/") // Remove if you don't intend to use PedroPathing
maven(url = "https://maven.brott.dev/") // Remove if you don't intend to use the FTC Dashboard (required if using PedroPathing)

implementation("com.rowanmcalpin.nextftc:core:0.5.5-beta1")
implementation("com.rowanmcalpin.nextftc:ftc:0.5.5-beta1")
implementation("com.rowanmcalpin.nextftc:pedro:0.5.5-beta1") // Remove if you don't intend to use PedroPathing
implementation("com.pedropathing:pedro:1.0.3") // Remove if you don't intend to use PedroPathing
implementation("com.acmerobotics.dashboard:dashboard:0.4.16") // Remove if you don't intend to use the FTC Dashboard (required if using PedroPathing)

Step 1: Create your class

The first step to creating your Subsystem is setting up the structure for it. There should only be one instance of each Subsystem class. To do this, there is a couple lines of boilerplate you will need to add to the top of every subsystem class you create. Here is the subsystem class template:

public class MySubsystem extends Subsystem {
    // BOILERPLATE
    public static final MySubsystem INSTANCE = new MySubsystem();
    private MySubsystem() { }

    // USER CODE HERE
}

In this case, let's call our subsystem Lift.

Step 2: Create your motor

Next, we need to set up a motor to power our lift. This is easy to do using the MotorEx class. Let's start by creating a variable to store our motor. It should be of type MotorEx. Note that we aren't setting the value immediately; this is because we don't have access to the hardware map until an OpMode is initialized.

public MotorEx motor;

We also need a Controller, since we want to move our motor. Let's use a PID controller. I recommend using a P value of 0.005 to start, and leaving I and D at zero. You should come back and tune it later, though.

public PIDController controller = new PIDController(new PIDCoefficients(0.005, 0.0, 0.0));

Next, we need a name. This is the name specified in the hardwareMap, so that NextFTC can find the motor. I called mine lift_motor, so that's the name I'm using. Use whatever name you've set in your configuration:

public String name = "lift_motor";

Finally, the last step to setting up a motor is instantiating the motor in the initialize() function. Let's do that now:

@Override
public void initialize() {
    motor = new MotorEx(name);
}

That's all you need to do to create a motor in NextFTC! To recap:

• Every motor needs to be stored in a variable
• Every motor needs a Controller
• Every motor needs a name
• Every motor must be instantiated in the initialize function.

We're not quite done, though. We still need to create our first 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, I'll walk you through creating three commands that each move the lift to a different height: toLow, toMiddle, and toHigh.

To control our motor, we will be using the RunToPosition command. In Java, the best way to create commands is by making functions that return instances of command classes.

Let's create our first RunToPosition command:

public Command toLow() {
    return new RunToPosition(motor, // MOTOR TO MOVE
            0.0, // TARGET POSITION, IN TICKS
            controller, // CONTROLLER TO IMPLEMENT
            this); // IMPLEMENTED SUBSYSTEM
}

Note the last parameter: subsystem. This is what tells NextFTC which commands should be allowed to run at the same time. If it weren't set, toLow would be able to run at the same time as other commands that use the Lift subsystem -- so there would be multiple things fighting to set the motor's power. Generally, you just need to pass this as the subsystem -- there are exceptions with more complicated custom commands.

Pretty easy, right? Let's duplicate it and update our function name and target position to create our other two commands:

public Command toMiddle() {
    return new RunToPosition(motor, // MOTOR TO MOVE
            500.0, // TARGET POSITION, IN TICKS
            controller, // CONTROLLER TO IMPLEMENT
            this); // IMPLEMENTED SUBSYSTEM
}

public Command toHigh() {
    return new RunToPosition(motor, // MOTOR TO MOVE
            1200.0, // TARGET POSITION, IN TICKS
            controller, // CONTROLLER TO IMPLEMENT
            this); // IMPLEMENTED SUBSYSTEM
}

Final result

That's it! You've created your first Subsystem! Here is the final result:

public class Lift extends Subsystem {
    // BOILERPLATE
    public static final Lift INSTANCE = new Lift();
    private Lift() { }

    // USER CODE
    public MotorEx motor;

    public PIDController controller = new PIDController(new PIDCoefficients(0.005, 0.0, 0.0));

    public String name = "lift_motor";

    public Command toLow() {
        return new RunToPosition(motor, // MOTOR TO MOVE
                0.0, // TARGET POSITION, IN TICKS
                controller, // CONTROLLER TO IMPLEMENT
                this); // IMPLEMENTED SUBSYSTEM
    }

    public Command toMiddle() {
        return new RunToPosition(motor, // MOTOR TO MOVE
                500.0, // TARGET POSITION, IN TICKS
                controller, // CONTROLLER TO IMPLEMENT
                this); // IMPLEMENTED SUBSYSTEM
    }

    public Command toHigh() {
        return new RunToPosition(motor, // MOTOR TO MOVE
                1200.0, // TARGET POSITION, IN TICKS
                controller, // CONTROLLER TO IMPLEMENT
                this); // IMPLEMENTED SUBSYSTEM
    }
    
    @Override
    public void initialize() {
        motor = new MotorEx(name);
    }
}

Step 1: Create your class

Just like with the Lift Subsystem, we need to start by creating our class. Remember the boilerplate!

public class Claw extends Subsystem {
    // BOILERPLATE
    public static final Claw INSTANCE = new Claw();
    private Claw() { }

    // USER CODE
    
}

Step 2: Create your servo

Now, since we're using a servo, instead of a motor, let's create a servo variable. Just like our motor variable from the lift subystem, we can't initialize it right away. Currently, there is no wrapper class for Servos, so you will be using the Qualcomm servo class.

public Servo servo;

Just like our motors, we also need a name for our servo. This is the name specified in the hardwareMap. I called mine claw_servo:

public String name = "claw_servo";

Finally, we need to initialize our servo in the initialize() function. Because there is no wrapper class, you will need to access the hardwareMap yourself. The easiest way to do this is by using OpModeData.INSTANCE.getHardwareMap(). NextFTC will automatically give you the active hardwareMap from your running OpMode (as long as you extend NextFTCOpMode or PedroOpMode):

@Override
public void initialize() {
    servo = OpModeData.INSTANCE.getHardwareMap().get(Servo.class, name);
}

To recap how you create servo-based Subsystems in NextFTC:

• Create a variable to store your Servo instance
• Create a variable to store the name
• In initialize(), get the Servo instance from the hardwareMap using your name variable.

Step 3: Create commands

Programming servo commands is very easy in NextFTC. Just like your Lift, you will be creating functions that return instances of Commands.

For servos, the command you will be using is ServoToPosition. You will pass your servo, a target position, and your subsystem (just like the Lift):

public Command open() {
    return new ServoToPosition(servo, // SERVO TO MOVE
            0.9, // POSITION TO MOVE TO
            this); // IMPLEMENTED SUBSYSTEM
}

Nice! Let's do the same with the close command:

public Command close() {
    return new ServoToPosition(servo, // SERVO TO MOVE
            0.2, // POSITION TO MOVE TO
            this); // IMPLEMENTED SUBSYSTEM
}

Final result

You've successfully created your claw subsystem! Here's the final result:

public class Claw extends Subsystem {
    // BOILERPLATE
    public static final Claw INSTANCE = new Claw();
    private Claw() { }

    // USER CODE
    public Servo servo;
    
    public String name = "claw_servo";

    public Command open() {
        return new ServoToPosition(servo, // SERVO TO MOVE
                0.9, // POSITION TO MOVE TO
                this); // IMPLEMENTED SUBSYSTEM
    }

    public Command close() {
        return new ServoToPosition(servo, // SERVO TO MOVE
                0.2, // POSITION TO MOVE TO
                this); // IMPLEMENTED SUBSYSTEM
    }

    @Override
    public void initialize() {
        servo = OpModeData.INSTANCE.getHardwareMap().get(Servo.class, name);
    }
}

Step 1: Create your class

OpModes in NextFTC will extend one of two classes: NextFTCOpMode and PedroOpMode, depending on if you're using PedroPathing.

This example will use NextFTCOpMode. Refer to the PedroPathing page to learn how to convert a NextFTCOpMode to a PedroOpMode and incorporate PedroPathing.

That being said, here is the basic structure for every Autonomous OpMode:

@Autonomous(name = "NextFTC Autonomous Program Java")
public class AutonomousProgram extends NextFTCOpMode {
    
}

That's not all, though. We want our autonomous program to use the Lift and Claw subsystems we created in the subsystems guide. To do that, we need to add them into the constructor of NextFTCOpMode. We will create a constructor for the AutonomousProgram class, in which we can call the super constructor with our subsystems:

public AutonomousProgram() {
    super(Claw.INSTANCE, Lift.INSTANCE);
}

This will tell NextFTC that we will be using those subystems in this OpMode, and will initialize them accordingly.

Step 2: Creating a routine

You've already learned how to create individual commands, such as motor and servo movements. Now, it's time to group them together into useful behaviors. This is where CommandGroups and routines come into play. Before we can create our own, we first need to understand how commands are run behind the scenes.

The CommandManager stores an internal list of actively running commands. It goes through each command and calls its update() function every single loop. It also determines which commands to cancel, handles subsystem conflicts, and offers additional functionality as well. The important thing to note is that all commands that are directly stored by the CommandManager run simultaneously. Knowing that, you may be wondering why ParallelGroups exist, if you can just schedule commands directly. Trust me, we'll get there. Before that, we need to understand what a SequentialGroup does.

A SequentialGroup stores a collection of commands and runs them in a row. Instead of scheduling all of its children at once, it schedules the first one, and then waits to schedule the next one until the first has completed, then continues scheduling them one by one until they've all completed. If you think about it a little bit, it may become apparent what a ParallelGroup is for. If you are using a SequentialGroup, you may have things you want to happen simultaneously within that group. For example:

1. Drive to a location
2. Simultaneously raise a lift and rotate an arm
3. Open a claw
4. Simultaneously lower the lift, reset the arm, and drive to a different location

This could only be accomplished using ParallelGroups in conjunction with SequentialGroups.

Now that we know what CommandGroups do, let's learn how to create them. For this example, we will create a routine that does the following:

1. Raise the lift to the high position
2. Simultaneously open the claw and move the lift to the middle position
3. Wait for half a second
4. Simultaneously close the claw and move the lift to the low position

This routine is unlikely to actually be useful in an autonomous program, but it will introduce you to the mental processes behind creating commands, and will give you the tools you need to create your own. Let's go ahead and create our command function.

public Command firstRoutine() {
    return new SequentialGroup();
}

I've made an empty SequentialGroup here, for demonstration purposes.

As mentioned above, we need to put something in our sequential group in order to avoid errors. In our list, we said the first thing we want to do is raise the lift to the high position. We can add the Lift.toHigh command into our group very easily:

public Command firstRoutine() {
    return new SequentialGroup(
            Lift.INSTANCE.toHigh()
    );
}

The next thing we wanted to do is simultaneously open the claw and move the lift to the middle position. To add another command to the group, add a comma at the end of your last item (in this case, Lift.INSTANCE.toHigh()) and add the next command on a new line (before the close parenthesis). In this case, we want to add a ParallelGroup because we want to do things simultaneously next.

public Command firstRoutine() {
    return new SequentialGroup(
            Lift.INSTANCE.toHigh(),
            new ParallelGroup()
    );
}

Just like with SequentialGroups, you shouldn't create empty ParallelGroups (although it won't cause an error like ParallelGroups do). Let's populate it with our Lift.INSTANCE.toMiddle() and Claw.INSTANCE.close() commands:

public Command firstRoutine() {
    return new SequentialGroup(
            Lift.INSTANCE.toHigh(),
            new ParallelGroup(
                    Lift.INSTANCE.toMiddle(),
                    Claw.INSTANCE.close()
            )
    );
}

Since command groups are also just commands, we can just continue to add commands after the ParallelGroup. Let's wait for half a second using a Delay command. That takes a single value, the amount of time to delay in seconds.

Let's create our delay:

public Command firstRoutine() {
    return new SequentialGroup(
            Lift.INSTANCE.toHigh(),
            new ParallelGroup(
                    Lift.INSTANCE.toMiddle(),
                    Claw.INSTANCE.close()
            ),
            new Delay(0.5)
    );
}

Finally, we can add our last ParallelGroup to our routine. The final routine looks like this:

public Command firstRoutine() {
    return new SequentialGroup(
            Lift.INSTANCE.toHigh(),
            new ParallelGroup(
                    Lift.INSTANCE.toMiddle(),
                    Claw.INSTANCE.close()
            ),
            new Delay(0.5),
            new ParallelGroup(
                    Claw.INSTANCE.open(),
                    Lift.INSTANCE.toLow()
            )
    );
}

Step 3: Running our routine

Now that we have our routine, we just need to run it. To do this, let's override the onStartButtonPressed() function. To schedule a command, you can either call CommandManager.INSTANCE.addCommand(commandToAdd()), or you can just do commandToAdd().invoke(). In this case, let's do the latter.

@Override
public void onStartButtonPressed() {
    firstRoutine().invoke();
}

Final result

That's it! You have created your very first autonomous program and, perhaps more importantly, learned about some of the tools you have at your disposal to create more complex autonomous programs.

Here is the final result:

@Autonomous(name = "NextFTC Autonomous Program Java")
public class AutonomousProgram extends NextFTCOpMode {
    public AutonomousProgram() {
        super(Claw.INSTANCE, Lift.INSTANCE);
    }

    public Command firstRoutine() {
        return new SequentialGroup(
                Lift.INSTANCE.toHigh(),
                new ParallelGroup(
                        Lift.INSTANCE.toMiddle(),
                        Claw.INSTANCE.close()
                ),
                new Delay(0.5),
                new ParallelGroup(
                        Claw.INSTANCE.open(),
                        Lift.INSTANCE.toLow()
                )
        );
    }

    @Override
    public void onStartButtonPressed() {
        firstRoutine().invoke();
    }
}